Fix test_body_motion recovery
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
parent
a59286f019
commit
cff7a69aa0
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue