summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--doc/classes/CollisionObject2D.xml4
-rw-r--r--doc/classes/CollisionObject3D.xml8
-rw-r--r--doc/classes/SoftBody3D.xml8
-rw-r--r--modules/bullet/godot_result_callbacks.cpp14
-rw-r--r--modules/bullet/space_bullet.cpp2
-rw-r--r--servers/physics_2d/body_pair_2d_sw.cpp40
-rw-r--r--servers/physics_2d/body_pair_2d_sw.h4
-rw-r--r--servers/physics_2d/collision_object_2d_sw.h4
-rw-r--r--servers/physics_2d/space_2d_sw.cpp2
-rw-r--r--servers/physics_3d/body_pair_3d_sw.cpp156
-rw-r--r--servers/physics_3d/body_pair_3d_sw.h9
-rw-r--r--servers/physics_3d/collision_object_3d_sw.h4
-rw-r--r--servers/physics_3d/space_3d_sw.cpp2
13 files changed, 162 insertions, 95 deletions
diff --git a/doc/classes/CollisionObject2D.xml b/doc/classes/CollisionObject2D.xml
index b6269520e1..6bb756ea2c 100644
--- a/doc/classes/CollisionObject2D.xml
+++ b/doc/classes/CollisionObject2D.xml
@@ -200,11 +200,11 @@
<members>
<member name="collision_layer" type="int" setter="set_collision_layer" getter="get_collision_layer" default="1">
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.
</member>
<member name="collision_mask" type="int" setter="set_collision_mask" getter="get_collision_mask" default="1">
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.
</member>
<member name="disable_mode" type="int" setter="set_disable_mode" getter="get_disable_mode" enum="CollisionObject2D.DisableMode" default="0">
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 ecfe758343..0210f6297f 100644
--- a/doc/classes/CollisionObject3D.xml
+++ b/doc/classes/CollisionObject3D.xml
@@ -171,12 +171,12 @@
</methods>
<members>
<member name="collision_layer" type="int" setter="set_collision_layer" getter="get_collision_layer" default="1">
- 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.
</member>
<member name="collision_mask" type="int" setter="set_collision_mask" getter="get_collision_mask" default="1">
- 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.
</member>
<member name="disable_mode" type="int" setter="set_disable_mode" getter="get_disable_mode" enum="CollisionObject3D.DisableMode" default="0">
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 16f80c43bc..53bd7e67bf 100644
--- a/doc/classes/SoftBody3D.xml
+++ b/doc/classes/SoftBody3D.xml
@@ -68,12 +68,12 @@
</methods>
<members>
<member name="collision_layer" type="int" setter="set_collision_layer" getter="get_collision_layer" default="1">
- The physics layers this SoftBody3D is in.
- Collidable objects can exist in any of 32 different layers. These layers work like a tagging system, and are not visual. A collidable can use these layers to select with which objects it can collide, using the collision_mask property.
- 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.
</member>
<member name="collision_mask" type="int" setter="set_collision_mask" getter="get_collision_mask" default="1">
- 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.
</member>
<member name="damping_coefficient" type="float" setter="set_damping_coefficient" getter="get_damping_coefficient" default="0.01">
</member>
diff --git a/modules/bullet/godot_result_callbacks.cpp b/modules/bullet/godot_result_callbacks.cpp
index 1fd656c9b4..399b102284 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<btCollisionObject *>(proxy0->m_clientObject);
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(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<btCollisionObject *>(proxy0->m_clientObject);
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(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<btCollisionObject *>(proxy0->m_clientObject);
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(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<btCollisionObject *>(proxy0->m_clientObject);
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(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<btCollisionObject *>(proxy0->m_clientObject);
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(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<btCollisionObject *>(proxy0->m_clientObject);
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(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<btCollisionObject *>(proxy0->m_clientObject);
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index 8c286a8629..583900e6bc 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<btCollisionObject *>(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<btCompoundShape *>(co->getCollisionShape());
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index 71d985a6c5..91d747b492 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 4b42b44c92..849a7e2430 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 14adb0bb18..55ffa9b1b8 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 43329fed2f..590c93a7be 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<Body2DSW *>(intersection_query_results[i]))) {
+ } else if (!p_body->collides_with(static_cast<Body2DSW *>(intersection_query_results[i]))) {
keep = false;
} else if (static_cast<Body2DSW *>(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 af680f23db..c27a2ecced 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 976982d1f1..19d6a46880 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<Contact> contacts;
diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h
index fb8dca8bb4..6ffab54645 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 bc149648d7..1037243d3b 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<Body3DSW *>(intersection_query_results[i]))) {
+ } else if (!p_body->collides_with(static_cast<Body3DSW *>(intersection_query_results[i]))) {
keep = false;
} else if (static_cast<Body3DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
keep = false;
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-12 13:39:26 +0000