Skip to content

Commit

Permalink
Fix rigid body ray cast CCD in 2D and 3D Godot Physics
Browse files Browse the repository at this point in the history 
For 2D:
Raycast CCD now works the same as in 3D, it changes the body's velocity
to place it at the impact position instead of generating a contact point
that causes a wrong push back.

For both 2D and 3D:
The raycast CCD process reads and modifies body velocities, so it needs
to be moved to pre_solve() instead of setup() to be processed linearly
on the main thread, otherwise multithreading can cause some CCD results
to be randomly lost when multiple collisions occur.
  • Loading branch information
@pouleyKetchoupp
pouleyKetchoupp committed Dec 10, 2021
1 parent f1e3c87 commit 30a608b
Show file tree
Hide file tree
Showing 4 changed files with 94 additions and 40 deletions.
84 changes: 55 additions & 29 deletions servers/physics_2d/godot_body_pair_2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,7 +160,7 @@ void GodotBodyPair2D::_validate_contacts() {
}
}

bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result) {
bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B) {
Vector2 motion = p_A->get_linear_velocity() * p_step;
real_t mlen = motion.length();
if (mlen < CMP_EPSILON) {
Expand All @@ -171,14 +171,18 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,

real_t min, max;
p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max);
bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction

if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
// Did it move enough in this direction to even attempt raycast?
// Let's say it should move more than 1/3 the size of the object in that axis.
bool fast_object = mlen > (max - min) * 0.3;
if (!fast_object) {
return false;
}

//cast a segment from support in motion normal, in the same direction of motion by motion length
//support is the worst case collision point, so real collision happened before
// Going too fast in that direction.

// Cast a segment from support in motion normal, in the same direction of motion by motion length.
// Support is the worst case collision point, so real collision happened before.
int a;
Vector2 s[2];
p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a);
Expand All @@ -187,28 +191,31 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,

Transform2D from_inv = p_xform_B.affine_inverse();

Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box
// Start from a little inside the bounding box.
Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1);
Vector2 local_to = from_inv.xform(to);

Vector2 rpos, rnorm;
if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) {
return false;
}

//ray hit something
// Check one-way collision based on motion direction.
if (p_A->get_shape(p_shape_A)->allows_one_way_collision() && p_B->is_shape_set_as_one_way_collision(p_shape_B)) {
Vector2 direction = p_xform_B.get_axis(1).normalized();
if (direction.dot(mnormal) < CMP_EPSILON) {
collided = false;
oneway_disabled = true;
return false;
}
}

// Shorten the linear velocity so it does not hit, but gets close enough,
// next frame will hit softly or soft enough.
Vector2 hitpos = p_xform_B.xform(rpos);

Vector2 contact_A = to;
Vector2 contact_B = hitpos;

//create a contact

if (p_swap_result) {
_contact_added_callback(contact_B, contact_A);
} else {
_contact_added_callback(contact_A, contact_B);
}
real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01;
p_A->set_linear_velocity(mnormal * (newlen / p_step));

return true;
}
Expand All @@ -222,6 +229,8 @@ real_t combine_friction(GodotBody2D *A, GodotBody2D *B) {
}

bool GodotBodyPair2D::setup(real_t p_step) {
check_ccd = false;

if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
collided = false;
return false;
Expand Down Expand Up @@ -269,24 +278,19 @@ bool GodotBodyPair2D::setup(real_t p_step) {

collided = GodotCollisionSolver2D::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis);
if (!collided) {
//test ccd (currently just a raycast)
oneway_disabled = false;

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;
}
check_ccd = true;
return true;
}

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;
}
check_ccd = true;
return true;
}

if (!collided) {
oneway_disabled = false;
return false;
}
return false;
}

if (oneway_disabled) {
Expand Down Expand Up @@ -335,7 +339,29 @@ bool GodotBodyPair2D::setup(real_t p_step) {
}

bool GodotBodyPair2D::pre_solve(real_t p_step) {
if (!collided || oneway_disabled) {
if (oneway_disabled) {
return false;
}

if (!collided) {
if (check_ccd) {
const Vector2 &offset_A = A->get_transform().get_origin();
Transform2D xform_Au = A->get_transform().untranslated();
Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);

Transform2D xform_Bu = B->get_transform();
xform_Bu.elements[2] -= offset_A;
Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);

if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) {
_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
}

if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) {
_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
}
}

return false;
}

Expand Down
3 changes: 2 additions & 1 deletion servers/physics_2d/godot_body_pair_2d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,10 +79,11 @@ class GodotBodyPair2D : public GodotConstraint2D {
Contact contacts[MAX_CONTACTS];
int contact_count = 0;
bool collided = false;
bool check_ccd = false;
bool oneway_disabled = false;
bool report_contacts_only = false;

bool _test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false);
bool _test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B);
void _validate_contacts();
static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self);
_FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B);
Expand Down
46 changes: 36 additions & 10 deletions servers/physics_3d/godot_body_pair_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,29 +172,35 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A,

real_t min, max;
p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max);
bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction

if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
// Did it move enough in this direction to even attempt raycast?
// Let's say it should move more than 1/3 the size of the object in that axis.
bool fast_object = mlen > (max - min) * 0.3;
if (!fast_object) {
return false;
}

//cast a segment from support in motion normal, in the same direction of motion by motion length
//support is the worst case collision point, so real collision happened before
// Going too fast in that direction.

// Cast a segment from support in motion normal, in the same direction of motion by motion length.
// Support is the worst case collision point, so real collision happened before.
Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized());
Vector3 from = p_xform_A.xform(s);
Vector3 to = from + motion;

Transform3D from_inv = p_xform_B.affine_inverse();

Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box
// Start from a little inside the bounding box.
Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1);
Vector3 local_to = from_inv.xform(to);

Vector3 rpos, rnorm;
if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm, true)) {
return false;
}

//shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough
// Shorten the linear velocity so it does not hit, but gets close enough,
// next frame will hit softly or soft enough.
Vector3 hitpos = p_xform_B.xform(rpos);

real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01;
Expand All @@ -212,6 +218,8 @@ real_t combine_friction(GodotBody3D *A, GodotBody3D *B) {
}

bool GodotBodyPair3D::setup(real_t p_step) {
check_ccd = false;

if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
collided = false;
return false;
Expand Down Expand Up @@ -248,14 +256,14 @@ bool GodotBodyPair3D::setup(real_t p_step) {
collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);

if (!collided) {
//test ccd (currently just a raycast)

if (A->is_continuous_collision_detection_enabled() && collide_A) {
_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
check_ccd = true;
return true;
}

if (B->is_continuous_collision_detection_enabled() && collide_B) {
_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
check_ccd = true;
return true;
}

return false;
Expand All @@ -266,6 +274,24 @@ bool GodotBodyPair3D::setup(real_t p_step) {

bool GodotBodyPair3D::pre_solve(real_t p_step) {
if (!collided) {
if (check_ccd) {
const Vector3 &offset_A = A->get_transform().get_origin();
Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3());
Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A);

Transform3D xform_Bu = B->get_transform();
xform_Bu.origin -= offset_A;
Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_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() && collide_B) {
_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
}
}

return false;
}

Expand Down
1 change: 1 addition & 0 deletions servers/physics_3d/godot_body_pair_3d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ class GodotBodyContact3D : public GodotConstraint3D {

Vector3 sep_axis;
bool collided = false;
bool check_ccd = false;

GodotSpace3D *space = nullptr;

Expand Down

0 comments on commit 30a608b

Please sign in to comment.