Merge pull request #46917 from nekomatata/solver-kinematic-bug-fix
Fix GodotPhysics solver with kinematic body set to report contacts
This commit is contained in:
commit
992de9c053
2 changed files with 40 additions and 25 deletions
|
@ -221,11 +221,21 @@ real_t combine_friction(Body2DSW *A, Body2DSW *B) {
|
||||||
|
|
||||||
bool BodyPair2DSW::setup(real_t p_step) {
|
bool BodyPair2DSW::setup(real_t p_step) {
|
||||||
//cannot collide
|
//cannot collide
|
||||||
if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer2D::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;
|
collided = false;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool report_contacts_only = false;
|
||||||
|
if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
|
if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
|
||||||
collided = false;
|
collided = false;
|
||||||
return false;
|
return false;
|
||||||
|
@ -350,51 +360,44 @@ bool BodyPair2DSW::setup(real_t p_step) {
|
||||||
for (int i = 0; i < contact_count; i++) {
|
for (int i = 0; i < contact_count; i++) {
|
||||||
Contact &c = contacts[i];
|
Contact &c = contacts[i];
|
||||||
|
|
||||||
|
c.active = false;
|
||||||
|
|
||||||
Vector2 global_A = xform_Au.xform(c.local_A);
|
Vector2 global_A = xform_Au.xform(c.local_A);
|
||||||
Vector2 global_B = xform_Bu.xform(c.local_B);
|
Vector2 global_B = xform_Bu.xform(c.local_B);
|
||||||
|
|
||||||
real_t depth = c.normal.dot(global_A - global_B);
|
real_t depth = c.normal.dot(global_A - global_B);
|
||||||
|
|
||||||
if (depth <= 0 || !c.reused) {
|
if (depth <= 0 || !c.reused) {
|
||||||
c.active = false;
|
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
c.active = true;
|
|
||||||
#ifdef DEBUG_ENABLED
|
#ifdef DEBUG_ENABLED
|
||||||
if (space->is_debugging_contacts()) {
|
if (space->is_debugging_contacts()) {
|
||||||
space->add_debug_contact(global_A + offset_A);
|
space->add_debug_contact(global_A + offset_A);
|
||||||
space->add_debug_contact(global_B + offset_A);
|
space->add_debug_contact(global_B + offset_A);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
int gather_A = A->can_report_contacts();
|
|
||||||
int gather_B = B->can_report_contacts();
|
|
||||||
|
|
||||||
c.rA = global_A;
|
c.rA = global_A;
|
||||||
c.rB = global_B - offset_B;
|
c.rB = global_B - offset_B;
|
||||||
|
|
||||||
if (gather_A | gather_B) {
|
if (A->can_report_contacts()) {
|
||||||
//Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
|
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());
|
||||||
global_A += offset_A;
|
|
||||||
global_B += offset_A;
|
|
||||||
|
|
||||||
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 ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) {
|
if (B->can_report_contacts()) {
|
||||||
c.active = false;
|
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 (report_contacts_only) {
|
||||||
collided = false;
|
collided = false;
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
c.active = true;
|
||||||
|
|
||||||
// Precompute normal mass, tangent mass, and bias.
|
// Precompute normal mass, tangent mass, and bias.
|
||||||
real_t rnA = c.rA.dot(c.normal);
|
real_t rnA = c.rA.dot(c.normal);
|
||||||
real_t rnB = c.rB.dot(c.normal);
|
real_t rnB = c.rB.dot(c.normal);
|
||||||
|
|
|
@ -213,11 +213,21 @@ real_t combine_friction(Body3DSW *A, Body3DSW *B) {
|
||||||
|
|
||||||
bool BodyPair3DSW::setup(real_t p_step) {
|
bool BodyPair3DSW::setup(real_t p_step) {
|
||||||
//cannot collide
|
//cannot collide
|
||||||
if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::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;
|
collided = false;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool report_contacts_only = false;
|
||||||
|
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
|
if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
|
||||||
collided = false;
|
collided = false;
|
||||||
return false;
|
return false;
|
||||||
|
@ -281,12 +291,9 @@ bool BodyPair3DSW::setup(real_t p_step) {
|
||||||
real_t depth = c.normal.dot(global_A - global_B);
|
real_t depth = c.normal.dot(global_A - global_B);
|
||||||
|
|
||||||
if (depth <= 0) {
|
if (depth <= 0) {
|
||||||
c.active = false;
|
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
c.active = true;
|
|
||||||
|
|
||||||
#ifdef DEBUG_ENABLED
|
#ifdef DEBUG_ENABLED
|
||||||
|
|
||||||
if (space->is_debugging_contacts()) {
|
if (space->is_debugging_contacts()) {
|
||||||
|
@ -310,6 +317,11 @@ bool BodyPair3DSW::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);
|
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;
|
c.active = true;
|
||||||
|
|
||||||
// Precompute normal mass, tangent mass, and bias.
|
// Precompute normal mass, tangent mass, and bias.
|
||||||
|
|
Loading…
Reference in a new issue