Refactored HullScene making tests easier to visualize.

src/sconvcol.h

@@ -524,7 +524,7 @@ float sch_query_face_directions(
     sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_n_B), &vert_B);
 
     simd4f vert_A;
-    simd4x4f_inv_ortho_matrix_point3_mul(trans_BtoA, &vert_B, &vert_A);
+    simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B, &vert_A);
     float distance = sch_plane_distance(&plane, &vert_A);
     if (distance > result->dist) {
       result->dist = distance;
@@ -730,7 +730,9 @@ bool sch_hull_sat(
   }
 
   sch_face_query query_B_A;
-  sch_query_face_directions(hull_B, &trans_BtoA, hull_A, &query_B_A);
+  simd4x4f trans_AtoB;
+  sch_simd4x4f_transform_inv(&trans_BtoA, &trans_AtoB);
+  sch_query_face_directions(hull_B, &trans_AtoB, hull_A, &query_B_A);
   if (query_B_A.dist > 0.f) {
     return true;
   }

src/vissim.cc

@@ -471,16 +471,28 @@ void SimulatorSceneRender(srcmdbuf* render_commands) {
   }
 }
 
-sch_hull box_hull;
+struct SceneHull {
-srmeshdata* box_hull_mesh;
+  sch_hull hull;
-simd4x4f box_hull_trasform {
+  srmeshdata* mesh = nullptr;
+  simd4x4f transform = {
       simd4f_create(1.f, 0.f, 0.0, 0.f),
       simd4f_create(0.f, 1.f, 0.f, 0.f),
       simd4f_create(0.f, 0.f, 1.f, 0.f),
       simd4f_create(0.f, 0.f, 0.f, 1.f)};
 
-void SconvHUllSceneInit() {
+  ~SceneHull() {
-  sch_create_unitbox(&box_hull);
+    if (mesh != nullptr) {
+      destroy();
+    }
+  };
+
+  void destroy() {
+    srmesh_destroy(mesh);
+    mesh = nullptr;
+  }
+
+  void createMesh() {
+    assert(mesh == nullptr);
 
     // create mesh for hull
     static srvrtxdata mesh_buffer_data[1024];
@@ -489,12 +501,10 @@ void SconvHUllSceneInit() {
     int index_count = -1;
     int triangle_count = 0;
 
-  sch_hull* hull = &box_hull;
-
     int face_vert0_idx = 0;
 
-  for (int fi = 0; fi < hull->num_faces; fi++) {
+    for (int fi = 0; fi < hull.num_faces; fi++) {
-    sch_face* face = &hull->faces[fi];
+      sch_face* face = &hull.faces[fi];
       sch_edge* edge0 = face->edge;
       sch_vert* face_vert0 = edge0->vert;
       sch_edge* cur_edge = edge0->next;
@@ -507,7 +517,7 @@ void SconvHUllSceneInit() {
       int tri_vert_2_idx = 0;
 
       sch_plane plane;
-    sch_hull_calc_plane(hull, fi, &plane);
+      sch_hull_calc_plane(&hull, fi, &plane);
 
       // add face_vert0 (later used as tri_vert_0)
       vertex_index++;
@@ -573,20 +583,45 @@ void SconvHUllSceneInit() {
       } while (next_edge->next != edge0);
     }
 
-  box_hull_mesh = srmesh_create(
+    mesh = srmesh_create(
         mesh_buffer_data,
         vertex_index + 1,
         index_buffer_data,
         index_count + 1,
         GL_TRIANGLES,
         index_count + 1);
-}
+  }
+};
 
-void SconvHUllSceneCleanup() { srmesh_destroy(box_hull_mesh); }
+struct SconvHullScene {
+  SceneHull mHullA;
+  SceneHull mHullB;
+  SceneHull* mCurrentHull;
 
-void SconvHUllSceneUpdate() {}
+  void init() {
+    sch_create_unitbox(&mHullA.hull);
+    mHullA.createMesh();
 
-void SconvHullSceneRender(srcmdbuf* render_commands) {
+    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));
+
+    simd4x4f translation;
+    simd4x4f_translation(&translation, -0.5f  - 0.51f * sqrtf(2.), 0.0f, 0.f);
+
+    simd4x4f_matrix_mul(&translation, &rot, &mHullB.transform);
+
+    mCurrentHull = &mHullA;
+  };
+
+  void destroy() {
+    mHullA.destroy();
+    mHullB.destroy();
+  }
+
+  void draw(srcmdbuf* render_commands) {
     // Populate render commands
     srcmdbuf_clear(render_commands);
 
@@ -610,25 +645,33 @@ void SconvHullSceneRender(srcmdbuf* render_commands) {
       }
     }
 
-
     ImGuizmo::Manipulate(
         (float*)&(gCameraState->mtxView),
         (float*)&(gProjMatrix),
         ImGuizmo::TRANSLATE,
         ImGuizmo::WORLD,
-      (float*)(&box_hull_trasform),
+        (float*)(&mCurrentHull->transform),
-      NULL
+        NULL);
-      );
 
-  // Render hull
+    // Render hulls
     srcmd cmd;
     cmd.type = SrndrCmdTypeMesh;
     cmd.color = simd4f_create(1.f, 1.f, 1.f, 1.f);
-  cmd.mat = box_hull_trasform;
+    cmd.mat = mHullA.transform;
-  cmd.meshdata = box_hull_mesh;
+    cmd.meshdata = mHullA.mesh;
 
     srcmdbuf_add(render_commands, &cmd);
-}
+
+    cmd.type = SrndrCmdTypeMesh;
+    cmd.color = simd4f_create(1.f, 1.f, 1.f, 1.f);
+    cmd.mat = mHullB.transform;
+    cmd.meshdata = mHullB.mesh;
+
+    srcmdbuf_add(render_commands, &cmd);
+  }
+};
+
+SconvHullScene gHullScene;
 
 void ImSRenderWidget(srndr* renderer, srview* view) {
   ZoneScoped;
@@ -777,7 +820,7 @@ int main() {
   // Scene init
   //
   simulator_init();
-  SconvHUllSceneInit();
+  gHullScene.init();
 
   simd4x4f_translation(&gCameraState->mtxView, 0.1f, 0.1f, -0.5f);
   simd4f eye = simd4f_create(5.f, 4.f, 5.f, 1.f);
@@ -929,14 +972,13 @@ int main() {
         ImGui::End();
       }
 
-      SconvHullSceneRender(gRndrCmds);
+      gHullScene.draw(gRndrCmds);
       //      SimulatorSceneRender(gRndrCmds);
       srndr_render(gRndr, gView, gRndrCmds);
       ImSRenderWidget(gRndr, gView);
     }
 
     simulator_update(gTimer->mDeltaTime);
-    SconvHUllSceneUpdate();
 
     {
       ZoneNamedN(render_submit, "RenderSubmit", true);
@@ -986,7 +1028,7 @@ int main() {
 
   gLog("Exiting application");
 
-  SconvHUllSceneCleanup();
+  gHullScene.destroy();
 
   srview_destroy(gView);
   srndr_destroy(gRndr);

tests/sconvcolTests.cc

@@ -350,13 +350,12 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
   sch_manifold_alloc(&manifold, SCH_BUILDER_MAX_NUM_FACE_VERTICES);
 
   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),
+        0.5f + 0.4f * sqrtf(2.),
-        simd4f_get_y(r),
+        0.f,
-        simd4f_get_z(r));
+        0.f);
 
     THEN("Boxes separated") {
       sch_hull_transform(&hull_B, translation);
@@ -374,7 +373,7 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
     }
 
     THEN("Boxes separated when supplying trans_A") {
-      simd4f r = simd4f_create(-0.5f  -0.4f * sqrtf(2.), 0.f, 0.f, 1.f);
+      simd4f r = simd4f_create(-0.5f  -0.51f * sqrtf(2.), 0.f, 0.f, 1.f);
       simd4x4f_translation(
           &translation,
           simd4f_get_x(r),
@@ -386,6 +385,41 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
       bool separated = sch_hull_sat(&hull_A, &translation, &hull_B, &identity, &manifold);
       REQUIRE(separated);
     }
+
+    THEN("Boxes separated when supplying rot_trans to B") {
+      simd4x4f identity;
+      simd4x4f_identity(&identity);
+
+      simd4x4f rot;
+      simd4x4f_axis_rotation(&rot, 0.4f, simd4f_create (0.f, 1.f, 0.f, 1.f));
+
+      simd4x4f translation;
+      simd4x4f_translation(&translation, 0.5f + 0.51f * sqrtf(2.), 0.0f, 0.f);
+
+      simd4x4f rot_trans;
+      simd4x4f_matrix_mul(&translation, &rot, &rot_trans);
+
+      bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &rot_trans, &manifold);
+      REQUIRE(separated);
+    }
+
+
+    THEN("Boxes separated when supplying rot_trans to A") {
+      simd4x4f identity;
+      simd4x4f_identity(&identity);
+
+      simd4x4f rot;
+      simd4x4f_axis_rotation(&rot, 0.4f, simd4f_create (0.f, 1.f, 0.f, 1.f));
+
+      simd4x4f translation;
+      simd4x4f_translation(&translation, -0.5f  - 0.51f * sqrtf(2.), 0.0f, 0.f);
+
+      simd4x4f rot_trans;
+      simd4x4f_matrix_mul(&translation, &rot, &rot_trans);
+
+      bool separated = sch_hull_sat(&hull_A, &rot_trans, &hull_B, &identity, &manifold);
+      REQUIRE(separated);
+    }
   }
 
   GIVEN("Box B rotated by 45 degrees around y and translated along x by 1.1") {