diff --git a/src/sconvcol.h b/src/sconvcol.h index a6d9851..23e3f08 100644 --- a/src/sconvcol.h +++ b/src/sconvcol.h @@ -550,8 +550,10 @@ float sch_query_edge_directions( simd4f dir_A; sch_edge_get_dir(edge_A, &dir_A); + simd4f dir_B_B; + sch_edge_get_dir(edge_B, &dir_B_B); simd4f dir_B; - sch_edge_get_dir(edge_B, &dir_B); + simd4x4f_inv_ortho_matrix_vector3_mul(trans_BtoA, &dir_B_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) { @@ -572,8 +574,11 @@ float sch_query_edge_directions( // 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_B; + sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B); simd4f vert_B; - sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B); + simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B_B, &vert_B); + float distance = sch_plane_distance(&plane_A, &vert_B); if (distance > result->dist) { diff --git a/src/vissim.cc b/src/vissim.cc index 2317d1f..b3f76ca 100644 --- a/src/vissim.cc +++ b/src/vissim.cc @@ -598,13 +598,7 @@ struct SconvHullScene { SceneHull mHullB; SceneHull* mCurrentHull; - void init() { - sch_create_unitbox(&mHullA.hull); - mHullA.createMesh(); - - sch_create_unitbox(&mHullB.hull); - mHullB.createMesh(); - + void setupSceneRotTrans() { simd4x4f rot; simd4x4f_axis_rotation(&rot, 0.4f, simd4f_create (0.f, 1.f, 0.f, 1.f)); @@ -612,6 +606,32 @@ struct SconvHullScene { simd4x4f_translation(&translation, -0.5f - 0.51f * sqrtf(2.), 0.0f, 0.f); simd4x4f_matrix_mul(&translation, &rot, &mHullB.transform); + } + + void setupSceneEdgeCollision() { + simd4x4f rot_y; + simd4x4f_axis_rotation(&rot_y, 0.25f * M_PI, simd4f_create (0.f, 1.f, 0.f, 1.f)); + + simd4x4f rot_z; + simd4x4f_axis_rotation(&rot_z, 0.25f * M_PI, simd4f_create (0.f, 0.f, 1.f, 1.f)); + + simd4x4f rot; + simd4x4f_matrix_mul(&rot_y, &rot_z, &rot); + + simd4x4f translation; + simd4x4f_translation(&translation, 0.98f, 0.f, -0.98f); + + simd4x4f_matrix_mul(&translation, &rot, &mHullB.transform); + } + + void init() { + sch_create_unitbox(&mHullA.hull); + mHullA.createMesh(); + + sch_create_unitbox(&mHullB.hull); + mHullB.createMesh(); + + setupSceneEdgeCollision(); mCurrentHull = &mHullA; }; diff --git a/tests/sconvcolTests.cc b/tests/sconvcolTests.cc index be97e2f..5db295d 100644 --- a/tests/sconvcolTests.cc +++ b/tests/sconvcolTests.cc @@ -422,6 +422,27 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") { } } + GIVEN ("Box B in edge-edge contact with A") { + simd4x4f rot_y; + simd4x4f_axis_rotation(&rot_y, 0.25f * M_PI, simd4f_create (0.f, 1.f, 0.f, 1.f)); + + simd4x4f rot_z; + simd4x4f_axis_rotation(&rot_z, 0.25f * M_PI, simd4f_create (0.f, 0.f, 1.f, 1.f)); + + simd4x4f trans_rot; + simd4x4f_matrix_mul(&rot_y, &rot_z, &trans_rot); + + simd4x4f translation; + simd4x4f_translation(&translation, 0.98f, 0.f, -0.98f); + + simd4x4f identity; + simd4x4f_identity(&identity); + + bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &trans_rot, &manifold); + 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);