summaryrefslogtreecommitdiff
path: root/servers/physics_3d/collision_solver_3d_sat.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d/collision_solver_3d_sat.cpp')
-rw-r--r--servers/physics_3d/collision_solver_3d_sat.cpp249
1 files changed, 165 insertions, 84 deletions
diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp
index b4902af8ec..736222c7d9 100644
--- a/servers/physics_3d/collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/collision_solver_3d_sat.cpp
@@ -42,10 +42,11 @@ struct _CollectorCallback {
Vector3 *prev_axis;
_FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) {
- if (swap)
+ if (swap) {
callback(p_point_B, p_point_A, userdata);
- else
+ } else {
callback(p_point_A, p_point_B, userdata);
+ }
}
};
@@ -116,10 +117,11 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_
real_t d = (c.dot(p_points_B[0]) - p_points_A[0].dot(c)) / rel_A.dot(c);
- if (d < 0.0)
+ if (d < 0.0) {
d = 0.0;
- else if (d > 1.0)
+ } else if (d > 1.0) {
d = 1.0;
+ }
Vector3 closest_A = p_points_A[0] + rel_A * d;
Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(closest_A, p_points_B);
@@ -207,8 +209,9 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_
Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d;
- if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B))
+ if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B)) {
continue;
+ }
p_callback->call(clipbuf_src[i], closest_B);
}
@@ -282,10 +285,11 @@ class SeparatorAxisTest {
public:
_FORCE_INLINE_ bool test_previous_axis() {
- if (callback && callback->prev_axis && *callback->prev_axis != Vector3())
+ if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) {
return test_axis(*callback->prev_axis);
- else
+ } else {
return true;
+ }
}
_FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) {
@@ -344,14 +348,16 @@ public:
_FORCE_INLINE_ void generate_contacts() {
// nothing to do, don't generate
- if (best_axis == Vector3(0.0, 0.0, 0.0))
+ if (best_axis == Vector3(0.0, 0.0, 0.0)) {
return;
+ }
if (!callback->callback) {
//just was checking intersection?
callback->collided = true;
- if (callback->prev_axis)
+ if (callback->prev_axis) {
*callback->prev_axis = best_axis;
+ }
return;
}
@@ -384,8 +390,9 @@ public:
}
callback->normal = best_axis;
- if (callback->prev_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;
@@ -416,11 +423,13 @@ static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform &p_tr
// previous axis
- if (!separator.test_previous_axis())
+ if (!separator.test_previous_axis()) {
return;
+ }
- if (!separator.test_axis((p_transform_a.origin - p_transform_b.origin).normalized()))
+ if (!separator.test_axis((p_transform_a.origin - p_transform_b.origin).normalized())) {
return;
+ }
separator.generate_contacts();
}
@@ -432,16 +441,18 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans
SeparatorAxisTest<SphereShape3DSW, BoxShape3DSW, withMargin> separator(sphere_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
- if (!separator.test_previous_axis())
+ if (!separator.test_previous_axis()) {
return;
+ }
// test faces
for (int i = 0; i < 3; i++) {
Vector3 axis = p_transform_b.basis.get_axis(i).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// calculate closest point to sphere
@@ -457,16 +468,18 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans
// use point to test axis
Vector3 point_axis = (p_transform_a.origin - cpoint).normalized();
- if (!separator.test_axis(point_axis))
+ if (!separator.test_axis(point_axis)) {
return;
+ }
// 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))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
separator.generate_contacts();
@@ -479,8 +492,9 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t
SeparatorAxisTest<SphereShape3DSW, CapsuleShape3DSW, withMargin> separator(sphere_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
- if (!separator.test_previous_axis())
+ if (!separator.test_previous_axis()) {
return;
+ }
//capsule sphere 1, sphere
@@ -488,15 +502,17 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t
Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis;
- if (!separator.test_axis((capsule_ball_1 - p_transform_a.origin).normalized()))
+ if (!separator.test_axis((capsule_ball_1 - p_transform_a.origin).normalized())) {
return;
+ }
//capsule sphere 2, sphere
Vector3 capsule_ball_2 = p_transform_b.origin - capsule_axis;
- if (!separator.test_axis((capsule_ball_2 - p_transform_a.origin).normalized()))
+ if (!separator.test_axis((capsule_ball_2 - p_transform_a.origin).normalized())) {
return;
+ }
//capsule edge, sphere
@@ -504,8 +520,9 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t
Vector3 axis = b2a.cross(capsule_axis).cross(capsule_axis).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
separator.generate_contacts();
}
@@ -521,8 +538,9 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo
SeparatorAxisTest<SphereShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
- if (!separator.test_previous_axis())
+ if (!separator.test_previous_axis()) {
return;
+ }
const Geometry::MeshData &mesh = convex_polygon_B->get_mesh();
@@ -537,8 +555,9 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo
for (int i = 0; i < face_count; i++) {
Vector3 axis = p_transform_b.xform(faces[i].plane).normal;
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// edges of B
@@ -552,8 +571,9 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo
Vector3 axis = n1.cross(n2).cross(n1).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// vertices of B
@@ -563,8 +583,9 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo
Vector3 axis = (v2 - v1).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
separator.generate_contacts();
@@ -583,8 +604,9 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_tran
p_transform_b.xform(face_B->vertex[2]),
};
- if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized()))
+ if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) {
return;
+ }
// edges and points of B
for (int i = 0; i < 3; i++) {
@@ -613,16 +635,18 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor
SeparatorAxisTest<BoxShape3DSW, BoxShape3DSW, withMargin> separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
- if (!separator.test_previous_axis())
+ if (!separator.test_previous_axis()) {
return;
+ }
// 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))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// test faces of B
@@ -630,8 +654,9 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor
for (int i = 0; i < 3; i++) {
Vector3 axis = p_transform_b.basis.get_axis(i).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// test combined edges
@@ -639,8 +664,9 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor
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()))
+ if (Math::is_zero_approx(axis.length_squared())) {
continue;
+ }
axis.normalize();
if (!separator.test_axis(axis)) {
@@ -684,14 +710,16 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor
//a ->b
Vector3 axis_a = p_transform_a.basis.get_axis(i);
- if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized()))
+ if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) {
return;
+ }
//b ->a
Vector3 axis_b = p_transform_b.basis.get_axis(i);
- if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized()))
+ if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) {
return;
+ }
}
}
@@ -705,15 +733,17 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran
SeparatorAxisTest<BoxShape3DSW, CapsuleShape3DSW, withMargin> separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
- if (!separator.test_previous_axis())
+ if (!separator.test_previous_axis()) {
return;
+ }
// faces of A
for (int i = 0; i < 3; i++) {
Vector3 axis = p_transform_a.basis.get_axis(i);
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
Vector3 cyl_axis = p_transform_b.basis.get_axis(2).normalized();
@@ -724,11 +754,13 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran
// cylinder
Vector3 box_axis = p_transform_a.basis.get_axis(i);
Vector3 axis = box_axis.cross(cyl_axis);
- if (Math::is_zero_approx(axis.length_squared()))
+ if (Math::is_zero_approx(axis.length_squared())) {
continue;
+ }
- if (!separator.test_axis(axis.normalized()))
+ if (!separator.test_axis(axis.normalized())) {
return;
+ }
}
// points of A, capsule cylinder
@@ -742,14 +774,16 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran
he.y *= (j * 2 - 1);
he.z *= (k * 2 - 1);
Vector3 point = p_transform_a.origin;
- for (int l = 0; l < 3; l++)
+ for (int l = 0; l < 3; l++) {
point += p_transform_a.basis.get_axis(l) * he[l];
+ }
//Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized();
Vector3 axis = Plane(cyl_axis, 0).project(point).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
}
}
@@ -772,16 +806,18 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran
// use point to test axis
Vector3 point_axis = (sphere_pos - cpoint).normalized();
- if (!separator.test_axis(point_axis))
+ if (!separator.test_axis(point_axis)) {
return;
+ }
// 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))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
}
@@ -799,8 +835,9 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform
SeparatorAxisTest<BoxShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
- if (!separator.test_previous_axis())
+ if (!separator.test_previous_axis()) {
return;
+ }
const Geometry::MeshData &mesh = convex_polygon_B->get_mesh();
@@ -815,16 +852,18 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform
for (int i = 0; i < 3; i++) {
Vector3 axis = p_transform_a.basis.get_axis(i).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// 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))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// A<->B edges
@@ -836,8 +875,9 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform
Vector3 axis = e1.cross(e2).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
}
@@ -867,8 +907,9 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform
//a ->b
Vector3 axis_a = p_transform_a.basis.get_axis(i);
- if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized()))
+ if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) {
return;
+ }
}
}
@@ -881,16 +922,18 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform
he.y *= (j * 2 - 1);
he.z *= (k * 2 - 1);
Vector3 point = p_transform_a.origin;
- for (int l = 0; l < 3; l++)
+ for (int l = 0; l < 3; l++) {
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);
- if (!separator.test_axis((point - p2).cross(n).cross(n).normalized()))
+ if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) {
return;
+ }
}
}
}
@@ -913,15 +956,17 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo
p_transform_b.xform(face_B->vertex[2]),
};
- if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized()))
+ if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) {
return;
+ }
// 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))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// combined edges
@@ -932,8 +977,9 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo
for (int j = 0; j < 3; j++) {
Vector3 axis = p_transform_a.basis.get_axis(j);
- if (!separator.test_axis(e.cross(axis).normalized()))
+ if (!separator.test_axis(e.cross(axis).normalized())) {
return;
+ }
}
}
@@ -962,8 +1008,9 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo
//a ->b
Vector3 axis_a = p_transform_a.basis.get_axis(i);
- if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized()))
+ if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) {
return;
+ }
}
}
@@ -976,8 +1023,9 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo
he.y *= (j * 2 - 1);
he.z *= (k * 2 - 1);
Vector3 point = p_transform_a.origin;
- for (int l = 0; l < 3; l++)
+ for (int l = 0; l < 3; l++) {
point += p_transform_a.basis.get_axis(l) * he[l];
+ }
for (int e = 0; e < 3; e++) {
Vector3 p1 = vertex[e];
@@ -985,8 +1033,9 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo
Vector3 n = (p2 - p1);
- if (!separator.test_axis((point - p2).cross(n).cross(n).normalized()))
+ if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) {
return;
+ }
}
}
}
@@ -1003,8 +1052,9 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_
SeparatorAxisTest<CapsuleShape3DSW, CapsuleShape3DSW, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
- if (!separator.test_previous_axis())
+ if (!separator.test_previous_axis()) {
return;
+ }
// some values
@@ -1018,34 +1068,43 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_
//balls-balls
- if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).normalized()))
+ if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).normalized())) {
return;
- if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).normalized()))
+ }
+ if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).normalized())) {
return;
+ }
- if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_1).normalized()))
+ if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_1).normalized())) {
return;
- if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_2).normalized()))
+ }
+ if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_2).normalized())) {
return;
+ }
// edges-balls
- if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).cross(capsule_A_axis).cross(capsule_A_axis).normalized()))
+ if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) {
return;
+ }
- if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).cross(capsule_A_axis).cross(capsule_A_axis).normalized()))
+ if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) {
return;
+ }
- if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_1).cross(capsule_B_axis).cross(capsule_B_axis).normalized()))
+ if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_1).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) {
return;
+ }
- if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_2).cross(capsule_B_axis).cross(capsule_B_axis).normalized()))
+ if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_2).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) {
return;
+ }
// edges
- if (!separator.test_axis(capsule_A_axis.cross(capsule_B_axis).normalized()))
+ if (!separator.test_axis(capsule_A_axis.cross(capsule_B_axis).normalized())) {
return;
+ }
separator.generate_contacts();
}
@@ -1061,8 +1120,9 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf
SeparatorAxisTest<CapsuleShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
- if (!separator.test_previous_axis())
+ if (!separator.test_previous_axis()) {
return;
+ }
const Geometry::MeshData &mesh = convex_polygon_B->get_mesh();
@@ -1076,8 +1136,9 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf
for (int i = 0; i < face_count; i++) {
Vector3 axis = p_transform_b.xform(faces[i].plane).normal;
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// edges of B, capsule cylinder
@@ -1087,8 +1148,9 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf
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();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// capsule balls, edges of B
@@ -1106,8 +1168,9 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf
Vector3 axis = n1.cross(n2).cross(n2).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
}
@@ -1127,8 +1190,9 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra
p_transform_b.xform(face_B->vertex[2]),
};
- if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized()))
+ if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) {
return;
+ }
// edges of B, capsule cylinder
@@ -1139,11 +1203,13 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra
Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3];
Vector3 axis = edge_axis.cross(capsule_axis).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
- if (!separator.test_axis((p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized()))
+ if (!separator.test_axis((p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized())) {
return;
+ }
for (int j = 0; j < 2; j++) {
// point-spheres
@@ -1151,15 +1217,17 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra
Vector3 n1 = sphere_pos - vertex[i];
- if (!separator.test_axis(n1.normalized()))
+ if (!separator.test_axis(n1.normalized())) {
return;
+ }
Vector3 n2 = edge_axis;
axis = n1.cross(n2).cross(n2);
- if (!separator.test_axis(axis.normalized()))
+ if (!separator.test_axis(axis.normalized())) {
return;
+ }
}
}
@@ -1185,8 +1253,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const
SeparatorAxisTest<ConvexPolygonShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
- if (!separator.test_previous_axis())
+ if (!separator.test_previous_axis()) {
return;
+ }
const Geometry::MeshData &mesh_A = convex_polygon_A->get_mesh();
@@ -1211,8 +1280,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const
Vector3 axis = p_transform_a.xform(faces_A[i].plane).normal;
//Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// faces of B
@@ -1220,8 +1290,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const
Vector3 axis = p_transform_b.xform(faces_B[i].plane).normal;
//Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// A<->B edges
@@ -1233,8 +1304,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const
Vector3 axis = e1.cross(e2).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
}
@@ -1244,8 +1316,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const
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()))
+ if (!separator.test_axis((va - p_transform_b.xform(vertices_B[j])).normalized())) {
return;
+ }
}
}
//edge-vertex (shell)
@@ -1258,8 +1331,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const
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()))
+ if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) {
return;
+ }
}
}
@@ -1271,8 +1345,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const
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()))
+ if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) {
return;
+ }
}
}
}
@@ -1302,16 +1377,18 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
p_transform_b.xform(face_B->vertex[2]),
};
- if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized()))
+ if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) {
return;
+ }
// 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();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
// A<->B edges
@@ -1323,8 +1400,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
Vector3 axis = e1.cross(e2).normalized();
- if (!separator.test_axis(axis))
+ if (!separator.test_axis(axis)) {
return;
+ }
}
}
@@ -1334,8 +1412,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
Vector3 va = p_transform_a.xform(vertices[i]);
for (int j = 0; j < 3; j++) {
- if (!separator.test_axis((va - vertex[j]).normalized()))
+ if (!separator.test_axis((va - vertex[j]).normalized())) {
return;
+ }
}
}
//edge-vertex (shell)
@@ -1348,8 +1427,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
for (int j = 0; j < 3; j++) {
Vector3 e3 = vertex[j];
- if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized()))
+ if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) {
return;
+ }
}
}
@@ -1361,8 +1441,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
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()))
+ if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) {
return;
+ }
}
}
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-25 00:13:18 +0000