diff --git a/COPYRIGHT.txt b/COPYRIGHT.txt index ff24d18e6f9..29cc9204550 100644 --- a/COPYRIGHT.txt +++ b/COPYRIGHT.txt @@ -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. diff --git a/doc/classes/SoftBody3D.xml b/doc/classes/SoftBody3D.xml index 04e201e1bd6..29f8ecd432f 100644 --- a/doc/classes/SoftBody3D.xml +++ b/doc/classes/SoftBody3D.xml @@ -77,8 +77,6 @@ - - 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 @@ [NodePath] to a [CollisionObject3D] this SoftBody3D should avoid clipping. - - @@ -109,8 +105,6 @@ The SoftBody3D's mass. - - diff --git a/modules/bullet/bullet_physics_server.cpp b/modules/bullet/bullet_physics_server.cpp index 26e9f5a044b..5fd00bb9439 100644 --- a/modules/bullet/bullet_physics_server.cpp +++ b/modules/bullet/bullet_physics_server.cpp @@ -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); diff --git a/modules/bullet/bullet_physics_server.h b/modules/bullet/bullet_physics_server.h index 97b719ae8e8..856ff749635 100644 --- a/modules/bullet/bullet_physics_server.h +++ b/modules/bullet/bullet_physics_server.h @@ -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 */ diff --git a/modules/bullet/soft_body_bullet.cpp b/modules/bullet/soft_body_bullet.cpp index 91a1934e07c..e3e95739812 100644 --- a/modules/bullet/soft_body_bullet.cpp +++ b/modules/bullet/soft_body_bullet.cpp @@ -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 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); diff --git a/modules/bullet/soft_body_bullet.h b/modules/bullet/soft_body_bullet.h index 23f6fba9a63..87023b2517d 100644 --- a/modules/bullet/soft_body_bullet.h +++ b/modules/bullet/soft_body_bullet.h @@ -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 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 &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; } diff --git a/scene/3d/soft_body_3d.cpp b/scene/3d/soft_body_3d.cpp index 2d8f22ab378..9eb71273b4b 100644 --- a/scene/3d/soft_body_3d.cpp +++ b/scene/3d/soft_body_3d.cpp @@ -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); } diff --git a/scene/3d/soft_body_3d.h b/scene/3d/soft_body_3d.h index 6e24a530bd1..f98df39209c 100644 --- a/scene/3d/soft_body_3d.h +++ b/scene/3d/soft_body_3d.h @@ -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(); diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index 8e21003a5f2..e87ff2364bd 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -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); diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index 6012ff1522f..06bafb16248 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -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); +} diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h index 4d049eafdca..74dddfa6aad 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -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 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 diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index 3847b813810..85221b77468 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -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; } diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index 651961433ce..187e08e00eb 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -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); } } }; @@ -680,7 +680,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 *separator = (SeparatorAxisTest *)p_userdata; Vector3 axis = (p_point_B - p_point_A); real_t depth = axis.length(); diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index fd9ea00d923..f655c4626c1 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -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(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(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; diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h index 81d87e9773d..34ac2c6d3f7 100644 --- a/servers/physics_3d/collision_solver_3d_sw.h +++ b/servers/physics_3d/collision_solver_3d_sw.h @@ -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); diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index aa7c11eec5b..f24d55e05e2 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -978,9 +978,9 @@ bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform & if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_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; diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index 6bbef099079..3d0063b0fa7 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -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 *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) { diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index afda161fa86..f92652bfadb 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -63,6 +63,7 @@ class PhysicsServer3DSW : public PhysicsServer3D { mutable RID_PtrOwner space_owner; mutable RID_PtrOwner area_owner; mutable RID_PtrOwner body_owner; + mutable RID_PtrOwner soft_body_owner; mutable RID_PtrOwner 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 *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 *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 */ diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h index f60e1332d54..49ae60db926 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.h +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -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); diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp new file mode 100644 index 00000000000..f63a470cbee --- /dev/null +++ b/servers/physics_3d/soft_body_3d_sw.cpp @@ -0,0 +1,1221 @@ +/*************************************************************************/ +/* soft_body_3d_sw.cpp */ +/*************************************************************************/ +/* 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. */ +/*************************************************************************/ + +#include "soft_body_3d_sw.h" +#include "space_3d_sw.h" + +#include "core/math/geometry_3d.h" +#include "core/templates/map.h" + +// Based on Bullet soft body. + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btSoftBody implementation by Nathanael Presson + +SoftBody3DSW::SoftBody3DSW() : + CollisionObject3DSW(TYPE_SOFT_BODY), + active_list(this) { + _set_static(false); +} + +void SoftBody3DSW::_shapes_changed() { +} + +void SoftBody3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + _set_transform(p_variant); + _set_inv_transform(get_transform().inverse()); + + apply_nodes_transform(get_transform()); + + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + // Not supported. + ERR_FAIL_MSG("Linear velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_MSG("Angular velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); + } break; + } +} + +Variant SoftBody3DSW::get_state(PhysicsServer3D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + return get_transform(); + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + ERR_FAIL_V_MSG(Vector3(), "Linear velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_V_MSG(Vector3(), "Angular velocity is not supported for Soft bodies."); + return Vector3(); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); + } break; + } + + return Variant(); +} + +void SoftBody3DSW::set_space(Space3DSW *p_space) { + if (get_space()) { + get_space()->soft_body_remove_from_active_list(&active_list); + + deinitialize_shape(); + } + + _set_space(p_space); + + if (get_space()) { + get_space()->soft_body_add_to_active_list(&active_list); + + if (bounds != AABB()) { + initialize_shape(true); + } + } +} + +void SoftBody3DSW::set_mesh(const Ref &p_mesh) { + destroy(); + + soft_mesh = p_mesh; + + if (soft_mesh.is_null()) { + return; + } + + Array arrays = soft_mesh->surface_get_arrays(0); + ERR_FAIL_COND(!(soft_mesh->surface_get_format(0) & RS::ARRAY_FORMAT_INDEX)); + + bool success = create_from_trimesh(arrays[RS::ARRAY_INDEX], arrays[RS::ARRAY_VERTEX]); + if (!success) { + destroy(); + soft_mesh = Ref(); + } +} + +void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) { + if (soft_mesh.is_null()) { + return; + } + + const uint32_t vertex_count = map_visual_to_physics.size(); + for (uint32_t i = 0; i < vertex_count; ++i) { + const uint32_t node_index = map_visual_to_physics[i]; + const Node &node = nodes[node_index]; + const Vector3 &vertex_position = node.x; + const Vector3 &vertex_normal = node.n; + + p_rendering_server_handler->set_vertex(i, &vertex_position); + p_rendering_server_handler->set_normal(i, &vertex_normal); + } + + p_rendering_server_handler->set_aabb(bounds); +} + +void SoftBody3DSW::update_normals() { + uint32_t i, ni; + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + nodes[i].n = Vector3(); + } + + for (i = 0, ni = faces.size(); i < ni; ++i) { + Face &face = faces[i]; + const Vector3 n = vec3_cross(face.n[0]->x - face.n[2]->x, face.n[0]->x - face.n[1]->x); + face.n[0]->n += n; + face.n[1]->n += n; + face.n[2]->n += n; + face.normal = n; + face.normal.normalize(); + } + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + real_t len = node.n.length(); + if (len > CMP_EPSILON) { + node.n /= len; + } + } +} + +void SoftBody3DSW::update_bounds() { + AABB prev_bounds = bounds; + prev_bounds.grow_by(collision_margin); + + bounds = AABB(); + + const uint32_t nodes_count = nodes.size(); + if (nodes_count == 0) { + deinitialize_shape(); + return; + } + + bool first = true; + bool moved = false; + for (uint32_t node_index = 0; node_index < nodes_count; ++node_index) { + const Node &node = nodes[node_index]; + if (!prev_bounds.has_point(node.x)) { + moved = true; + } + if (first) { + bounds.position = node.x; + first = false; + } else { + bounds.expand_to(node.x); + } + } + + if (get_space()) { + initialize_shape(moved); + } +} + +void SoftBody3DSW::update_constants() { + reset_link_rest_lengths(); + update_link_constants(); + update_area(); +} + +void SoftBody3DSW::update_area() { + int i, ni; + + // Face area. + for (i = 0, ni = faces.size(); i < ni; ++i) { + Face &face = faces[i]; + + const Vector3 &x0 = face.n[0]->x; + const Vector3 &x1 = face.n[1]->x; + const Vector3 &x2 = face.n[2]->x; + + const Vector3 a = x1 - x0; + const Vector3 b = x2 - x0; + const Vector3 cr = vec3_cross(a, b); + face.ra = cr.length(); + } + + // Node area. + LocalVector counts; + counts.resize(nodes.size()); + memset(counts.ptr(), 0, counts.size() * sizeof(int)); + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + nodes[i].area = 0.0; + } + + for (i = 0, ni = faces.size(); i < ni; ++i) { + const Face &face = faces[i]; + for (int j = 0; j < 3; ++j) { + const int index = (int)(face.n[j] - &nodes[0]); + counts[index]++; + face.n[j]->area += Math::abs(face.ra); + } + } + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + if (counts[i] > 0) { + nodes[i].area /= (real_t)counts[i]; + } else { + nodes[i].area = 0.0; + } + } +} + +void SoftBody3DSW::reset_link_rest_lengths() { + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.rl = (link.n[0]->x - link.n[1]->x).length(); + link.c1 = link.rl * link.rl; + } +} + +void SoftBody3DSW::update_link_constants() { + real_t inv_linear_stiffness = 1.0 / linear_stiffness; + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.c0 = (link.n[0]->im + link.n[1]->im) * inv_linear_stiffness; + } +} + +void SoftBody3DSW::apply_nodes_transform(const Transform &p_transform) { + if (soft_mesh.is_null()) { + return; + } + + uint32_t node_count = nodes.size(); + Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; + for (uint32_t node_index = 0; node_index < node_count; ++node_index) { + Node &node = nodes[node_index]; + + node.x = p_transform.xform(node.x); + node.q = node.x; + node.v = Vector3(); + node.bv = Vector3(); + + AABB node_aabb(node.x, leaf_size); + node_tree.update(node.leaf, node_aabb); + } + + face_tree.clear(); + + update_normals(); + update_bounds(); + update_constants(); +} + +Vector3 SoftBody3DSW::get_vertex_position(int p_index) const { + if (soft_mesh.is_null()) { + return Vector3(); + } + + ERR_FAIL_INDEX_V(p_index, (int)map_visual_to_physics.size(), Vector3()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND_V(node_index >= nodes.size(), Vector3()); + return nodes[node_index].x; +} + +void SoftBody3DSW::set_vertex_position(int p_index, const Vector3 &p_position) { + if (soft_mesh.is_null()) { + return; + } + + ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.q = node.x; + node.x = p_position; +} + +void SoftBody3DSW::pin_vertex(int p_index) { + if (is_vertex_pinned(p_index)) { + return; + } + + pinned_vertices.push_back(p_index); + + if (!soft_mesh.is_null()) { + ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.im = 0.0; + } +} + +void SoftBody3DSW::unpin_vertex(int p_index) { + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + if (p_index == pinned_vertices[i]) { + pinned_vertices.remove(i); + + if (!soft_mesh.is_null()) { + ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + real_t inv_node_mass = nodes.size() * inv_total_mass; + + Node &node = nodes[node_index]; + node.im = inv_node_mass; + } + + return; + } + } +} + +void SoftBody3DSW::unpin_all_vertices() { + if (!soft_mesh.is_null()) { + real_t inv_node_mass = nodes.size() * inv_total_mass; + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + uint32_t vertex_index = pinned_vertices[i]; + + ERR_CONTINUE(vertex_index >= map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[vertex_index]; + + ERR_CONTINUE(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.im = inv_node_mass; + } + } + + pinned_vertices.clear(); +} + +bool SoftBody3DSW::is_vertex_pinned(int p_index) const { + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + if (p_index == pinned_vertices[i]) { + return true; + } + } + + return false; +} + +uint32_t SoftBody3DSW::get_node_count() const { + return nodes.size(); +} + +real_t SoftBody3DSW::get_node_inv_mass(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), 0.0); + return nodes[p_node_index].im; +} + +Vector3 SoftBody3DSW::get_node_position(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].x; +} + +Vector3 SoftBody3DSW::get_node_velocity(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].v; +} + +Vector3 SoftBody3DSW::get_node_biased_velocity(uint32_t p_node_index) const { + ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + return nodes[p_node_index].bv; +} + +void SoftBody3DSW::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { + ERR_FAIL_COND(p_node_index >= nodes.size()); + Node &node = nodes[p_node_index]; + node.v += p_impulse * node.im; +} + +void SoftBody3DSW::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { + ERR_FAIL_COND(p_node_index >= nodes.size()); + Node &node = nodes[p_node_index]; + node.bv += p_impulse * node.im; +} + +uint32_t SoftBody3DSW::get_face_count() const { + return faces.size(); +} + +void SoftBody3DSW::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const { + ERR_FAIL_COND(p_face_index >= faces.size()); + const Face &face = faces[p_face_index]; + r_point_1 = face.n[0]->x; + r_point_2 = face.n[1]->x; + r_point_3 = face.n[2]->x; +} + +Vector3 SoftBody3DSW::get_face_normal(uint32_t p_face_index) const { + ERR_FAIL_COND_V(p_face_index >= faces.size(), Vector3()); + return faces[p_face_index].normal; +} + +bool SoftBody3DSW::create_from_trimesh(const Vector &p_indices, const Vector &p_vertices) { + uint32_t node_count = 0; + LocalVector vertices; + const int visual_vertex_count(p_vertices.size()); + + LocalVector triangles; + const uint32_t triangle_count(p_indices.size() / 3); + triangles.resize(triangle_count * 3); + + // Merge all overlapping vertices and create a map of physical vertices to visual vertices. + { + // Process vertices. + { + uint32_t vertex_count = 0; + Map unique_vertices; + + vertices.resize(visual_vertex_count); + map_visual_to_physics.resize(visual_vertex_count); + + for (int visual_vertex_index = 0; visual_vertex_index < visual_vertex_count; ++visual_vertex_index) { + const Vector3 &vertex = p_vertices[visual_vertex_index]; + + Map::Element *e = unique_vertices.find(vertex); + uint32_t vertex_id; + if (e) { + // Already existing. + vertex_id = e->value(); + } else { + // Create new one. + vertex_id = vertex_count++; + unique_vertices[vertex] = vertex_id; + vertices[vertex_id] = vertex; + } + + map_visual_to_physics[visual_vertex_index] = vertex_id; + } + + vertices.resize(vertex_count); + } + + // Process triangles. + { + for (uint32_t triangle_index = 0; triangle_index < triangle_count; ++triangle_index) { + for (int i = 0; i < 3; ++i) { + int visual_index = 3 * triangle_index + i; + int physics_index = map_visual_to_physics[p_indices[visual_index]]; + triangles[visual_index] = physics_index; + node_count = MAX((int)node_count, physics_index); + } + } + } + } + + ++node_count; + + // Create nodes from vertices. + nodes.resize(node_count); + real_t inv_node_mass = node_count * inv_total_mass; + Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; + for (uint32_t i = 0; i < node_count; ++i) { + Node &node = nodes[i]; + node.s = vertices[i]; + node.x = node.s; + node.q = node.s; + node.im = inv_node_mass; + + AABB node_aabb(node.x, leaf_size); + node.leaf = node_tree.insert(node_aabb, &node); + + node.index = i; + } + + // Create links and faces from triangles. + LocalVector chks; + chks.resize(node_count * node_count); + memset(chks.ptr(), 0, chks.size() * sizeof(bool)); + + for (uint32_t i = 0; i < triangle_count * 3; i += 3) { + const int idx[] = { triangles[i], triangles[i + 1], triangles[i + 2] }; + + for (int j = 2, k = 0; k < 3; j = k++) { + int chk = idx[k] * node_count + idx[j]; + if (!chks[chk]) { + chks[chk] = true; + int inv_chk = idx[j] * node_count + idx[k]; + chks[inv_chk] = true; + + append_link(idx[j], idx[k]); + } + } + + append_face(idx[0], idx[1], idx[2]); + } + + // Set pinned nodes. + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + int pinned_vertex = pinned_vertices[i]; + + ERR_CONTINUE(pinned_vertex >= visual_vertex_count); + uint32_t node_index = map_visual_to_physics[pinned_vertex]; + + ERR_CONTINUE(node_index >= node_count); + Node &node = nodes[node_index]; + node.im = 0.0; + } + + generate_bending_constraints(2); + reoptimize_link_order(); + + update_constants(); + update_normals(); + update_bounds(); + + return true; +} + +void SoftBody3DSW::generate_bending_constraints(int p_distance) { + uint32_t i, j; + + if (p_distance > 1) { + // Build graph. + const uint32_t n = nodes.size(); + const unsigned inf = (~(unsigned)0) >> 1; + const uint32_t adj_size = n * n; + unsigned *adj = memnew_arr(unsigned, adj_size); + +#define IDX(_x_, _y_) ((_y_)*n + (_x_)) + for (j = 0; j < n; ++j) { + for (i = 0; i < n; ++i) { + int idx_ij = j * n + i; + int idx_ji = i * n + j; + if (i != j) { + adj[idx_ij] = adj[idx_ji] = inf; + } else { + adj[idx_ij] = adj[idx_ji] = 0; + } + } + } + for (i = 0; i < links.size(); ++i) { + const int ia = (int)(links[i].n[0] - &nodes[0]); + const int ib = (int)(links[i].n[1] - &nodes[0]); + int idx = ib * n + ia; + int idx_inv = ia * n + ib; + adj[idx] = 1; + adj[idx_inv] = 1; + } + + // Special optimized case for distance == 2. + if (p_distance == 2) { + LocalVector> node_links; + + // Build node links. + node_links.resize(nodes.size()); + + for (i = 0; i < links.size(); ++i) { + const int ia = (int)(links[i].n[0] - &nodes[0]); + const int ib = (int)(links[i].n[1] - &nodes[0]); + if (node_links[ia].find(ib) == -1) { + node_links[ia].push_back(ib); + } + + if (node_links[ib].find(ia) == -1) { + node_links[ib].push_back(ia); + } + } + for (uint32_t ii = 0; ii < node_links.size(); ii++) { + for (uint32_t jj = 0; jj < node_links[ii].size(); jj++) { + int k = node_links[ii][jj]; + for (uint32_t kk = 0; kk < node_links[k].size(); kk++) { + int l = node_links[k][kk]; + if ((int)ii != l) { + int idx_ik = k * n + ii; + int idx_kj = l * n + k; + const unsigned sum = adj[idx_ik] + adj[idx_kj]; + ERR_FAIL_COND(sum != 2); + int idx_ij = l * n + ii; + if (adj[idx_ij] > sum) { + int idx_ji = l * n + ii; + adj[idx_ij] = adj[idx_ji] = sum; + } + } + } + } + } + } else { + // Generic Floyd's algorithm. + for (uint32_t k = 0; k < n; ++k) { + for (j = 0; j < n; ++j) { + for (i = j + 1; i < n; ++i) { + int idx_ik = k * n + i; + int idx_kj = j * n + k; + const unsigned sum = adj[idx_ik] + adj[idx_kj]; + int idx_ij = j * n + i; + if (adj[idx_ij] > sum) { + int idx_ji = j * n + i; + adj[idx_ij] = adj[idx_ji] = sum; + } + } + } + } + } + + // Build links. + for (j = 0; j < n; ++j) { + for (i = j + 1; i < n; ++i) { + int idx_ij = j * n + i; + if (adj[idx_ij] == (unsigned)p_distance) { + append_link(i, j); + } + } + } + memdelete_arr(adj); + } +} + +//=================================================================== +// +// +// This function takes in a list of interdependent Links and tries +// to maximize the distance between calculation +// of dependent links. This increases the amount of parallelism that can +// be exploited by out-of-order instruction processors with large but +// (inevitably) finite instruction windows. +// +//=================================================================== + +// A small structure to track lists of dependent link calculations. +class LinkDeps { +public: + int value; // A link calculation that is dependent on this one + // Positive values = "input A" while negative values = "input B" + LinkDeps *next; // Next dependence in the list +}; +typedef LinkDeps *LinkDepsPtr; + +void SoftBody3DSW::reoptimize_link_order() { + const int reop_not_dependent = -1; + const int reop_node_complete = -2; + + uint32_t i, link_count = links.size(), node_count = nodes.size(); + Link *lr; + int ar, br; + Node *node0 = &(nodes[0]); + Node *node1 = &(nodes[1]); + LinkDepsPtr link_dep; + int ready_list_head, ready_list_tail, link_num, link_dep_frees, dep_link; + + // Allocate temporary buffers. + int *node_written_at = memnew_arr(int, node_count + 1); // What link calculation produced this node's current values? + int *link_dep_A = memnew_arr(int, link_count); // Link calculation input is dependent upon prior calculation #N + int *link_dep_B = memnew_arr(int, link_count); + int *ready_list = memnew_arr(int, link_count); // List of ready-to-process link calculations (# of links, maximum) + LinkDeps *link_dep_free_list = memnew_arr(LinkDeps, 2 * link_count); // Dependent-on-me list elements (2x# of links, maximum) + LinkDepsPtr *link_dep_list_starts = memnew_arr(LinkDepsPtr, link_count); // Start nodes of dependent-on-me lists, one for each link + + // Copy the original, unsorted links to a side buffer. + Link *link_buffer = memnew_arr(Link, link_count); + memcpy(link_buffer, &(links[0]), sizeof(Link) * link_count); + + // Clear out the node setup and ready list. + for (i = 0; i < node_count + 1; i++) { + node_written_at[i] = reop_not_dependent; + } + for (i = 0; i < link_count; i++) { + link_dep_list_starts[i] = nullptr; + } + ready_list_head = ready_list_tail = link_dep_frees = 0; + + // Initial link analysis to set up data structures. + for (i = 0; i < link_count; i++) { + // Note which prior link calculations we are dependent upon & build up dependence lists. + lr = &(links[i]); + ar = (lr->n[0] - node0) / (node1 - node0); + br = (lr->n[1] - node0) / (node1 - node0); + if (node_written_at[ar] > reop_not_dependent) { + link_dep_A[i] = node_written_at[ar]; + link_dep = &link_dep_free_list[link_dep_frees++]; + link_dep->value = i; + link_dep->next = link_dep_list_starts[node_written_at[ar]]; + link_dep_list_starts[node_written_at[ar]] = link_dep; + } else { + link_dep_A[i] = reop_not_dependent; + } + if (node_written_at[br] > reop_not_dependent) { + link_dep_B[i] = node_written_at[br]; + link_dep = &link_dep_free_list[link_dep_frees++]; + link_dep->value = -(int)(i + 1); + link_dep->next = link_dep_list_starts[node_written_at[br]]; + link_dep_list_starts[node_written_at[br]] = link_dep; + } else { + link_dep_B[i] = reop_not_dependent; + } + + // Add this link to the initial ready list, if it is not dependent on any other links. + if ((link_dep_A[i] == reop_not_dependent) && (link_dep_B[i] == reop_not_dependent)) { + ready_list[ready_list_tail++] = i; + link_dep_A[i] = link_dep_B[i] = reop_node_complete; // Probably not needed now. + } + + // Update the nodes to mark which ones are calculated by this link. + node_written_at[ar] = node_written_at[br] = i; + } + + // Process the ready list and create the sorted list of links: + // -- By treating the ready list as a queue, we maximize the distance between any + // inter-dependent node calculations. + // -- All other (non-related) nodes in the ready list will automatically be inserted + // in between each set of inter-dependent link calculations by this loop. + i = 0; + while (ready_list_head != ready_list_tail) { + // Use ready list to select the next link to process. + link_num = ready_list[ready_list_head++]; + // Copy the next-to-calculate link back into the original link array. + links[i++] = link_buffer[link_num]; + + // Free up any link inputs that are dependent on this one. + link_dep = link_dep_list_starts[link_num]; + while (link_dep) { + dep_link = link_dep->value; + if (dep_link >= 0) { + link_dep_A[dep_link] = reop_not_dependent; + } else { + dep_link = -dep_link - 1; + link_dep_B[dep_link] = reop_not_dependent; + } + // Add this dependent link calculation to the ready list if *both* inputs are clear. + if ((link_dep_A[dep_link] == reop_not_dependent) && (link_dep_B[dep_link] == reop_not_dependent)) { + ready_list[ready_list_tail++] = dep_link; + link_dep_A[dep_link] = link_dep_B[dep_link] = reop_node_complete; // Probably not needed now. + } + link_dep = link_dep->next; + } + } + + // Delete the temporary buffers. + memdelete_arr(node_written_at); + memdelete_arr(link_dep_A); + memdelete_arr(link_dep_B); + memdelete_arr(ready_list); + memdelete_arr(link_dep_free_list); + memdelete_arr(link_dep_list_starts); + memdelete_arr(link_buffer); +} + +void SoftBody3DSW::append_link(uint32_t p_node1, uint32_t p_node2) { + if (p_node1 == p_node2) { + return; + } + + Node *node1 = &nodes[p_node1]; + Node *node2 = &nodes[p_node2]; + + Link link; + link.n[0] = node1; + link.n[1] = node2; + link.rl = (node1->x - node2->x).length(); + + links.push_back(link); +} + +void SoftBody3DSW::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) { + if (p_node1 == p_node2) { + return; + } + if (p_node1 == p_node3) { + return; + } + if (p_node2 == p_node3) { + return; + } + + Node *node1 = &nodes[p_node1]; + Node *node2 = &nodes[p_node2]; + Node *node3 = &nodes[p_node3]; + + Face face; + face.n[0] = node1; + face.n[1] = node2; + face.n[2] = node3; + + face.index = faces.size(); + + faces.push_back(face); +} + +void SoftBody3DSW::set_iteration_count(int p_val) { + iteration_count = p_val; +} + +void SoftBody3DSW::set_total_mass(real_t p_val) { + ERR_FAIL_COND(p_val < 0.0); + + inv_total_mass = 1.0 / p_val; + real_t mass_factor = total_mass * inv_total_mass; + total_mass = p_val; + + uint32_t node_count = nodes.size(); + for (uint32_t node_index = 0; node_index < node_count; ++node_index) { + Node &node = nodes[node_index]; + node.im *= mass_factor; + } + + update_constants(); +} + +void SoftBody3DSW::set_collision_margin(real_t p_val) { + collision_margin = p_val; +} + +void SoftBody3DSW::set_linear_stiffness(real_t p_val) { + linear_stiffness = p_val; +} + +void SoftBody3DSW::set_pressure_coefficient(real_t p_val) { + pressure_coefficient = p_val; +} + +void SoftBody3DSW::set_damping_coefficient(real_t p_val) { + damping_coefficient = p_val; +} + +void SoftBody3DSW::set_drag_coefficient(real_t p_val) { + drag_coefficient = p_val; +} + +void SoftBody3DSW::add_velocity(const Vector3 &p_velocity) { + for (uint32_t i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + if (node.im > 0) { + node.v += p_velocity; + } + } +} + +void SoftBody3DSW::apply_forces() { + if (pressure_coefficient < CMP_EPSILON) { + return; + } + + if (nodes.is_empty()) { + return; + } + + uint32_t i, ni; + + // Calculate volume. + real_t volume = 0.0; + const Vector3 &org = nodes[0].x; + for (i = 0, ni = faces.size(); i < ni; ++i) { + const Face &face = faces[i]; + volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org)); + } + volume /= 6.0; + + // Apply per node forces. + real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + if (node.im > 0) { + node.f += node.n * (node.area * ivolumetp); + } + } +} + +void SoftBody3DSW::predict_motion(real_t p_delta) { + const real_t inv_delta = 1.0 / p_delta; + + ERR_FAIL_COND(!get_space()); + + Area3DSW *def_area = get_space()->get_default_area(); + ERR_FAIL_COND(!def_area); + + // Apply forces. + Vector3 gravity = def_area->get_gravity_vector() * def_area->get_gravity(); + add_velocity(gravity * p_delta); + apply_forces(); + + // Avoid soft body from 'exploding' so use some upper threshold of maximum motion + // that a node can travel per frame. + const real_t max_displacement = 1000.0; + real_t clamp_delta_v = max_displacement * inv_delta; + + // Integrate. + uint32_t i, ni; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + node.q = node.x; + Vector3 delta_v = node.f * node.im * p_delta; + for (int c = 0; c < 3; c++) { + delta_v[c] = CLAMP(delta_v[c], -clamp_delta_v, clamp_delta_v); + } + node.v += delta_v; + node.x += node.v * p_delta; + node.f = Vector3(); + } + + // Bounds and tree update. + update_bounds(); + + // Node tree update. + for (i = 0, ni = nodes.size(); i < ni; ++i) { + const Node &node = nodes[i]; + + AABB node_aabb(node.x, Vector3()); + node_aabb.expand_to(node.x + node.v * p_delta); + node_aabb.grow_by(collision_margin); + + node_tree.update(node.leaf, node_aabb); + } + + // Face tree update. + if (!face_tree.is_empty()) { + update_face_tree(p_delta); + } + + // Optimize node tree. + node_tree.optimize_incremental(1); + face_tree.optimize_incremental(1); +} + +void SoftBody3DSW::solve_constraints(real_t p_delta) { + const real_t inv_delta = 1.0 / p_delta; + + uint32_t i, ni; + + for (i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + link.c3 = link.n[1]->q - link.n[0]->q; + link.c2 = 1 / (link.c3.length_squared() * link.c0); + } + + // Solve velocities. + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + node.x = node.q + node.v * p_delta; + } + + // Solve positions. + for (int isolve = 0; isolve < iteration_count; ++isolve) { + const real_t ti = isolve / (real_t)iteration_count; + solve_links(1.0, ti); + } + const real_t vc = (1.0 - damping_coefficient) * inv_delta; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + + node.x += node.bv * p_delta; + node.bv = Vector3(); + + node.v = (node.x - node.q) * vc; + + node.q = node.x; + } + + update_normals(); +} + +void SoftBody3DSW::solve_links(real_t kst, real_t ti) { + for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { + Link &link = links[i]; + if (link.c0 > 0) { + Node &node_a = *link.n[0]; + Node &node_b = *link.n[1]; + const Vector3 del = node_b.x - node_a.x; + const real_t len = del.length_squared(); + if (link.c1 + len > CMP_EPSILON) { + const real_t k = ((link.c1 - len) / (link.c0 * (link.c1 + len))) * kst; + node_a.x -= del * (k * node_a.im); + node_b.x += del * (k * node_b.im); + } + } + } +} + +struct AABBQueryResult { + const SoftBody3DSW *soft_body = nullptr; + void *userdata = nullptr; + SoftBody3DSW::QueryResultCallback result_callback = nullptr; + + _FORCE_INLINE_ bool operator()(void *p_data) { + return result_callback(soft_body->get_node_index(p_data), userdata); + }; +}; + +void SoftBody3DSW::query_aabb(const AABB &p_aabb, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) { + AABBQueryResult query_result; + query_result.soft_body = this; + query_result.result_callback = p_result_callback; + query_result.userdata = p_userdata; + + node_tree.aabb_query(p_aabb, query_result); +} + +struct RayQueryResult { + const SoftBody3DSW *soft_body = nullptr; + void *userdata = nullptr; + SoftBody3DSW::QueryResultCallback result_callback = nullptr; + + _FORCE_INLINE_ bool operator()(void *p_data) { + return result_callback(soft_body->get_face_index(p_data), userdata); + }; +}; + +void SoftBody3DSW::query_ray(const Vector3 &p_from, const Vector3 &p_to, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) { + if (face_tree.is_empty()) { + initialize_face_tree(); + } + + RayQueryResult query_result; + query_result.soft_body = this; + query_result.result_callback = p_result_callback; + query_result.userdata = p_userdata; + + face_tree.ray_query(p_from, p_to, query_result); +} + +void SoftBody3DSW::initialize_face_tree() { + face_tree.clear(); + for (uint32_t i = 0; i < faces.size(); ++i) { + Face &face = faces[i]; + + AABB face_aabb; + + face_aabb.position = face.n[0]->x; + face_aabb.expand_to(face.n[1]->x); + face_aabb.expand_to(face.n[2]->x); + + face_aabb.grow_by(collision_margin); + + face.leaf = face_tree.insert(face_aabb, &face); + } +} + +void SoftBody3DSW::update_face_tree(real_t p_delta) { + for (uint32_t i = 0; i < faces.size(); ++i) { + const Face &face = faces[i]; + + AABB face_aabb; + + const Node *node0 = face.n[0]; + face_aabb.position = node0->x; + face_aabb.expand_to(node0->x + node0->v * p_delta); + + const Node *node1 = face.n[1]; + face_aabb.expand_to(node1->x); + face_aabb.expand_to(node1->x + node1->v * p_delta); + + const Node *node2 = face.n[2]; + face_aabb.expand_to(node2->x); + face_aabb.expand_to(node2->x + node2->v * p_delta); + + face_aabb.grow_by(collision_margin); + + face_tree.update(face.leaf, face_aabb); + } +} + +void SoftBody3DSW::initialize_shape(bool p_force_move) { + if (get_shape_count() == 0) { + SoftBodyShape3DSW *soft_body_shape = memnew(SoftBodyShape3DSW(this)); + add_shape(soft_body_shape); + } else if (p_force_move) { + SoftBodyShape3DSW *soft_body_shape = static_cast(get_shape(0)); + soft_body_shape->update_bounds(); + } +} + +void SoftBody3DSW::deinitialize_shape() { + if (get_shape_count() > 0) { + Shape3DSW *shape = get_shape(0); + remove_shape(shape); + memdelete(shape); + } +} + +void SoftBody3DSW::destroy() { + map_visual_to_physics.clear(); + + node_tree.clear(); + face_tree.clear(); + + nodes.clear(); + links.clear(); + faces.clear(); + + bounds = AABB(); + deinitialize_shape(); +} + +void SoftBodyShape3DSW::update_bounds() { + ERR_FAIL_COND(!soft_body); + + AABB collision_aabb = soft_body->get_bounds(); + collision_aabb.grow_by(soft_body->get_collision_margin()); + configure(collision_aabb); +} + +SoftBodyShape3DSW::SoftBodyShape3DSW(SoftBody3DSW *p_soft_body) { + soft_body = p_soft_body; + update_bounds(); +} + +struct _SoftBodyIntersectSegmentInfo { + const SoftBody3DSW *soft_body = nullptr; + Vector3 from; + Vector3 dir; + Vector3 hit_position; + uint32_t hit_face_index = -1; + real_t hit_dist_sq = Math_INF; + + static bool process_hit(uint32_t p_face_index, void *p_userdata) { + _SoftBodyIntersectSegmentInfo &query_info = *(_SoftBodyIntersectSegmentInfo *)(p_userdata); + + Vector3 points[3]; + query_info.soft_body->get_face_points(p_face_index, points[0], points[1], points[2]); + + Vector3 result; + if (Geometry3D::ray_intersects_triangle(query_info.from, query_info.dir, points[0], points[1], points[2], &result)) { + real_t dist_sq = query_info.from.distance_squared_to(result); + if (dist_sq < query_info.hit_dist_sq) { + query_info.hit_dist_sq = dist_sq; + query_info.hit_position = result; + query_info.hit_face_index = p_face_index; + } + } + + // Continue with the query. + return false; + } +}; + +bool SoftBodyShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + _SoftBodyIntersectSegmentInfo query_info; + query_info.soft_body = soft_body; + query_info.from = p_begin; + query_info.dir = (p_end - p_begin).normalized(); + + soft_body->query_ray(p_begin, p_end, _SoftBodyIntersectSegmentInfo::process_hit, &query_info); + + if (query_info.hit_dist_sq != Math_INF) { + r_result = query_info.hit_position; + r_normal = soft_body->get_face_normal(query_info.hit_face_index); + return true; + } + + return false; +} + +bool SoftBodyShape3DSW::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 SoftBodyShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + ERR_FAIL_V_MSG(Vector3(), "Get closest point is not supported for soft bodies."); +} diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h new file mode 100644 index 00000000000..6c0451ff450 --- /dev/null +++ b/servers/physics_3d/soft_body_3d_sw.h @@ -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 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 nodes; + LocalVector links; + LocalVector faces; + + DynamicBVH node_tree; + DynamicBVH face_tree; + + LocalVector 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 pinned_vertices; + + SelfList active_list; + + Set constraints; + + VSet 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 &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 &get_exceptions() const { return exceptions; } + + virtual void set_space(Space3DSW *p_space); + + void set_mesh(const Ref &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 &p_indices, const Vector &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 diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index c8741dc930e..c2110a7a445 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -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(intersection_query_results[i])->test_collision_mask(p_body)) == 0) { keep = false; } else if (static_cast(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(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(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::List &Space3DSW::get_moved_area_list() const { return area_moved_list; } +const SelfList::List &Space3DSW::get_active_soft_body_list() const { + return active_soft_body_list; +} + +void Space3DSW::soft_body_add_to_active_list(SelfList *p_soft_body) { + active_soft_body_list.add(p_soft_body); +} + +void Space3DSW::soft_body_remove_from_active_list(SelfList *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(); diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index eed3d86a727..3a8f452e548 100644 --- a/servers/physics_3d/space_3d_sw.h +++ b/servers/physics_3d/space_3d_sw.h @@ -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::List state_query_list; SelfList::List monitor_query_list; SelfList::List area_moved_list; + SelfList::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 *p_area); const SelfList::List &get_moved_area_list() const; + const SelfList::List &get_active_soft_body_list() const; + void soft_body_add_to_active_list(SelfList *p_soft_body); + void soft_body_remove_from_active_list(SelfList *p_soft_body); + BroadPhase3DSW *get_broadphase(); void add_object(CollisionObject3DSW *p_object); diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index d9370de6a3e..9a73e11562f 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -146,6 +146,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { const SelfList::List *body_list = &p_space->get_active_body_list(); + const SelfList::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 *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 *)aml.first()); //faster to remove here } + sb = soft_body_list->first(); + while (sb) { + for (const Set::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); diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp index af25029f049..586845de99d 100644 --- a/servers/physics_server_3d.cpp +++ b/servers/physics_server_3d.cpp @@ -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); diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h index e16857192ca..69f5c1c0adf 100644 --- a/servers/physics_server_3d.h +++ b/servers/physics_server_3d.h @@ -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;