From 940838c1742c7f7ba9ca94321502ac3263252255 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Thu, 15 Jul 2021 12:38:48 -0700 Subject: [PATCH] One-directional collision layer check for rigid bodies and soft bodies Check for each body individually if it collides with the other one or ignores it. When a body is being ignored, the other body's mass is considered infinite when applying impulses to avoid extra overlapping. --- doc/classes/CollisionObject2D.xml | 4 +- doc/classes/CollisionObject3D.xml | 8 +- doc/classes/SoftBody3D.xml | 8 +- modules/bullet/godot_result_callbacks.cpp | 14 +- modules/bullet/space_bullet.cpp | 2 +- servers/physics_2d/body_pair_2d_sw.cpp | 40 ++--- servers/physics_2d/body_pair_2d_sw.h | 4 +- servers/physics_2d/collision_object_2d_sw.h | 4 +- servers/physics_2d/space_2d_sw.cpp | 2 +- servers/physics_3d/body_pair_3d_sw.cpp | 156 ++++++++++++++------ servers/physics_3d/body_pair_3d_sw.h | 9 +- servers/physics_3d/collision_object_3d_sw.h | 4 +- servers/physics_3d/space_3d_sw.cpp | 2 +- 13 files changed, 162 insertions(+), 95 deletions(-) diff --git a/doc/classes/CollisionObject2D.xml b/doc/classes/CollisionObject2D.xml index b0afbb201ca..ddc9a04687d 100644 --- a/doc/classes/CollisionObject2D.xml +++ b/doc/classes/CollisionObject2D.xml @@ -260,11 +260,11 @@ The physics layers this CollisionObject2D is in. Collision objects can exist in one or more of 32 different layers. See also [member collision_mask]. - [b]Note:[/b] A contact is detected if object A is in any of the layers that object B scans, or object B is in any layers that object A scans. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. + [b]Note:[/b] Object A can detect a contact with object B only if object B is in any of the layers that object A scans. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. The physics layers this CollisionObject2D scans. Collision objects can scan one or more of 32 different layers. See also [member collision_layer]. - [b]Note:[/b] A contact is detected if object A is in any of the layers that object B scans, or object B is in any layers that object A scans. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. + [b]Note:[/b] Object A can detect a contact with object B only if object B is in any of the layers that object A scans. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. Defines the behavior in physics when [member Node.process_mode] is set to [constant Node.PROCESS_MODE_DISABLED]. See [enum DisableMode] for more details about the different modes. diff --git a/doc/classes/CollisionObject3D.xml b/doc/classes/CollisionObject3D.xml index dfd1e85e561..ea505997c00 100644 --- a/doc/classes/CollisionObject3D.xml +++ b/doc/classes/CollisionObject3D.xml @@ -223,12 +223,12 @@ - The physics layers this CollisionObject3D is in. Collision objects can exist in one or more of 32 different layers. See also [member collision_mask]. - [b]Note:[/b] A contact is detected if object A is in any of the layers that object B scans, or object B is in any layers that object A scans. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. + The physics layers this CollisionObject3D [b]is in[/b]. Collision objects can exist in one or more of 32 different layers. See also [member collision_mask]. + [b]Note:[/b] Object A can detect a contact with object B only if object B is in any of the layers that object A scans. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. - The physics layers this CollisionObject3D scans. Collision objects can scan one or more of 32 different layers. See also [member collision_layer]. - [b]Note:[/b] A contact is detected if object A is in any of the layers that object B scans, or object B is in any layers that object A scans. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. + The physics layers this CollisionObject3D [b]scans[/b]. Collision objects can scan one or more of 32 different layers. See also [member collision_layer]. + [b]Note:[/b] Object A can detect a contact with object B only if object B is in any of the layers that object A scans. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. Defines the behavior in physics when [member Node.process_mode] is set to [constant Node.PROCESS_MODE_DISABLED]. See [enum DisableMode] for more details about the different modes. diff --git a/doc/classes/SoftBody3D.xml b/doc/classes/SoftBody3D.xml index b143c60d9d9..601b3442cd3 100644 --- a/doc/classes/SoftBody3D.xml +++ b/doc/classes/SoftBody3D.xml @@ -84,12 +84,12 @@ - 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. - A contact is detected if object A is in any of the layers that object B scans, or object B is in any layer scanned by object A. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. + The physics layers this SoftBody3D [b]is in[/b]. Collision objects can exist in one or more of 32 different layers. See also [member collision_mask]. + [b]Note:[/b] Object A can detect a contact with object B only if object B is in any of the layers that object A scans. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. - The physics layers this SoftBody3D scans for collisions. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. + The physics layers this SoftBody3D [b]scans[/b]. Collision objects can scan one or more of 32 different layers. See also [member collision_layer]. + [b]Note:[/b] Object A can detect a contact with object B only if object B is in any of the layers that object A scans. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. diff --git a/modules/bullet/godot_result_callbacks.cpp b/modules/bullet/godot_result_callbacks.cpp index 1fd656c9b4f..399b102284a 100644 --- a/modules/bullet/godot_result_callbacks.cpp +++ b/modules/bullet/godot_result_callbacks.cpp @@ -52,7 +52,7 @@ bool GodotFilterCallback::needBroadphaseCollision(btBroadphaseProxy *proxy0, btB } bool GodotClosestRayResultCallback::needsCollision(btBroadphaseProxy *proxy0) const { - if (m_collisionFilterGroup & proxy0->m_collisionFilterMask) { + if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) { btCollisionObject *btObj = static_cast(proxy0->m_clientObject); CollisionObjectBullet *gObj = static_cast(btObj->getUserPointer()); @@ -85,7 +85,7 @@ bool GodotAllConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) con return false; } - if (m_collisionFilterGroup & proxy0->m_collisionFilterMask) { + if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) { btCollisionObject *btObj = static_cast(proxy0->m_clientObject); CollisionObjectBullet *gObj = static_cast(btObj->getUserPointer()); if (m_exclude->has(gObj->get_self())) { @@ -117,7 +117,7 @@ btScalar GodotAllConvexResultCallback::addSingleResult(btCollisionWorld::LocalCo } bool GodotKinClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const { - if (m_collisionFilterGroup & proxy0->m_collisionFilterMask) { + if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) { btCollisionObject *btObj = static_cast(proxy0->m_clientObject); CollisionObjectBullet *gObj = static_cast(btObj->getUserPointer()); if (gObj == m_self_object) { @@ -143,7 +143,7 @@ bool GodotKinClosestConvexResultCallback::needsCollision(btBroadphaseProxy *prox } bool GodotClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const { - if (m_collisionFilterGroup & proxy0->m_collisionFilterMask) { + if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) { btCollisionObject *btObj = static_cast(proxy0->m_clientObject); CollisionObjectBullet *gObj = static_cast(btObj->getUserPointer()); @@ -180,7 +180,7 @@ bool GodotAllContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) co return false; } - if (m_collisionFilterGroup & proxy0->m_collisionFilterMask) { + if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) { btCollisionObject *btObj = static_cast(proxy0->m_clientObject); CollisionObjectBullet *gObj = static_cast(btObj->getUserPointer()); @@ -235,7 +235,7 @@ bool GodotContactPairContactResultCallback::needsCollision(btBroadphaseProxy *pr return false; } - if (m_collisionFilterGroup & proxy0->m_collisionFilterMask) { + if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) { btCollisionObject *btObj = static_cast(proxy0->m_clientObject); CollisionObjectBullet *gObj = static_cast(btObj->getUserPointer()); @@ -277,7 +277,7 @@ btScalar GodotContactPairContactResultCallback::addSingleResult(btManifoldPoint } bool GodotRestInfoContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const { - if (m_collisionFilterGroup & proxy0->m_collisionFilterMask) { + if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) { btCollisionObject *btObj = static_cast(proxy0->m_clientObject); CollisionObjectBullet *gObj = static_cast(btObj->getUserPointer()); diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index 8c286a8629b..583900e6bc6 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -1133,7 +1133,7 @@ public: virtual bool process(const btBroadphaseProxy *proxy) { btCollisionObject *co = static_cast(proxy->m_clientObject); if (co->getInternalType() <= btCollisionObject::CO_RIGID_BODY) { - if (self_collision_object != proxy->m_clientObject && (collision_layer & proxy->m_collisionFilterMask)) { + if (self_collision_object != proxy->m_clientObject && (proxy->collision_layer & m_collisionFilterMask)) { if (co->getCollisionShape()->isCompound()) { const btCompoundShape *cs = static_cast(co->getCollisionShape()); diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index 71d985a6c51..91d747b492e 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -226,16 +226,16 @@ real_t combine_friction(Body2DSW *A, Body2DSW *B) { } bool BodyPair2DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); - if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { collided = false; return false; } + collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B); + collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A); + report_contacts_only = false; - if (!dynamic_A && !dynamic_B) { + if (!collide_A && !collide_B) { if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { report_contacts_only = true; } else { @@ -275,13 +275,13 @@ bool BodyPair2DSW::setup(real_t p_step) { if (!collided) { //test ccd (currently just a raycast) - if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_A) { + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { collided = true; } } - if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_B) { + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { collided = true; } @@ -374,6 +374,12 @@ bool BodyPair2DSW::pre_solve(real_t p_step) { const Transform2D &transform_A = A->get_transform(); const Transform2D &transform_B = B->get_transform(); + real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0; + real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; c.active = false; @@ -384,7 +390,7 @@ bool BodyPair2DSW::pre_solve(real_t p_step) { Vector2 axis = global_A - global_B; real_t depth = axis.dot(c.normal); - if (depth <= 0 || !c.reused) { + if (depth <= 0.0 || !c.reused) { continue; } @@ -416,15 +422,15 @@ bool BodyPair2DSW::pre_solve(real_t p_step) { // Precompute normal mass, tangent mass, and bias. real_t rnA = c.rA.dot(c.normal); real_t rnB = c.rB.dot(c.normal); - real_t kNormal = A->get_inv_mass() + B->get_inv_mass(); - kNormal += A->get_inv_inertia() * (c.rA.dot(c.rA) - rnA * rnA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rnB * rnB); + real_t kNormal = inv_mass_A + inv_mass_B; + kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB); c.mass_normal = 1.0f / kNormal; Vector2 tangent = c.normal.orthogonal(); real_t rtA = c.rA.dot(tangent); real_t rtB = c.rB.dot(tangent); - real_t kTangent = A->get_inv_mass() + B->get_inv_mass(); - kTangent += A->get_inv_inertia() * (c.rA.dot(c.rA) - rtA * rtA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rtB * rtB); + real_t kTangent = inv_mass_A + inv_mass_B; + kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB); c.mass_tangent = 1.0f / kTangent; c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); @@ -436,10 +442,10 @@ bool BodyPair2DSW::pre_solve(real_t p_step) { // Apply normal + friction impulse Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; - if (dynamic_A) { + if (collide_A) { A->apply_impulse(-P, c.rA); } - if (dynamic_B) { + if (collide_B) { B->apply_impulse(P, c.rB); } } @@ -493,10 +499,10 @@ void BodyPair2DSW::solve(real_t p_step) { Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); - if (dynamic_A) { + if (collide_A) { A->apply_bias_impulse(-jb, c.rA); } - if (dynamic_B) { + if (collide_B) { B->apply_bias_impulse(jb, c.rB); } @@ -513,10 +519,10 @@ void BodyPair2DSW::solve(real_t p_step) { Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); - if (dynamic_A) { + if (collide_A) { A->apply_impulse(-j, c.rA); } - if (dynamic_B) { + if (collide_B) { B->apply_impulse(j, c.rB); } } diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h index 4b42b44c926..849a7e24307 100644 --- a/servers/physics_2d/body_pair_2d_sw.h +++ b/servers/physics_2d/body_pair_2d_sw.h @@ -50,8 +50,8 @@ class BodyPair2DSW : public Constraint2DSW { int shape_A = 0; int shape_B = 0; - bool dynamic_A = false; - bool dynamic_B = false; + bool collide_A = false; + bool collide_B = false; Space2DSW *space = nullptr; diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index 14adb0bb180..55ffa9b1b81 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -186,8 +186,8 @@ public: void set_pickable(bool p_pickable) { pickable = p_pickable; } _FORCE_INLINE_ bool is_pickable() const { return pickable; } - _FORCE_INLINE_ bool layer_in_mask(CollisionObject2DSW *p_other) const { - return collision_layer & p_other->collision_mask; + _FORCE_INLINE_ bool collides_with(CollisionObject2DSW *p_other) const { + return p_other->collision_layer & collision_mask; } _FORCE_INLINE_ bool interacts_with(CollisionObject2DSW *p_other) const { diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 43329fed2f3..590c93a7beb 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -508,7 +508,7 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { keep = false; } else if (intersection_query_results[i]->get_type() == CollisionObject2DSW::TYPE_AREA) { keep = false; - } else if (!p_body->layer_in_mask(static_cast(intersection_query_results[i]))) { + } else if (!p_body->collides_with(static_cast(intersection_query_results[i]))) { 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())) { keep = false; diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index af680f23db6..c27a2ecced5 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -212,16 +212,16 @@ real_t combine_friction(Body3DSW *A, Body3DSW *B) { } bool BodyPair3DSW::setup(real_t p_step) { - dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { collided = false; return false; } + collide_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && A->collides_with(B); + collide_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && B->collides_with(A); + report_contacts_only = false; - if (!dynamic_A && !dynamic_B) { + if (!collide_A && !collide_B) { if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { report_contacts_only = true; } else { @@ -250,11 +250,11 @@ bool BodyPair3DSW::setup(real_t p_step) { if (!collided) { //test ccd (currently just a raycast) - if (A->is_continuous_collision_detection_enabled() && dynamic_A && !dynamic_B) { + if (A->is_continuous_collision_detection_enabled() && collide_A) { _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); } - if (B->is_continuous_collision_detection_enabled() && dynamic_B && !dynamic_A) { + if (B->is_continuous_collision_detection_enabled() && collide_B) { _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); } @@ -293,6 +293,15 @@ bool BodyPair3DSW::pre_solve(real_t p_step) { const Basis &basis_A = A->get_transform().basis; const Basis &basis_B = B->get_transform().basis; + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis; + const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; c.active = false; @@ -303,7 +312,7 @@ bool BodyPair3DSW::pre_solve(real_t p_step) { Vector3 axis = global_A - global_B; real_t depth = axis.dot(c.normal); - if (depth <= 0) { + if (depth <= 0.0) { continue; } @@ -339,9 +348,9 @@ bool BodyPair3DSW::pre_solve(real_t p_step) { do_process = true; // Precompute normal mass, tangent mass, and bias. - Vector3 inertia_A = A->get_inv_inertia_tensor().xform(c.rA.cross(c.normal)); - Vector3 inertia_B = B->get_inv_inertia_tensor().xform(c.rB.cross(c.normal)); - real_t kNormal = A->get_inv_mass() + B->get_inv_mass(); + Vector3 inertia_A = inv_inertia_tensor_A.xform(c.rA.cross(c.normal)); + Vector3 inertia_B = inv_inertia_tensor_B.xform(c.rB.cross(c.normal)); + real_t kNormal = inv_mass_A + inv_mass_B; kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB)); c.mass_normal = 1.0f / kNormal; @@ -349,10 +358,10 @@ bool BodyPair3DSW::pre_solve(real_t p_step) { c.depth = depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - if (dynamic_A) { + if (collide_A) { A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); } - if (dynamic_B) { + if (collide_B) { B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); } c.acc_bias_impulse = 0; @@ -378,6 +387,15 @@ void BodyPair3DSW::solve(real_t p_step) { const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis; + const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; if (!c.active) { @@ -401,10 +419,10 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - if (dynamic_A) { + if (collide_A) { A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av); } - if (dynamic_B) { + if (collide_B) { B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av); } @@ -415,16 +433,16 @@ void BodyPair3DSW::solve(real_t p_step) { vbn = dbv.dot(c.normal); if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { - real_t jbn_com = (-vbn + c.bias) / (A->get_inv_mass() + B->get_inv_mass()); + real_t jbn_com = (-vbn + c.bias) / (inv_mass_A + inv_mass_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); - if (dynamic_A) { + if (collide_A) { A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); } - if (dynamic_B) { + if (collide_B) { B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); } } @@ -446,10 +464,10 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - if (dynamic_A) { + if (collide_A) { A->apply_impulse(-j, c.rA + A->get_center_of_mass()); } - if (dynamic_B) { + if (collide_B) { B->apply_impulse(j, c.rB + B->get_center_of_mass()); } @@ -473,11 +491,11 @@ void BodyPair3DSW::solve(real_t p_step) { if (tvl > MIN_VELOCITY) { tv /= tvl; - Vector3 temp1 = A->get_inv_inertia_tensor().xform(c.rA.cross(tv)); - Vector3 temp2 = B->get_inv_inertia_tensor().xform(c.rB.cross(tv)); + Vector3 temp1 = inv_inertia_tensor_A.xform(c.rA.cross(tv)); + Vector3 temp2 = inv_inertia_tensor_B.xform(c.rB.cross(tv)); real_t t = -tvl / - (A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); + (inv_mass_A + inv_mass_B + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); Vector3 jt = t * tv; @@ -493,10 +511,10 @@ void BodyPair3DSW::solve(real_t p_step) { jt = c.acc_tangent_impulse - jtOld; - if (dynamic_A) { + if (collide_A) { A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); } - if (dynamic_B) { + if (collide_B) { B->apply_impulse(jt, c.rB + B->get_center_of_mass()); } @@ -595,13 +613,23 @@ void BodySoftBodyPair3DSW::validate_contacts() { } bool BodySoftBodyPair3DSW::setup(real_t p_step) { - body_dynamic = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); - if (!body->interacts_with(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) { collided = false; return false; } + body_collides = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && body->collides_with(soft_body); + soft_body_collides = soft_body->collides_with(body); + + if (!body_collides && !soft_body_collides) { + if (body->get_max_contacts_reported() > 0) { + report_contacts_only = true; + } else { + collided = false; + return false; + } + } + const Transform3D &xform_Au = body->get_transform(); Transform3D xform_A = xform_Au * body->get_shape_transform(body_shape); @@ -639,13 +667,20 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) { const Transform3D &transform_A = body->get_transform(); + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis; + + real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0; + 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) { + real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0; + if ((node_inv_mass == 0.0) && (body_inv_mass == 0.0)) { continue; } @@ -654,15 +689,11 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) { Vector3 axis = global_A - global_B; real_t depth = axis.dot(c.normal); - if (depth <= 0) { + if (depth <= 0.0) { continue; } - c.active = true; - do_process = true; - #ifdef DEBUG_ENABLED - if (space->is_debugging_contacts()) { space->add_debug_contact(global_A); space->add_debug_contact(global_B); @@ -677,13 +708,21 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) { 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_dynamic) { + if (report_contacts_only) { + collided = false; + continue; + } + + c.active = true; + do_process = true; + + if (body_collides) { 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; + Vector3 inertia_A = body_inv_inertia_tensor.xform(c.rA.cross(c.normal)); + real_t kNormal = body_inv_mass + node_inv_mass; kNormal += c.normal.dot(inertia_A.cross(c.rA)); c.mass_normal = 1.0f / kNormal; @@ -691,10 +730,12 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) { c.depth = depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - if (body_dynamic) { + if (body_collides) { body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass()); } - soft_body->apply_node_impulse(c.index_B, j_vec); + if (soft_body_collides) { + soft_body->apply_node_impulse(c.index_B, j_vec); + } c.acc_bias_impulse = 0; c.acc_bias_impulse_center_of_mass = 0; @@ -719,6 +760,13 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis; + + real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0; + uint32_t contact_count = contacts.size(); for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { Contact &c = contacts[contact_index]; @@ -728,6 +776,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { c.active = false; + real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0; + // 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; @@ -741,10 +791,12 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - if (body_dynamic) { + if (body_collides) { body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av); } - soft_body->apply_node_bias_impulse(c.index_B, jb); + if (soft_body_collides) { + 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; @@ -752,16 +804,18 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { 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 jbn_com = (-vbn + c.bias) / (body_inv_mass + node_inv_mass); 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); - if (body_dynamic) { + if (body_collides) { body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f); } - soft_body->apply_node_bias_impulse(c.index_B, jb_com); + if (soft_body_collides) { + soft_body->apply_node_bias_impulse(c.index_B, jb_com); + } } c.active = true; @@ -780,10 +834,12 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - if (body_dynamic) { + if (body_collides) { body->apply_impulse(-j, c.rA + body->get_center_of_mass()); } - soft_body->apply_node_impulse(c.index_B, j); + if (soft_body_collides) { + soft_body->apply_node_impulse(c.index_B, j); + } c.active = true; } @@ -804,10 +860,10 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { if (tvl > MIN_VELOCITY) { tv /= tvl; - Vector3 temp1 = body->get_inv_inertia_tensor().xform(c.rA.cross(tv)); + Vector3 temp1 = body_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))); + (body_inv_mass + node_inv_mass + tv.dot(temp1.cross(c.rA))); Vector3 jt = t * tv; @@ -823,10 +879,12 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { jt = c.acc_tangent_impulse - jtOld; - if (body_dynamic) { + if (body_collides) { body->apply_impulse(-jt, c.rA + body->get_center_of_mass()); } - soft_body->apply_node_impulse(c.index_B, jt); + if (soft_body_collides) { + soft_body->apply_node_impulse(c.index_B, jt); + } c.active = true; } diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h index 976982d1f11..19d6a46880d 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -83,8 +83,8 @@ class BodyPair3DSW : public BodyContact3DSW { int shape_A = 0; int shape_B = 0; - bool dynamic_A = false; - bool dynamic_B = false; + bool collide_A = false; + bool collide_B = false; bool report_contacts_only = false; @@ -115,7 +115,10 @@ class BodySoftBodyPair3DSW : public BodyContact3DSW { int body_shape = 0; - bool body_dynamic = false; + bool body_collides = false; + bool soft_body_collides = false; + + bool report_contacts_only = false; LocalVector contacts; diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index fb8dca8bb42..6ffab546450 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -166,8 +166,8 @@ public: } _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } - _FORCE_INLINE_ bool layer_in_mask(CollisionObject3DSW *p_other) const { - return collision_layer & p_other->collision_mask; + _FORCE_INLINE_ bool collides_with(CollisionObject3DSW *p_other) const { + return p_other->collision_layer & collision_mask; } _FORCE_INLINE_ bool interacts_with(CollisionObject3DSW *p_other) const { diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index bc149648d7a..1037243d3b0 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -549,7 +549,7 @@ 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_SOFT_BODY) { keep = false; - } else if (!p_body->layer_in_mask(static_cast(intersection_query_results[i]))) { + } else if (!p_body->collides_with(static_cast(intersection_query_results[i]))) { 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())) { keep = false;