martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/sconvcol.h
View File

@ -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;

12
tests/sconvcolTests.cc
View File

@ -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);