From 7830840d81c53f830bbc05b7bb323deede53ac72 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Sun, 7 Nov 2021 20:19:25 +0100
Subject: [PATCH] Refactored HullScene making tests easier to visualize.

---
 src/sconvcol.h         |   6 +-
 src/vissim.cc          | 308 +++++++++++++++++++++++------------------
 tests/sconvcolTests.cc |  44 +++++-
 3 files changed, 218 insertions(+), 140 deletions(-)

diff --git a/src/sconvcol.h b/src/sconvcol.h
index de639d4..a6d9851 100644
--- a/src/sconvcol.h
+++ b/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;
   }
diff --git a/src/vissim.cc b/src/vissim.cc
index 59d1154..2317d1f 100644
--- a/src/vissim.cc
+++ b/src/vissim.cc
@@ -471,92 +471,62 @@ void SimulatorSceneRender(srcmdbuf* render_commands) {
   }
 }
 
-sch_hull box_hull;
-srmeshdata* box_hull_mesh;
-simd4x4f box_hull_trasform {
-    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)};
+struct SceneHull {
+  sch_hull hull;
+  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() {
-  sch_create_unitbox(&box_hull);
+  ~SceneHull() {
+    if (mesh != nullptr) {
+      destroy();
+    }
+  };
 
-  // create mesh for hull
-  static srvrtxdata mesh_buffer_data[1024];
-  static GLuint index_buffer_data[1024];
-  int vertex_index = -1;
-  int index_count = -1;
-  int triangle_count = 0;
+  void destroy() {
+    srmesh_destroy(mesh);
+    mesh = nullptr;
+  }
 
-  sch_hull* hull = &box_hull;
+  void createMesh() {
+    assert(mesh == nullptr);
 
-  int face_vert0_idx = 0;
+    // create mesh for hull
+    static srvrtxdata mesh_buffer_data[1024];
+    static GLuint index_buffer_data[1024];
+    int vertex_index = -1;
+    int index_count = -1;
+    int triangle_count = 0;
 
-  for (int fi = 0; fi < hull->num_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;
-    sch_vert* cur_vert = nullptr;
-    sch_edge* next_edge = nullptr;
-    sch_vert* next_vert = nullptr;
+    int face_vert0_idx = 0;
 
-    int tri_vert_0_idx = 0;
-    int tri_vert_1_idx = 0;
-    int tri_vert_2_idx = 0;
+    for (int fi = 0; fi < hull.num_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;
+      sch_vert* cur_vert = nullptr;
+      sch_edge* next_edge = nullptr;
+      sch_vert* next_vert = nullptr;
 
-    sch_plane plane;
-    sch_hull_calc_plane(hull, fi, &plane);
+      int tri_vert_0_idx = 0;
+      int tri_vert_1_idx = 0;
+      int tri_vert_2_idx = 0;
 
-    // add face_vert0 (later used as tri_vert_0)
-    vertex_index++;
-    face_vert0_idx = vertex_index;
-    tri_vert_0_idx = face_vert0_idx;
-    srvrtxdata* mesh_vert = &mesh_buffer_data[tri_vert_0_idx];
-    mesh_vert->x = simd4f_get_x(face_vert0->p);
-    mesh_vert->y = simd4f_get_y(face_vert0->p);
-    mesh_vert->z = simd4f_get_z(face_vert0->p);
-    mesh_vert->w = 1.f;
-    mesh_vert->nx = simd4f_get_x(plane.n);
-    mesh_vert->ny = simd4f_get_y(plane.n);
-    mesh_vert->nz = simd4f_get_z(plane.n);
-    mesh_vert->color[0] = fabs(mesh_vert->nx) * 255;
-    mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
-    mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
-    mesh_vert->color[3] = 255;
+      sch_plane plane;
+      sch_hull_calc_plane(&hull, fi, &plane);
 
-    // add tri_vert_1
-    cur_vert = cur_edge->vert;
-    tri_vert_1_idx = ++vertex_index;
-    mesh_vert = &mesh_buffer_data[tri_vert_1_idx];
-    mesh_vert->x = simd4f_get_x(cur_vert->p);
-    mesh_vert->y = simd4f_get_y(cur_vert->p);
-    mesh_vert->z = simd4f_get_z(cur_vert->p);
-    mesh_vert->w = 1.f;
-    mesh_vert->nx = simd4f_get_x(plane.n);
-    mesh_vert->ny = simd4f_get_y(plane.n);
-    mesh_vert->nz = simd4f_get_z(plane.n);
-    mesh_vert->color[0] = fabs(mesh_vert->nx) * 255;
-    mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
-    mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
-    mesh_vert->color[3] = 255;
-
-    do {
-      // add face_vert0
-      index_buffer_data[++index_count] = tri_vert_0_idx;
-
-      // add face_vert1
-      index_buffer_data[++index_count] = tri_vert_1_idx;
-
-      next_edge = cur_edge->next;
-      next_vert = next_edge->vert;
-      tri_vert_2_idx = ++vertex_index;
-
-      mesh_vert = &mesh_buffer_data[tri_vert_2_idx];
-      mesh_vert->x = simd4f_get_x(next_vert->p);
-      mesh_vert->y = simd4f_get_y(next_vert->p);
-      mesh_vert->z = simd4f_get_z(next_vert->p);
+      // add face_vert0 (later used as tri_vert_0)
+      vertex_index++;
+      face_vert0_idx = vertex_index;
+      tri_vert_0_idx = face_vert0_idx;
+      srvrtxdata* mesh_vert = &mesh_buffer_data[tri_vert_0_idx];
+      mesh_vert->x = simd4f_get_x(face_vert0->p);
+      mesh_vert->y = simd4f_get_y(face_vert0->p);
+      mesh_vert->z = simd4f_get_z(face_vert0->p);
       mesh_vert->w = 1.f;
       mesh_vert->nx = simd4f_get_x(plane.n);
       mesh_vert->ny = simd4f_get_y(plane.n);
@@ -565,70 +535,143 @@ void SconvHUllSceneInit() {
       mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
       mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
       mesh_vert->color[3] = 255;
-      index_buffer_data[++index_count] = tri_vert_2_idx;
 
-      triangle_count++;
-      cur_edge = next_edge;
-      tri_vert_1_idx = tri_vert_2_idx;
-    } while (next_edge->next != edge0);
-  }
+      // add tri_vert_1
+      cur_vert = cur_edge->vert;
+      tri_vert_1_idx = ++vertex_index;
+      mesh_vert = &mesh_buffer_data[tri_vert_1_idx];
+      mesh_vert->x = simd4f_get_x(cur_vert->p);
+      mesh_vert->y = simd4f_get_y(cur_vert->p);
+      mesh_vert->z = simd4f_get_z(cur_vert->p);
+      mesh_vert->w = 1.f;
+      mesh_vert->nx = simd4f_get_x(plane.n);
+      mesh_vert->ny = simd4f_get_y(plane.n);
+      mesh_vert->nz = simd4f_get_z(plane.n);
+      mesh_vert->color[0] = fabs(mesh_vert->nx) * 255;
+      mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
+      mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
+      mesh_vert->color[3] = 255;
 
-  box_hull_mesh = srmesh_create(
-      mesh_buffer_data,
-      vertex_index + 1,
-      index_buffer_data,
-      index_count + 1,
-      GL_TRIANGLES,
-      index_count + 1);
-}
+      do {
+        // add face_vert0
+        index_buffer_data[++index_count] = tri_vert_0_idx;
 
-void SconvHUllSceneCleanup() { srmesh_destroy(box_hull_mesh); }
+        // add face_vert1
+        index_buffer_data[++index_count] = tri_vert_1_idx;
 
-void SconvHUllSceneUpdate() {}
+        next_edge = cur_edge->next;
+        next_vert = next_edge->vert;
+        tri_vert_2_idx = ++vertex_index;
 
-void SconvHullSceneRender(srcmdbuf* render_commands) {
-  // Populate render commands
-  srcmdbuf_clear(render_commands);
+        mesh_vert = &mesh_buffer_data[tri_vert_2_idx];
+        mesh_vert->x = simd4f_get_x(next_vert->p);
+        mesh_vert->y = simd4f_get_y(next_vert->p);
+        mesh_vert->z = simd4f_get_z(next_vert->p);
+        mesh_vert->w = 1.f;
+        mesh_vert->nx = simd4f_get_x(plane.n);
+        mesh_vert->ny = simd4f_get_y(plane.n);
+        mesh_vert->nz = simd4f_get_z(plane.n);
+        mesh_vert->color[0] = fabs(mesh_vert->nx) * 255;
+        mesh_vert->color[1] = fabs(mesh_vert->ny) * 255;
+        mesh_vert->color[2] = fabs(mesh_vert->nz) * 255;
+        mesh_vert->color[3] = 255;
+        index_buffer_data[++index_count] = tri_vert_2_idx;
 
-  srcmd rcmd;
-  srcmd_clear(&rcmd);
-
-  rcmd.type = SRndrCmdTypeFrame;
-  srcmdbuf_add(render_commands, &rcmd);
-
-  rcmd.type = SRndrCmdTypeGrid;
-  float origin[3] = {0.f, 0.f, 0.f};
-
-  for (int i = -1; i <= 1; i++) {
-    for (int j = -1; j <= 1; j++) {
-      simd4x4f_translation(
-          &rcmd.mat,
-          origin[0] + i * 10.0f,
-          origin[1],
-          origin[2] + j * 10.f);
-      srcmdbuf_add(render_commands, &rcmd);
+        triangle_count++;
+        cur_edge = next_edge;
+        tri_vert_1_idx = tri_vert_2_idx;
+      } while (next_edge->next != edge0);
     }
+
+    mesh = srmesh_create(
+        mesh_buffer_data,
+        vertex_index + 1,
+        index_buffer_data,
+        index_count + 1,
+        GL_TRIANGLES,
+        index_count + 1);
+  }
+};
+
+struct SconvHullScene {
+  SceneHull mHullA;
+  SceneHull mHullB;
+  SceneHull* mCurrentHull;
+
+  void init() {
+    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));
+
+    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);
 
-  ImGuizmo::Manipulate(
-      (float*)&(gCameraState->mtxView),
-      (float*)&(gProjMatrix),
-      ImGuizmo::TRANSLATE,
-      ImGuizmo::WORLD,
-      (float*)(&box_hull_trasform),
-      NULL
-      );
+    srcmd rcmd;
+    srcmd_clear(&rcmd);
 
-  // Render hull
-  srcmd cmd;
-  cmd.type = SrndrCmdTypeMesh;
-  cmd.color = simd4f_create(1.f, 1.f, 1.f, 1.f);
-  cmd.mat = box_hull_trasform;
-  cmd.meshdata = box_hull_mesh;
+    rcmd.type = SRndrCmdTypeFrame;
+    srcmdbuf_add(render_commands, &rcmd);
 
-  srcmdbuf_add(render_commands, &cmd);
-}
+    rcmd.type = SRndrCmdTypeGrid;
+    float origin[3] = {0.f, 0.f, 0.f};
+
+    for (int i = -1; i <= 1; i++) {
+      for (int j = -1; j <= 1; j++) {
+        simd4x4f_translation(
+            &rcmd.mat,
+            origin[0] + i * 10.0f,
+            origin[1],
+            origin[2] + j * 10.f);
+        srcmdbuf_add(render_commands, &rcmd);
+      }
+    }
+
+    ImGuizmo::Manipulate(
+        (float*)&(gCameraState->mtxView),
+        (float*)&(gProjMatrix),
+        ImGuizmo::TRANSLATE,
+        ImGuizmo::WORLD,
+        (float*)(&mCurrentHull->transform),
+        NULL);
+
+    // Render hulls
+    srcmd cmd;
+    cmd.type = SrndrCmdTypeMesh;
+    cmd.color = simd4f_create(1.f, 1.f, 1.f, 1.f);
+    cmd.mat = mHullA.transform;
+    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);
diff --git a/tests/sconvcolTests.cc b/tests/sconvcolTests.cc
index 6d464d4..be97e2f 100644
--- a/tests/sconvcolTests.cc
+++ b/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),
-        simd4f_get_y(r),
-        simd4f_get_z(r));
+        0.5f + 0.4f * sqrtf(2.),
+        0.f,
+        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") {