maxmustermann/godot
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Merge pull request #46937 from nekomatata/soft-body-support

Browse source 
SoftBody support in GodotPhysics 3D
This commit is contained in:
Rémi Verschelde 2021-03-20 21:45:20 +01:00 committed by GitHub
parent 793000c6a9 d5ea4acd2d
commit fa681d04b7
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
26 changed files with 2489 additions and 359 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
COPYRIGHT.txt
View file

@ -87,6 +87,8 @@ Files: ./servers/physics_3d/gjk_epa.cpp
./servers/physics_3d/joints/pin_joint_3d_sw.h
./servers/physics_3d/joints/slider_joint_3d_sw.cpp
./servers/physics_3d/joints/slider_joint_3d_sw.h
./servers/physics_3d/soft_body_3d_sw.cpp
./servers/physics_3d/soft_body_3d_sw.h
Comment: Bullet Continuous Collision Detection and Physics Library
Copyright: 2003-2008, Erwin Coumans
2007-2021, Juan Linietsky, Ariel Manzur.

6
doc/classes/SoftBody3D.xml
View file

@ -77,8 +77,6 @@
</method>
</methods>
<members>
<member name="angular_stiffness" type="float" setter="set_angular_stiffness" getter="get_angular_stiffness" default="0.0">
</member>
<member name="collision_layer" type="int" setter="set_collision_layer" getter="get_collision_layer" default="1">
The physics layers this SoftBody3D is in.
Collidable objects can exist in any of 32 different layers. These layers work like a tagging system, and are not visual. A collidable can use these layers to select with which objects it can collide, using the collision_mask property.
@ -96,8 +94,6 @@
<member name="parent_collision_ignore" type="NodePath" setter="set_parent_collision_ignore" getter="get_parent_collision_ignore" default="NodePath(&quot;&quot;)">
[NodePath] to a [CollisionObject3D] this SoftBody3D should avoid clipping.
</member>
<member name="pose_matching_coefficient" type="float" setter="set_pose_matching_coefficient" getter="get_pose_matching_coefficient" default="0.0">
</member>
<member name="pressure_coefficient" type="float" setter="set_pressure_coefficient" getter="get_pressure_coefficient" default="0.0">
</member>
<member name="ray_pickable" type="bool" setter="set_ray_pickable" getter="is_ray_pickable" default="true">
@ -109,8 +105,6 @@
<member name="total_mass" type="float" setter="set_total_mass" getter="get_total_mass" default="0.0">
The SoftBody3D's mass.
</member>
<member name="volume_stiffness" type="float" setter="set_volume_stiffness" getter="get_volume_stiffness" default="0.0">
</member>
</members>
<constants>
</constants>

96
modules/bullet/bullet_physics_server.cpp
View file

@ -433,12 +433,6 @@ void BulletPhysicsServer3D::area_set_ray_pickable(RID p_area, bool p_enable) {
area->set_ray_pickable(p_enable);
}
bool BulletPhysicsServer3D::area_is_ray_pickable(RID p_area) const {
AreaBullet *area = area_owner.getornull(p_area);
ERR_FAIL_COND_V(!area, false);
return area->is_ray_pickable();
}
RID BulletPhysicsServer3D::body_create(BodyMode p_mode, bool p_init_sleeping) {
RigidBodyBullet *body = bulletnew(RigidBodyBullet);
body->set_mode(p_mode);
@ -842,12 +836,6 @@ void BulletPhysicsServer3D::body_set_ray_pickable(RID p_body, bool p_enable) {
body->set_ray_pickable(p_enable);
}
bool BulletPhysicsServer3D::body_is_ray_pickable(RID p_body) const {
RigidBodyBullet *body = rigid_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, false);
return body->is_ray_pickable();
}
PhysicsDirectBodyState3D *BulletPhysicsServer3D::body_get_direct_state(RID p_body) {
RigidBodyBullet *body = rigid_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, nullptr);
@ -880,7 +868,7 @@ RID BulletPhysicsServer3D::soft_body_create(bool p_init_sleeping) {
CreateThenReturnRID(soft_body_owner, body);
}
void BulletPhysicsServer3D::soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) {
void BulletPhysicsServer3D::soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
@ -922,6 +910,13 @@ void BulletPhysicsServer3D::soft_body_set_mesh(RID p_body, const REF &p_mesh) {
body->set_soft_mesh(p_mesh);
}
AABB BulletPhysicsServer::soft_body_get_bounds(RID p_body) const {
SoftBodyBullet *body = soft_body_owner.get(p_body);
ERR_FAIL_COND_V(!body, AABB());
return body->get_bounds();
}
void BulletPhysicsServer3D::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
@ -1002,34 +997,19 @@ void BulletPhysicsServer3D::soft_body_set_transform(RID p_body, const Transform
body->set_soft_transform(p_transform);
}
Vector3 BulletPhysicsServer3D::soft_body_get_vertex_position(RID p_body, int vertex_index) const {
const SoftBodyBullet *body = soft_body_owner.getornull(p_body);
Vector3 pos;
ERR_FAIL_COND_V(!body, pos);
body->get_node_position(vertex_index, pos);
return pos;
}
void BulletPhysicsServer3D::soft_body_set_ray_pickable(RID p_body, bool p_enable) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
body->set_ray_pickable(p_enable);
}
bool BulletPhysicsServer3D::soft_body_is_ray_pickable(RID p_body) const {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, false);
return body->is_ray_pickable();
}
void BulletPhysicsServer3D::soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
body->set_simulation_precision(p_simulation_precision);
}
int BulletPhysicsServer3D::soft_body_get_simulation_precision(RID p_body) {
int BulletPhysicsServer3D::soft_body_get_simulation_precision(RID p_body) const {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, 0.f);
return body->get_simulation_precision();
@ -1041,13 +1021,13 @@ void BulletPhysicsServer3D::soft_body_set_total_mass(RID p_body, real_t p_total_
body->set_total_mass(p_total_mass);
}
real_t BulletPhysicsServer3D::soft_body_get_total_mass(RID p_body) {
real_t BulletPhysicsServer3D::soft_body_get_total_mass(RID p_body) const {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, 0.f);
return body->get_total_mass();
}
void BulletPhysicsServer3D::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) {
void BulletPhysicsServer3D::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) const {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
body->set_linear_stiffness(p_stiffness);
@ -1059,61 +1039,25 @@ real_t BulletPhysicsServer3D::soft_body_get_linear_stiffness(RID p_body) {
return body->get_linear_stiffness();
}
void BulletPhysicsServer3D::soft_body_set_angular_stiffness(RID p_body, real_t p_stiffness) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
body->set_angular_stiffness(p_stiffness);
}
real_t BulletPhysicsServer3D::soft_body_get_angular_stiffness(RID p_body) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, 0.f);
return body->get_angular_stiffness();
}
void BulletPhysicsServer3D::soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
body->set_volume_stiffness(p_stiffness);
}
real_t BulletPhysicsServer3D::soft_body_get_volume_stiffness(RID p_body) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, 0.f);
return body->get_volume_stiffness();
}
void BulletPhysicsServer3D::soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
body->set_pressure_coefficient(p_pressure_coefficient);
}
real_t BulletPhysicsServer3D::soft_body_get_pressure_coefficient(RID p_body) {
real_t BulletPhysicsServer3D::soft_body_get_pressure_coefficient(RID p_body) const {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, 0.f);
return body->get_pressure_coefficient();
}
void BulletPhysicsServer3D::soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
return body->set_pose_matching_coefficient(p_pose_matching_coefficient);
}
real_t BulletPhysicsServer3D::soft_body_get_pose_matching_coefficient(RID p_body) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, 0.f);
return body->get_pose_matching_coefficient();
}
void BulletPhysicsServer3D::soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
body->set_damping_coefficient(p_damping_coefficient);
}
real_t BulletPhysicsServer3D::soft_body_get_damping_coefficient(RID p_body) {
real_t BulletPhysicsServer3D::soft_body_get_damping_coefficient(RID p_body) const {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, 0.f);
return body->get_damping_coefficient();
@ -1125,7 +1069,7 @@ void BulletPhysicsServer3D::soft_body_set_drag_coefficient(RID p_body, real_t p_
body->set_drag_coefficient(p_drag_coefficient);
}
real_t BulletPhysicsServer3D::soft_body_get_drag_coefficient(RID p_body) {
real_t BulletPhysicsServer3D::soft_body_get_drag_coefficient(RID p_body) const {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, 0.f);
return body->get_drag_coefficient();
@ -1137,7 +1081,7 @@ void BulletPhysicsServer3D::soft_body_move_point(RID p_body, int p_point_index,
body->set_node_position(p_point_index, p_global_position);
}
Vector3 BulletPhysicsServer3D::soft_body_get_point_global_position(RID p_body, int p_point_index) {
Vector3 BulletPhysicsServer3D::soft_body_get_point_global_position(RID p_body, int p_point_index) const {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, Vector3(0., 0., 0.));
Vector3 pos;
@ -1145,14 +1089,6 @@ Vector3 BulletPhysicsServer3D::soft_body_get_point_global_position(RID p_body, i
return pos;
}
Vector3 BulletPhysicsServer3D::soft_body_get_point_offset(RID p_body, int p_point_index) const {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, Vector3());
Vector3 res;
body->get_node_offset(p_point_index, res);
return res;
}
void BulletPhysicsServer3D::soft_body_remove_all_pinned_points(RID p_body) {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
@ -1165,7 +1101,7 @@ void BulletPhysicsServer3D::soft_body_pin_point(RID p_body, int p_point_index, b
body->set_node_mass(p_point_index, p_pin ? 0 : 1);
}
bool BulletPhysicsServer3D::soft_body_is_point_pinned(RID p_body, int p_point_index) {
bool BulletPhysicsServer3D::soft_body_is_point_pinned(RID p_body, int p_point_index) const {
SoftBodyBullet *body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, 0.f);
return body->get_node_mass(p_point_index);

35
modules/bullet/bullet_physics_server.h
View file

@ -163,7 +163,6 @@ public:
virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override;
virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override;
virtual void area_set_ray_pickable(RID p_area, bool p_enable) override;
virtual bool area_is_ray_pickable(RID p_area) const override;
/* RIGID BODY API */
@ -250,7 +249,6 @@ public:
virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) override;
virtual void body_set_ray_pickable(RID p_body, bool p_enable) override;
virtual bool body_is_ray_pickable(RID p_body) const override;
// this function only works on physics process, errors and returns null otherwise
virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override;
@ -262,13 +260,15 @@ public:
virtual RID soft_body_create(bool p_init_sleeping = false) override;
virtual void soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) override;
virtual void soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) override;
virtual void soft_body_set_space(RID p_body, RID p_space) override;
virtual RID soft_body_get_space(RID p_body) const override;
virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) override;
virtual AABB soft_body_get_bounds(RID p_body) const override;
virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override;
virtual uint32_t soft_body_get_collision_layer(RID p_body) const override;
@ -284,46 +284,33 @@ public:
/// Special function. This function has bad performance
virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) override;
virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const override;
virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override;
virtual bool soft_body_is_ray_pickable(RID p_body) const override;
virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override;
virtual int soft_body_get_simulation_precision(RID p_body) override;
virtual int soft_body_get_simulation_precision(RID p_body) const override;
virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override;
virtual real_t soft_body_get_total_mass(RID p_body) override;
virtual real_t soft_body_get_total_mass(RID p_body) const override;
virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override;
virtual real_t soft_body_get_linear_stiffness(RID p_body) override;
virtual void soft_body_set_angular_stiffness(RID p_body, real_t p_stiffness) override;
virtual real_t soft_body_get_angular_stiffness(RID p_body) override;
virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) override;
virtual real_t soft_body_get_volume_stiffness(RID p_body) override;
virtual real_t soft_body_get_linear_stiffness(RID p_body) const override;
virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override;
virtual real_t soft_body_get_pressure_coefficient(RID p_body) override;
virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) override;
virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) override;
virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override;
virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override;
virtual real_t soft_body_get_damping_coefficient(RID p_body) override;
virtual real_t soft_body_get_damping_coefficient(RID p_body) const override;
virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override;
virtual real_t soft_body_get_drag_coefficient(RID p_body) override;
virtual real_t soft_body_get_drag_coefficient(RID p_body) const override;
virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override;
virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) override;
virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const override;
virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override;
virtual void soft_body_remove_all_pinned_points(RID p_body) override;
virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override;
virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) override;
virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override;
/* JOINT API */

63
modules/bullet/soft_body_bullet.cpp
View file

@ -65,7 +65,7 @@ void SoftBodyBullet::on_enter_area(AreaBullet *p_area) {}
void SoftBodyBullet::on_exit_area(AreaBullet *p_area) {}
void SoftBodyBullet::update_rendering_server(SoftBodyRenderingServerHandler *p_rendering_server_handler) {
void SoftBodyBullet::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) {
if (!bt_soft_body) {
return;
}
@ -141,6 +141,24 @@ void SoftBodyBullet::set_soft_transform(const Transform &p_transform) {
move_all_nodes(p_transform);
}
AABB SoftBodyBullet::get_bounds() const {
if (!bt_soft_body) {
return AABB();
}
btVector3 aabb_min;
btVector3 aabb_max;
bt_soft_body->getAabb(aabb_min, aabb_max);
btVector3 size(aabb_max - aabb_min);
AABB aabb;
B_TO_G(aabb_min, aabb.position);
B_TO_G(size, aabb.size);
return aabb;
}
void SoftBodyBullet::move_all_nodes(const Transform &p_transform) {
if (!bt_soft_body) {
return;
@ -169,25 +187,6 @@ void SoftBodyBullet::get_node_position(int p_node_index, Vector3 &r_position) co
}
}
void SoftBodyBullet::get_node_offset(int p_node_index, Vector3 &r_offset) const {
if (soft_mesh.is_null()) {
return;
}
Array arrays = soft_mesh->surface_get_arrays(0);
Vector<Vector3> vertices(arrays[RS::ARRAY_VERTEX]);
if (0 <= p_node_index && vertices.size() > p_node_index) {
r_offset = vertices[p_node_index];
}
}
void SoftBodyBullet::get_node_offset(int p_node_index, btVector3 &r_offset) const {
Vector3 off;
get_node_offset(p_node_index, off);
G_TO_B(off, r_offset);
}
void SoftBodyBullet::set_node_mass(int node_index, btScalar p_mass) {
if (0 >= p_mass) {
pin_node(node_index);
@ -259,20 +258,6 @@ void SoftBodyBullet::set_linear_stiffness(real_t p_val) {
}
}
void SoftBodyBullet::set_angular_stiffness(real_t p_val) {
angular_stiffness = p_val;
if (bt_soft_body) {
mat0->m_kAST = angular_stiffness;
}
}
void SoftBodyBullet::set_volume_stiffness(real_t p_val) {
volume_stiffness = p_val;
if (bt_soft_body) {
mat0->m_kVST = volume_stiffness;
}
}
void SoftBodyBullet::set_simulation_precision(int p_val) {
simulation_precision = p_val;
if (bt_soft_body) {
@ -290,13 +275,6 @@ void SoftBodyBullet::set_pressure_coefficient(real_t p_val) {
}
}
void SoftBodyBullet::set_pose_matching_coefficient(real_t p_val) {
pose_matching_coefficient = p_val;
if (bt_soft_body) {
bt_soft_body->m_cfg.kMT = pose_matching_coefficient;
}
}
void SoftBodyBullet::set_damping_coefficient(real_t p_val) {
damping_coefficient = p_val;
if (bt_soft_body) {
@ -409,8 +387,6 @@ void SoftBodyBullet::setup_soft_body() {
bt_soft_body->generateBendingConstraints(2, mat0);
mat0->m_kLST = linear_stiffness;
mat0->m_kAST = angular_stiffness;
mat0->m_kVST = volume_stiffness;
// Clusters allow to have Soft vs Soft collision but doesn't work well right now
@ -430,7 +406,6 @@ void SoftBodyBullet::setup_soft_body() {
bt_soft_body->m_cfg.kDP = damping_coefficient;
bt_soft_body->m_cfg.kDG = drag_coefficient;
bt_soft_body->m_cfg.kPR = pressure_coefficient;
bt_soft_body->m_cfg.kMT = pose_matching_coefficient;
bt_soft_body->setTotalMass(total_mass);
btSoftBodyHelpers::ReoptimizeLinkOrder(bt_soft_body);

22
modules/bullet/soft_body_bullet.h
View file

@ -55,6 +55,8 @@
@author AndreaCatania
*/
class RenderingServerHandler;
class SoftBodyBullet : public CollisionObjectBullet {
private:
btSoftBody *bt_soft_body = nullptr;
@ -67,10 +69,7 @@ private:
int simulation_precision = 5;
real_t total_mass = 1.;
real_t linear_stiffness = 0.5; // [0,1]
real_t angular_stiffness = 0.5; // [0,1]
real_t volume_stiffness = 0.5; // [0,1]
real_t pressure_coefficient = 0.; // [-inf,+inf]
real_t pose_matching_coefficient = 0.; // [0,1]
real_t damping_coefficient = 0.01; // [0,1]
real_t drag_coefficient = 0.; // [0,1]
Vector<int> pinned_nodes;
@ -99,7 +98,7 @@ public:
_FORCE_INLINE_ btSoftBody *get_bt_soft_body() const { return bt_soft_body; }
void update_rendering_server(class SoftBodyRenderingServerHandler *p_rendering_server_handler);
void update_rendering_server(RenderingServerHandler *p_rendering_server_handler);
void set_soft_mesh(const Ref<Mesh> &p_mesh);
void destroy_soft_body();
@ -107,14 +106,12 @@ public:
// Special function. This function has bad performance
void set_soft_transform(const Transform &p_transform);
AABB get_bounds() const;
void move_all_nodes(const Transform &p_transform);
void set_node_position(int node_index, const Vector3 &p_global_position);
void set_node_position(int node_index, const btVector3 &p_global_position);
void get_node_position(int node_index, Vector3 &r_position) const;
// Heavy function, Please cache this info
void get_node_offset(int node_index, Vector3 &r_offset) const;
// Heavy function, Please cache this info
void get_node_offset(int node_index, btVector3 &r_offset) const;
void set_node_mass(int node_index, btScalar p_mass);
btScalar get_node_mass(int node_index) const;
@ -129,21 +126,12 @@ public:
void set_linear_stiffness(real_t p_val);
_FORCE_INLINE_ real_t get_linear_stiffness() const { return linear_stiffness; }
void set_angular_stiffness(real_t p_val);
_FORCE_INLINE_ real_t get_angular_stiffness() const { return angular_stiffness; }
void set_volume_stiffness(real_t p_val);
_FORCE_INLINE_ real_t get_volume_stiffness() const { return volume_stiffness; }
void set_simulation_precision(int p_val);
_FORCE_INLINE_ int get_simulation_precision() const { return simulation_precision; }
void set_pressure_coefficient(real_t p_val);
_FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; }
void set_pose_matching_coefficient(real_t p_val);
_FORCE_INLINE_ real_t get_pose_matching_coefficient() const { return pose_matching_coefficient; }
void set_damping_coefficient(real_t p_val);
_FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; }

70
scene/3d/soft_body_3d.cpp
View file

@ -37,7 +37,6 @@
#include "scene/3d/collision_object_3d.h"
#include "scene/3d/physics_body_3d.h"
#include "scene/3d/skeleton_3d.h"
#include "servers/physics_server_3d.h"
SoftBodyRenderingServerHandler::SoftBodyRenderingServerHandler() {}
@ -48,27 +47,28 @@ void SoftBodyRenderingServerHandler::prepare(RID p_mesh, int p_surface) {
mesh = p_mesh;
surface = p_surface;
#ifndef _MSC_VER
#warning Softbody is not working, needs to be redone considering that these functions no longer exist
#endif
#if 0
const uint32_t surface_format = RS::get_singleton()->mesh_surface_get_format(mesh, surface);
const int surface_vertex_len = RS::get_singleton()->mesh_surface_get_array_len(mesh, p_surface);
const int surface_index_len = RS::get_singleton()->mesh_surface_get_array_index_len(mesh, p_surface);
uint32_t surface_offsets[RS::ARRAY_MAX];
buffer = RS::get_singleton()->mesh_surface_get_array(mesh, surface);
stride = RS::get_singleton()->mesh_surface_make_offsets_from_format(surface_format, surface_vertex_len, surface_index_len, surface_offsets);
RS::SurfaceData surface_data = RS::get_singleton()->mesh_get_surface(mesh, surface);
uint32_t surface_offsets[RS::ARRAY_MAX];
uint32_t vertex_stride;
uint32_t attrib_stride;
uint32_t skin_stride;
RS::get_singleton()->mesh_surface_make_offsets_from_format(surface_data.format, surface_data.vertex_count, surface_data.index_count, surface_offsets, vertex_stride, attrib_stride, skin_stride);
buffer = surface_data.vertex_data;
stride = vertex_stride;
offset_vertices = surface_offsets[RS::ARRAY_VERTEX];
offset_normal = surface_offsets[RS::ARRAY_NORMAL];
#endif
}
void SoftBodyRenderingServerHandler::clear() {
if (mesh.is_valid()) {
buffer.resize(0);
}
buffer.resize(0);
stride = 0;
offset_vertices = 0;
offset_normal = 0;
surface = 0;
mesh = RID();
}
@ -77,7 +77,7 @@ void SoftBodyRenderingServerHandler::open() {
}
void SoftBodyRenderingServerHandler::close() {
//write_buffer.release();
write_buffer = nullptr;
}
void SoftBodyRenderingServerHandler::commit_changes() {
@ -309,6 +309,8 @@ void SoftBody3D::_notification(int p_what) {
}
void SoftBody3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_physics_rid"), &SoftBody3D::get_physics_rid);
ClassDB::bind_method(D_METHOD("set_collision_mask", "collision_mask"), &SoftBody3D::set_collision_mask);
ClassDB::bind_method(D_METHOD("get_collision_mask"), &SoftBody3D::get_collision_mask);
@ -337,18 +339,9 @@ void SoftBody3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_linear_stiffness", "linear_stiffness"), &SoftBody3D::set_linear_stiffness);
ClassDB::bind_method(D_METHOD("get_linear_stiffness"), &SoftBody3D::get_linear_stiffness);
ClassDB::bind_method(D_METHOD("set_angular_stiffness", "angular_stiffness"), &SoftBody3D::set_angular_stiffness);
ClassDB::bind_method(D_METHOD("get_angular_stiffness"), &SoftBody3D::get_angular_stiffness);
ClassDB::bind_method(D_METHOD("set_volume_stiffness", "volume_stiffness"), &SoftBody3D::set_volume_stiffness);
ClassDB::bind_method(D_METHOD("get_volume_stiffness"), &SoftBody3D::get_volume_stiffness);
ClassDB::bind_method(D_METHOD("set_pressure_coefficient", "pressure_coefficient"), &SoftBody3D::set_pressure_coefficient);
ClassDB::bind_method(D_METHOD("get_pressure_coefficient"), &SoftBody3D::get_pressure_coefficient);
ClassDB::bind_method(D_METHOD("set_pose_matching_coefficient", "pose_matching_coefficient"), &SoftBody3D::set_pose_matching_coefficient);
ClassDB::bind_method(D_METHOD("get_pose_matching_coefficient"), &SoftBody3D::get_pose_matching_coefficient);
ClassDB::bind_method(D_METHOD("set_damping_coefficient", "damping_coefficient"), &SoftBody3D::set_damping_coefficient);
ClassDB::bind_method(D_METHOD("get_damping_coefficient"), &SoftBody3D::get_damping_coefficient);
@ -366,12 +359,9 @@ void SoftBody3D::_bind_methods() {
ADD_PROPERTY(PropertyInfo(Variant::INT, "simulation_precision", PROPERTY_HINT_RANGE, "1,100,1"), "set_simulation_precision", "get_simulation_precision");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "total_mass", PROPERTY_HINT_RANGE, "0.01,10000,1"), "set_total_mass", "get_total_mass");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "linear_stiffness", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_linear_stiffness", "get_linear_stiffness");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "angular_stiffness", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_angular_stiffness", "get_angular_stiffness");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "volume_stiffness", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_volume_stiffness", "get_volume_stiffness");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "pressure_coefficient"), "set_pressure_coefficient", "get_pressure_coefficient");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "damping_coefficient", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_damping_coefficient", "get_damping_coefficient");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "drag_coefficient", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_drag_coefficient", "get_drag_coefficient");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "pose_matching_coefficient", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_pose_matching_coefficient", "get_pose_matching_coefficient");
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "ray_pickable"), "set_ray_pickable", "is_ray_pickable");
}
@ -612,34 +602,10 @@ real_t SoftBody3D::get_linear_stiffness() {
return PhysicsServer3D::get_singleton()->soft_body_get_linear_stiffness(physics_rid);
}
void SoftBody3D::set_angular_stiffness(real_t p_angular_stiffness) {
PhysicsServer3D::get_singleton()->soft_body_set_angular_stiffness(physics_rid, p_angular_stiffness);
}
real_t SoftBody3D::get_angular_stiffness() {
return PhysicsServer3D::get_singleton()->soft_body_get_angular_stiffness(physics_rid);
}
void SoftBody3D::set_volume_stiffness(real_t p_volume_stiffness) {
PhysicsServer3D::get_singleton()->soft_body_set_volume_stiffness(physics_rid, p_volume_stiffness);
}
real_t SoftBody3D::get_volume_stiffness() {
return PhysicsServer3D::get_singleton()->soft_body_get_volume_stiffness(physics_rid);
}
real_t SoftBody3D::get_pressure_coefficient() {
return PhysicsServer3D::get_singleton()->soft_body_get_pressure_coefficient(physics_rid);
}
void SoftBody3D::set_pose_matching_coefficient(real_t p_pose_matching_coefficient) {
PhysicsServer3D::get_singleton()->soft_body_set_pose_matching_coefficient(physics_rid, p_pose_matching_coefficient);
}
real_t SoftBody3D::get_pose_matching_coefficient() {
return PhysicsServer3D::get_singleton()->soft_body_get_pose_matching_coefficient(physics_rid);
}
void SoftBody3D::set_pressure_coefficient(real_t p_pressure_coefficient) {
PhysicsServer3D::get_singleton()->soft_body_set_pressure_coefficient(physics_rid, p_pressure_coefficient);
}

20
scene/3d/soft_body_3d.h
View file

@ -32,10 +32,11 @@
#define SOFT_PHYSICS_BODY_H
#include "scene/3d/mesh_instance_3d.h"
#include "servers/physics_server_3d.h"
class SoftBody3D;
class SoftBodyRenderingServerHandler {
class SoftBodyRenderingServerHandler : public RenderingServerHandler {
friend class SoftBody3D;
RID mesh;
@ -57,9 +58,9 @@ private:
void commit_changes();
public:
void set_vertex(int p_vertex_id, const void *p_vector3);
void set_normal(int p_vertex_id, const void *p_vector3);
void set_aabb(const AABB &p_aabb);
void set_vertex(int p_vertex_id, const void *p_vector3) override;
void set_normal(int p_vertex_id, const void *p_vector3) override;
void set_aabb(const AABB &p_aabb) override;
};
class SoftBody3D : public MeshInstance3D {
@ -122,6 +123,8 @@ public:
void prepare_physics_server();
void become_mesh_owner();
RID get_physics_rid() const { return physics_rid; }
void set_collision_mask(uint32_t p_mask);
uint32_t get_collision_mask() const;
@ -149,18 +152,9 @@ public:
void set_linear_stiffness(real_t p_linear_stiffness);
real_t get_linear_stiffness();
void set_angular_stiffness(real_t p_angular_stiffness);
real_t get_angular_stiffness();
void set_volume_stiffness(real_t p_volume_stiffness);
real_t get_volume_stiffness();
void set_pressure_coefficient(real_t p_pressure_coefficient);
real_t get_pressure_coefficient();
void set_pose_matching_coefficient(real_t p_pose_matching_coefficient);
real_t get_pose_matching_coefficient();
void set_damping_coefficient(real_t p_damping_coefficient);
real_t get_damping_coefficient();

6
servers/physics_3d/body_3d_sw.h
View file

@ -290,10 +290,10 @@ public:
void update_inertias();
_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
_FORCE_INLINE_ Vector3 get_inv_inertia() const { return _inv_inertia; }
_FORCE_INLINE_ Basis get_inv_inertia_tensor() const { return _inv_inertia_tensor; }
_FORCE_INLINE_ const Vector3 &get_inv_inertia() const { return _inv_inertia; }
_FORCE_INLINE_ const Basis &get_inv_inertia_tensor() const { return _inv_inertia_tensor; }
_FORCE_INLINE_ real_t get_friction() const { return friction; }
_FORCE_INLINE_ Vector3 get_gravity() const { return gravity; }
_FORCE_INLINE_ const Vector3 &get_gravity() const { return gravity; }
_FORCE_INLINE_ real_t get_bounce() const { return bounce; }
void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock);

312
servers/physics_3d/body_pair_3d_sw.cpp
View file

@ -49,12 +49,12 @@
#define MIN_VELOCITY 0.0001
#define MAX_BIAS_ROTATION (Math_PI / 8)
void BodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
void BodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
BodyPair3DSW *pair = (BodyPair3DSW *)p_userdata;
pair->contact_added_callback(p_point_A, p_point_B);
pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B);
}
void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B) {
void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
// check if we already have the contact
//Vector3 local_A = A->get_inv_transform().xform(p_point_A);
@ -73,6 +73,8 @@ void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, const Vector
contact.acc_bias_impulse = 0;
contact.acc_bias_impulse_center_of_mass = 0;
contact.acc_tangent_impulse = Vector3();
contact.index_A = p_index_A;
contact.index_B = p_index_B;
contact.local_A = local_A;
contact.local_B = local_B;
contact.normal = (p_point_A - p_point_B).normalized();
@ -456,7 +458,7 @@ void BodyPair3DSW::solve(real_t p_step) {
}
BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B) :
Constraint3DSW(_arr, 2) {
BodyContact3DSW(_arr, 2) {
A = p_A;
B = p_B;
shape_A = p_shape_A;
@ -472,3 +474,305 @@ BodyPair3DSW::~BodyPair3DSW() {
A->remove_constraint(this);
B->remove_constraint(this);
}
void BodySoftBodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
BodySoftBodyPair3DSW *pair = (BodySoftBodyPair3DSW *)p_userdata;
pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B);
}
void BodySoftBodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
Vector3 local_A = body->get_inv_transform().xform(p_point_A);
Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B);
Contact contact;
contact.index_A = p_index_A;
contact.index_B = p_index_B;
contact.acc_normal_impulse = 0;
contact.acc_bias_impulse = 0;
contact.acc_bias_impulse_center_of_mass = 0;
contact.acc_tangent_impulse = Vector3();
contact.local_A = local_A;
contact.local_B = local_B;
contact.normal = (p_point_A - p_point_B).normalized();
contact.mass_normal = 0;
// Attempt to determine if the contact will be reused.
real_t contact_recycle_radius = space->get_contact_recycle_radius();
uint32_t contact_count = contacts.size();
for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
Contact &c = contacts[contact_index];
if (c.index_B == p_index_B) {
if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) &&
c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) {
contact.acc_normal_impulse = c.acc_normal_impulse;
contact.acc_bias_impulse = c.acc_bias_impulse;
contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass;
contact.acc_tangent_impulse = c.acc_tangent_impulse;
}
c = contact;
return;
}
}
contacts.push_back(contact);
}
void BodySoftBodyPair3DSW::validate_contacts() {
// Make sure to erase contacts that are no longer valid.
const Transform &transform_A = body->get_transform();
real_t contact_max_separation = space->get_contact_max_separation();
uint32_t contact_count = contacts.size();
for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
Contact &c = contacts[contact_index];
Vector3 global_A = transform_A.xform(c.local_A);
Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
Vector3 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) {
// Contact no longer needed, remove.
if ((contact_index + 1) < contact_count) {
// Swap with the last one.
SWAP(c, contacts[contact_count - 1]);
}
contact_index--;
contact_count--;
}
}
contacts.resize(contact_count);
}
bool BodySoftBodyPair3DSW::setup(real_t p_step) {
if (!body->test_collision_mask(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) {
collided = false;
return false;
}
if (body->is_shape_set_as_disabled(body_shape)) {
collided = false;
return false;
}
const Transform &xform_Au = body->get_transform();
Transform xform_A = xform_Au * body->get_shape_transform(body_shape);
Transform xform_Bu = soft_body->get_transform();
Transform xform_B = xform_Bu * soft_body->get_shape_transform(0);
validate_contacts();
Shape3DSW *shape_A_ptr = body->get_shape(body_shape);
Shape3DSW *shape_B_ptr = soft_body->get_shape(0);
bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
this->collided = collided;
real_t max_penetration = space->get_contact_max_allowed_penetration();
real_t bias = (real_t)0.3;
if (shape_A_ptr->get_custom_bias()) {
bias = shape_A_ptr->get_custom_bias();
}
real_t inv_dt = 1.0 / p_step;
uint32_t contact_count = contacts.size();
for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
Contact &c = contacts[contact_index];
c.active = false;
real_t node_inv_mass = soft_body->get_node_inv_mass(c.index_B);
if (node_inv_mass == 0.0) {
continue;
}
Vector3 global_A = xform_Au.xform(c.local_A);
Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
real_t depth = c.normal.dot(global_A - global_B);
if (depth <= 0) {
continue;
}
c.active = true;
#ifdef DEBUG_ENABLED
if (space->is_debugging_contacts()) {
space->add_debug_contact(global_A);
space->add_debug_contact(global_B);
}
#endif
c.rA = global_A - xform_Au.origin - body->get_center_of_mass();
c.rB = global_B;
if (body->can_report_contacts()) {
Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity();
body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA);
}
if (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
body->set_active(true);
}
// Precompute normal mass, tangent mass, and bias.
Vector3 inertia_A = body->get_inv_inertia_tensor().xform(c.rA.cross(c.normal));
real_t kNormal = body->get_inv_mass() + node_inv_mass;
kNormal += c.normal.dot(inertia_A.cross(c.rA));
c.mass_normal = 1.0f / kNormal;
c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
c.depth = depth;
Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
body->apply_impulse(c.rA + body->get_center_of_mass(), -j_vec);
soft_body->apply_node_impulse(c.index_B, j_vec);
c.acc_bias_impulse = 0;
c.acc_bias_impulse_center_of_mass = 0;
c.bounce = body->get_bounce();
if (c.bounce) {
Vector3 crA = body->get_angular_velocity().cross(c.rA);
Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA;
// Normal impulse.
c.bounce = c.bounce * dv.dot(c.normal);
}
}
return true;
}
void BodySoftBodyPair3DSW::solve(real_t p_step) {
if (!collided) {
return;
}
uint32_t contact_count = contacts.size();
for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
Contact &c = contacts[contact_index];
if (!c.active) {
continue;
}
c.active = false;
// Bias impulse.
Vector3 crbA = body->get_biased_angular_velocity().cross(c.rA);
Vector3 dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA;
real_t vbn = dbv.dot(c.normal);
if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
real_t jbn = (-vbn + c.bias) * c.mass_normal;
real_t jbnOld = c.acc_bias_impulse;
c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
body->apply_bias_impulse(c.rA + body->get_center_of_mass(), -jb, MAX_BIAS_ROTATION / p_step);
soft_body->apply_node_bias_impulse(c.index_B, jb);
crbA = body->get_biased_angular_velocity().cross(c.rA);
dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA;
vbn = dbv.dot(c.normal);
if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
real_t jbn_com = (-vbn + c.bias) / (body->get_inv_mass() + soft_body->get_node_inv_mass(c.index_B));
real_t jbnOld_com = c.acc_bias_impulse_center_of_mass;
c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f);
Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
body->apply_bias_impulse(body->get_center_of_mass(), -jb_com, 0.0f);
soft_body->apply_node_bias_impulse(c.index_B, -jb_com);
}
c.active = true;
}
Vector3 crA = body->get_angular_velocity().cross(c.rA);
Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA;
// Normal impulse.
real_t vn = dv.dot(c.normal);
if (Math::abs(vn) > MIN_VELOCITY) {
real_t jn = -(c.bounce + vn) * c.mass_normal;
real_t jnOld = c.acc_normal_impulse;
c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
body->apply_impulse(c.rA + body->get_center_of_mass(), -j);
soft_body->apply_node_impulse(c.index_B, j);
c.active = true;
}
// Friction impulse.
real_t friction = body->get_friction();
Vector3 lvA = body->get_linear_velocity() + body->get_angular_velocity().cross(c.rA);
Vector3 lvB = soft_body->get_node_velocity(c.index_B);
Vector3 dtv = lvB - lvA;
real_t tn = c.normal.dot(dtv);
// Tangential velocity.
Vector3 tv = dtv - c.normal * tn;
real_t tvl = tv.length();
if (tvl > MIN_VELOCITY) {
tv /= tvl;
Vector3 temp1 = body->get_inv_inertia_tensor().xform(c.rA.cross(tv));
real_t t = -tvl /
(body->get_inv_mass() + soft_body->get_node_inv_mass(c.index_B) + tv.dot(temp1.cross(c.rA)));
Vector3 jt = t * tv;
Vector3 jtOld = c.acc_tangent_impulse;
c.acc_tangent_impulse += jt;
real_t fi_len = c.acc_tangent_impulse.length();
real_t jtMax = c.acc_normal_impulse * friction;
if (fi_len > CMP_EPSILON && fi_len > jtMax) {
c.acc_tangent_impulse *= jtMax / fi_len;
}
jt = c.acc_tangent_impulse - jtOld;
body->apply_impulse(c.rA + body->get_center_of_mass(), -jt);
soft_body->apply_node_impulse(c.index_B, jt);
c.active = true;
}
}
}
BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) {
body = p_A;
soft_body = p_B;
body_shape = p_shape_A;
space = p_A->get_space();
body->add_constraint(this, 0);
soft_body->add_constraint(this);
}
BodySoftBodyPair3DSW::~BodySoftBodyPair3DSW() {
body->remove_constraint(this);
soft_body->remove_constraint(this);
}

87
servers/physics_3d/body_pair_3d_sw.h
View file

@ -28,13 +28,45 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef BODY_PAIR_SW_H
#define BODY_PAIR_SW_H
#ifndef BODY_PAIR_3D_SW_H
#define BODY_PAIR_3D_SW_H
#include "body_3d_sw.h"
#include "constraint_3d_sw.h"
#include "core/templates/local_vector.h"
#include "soft_body_3d_sw.h"
class BodyPair3DSW : public Constraint3DSW {
class BodyContact3DSW : public Constraint3DSW {
protected:
struct Contact {
Vector3 position;
Vector3 normal;
int index_A, index_B;
Vector3 local_A, local_B;
real_t acc_normal_impulse; // accumulated normal impulse (Pn)
Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt)
real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb)
real_t acc_bias_impulse_center_of_mass; // accumulated normal impulse for position bias applied to com
real_t mass_normal;
real_t bias;
real_t bounce;
real_t depth;
bool active;
Vector3 rA, rB; // Offset in world orientation with respect to center of mass
};
Vector3 sep_axis;
bool collided;
Space3DSW *space;
BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) :
Constraint3DSW(p_body_ptr, p_body_count) {
}
};
class BodyPair3DSW : public BodyContact3DSW {
enum {
MAX_CONTACTS = 4
};
@ -51,39 +83,18 @@ class BodyPair3DSW : public Constraint3DSW {
int shape_A;
int shape_B;
struct Contact {
Vector3 position;
Vector3 normal;
Vector3 local_A, local_B;
real_t acc_normal_impulse; // accumulated normal impulse (Pn)
Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt)
real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb)
real_t acc_bias_impulse_center_of_mass; // accumulated normal impulse for position bias applied to com
real_t mass_normal;
real_t bias;
real_t bounce;
real_t depth;
bool active;
Vector3 rA, rB; // Offset in world orientation with respect to center of mass
};
Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection
Vector3 sep_axis;
Contact contacts[MAX_CONTACTS];
int contact_count;
bool collided;
static void _contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata);
static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
void contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B);
void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B);
void validate_contacts();
bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B);
Space3DSW *space;
public:
bool setup(real_t p_step);
void solve(real_t p_step);
@ -92,4 +103,26 @@ public:
~BodyPair3DSW();
};
#endif // BODY_PAIR__SW_H
class BodySoftBodyPair3DSW : public BodyContact3DSW {
Body3DSW *body;
SoftBody3DSW *soft_body;
int body_shape;
LocalVector<Contact> contacts;
static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B);
void validate_contacts();
public:
bool setup(real_t p_step);
void solve(real_t p_step);
BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B);
~BodySoftBodyPair3DSW();
};
#endif // BODY_PAIR_3D_SW_H

7
servers/physics_3d/collision_object_3d_sw.h
View file

@ -48,7 +48,8 @@ class CollisionObject3DSW : public ShapeOwner3DSW {
public:
enum Type {
TYPE_AREA,
TYPE_BODY
TYPE_BODY,
TYPE_SOFT_BODY,
};
private:
@ -129,8 +130,8 @@ public:
_FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
_FORCE_INLINE_ real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; }
_FORCE_INLINE_ Transform get_transform() const { return transform; }
_FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; }
_FORCE_INLINE_ const Transform &get_transform() const { return transform; }
_FORCE_INLINE_ const Transform &get_inv_transform() const { return inv_transform; }
_FORCE_INLINE_ Space3DSW *get_space() const { return space; }
_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }

6
servers/physics_3d/collision_solver_3d_sat.cpp
View file

@ -74,9 +74,9 @@ struct _CollectorCallback {
_FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) {
if (swap) {
callback(p_point_B, p_point_A, userdata);
callback(p_point_B, 0, p_point_A, 0, userdata);
} else {
callback(p_point_A, p_point_B, userdata);
callback(p_point_A, 0, p_point_B, 0, userdata);
}
}
};
@ -685,7 +685,7 @@ public:
return true;
}
static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
SeparatorAxisTest<ShapeA, ShapeB, withMargin> *separator = (SeparatorAxisTest<ShapeA, ShapeB, withMargin> *)p_userdata;
Vector3 axis = (p_point_B - p_point_A);
real_t depth = axis.length();

158
servers/physics_3d/collision_solver_3d_sw.cpp
View file

@ -30,6 +30,7 @@
#include "collision_solver_3d_sw.h"
#include "collision_solver_3d_sat.h"
#include "soft_body_3d_sw.h"
#include "gjk_epa.h"
@ -78,9 +79,9 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T
if (p_result_callback) {
if (p_swap_result) {
p_result_callback(supports[i], support_A, p_userdata);
p_result_callback(supports[i], 0, support_A, 0, p_userdata);
} else {
p_result_callback(support_A, supports[i], p_userdata);
p_result_callback(support_A, 0, supports[i], 0, p_userdata);
}
}
}
@ -113,14 +114,148 @@ bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform
if (p_result_callback) {
if (p_swap_result) {
p_result_callback(support_B, support_A, p_userdata);
p_result_callback(support_B, 0, support_A, 0, p_userdata);
} else {
p_result_callback(support_A, support_B, p_userdata);
p_result_callback(support_A, 0, support_B, 0, p_userdata);
}
}
return true;
}
struct _SoftBodyContactCollisionInfo {
int node_index = 0;
CollisionSolver3DSW::CallbackResult result_callback = nullptr;
void *userdata = nullptr;
bool swap_result = false;
int contact_count = 0;
};
void CollisionSolver3DSW::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
_SoftBodyContactCollisionInfo &cinfo = *(_SoftBodyContactCollisionInfo *)(p_userdata);
++cinfo.contact_count;
if (cinfo.swap_result) {
cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata);
} else {
cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, cinfo.userdata);
}
}
struct _SoftBodyQueryInfo {
SoftBody3DSW *soft_body = nullptr;
const Shape3DSW *shape_A = nullptr;
const Shape3DSW *shape_B = nullptr;
Transform transform_A;
Transform node_transform;
_SoftBodyContactCollisionInfo contact_info;
#ifdef DEBUG_ENABLED
int node_query_count = 0;
int convex_query_count = 0;
#endif
};
bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) {
_SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata);
Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index);
Transform transform_B;
transform_B.origin = query_cinfo.node_transform.xform(node_position);
query_cinfo.contact_info.node_index = p_node_index;
solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info);
#ifdef DEBUG_ENABLED
++query_cinfo.node_query_count;
#endif
// Continue with the query.
return false;
}
void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) {
_SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata);
query_cinfo.shape_A = p_convex;
// Calculate AABB for internal soft body query (in world space).
AABB shape_aabb;
for (int i = 0; i < 3; i++) {
Vector3 axis;
axis[i] = 1.0;
real_t smin, smax;
p_convex->project_range(axis, query_cinfo.transform_A, smin, smax);
shape_aabb.position[i] = smin;
shape_aabb.size[i] = smax - smin;
}
shape_aabb.grow_by(query_cinfo.soft_body->get_collision_margin());
query_cinfo.soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo);
#ifdef DEBUG_ENABLED
++query_cinfo.convex_query_count;
#endif
}
bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
const SoftBodyShape3DSW *soft_body_shape_B = static_cast<const SoftBodyShape3DSW *>(p_shape_B);
SoftBody3DSW *soft_body = soft_body_shape_B->get_soft_body();
const Transform &world_to_local = soft_body->get_inv_transform();
const real_t collision_margin = soft_body->get_collision_margin();
SphereShape3DSW sphere_shape;
sphere_shape.set_data(collision_margin);
_SoftBodyQueryInfo query_cinfo;
query_cinfo.contact_info.result_callback = p_result_callback;
query_cinfo.contact_info.userdata = p_userdata;
query_cinfo.contact_info.swap_result = p_swap_result;
query_cinfo.soft_body = soft_body;
query_cinfo.node_transform = p_transform_B * world_to_local;
query_cinfo.shape_A = p_shape_A;
query_cinfo.transform_A = p_transform_A;
query_cinfo.shape_B = &sphere_shape;
if (p_shape_A->is_concave()) {
// In case of concave shape, query convex shapes first.
const ConcaveShape3DSW *concave_shape_A = static_cast<const ConcaveShape3DSW *>(p_shape_A);
AABB soft_body_aabb = soft_body->get_bounds();
soft_body_aabb.grow_by(collision_margin);
// Calculate AABB for internal concave shape query (in local space).
AABB local_aabb;
for (int i = 0; i < 3; i++) {
Vector3 axis(p_transform_A.basis.get_axis(i));
real_t axis_scale = 1.0 / axis.length();
real_t smin = soft_body_aabb.position[i];
real_t smax = smin + soft_body_aabb.size[i];
smin *= axis_scale;
smax *= axis_scale;
local_aabb.position[i] = smin;
local_aabb.size[i] = smax - smin;
}
concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo);
} else {
AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb());
shape_aabb.grow_by(collision_margin);
soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo);
}
return (query_cinfo.contact_info.contact_count > 0);
}
struct _ConcaveCollisionInfo {
const Transform *transform_A;
const Shape3DSW *shape_A;
@ -215,6 +350,9 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
if (type_B == PhysicsServer3D::SHAPE_RAY) {
return false;
}
if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) {
return false;
}
if (swap) {
return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
@ -233,6 +371,18 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
}
} else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) {
if (type_A == PhysicsServer3D::SHAPE_SOFT_BODY) {
// Soft Body / Soft Body not supported.
return false;
}
if (swap) {
return solve_soft_body(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
} else {
return solve_soft_body(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
}
} else if (concave_B) {
if (concave_A) {
return false;

6
servers/physics_3d/collision_solver_3d_sw.h
View file

@ -35,12 +35,16 @@
class CollisionSolver3DSW {
public:
typedef void (*CallbackResult)(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata);
typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
private:
static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata);
static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
static void soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex);
static void concave_callback(void *p_userdata, Shape3DSW *p_convex);
static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_soft_body(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_concave(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0);
static void concave_distance_callback(void *p_userdata, Shape3DSW *p_convex);
static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);

4
servers/physics_3d/gjk_epa.cpp
View file

@ -1011,9 +1011,9 @@ bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &
if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) {
if (p_result_callback) {
if (p_swap) {
p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata);
p_result_callback(res.witnesses[1], 0, res.witnesses[0], 0, p_userdata);
} else {
p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata);
p_result_callback(res.witnesses[0], 0, res.witnesses[1], 0, p_userdata);
}
}
return true;

281
servers/physics_3d/physics_server_3d_sw.cpp
View file

@ -611,9 +611,18 @@ uint32_t PhysicsServer3DSW::body_get_collision_mask(RID p_body) const {
void PhysicsServer3DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) {
Body3DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
if (body) {
body->set_instance_id(p_id);
return;
}
body->set_instance_id(p_id);
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
if (soft_body) {
soft_body->set_instance_id(p_id);
return;
}
ERR_FAIL_MSG("Invalid ID.");
};
ObjectID PhysicsServer3DSW::body_get_object_instance_id(RID p_body) const {
@ -893,6 +902,266 @@ PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) {
return direct_state;
}
/* SOFT BODY */
RID PhysicsServer3DSW::soft_body_create() {
SoftBody3DSW *soft_body = memnew(SoftBody3DSW);
RID rid = soft_body_owner.make_rid(soft_body);
soft_body->set_self(rid);
return rid;
}
void PhysicsServer3DSW::soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->update_rendering_server(p_rendering_server_handler);
}
void PhysicsServer3DSW::soft_body_set_space(RID p_body, RID p_space) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
Space3DSW *space = nullptr;
if (p_space.is_valid()) {
space = space_owner.getornull(p_space);
ERR_FAIL_COND(!space);
}
if (soft_body->get_space() == space) {
return;
}
soft_body->set_space(space);
}
RID PhysicsServer3DSW::soft_body_get_space(RID p_body) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, RID());
Space3DSW *space = soft_body->get_space();
if (!space) {
return RID();
}
return space->get_self();
}
void PhysicsServer3DSW::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_collision_layer(p_layer);
}
uint32_t PhysicsServer3DSW::soft_body_get_collision_layer(RID p_body) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, 0);
return soft_body->get_collision_layer();
}
void PhysicsServer3DSW::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_collision_mask(p_mask);
}
uint32_t PhysicsServer3DSW::soft_body_get_collision_mask(RID p_body) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, 0);
return soft_body->get_collision_mask();
}
void PhysicsServer3DSW::soft_body_add_collision_exception(RID p_body, RID p_body_b) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->add_exception(p_body_b);
}
void PhysicsServer3DSW::soft_body_remove_collision_exception(RID p_body, RID p_body_b) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->remove_exception(p_body_b);
}
void PhysicsServer3DSW::soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
for (int i = 0; i < soft_body->get_exceptions().size(); i++) {
p_exceptions->push_back(soft_body->get_exceptions()[i]);
}
}
void PhysicsServer3DSW::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_state(p_state, p_variant);
}
Variant PhysicsServer3DSW::soft_body_get_state(RID p_body, BodyState p_state) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, Variant());
return soft_body->get_state(p_state);
}
void PhysicsServer3DSW::soft_body_set_transform(RID p_body, const Transform &p_transform) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_state(BODY_STATE_TRANSFORM, p_transform);
}
void PhysicsServer3DSW::soft_body_set_ray_pickable(RID p_body, bool p_enable) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_ray_pickable(p_enable);
}
void PhysicsServer3DSW::soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_iteration_count(p_simulation_precision);
}
int PhysicsServer3DSW::soft_body_get_simulation_precision(RID p_body) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, 0.f);
return soft_body->get_iteration_count();
}
void PhysicsServer3DSW::soft_body_set_total_mass(RID p_body, real_t p_total_mass) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_total_mass(p_total_mass);
}
real_t PhysicsServer3DSW::soft_body_get_total_mass(RID p_body) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, 0.f);
return soft_body->get_total_mass();
}
void PhysicsServer3DSW::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_linear_stiffness(p_stiffness);
}
real_t PhysicsServer3DSW::soft_body_get_linear_stiffness(RID p_body) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, 0.f);
return soft_body->get_linear_stiffness();
}
void PhysicsServer3DSW::soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_pressure_coefficient(p_pressure_coefficient);
}
real_t PhysicsServer3DSW::soft_body_get_pressure_coefficient(RID p_body) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, 0.f);
return soft_body->get_pressure_coefficient();
}
void PhysicsServer3DSW::soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_damping_coefficient(p_damping_coefficient);
}
real_t PhysicsServer3DSW::soft_body_get_damping_coefficient(RID p_body) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, 0.f);
return soft_body->get_damping_coefficient();
}
void PhysicsServer3DSW::soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_drag_coefficient(p_drag_coefficient);
}
real_t PhysicsServer3DSW::soft_body_get_drag_coefficient(RID p_body) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, 0.f);
return soft_body->get_drag_coefficient();
}
void PhysicsServer3DSW::soft_body_set_mesh(RID p_body, const REF &p_mesh) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_mesh(p_mesh);
}
AABB PhysicsServer3DSW::soft_body_get_bounds(RID p_body) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, AABB());
return soft_body->get_bounds();
}
void PhysicsServer3DSW::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->set_vertex_position(p_point_index, p_global_position);
}
Vector3 PhysicsServer3DSW::soft_body_get_point_global_position(RID p_body, int p_point_index) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, Vector3());
return soft_body->get_vertex_position(p_point_index);
}
void PhysicsServer3DSW::soft_body_remove_all_pinned_points(RID p_body) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
soft_body->unpin_all_vertices();
}
void PhysicsServer3DSW::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND(!soft_body);
if (p_pin) {
soft_body->pin_vertex(p_point_index);
} else {
soft_body->unpin_vertex(p_point_index);
}
}
bool PhysicsServer3DSW::soft_body_is_point_pinned(RID p_body, int p_point_index) const {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
ERR_FAIL_COND_V(!soft_body, false);
return soft_body->is_vertex_pinned(p_point_index);
}
/* JOINT API */
RID PhysicsServer3DSW::joint_create() {
@ -1278,7 +1547,13 @@ void PhysicsServer3DSW::free(RID p_rid) {
body_owner.free(p_rid);
memdelete(body);
} else if (soft_body_owner.owns(p_rid)) {
SoftBody3DSW *soft_body = soft_body_owner.getornull(p_rid);
soft_body->set_space(nullptr);
soft_body_owner.free(p_rid);
memdelete(soft_body);
} else if (area_owner.owns(p_rid)) {
Area3DSW *area = area_owner.getornull(p_rid);
@ -1444,7 +1719,7 @@ void PhysicsServer3DSW::_update_shapes() {
}
}
void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
CollCbkData *cbk = (CollCbkData *)p_userdata;
if (cbk->max == 0) {

81
servers/physics_3d/physics_server_3d_sw.h
View file

@ -63,6 +63,7 @@ class PhysicsServer3DSW : public PhysicsServer3D {
mutable RID_PtrOwner<Space3DSW, true> space_owner;
mutable RID_PtrOwner<Area3DSW, true> area_owner;
mutable RID_PtrOwner<Body3DSW, true> body_owner;
mutable RID_PtrOwner<SoftBody3DSW, true> soft_body_owner;
mutable RID_PtrOwner<Joint3DSW, true> joint_owner;
//void _clear_query(QuerySW *p_query);
@ -79,7 +80,7 @@ public:
Vector3 *ptr;
};
static void _shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata);
static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
virtual RID plane_shape_create() override;
virtual RID ray_shape_create() override;
@ -252,68 +253,58 @@ public:
/* SOFT BODY */
virtual RID soft_body_create() override { return RID(); }
virtual RID soft_body_create() override;
virtual void soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) override {}
virtual void soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) override;
virtual void soft_body_set_space(RID p_body, RID p_space) override {}
virtual RID soft_body_get_space(RID p_body) const override { return RID(); }
virtual void soft_body_set_space(RID p_body, RID p_space) override;
virtual RID soft_body_get_space(RID p_body) const override;
virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override {}
virtual uint32_t soft_body_get_collision_layer(RID p_body) const override { return 0; }
virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override;
virtual uint32_t soft_body_get_collision_layer(RID p_body) const override;
virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override {}
virtual uint32_t soft_body_get_collision_mask(RID p_body) const override { return 0; }
virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override;
virtual uint32_t soft_body_get_collision_mask(RID p_body) const override;
virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override {}
virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override {}
virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override {}
virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override;
virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override;
virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override;
virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override {}
virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override { return Variant(); }
virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override;
virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) override {}
virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const override { return Vector3(); }
virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) override;
virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override {}
virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override;
virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override {}
virtual int soft_body_get_simulation_precision(RID p_body) const override { return 0; }
virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override;
virtual int soft_body_get_simulation_precision(RID p_body) const override;
virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override {}
virtual real_t soft_body_get_total_mass(RID p_body) const override { return 0.; }
virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override;
virtual real_t soft_body_get_total_mass(RID p_body) const override;
virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override {}
virtual real_t soft_body_get_linear_stiffness(RID p_body) const override { return 0.; }
virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override;
virtual real_t soft_body_get_linear_stiffness(RID p_body) const override;
virtual void soft_body_set_angular_stiffness(RID p_body, real_t p_stiffness) override {}
virtual real_t soft_body_get_angular_stiffness(RID p_body) const override { return 0.; }
virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override;
virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override;
virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) override {}
virtual real_t soft_body_get_volume_stiffness(RID p_body) const override { return 0.; }
virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override;
virtual real_t soft_body_get_damping_coefficient(RID p_body) const override;
virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override {}
virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override { return 0.; }
virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override;
virtual real_t soft_body_get_drag_coefficient(RID p_body) const override;
virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) override {}
virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) const override { return 0.; }
virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) override;
virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override {}
virtual real_t soft_body_get_damping_coefficient(RID p_body) const override { return 0.; }
virtual AABB soft_body_get_bounds(RID p_body) const override;
virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override {}
virtual real_t soft_body_get_drag_coefficient(RID p_body) const override { return 0.; }
virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override;
virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override;
virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) override {}
virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override {}
virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override { return Vector3(); }
virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const override { return Vector3(); }
virtual void soft_body_remove_all_pinned_points(RID p_body) override {}
virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override {}
virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override { return false; }
virtual void soft_body_remove_all_pinned_points(RID p_body) override;
virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override;
virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override;
/* JOINT API */

15
servers/physics_3d/physics_server_3d_wrap_mt.h
View file

@ -273,7 +273,7 @@ public:
FUNCRID(soft_body)
FUNC2(soft_body_update_rendering_server, RID, class SoftBodyRenderingServerHandler *)
FUNC2(soft_body_update_rendering_server, RID, class RenderingServerHandler *)
FUNC2(soft_body_set_space, RID, RID)
FUNC1RC(RID, soft_body_get_space, RID)
@ -294,7 +294,6 @@ public:
FUNC2RC(Variant, soft_body_get_state, RID, BodyState);
FUNC2(soft_body_set_transform, RID, const Transform &);
FUNC2RC(Vector3, soft_body_get_vertex_position, RID, int);
FUNC2(soft_body_set_simulation_precision, RID, int);
FUNC1RC(int, soft_body_get_simulation_precision, RID);
@ -305,18 +304,9 @@ public:
FUNC2(soft_body_set_linear_stiffness, RID, real_t);
FUNC1RC(real_t, soft_body_get_linear_stiffness, RID);
FUNC2(soft_body_set_angular_stiffness, RID, real_t);
FUNC1RC(real_t, soft_body_get_angular_stiffness, RID);
FUNC2(soft_body_set_volume_stiffness, RID, real_t);
FUNC1RC(real_t, soft_body_get_volume_stiffness, RID);
FUNC2(soft_body_set_pressure_coefficient, RID, real_t);
FUNC1RC(real_t, soft_body_get_pressure_coefficient, RID);
FUNC2(soft_body_set_pose_matching_coefficient, RID, real_t);
FUNC1RC(real_t, soft_body_get_pose_matching_coefficient, RID);
FUNC2(soft_body_set_damping_coefficient, RID, real_t);
FUNC1RC(real_t, soft_body_get_damping_coefficient, RID);
@ -325,9 +315,10 @@ public:
FUNC2(soft_body_set_mesh, RID, const REF &);
FUNC1RC(AABB, soft_body_get_bounds, RID);
FUNC3(soft_body_move_point, RID, int, const Vector3 &);
FUNC2RC(Vector3, soft_body_get_point_global_position, RID, int);
FUNC2RC(Vector3, soft_body_get_point_offset, RID, int);
FUNC1(soft_body_remove_all_pinned_points, RID);
FUNC3(soft_body_pin_point, RID, int, bool);

1221
servers/physics_3d/soft_body_3d_sw.cpp Normal file
View file

File diff suppressed because it is too large Load diff

247
servers/physics_3d/soft_body_3d_sw.h Normal file
View file

@ -0,0 +1,247 @@
/*************************************************************************/
/* soft_body_3d_sw.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef SOFT_BODY_3D_SW_H
#define SOFT_BODY_3D_SW_H
#include "collision_object_3d_sw.h"
#include "core/math/aabb.h"
#include "core/math/dynamic_bvh.h"
#include "core/math/vector3.h"
#include "core/templates/local_vector.h"
#include "core/templates/set.h"
#include "core/templates/vset.h"
#include "scene/resources/mesh.h"
class Constraint3DSW;
class SoftBody3DSW : public CollisionObject3DSW {
Ref<Mesh> soft_mesh;
struct Node {
Vector3 s; // Source position
Vector3 x; // Position
Vector3 q; // Previous step position/Test position
Vector3 f; // Force accumulator
Vector3 v; // Velocity
Vector3 bv; // Biased Velocity
Vector3 n; // Normal
real_t area = 0.0; // Area
real_t im = 0.0; // 1/mass
DynamicBVH::ID leaf; // Leaf data
uint32_t index = 0;
};
struct Link {
Vector3 c3; // gradient
Node *n[2] = { nullptr, nullptr }; // Node pointers
real_t rl = 0.0; // Rest length
real_t c0 = 0.0; // (ima+imb)*kLST
real_t c1 = 0.0; // rl^2
real_t c2 = 0.0; // |gradient|^2/c0
};
struct Face {
Node *n[3] = { nullptr, nullptr, nullptr }; // Node pointers
Vector3 normal; // Normal
real_t ra = 0.0; // Rest area
DynamicBVH::ID leaf; // Leaf data
uint32_t index = 0;
};
LocalVector<Node> nodes;
LocalVector<Link> links;
LocalVector<Face> faces;
DynamicBVH node_tree;
DynamicBVH face_tree;
LocalVector<uint32_t> map_visual_to_physics;
AABB bounds;
real_t collision_margin = 0.05;
real_t total_mass = 1.0;
real_t inv_total_mass = 1.0;
int iteration_count = 5;
real_t linear_stiffness = 0.5; // [0,1]
real_t pressure_coefficient = 0.0; // [-inf,+inf]
real_t damping_coefficient = 0.01; // [0,1]
real_t drag_coefficient = 0.0; // [0,1]
LocalVector<int> pinned_vertices;
SelfList<SoftBody3DSW> active_list;
Set<Constraint3DSW *> constraints;
VSet<RID> exceptions;
public:
SoftBody3DSW();
const AABB &get_bounds() const { return bounds; }
void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant);
Variant get_state(PhysicsServer3D::BodyState p_state) const;
_FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); }
_FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); }
_FORCE_INLINE_ const Set<Constraint3DSW *> &get_constraints() const { return constraints; }
_FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
_FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
_FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); }
_FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); }
_FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
virtual void set_space(Space3DSW *p_space);
void set_mesh(const Ref<Mesh> &p_mesh);
void update_rendering_server(RenderingServerHandler *p_rendering_server_handler);
Vector3 get_vertex_position(int p_index) const;
void set_vertex_position(int p_index, const Vector3 &p_position);
void pin_vertex(int p_index);
void unpin_vertex(int p_index);
void unpin_all_vertices();
bool is_vertex_pinned(int p_index) const;
uint32_t get_node_count() const;
real_t get_node_inv_mass(uint32_t p_node_index) const;
Vector3 get_node_position(uint32_t p_node_index) const;
Vector3 get_node_velocity(uint32_t p_node_index) const;
Vector3 get_node_biased_velocity(uint32_t p_node_index) const;
void apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse);
void apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse);
uint32_t get_face_count() const;
void get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const;
Vector3 get_face_normal(uint32_t p_face_index) const;
void set_iteration_count(int p_val);
_FORCE_INLINE_ real_t get_iteration_count() const { return iteration_count; }
void set_total_mass(real_t p_val);
_FORCE_INLINE_ real_t get_total_mass() const { return total_mass; }
_FORCE_INLINE_ real_t get_total_inv_mass() const { return inv_total_mass; }
void set_collision_margin(real_t p_val);
_FORCE_INLINE_ real_t get_collision_margin() const { return collision_margin; }
void set_linear_stiffness(real_t p_val);
_FORCE_INLINE_ real_t get_linear_stiffness() const { return linear_stiffness; }
void set_pressure_coefficient(real_t p_val);
_FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; }
void set_damping_coefficient(real_t p_val);
_FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; }
void set_drag_coefficient(real_t p_val);
_FORCE_INLINE_ real_t get_drag_coefficient() const { return drag_coefficient; }
void predict_motion(real_t p_delta);
void solve_constraints(real_t p_delta);
_FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return ((Node *)p_node)->index; }
_FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return ((Face *)p_face)->index; }
// Return true to stop the query.
// p_index is the node index for AABB query, face index for Ray query.
typedef bool (*QueryResultCallback)(uint32_t p_index, void *p_userdata);
void query_aabb(const AABB &p_aabb, QueryResultCallback p_result_callback, void *p_userdata);
void query_ray(const Vector3 &p_from, const Vector3 &p_to, QueryResultCallback p_result_callback, void *p_userdata);
protected:
virtual void _shapes_changed();
private:
void update_normals();
void update_bounds();
void update_constants();
void update_area();
void reset_link_rest_lengths();
void update_link_constants();
void apply_nodes_transform(const Transform &p_transform);
void add_velocity(const Vector3 &p_velocity);
void apply_forces();
bool create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices);
void generate_bending_constraints(int p_distance);
void reoptimize_link_order();
void append_link(uint32_t p_node1, uint32_t p_node2);
void append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3);
void solve_links(real_t kst, real_t ti);
void initialize_face_tree();
void update_face_tree(real_t p_delta);
void initialize_shape(bool p_force_move = true);
void deinitialize_shape();
void destroy();
};
class SoftBodyShape3DSW : public Shape3DSW {
SoftBody3DSW *soft_body = nullptr;
public:
SoftBody3DSW *get_soft_body() const { return soft_body; }
virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SOFT_BODY; }
virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { r_min = r_max = 0.0; }
virtual Vector3 get_support(const Vector3 &p_normal) const { return Vector3(); }
virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; }
virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
virtual bool intersect_point(const Vector3 &p_point) const;
virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
virtual Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); }
virtual void set_data(const Variant &p_data) {}
virtual Variant get_data() const { return Variant(); }
void update_bounds();
SoftBodyShape3DSW(SoftBody3DSW *p_soft_body);
~SoftBodyShape3DSW() {}
};
#endif // SOFT_BODY_3D_SW_H

33
servers/physics_3d/space_3d_sw.cpp
View file

@ -47,6 +47,10 @@ _FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint
return false;
}
if (p_object->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY && !p_collide_with_bodies) {
return false;
}
return true;
}
@ -407,7 +411,7 @@ struct _RestCallbackData {
real_t min_allowed_depth;
};
static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
_RestCallbackData *rd = (_RestCallbackData *)p_userdata;
Vector3 contact_rel = p_point_B - p_point_A;
@ -544,6 +548,8 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) {
keep = false;
} else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_AREA) {
keep = false;
} else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY) {
keep = false;
} else if ((static_cast<Body3DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) {
keep = false;
} else if (static_cast<Body3DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
@ -1054,14 +1060,23 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll
Area3DSW *area_b = static_cast<Area3DSW *>(B);
Area2Pair3DSW *area2_pair = memnew(Area2Pair3DSW(area_b, p_subindex_B, area, p_subindex_A));
return area2_pair;
} else if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) {
// Area/Soft Body, not supported.
} else {
Body3DSW *body = static_cast<Body3DSW *>(B);
AreaPair3DSW *area_pair = memnew(AreaPair3DSW(body, p_subindex_B, area, p_subindex_A));
return area_pair;
}
} else if (type_A == CollisionObject3DSW::TYPE_BODY) {
if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) {
BodySoftBodyPair3DSW *soft_pair = memnew(BodySoftBodyPair3DSW((Body3DSW *)A, p_subindex_A, (SoftBody3DSW *)B));
return soft_pair;
} else {
BodyPair3DSW *b = memnew(BodyPair3DSW((Body3DSW *)A, p_subindex_A, (Body3DSW *)B, p_subindex_B));
return b;
}
} else {
BodyPair3DSW *b = memnew(BodyPair3DSW((Body3DSW *)A, p_subindex_A, (Body3DSW *)B, p_subindex_B));
return b;
// Soft Body/Soft Body, not supported.
}
return nullptr;
@ -1144,6 +1159,18 @@ const SelfList<Area3DSW>::List &Space3DSW::get_moved_area_list() const {
return area_moved_list;
}
const SelfList<SoftBody3DSW>::List &Space3DSW::get_active_soft_body_list() const {
return active_soft_body_list;
}
void Space3DSW::soft_body_add_to_active_list(SelfList<SoftBody3DSW> *p_soft_body) {
active_soft_body_list.add(p_soft_body);
}
void Space3DSW::soft_body_remove_from_active_list(SelfList<SoftBody3DSW> *p_soft_body) {
active_soft_body_list.remove(p_soft_body);
}
void Space3DSW::call_queries() {
while (state_query_list.first()) {
Body3DSW *b = state_query_list.first()->self();

6
servers/physics_3d/space_3d_sw.h
View file

@ -40,6 +40,7 @@
#include "core/config/project_settings.h"
#include "core/templates/hash_map.h"
#include "core/typedefs.h"
#include "soft_body_3d_sw.h"
class PhysicsDirectSpaceState3DSW : public PhysicsDirectSpaceState3D {
GDCLASS(PhysicsDirectSpaceState3DSW, PhysicsDirectSpaceState3D);
@ -82,6 +83,7 @@ private:
SelfList<Body3DSW>::List state_query_list;
SelfList<Area3DSW>::List monitor_query_list;
SelfList<Area3DSW>::List area_moved_list;
SelfList<SoftBody3DSW>::List active_soft_body_list;
static void *_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self);
static void _broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self);
@ -145,6 +147,10 @@ public:
void area_remove_from_moved_list(SelfList<Area3DSW> *p_area);
const SelfList<Area3DSW>::List &get_moved_area_list() const;
const SelfList<SoftBody3DSW>::List &get_active_soft_body_list() const;
void soft_body_add_to_active_list(SelfList<SoftBody3DSW> *p_soft_body);
void soft_body_remove_from_active_list(SelfList<SoftBody3DSW> *p_soft_body);
BroadPhase3DSW *get_broadphase();
void add_object(CollisionObject3DSW *p_object);

33
servers/physics_3d/step_3d_sw.cpp
View file

@ -146,6 +146,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
const SelfList<Body3DSW>::List *body_list = &p_space->get_active_body_list();
const SelfList<SoftBody3DSW>::List *soft_body_list = &p_space->get_active_soft_body_list();
/* INTEGRATE FORCES */
uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec();
@ -160,6 +162,15 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
active_count++;
}
/* UPDATE SOFT BODY MOTION */
const SelfList<SoftBody3DSW> *sb = soft_body_list->first();
while (sb) {
sb->self()->predict_motion(p_delta);
sb = sb->next();
active_count++;
}
p_space->set_active_objects(active_count);
{ //profile
@ -214,6 +225,20 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here
}
sb = soft_body_list->first();
while (sb) {
for (const Set<Constraint3DSW *>::Element *E = sb->self()->get_constraints().front(); E; E = E->next()) {
Constraint3DSW *c = E->get();
if (c->get_island_step() == _step)
continue;
c->set_island_step(_step);
c->set_island_next(NULL);
c->set_island_list_next(constraint_island_list);
constraint_island_list = c;
}
sb = sb->next();
}
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
@ -272,6 +297,14 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
}
}
/* UPDATE SOFT BODY CONSTRAINTS */
sb = soft_body_list->first();
while (sb) {
sb->self()->solve_constraints(p_delta);
sb = sb->next();
}
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);

5
servers/physics_server_3d.cpp
View file

@ -556,6 +556,10 @@ void PhysicsServer3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("body_get_direct_state", "body"), &PhysicsServer3D::body_get_direct_state);
/* SOFT BODY API */
ClassDB::bind_method(D_METHOD("soft_body_get_bounds", "body"), &PhysicsServer3D::soft_body_get_bounds);
/* JOINT API */
ClassDB::bind_method(D_METHOD("joint_create"), &PhysicsServer3D::joint_create);
@ -693,6 +697,7 @@ void PhysicsServer3D::_bind_methods() {
BIND_ENUM_CONSTANT(SHAPE_CONVEX_POLYGON);
BIND_ENUM_CONSTANT(SHAPE_CONCAVE_POLYGON);
BIND_ENUM_CONSTANT(SHAPE_HEIGHTMAP);
BIND_ENUM_CONSTANT(SHAPE_SOFT_BODY);
BIND_ENUM_CONSTANT(SHAPE_CUSTOM);
BIND_ENUM_CONSTANT(AREA_PARAM_GRAVITY);

26
servers/physics_server_3d.h
View file

@ -216,6 +216,15 @@ public:
PhysicsShapeQueryResult3D();
};
class RenderingServerHandler {
public:
virtual void set_vertex(int p_vertex_id, const void *p_vector3) = 0;
virtual void set_normal(int p_vertex_id, const void *p_vector3) = 0;
virtual void set_aabb(const AABB &p_aabb) = 0;
virtual ~RenderingServerHandler() {}
};
class PhysicsServer3D : public Object {
GDCLASS(PhysicsServer3D, Object);
@ -237,6 +246,7 @@ public:
SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
SHAPE_CONCAVE_POLYGON, ///< vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array)
SHAPE_HEIGHTMAP, ///< dict( int:"width", int:"depth",float:"cell_size", float_array:"heights"
SHAPE_SOFT_BODY, ///< Used internally, can't be created from the physics server.
SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
};
@ -522,13 +532,15 @@ public:
virtual RID soft_body_create() = 0;
virtual void soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) = 0;
virtual void soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) = 0;
virtual void soft_body_set_space(RID p_body, RID p_space) = 0;
virtual RID soft_body_get_space(RID p_body) const = 0;
virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) = 0;
virtual AABB soft_body_get_bounds(RID p_body) const = 0;
virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) = 0;
virtual uint32_t soft_body_get_collision_layer(RID p_body) const = 0;
@ -543,7 +555,6 @@ public:
virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const = 0;
virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) = 0;
virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const = 0;
virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) = 0;
@ -556,18 +567,9 @@ public:
virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) = 0;
virtual real_t soft_body_get_linear_stiffness(RID p_body) const = 0;
virtual void soft_body_set_angular_stiffness(RID p_body, real_t p_stiffness) = 0;
virtual real_t soft_body_get_angular_stiffness(RID p_body) const = 0;
virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) = 0;
virtual real_t soft_body_get_volume_stiffness(RID p_body) const = 0;
virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) = 0;
virtual real_t soft_body_get_pressure_coefficient(RID p_body) const = 0;
virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) = 0;
virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) const = 0;
virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) = 0;
virtual real_t soft_body_get_damping_coefficient(RID p_body) const = 0;
@ -577,8 +579,6 @@ public:
virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) = 0;
virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const = 0;
virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const = 0;
virtual void soft_body_remove_all_pinned_points(RID p_body) = 0;
virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) = 0;
virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const = 0;