From 87f07397d2b0c3d0575e8610e12ec440e4fa3500 Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Wed, 25 Aug 2021 22:29:18 +0200 Subject: [PATCH] Working on edge-edge contact manifold generation. Possibly found issue for face-face contact generation. --- src/sconvcol.h | 54 +++++++++++++++++++++++++++++++++++++----- tests/sconvcolTests.cc | 12 +++++++++- 2 files changed, 59 insertions(+), 7 deletions(-) diff --git a/src/sconvcol.h b/src/sconvcol.h index 5d6a834..439308c 100644 --- a/src/sconvcol.h +++ b/src/sconvcol.h @@ -28,6 +28,7 @@ typedef struct sch_hull sch_hull; typedef struct sch_plane sch_plane; typedef struct sch_hull_builder sch_hull_builder; typedef struct sch_face_query sch_face_query; +typedef struct sch_edge_query sch_edge_query; typedef struct sch_manifold sch_manifold; struct sch_edge { @@ -68,6 +69,13 @@ struct sch_face_query { simd4f vert; }; +struct sch_edge_query { + float dist; + sch_plane plane; + int edge_A_idx; + int edge_B_idx; +}; + struct sch_manifold { int num_points; int num_vertices; @@ -152,6 +160,8 @@ void sch_clip_faces (const sch_face* ref_face, const sch_face* inc_face, sch_man void sch_create_face_contact (const sch_face_query* query_A_B, const sch_hull* hull_A, const sch_face_query* query_B_A, const sch_hull* hull_B, sch_manifold *result); +void sch_create_edge_contact (const sch_edge_query* query_edge, const sch_hull* hull_A, const sch_hull* hull_B, sch_manifold* result); + bool sch_hull_sat(const sch_hull* hull_A, const sch_hull* hull_B, sch_manifold* result); int sch_hull_is_vertex_concave(const sch_hull* hull, const simd4f p); @@ -497,7 +507,7 @@ float sch_query_face_directions (const sch_hull* hull_A, const sch_hull* hull_B, return result->dist; } -float sch_query_edge_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_face_query* result) { +float sch_query_edge_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_edge_query* result) { result->dist = -FLT_MAX; for (int iAe = 0; iAe < hull_A->num_edges; iAe++) { for (int iBe = 0; iBe < hull_B->num_edges; iBe++) { @@ -534,7 +544,8 @@ float sch_query_edge_directions (const sch_hull* hull_A, const sch_hull* hull_B, if (distance > result->dist) { result->dist = distance; result->plane = plane_A; - result->vert = vert_B; + result->edge_A_idx = iAe; + result->edge_B_idx = iBe; } } } @@ -635,6 +646,37 @@ void sch_create_face_contact (const sch_face_query* query_A_B, const sch_hull* h } } +void sch_create_edge_contact (const sch_edge_query* query_edge, const sch_hull* hull_A, const sch_hull* hull_B, sch_manifold* result) { + sch_edge edge_A = hull_A->edges[query_edge->edge_A_idx]; + sch_edge edge_B = hull_B->edges[query_edge->edge_B_idx]; + + sch_plane plane_A_n; + plane_A_n.p = edge_A.vert->p; + plane_A_n.n = simd4f_normalize3(simd4f_cross3(simd4f_sub(edge_A.vert->p, edge_A.next->vert->p), query_edge->plane.n)); + simd4f edge_B_projected; + bool is_edge_B_intersecting; + is_edge_B_intersecting = sch_plane_intersection(&plane_A_n, &edge_B.vert->p, &edge_B.next->vert->p, &edge_B_projected); + assert (is_edge_B_intersecting); + + sch_plane plane_B_n; + plane_B_n.p = edge_B.vert->p; + plane_B_n.n = simd4f_normalize3(simd4f_cross3(simd4f_sub(edge_B.vert->p, edge_B.next->vert->p), query_edge->plane.n)); + simd4f edge_A_projected; + bool is_edge_A_intersecting; + is_edge_A_intersecting = sch_plane_intersection(&plane_B_n, &edge_A.vert->p, &edge_A.next->vert->p, &edge_A_projected); + assert (is_edge_A_intersecting); + + sch_manifold_add_point(result,simd4f_add(simd4f_mul(edge_B_projected, simd4f_splat(0.5f)), + simd4f_mul(edge_B_projected, simd4f_splat(0.5f)))); + result->normal = query_edge->plane.n; + +#ifndef NDEBUG + simd4f closest_points_dir = simd4f_normalize3(simd4f_sub (edge_B_projected, edge_A_projected)); + float normal_error = 1.0f - simd4f_dot3_scalar(closest_points_dir, result->normal); + assert (fabs(normal_error) < SCH_EPS); +#endif +} + bool sch_hull_sat( const sch_hull* hull_A, const sch_hull* hull_B, @@ -651,18 +693,18 @@ bool sch_hull_sat( return true; } - sch_face_query query_edge; + sch_edge_query query_edge; sch_query_edge_directions (hull_A, hull_B, &query_edge); if (query_edge.dist > 0.f) { return true; } - bool is_face_contact_A = query_A_B.dist >= query_edge.dist; - bool is_face_contact_B = query_B_A.dist >= query_edge.dist; + bool is_face_contact_A = query_A_B.dist > query_edge.dist; + bool is_face_contact_B = query_B_A.dist > query_edge.dist; if (is_face_contact_A && is_face_contact_B) { sch_create_face_contact (&query_A_B, hull_A, &query_B_A, hull_B, result); } else { - // sch_create_edge_contact (manifold, &query_edge, &hull_A, &hull_B); + sch_create_edge_contact (&query_edge, hull_A, hull_B, result); } return false; diff --git a/tests/sconvcolTests.cc b/tests/sconvcolTests.cc index 305cead..90201c4 100644 --- a/tests/sconvcolTests.cc +++ b/tests/sconvcolTests.cc @@ -378,7 +378,17 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") { GIVEN("Box B translated by 1z and rotated 45 degrees around z") { sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 0.f, 1.f, 1.f)); - sch_hull_translate(&hull_B, 0., 0.f, 1.f); + sch_hull_translate(&hull_B, 0., 0.f, 0.97f); + + THEN("Boxes overlap") { + bool separated = sch_hull_sat(&hull_A, &hull_B, &manifold); + REQUIRE(!separated); + REQUIRE(manifold.num_points == 8); + } + } + + GIVEN("Box B translated by 1x, 1.001z") { + sch_hull_translate(&hull_B, 1., 0.f, 1.00f); THEN("Boxes overlap") { bool separated = sch_hull_sat(&hull_A, &hull_B, &manifold);