Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[3.2] Fix test_body_motion recovery and rest info #46149

Merged
merged 1 commit into from
Feb 18, 2021
Merged

[3.2] Fix test_body_motion recovery and rest info #46149

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
97 changes: 62 additions & 35 deletions servers/physics/space_sw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -378,6 +378,8 @@ struct _RestCallbackData {

const CollisionObjectSW *object;
const CollisionObjectSW *best_object;
int local_shape;
int best_local_shape;
int shape;
int best_shape;
Vector3 best_contact;
Expand All @@ -402,6 +404,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 PhysicsDirectSpaceStateSW::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) {

Expand Down Expand Up @@ -737,8 +740,13 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
body_aabb = body_aabb.grow(p_margin);

float 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

Expand Down Expand Up @@ -791,14 +799,26 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve

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();
float d = n.dot(b);

// Compute depth on recovered motion.
float 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()) {
collided = false;
break;
}

recovered = true;

body_transform.origin += recover_motion;
body_aabb.position += recover_motion;

Expand Down Expand Up @@ -849,14 +869,14 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve

//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 (CollisionSolverSW::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 (!CollisionSolverSW::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;
Expand All @@ -866,13 +886,12 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
//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);

Expand Down Expand Up @@ -917,18 +936,12 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
}

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;
Expand All @@ -937,25 +950,41 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
rcd.best_len = 0;
rcd.best_object = NULL;
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);
ShapeSW *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 j = from_shape; j < to_shape; j++) {

for (int i = 0; i < amount; i++) {
if (p_body->is_shape_set_as_disabled(j))
continue;

const CollisionObjectSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
Transform body_shape_xform = ugt * p_body->get_shape_transform(j);
ShapeSW *body_shape = p_body->get_shape(j);

rcd.object = col_obj;
rcd.shape = shape_idx;
bool sc = CollisionSolverSW::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, NULL, p_margin);
if (!sc)
if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::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 CollisionObjectSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];

rcd.object = col_obj;
rcd.shape = shape_idx;
rcd.local_shape = j;
bool sc = CollisionSolverSW::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, NULL, p_margin);
if (!sc)
continue;
}
}

if (rcd.best_len != 0) {
Expand All @@ -964,7 +993,7 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
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);
Expand All @@ -980,16 +1009,14 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
}

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());
}
if (!collided && r_result) {

collided = false;
}
r_result->motion = p_motion;
r_result->remainder = Vector3();
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}

return collided;
Expand Down
86 changes: 63 additions & 23 deletions servers/physics_2d/space_2d_sw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -370,6 +370,7 @@ struct _RestCallbackData2D {
Vector2 best_normal;
real_t best_len;
Vector2 valid_dir;
real_t valid_depth;
real_t min_allowed_depth;
};

Expand All @@ -380,19 +381,25 @@ 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)
if (len < rd->min_allowed_depth) {
return;
}

Vector2 normal = contact_rel / len;

if (rd->valid_dir != Vector2() && rd->valid_dir.dot(normal) > -CMP_EPSILON)
if (len <= rd->best_len) {
return;
}

if (len < rd->min_allowed_depth)
return;
Vector2 normal = contact_rel / len;

if (len <= rd->best_len)
return;
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;
Expand Down Expand Up @@ -735,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;

float separation_margin = MIN(p_margin, MAX(0.0, p_motion.length() - CMP_EPSILON)); //don't separate by more than the intended motion
float 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

Expand Down Expand Up @@ -817,7 +827,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, NULL, separation_margin)) {
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, NULL, p_margin)) {
did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
}

Expand All @@ -843,19 +853,29 @@ 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();
float d = n.dot(b);

// Compute depth on recovered motion.
float 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()) {
collided = false;
break;
}

recovered = true;

body_transform.elements[2] += recover_motion;
body_aabb.position += recover_motion;

Expand Down Expand Up @@ -932,7 +952,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), NULL, NULL, NULL, 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;
Expand All @@ -942,13 +965,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(), NULL, NULL, &sep, 0);

if (collided) {
Expand All @@ -972,7 +994,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(), Physics2DServerSW::_shape_col_cbk, &cbk, &sep, 0);
if (!collided || cbk.amount == 0) {
continue;
Expand Down Expand Up @@ -1005,11 +1027,11 @@ 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;
Expand All @@ -1019,9 +1041,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
rcd.best_len = 0;
rcd.best_object = NULL;
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();

Expand Down Expand Up @@ -1070,8 +1093,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();

float 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() == Physics2DServer::BODY_MODE_KINEMATIC || b->get_mode() == Physics2DServer::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 * Physics2DDirectBodyStateSW::singleton->step;
float 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;
Expand Down