godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix rigid body ray cast CCD in 2D and 3D Godot Physics 

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.
This commit is contained in:
PouleyKetchoupp 2021-12-09 16:36:39 -07:00
parent f1e3c87244
commit 30a608b7b9
4 changed files with 94 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
servers/physics_2d/godot_body_pair_2d.cpp
View File

@ -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) {
@ -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);
@ -187,7 +191,8 @@ 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;
@ -195,20 +200,22 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,
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;
}
@ -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;
@ -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) {
@ -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;
}

3
servers/physics_2d/godot_body_pair_2d.h
View File

@ -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);

46
servers/physics_3d/godot_body_pair_3d.cpp
View File

@ -172,21 +172,26 @@ 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;
@ -194,7 +199,8 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A,
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;
@ -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;
@ -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;
@ -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;
}

1
servers/physics_3d/godot_body_pair_3d.h
View File

@ -60,6 +60,7 @@ protected:
Vector3 sep_axis;
bool collided = false;
bool check_ccd = false;
GodotSpace3D *space = nullptr;