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

Jump to bottom

Fix test_body_motion recovery and rest info #46148

Merged
merged 1 commit into from
Feb 18, 2021
Merged

Fix test_body_motion recovery and rest info #46148

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
84 changes: 61 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 @@ -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;
};

Expand All @@ -385,22 +386,24 @@ 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;
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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
}

Expand All @@ -845,18 +851,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();
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()) {
collided = false;
break;
}

recovered = true;

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

Expand Down Expand Up @@ -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;
Expand All @@ -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) {
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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();

Expand Down Expand Up @@ -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;
Expand Down
95 changes: 61 additions & 34 deletions servers/physics_3d/space_3d_sw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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) {
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -791,14 +799,26 @@ 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()) {
collided = false;
break;
}

recovered = true;

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

Expand Down Expand Up @@ -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;
Expand All @@ -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);

Expand Down Expand Up @@ -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;
Expand All @@ -930,33 +944,48 @@ 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 j = from_shape; j < to_shape; j++) {
if (p_body->is_shape_set_as_disabled(j)) {
continue;
}

for (int i = 0; i < amount; i++) {
const CollisionObject3DSW *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);
Shape3DSW *body_shape = p_body->get_shape(j);

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) {
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) {
if (r_result) {
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 @@ -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;
}

Expand Down