[3.x] Fix test_body_motion() sometimes failing to extracting collision information

doc/classes/Physics2DServer.xml

@@ -1115,8 +1115,6 @@
  <constant name="SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS" value="6" enum="SpaceParameter">
  Constant to set/get the default solver bias for all physics constraints. A solver bias is a factor controlling how much two objects "rebound", after violating a constraint, to avoid leaving them in that state because of numerical imprecision.
  </constant>
- <constant name="SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH" value="7" enum="SpaceParameter">
- </constant>
  <constant name="SHAPE_LINE" value="0" enum="ShapeType">
  This is the constant for creating line shapes. A line shape is an infinite line with an origin point, and a normal. Thus, it can be used for front/behind checks.
  </constant>

doc/classes/PhysicsServer.xml

@@ -1621,8 +1621,6 @@
  <constant name="SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS" value="7" enum="SpaceParameter">
  Constant to set/get the default solver bias for all physics constraints. A solver bias is a factor controlling how much two objects "rebound", after violating a constraint, to avoid leaving them in that state because of numerical imprecision.
  </constant>
- <constant name="SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH" value="8" enum="SpaceParameter">
- </constant>
  <constant name="BODY_AXIS_LINEAR_X" value="1" enum="BodyAxis">
  </constant>
  <constant name="BODY_AXIS_LINEAR_Y" value="2" enum="BodyAxis">

servers/physics/collision_solver_sat.cpp

@@ -207,22 +207,12 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_
  SWAP(clipbuf_src, clipbuf_dst);
  }
 
- // generate contacts
- //Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]);
-
  for (int i = 0; i < clipbuf_len; i++) {
 
  real_t d = plane_B.distance_to(clipbuf_src[i]);
- /*
- if (d>CMP_EPSILON)
- continue;
- */
 
  Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d;
 
- if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B))
- continue;
-
  p_callback->call(clipbuf_src[i], closest_B);
  }
 }
@@ -334,7 +324,7 @@ class SeparatorAxisTest {
  min_B -= (min_A + max_A) * 0.5;
  max_B -= (min_A + max_A) * 0.5;
 
- if (min_B > 0.0 || max_B < 0.0) {
+ if (min_B >= 0.0 || max_B <= 0.0) {
  separator_axis = axis;
  return false; // doesn't contain 0
  }

servers/physics/collision_solver_sw.cpp

@@ -33,9 +33,6 @@
 
 #include "gjk_epa.h"
 
-#define collision_solver sat_calculate_penetration
-//#define collision_solver gjk_epa_calculate_penetration
-
 bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
 
  const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A);
@@ -118,14 +115,15 @@ struct _ConcaveCollisionInfo {
  real_t margin_A;
  real_t margin_B;
  Vector3 close_A, close_B;
+ real_t distance;
 };
 
 void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) {
 
  _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
  cinfo.aabb_tests++;
 
- bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, NULL, cinfo.margin_A, cinfo.margin_B);
+ bool collided = sat_calculate_penetration(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, NULL, cinfo.margin_A, cinfo.margin_B);
  if (!collided)
  return;
 
@@ -231,33 +229,33 @@ bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform &
 
  } else {
 
- return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A, p_margin_B);
+ return sat_calculate_penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A, p_margin_B);
  }
 }
 
 void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_convex) {
 
  _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
  cinfo.aabb_tests++;
- if (cinfo.collided)
- return;
 
- Vector3 close_A, close_B;
- cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B);
+ GjkEpaResult result;
+ if (!gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, result)) {
+ ERR_FAIL_COND_MSG(result.status != GjkEpaResult::Penetrating, "GJK EPA Algorithm failed.");
+ cinfo.collisions++;
+ cinfo.collided = true;
+ gjk_epa_calculate_penetration(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, result);
+ }
 
- if (cinfo.collided)
- return;
- if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) {
+ if (!cinfo.tested || result.distance < cinfo.distance) {
 
- cinfo.close_A = close_A;
- cinfo.close_B = close_B;
+ cinfo.close_A = result.witnesses[0];
+ cinfo.close_B = result.witnesses[1];
+ cinfo.distance = result.distance;
  cinfo.tested = true;
  }
-
- cinfo.collisions++;
 }
 
-bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {
+real_t CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {
 
  const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A);
  if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE)
@@ -270,9 +268,8 @@ bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Tra
 
  p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count);
 
- bool collided = false;
  Vector3 closest;
- real_t closest_d = 0;
+ real_t closest_d = 10e20;
 
  for (int i = 0; i < support_count; i++) {
 
@@ -281,34 +278,25 @@ bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Tra
  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 CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *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) {
+real_t CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint) {
 
- if (p_shape_A->is_concave())
- return false;
+ ERR_FAIL_COND_V_MSG(p_shape_A->is_concave(), 0, "Cannot test concave shape.");
 
  if (p_shape_B->get_type() == PhysicsServer::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()) {
+ 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_A->is_concave())
- return false;
+ if (p_shape_B->is_concave()) {
 
  const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B);
 
@@ -359,14 +347,23 @@ bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform
  }
 
  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 {
+ 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 gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, r_point_A, r_point_B); //should pass sepaxis..
+ 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;
 }

servers/physics/collision_solver_sw.h

@@ -43,11 +43,11 @@ class CollisionSolverSW {
  static bool solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
  static bool solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *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, ShapeSW *p_convex);
- static bool solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
+ static real_t solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
 
 public:
  static bool solve_static(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0);
- static bool solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *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 = NULL);
+ static real_t solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *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

servers/physics/gjk_epa.cpp

@@ -82,20 +82,6 @@ GJK-EPA collision solver by Nathanael Presson, 2008
 
 namespace GjkEpa2 {
 
-
-struct sResults {
- enum eStatus {
- Separated, /* Shapes doesn't penetrate */
- Penetrating, /* Shapes are penetrating */
- GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
- EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */
- } status;
-
- Vector3 witnesses[2];
- Vector3 normal;
- real_t distance;
-};
-
 // Shorthands
 typedef unsigned int U;
 typedef unsigned char U1;
@@ -785,14 +771,14 @@ struct GJK
  //
  static void Initialize( const ShapeSW* shape0,const Transform& wtrs0,
  const ShapeSW* shape1,const Transform& wtrs1,
- sResults& results,
+ GjkEpaResult& results,
  tShape& shape,
  bool withmargins)
  {
  /* Results */
  results.witnesses[0] =
  results.witnesses[1] = Vector3(0,0,0);
- results.status = sResults::Separated;
+ results.status = GjkEpaResult::Separated;
  /* Shape */
  shape.m_shapes[0] = shape0;
  shape.m_shapes[1] = shape1;
@@ -815,7 +801,7 @@ bool Distance( const ShapeSW* shape0,
  const ShapeSW* shape1,
  const Transform& wtrs1,
  const Vector3& guess,
- sResults& results)
+ GjkEpaResult& results)
 {
  tShape shape;
  Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
@@ -841,8 +827,8 @@ bool Distance( const ShapeSW* shape0,
  else
  {
  results.status = gjk_status==GJK::eStatus::Inside?
- sResults::Penetrating :
- sResults::GJK_Failed ;
+ GjkEpaResult::Penetrating :
+ GjkEpaResult::GJK_Failed ;
  return(false);
  }
 }
@@ -853,7 +839,7 @@ bool Penetration( const ShapeSW* shape0,
  const ShapeSW* shape1,
  const Transform& wtrs1,
  const Vector3& guess,
- sResults& results
+ GjkEpaResult& results
  )
 {
  tShape shape;
@@ -873,17 +859,17 @@ bool Penetration( const ShapeSW* shape0,
  {
  w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
  }
- results.status = sResults::Penetrating;
+ results.status = GjkEpaResult::Penetrating;
  results.witnesses[0] = w0;
  results.witnesses[1] = w0-epa.m_normal*epa.m_depth;
  results.normal = -epa.m_normal;
  results.distance = -epa.m_depth;
  return(true);
- } else results.status=sResults::EPA_Failed;
+ } else results.status=GjkEpaResult::EPA_Failed;
  }
  break;
  case GJK::eStatus::Failed:
- results.status=sResults::GJK_Failed;
+ results.status=GjkEpaResult::GJK_Failed;
  break;
  default: {}
  }
@@ -914,33 +900,12 @@ bool Penetration( const ShapeSW* shape0,
 
 /* clang-format on */
 
-bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) {
+bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, GjkEpaResult &r_result) {
 
- GjkEpa2::sResults res;
-
- if (GjkEpa2::Distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) {
-
- r_result_A = res.witnesses[0];
- r_result_B = res.witnesses[1];
- return true;
- }
-
- return false;
+ return GjkEpa2::Distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, r_result);
 }
 
-bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap) {
-
- GjkEpa2::sResults res;
-
- if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) {
- if (p_result_callback) {
- if (p_swap)
- p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata);
- else
- p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata);
- }
- return true;
- }
+bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, GjkEpaResult &r_result) {
 
- return false;
+ return GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, r_result);
 }

servers/physics/gjk_epa.h

@@ -34,7 +34,19 @@
 #include "collision_solver_sw.h"
 #include "shape_sw.h"
 
-bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false);
-bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B);
+typedef struct {
+ enum eStatus {
+ Separated, // Shapes doesn't penetrate
+ Penetrating, // Shapes are penetrating
+ GJK_Failed, // GJK phase fail, no big issue, shapes are probably just 'touching'
+ EPA_Failed // EPA phase fail, bigger problem, need to save parameters, and debug
+ } status;
+ Vector3 witnesses[2];
+ Vector3 normal;
+ real_t distance;
+} GjkEpaResult;
+
+bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, GjkEpaResult &r_result);
+bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, GjkEpaResult &r_result);
 
 #endif

servers/physics/physics_server_sw.h

@@ -73,9 +73,9 @@ class PhysicsServerSW : public PhysicsServer {
 
  struct CollCbkData {
 
- int max;
- int amount;
- Vector3 *ptr;
+ int max = 0;
+ int amount = 0;
+ Vector3 *ptr = nullptr;
  };
 
  static void _shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata);