diff --git a/src/sconvcol.h b/src/sconvcol.h index 23e3f08..6d8f3ac 100644 --- a/src/sconvcol.h +++ b/src/sconvcol.h @@ -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); -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( 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; } -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* last_edge = edge; float normal_dot_edge = -1.; + simd4f n_local; + simd4x4f_inv_ortho_matrix_vector3_mul(trans, &n, &n_local); + do { 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.) { 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); - *out_vert = edge->vert->p; + simd4x4f_matrix_point3_mul(trans, &edge->vert->p, out_vert); } float sch_query_face_directions( @@ -519,18 +522,14 @@ float sch_query_face_directions( sch_plane plane; sch_hull_calc_plane(hull_A, fi, &plane); simd4f vert_B; - simd4f plane_n_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); + sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane.n), &vert_B); - simd4f vert_A; - simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B, &vert_A); - float distance = sch_plane_distance(&plane, &vert_A); + float distance = sch_plane_distance(&plane, &vert_B); if (distance > result->dist) { result->dist = distance; result->face_idx = fi; 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 // 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); + simd4x4f identity; + simd4x4f_identity(&identity); + sch_hull_get_support(hull_A, &identity, plane_A.n, &plane_A.p); 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; simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B_B, &vert_B); diff --git a/tests/sconvcolTests.cc b/tests/sconvcolTests.cc index 5db295d..f2b5cfd 100644 --- a/tests/sconvcolTests.cc +++ b/tests/sconvcolTests.cc @@ -261,10 +261,13 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") { sch_hull hull; sch_create_unitbox(&hull); + simd4x4f identity; + simd4x4f_identity(&identity); + 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); + sch_hull_get_support(&hull, &identity, 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)); @@ -274,7 +277,7 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") { 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); + sch_hull_get_support(&hull, &identity, 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)); @@ -284,7 +287,7 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") { 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); + sch_hull_get_support(&hull, &identity, normal, &support); THEN ("y component of support is 0.5") { REQUIRE (simd4f_get_y(support) == 0.5); } @@ -639,4 +642,4 @@ TEST_CASE ("ClipFaces", "[sconvcol]") { sch_manifold_free_memory(&manifold); } } -} \ No newline at end of file +}