Sara/behaviour-tree-test
1
0
Fork 0
Code Issues Pull requests Projects Releases 2 Packages Wiki Activity Actions

Merge pull request #50625 from nekomatata/body-one-direction-layers

Browse source 
One-directional collision layer check for rigid bodies and soft bodies
This commit is contained in:
Rémi Verschelde 2021-07-31 22:12:46 +02:00 • committed by GitHub
commit 6acbcf7a86
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
13 changed files with 162 additions and 95 deletions
Download patch file Download diff file Expand all files Collapse all files

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;