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

sconvcol: querying for support vertex now needs transformation of the hull.

master
Martin Felis 2021-11-16 14:53:46 +01:00
parent 33f08ff39b
commit 71bed07ed1
2 changed files with 21 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/sconvcol.h
View File

@ -168,7 +168,7 @@ void sch_hull_calc_plane(const sch_hull* hull, const int index, sch_plane* out_p
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, simd4x4f* trans, simd4f n, simd4f* out_vert);
float sch_query_face_directions( float sch_query_face_directions(
const sch_hull* hull_A, const sch_hull* hull_A,
@ -489,14 +489,17 @@ sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4
return NULL; return NULL;
} }
void sch_hull_get_support(const sch_hull* hull, simd4f normal, simd4f* out_vert) { void sch_hull_get_support(const sch_hull* hull, simd4x4f* trans, simd4f n, simd4f* out_vert) {
sch_edge* edge = hull->faces[0].edge; sch_edge* edge = hull->faces[0].edge;
sch_edge* last_edge = edge; sch_edge* last_edge = edge;
float normal_dot_edge = -1.; float normal_dot_edge = -1.;
simd4f n_local;
simd4x4f_inv_ortho_matrix_vector3_mul(trans, &n, &n_local);
do { do {
simd4f dir = simd4f_normalize3(simd4f_sub (edge->next->vert->p, edge->vert->p)); simd4f dir = simd4f_normalize3(simd4f_sub (edge->next->vert->p, edge->vert->p));
normal_dot_edge = simd4f_dot3_scalar(dir, normal); normal_dot_edge = simd4f_dot3_scalar(dir, n_local);
if (normal_dot_edge <= 0.) { if (normal_dot_edge <= 0.) {
edge = edge->twin->next; edge = edge->twin->next;
@ -506,7 +509,7 @@ void sch_hull_get_support(const sch_hull* hull, simd4f normal, simd4f* out_vert)
} }
} while (last_edge != edge || normal_dot_edge > 0); } while (last_edge != edge || normal_dot_edge > 0);
*out_vert = edge->vert->p; simd4x4f_matrix_point3_mul(trans, &edge->vert->p, out_vert);
} }
float sch_query_face_directions( float sch_query_face_directions(
@ -519,18 +522,14 @@ float sch_query_face_directions(
sch_plane plane; sch_plane plane;
sch_hull_calc_plane(hull_A, fi, &plane); sch_hull_calc_plane(hull_A, fi, &plane);
simd4f vert_B; simd4f vert_B;
simd4f plane_n_B; sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane.n), &vert_B);
simd4x4f_inv_ortho_matrix_vector3_mul(trans_BtoA, &plane.n, &plane_n_B);
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_n_B), &vert_B);
simd4f vert_A; float distance = sch_plane_distance(&plane, &vert_B);
simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B, &vert_A);
float distance = sch_plane_distance(&plane, &vert_A);
if (distance > result->dist) { if (distance > result->dist) {
result->dist = distance; result->dist = distance;
result->face_idx = fi; result->face_idx = fi;
result->plane = plane; result->plane = plane;
result->vert = vert_A; result->vert = vert_B;
} }
} }
@ -572,10 +571,12 @@ float sch_query_edge_directions(
// Note: in Gregorius talk he uses the origin of edge_A. This is wrong as // 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 // 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. // 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); simd4x4f identity;
simd4x4f_identity(&identity);
sch_hull_get_support(hull_A, &identity, plane_A.n, &plane_A.p);
simd4f vert_B_B; simd4f vert_B_B;
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B); sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B);
simd4f vert_B; simd4f vert_B;
simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B_B, &vert_B); simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B_B, &vert_B);

11
tests/sconvcolTests.cc
View File

@ -261,10 +261,13 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") {
sch_hull hull; sch_hull hull;
sch_create_unitbox(&hull); sch_create_unitbox(&hull);
simd4x4f identity;
simd4x4f_identity(&identity);
WHEN("Querying support for 2, 2, 2") { WHEN("Querying support for 2, 2, 2") {
simd4f support; simd4f support;
simd4f normal = simd4f_create(2.f, 2.f, 2.f, 1.f); simd4f normal = simd4f_create(2.f, 2.f, 2.f, 1.f);
sch_hull_get_support(&hull, normal, &support); sch_hull_get_support(&hull, &identity, normal, &support);
THEN ("support is 0.5, 0.5, 0.5") { THEN ("support is 0.5, 0.5, 0.5") {
simd4f reference = simd4f_create (0.5f, 0.5f, 0.5f, 1.f); simd4f reference = simd4f_create (0.5f, 0.5f, 0.5f, 1.f);
REQUIRE (sch_simd4f_equal(reference, support)); REQUIRE (sch_simd4f_equal(reference, support));
@ -274,7 +277,7 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") {
WHEN("Querying support for -2, -2, -2") { WHEN("Querying support for -2, -2, -2") {
simd4f support; simd4f support;
simd4f normal = simd4f_create(-2.f, -2.f, -2.f, 1.f); simd4f normal = simd4f_create(-2.f, -2.f, -2.f, 1.f);
sch_hull_get_support(&hull, normal, &support); sch_hull_get_support(&hull, &identity, normal, &support);
THEN ("support is -0.5, -0.5, -0.5") { THEN ("support is -0.5, -0.5, -0.5") {
simd4f reference = simd4f_create (-0.5f, -0.5f, -0.5f, 1.f); simd4f reference = simd4f_create (-0.5f, -0.5f, -0.5f, 1.f);
REQUIRE (sch_simd4f_equal(reference, support)); REQUIRE (sch_simd4f_equal(reference, support));
@ -284,7 +287,7 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") {
WHEN("Querying support for 0, 1, 0") { WHEN("Querying support for 0, 1, 0") {
simd4f support; simd4f support;
simd4f normal = simd4f_create(0.f, 1.f, 0.f, 1.f); simd4f normal = simd4f_create(0.f, 1.f, 0.f, 1.f);
sch_hull_get_support(&hull, normal, &support); sch_hull_get_support(&hull, &identity, normal, &support);
THEN ("y component of support is 0.5") { THEN ("y component of support is 0.5") {
REQUIRE (simd4f_get_y(support) == 0.5); REQUIRE (simd4f_get_y(support) == 0.5);
} }
@ -639,4 +642,4 @@ TEST_CASE ("ClipFaces", "[sconvcol]") {
sch_manifold_free_memory(&manifold); sch_manifold_free_memory(&manifold);
} }
} }
} }