From 2570f322e11fa3076a423377018fd40d26cce0ba Mon Sep 17 00:00:00 2001
From: UbitUmarov <ajlduarte@sapo.pt>
Date: Mon, 14 Aug 2023 21:49:38 +0100
Subject: [PATCH] ubode: fix a box-mesh collision issue

---
 .../ode/src/collision_trimesh_box.cpp         | 54 ++++++++-----------
 1 file changed, 23 insertions(+), 31 deletions(-)

diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_box.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_box.cpp
index 1dbf2ed9..ae772aec 100644
--- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_box.cpp
+++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_box.cpp
@@ -642,8 +642,8 @@ static bool _cldClosestPointOnTwoLines(
     // if denominator is positive
     if (fd > dEpsilon)
     {
-        fd = 1.0f / fd;
         // calculate points of closest approach
+        fd = 1.0f / fd;
         dVector3 vp;
         dSubtractVectors3r4(vp, vPoint2, vPoint1);
 
@@ -672,31 +672,24 @@ void sTrimeshBoxColliderData::_cldClipping(const dVector3 &v0, const dVector3 &v
         dVector3 vub, vPb, vPa;
         dCopyVector3r4(vPa, m_vHullBoxPos);
 
-        dVector3 *rot = (dVector3*)m_BoxRotTransposed;
-
         // calculate point on box edge
-        if(dCalcVectorDot3(m_vBestNormal, *rot) > 0)
-            dAddScaledVector3r4(vPa, *rot, m_vBoxHalfSize[0]);
+        if (dCalcVectorDot3(m_vBestNormal, m_BoxRotTransposed) > 0)
+            dAddScaledVector3r4(vPa, m_BoxRotTransposed, m_vBoxHalfSize[0]);
         else
-            dAddScaledVector3r4(vPa, *rot, -m_vBoxHalfSize[0]);
-        
-        rot += 4;
-        if (dCalcVectorDot3(m_vBestNormal, *rot) > 0)
-            dAddScaledVector3r4(vPa, *rot, m_vBoxHalfSize[1]);
-        else
-            dAddScaledVector3r4(vPa, *rot, -m_vBoxHalfSize[1]);
+            dAddScaledVector3r4(vPa, m_BoxRotTransposed, -m_vBoxHalfSize[0]);
 
-        rot += 4;
-        if (dCalcVectorDot3(m_vBestNormal, *rot))
-            dAddScaledVector3r4(vPa, *rot, m_vBoxHalfSize[2]);
+        if (dCalcVectorDot3(m_vBestNormal, m_BoxRotTransposed + 4) > 0)
+            dAddScaledVector3r4(vPa, m_BoxRotTransposed + 4, m_vBoxHalfSize[1]);
         else
-            dAddScaledVector3r4(vPa, *rot, -m_vBoxHalfSize[2]);
+            dAddScaledVector3r4(vPa, m_BoxRotTransposed + 4, -m_vBoxHalfSize[1]);
+
+        if (dCalcVectorDot3(m_vBestNormal, m_BoxRotTransposed + 8))
+            dAddScaledVector3r4(vPa, m_BoxRotTransposed + 8, m_vBoxHalfSize[2]);
+        else
+            dAddScaledVector3r4(vPa, m_BoxRotTransposed + 8, -m_vBoxHalfSize[2]);
 
         int iEdge = (m_iBestAxis - 5) % 3;
 
-        // setup direction parameter for box edge
-        dVector3& vua = *(dVector3*)(m_BoxRotTransposed + 4 * iEdge);
-
         // decide which edge is on triangle
         switch(iEdge)
         {
@@ -718,6 +711,10 @@ void sTrimeshBoxColliderData::_cldClipping(const dVector3 &v0, const dVector3 &v
 
         dReal fParam1, fParam2;
 
+        // setup direction parameter for box edge
+        int col = (m_iBestAxis - 5) / 3;
+        dVector3& vua = *(dVector3*)(m_BoxRotTransposed + 4 * col);
+
         // find two closest points on both edges
         _cldClosestPointOnTwoLines(vPa, vua, vPb, vub, fParam1, fParam2);
         dAddScaledVector3r4(vPa, vua, fParam1);
@@ -1045,13 +1042,7 @@ static void dQueryBTLPotentialCollisionTriangles(OBBCollider &Collider,
     Box.mCenter.x = vPosBox[0];
     Box.mCenter.y = vPosBox[1];
     Box.mCenter.z = vPosBox[2];
-
-    // It is a potential issue to explicitly cast to float 
-    // if custom width floating point type is introduced in OPCODE.
-    // It is necessary to make a typedef and cast to it
-    // (e.g. typedef float opc_float;)
-    // However I'm not sure in what header it should be added.
-
+    
     Box.mExtents.x = cData.m_vBoxHalfSize[0];
     Box.mExtents.y = cData.m_vBoxHalfSize[1];
     Box.mExtents.z = cData.m_vBoxHalfSize[2];
@@ -1102,7 +1093,8 @@ static void dQueryBTLPotentialCollisionTriangles(OBBCollider &Collider,
     else
     {
         Collider.SetTemporalCoherence(false);
-        Collider.Collide(BoxCache, Box, TriMesh->Data->BVTree, null, &MakeMatrix(vPosMesh, mRotMesh, amatrix));
+        Collider.Collide(BoxCache, Box, TriMesh->Data->BVTree, null,
+            &MakeMatrix(vPosMesh, mRotMesh, amatrix));
     }
 }
 
@@ -1122,7 +1114,8 @@ int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts,
     TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
     OBBCollider& Collider = pccColliderCache->_OBBCollider;
 
-    dQueryBTLPotentialCollisionTriangles(Collider, cData, TriMesh, BoxGeom, pccColliderCache->defaultBoxCache);
+    dQueryBTLPotentialCollisionTriangles(Collider, cData, TriMesh, BoxGeom,
+        pccColliderCache->defaultBoxCache);
 
     if (!Collider.GetContactStatus()) {
         // no collision occurred
@@ -1176,7 +1169,8 @@ int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts,
 //   position and normal as an existing contact, but a larger
 //   penetration depth, this new depth is used instead
 //
-static void GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
+static void
+GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
     dxGeom* in_g1, dxGeom* in_g2, int TriIndex,
     const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
     int& OutTriCount)
@@ -1229,9 +1223,7 @@ static void GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stri
             }
         }
         if (duplicate || OutTriCount == (in_Flags & NUMC_MASK))
-        {
             return;
-        }
     }
     else
     {