Skip to content

Commit

Permalink
Make CollisionSolverSW::solve_distance return distance
Browse files Browse the repository at this point in the history 
Generate appropriate error messages if it fails.
  • Loading branch information
@madmiraal
madmiraal committed Feb 14, 2021
1 parent 24cd0a3 commit 3540339
Show file tree
Hide file tree
Showing 3 changed files with 39 additions and 60 deletions.
75 changes: 31 additions & 44 deletions servers/physics_3d/collision_solver_3d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,6 +132,7 @@ struct _ConcaveCollisionInfo {
real_t margin_A;
real_t margin_B;
Vector3 close_A, close_B;
real_t distance;
};

void CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex) {
Expand Down Expand Up @@ -249,27 +250,24 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *p_convex) {
_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
cinfo.aabb_tests++;
if (cinfo.collided) {
return;
}

GjkEpaResult result;
if (!gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, result)) {
cinfo.collided = true;
ERR_FAIL_COND_MSG(result.status != GjkEpaResult::Penetrating, "GJK EPA Algorithm failed.");
return;
cinfo.collisions++;
cinfo.collided = true;
gjk_epa_calculate_penetration(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, result);
}

if (!cinfo.tested || result.distance < cinfo.close_A.distance_squared_to(cinfo.close_B)) {
if (!cinfo.tested || result.distance < cinfo.distance) {
cinfo.close_A = result.witnesses[0];
cinfo.close_B = result.witnesses[1];
cinfo.distance = result.distance;
cinfo.tested = true;
}

cinfo.collisions++;
}

bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {
real_t CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {
const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A);
if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) {
return false;
Expand Down Expand Up @@ -299,45 +297,32 @@ bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const
}
}

bool collided = false;
Vector3 closest;
real_t closest_d = 0;
real_t closest_d = 10e20;

for (int i = 0; i < support_count; i++) {
supports[i] = p_transform_B.xform(supports[i]);
real_t d = p.distance_to(supports[i]);
if (i == 0 || d < closest_d) {
closest = supports[i];
closest_d = d;
if (d <= 0) {
collided = true;
}
}
}

r_point_A = p.project(closest);
r_point_B = closest;

return collided;
return closest_d;
}

bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) {
if (p_shape_A->is_concave()) {
return false;
}
real_t CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint) {
ERR_FAIL_COND_V_MSG(p_shape_A->is_concave(), 0, "Cannot test concave shape.");

if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) {
Vector3 a, b;
bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b);
r_point_A = b;
r_point_B = a;
return !col;

} else if (p_shape_B->is_concave()) {
if (p_shape_A->is_concave()) {
return false;
}
return solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, r_point_A, r_point_B);
}

if (p_shape_B->is_concave()) {
const ConcaveShape3DSW *concave_B = static_cast<const ConcaveShape3DSW *>(p_shape_B);

_ConcaveCollisionInfo cinfo;
Expand Down Expand Up @@ -386,21 +371,23 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans
}

concave_B->cull(local_aabb, concave_distance_callback, &cinfo);
if (!cinfo.collided) {
r_point_A = cinfo.close_A;
r_point_B = cinfo.close_B;
}

return !cinfo.collided;
} else {
GjkEpaResult result;
if (gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, result)) {
r_point_A = result.witnesses[0];
r_point_B = result.witnesses[1];
return true;
} else {
ERR_FAIL_COND_V_MSG(result.status != GjkEpaResult::Penetrating, false, "GJK EPA Algorithm failed.");
return false;
}
ERR_FAIL_COND_V_MSG(!cinfo.tested, 0, "Failed to extract ConcaveShape distance information");

r_point_A = cinfo.close_A;
r_point_B = cinfo.close_B;

return cinfo.distance;
}

GjkEpaResult result;
if (!gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, result)) {
ERR_FAIL_COND_V_MSG(result.status != GjkEpaResult::Penetrating, 0, "GJK EPA Algorithm failed.");
gjk_epa_calculate_penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, result);
}

r_point_A = result.witnesses[0];
r_point_B = result.witnesses[1];

return result.distance;
}
4 changes: 2 additions & 2 deletions servers/physics_3d/collision_solver_3d_sw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,11 +43,11 @@ class CollisionSolver3DSW {
static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_concave(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0);
static void concave_distance_callback(void *p_userdata, Shape3DSW *p_convex);
static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
static real_t solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);

public:
static bool solve_static(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
static bool solve_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr);
static real_t solve_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint = AABB());
};

#endif // COLLISION_SOLVER__SW_H
20 changes: 6 additions & 14 deletions servers/physics_3d/space_3d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -227,7 +227,6 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
AABB aabb = p_shape_xform.xform(shape->get_aabb());
aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size));
aabb = aabb.grow(p_margin);
Vector3 motion_normal = p_motion.normalized();

int pair_count = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);

Expand Down Expand Up @@ -255,14 +254,12 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor

// Does it collide if going all the way?
Vector3 point_A, point_B;
Vector3 sep_axis = motion_normal;
if (CollisionSolver3DSW::solve_distance(&mshape, p_shape_xform, col_shape, col_shape_xform, point_A, point_B, aabb, &sep_axis)) {
if (CollisionSolver3DSW::solve_distance(&mshape, p_shape_xform, col_shape, col_shape_xform, point_A, point_B, aabb) >= 0) {
continue;
}

// Ignore objects it's inside of.
sep_axis = motion_normal;
if (!CollisionSolver3DSW::solve_distance(shape, p_shape_xform, col_shape, col_shape_xform, point_A, point_B, aabb, &sep_axis)) {
if (CollisionSolver3DSW::solve_distance(shape, p_shape_xform, col_shape, col_shape_xform, point_A, point_B, aabb) < 0) {
continue;
}

Expand All @@ -274,9 +271,8 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
for (int step = 0; step < 8; step++) { // Steps should be customizable.
real_t ofs = (low + hi) * 0.5;

sep_axis = motion_normal; // Important optimization for this to work fast enough.
mshape.motion = xform_inv.basis.xform(p_motion * ofs);
if (CollisionSolver3DSW::solve_distance(&mshape, p_shape_xform, col_shape, col_shape_xform, point_A, point_B, aabb, &sep_axis)) {
if (CollisionSolver3DSW::solve_distance(&mshape, p_shape_xform, col_shape, col_shape_xform, point_A, point_B, aabb) >= 0) {
low = ofs;
closest_A = point_A;
closest_B = point_B;
Expand Down Expand Up @@ -754,7 +750,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_xform, con
AABB motion_aabb = body_aabb;
motion_aabb.position += p_motion;
motion_aabb = motion_aabb.merge(body_aabb);
Vector3 motion_normal = p_motion.normalized();
Vector3 point_A, point_B;

int pair_count = _cull_aabb_for_body(p_body, motion_aabb);
Expand Down Expand Up @@ -789,14 +784,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_xform, con
const Transform col_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx);

// Does it collide if going all the way?
Vector3 sep_axis = motion_normal;
if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_shape, col_shape_xform, point_A, point_B, motion_aabb, &sep_axis)) {
if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_shape, col_shape_xform, point_A, point_B, motion_aabb) >= 0) {
continue;
}

// Is it stuck?
sep_axis = motion_normal;
if (!CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_shape, col_shape_xform, point_A, point_B, motion_aabb, &sep_axis)) {
if (CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_shape, col_shape_xform, point_A, point_B, motion_aabb) < 0) {
stuck = true;
break;
}
Expand All @@ -808,9 +801,8 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_xform, con
for (int step = 0; step < 8; step++) { // Steps should be customizable.
real_t ofs = (low + hi) * 0.5;

sep_axis = motion_normal; // Important optimization for this to work fast enough.
mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs);
if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_shape, col_shape_xform, point_A, point_B, motion_aabb, &sep_axis)) {
if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_shape, col_shape_xform, point_A, point_B, motion_aabb) >= 0) {
low = ofs;
} else {
hi = ofs;
Expand Down

0 comments on commit 3540339

Please sign in to comment.