add ode

1 files changed, 463 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_internal_impl.h b/libs/ode-0.16.1/ode/src/collision_trimesh_internal_impl.h
new file mode 100644
index 0000000..be41ff5
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/collision_trimesh_internal_impl.h
@@ -0,0 +1,463 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh base template method implementations by Oleh Derevenko (C) 2016-2019
+
+
+#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_IMPL_H_
+#define _ODE_COLLISION_TRIMESH_INTERNAL_IMPL_H_
+
+
+#include "collision_trimesh_internal.h"
+
+
+#if dTRIMESH_ENABLED
+
+
+template<typename tcoordfloat, typename tindexint>
+/*static */
+void dxTriDataBase::retrieveTriangleVertexPoints(dVector3 out_Points[dMTV__MAX], unsigned triangleIndex,
+    const tcoordfloat *vertexInstances, int vertexStride, const tindexint *triangleVertexIndices, int triangleStride)
+{
+    const tindexint *triangleIndicesOfInterest = (const tindexint *)((uint8 *)triangleVertexIndices + (sizeint)triangleIndex * triangleStride);
+    for (unsigned trianglePoint = dMTV__MIN; trianglePoint != dMTV__MAX; ++trianglePoint)
+    {
+        unsigned vertexIndex = triangleIndicesOfInterest[trianglePoint];
+        tcoordfloat *pointVertex = (tcoordfloat *)((uint8 *)vertexInstances + (sizeint)vertexIndex * vertexStride);
+        dAssignVector3(out_Points[trianglePoint], (dReal)pointVertex[dSA_X], (dReal)pointVertex[dSA_Y], (dReal)pointVertex[dSA_Z]);
+        dSASSERT(dSA_X == 0);
+        dSASSERT(dSA_Y == 1);
+        dSASSERT(dSA_Z == 2);
+    }
+}
+
+
+template<class TMeshDataAccessor>
+/*static */
+void dxTriDataBase::meaningfulPreprocess_SetupEdgeRecords(EdgeRecord *edges, sizeint numEdges, const TMeshDataAccessor &dataAccessor)
+{
+    unsigned vertexIndices[dMTV__MAX];
+    // Make a list of every edge in the mesh
+    unsigned triangleIdx = 0;
+    for (sizeint edgeIdx = 0; edgeIdx != numEdges; ++triangleIdx, edgeIdx += dMTV__MAX)
+    {
+        dataAccessor.getTriangleVertexIndices(vertexIndices, triangleIdx);
+        edges[edgeIdx + dMTV_FIRST].setupEdge(dMTV_FIRST, triangleIdx, vertexIndices);
+        edges[edgeIdx + dMTV_SECOND].setupEdge(dMTV_SECOND, triangleIdx, vertexIndices);
+        edges[edgeIdx + dMTV_THIRD].setupEdge(dMTV_THIRD, triangleIdx, vertexIndices);
+    }
+}
+
+template<class TMeshDataAccessor>
+/*static */
+void dxTriDataBase::meaningfulPreprocess_buildEdgeFlags(uint8 *useFlags/*=NULL*/, IFaceAngleStorageControl *faceAngles/*=NULL*/, 
+    EdgeRecord *edges, sizeint numEdges, VertexRecord *vertices, 
+    const dReal *externalNormals/*=NULL*/, const TMeshDataAccessor &dataAccessor)
+{
+    dIASSERT(useFlags != NULL || faceAngles != NULL);
+    dIASSERT(numEdges != 0);
+
+    const bool negativeAnglesStored = faceAngles != NULL && faceAngles->areNegativeAnglesStored();
+
+    // Go through the sorted list of edges and flag all the edges and vertices that we need to use
+    EdgeRecord *const lastEdge = edges + (numEdges - 1);
+    for (EdgeRecord *currEdge = edges; ; ++currEdge)
+    {
+        // Handle the last edge separately to have an optimizer friendly loop
+        if (currEdge >= lastEdge)
+        {
+            // This is a boundary edge
+            if (currEdge == lastEdge)
+            {
+                if (faceAngles != NULL)
+                {
+                    buildBoundaryEdgeAngle(faceAngles, currEdge);
+                }
+
+                if (useFlags != NULL)
+                {
+                    // For the last element EdgeRecord::kAbsVertexUsed assignment can be skipped as noone is going to need it any more
+                    useFlags[currEdge[0].m_triIdx] |= ((edges[currEdge[0].m_vertIdx1].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0 ? currEdge[0].m_vert1Flags : 0) 
+                        | ((edges[currEdge[0].m_vertIdx2].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0 ? currEdge[0].m_vert2Flags : 0)
+                        | currEdge[0].m_edgeFlags;
+                }
+            }
+
+            break;
+        }
+
+        unsigned vertIdx1 = currEdge[0].m_vertIdx1;
+        unsigned vertIdx2 = currEdge[0].m_vertIdx2;
+
+        if (vertIdx2 == currEdge[1].m_vertIdx2 // Check second vertex first as it is more likely to change taking the sorting rules into account
+            && vertIdx1 == currEdge[1].m_vertIdx1)
+        {
+            // We let the dot threshold for concavity get slightly negative to allow for rounding errors
+            const float kConcaveThreshold = 0.000001f;
+
+            const dVector3 *pSecondTriangleEdgeToUse = NULL, *pFirstTriangleToUse = NULL;
+            dVector3 secondTriangleMatchingEdge;
+            dVector3 firstTriangle[dMTV__MAX];
+            dVector3 secondOppositeVertexSegment, triangleNormal;
+            dReal lengthSquareProduct, secondOppositeSegmentLengthSquare;
+
+            // Calculate orthogonal vector from the matching edge of the second triangle to its opposite point
+            {
+                dVector3 secondTriangle[dMTV__MAX];
+                dataAccessor.getTriangleVertexPoints(secondTriangle, currEdge[1].m_triIdx);
+
+                // Get the vertex opposite this edge in the second triangle
+                dMeshTriangleVertex secondOppositeVertex = currEdge[1].getOppositeVertexIndex();
+                dMeshTriangleVertex secondEdgeStart = secondOppositeVertex + 1 != dMTV__MAX ? (dMeshTriangleVertex)(secondOppositeVertex + 1) : dMTV__MIN;
+                dMeshTriangleVertex secondEdgeEnd = (dMeshTriangleVertex)(dMTV_FIRST + dMTV_SECOND + dMTV_THIRD - secondEdgeStart - secondOppositeVertex);
+
+                dSubtractVectors3(secondTriangleMatchingEdge, secondTriangle[secondEdgeEnd], secondTriangle[secondEdgeStart]);
+
+                if (dSafeNormalize3(secondTriangleMatchingEdge))
+                {
+                    pSecondTriangleEdgeToUse = &secondTriangleMatchingEdge;
+
+                    dVector3 secondTriangleOppositeEdge;
+                    dSubtractVectors3(secondTriangleOppositeEdge, secondTriangle[secondOppositeVertex], secondTriangle[secondEdgeStart]);
+                    dReal dProjectionLength = dCalcVectorDot3(secondTriangleOppositeEdge, secondTriangleMatchingEdge);
+                    dAddVectorScaledVector3(secondOppositeVertexSegment, secondTriangleOppositeEdge, secondTriangleMatchingEdge, -dProjectionLength);
+                }
+                else
+                {
+                    dSubtractVectors3(secondOppositeVertexSegment, secondTriangle[secondOppositeVertex], secondTriangle[secondEdgeStart]);
+                }
+
+                secondOppositeSegmentLengthSquare = dCalcVectorLengthSquare3(secondOppositeVertexSegment);
+            }
+
+            // Either calculate the normal from triangle vertices...
+            if (externalNormals == NULL)
+            {
+                // Get the normal of the first triangle
+                dataAccessor.getTriangleVertexPoints(firstTriangle, currEdge[0].m_triIdx);
+                pFirstTriangleToUse = &firstTriangle[dMTV__MIN];
+
+                dVector3 firstEdge, secondEdge;
+                dSubtractVectors3(secondEdge, firstTriangle[dMTV_THIRD], firstTriangle[dMTV_SECOND]);
+                dSubtractVectors3(firstEdge, firstTriangle[dMTV_FIRST], firstTriangle[dMTV_SECOND]);
+                dCalcVectorCross3(triangleNormal, secondEdge, firstEdge);
+                dReal normalLengthSuqare = dCalcVectorLengthSquare3(triangleNormal);
+                lengthSquareProduct = secondOppositeSegmentLengthSquare * normalLengthSuqare;
+            }
+            // ...or use the externally supplied normals
+            else
+            {
+                const dReal *pTriangleExternalNormal = externalNormals + currEdge[0].m_triIdx * dSA__MAX;
+                dAssignVector3(triangleNormal, pTriangleExternalNormal[dSA_X], pTriangleExternalNormal[dSA_Y], pTriangleExternalNormal[dSA_Z]);
+                // normalLengthSuqare = REAL(1.0);
+                dUASSERT(dFabs(dCalcVectorLengthSquare3(triangleNormal) - REAL(1.0)) < REAL(0.25) * kConcaveThreshold * kConcaveThreshold, "Mesh triangle normals must be normalized");
+
+                lengthSquareProduct = secondOppositeSegmentLengthSquare/* * normalLengthSuqare*/;
+            }
+
+            dReal normalSegmentDot = dCalcVectorDot3(triangleNormal, secondOppositeVertexSegment);
+
+            // This is a concave edge, leave it for the next pass
+            // OD: This is the "dot >= kConcaveThresh" check, but since the vectros were not normalized to save on roots and divisions,
+            // the check against zero is performed first and then the dot product is squared and compared against the threshold multiplied by lengths' squares
+            // OD: Originally, there was dot > -kConcaveThresh check, but this does not seem to be a good idea
+            // as it can mark all edges on potentially large (nearly) flat surfaces concave.
+            if (normalSegmentDot > REAL(0.0) && normalSegmentDot * normalSegmentDot >= kConcaveThreshold * kConcaveThreshold * lengthSquareProduct)
+            {
+                if (faceAngles != NULL)
+                {
+                    buildConcaveEdgeAngle(faceAngles, negativeAnglesStored, currEdge, normalSegmentDot, lengthSquareProduct,
+                        triangleNormal, secondOppositeVertexSegment,
+                        pSecondTriangleEdgeToUse, pFirstTriangleToUse, dataAccessor);
+                }
+
+                if (useFlags != NULL)
+                {
+                    // Mark the vertices of a concave edge to prevent their use
+                    unsigned absVertexFlags1 = edges[vertIdx1].m_absVertexFlags;
+                    edges[vertIdx1].m_absVertexFlags |= absVertexFlags1 | EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED;
+
+                    if ((absVertexFlags1 & (EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED)) == EdgeRecord::AVF_VERTEX_USED)
+                    {
+                        // If the vertex was already used from other triangles but then discovered 
+                        // to have a concave edge, unmark the previous use
+                        unsigned usedFromEdgeIndex = vertices[vertIdx1].m_UsedFromEdgeIndex;
+                        const EdgeRecord *usedFromEdge = edges + usedFromEdgeIndex;
+                        unsigned usedInTriangleIndex = usedFromEdge->m_triIdx;
+                        uint8 usedVertFlags = usedFromEdge->m_vertIdx1 == vertIdx1 ? usedFromEdge->m_vert1Flags : usedFromEdge->m_vert2Flags;
+                        useFlags[usedInTriangleIndex] ^= usedVertFlags;
+                        dIASSERT((useFlags[usedInTriangleIndex] & usedVertFlags) == 0);
+                    }
+
+                    unsigned absVertexFlags2 = edges[vertIdx2].m_absVertexFlags;
+                    edges[vertIdx2].m_absVertexFlags = absVertexFlags2 | EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED;
+
+                    if ((absVertexFlags2 & (EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED)) == EdgeRecord::AVF_VERTEX_USED)
+                    {
+                        // Similarly unmark the possible previous use of the edge's second vertex
+                        unsigned usedFromEdgeIndex = vertices[vertIdx2].m_UsedFromEdgeIndex;
+                        const EdgeRecord *usedFromEdge = edges + usedFromEdgeIndex;
+                        unsigned usedInTriangleIndex = usedFromEdge->m_triIdx;
+                        uint8 usedVertFlags = usedFromEdge->m_vertIdx1 == vertIdx2 ? usedFromEdge->m_vert1Flags : usedFromEdge->m_vert2Flags;
+                        useFlags[usedInTriangleIndex] ^= usedVertFlags;
+                        dIASSERT((useFlags[usedInTriangleIndex] & usedVertFlags) == 0);
+                    }
+                }
+            }
+            // If this is a convex edge, mark its vertices and edge as used
+            else
+            {
+                if (faceAngles != NULL)
+                {
+                    buildConvexEdgeAngle(faceAngles, currEdge, normalSegmentDot, lengthSquareProduct,
+                        triangleNormal, secondOppositeVertexSegment,
+                        pSecondTriangleEdgeToUse, pFirstTriangleToUse, dataAccessor);
+                }
+
+                if (useFlags != NULL)
+                {
+                    EdgeRecord *edgeToUse = currEdge;
+                    unsigned triIdx = edgeToUse[0].m_triIdx;
+                    unsigned triIdx1 = edgeToUse[1].m_triIdx;
+                    
+                    unsigned triUseFlags = useFlags[triIdx];
+                    unsigned triUseFlags1 = useFlags[triIdx1];
+
+                    // Choose to add flags to the bitmask that already has more edges
+                    // (to group flags in selected triangles rather than scattering them evenly)
+                    if ((triUseFlags1 & CUF__USE_ALL_EDGES) > (triUseFlags & CUF__USE_ALL_EDGES))
+                    {
+                        triIdx = triIdx1;
+                        triUseFlags = triUseFlags1;
+                        edgeToUse = edgeToUse + 1;
+                    }
+
+                    if ((edges[vertIdx1].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0)
+                    {
+                        // Only add each vertex once and set a mark to prevent further additions
+                        edges[vertIdx1].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED;
+                        // Also remember the index the vertex flags are going to be applied to 
+                        // to allow easily clear the vertex from the use flags if any concave edges are found to connect to it
+                        vertices[vertIdx1].m_UsedFromEdgeIndex = (unsigned)(edgeToUse - edges);
+                        triUseFlags |= edgeToUse[0].m_vert1Flags;
+                    }
+
+                    // Same processing for the second vertex...
+                    if ((edges[vertIdx2].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0)
+                    {
+                        edges[vertIdx2].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED;
+                        vertices[vertIdx2].m_UsedFromEdgeIndex = (unsigned)(edgeToUse - edges);
+                        triUseFlags |= edgeToUse[0].m_vert2Flags;
+                    }
+
+                    // And finally store the use flags adding the edge flags in
+                    useFlags[triIdx] = triUseFlags | edgeToUse[0].m_edgeFlags;
+                }
+            }
+
+            // Skip the second edge
+            ++currEdge;
+        }
+        // This is a boundary edge
+        else
+        {
+            if (faceAngles != NULL)
+            {
+                buildBoundaryEdgeAngle(faceAngles, currEdge);
+            }
+
+            if (useFlags != NULL)
+            {
+                unsigned triIdx = currEdge[0].m_triIdx;
+                unsigned triUseExtraFlags = 0;
+                
+                if ((edges[vertIdx1].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0)
+                {
+                    edges[vertIdx1].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED;
+                    vertices[vertIdx1].m_UsedFromEdgeIndex = (unsigned)(currEdge - edges);
+                    triUseExtraFlags |= currEdge[0].m_vert1Flags;
+                }
+
+                if ((edges[vertIdx2].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0)
+                {
+                    edges[vertIdx2].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED;
+                    vertices[vertIdx2].m_UsedFromEdgeIndex = (unsigned)(currEdge - edges);
+                    triUseExtraFlags |= currEdge[0].m_vert2Flags;
+                }
+
+                useFlags[triIdx] |= triUseExtraFlags | currEdge[0].m_edgeFlags;
+            }
+        }
+    }
+}
+
+/*static */
+void dxTriDataBase::buildBoundaryEdgeAngle(IFaceAngleStorageControl *faceAngles, 
+    EdgeRecord *currEdge)
+{
+    const dReal faceAngle = REAL(0.0);
+
+    dMeshTriangleVertex firstVertexStartIndex = currEdge[0].getEdgeStartVertexIndex();
+    faceAngles->assignFacesAngleIntoStorage(currEdge[0].m_triIdx, firstVertexStartIndex, faceAngle);
+    // -- For boundary edges, only the first element is valid
+    // dMeshTriangleVertex secondVertexStartIndex = currEdge[1].getEdgeStartVertexIndex();
+    // faceAngles->assignFacesAngleIntoStorage(currEdge[1].m_TriIdx, secondVertexStartIndex, faceAngle);
+}
+
+template<class TMeshDataAccessor>
+/*static */
+void dxTriDataBase::buildConcaveEdgeAngle(IFaceAngleStorageControl *faceAngles, bool negativeAnglesStored, 
+    EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct,
+    const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment,
+    const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, 
+    const TMeshDataAccessor &dataAccessor)
+{
+    dReal faceAngle;
+    dMeshTriangleVertex firstVertexStartIndex = currEdge[0].getEdgeStartVertexIndex();
+
+    // Check if concave angles are stored at all
+    if (negativeAnglesStored)
+    {
+        // The length square product can become zero due to precision loss
+        // when both the normal and the opposite edge vectors are very small.
+        if (lengthSquareProduct != REAL(0.0))
+        {
+            faceAngle = -calculateEdgeAngleValidated(firstVertexStartIndex,
+                currEdge, normalSegmentDot, lengthSquareProduct, triangleNormal, secondOppositeVertexSegment,
+                pSecondTriangleMatchingEdge, pFirstTriangle, dataAccessor);
+        }
+        else
+        {
+            faceAngle = REAL(0.0);
+        }
+    }
+    else
+    {
+        // If concave angles ate not stored, set an arbitrary negative value
+        faceAngle = -(dReal)M_PI;
+    }
+
+    faceAngles->assignFacesAngleIntoStorage(currEdge[0].m_triIdx, firstVertexStartIndex, faceAngle);
+    dMeshTriangleVertex secondVertexStartIndex = currEdge[1].getEdgeStartVertexIndex();
+    faceAngles->assignFacesAngleIntoStorage(currEdge[1].m_triIdx, secondVertexStartIndex, faceAngle);
+}
+
+template<class TMeshDataAccessor>
+/*static */
+void dxTriDataBase::buildConvexEdgeAngle(IFaceAngleStorageControl *faceAngles, 
+    EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct,
+    const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment,
+    const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, 
+    const TMeshDataAccessor &dataAccessor)
+{
+    dReal faceAngle;
+    dMeshTriangleVertex firstVertexStartIndex = currEdge[0].getEdgeStartVertexIndex();
+
+    // The length square product can become zero due to precision loss
+    // when both the normal and the opposite edge vectors are very small.
+    if (normalSegmentDot < REAL(0.0) && lengthSquareProduct != REAL(0.0))
+    {
+        faceAngle = calculateEdgeAngleValidated(firstVertexStartIndex,
+            currEdge, -normalSegmentDot, lengthSquareProduct, triangleNormal, secondOppositeVertexSegment,
+            pSecondTriangleMatchingEdge, pFirstTriangle, dataAccessor);
+    }
+    else
+    {
+        faceAngle = REAL(0.0);
+    }
+
+    faceAngles->assignFacesAngleIntoStorage(currEdge[0].m_triIdx, firstVertexStartIndex, faceAngle);
+    dMeshTriangleVertex secondVertexStartIndex = currEdge[1].getEdgeStartVertexIndex();
+    faceAngles->assignFacesAngleIntoStorage(currEdge[1].m_triIdx, secondVertexStartIndex, faceAngle);
+}
+
+template<class TMeshDataAccessor>
+/*static */
+dReal dxTriDataBase::calculateEdgeAngleValidated(unsigned firstVertexStartIndex,
+    EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct,
+    const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment,
+    const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, 
+    const TMeshDataAccessor &dataAccessor)
+{
+    dIASSERT(lengthSquareProduct >= REAL(0.0));
+
+    dReal result;
+    dReal angleCosine = normalSegmentDot / dSqrt(lengthSquareProduct);
+
+    if (angleCosine < REAL(1.0))
+    {
+        dVector3 normalSecondOppositeSegmentCross;
+        dCalcVectorCross3(normalSecondOppositeSegmentCross, triangleNormal, secondOppositeVertexSegment);
+
+        dReal secondTriangleEdgeDirectionCheck;
+
+        if (pSecondTriangleMatchingEdge != NULL)
+        {
+            // Check the cross product against the second triangle edge, if possible...
+            secondTriangleEdgeDirectionCheck = dCalcVectorDot3(normalSecondOppositeSegmentCross, *pSecondTriangleMatchingEdge);
+        }
+        else
+        {
+            // ...if not, calculate the supposed direction of the second triangle's edge 
+            // as negative of first triangle edge. For that cross-multiply the precomputed
+            // first triangle normal by vector from the degenerate edge to its opposite vertex.
+
+            // Retrieve the first triangle points if necessary
+            dVector3 firstTriangleStorage[dMTV__MAX];
+            const dVector3 *pFirstTriangleToUse = pFirstTriangle;
+
+            if (pFirstTriangle == NULL)
+            {
+                dataAccessor.getTriangleVertexPoints(firstTriangleStorage, currEdge[0].m_triIdx);
+                pFirstTriangleToUse = &firstTriangleStorage[dMTV__MIN];
+            }
+
+            // Calculate the opposite vector
+            unsigned firstTriangleOppositeIndex = firstVertexStartIndex != dMTV__MIN ? firstVertexStartIndex - 1 : dMTV__MAX - 1;
+
+            dVector3 firstOppositeVertexSegment;
+            dSubtractVectors3(firstOppositeVertexSegment, pFirstTriangleToUse[firstTriangleOppositeIndex], pFirstTriangleToUse[firstVertexStartIndex]);
+
+            dVector3 normalFirstOppositeSegmentCross;
+            dCalcVectorCross3(normalFirstOppositeSegmentCross, triangleNormal, firstOppositeVertexSegment);
+
+            // And finally calculate the dot product to compare vector directions
+            secondTriangleEdgeDirectionCheck = dCalcVectorDot3(normalSecondOppositeSegmentCross, normalFirstOppositeSegmentCross);
+        }
+
+        // Negative product means the angle absolute value is less than M_PI_2, positive - greater.
+        result = secondTriangleEdgeDirectionCheck < REAL(0.0) ? dAsin(angleCosine) : (dReal)M_PI_2 + dAcos(angleCosine);
+    }
+    else
+    {
+        result = (dReal)M_PI_2;
+        dIASSERT(angleCosine - REAL(1.0) < 1e-4); // The computational error can not be too high because the dot product had been verified to be greater than the concave threshold above
+    }
+
+    return result;
+}
+
+
+#endif // #if dTRIMESH_ENABLED
+
+
+#endif // #ifndef _ODE_COLLISION_TRIMESH_INTERNAL_IMPL_H_