Added sch_hull_sat to check whether we can find a separating axes between two hulls.
Martin Felis
parent
e93a659daf
commit
4834b483ef
147
src/sconvcol.h
147
src/sconvcol.h
|
|
@ -7,6 +7,7 @@ extern "C" {
|
|||
|
||||
#include <assert.h>
|
||||
#include <stdbool.h>
|
||||
#include <float.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);
|
||||
}
|
||||
|
||||
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_vert sch_vert;
|
||||
typedef struct sch_face sch_face;
|
||||
|
|
@ -42,6 +47,7 @@ struct sch_hull {
|
|||
sch_face* faces;
|
||||
sch_edge* edges;
|
||||
sch_vert* vertices;
|
||||
simd4f center;
|
||||
int num_faces;
|
||||
int num_edges;
|
||||
int num_vertices;
|
||||
|
|
@ -53,11 +59,10 @@ struct sch_plane {
|
|||
};
|
||||
|
||||
struct sch_face_query {
|
||||
sch_hull* hull_A;
|
||||
sch_hull* hull_B;
|
||||
float dist;
|
||||
int face_idx_A;
|
||||
int face_idx_B;
|
||||
int face_idx;
|
||||
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_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);
|
||||
|
||||
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);
|
||||
|
||||
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_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);
|
||||
}
|
||||
|
||||
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) {
|
||||
free (hull->faces);
|
||||
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;
|
||||
}
|
||||
|
||||
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) {
|
||||
sch_plane plane;
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -34,8 +34,6 @@ TEST_CASE("Edge Get Direction", "[sconvcol]") {
|
|||
REQUIRE(1.f == simd4f_dot3_scalar(dir, dir_ref));
|
||||
}
|
||||
|
||||
TEST_CASE("ManualCubeHull", "[sconvcol]") {}
|
||||
|
||||
TEST_CASE("HullBuilder", "[sconvcol]") {
|
||||
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);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue