Working on edge-edge contact manifold generation. Possibly found issue for face-face contact generation.

master
Martin Felis 2021-08-25 22:29:18 +02:00
parent 0e378b6a60
commit 87f07397d2
2 changed files with 59 additions and 7 deletions

View File

@ -28,6 +28,7 @@ typedef struct sch_hull sch_hull;
typedef struct sch_plane sch_plane; typedef struct sch_plane sch_plane;
typedef struct sch_hull_builder sch_hull_builder; typedef struct sch_hull_builder sch_hull_builder;
typedef struct sch_face_query sch_face_query; typedef struct sch_face_query sch_face_query;
typedef struct sch_edge_query sch_edge_query;
typedef struct sch_manifold sch_manifold; typedef struct sch_manifold sch_manifold;
struct sch_edge { struct sch_edge {
@ -68,6 +69,13 @@ struct sch_face_query {
simd4f vert; simd4f vert;
}; };
struct sch_edge_query {
float dist;
sch_plane plane;
int edge_A_idx;
int edge_B_idx;
};
struct sch_manifold { struct sch_manifold {
int num_points; int num_points;
int num_vertices; 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_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); 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); 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; 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; result->dist = -FLT_MAX;
for (int iAe = 0; iAe < hull_A->num_edges; iAe++) { for (int iAe = 0; iAe < hull_A->num_edges; iAe++) {
for (int iBe = 0; iBe < hull_B->num_edges; iBe++) { 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) { if (distance > result->dist) {
result->dist = distance; result->dist = distance;
result->plane = plane_A; 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( bool sch_hull_sat(
const sch_hull* hull_A, const sch_hull* hull_A,
const sch_hull* hull_B, const sch_hull* hull_B,
@ -651,18 +693,18 @@ bool sch_hull_sat(
return true; return true;
} }
sch_face_query query_edge; sch_edge_query query_edge;
sch_query_edge_directions (hull_A, hull_B, &query_edge); sch_query_edge_directions (hull_A, hull_B, &query_edge);
if (query_edge.dist > 0.f) { if (query_edge.dist > 0.f) {
return true; return true;
} }
bool is_face_contact_A = query_A_B.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; bool is_face_contact_B = query_B_A.dist > query_edge.dist;
if (is_face_contact_A && is_face_contact_B) { if (is_face_contact_A && is_face_contact_B) {
sch_create_face_contact (&query_A_B, hull_A, &query_B_A, hull_B, result); sch_create_face_contact (&query_A_B, hull_A, &query_B_A, hull_B, result);
} else { } 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; return false;

View File

@ -378,7 +378,17 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
GIVEN("Box B translated by 1z and rotated 45 degrees around z") { 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_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") { THEN("Boxes overlap") {
bool separated = sch_hull_sat(&hull_A, &hull_B, &manifold); bool separated = sch_hull_sat(&hull_A, &hull_B, &manifold);