Nixyri/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2024-11-10 14:12:51 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

One-directional collision layer check for rigid bodies and soft bodies

Browse Source 
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.
This commit is contained in:
PouleyKetchoupp 2021-07-15 12:38:48 -07:00
parent 617327118b
commit 940838c174
13 changed files with 162 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
doc/classes/CollisionObject2D.xml
View File

@ -260,11 +260,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.

8
doc/classes/CollisionObject3D.xml
View File

@ -223,12 +223,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.

8
doc/classes/SoftBody3D.xml
View File

@ -84,12 +84,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>

14
modules/bullet/godot_result_callbacks.cpp
View File

@ -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());

2
modules/bullet/space_bullet.cpp
View File

@ -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());

40
servers/physics_2d/body_pair_2d_sw.cpp
View File

@ -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);
}
}

4
servers/physics_2d/body_pair_2d_sw.h
View File

@ -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;

4
servers/physics_2d/collision_object_2d_sw.h
View File

@ -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 {

2
servers/physics_2d/space_2d_sw.cpp
View File

@ -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;

156
servers/physics_3d/body_pair_3d_sw.cpp
View File

@ -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;
}

9
servers/physics_3d/body_pair_3d_sw.h
View File

@ -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;

4
servers/physics_3d/collision_object_3d_sw.h
View File

@ -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 {

2
servers/physics_3d/space_3d_sw.cpp
View File

@ -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;