summaryrefslogtreecommitdiff
path: root/servers/physics/collision_solver_sat.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics/collision_solver_sat.cpp')
-rw-r--r--servers/physics/collision_solver_sat.cpp91
1 files changed, 63 insertions, 28 deletions
diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp
index e587485fcb..294b1df241 100644
--- a/servers/physics/collision_solver_sat.cpp
+++ b/servers/physics/collision_solver_sat.cpp
@@ -44,12 +44,6 @@ struct _CollectorCallback {
_FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) {
- /*
- if (normal.dot(p_point_A) >= normal.dot(p_point_B))
- return;
- print_line("** A: "+p_point_A+" B: "+p_point_B+" D: "+rtos(p_point_A.distance_to(p_point_B)));
- */
-
if (swap)
callback(p_point_B, p_point_A, userdata);
else
@@ -217,8 +211,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_
// generate contacts
//Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]);
- int added = 0;
-
for (int i = 0; i < clipbuf_len; i++) {
real_t d = plane_B.distance_to(clipbuf_src[i]);
@@ -233,7 +225,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_
continue;
p_callback->call(clipbuf_src[i], closest_B);
- added++;
}
}
@@ -351,7 +342,9 @@ public:
//use the smallest depth
- min_B = -min_B;
+ if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0
+ min_B = -min_B;
+ }
if (max_B < min_B) {
if (max_B < best_depth) {
@@ -411,26 +404,13 @@ public:
supports_B[i] += best_axis * margin_B;
}
}
- /*
- print_line("best depth: "+rtos(best_depth));
- print_line("best axis: "+(best_axis));
- for(int i=0;i<support_count_A;i++) {
-
- print_line("A-"+itos(i)+": "+supports_A[i]);
- }
- for(int i=0;i<support_count_B;i++) {
- print_line("B-"+itos(i)+": "+supports_B[i]);
- }
-*/
callback->normal = best_axis;
if (callback->prev_axis)
*callback->prev_axis = best_axis;
_generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback);
callback->collided = true;
- //CollisionSolverSW::CallbackResult cbk=NULL;
- //cbk(Vector3(),Vector3(),NULL);
}
_FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform &p_transform_A, const ShapeB *p_shape_B, const Transform &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) {
@@ -446,9 +426,6 @@ public:
};
/****** SAT TESTS *******/
-/****** SAT TESTS *******/
-/****** SAT TESTS *******/
-/****** SAT TESTS *******/
typedef void (*CollisionFunc)(const ShapeSW *, const Transform &, const ShapeSW *, const Transform &, _CollectorCallback *p_callback, real_t, real_t);
@@ -561,6 +538,12 @@ static void _collision_sphere_capsule(const ShapeSW *p_a, const Transform &p_tra
}
template <bool withMargin>
+static void _collision_sphere_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+
+ return;
+}
+
+template <bool withMargin>
static void _collision_sphere_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a);
@@ -852,6 +835,12 @@ static void _collision_box_capsule(const ShapeSW *p_a, const Transform &p_transf
}
template <bool withMargin>
+static void _collision_box_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+
+ return;
+}
+
+template <bool withMargin>
static void _collision_box_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a);
@@ -1128,6 +1117,12 @@ static void _collision_capsule_capsule(const ShapeSW *p_a, const Transform &p_tr
}
template <bool withMargin>
+static void _collision_capsule_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+
+ return;
+}
+
+template <bool withMargin>
static void _collision_capsule_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a);
@@ -1248,6 +1243,24 @@ static void _collision_capsule_face(const ShapeSW *p_a, const Transform &p_trans
}
template <bool withMargin>
+static void _collision_cylinder_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+
+ return;
+}
+
+template <bool withMargin>
+static void _collision_cylinder_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+
+ return;
+}
+
+template <bool withMargin>
+static void _collision_cylinder_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+
+ return;
+}
+
+template <bool withMargin>
static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW *>(p_a);
@@ -1476,59 +1489,81 @@ bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_tran
ERR_FAIL_COND_V(type_B == PhysicsServer::SHAPE_RAY, false);
ERR_FAIL_COND_V(p_shape_B->is_concave(), false);
- static const CollisionFunc collision_table[5][5] = {
+ static const CollisionFunc collision_table[6][6] = {
{ _collision_sphere_sphere<false>,
_collision_sphere_box<false>,
_collision_sphere_capsule<false>,
+ _collision_sphere_cylinder<false>,
_collision_sphere_convex_polygon<false>,
_collision_sphere_face<false> },
{ 0,
_collision_box_box<false>,
_collision_box_capsule<false>,
+ _collision_box_cylinder<false>,
_collision_box_convex_polygon<false>,
_collision_box_face<false> },
{ 0,
0,
_collision_capsule_capsule<false>,
+ _collision_capsule_cylinder<false>,
_collision_capsule_convex_polygon<false>,
_collision_capsule_face<false> },
{ 0,
0,
0,
+ _collision_cylinder_cylinder<false>,
+ _collision_cylinder_convex_polygon<false>,
+ _collision_cylinder_face<false> },
+ { 0,
+ 0,
+ 0,
+ 0,
_collision_convex_polygon_convex_polygon<false>,
_collision_convex_polygon_face<false> },
{ 0,
0,
0,
0,
+ 0,
0 },
};
- static const CollisionFunc collision_table_margin[5][5] = {
+ static const CollisionFunc collision_table_margin[6][6] = {
{ _collision_sphere_sphere<true>,
_collision_sphere_box<true>,
_collision_sphere_capsule<true>,
+ _collision_sphere_cylinder<true>,
_collision_sphere_convex_polygon<true>,
_collision_sphere_face<true> },
{ 0,
_collision_box_box<true>,
_collision_box_capsule<true>,
+ _collision_box_cylinder<true>,
_collision_box_convex_polygon<true>,
_collision_box_face<true> },
{ 0,
0,
_collision_capsule_capsule<true>,
+ _collision_capsule_cylinder<true>,
_collision_capsule_convex_polygon<true>,
_collision_capsule_face<true> },
{ 0,
0,
0,
+ _collision_cylinder_cylinder<true>,
+ _collision_cylinder_convex_polygon<true>,
+ _collision_cylinder_face<true> },
+ { 0,
+ 0,
+ 0,
+ 0,
_collision_convex_polygon_convex_polygon<true>,
_collision_convex_polygon_face<true> },
{ 0,
0,
0,
0,
+ 0,
0 },
};
generated by cgit v1.2.3 (git 2.25.1) at 2025-08-08 06:20:01 +0000