From 940838c1742c7f7ba9ca94321502ac3263252255 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Thu, 15 Jul 2021 12:38:48 -0700 Subject: 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. --- servers/physics_3d/body_pair_3d_sw.cpp | 156 ++++++++++++++++++++++----------- 1 file changed, 107 insertions(+), 49 deletions(-) (limited to 'servers/physics_3d/body_pair_3d_sw.cpp') 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; } -- cgit v1.2.3