asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Fix test_body_motion recovery

Browse source 
This change makes test_body_motion more reliable when the kinematic body
recovers from being stuck.

- When recovery occurs, the rest information is generated, in order to
make sure collision results from test_move, move_and_collide and
move_and_slide are consistent and return a collision in case of overlap.

- The new calculation for recovery vector makes sure the recovery is
never more than the overlap depth between shapes.
This can help with cases where the kinematic body overlaps with several
shapes.
Recovery is made iteratively, without forcing a full overlap at each
step. This helps with getting proper rest information when recovery
occurs.

- One Way Collision:
When attempting motion, contact direction is checked against motion
before skipping in order to solve cases where kinematic bodies can sink
into one-way collision shapes.
Rest info now sets max contact depth in order to properly handle one-way
collision.

- Low speed motion is now handled in the rest info, by never setting
min_allowed_depth lower than motion length.
Separation is always applied with full margin, otherwise contact is lost
when low speed motion occurs right after higher speed motion.

- Similar changes are applied to 3D in order to make 2D and 3D
consistent.
This commit is contained in:
PouleyKetchoupp 2021-02-17 18:27:19 -07:00
parent a59286f019
commit cff7a69aa0
2 changed files with 125 additions and 60 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

86
servers/physics_2d/space_2d_sw.cpp
View file

@ -376,6 +376,7 @@ struct _RestCallbackData2D {
Vector2 best_normal;
real_t best_len;
Vector2 valid_dir;
real_t valid_depth;
real_t min_allowed_depth;
};
@ -385,16 +386,6 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
Vector2 contact_rel = p_point_B - p_point_A;
real_t len = contact_rel.length();
if (len == 0) {
return;
}
Vector2 normal = contact_rel / len;
if (rd->valid_dir != Vector2() && rd->valid_dir.dot(normal) > -CMP_EPSILON) {
return;
}
if (len < rd->min_allowed_depth) {
return;
}
@ -403,6 +394,18 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
return;
}
Vector2 normal = contact_rel / len;
if (rd->valid_dir != Vector2()) {
if (len > rd->valid_depth) {
return;
}
if (rd->valid_dir.dot(normal) > -CMP_EPSILON) {
return;
}
}
rd->best_len = len;
rd->best_contact = p_point_B;
rd->best_normal = normal;
@ -739,10 +742,13 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs];
int excluded_shape_pair_count = 0;
real_t separation_margin = MIN(p_margin, MAX(0.0, p_motion.length() - CMP_EPSILON)); //don't separate by more than the intended motion
real_t motion_length = p_motion.length();
Vector2 motion_normal = p_motion / motion_length;
Transform2D body_transform = p_from;
bool recovered = false;
{
//STEP 1, FREE BODY IF STUCK
@ -819,7 +825,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
bool did_collide = false;
Shape2DSW *against_shape = col_obj->get_shape(shape_idx);
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, separation_margin)) {
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_margin)) {
did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
}
@ -845,11 +851,20 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
}
Vector2 recover_motion;
for (int i = 0; i < cbk.amount; i++) {
Vector2 a = sr[i * 2 + 0];
Vector2 b = sr[i * 2 + 1];
recover_motion += (b - a) / cbk.amount;
// Compute plane on b towards a.
Vector2 n = (a - b).normalized();
real_t d = n.dot(b);
// Compute depth on recovered motion.
real_t depth = n.dot(a + recover_motion) - d;
if (depth > 0.0) {
// Only recover if there is penetration.
recover_motion -= n * depth * 0.4;
}
}
if (recover_motion == Vector2()) {
@ -857,6 +872,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
break;
}
recovered = true;
body_transform.elements[2] += recover_motion;
body_aabb.position += recover_motion;
@ -929,7 +946,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
//test initial overlap
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
continue;
Vector2 direction = col_obj_shape_xform.get_axis(1).normalized();
if (motion_normal.dot(direction) < 0) {
continue;
}
}
stuck = true;
@ -939,13 +959,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
//just do kinematic solving
real_t low = 0;
real_t hi = 1;
Vector2 mnormal = p_motion.normalized();
for (int k = 0; k < 8; k++) { //steps should be customizable..
real_t ofs = (low + hi) * 0.5;
Vector2 sep = mnormal; //important optimization for this to work fast enough
Vector2 sep = motion_normal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
if (collided) {
@ -966,7 +985,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
cbk.valid_depth = 10e20;
Vector2 sep = mnormal; //important optimization for this to work fast enough
Vector2 sep = motion_normal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), PhysicsServer2DSW::_shape_col_cbk, &cbk, &sep, 0);
if (!collided || cbk.amount == 0) {
continue;
@ -997,11 +1016,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
}
bool collided = false;
if (safe >= 1) {
best_shape = -1; //no best shape with cast, reset to -1
}
if (safe < 1) {
if (recovered || (safe < 1)) {
if (safe >= 1) {
best_shape = -1; //no best shape with cast, reset to -1
}
//it collided, let's get the rest info in unsafe advance
Transform2D ugt = body_transform;
ugt.elements[2] += p_motion * unsafe;
@ -1010,9 +1030,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
rcd.best_len = 0;
rcd.best_object = nullptr;
rcd.best_shape = 0;
rcd.min_allowed_depth = test_motion_min_contact_depth;
//optimization
// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth);
int from_shape = best_shape != -1 ? best_shape : 0;
int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
@ -1060,8 +1081,25 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
rcd.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it won't work
if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
const Body2DSW *b = static_cast<const Body2DSW *>(col_obj);
if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) {
//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
Vector2 lv = b->get_linear_velocity();
//compute displacement from linear velocity
Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step;
real_t motion_len = motion.length();
motion.normalize();
rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0);
}
}
} else {
rcd.valid_dir = Vector2();
rcd.valid_depth = 0;
}
rcd.object = col_obj;

99
servers/physics_3d/space_3d_sw.cpp
View file

@ -384,6 +384,8 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_
struct _RestCallbackData {
const CollisionObject3DSW *object;
const CollisionObject3DSW *best_object;
int local_shape;
int best_local_shape;
int shape;
int best_shape;
Vector3 best_contact;
@ -409,6 +411,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B,
rd->best_normal = contact_rel / len;
rd->best_object = rd->object;
rd->best_shape = rd->shape;
rd->best_local_shape = rd->local_shape;
}
bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
@ -739,8 +742,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
body_aabb = body_aabb.grow(p_margin);
real_t motion_length = p_motion.length();
Vector3 motion_normal = p_motion / motion_length;
Transform body_transform = p_from;
bool recovered = false;
{
//STEP 1, FREE BODY IF STUCK
@ -791,7 +799,17 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
for (int i = 0; i < cbk.amount; i++) {
Vector3 a = sr[i * 2 + 0];
Vector3 b = sr[i * 2 + 1];
recover_motion += (b - a) / cbk.amount;
// Compute plane on b towards a.
Vector3 n = (a - b).normalized();
real_t d = n.dot(b);
// Compute depth on recovered motion.
real_t depth = n.dot(a + recover_motion) - d;
if (depth > 0.0) {
// Only recover if there is penetration.
recover_motion -= n * depth * 0.4;
}
}
if (recover_motion == Vector3()) {
@ -799,6 +817,8 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
break;
}
recovered = true;
body_transform.origin += recover_motion;
body_aabb.position += recover_motion;
@ -848,14 +868,14 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
//test initial overlap, does it collide if going all the way?
Vector3 point_A, point_B;
Vector3 sep_axis = p_motion.normalized();
Vector3 sep_axis = motion_normal;
Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
continue;
}
sep_axis = p_motion.normalized();
sep_axis = motion_normal;
if (!CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
stuck = true;
@ -865,13 +885,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
//just do kinematic solving
real_t low = 0;
real_t hi = 1;
Vector3 mnormal = p_motion.normalized();
for (int k = 0; k < 8; k++) { //steps should be customizable..
real_t ofs = (low + hi) * 0.5;
Vector3 sep = mnormal; //important optimization for this to work fast enough
Vector3 sep = motion_normal; //important optimization for this to work fast enough
mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs);
@ -912,16 +931,11 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
}
bool collided = false;
if (safe >= 1) {
//not collided
collided = false;
if (r_result) {
r_result->motion = p_motion;
r_result->remainder = Vector3();
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
if (recovered || (safe < 1)) {
if (safe >= 1) {
best_shape = -1; //no best shape with cast, reset to -1
}
} else {
//it collided, let's get the rest info in unsafe advance
Transform ugt = body_transform;
ugt.origin += p_motion * unsafe;
@ -930,25 +944,40 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
rcd.best_len = 0;
rcd.best_object = nullptr;
rcd.best_shape = 0;
rcd.min_allowed_depth = test_motion_min_contact_depth;
Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
Shape3DSW *body_shape = p_body->get_shape(best_shape);
// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth);
body_aabb.position += p_motion * unsafe;
int from_shape = best_shape != -1 ? best_shape : 0;
int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int i = 0; i < amount; i++) {
const CollisionObject3DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
rcd.object = col_obj;
rcd.shape = shape_idx;
bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin);
if (!sc) {
for (int j = from_shape; j < to_shape; j++) {
if (p_body->is_shape_set_as_disabled(j)) {
continue;
}
Transform body_shape_xform = ugt * p_body->get_shape_transform(j);
Shape3DSW *body_shape = p_body->get_shape(j);
if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) {
continue;
}
body_aabb.position += p_motion * unsafe;
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int i = 0; i < amount; i++) {
const CollisionObject3DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
rcd.object = col_obj;
rcd.shape = shape_idx;
bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin);
if (!sc) {
continue;
}
}
}
if (rcd.best_len != 0) {
@ -956,7 +985,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
r_result->collider = rcd.best_object->get_self();
r_result->collider_id = rcd.best_object->get_instance_id();
r_result->collider_shape = rcd.best_shape;
r_result->collision_local_shape = best_shape;
r_result->collision_local_shape = rcd.best_local_shape;
r_result->collision_normal = rcd.best_normal;
r_result->collision_point = rcd.best_contact;
//r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
@ -972,17 +1001,15 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
}
collided = true;
} else {
if (r_result) {
r_result->motion = p_motion;
r_result->remainder = Vector3();
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}
collided = false;
}
}
if (!collided && r_result) {
r_result->motion = p_motion;
r_result->remainder = Vector3();
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}
return collided;
}