Added sch_hull_sat to check whether we can find a separating axes between two hulls.

master
Martin Felis 2021-05-30 00:19:57 +02:00
parent e93a659daf
commit 4834b483ef
2 changed files with 283 additions and 6 deletions

View File

@ -7,6 +7,7 @@ extern "C" {
#include <assert.h> #include <assert.h>
#include <stdbool.h> #include <stdbool.h>
#include <float.h>
#include "vectorial/simd4x4f.h" #include "vectorial/simd4x4f.h"
@ -14,6 +15,10 @@ inline bool sch_simd4f_equal(simd4f a, simd4f b) {
return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) == 0.f); return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) == 0.f);
} }
inline bool sch_simd4f_close(simd4f a, simd4f b, float tol) {
return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) < tol * tol);
}
typedef struct sch_edge sch_edge; typedef struct sch_edge sch_edge;
typedef struct sch_vert sch_vert; typedef struct sch_vert sch_vert;
typedef struct sch_face sch_face; typedef struct sch_face sch_face;
@ -42,6 +47,7 @@ struct sch_hull {
sch_face* faces; sch_face* faces;
sch_edge* edges; sch_edge* edges;
sch_vert* vertices; sch_vert* vertices;
simd4f center;
int num_faces; int num_faces;
int num_edges; int num_edges;
int num_vertices; int num_vertices;
@ -53,11 +59,10 @@ struct sch_plane {
}; };
struct sch_face_query { struct sch_face_query {
sch_hull* hull_A;
sch_hull* hull_B;
float dist; float dist;
int face_idx_A; int face_idx;
int face_idx_B; sch_plane plane;
simd4f vert;
}; };
// //
@ -107,12 +112,22 @@ void sch_edge_get_dir(const sch_edge* edge, simd4f* out_dir);
void sch_hull_free_memory (sch_hull* hull); void sch_hull_free_memory (sch_hull* hull);
void sch_hull_translate (sch_hull* hull, float x, float y, float z);
void sch_hull_rotate (sch_hull* hull, const float radians, const simd4f axis);
void sch_hull_transform (sch_hull* hull, simd4x4f mat);
void sch_hull_get_plane(const sch_hull* hull, const int index, sch_plane* out_plane); void sch_hull_get_plane(const sch_hull* hull, const int index, sch_plane* out_plane);
sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4f v1); sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4f v1);
void sch_hull_get_support(const sch_hull* hull, simd4f n, simd4f* out_vert); void sch_hull_get_support(const sch_hull* hull, simd4f n, simd4f* out_vert);
float sch_query_face_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_face_query* result);
bool sch_hull_sat (const sch_hull* hullA, const sch_hull* hullB);
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);
int sch_hull_is_closed (const sch_hull* hull); int sch_hull_is_closed (const sch_hull* hull);
@ -203,6 +218,35 @@ void sch_builder_allocate_memory (sch_hull_builder* builder, sch_hull* out_hull)
out_hull->edges = (sch_edge*)malloc(sizeof(sch_edge) * num_edges); out_hull->edges = (sch_edge*)malloc(sizeof(sch_edge) * num_edges);
} }
void sch_hull_translate (sch_hull* hull, float x, float y, float z) {
simd4f r = simd4f_create (x, y, z, 0.f);
hull->center = simd4f_add (hull->center, r);
for (int vi = 0; vi < hull->num_vertices; vi++) {
hull->vertices[vi].p = simd4f_add (hull->vertices[vi].p, r);
}
}
void sch_hull_rotate (sch_hull* hull, const float radians, const simd4f axis) {
simd4x4f rot_mat;
simd4x4f_axis_rotation(&rot_mat, radians, axis);
simd4x4f_matrix_vector_mul(&rot_mat, &hull->center, &hull->center);
for (int vi = 0; vi < hull->num_vertices; vi++) {
simd4x4f_matrix_vector_mul(&rot_mat, &hull->vertices[vi].p, &hull->vertices[vi].p);
}
}
void sch_hull_transform (sch_hull* hull, simd4x4f mat) {
simd4f v_temp = hull->center;
simd4x4f_matrix_vector_mul(&mat, &v_temp, &hull->center);
for (int vi = 0; vi < hull->num_vertices; vi++) {
v_temp = hull->vertices[vi].p;
simd4x4f_matrix_vector_mul(&mat, &v_temp, &hull->vertices[vi].p);
}
}
void sch_hull_free_memory (sch_hull* hull) { void sch_hull_free_memory (sch_hull* hull) {
free (hull->faces); free (hull->faces);
hull->faces = NULL; hull->faces = NULL;
@ -359,6 +403,100 @@ void sch_hull_get_support(const sch_hull* hull, simd4f normal, simd4f* out_vert)
*out_vert = edge->vert->p; *out_vert = edge->vert->p;
} }
float sch_query_face_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_face_query* result) {
result->dist = -FLT_MAX;
for (int fi = 0; fi < hull_A->num_faces; fi++) {
sch_plane plane;
sch_hull_get_plane(hull_A, fi, &plane);
simd4f vert;
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane.n), &vert);
float distance = sch_plane_distance(&plane, &vert);
if (distance > result->dist) {
result->dist = distance;
result->face_idx = fi;
result->plane = plane;
result->vert = vert;
}
}
return result->dist;
}
float sch_query_edge_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_face_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++) {
sch_edge* edge_A = &hull_A->edges[iAe];
sch_edge* edge_B = &hull_B->edges[iBe];
simd4f dir_A;
sch_edge_get_dir(edge_A, &dir_A);
simd4f dir_B;
sch_edge_get_dir(edge_B, &dir_B);
simd4f axis = simd4f_cross3 (dir_A, dir_B);
if (simd4f_dot3_scalar(dir_A, simd4f_sub(edge_A->vert->p, hull_A->center) ) < 0.f) {
axis = simd4f_sub(simd4f_zero(), axis);
}
if (simd4f_get_x(simd4f_length3_squared(axis)) < 0.001) {
continue;
}
axis = simd4f_normalize3(axis);
sch_plane plane_A;
plane_A.n = axis;
// Note: in Gregorius talk he uses the origin of edge_A. This is wrong as
// there are likely points in hull_A that are further out along the plane
// normal. We therefore use the support point of hull_A along the axis.
sch_hull_get_support(hull_A, plane_A.n, &plane_A.p);
simd4f vert_B;
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B);
float distance = sch_plane_distance(&plane_A, &vert_B);
if (distance > result->dist) {
result->dist = distance;
result->plane = plane_A;
result->vert = vert_B;
}
}
}
return result->dist;
}
bool sch_hull_sat (const sch_hull* hull_A, const sch_hull* hull_B) {
sch_face_query query_A_B;
sch_query_face_directions(hull_A, hull_B, &query_A_B);
if (query_A_B.dist > 0.f) {
return true;
}
sch_face_query query_B_A;
sch_query_face_directions(hull_B, hull_A, &query_B_A);
if (query_B_A.dist > 0.f) {
return true;
}
sch_face_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;
if (is_face_contact_A && is_face_contact_B) {
// sch_create_face_contact (manifold, &query_A_B, &hull_A, &query_B_A, &hull_B);
} else {
// sch_create_edge_contact (manifold, &query_edge, &hull_A, &hull_B);
}
return false;
}
int sch_hull_is_vertex_concave(const sch_hull* hull, const simd4f v) { int sch_hull_is_vertex_concave(const sch_hull* hull, const simd4f v) {
sch_plane plane; sch_plane plane;
for (int i = 0; i < hull->num_faces; i++) { for (int i = 0; i < hull->num_faces; i++) {
@ -482,6 +620,7 @@ void sch_create_unitbox(sch_hull* out_hull) {
sch_builder_face_end(&builder); sch_builder_face_end(&builder);
int hull_result = sch_builder_create_hull(&builder, out_hull); int hull_result = sch_builder_create_hull(&builder, out_hull);
out_hull->center = simd4f_create(0.f, 0.f, 0.f, 1.f);
assert (hull_result == SchHullResultOK); assert (hull_result == SchHullResultOK);
} }

View File

@ -34,8 +34,6 @@ TEST_CASE("Edge Get Direction", "[sconvcol]") {
REQUIRE(1.f == simd4f_dot3_scalar(dir, dir_ref)); REQUIRE(1.f == simd4f_dot3_scalar(dir, dir_ref));
} }
TEST_CASE("ManualCubeHull", "[sconvcol]") {}
TEST_CASE("HullBuilder", "[sconvcol]") { TEST_CASE("HullBuilder", "[sconvcol]") {
sch_hull_builder builder; sch_hull_builder builder;
@ -238,3 +236,143 @@ TEST_CASE("HullBuilder", "[sconvcol]") {
} }
} }
} }
TEST_CASE("UnitCubeSupport", "[sconvcol]") {
GIVEN("A unit cube hull") {
sch_hull hull;
sch_create_unitbox(&hull);
WHEN("Querying support for 2, 2, 2") {
simd4f support;
simd4f normal = simd4f_create(2.f, 2.f, 2.f, 1.f);
sch_hull_get_support(&hull, normal, &support);
THEN ("support is 0.5, 0.5, 0.5") {
simd4f reference = simd4f_create (0.5f, 0.5f, 0.5f, 1.f);
REQUIRE (sch_simd4f_equal(reference, support));
}
}
WHEN("Querying support for -2, -2, -2") {
simd4f support;
simd4f normal = simd4f_create(-2.f, -2.f, -2.f, 1.f);
sch_hull_get_support(&hull, normal, &support);
THEN ("support is -0.5, -0.5, -0.5") {
simd4f reference = simd4f_create (-0.5f, -0.5f, -0.5f, 1.f);
REQUIRE (sch_simd4f_equal(reference, support));
}
}
WHEN("Querying support for 0, 1, 0") {
simd4f support;
simd4f normal = simd4f_create(0.f, 1.f, 0.f, 1.f);
sch_hull_get_support(&hull, normal, &support);
THEN ("y component of support is 0.5") {
REQUIRE (simd4f_get_y(support) == 0.5);
}
}
sch_hull_free_memory(&hull);
}
}
TEST_CASE("UnitCubeTransform", "[sconvcol]") {
GIVEN("A unit cube hull") {
sch_hull hull;
sch_create_unitbox(&hull);
WHEN ("not transforming cube") {
THEN ("center is at 0,0,0") {
REQUIRE (sch_simd4f_equal(simd4f_create(0.f, 0.f, 0.f, 1.f), hull.center));
}
}
WHEN ("translating by 1, 2, 3") {
sch_hull_translate(&hull, 1.f, 2.f, 3.f);
THEN ("center is at 1, 2, 3") {
REQUIRE (sch_simd4f_equal(simd4f_create(1.f, 2.f, 3.f, 1.f), hull.center));
}
THEN ("is equivalent to transform by translation matrix") {
sch_hull hull_trans_mat;
sch_create_unitbox(&hull_trans_mat);
simd4x4f trans_mat;
simd4x4f_translation(&trans_mat, 1.f, 2.f, 3.f);
sch_hull_transform(&hull_trans_mat, trans_mat);
REQUIRE (sch_simd4f_equal(hull.center, hull_trans_mat.center));
sch_hull_free_memory(&hull_trans_mat);
}
}
WHEN ("rotating around y by 90 degrees") {
simd4f vert0 = simd4f_create (0.5f, -0.5f, 0.5f, 1.f);
REQUIRE (sch_simd4f_equal(vert0, hull.vertices[0].p));
simd4f vert0_rot = simd4f_create (0.5f, -0.5f, -0.5f, 1.f);
sch_hull_rotate(&hull, M_PI * 0.5, simd4f_create (1.f, 0.f, 0.f, 1.f));
THEN ("center is at 0, 0, 0") {
REQUIRE (sch_simd4f_equal(simd4f_create(0.f, 0.f, 0.f, 1.f), hull.center));
REQUIRE (sch_simd4f_close(vert0_rot, hull.vertices[0].p, 1.0e-5f));
}
}
sch_hull_free_memory(&hull);
}
}
TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
sch_hull hull_A;
sch_create_unitbox(&hull_A);
sch_hull hull_B;
sch_create_unitbox(&hull_B);
GIVEN ("Two unit boxes separated by 1.1 along x axis") {
simd4f r = simd4f_create(0.5f + 0.4f * sqrtf(2.), 0.f, 0.f, 1.f);
simd4x4f translation;
simd4x4f_translation(
&translation,
simd4f_get_x(r),
simd4f_get_y(r),
simd4f_get_z(r));
sch_hull_transform(&hull_B, translation);
THEN("Boxes separated") {
bool separated = sch_hull_sat(&hull_A, &hull_B);
REQUIRE(separated);
}
}
GIVEN("Box B rotated by 45 degrees around y and translated along x by 1.1") {
sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 1.f, 0.f, 1.f));
sch_hull_translate(&hull_B, 1.1f, 0.f, 0.f);
THEN("Boxes overlap") {
bool separated = sch_hull_sat(&hull_A, &hull_B);
REQUIRE(!separated);
}
}
GIVEN("Box A rotated by 45 deg around z, Box B rotated by 45 degrees around y and translated along x by sqrt(2)") {
sch_hull_rotate(&hull_A, 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, 1.f, 0.f, 1.f));
sch_hull_translate(&hull_B, sqrt(2.f), 0.f, 0.f);
THEN("Boxes overlap") {
bool separated = sch_hull_sat(&hull_A, &hull_B);
REQUIRE(!separated);
}
WHEN("Shifted by another 0.001 along x") {
sch_hull_translate(&hull_B, 0.001f, 0.f, 0.f);
THEN("Boxes are separated") {
bool separated = sch_hull_sat(&hull_A, &hull_B);
REQUIRE(separated);
}
}
}
sch_hull_free_memory(&hull_A);
sch_hull_free_memory(&hull_B);
}