diff options
Diffstat (limited to 'servers/physics_3d/collision_solver_3d_sat.cpp')
| -rw-r--r-- | servers/physics_3d/collision_solver_3d_sat.cpp | 93 |
1 files changed, 0 insertions, 93 deletions
diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index 5096b080ab..b4902af8ec 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -34,7 +34,6 @@ #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.02 struct _CollectorCallback { - CollisionSolver3DSW::CallbackResult callback; void *userdata; bool swap; @@ -43,7 +42,6 @@ struct _CollectorCallback { Vector3 *prev_axis; _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { - if (swap) callback(p_point_B, p_point_A, userdata); else @@ -54,7 +52,6 @@ struct _CollectorCallback { typedef void (*GenerateContactsFunc)(const Vector3 *, int, const Vector3 *, int, _CollectorCallback *); static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 1); ERR_FAIL_COND(p_point_count_B != 1); @@ -64,7 +61,6 @@ static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_poin } static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 1); ERR_FAIL_COND(p_point_count_B != 2); @@ -75,7 +71,6 @@ static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point } static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 1); ERR_FAIL_COND(p_point_count_B < 3); @@ -87,7 +82,6 @@ static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point } static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 2); ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix @@ -99,7 +93,6 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_ Vector3 c = rel_A.cross(rel_B).cross(rel_B); if (Math::is_zero_approx(rel_A.dot(c))) { - // should handle somehow.. //ERR_PRINT("TODO FIX"); //return; @@ -134,7 +127,6 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_ } static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A < 2); ERR_FAIL_COND(p_point_count_B < 3); @@ -150,7 +142,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ // copy A points to clipbuf_src for (int i = 0; i < p_point_count_A; i++) { - clipbuf_src[i] = p_points_A[i]; } @@ -158,7 +149,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ // go through all of B points for (int i = 0; i < p_point_count_B; i++) { - int i_n = (i + 1) % p_point_count_B; Vector3 edge0_B = p_points_B[i]; @@ -172,7 +162,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ int dst_idx = 0; bool edge = clipbuf_len == 2; for (int j = 0; j < clipbuf_len; j++) { - int j_n = (j + 1) % clipbuf_len; Vector3 edge0_A = clipbuf_src[j]; @@ -190,7 +179,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ // check for different sides and non coplanar //if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) { if ((dist0 * dist1) < 0 && !(edge && j)) { - // calculate intersection Vector3 rel = edge1_A - edge0_A; real_t den = clip.normal.dot(rel); @@ -211,7 +199,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ //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) @@ -228,7 +215,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ } static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A < 1); ERR_FAIL_COND(p_point_count_B < 1); @@ -267,7 +253,6 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po points_A = p_points_B; points_B = p_points_A; } else { - pointcount_B = p_point_count_B; pointcount_A = p_point_count_A; points_A = p_points_A; @@ -284,7 +269,6 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po template <class ShapeA, class ShapeB, bool withMargin = false> class SeparatorAxisTest { - const ShapeA *shape_A; const ShapeB *shape_B; const Transform *transform_A; @@ -298,7 +282,6 @@ class SeparatorAxisTest { public: _FORCE_INLINE_ bool test_previous_axis() { - if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) return test_axis(*callback->prev_axis); else @@ -306,7 +289,6 @@ public: } _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) { - Vector3 axis = p_axis; if (Math::abs(axis.x) < CMP_EPSILON && @@ -361,7 +343,6 @@ public: } _FORCE_INLINE_ void generate_contacts() { - // nothing to do, don't generate if (best_axis == Vector3(0.0, 0.0, 0.0)) return; @@ -384,7 +365,6 @@ public: } if (withMargin) { - for (int i = 0; i < support_count_A; i++) { supports_A[i] += -best_axis * margin_A; } @@ -398,7 +378,6 @@ public: } if (withMargin) { - for (int i = 0; i < support_count_B; i++) { supports_B[i] += best_axis * margin_B; } @@ -430,7 +409,6 @@ typedef void (*CollisionFunc)(const Shape3DSW *, const Transform &, const Shape3 template <bool withMargin> static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const SphereShape3DSW *sphere_B = static_cast<const SphereShape3DSW *>(p_b); @@ -449,7 +427,6 @@ static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform &p_tr template <bool withMargin> static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); @@ -461,7 +438,6 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans // test faces for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); if (!separator.test_axis(axis)) @@ -487,7 +463,6 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans // test edges for (int i = 0; i < 3; i++) { - Vector3 axis = point_axis.cross(p_transform_b.basis.get_axis(i)).cross(p_transform_b.basis.get_axis(i)).normalized(); if (!separator.test_axis(axis)) @@ -499,7 +474,6 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans template <bool withMargin> static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); @@ -542,7 +516,6 @@ static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform &p_ template <bool withMargin> static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -562,7 +535,6 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; if (!separator.test_axis(axis)) @@ -571,7 +543,6 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo // edges of B for (int i = 0; i < edge_count; i++) { - Vector3 v1 = p_transform_b.xform(vertices[edges[i].a]); Vector3 v2 = p_transform_b.xform(vertices[edges[i].b]); Vector3 v3 = p_transform_a.origin; @@ -587,7 +558,6 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo // vertices of B for (int i = 0; i < vertex_count; i++) { - Vector3 v1 = p_transform_b.xform(vertices[i]); Vector3 v2 = p_transform_a.origin; @@ -602,7 +572,6 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo template <bool withMargin> static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -619,7 +588,6 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_tran // edges and points of B for (int i = 0; i < 3; i++) { - Vector3 n1 = vertex[i] - p_transform_a.origin; if (!separator.test_axis(n1.normalized())) { @@ -640,7 +608,6 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_tran template <bool withMargin> static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); @@ -652,7 +619,6 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor // test faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); if (!separator.test_axis(axis)) @@ -662,7 +628,6 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor // test faces of B for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); if (!separator.test_axis(axis)) @@ -671,9 +636,7 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor // test combined edges for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - Vector3 axis = p_transform_a.basis.get_axis(i).cross(p_transform_b.basis.get_axis(j)); if (Math::is_zero_approx(axis.length_squared())) @@ -718,7 +681,6 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor //now try edges, which become cylinders! for (int i = 0; i < 3; i++) { - //a ->b Vector3 axis_a = p_transform_a.basis.get_axis(i); @@ -738,7 +700,6 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor template <bool withMargin> static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); @@ -749,7 +710,6 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i); if (!separator.test_axis(axis)) @@ -761,7 +721,6 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // edges of A, capsule cylinder for (int i = 0; i < 3; i++) { - // cylinder Vector3 box_axis = p_transform_a.basis.get_axis(i); Vector3 axis = box_axis.cross(cyl_axis); @@ -798,7 +757,6 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // capsule balls, edges of A for (int i = 0; i < 2; i++) { - Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -820,7 +778,6 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // test edges of A for (int j = 0; j < 3; j++) { - Vector3 axis = point_axis.cross(p_transform_a.basis.get_axis(j)).cross(p_transform_a.basis.get_axis(j)).normalized(); if (!separator.test_axis(axis)) @@ -837,7 +794,6 @@ static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform &p_tra template <bool withMargin> static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -857,7 +813,6 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); if (!separator.test_axis(axis)) @@ -866,7 +821,6 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; if (!separator.test_axis(axis)) @@ -875,11 +829,9 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform // A<->B edges for (int i = 0; i < 3; i++) { - Vector3 e1 = p_transform_a.basis.get_axis(i); for (int j = 0; j < edge_count; j++) { - Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); Vector3 axis = e1.cross(e2).normalized(); @@ -890,10 +842,8 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform } if (withMargin) { - // calculate closest points between vertices and box edges for (int v = 0; v < vertex_count; v++) { - Vector3 vtxb = p_transform_b.xform(vertices[v]); Vector3 ab_vec = vtxb - p_transform_a.origin; @@ -914,7 +864,6 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform //now try edges, which become cylinders! for (int i = 0; i < 3; i++) { - //a ->b Vector3 axis_a = p_transform_a.basis.get_axis(i); @@ -936,7 +885,6 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform point += p_transform_a.basis.get_axis(l) * he[l]; for (int e = 0; e < edge_count; e++) { - Vector3 p1 = p_transform_b.xform(vertices[edges[e].a]); Vector3 p2 = p_transform_b.xform(vertices[edges[e].b]); Vector3 n = (p2 - p1); @@ -954,7 +902,6 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform template <bool withMargin> static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -971,7 +918,6 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); if (!separator.test_axis(axis)) @@ -981,11 +927,9 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo // combined edges for (int i = 0; i < 3; i++) { - Vector3 e = vertex[i] - vertex[(i + 1) % 3]; for (int j = 0; j < 3; j++) { - Vector3 axis = p_transform_a.basis.get_axis(j); if (!separator.test_axis(e.cross(axis).normalized())) @@ -994,10 +938,8 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo } if (withMargin) { - // calculate closest points between vertices and box edges for (int v = 0; v < 3; v++) { - Vector3 ab_vec = vertex[v] - p_transform_a.origin; Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); @@ -1017,7 +959,6 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo //now try edges, which become cylinders! for (int i = 0; i < 3; i++) { - //a ->b Vector3 axis_a = p_transform_a.basis.get_axis(i); @@ -1039,7 +980,6 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo point += p_transform_a.basis.get_axis(l) * he[l]; for (int e = 0; e < 3; e++) { - Vector3 p1 = vertex[e]; Vector3 p2 = vertex[(e + 1) % 3]; @@ -1058,7 +998,6 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo template <bool withMargin> static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); @@ -1117,7 +1056,6 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p template <bool withMargin> static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -1136,7 +1074,6 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = p_transform_b.xform(faces[i].plane).normal; if (!separator.test_axis(axis)) @@ -1146,7 +1083,6 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf // edges of B, capsule cylinder for (int i = 0; i < edge_count; i++) { - // cylinder Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]); Vector3 axis = edge_axis.cross(p_transform_a.basis.get_axis(2)).normalized(); @@ -1158,7 +1094,6 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf // capsule balls, edges of B for (int i = 0; i < 2; i++) { - // edges of B, capsule cylinder Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); @@ -1166,7 +1101,6 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); for (int j = 0; j < edge_count; j++) { - Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].a]); Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); @@ -1182,7 +1116,6 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf template <bool withMargin> static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -1202,7 +1135,6 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); for (int i = 0; i < 3; i++) { - // edge-cylinder Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; Vector3 axis = edge_axis.cross(capsule_axis).normalized(); @@ -1214,7 +1146,6 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra return; for (int j = 0; j < 2; j++) { - // point-spheres Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis); @@ -1249,7 +1180,6 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr template <bool withMargin> static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); @@ -1278,7 +1208,6 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const // faces of A for (int i = 0; i < face_count_A; i++) { - Vector3 axis = p_transform_a.xform(faces_A[i].plane).normal; //Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized(); @@ -1288,7 +1217,6 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const // faces of B for (int i = 0; i < face_count_B; i++) { - Vector3 axis = p_transform_b.xform(faces_B[i].plane).normal; //Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized(); @@ -1298,11 +1226,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const // A<->B edges for (int i = 0; i < edge_count_A; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]) - p_transform_a.basis.xform(vertices_A[edges_A[i].b]); for (int j = 0; j < edge_count_B; j++) { - Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[j].a]) - p_transform_b.basis.xform(vertices_B[edges_B[j].b]); Vector3 axis = e1.cross(e2).normalized(); @@ -1313,14 +1239,11 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const } if (withMargin) { - //vertex-vertex for (int i = 0; i < vertex_count_A; i++) { - Vector3 va = p_transform_a.xform(vertices_A[i]); for (int j = 0; j < vertex_count_B; j++) { - if (!separator.test_axis((va - p_transform_b.xform(vertices_B[j])).normalized())) return; } @@ -1328,13 +1251,11 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const //edge-vertex (shell) for (int i = 0; i < edge_count_A; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]); Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].b]); Vector3 n = (e2 - e1); for (int j = 0; j < vertex_count_B; j++) { - Vector3 e3 = p_transform_b.xform(vertices_B[j]); if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) @@ -1343,13 +1264,11 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const } for (int i = 0; i < edge_count_B; i++) { - Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].a]); Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].b]); Vector3 n = (e2 - e1); for (int j = 0; j < vertex_count_A; j++) { - Vector3 e3 = p_transform_a.xform(vertices_A[j]); if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) @@ -1363,7 +1282,6 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const template <bool withMargin> static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -1389,7 +1307,6 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform // faces of A for (int i = 0; i < face_count; i++) { - //Vector3 axis = p_transform_a.xform( faces[i].plane ).normal; Vector3 axis = p_transform_a.basis.xform(faces[i].plane.normal).normalized(); @@ -1399,11 +1316,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform // A<->B edges for (int i = 0; i < edge_count; i++) { - Vector3 e1 = p_transform_a.xform(vertices[edges[i].a]) - p_transform_a.xform(vertices[edges[i].b]); for (int j = 0; j < 3; j++) { - Vector3 e2 = vertex[j] - vertex[(j + 1) % 3]; Vector3 axis = e1.cross(e2).normalized(); @@ -1414,14 +1329,11 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform } if (withMargin) { - //vertex-vertex for (int i = 0; i < vertex_count; i++) { - Vector3 va = p_transform_a.xform(vertices[i]); for (int j = 0; j < 3; j++) { - if (!separator.test_axis((va - vertex[j]).normalized())) return; } @@ -1429,13 +1341,11 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform //edge-vertex (shell) for (int i = 0; i < edge_count; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].a]); Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].b]); Vector3 n = (e2 - e1); for (int j = 0; j < 3; j++) { - Vector3 e3 = vertex[j]; if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) @@ -1444,13 +1354,11 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform } for (int i = 0; i < 3; i++) { - Vector3 e1 = vertex[i]; Vector3 e2 = vertex[(i + 1) % 3]; Vector3 n = (e2 - e1); for (int j = 0; j < vertex_count; j++) { - Vector3 e3 = p_transform_a.xform(vertices[j]); if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) @@ -1463,7 +1371,6 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform } bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { - PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_PLANE, false); |