EPAConvexHullBuilder.h

Go to the documentation of this file.

// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
 
#pragma once
 
// Define to validate the integrity of the hull structure
//#define JPH_EPA_CONVEX_BUILDER_VALIDATE
 
// Define to draw the building of the hull for debugging purposes
//#define JPH_EPA_CONVEX_BUILDER_DRAW
 
#include <Jolt/Core/NonCopyable.h>
 
#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
    #include <Jolt/Renderer/DebugRenderer.h>
    #include <Jolt/Core/StringTools.h>
#endif
 
JPH_NAMESPACE_BEGIN
 
class EPAConvexHullBuilder : public NonCopyable
{
private:
#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
    static constexpr Real cDrawScale = 10;
#endif
 
public:
    // Due to the Euler characteristic (https://en.wikipedia.org/wiki/Euler_characteristic) we know that Vertices - Edges + Faces = 2
    // In our case we only have triangles and they are always fully connected, so each edge is shared exactly between 2 faces: Edges = Faces * 3 / 2
    // Substituting: Vertices = Faces / 2 + 2 which is approximately Faces / 2.
    static constexpr int cMaxTriangles = 256;               
    static constexpr int cMaxPoints = cMaxTriangles / 2;    
 
    // Constants
    static constexpr int cMaxEdgeLength = 128;              
    static constexpr float cMinTriangleArea = 1.0e-10f;     
    static constexpr float cBarycentricEpsilon = 1.0e-3f;   
 
    // Forward declare
    class Triangle;
 
    class Edge
    {
    public:
        Triangle *      mNeighbourTriangle;                 
        int             mNeighbourEdge;                     
 
        int             mStartIdx;                          
    };
 
    using Edges = StaticArray<Edge, cMaxEdgeLength>;
    using NewTriangles = StaticArray<Triangle *, cMaxEdgeLength>;
 
    class Triangle : public NonCopyable
    {
    public:
        inline          Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions);
 
        inline bool     IsFacing(Vec3Arg inPosition) const
        {
            JPH_ASSERT(!mRemoved);
            return mNormal.Dot(inPosition - mCentroid) > 0.0f;
        }
 
        inline bool     IsFacingOrigin() const
        {
            JPH_ASSERT(!mRemoved);
            return mNormal.Dot(mCentroid) < 0.0f;
        }
 
        inline const Edge & GetNextEdge(int inIndex) const
        {
            return mEdge[(inIndex + 1) % 3];
        }
 
        Edge            mEdge[3];                           
        Vec3            mNormal;                            
        Vec3            mCentroid;                          
        float           mClosestLenSq = FLT_MAX;            
        float           mLambda[2];                         
        bool            mLambdaRelativeTo0;                 
        bool            mClosestPointInterior = false;      
        bool            mRemoved = false;                   
        bool            mInQueue = false;                   
#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
        int             mIteration;                         
#endif
    };
 
    class TriangleFactory : public NonCopyable
    {
    private:
        union alignas(Triangle) Block
        {
            uint8       mTriangle[sizeof(Triangle)];
            Block *     mNextFree;
        };
 
        Block           mTriangles[cMaxTriangles];          
        Block *         mNextFree = nullptr;                
        int             mHighWatermark = 0;                 
 
    public:
        void            Clear()
        {
            mNextFree = nullptr;
            mHighWatermark = 0;
        }
 
        Triangle *      CreateTriangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions)
        {
            Triangle *t;
            if (mNextFree != nullptr)
            {
                // Entry available from the free list
                t = reinterpret_cast<Triangle *>(&mNextFree->mTriangle);
                mNextFree = mNextFree->mNextFree;
            }
            else
            {
                // Allocate from never used before triangle store
                if (mHighWatermark >= cMaxTriangles)
                    return nullptr; // Buffer full
                t = reinterpret_cast<Triangle *>(&mTriangles[mHighWatermark].mTriangle);
                ++mHighWatermark;
            }
 
            // Call constructor
            new (t) Triangle(inIdx0, inIdx1, inIdx2, inPositions);
 
            return t;
        }
 
        void            FreeTriangle(Triangle *inT)
        {
            // Destruct triangle
            inT->~Triangle();
#ifdef _DEBUG
            memset(inT, 0xcd, sizeof(Triangle));
#endif
 
            // Add triangle to the free list
            Block *tu = reinterpret_cast<Block *>(inT);
            tu->mNextFree = mNextFree;
            mNextFree = tu;
        }
    };
 
    // Typedefs
    using PointsBase = StaticArray<Vec3, cMaxPoints>;
    using Triangles = StaticArray<Triangle *, cMaxTriangles>;
 
    class Points : public PointsBase
    {
    public:
        size_type &     GetSizeRef()
        {
            return mSize;
        }
    };
 
    class TriangleQueue : public Triangles
    {
    public:
        static bool     sTriangleSorter(const Triangle *inT1, const Triangle *inT2)
        {
            return inT1->mClosestLenSq > inT2->mClosestLenSq;
        }
 
        void            push_back(Triangle *inT)
        {
            // Add to base
            Triangles::push_back(inT);
 
            // Mark in queue
            inT->mInQueue = true;
 
            // Resort heap
            std::push_heap(begin(), end(), sTriangleSorter);
        }
 
        Triangle *      PeekClosest()
        {
            return front();
        }
 
        Triangle *      PopClosest()
        {
            // Move closest to end
            std::pop_heap(begin(), end(), sTriangleSorter);
 
            // Remove last triangle
            Triangle *t = back();
            pop_back();
            return t;
        }
    };
 
    explicit            EPAConvexHullBuilder(const Points &inPositions) :
        mPositions(inPositions)
    {
#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
        mIteration = 0;
        mOffset = RVec3::sZero();
#endif
    }
 
    void                Initialize(int inIdx1, int inIdx2, int inIdx3)
    {
        // Release triangles
        mFactory.Clear();
 
        // Create triangles (back to back)
        Triangle *t1 = CreateTriangle(inIdx1, inIdx2, inIdx3);
        Triangle *t2 = CreateTriangle(inIdx1, inIdx3, inIdx2);
 
        // Link triangles edges
        sLinkTriangle(t1, 0, t2, 2);
        sLinkTriangle(t1, 1, t2, 1);
        sLinkTriangle(t1, 2, t2, 0);
 
        // Always add both triangles to the priority queue
        mTriangleQueue.push_back(t1);
        mTriangleQueue.push_back(t2);
 
#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
        // Draw current state
        DrawState();
 
        // Increment iteration counter
        ++mIteration;
#endif
    }
 
    bool                HasNextTriangle() const
    {
        return !mTriangleQueue.empty();
    }
 
    Triangle *          PeekClosestTriangleInQueue()
    {
        return mTriangleQueue.PeekClosest();
    }
 
    Triangle *          PopClosestTriangleFromQueue()
    {
        return mTriangleQueue.PopClosest();
    }
 
    Triangle *          FindFacingTriangle(Vec3Arg inPosition, float &outBestDistSq)
    {
        Triangle *best = nullptr;
        float best_dist_sq = 0.0f;
 
        for (Triangle *t : mTriangleQueue)
            if (!t->mRemoved)
            {
                float dot = t->mNormal.Dot(inPosition - t->mCentroid);
                if (dot > 0.0f)
                {
                    float dist_sq = dot * dot / t->mNormal.LengthSq();
                    if (dist_sq > best_dist_sq)
                    {
                        best = t;
                        best_dist_sq = dist_sq;
                    }
                }
            }
 
        outBestDistSq = best_dist_sq;
        return best;
    }
 
    bool                AddPoint(Triangle *inFacingTriangle, int inIdx, float inClosestDistSq, NewTriangles &outTriangles)
    {
        // Get position
        Vec3 pos = mPositions[inIdx];
 
#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
        // Draw new support point
        DrawMarker(pos, Color::sYellow, 1.0f);
#endif
 
#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
        // Check if structure is intact
        ValidateTriangles();
#endif
 
        // Find edge of convex hull of triangles that are not facing the new vertex w
        Edges edges;
        if (!FindEdge(inFacingTriangle, pos, edges))
            return false;
 
        // Create new triangles
        int num_edges = edges.size();
        for (int i = 0; i < num_edges; ++i)
        {
            // Create new triangle
            Triangle *nt = CreateTriangle(edges[i].mStartIdx, edges[(i + 1) % num_edges].mStartIdx, inIdx);
            if (nt == nullptr)
                return false;
            outTriangles.push_back(nt);
 
            // Check if we need to put this triangle in the priority queue
            if ((nt->mClosestPointInterior && nt->mClosestLenSq < inClosestDistSq)  // For the main algorithm
                || nt->mClosestLenSq < 0.0f)                                        // For when the origin is not inside the hull yet
                mTriangleQueue.push_back(nt);
        }
 
        // Link edges
        for (int i = 0; i < num_edges; ++i)
        {
            sLinkTriangle(outTriangles[i], 0, edges[i].mNeighbourTriangle, edges[i].mNeighbourEdge);
            sLinkTriangle(outTriangles[i], 1, outTriangles[(i + 1) % num_edges], 2);
        }
 
#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
        // Check if structure is intact
        ValidateTriangles();
#endif
 
#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
        // Draw state of the hull
        DrawState();
 
        // Increment iteration counter
        ++mIteration;
#endif
 
        return true;
    }
 
    void                FreeTriangle(Triangle *inT)
    {
#ifdef JPH_ENABLE_ASSERTS
        // Make sure that this triangle is not connected
        JPH_ASSERT(inT->mRemoved);
        for (const Edge &e : inT->mEdge)
            JPH_ASSERT(e.mNeighbourTriangle == nullptr);
#endif
 
#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
        // Remove from list of all triangles
        Triangles::iterator i = std::find(mTriangles.begin(), mTriangles.end(), inT);
        JPH_ASSERT(i != mTriangles.end());
        mTriangles.erase(i);
#endif
 
        mFactory.FreeTriangle(inT);
    }
 
private:
    Triangle *          CreateTriangle(int inIdx1, int inIdx2, int inIdx3)
    {
        // Call provider to create triangle
        Triangle *t = mFactory.CreateTriangle(inIdx1, inIdx2, inIdx3, mPositions.data());
        if (t == nullptr)
            return nullptr;
 
#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
        // Remember iteration counter
        t->mIteration = mIteration;
#endif
 
#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
        // Add to list of triangles for debugging purposes
        mTriangles.push_back(t);
#endif
 
        return t;
    }
 
    static void         sLinkTriangle(Triangle *inT1, int inEdge1, Triangle *inT2, int inEdge2)
    {
        JPH_ASSERT(inEdge1 >= 0 && inEdge1 < 3);
        JPH_ASSERT(inEdge2 >= 0 && inEdge2 < 3);
        Edge &e1 = inT1->mEdge[inEdge1];
        Edge &e2 = inT2->mEdge[inEdge2];
 
        // Check not connected yet
        JPH_ASSERT(e1.mNeighbourTriangle == nullptr);
        JPH_ASSERT(e2.mNeighbourTriangle == nullptr);
 
        // Check vertices match
        JPH_ASSERT(e1.mStartIdx == inT2->GetNextEdge(inEdge2).mStartIdx);
        JPH_ASSERT(e2.mStartIdx == inT1->GetNextEdge(inEdge1).mStartIdx);
 
        // Link up
        e1.mNeighbourTriangle = inT2;
        e1.mNeighbourEdge = inEdge2;
        e2.mNeighbourTriangle = inT1;
        e2.mNeighbourEdge = inEdge1;
    }
 
    void                UnlinkTriangle(Triangle *inT)
    {
        // Unlink from neighbours
        for (int i = 0; i < 3; ++i)
        {
            Edge &edge = inT->mEdge[i];
            if (edge.mNeighbourTriangle != nullptr)
            {
                Edge &neighbour_edge = edge.mNeighbourTriangle->mEdge[edge.mNeighbourEdge];
 
                // Validate that neighbour points to us
                JPH_ASSERT(neighbour_edge.mNeighbourTriangle == inT);
                JPH_ASSERT(neighbour_edge.mNeighbourEdge == i);
 
                // Unlink
                neighbour_edge.mNeighbourTriangle = nullptr;
                edge.mNeighbourTriangle = nullptr;
            }
        }
 
        // If this triangle is not in the priority queue, we can delete it now
        if (!inT->mInQueue)
            FreeTriangle(inT);
    }
 
    bool                FindEdge(Triangle *inFacingTriangle, Vec3Arg inVertex, Edges &outEdges)
    {
        // Assert that we were given an empty array
        JPH_ASSERT(outEdges.empty());
 
        // Should start with a facing triangle
        JPH_ASSERT(inFacingTriangle->IsFacing(inVertex));
 
        // Flag as removed
        inFacingTriangle->mRemoved = true;
 
        // Instead of recursing, we build our own stack with the information we need
        struct StackEntry
        {
            Triangle *  mTriangle;
            int         mEdge;
            int         mIter;
        };
        StackEntry stack[cMaxEdgeLength];
        int cur_stack_pos = 0;
 
        // Start with the triangle / edge provided
        stack[0].mTriangle = inFacingTriangle;
        stack[0].mEdge = 0;
        stack[0].mIter = -1; // Start with edge 0 (is incremented below before use)
 
        // Next index that we expect to find, if we don't then there are 'islands'
        int next_expected_start_idx = -1;
 
        for (;;)
        {
            StackEntry &cur_entry = stack[cur_stack_pos];
 
            // Next iteration
            if (++cur_entry.mIter >= 3)
            {
                // This triangle needs to be removed, unlink it now
                UnlinkTriangle(cur_entry.mTriangle);
 
                // Pop from stack
                if (--cur_stack_pos < 0)
                    break;
            }
            else
            {
                // Visit neighbour
                Edge &e = cur_entry.mTriangle->mEdge[(cur_entry.mEdge + cur_entry.mIter) % 3];
                Triangle *n = e.mNeighbourTriangle;
                if (n != nullptr && !n->mRemoved)
                {
                    // Check if vertex is on the front side of this triangle
                    if (n->IsFacing(inVertex))
                    {
                        // Vertex on front, this triangle needs to be removed
                        n->mRemoved = true;
 
                        // Add element to the stack of elements to visit
                        cur_stack_pos++;
                        JPH_ASSERT(cur_stack_pos < cMaxEdgeLength);
                        StackEntry &new_entry = stack[cur_stack_pos];
                        new_entry.mTriangle = n;
                        new_entry.mEdge = e.mNeighbourEdge;
                        new_entry.mIter = 0; // Is incremented before use, we don't need to test this edge again since we came from it
                    }
                    else
                    {
                        // Detect if edge doesn't connect to previous edge, if this happens we have found and 'island' which means
                        // the newly added point is so close to the triangles of the hull that we classified some (nearly) coplanar
                        // triangles as before and some behind the point. At this point we just abort adding the point because
                        // we've reached numerical precision.
                        // Note that we do not need to test if the first and last edge connect, since when there are islands
                        // there should be at least 2 disconnects.
                        if (e.mStartIdx != next_expected_start_idx && next_expected_start_idx != -1)
                            return false;
 
                        // Next expected index is the start index of our neighbour's edge
                        next_expected_start_idx = n->mEdge[e.mNeighbourEdge].mStartIdx;
 
                        // Vertex behind, keep edge
                        outEdges.push_back(e);
                    }
                }
            }
        }
 
        // Assert that we have a fully connected loop
        JPH_ASSERT(outEdges.empty() || outEdges[0].mStartIdx == next_expected_start_idx);
 
#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
        // Draw edge of facing triangles
        for (int i = 0; i < (int)outEdges.size(); ++i)
        {
            RVec3 edge_start = cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]);
            DebugRenderer::sInstance->DrawArrow(edge_start, cDrawScale * (mOffset + mPositions[outEdges[(i + 1) % outEdges.size()].mStartIdx]), Color::sYellow, 0.01f);
            DebugRenderer::sInstance->DrawText3D(edge_start, ConvertToString(outEdges[i].mStartIdx), Color::sWhite);
        }
 
        // Draw the state with the facing triangles removed
        DrawState();
#endif
 
        // When we start with two triangles facing away from each other and adding a point that is on the plane,
        // sometimes we consider the point in front of both causing both triangles to be removed resulting in an empty edge list.
        // In this case we fail to add the point which will result in no collision reported (the shapes are contacting in 1 point so there's 0 penetration)
        return outEdges.size() >= 3;
    }
 
#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
    void                ValidateTriangle(const Triangle *inT) const
    {
        if (inT->mRemoved)
        {
            // Valdiate that removed triangles are not connected to anything
            for (const Edge &my_edge : inT->mEdge)
                JPH_ASSERT(my_edge.mNeighbourTriangle == nullptr);
        }
        else
        {
            for (int i = 0; i < 3; ++i)
            {
                const Edge &my_edge = inT->mEdge[i];
 
                // Assert that we have a neighbour
                const Triangle *nb = my_edge.mNeighbourTriangle;
                JPH_ASSERT(nb != nullptr);
 
                if (nb != nullptr)
                {
                    // Assert that our neighbours edge points to us
                    const Edge &nb_edge = nb->mEdge[my_edge.mNeighbourEdge];
                    JPH_ASSERT(nb_edge.mNeighbourTriangle == inT);
                    JPH_ASSERT(nb_edge.mNeighbourEdge == i);
 
                    // Assert that the next edge of the neighbour points to the same vertex as this edge's vertex
                    const Edge &nb_next_edge = nb->GetNextEdge(my_edge.mNeighbourEdge);
                    JPH_ASSERT(nb_next_edge.mStartIdx == my_edge.mStartIdx);
 
                    // Assert that my next edge points to the same vertex as my neighbours vertex
                    const Edge &my_next_edge = inT->GetNextEdge(i);
                    JPH_ASSERT(my_next_edge.mStartIdx == nb_edge.mStartIdx);
                }
            }
        }
    }
 
    void                ValidateTriangles() const
    {
        for (const Triangle *t : mTriangles)
            ValidateTriangle(t);
    }
#endif
 
#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
public:
    void                DrawState()
    {
        // Draw origin
        DebugRenderer::sInstance->DrawCoordinateSystem(RMat44::sTranslation(cDrawScale * mOffset), 1.0f);
 
        // Draw triangles
        for (const Triangle *t : mTriangles)
            if (!t->mRemoved)
            {
                // Calculate the triangle vertices
                RVec3 p1 = cDrawScale * (mOffset + mPositions[t->mEdge[0].mStartIdx]);
                RVec3 p2 = cDrawScale * (mOffset + mPositions[t->mEdge[1].mStartIdx]);
                RVec3 p3 = cDrawScale * (mOffset + mPositions[t->mEdge[2].mStartIdx]);
 
                // Draw triangle
                DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, Color::sGetDistinctColor(t->mIteration));
                DebugRenderer::sInstance->DrawWireTriangle(p1, p2, p3, Color::sGrey);
 
                // Draw normal
                RVec3 centroid = cDrawScale * (mOffset + t->mCentroid);
                float len = t->mNormal.Length();
                if (len > 0.0f)
                    DebugRenderer::sInstance->DrawArrow(centroid, centroid + t->mNormal / len, Color::sDarkGreen, 0.01f);
            }
 
        // Determine max position
        float min_x = FLT_MAX;
        float max_x = -FLT_MAX;
        for (Vec3 p : mPositions)
        {
            min_x = min(min_x, p.GetX());
            max_x = max(max_x, p.GetX());
        }
 
        // Offset to the right
        mOffset += Vec3(max_x - min_x + 0.5f, 0.0f, 0.0f);
    }
 
    void                DrawLabel(const string_view &inText)
    {
        DebugRenderer::sInstance->DrawText3D(cDrawScale * mOffset, inText, Color::sWhite, 0.1f * cDrawScale);
 
        mOffset += Vec3(5.0f, 0.0f, 0.0f);
    }
 
    void                DrawGeometry(const DebugRenderer::GeometryRef &inGeometry, ColorArg inColor)
    {
        RMat44 origin = RMat44::sScale(Vec3::sReplicate(cDrawScale)) * RMat44::sTranslation(mOffset);
        DebugRenderer::sInstance->DrawGeometry(origin, inGeometry->mBounds.Transformed(origin), inGeometry->mBounds.GetExtent().LengthSq(), inColor, inGeometry);
 
        mOffset += Vec3(inGeometry->mBounds.GetSize().GetX(), 0, 0);
    }
 
    void                DrawWireTriangle(const Triangle &inTriangle, ColorArg inColor)
    {
        RVec3 prev = cDrawScale * (mOffset + mPositions[inTriangle.mEdge[2].mStartIdx]);
        for (const Edge &edge : inTriangle.mEdge)
        {
            RVec3 cur = cDrawScale * (mOffset + mPositions[edge.mStartIdx]);
            DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f);
            prev = cur;
        }
    }
 
    void                DrawMarker(Vec3Arg inPosition, ColorArg inColor, float inSize)
    {
        DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + inPosition), inColor, inSize);
    }
 
    void                DrawArrow(Vec3Arg inFrom, Vec3Arg inTo, ColorArg inColor, float inArrowSize)
    {
        DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + inFrom), cDrawScale * (mOffset + inTo), inColor, inArrowSize);
    }
#endif
 
private:
    TriangleFactory     mFactory;                           
    const Points &      mPositions;                         
    TriangleQueue       mTriangleQueue;                     
 
#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
    Triangles           mTriangles;                         
#endif
 
#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
    int                 mIteration;                         
    RVec3               mOffset;                            
#endif
};
 
// The determinant that is calculated in the Triangle constructor is really sensitive
// to numerical round off, disable the fmadd instructions to maintain precision.
JPH_PRECISE_MATH_ON
 
EPAConvexHullBuilder::Triangle::Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions)
{
    // Fill in indexes
    JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2);
    mEdge[0].mStartIdx = inIdx0;
    mEdge[1].mStartIdx = inIdx1;
    mEdge[2].mStartIdx = inIdx2;
 
    // Clear links
    mEdge[0].mNeighbourTriangle = nullptr;
    mEdge[1].mNeighbourTriangle = nullptr;
    mEdge[2].mNeighbourTriangle = nullptr;
 
    // Get vertex positions
    Vec3 y0 = inPositions[inIdx0];
    Vec3 y1 = inPositions[inIdx1];
    Vec3 y2 = inPositions[inIdx2];
 
    // Calculate centroid
    mCentroid = (y0 + y1 + y2) / 3.0f;
 
    // Calculate edges
    Vec3 y10 = y1 - y0;
    Vec3 y20 = y2 - y0;
    Vec3 y21 = y2 - y1;
 
    // The most accurate normal is calculated by using the two shortest edges
    // See: https://box2d.org/posts/2014/01/troublesome-triangle/
    // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length).
    // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal.
    // We first check which of the edges is shorter.
    float y20_dot_y20 = y20.Dot(y20);
    float y21_dot_y21 = y21.Dot(y21);
    if (y20_dot_y20 < y21_dot_y21)
    {
        // We select the edges y10 and y20
        mNormal = y10.Cross(y20);
 
        // Check if triangle is degenerate
        float normal_len_sq = mNormal.LengthSq();
        if (normal_len_sq > cMinTriangleArea)
        {
            // Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal|
            // Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates and then calculating the closest
            // point based on those coordinates. Note that we preserve the sign of the distance to check on which side the origin is.
            float c_dot_n = mCentroid.Dot(mNormal);
            mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq;
 
            // Calculate closest point to origin using barycentric coordinates:
            //
            // v = y0 + l0 * (y1 - y0) + l1 * (y2 - y0)
            // v . (y1 - y0) = 0
            // v . (y2 - y0) = 0
            //
            // Written in matrix form:
            //
            // | y10.y10  y20.y10 | | l0 | = | -y0.y10 |
            // | y10.y20  y20.y20 | | l1 |   | -y0.y20 |
            //
            // (y10 = y1 - y0 etc.)
            //
            // Cramers rule to invert matrix:
            float y10_dot_y10 = y10.LengthSq();
            float y10_dot_y20 = y10.Dot(y20);
            float determinant = y10_dot_y10 * y20_dot_y20 - y10_dot_y20 * y10_dot_y20;
            if (determinant > 0.0f) // If determinant == 0 then the system is linearly dependent and the triangle is degenerate, since y10.10 * y20.y20 > y10.y20^2 it should also be > 0
            {
                float y0_dot_y10 = y0.Dot(y10);
                float y0_dot_y20 = y0.Dot(y20);
                float l0 = (y10_dot_y20 * y0_dot_y20 - y20_dot_y20 * y0_dot_y10) / determinant;
                float l1 = (y10_dot_y20 * y0_dot_y10 - y10_dot_y10 * y0_dot_y20) / determinant;
                mLambda[0] = l0;
                mLambda[1] = l1;
                mLambdaRelativeTo0 = true;
 
                // Check if closest point is interior to the triangle. For a convex hull which contains the origin each face must contain the origin, but because
                // our faces are triangles, we can have multiple coplanar triangles and only 1 will have the origin as an interior point. We want to use this triangle
                // to calculate the contact points because it gives the most accurate results, so we will only add these triangles to the priority queue.
                if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon)
                    mClosestPointInterior = true;
            }
        }
    }
    else
    {
        // We select the edges y10 and y21
        mNormal = y10.Cross(y21);
 
        // Check if triangle is degenerate
        float normal_len_sq = mNormal.LengthSq();
        if (normal_len_sq > cMinTriangleArea)
        {
            // Again calculate distance between triangle and origin
            float c_dot_n = mCentroid.Dot(mNormal);
            mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq;
 
            // Calculate closest point to origin using barycentric coordinates but this time using y1 as the reference vertex
            //
            // v = y1 + l0 * (y0 - y1) + l1 * (y2 - y1)
            // v . (y0 - y1) = 0
            // v . (y2 - y1) = 0
            //
            // Written in matrix form:
            //
            // | y10.y10  -y21.y10 | | l0 | = | y1.y10 |
            // | -y10.y21  y21.y21 | | l1 |   | -y1.y21 |
            //
            // Cramers rule to invert matrix:
            float y10_dot_y10 = y10.LengthSq();
            float y10_dot_y21 = y10.Dot(y21);
            float determinant = y10_dot_y10 * y21_dot_y21 - y10_dot_y21 * y10_dot_y21;
            if (determinant > 0.0f)
            {
                float y1_dot_y10 = y1.Dot(y10);
                float y1_dot_y21 = y1.Dot(y21);
                float l0 = (y21_dot_y21 * y1_dot_y10 - y10_dot_y21 * y1_dot_y21) / determinant;
                float l1 = (y10_dot_y21 * y1_dot_y10 - y10_dot_y10 * y1_dot_y21) / determinant;
                mLambda[0] = l0;
                mLambda[1] = l1;
                mLambdaRelativeTo0 = false;
 
                // Again check if the closest point is inside the triangle
                if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon)
                    mClosestPointInterior = true;
            }
        }
    }
}
 
JPH_PRECISE_MATH_OFF
 
JPH_NAMESPACE_END