Skip to content

Commit

Permalink
Merge pull request #47266 from nekomatata/solver-kinematic-bug-fix-3.4
Browse files Browse the repository at this point in the history 
[3.4] Fix GodotPhysics solver with kinematic body set to report contacts
  • Loading branch information
@akien-mga
akien-mga authored Aug 4, 2021
2 parents 2269c2e + 6fdffac commit d5a0b2e
Show file tree
Hide file tree
Showing 2 changed files with 38 additions and 24 deletions.
20 changes: 16 additions & 4 deletions servers/physics/body_pair_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -211,11 +211,21 @@ real_t combine_friction(BodySW *A, BodySW *B) {

bool BodyPairSW::setup(real_t p_step) {
//cannot collide
if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) {
if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
collided = false;
return false;
}

bool report_contacts_only = false;
if ((A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) {
if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
report_contacts_only = true;
} else {
collided = false;
return false;
}
}

offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();

validate_contacts();
Expand Down Expand Up @@ -274,12 +284,9 @@ bool BodyPairSW::setup(real_t p_step) {
real_t depth = c.normal.dot(global_A - global_B);

if (depth <= 0) {
c.active = false;
continue;
}

c.active = true;

#ifdef DEBUG_ENABLED

if (space->is_debugging_contacts()) {
Expand All @@ -303,6 +310,11 @@ bool BodyPairSW::setup(real_t p_step) {
B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB);
}

if (report_contacts_only) {
collided = false;
continue;
}

c.active = true;

// Precompute normal mass, tangent mass, and bias.
Expand Down
42 changes: 22 additions & 20 deletions servers/physics_2d/body_pair_2d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -221,11 +221,21 @@ real_t combine_friction(Body2DSW *A, Body2DSW *B) {

bool BodyPair2DSW::setup(real_t p_step) {
//cannot collide
if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) {
if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
collided = false;
return false;
}

bool report_contacts_only = false;
if ((A->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC) && (B->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC)) {
if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
report_contacts_only = true;
} else {
collided = false;
return false;
}
}

//use local A coordinates to avoid numerical issues on collision detection
offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();

Expand Down Expand Up @@ -344,52 +354,44 @@ bool BodyPair2DSW::setup(real_t p_step) {

for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
c.active = false;

Vector2 global_A = xform_Au.xform(c.local_A);
Vector2 global_B = xform_Bu.xform(c.local_B);

real_t depth = c.normal.dot(global_A - global_B);

if (depth <= 0 || !c.reused) {
c.active = false;
continue;
}

c.active = true;
#ifdef DEBUG_ENABLED
if (space->is_debugging_contacts()) {
space->add_debug_contact(global_A + offset_A);
space->add_debug_contact(global_B + offset_A);
}
#endif
int gather_A = A->can_report_contacts();
int gather_B = B->can_report_contacts();

c.rA = global_A;
c.rB = global_B - offset_B;

if (gather_A | gather_B) {
//Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );

global_A += offset_A;
global_B += offset_A;
if (A->can_report_contacts()) {
Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity());
}

if (gather_A) {
Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity());
}
if (gather_B) {
Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity());
}
if (B->can_report_contacts()) {
Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity());
}

if ((A->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC)) {
c.active = false;
if (report_contacts_only) {
collided = false;
continue;
}

c.active = true;

// Precompute normal mass, tangent mass, and bias.
real_t rnA = c.rA.dot(c.normal);
real_t rnB = c.rB.dot(c.normal);
Expand Down

0 comments on commit d5a0b2e

Please sign in to comment.