WIP: edge-edge collision with supplied transforms.

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) {

src/vissim.cc

@@ -598,13 +598,7 @@ struct SconvHullScene {
   SceneHull mHullB;
   SceneHull* mCurrentHull;
 
-  void init() {
+  void setupSceneRotTrans() {
-    sch_create_unitbox(&mHullA.hull);
-    mHullA.createMesh();
-
-    sch_create_unitbox(&mHullB.hull);
-    mHullB.createMesh();
-
     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;
   };

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);