ConvexHullBuilder.h

Go to the documentation of this file.

//
//      ██╗██████╗     ██╗     ██╗██████╗ ███████╗
//      ██║██╔══██╗    ██║     ██║██╔══██╗██╔════╝      ** JPLSpatial **
//      ██║██████╔╝    ██║     ██║██████╔╝███████╗
// ██   ██║██╔═══╝     ██║     ██║██╔══██╗╚════██║      https://github.com/Jaytheway/JPLSpatial
// ╚█████╔╝██║         ███████╗██║██████╔╝███████║
//  ╚════╝ ╚═╝         ╚══════╝╚═╝╚═════╝ ╚══════╝
//
//   Copyright 2024 Jaroslav Pevno, JPLSpatial is offered under the terms of the ISC license:
//
//   Permission to use, copy, modify, and/or distribute this software for any purpose with or
//   without fee is hereby granted, provided that the above copyright notice and this permission
//   notice appear in all copies. THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
//   WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
//   AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR
//   CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
//   WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
//   CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 
#pragma once
 
#include "JPLSpatial/ErrorReporting.h"
 
#include "JPLSpatial/Math/Math.h"
#include "JPLSpatial/Math/Vec3Traits.h"
#include "JPLSpatial/Math/ClosestPoint.h"
#include "JPLSpatial/Memory/Memory.h"
#include "JPLSpatial/Geometry/Triangulation.h"
 
#include <cstdlib>
#include <vector>
#include <memory>
#include <limits>
#include <span>
#include <unordered_set>
 
//#ifndef JPL_ENABLE_VALIDATION
//#define JPL_ENABLE_VALIDATION 0
//#endif
 
#if defined(JPL_ENABLE_VALIDATION)// && JPL_ENABLE_VALIDATION
#include <format>
#include <iostream>
#endif // JPL_ENABLE_VALIDATION
 
namespace JPL
{
 
    //==========================================================================
    template<CVec3 Vec3Type>
    class ConvexHullBuilder
    {
        using Vec3 = Vec3Type;
 
        template<class T>
        using Array = std::pmr::vector<T>;
 
        ConvexHullBuilder(const ConvexHullBuilder&) = delete;
        ConvexHullBuilder& operator=(const ConvexHullBuilder&) = delete;
    public:
        // Forward declare
        class Face;
 
        class Edge
        {
            Edge(const Edge&) = delete;
            Edge& operator =(const Edge&) = delete;
        public:
 
            Edge(Face* inFace, int inStartIdx);
 
            Edge* GetPreviousEdge();
 
            Face* mFace;                    
            Edge* mNextEdge = nullptr;      
            Edge* mNeighbourEdge = nullptr; 
            int mStartIdx;                  
        };
 
    public:
        using ConflictList = Array<int>;
 
        class Face final
        {
            Face(const Face&) = delete;
            Face& operator=(const Face&) = delete;
        public:
            Face() = default;
            ~Face();
 
            void Initialize(int inIdx0, int inIdx1, int inIdx2, std::span<const Vec3> inPositions);
 
            void CalculateNormalAndCentroid(std::span<const Vec3> inPositions);
 
            template<class Vec3i>
            bool Triangulate(std::span<const Vec3> inPositions, Array<Vec3i>& outTris) const;
 
            bool IsFacing(const Vec3& inPosition) const;
 
            Vec3 mNormal;                           
            Vec3 mCentroid;                         
            ConflictList mConflictList;             
            Edge* mFirstEdge = nullptr;             
            float mFurthestPointDistanceSq = 0.0f;  
            bool mRemoved = false;                  
#ifdef JPL_CONVEX_BUILDER_DEBUG
            int mIteration;                         
#endif
 
        private:
            Edge* AllocateEdge(int inStartIdx);
            void FreeEdge(Edge* edge);
        };
 
        // Typedefs
        using Positions = std::span<const Vec3>;
        using Faces = Array<Face*>;
 
        ConvexHullBuilder(Positions inPositions);
        ~ConvexHullBuilder();
 
        enum class EResult
        {
            Success,            
            MaxVerticesReached, 
            TooFewPoints,       
            TooFewFaces,        
            Degenerate,         
        };
 
        EResult Initialize(int inMaxVertices, float inTolerance, const char*& outError);
 
        int GetNumVerticesUsed() const;
 
        bool ContainsFace(const Array<int>& inIndices) const;
 
#if 0
        void DetermineMaxError(Face*& outFaceWithMaxError, float& outMaxError, int& outMaxErrorPositionIdx, float& outCoplanarDistance) const;
#endif
 
        const Faces& GetFaces() const { return mFaces; }
 
        template<class Vec3i>
        void GetTriangles(Array<Vec3i>& outTris);
 
    private:
        static constexpr float cMinTriangleAreaSq = 1.0e-12f;
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
        static constexpr Real cDrawScale = 10;
#endif
 
        class FullEdge
        {
        public:
            Edge* mNeighbourEdge;   
            int mStartIdx;          
            int mEndIdx;            
        };
 
        // Private typedefs
        using FullEdges = Array<FullEdge>;
 
        float DetermineCoplanarDistance() const;
 
        void GetFaceForPoint(const Vec3& inPoint, const Faces& inFaces, Face*& outFace, float& outDistSq) const;
 
        float GetDistanceToEdgeSq(const Vec3& inPoint, const Face* inFace) const;
 
        bool AssignPointToFace(int inPositionIdx, const Faces& inFaces, float inToleranceSq);
 
        void AddPoint(Face* inFacingFace, int inIdx, float inCoplanarToleranceSq, Faces& outNewFaces);
 
        void GarbageCollectFaces();
 
        Face* CreateFace();
 
        Face* CreateTriangle(int inIdx1, int inIdx2, int inIdx3);
 
        void FreeFace(Face* inFace);
 
        void FreeFaces();
        static void  sLinkFace(Edge* inEdge1, Edge* inEdge2);
 
        static void sUnlinkFace(Face* inFace);
 
        void FindEdge(Face* inFacingFace, const Vec3& inVertex, FullEdges& outEdges) const;
 
        void MergeFaces(Edge* inEdge);
 
        void MergeDegenerateFace(Face* inFace, Faces& ioAffectedFaces);
 
        void MergeCoplanarOrConcaveFaces(Face* inFace, float inCoplanarToleranceSq, Faces& ioAffectedFaces);
 
        static void sMarkAffected(Face* inFace, Faces& ioAffectedFaces);
 
        void RemoveInvalidEdges(Face* inFace, Faces& ioAffectedFaces);
 
        bool RemoveTwoEdgeFace(Face* inFace, Faces& ioAffectedFaces) const;
 
#ifdef JPL_ENABLE_VALIDATION
        void DumpFace(const Face* inFace) const;
 
        void ValidateFace(const Face* inFace) const;
 
        void ValidateFaces() const;
#endif
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
        void DrawState(bool inDrawConflictList = false) const;
 
        void DrawWireFace(const Face* inFace, ColorArg inColor) const;
 
        void DrawEdge(const Edge* inEdge, ColorArg inColor) const;
#endif
 
#ifdef JPL_CONVEX_BUILDER_DUMP_SHAPE
        void DumpShape() const;
#endif
 
        const Positions mPositions;                 
        Faces mFaces;                               
 
        struct Coplanar
        {
            int mPositionIdx;                       
            float mDistanceSq;                      
        };
        using CoplanarList = Array<Coplanar>;
 
        CoplanarList mCoplanarList;                 
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
        int mIteration;         
        mutable RVec3 mOffset;  
        Vec3 mDelta;            
#endif
    };
} // namespace JPL
 
//==============================================================================
//
//   Code beyond this point is implementation detail...
//
//==============================================================================
 
namespace JPL
{
    //==========================================================================
    template<CVec3 Vec3Type>
    inline ConvexHullBuilder<Vec3Type>::Edge::Edge(ConvexHullBuilder<Vec3Type>::Face* inFace, int inStartIdx)
        : mFace(inFace), mStartIdx(inStartIdx)
    {
    }
 
    template<CVec3 Vec3Type>
    inline ConvexHullBuilder<Vec3Type>::Edge* ConvexHullBuilder<Vec3Type>::Edge::GetPreviousEdge()
    {
        Edge* prev_edge = this;
        while (prev_edge->mNextEdge != this)
            prev_edge = prev_edge->mNextEdge;
        return prev_edge;
    }
 
    template<CVec3 Vec3Type>
    inline ConvexHullBuilder<Vec3Type>::Face::~Face()
    {
        // Free all edges
        if (Edge* e = mFirstEdge)
        {
            do
            {
                Edge* next = e->mNextEdge;
                FreeEdge(e);
                e = next;
            } while (e != mFirstEdge);
        }
    }
 
    //==========================================================================
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::Face::Initialize(int inIdx0, int inIdx1, int inIdx2, std::span<const Vec3> inPositions)
    {
        JPL_ASSERT(mFirstEdge == nullptr);
        JPL_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2);
 
        // Create 3 edges
        auto* e0 = AllocateEdge(inIdx0);
        auto* e1 = AllocateEdge(inIdx1);
        auto* e2 = AllocateEdge(inIdx2);
 
        // Link edges
        e0->mNextEdge = e1;
        e1->mNextEdge = e2;
        e2->mNextEdge = e0;
        mFirstEdge = e0;
 
        CalculateNormalAndCentroid(inPositions);
    }
 
    template<CVec3 Vec3Type>
    inline auto ConvexHullBuilder<Vec3Type>::Face::AllocateEdge(int inStartIdx) -> Edge*
    {
        return DefaultNew<Edge>(this, inStartIdx);
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::Face::FreeEdge(Edge* edge)
    {
        DefaultDelete(edge);
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::Face::CalculateNormalAndCentroid(std::span<const Vec3> inPositions)
    {
        // Get point that we use to construct a triangle fan
        Edge* e = mFirstEdge;
        Vec3 y0 = inPositions[e->mStartIdx];
 
        // Get the 2nd point
        e = e->mNextEdge;
        Vec3 y1 = inPositions[e->mStartIdx];
 
        // Start accumulating the centroid
        mCentroid = y0 + y1;
        int n = 2;
 
        // Start accumulating the normal
        mNormal = Vec3{ 0.0f, 0.0f, 0.0f };
 
        // Loop over remaining edges accumulating normals in a triangle fan fashion
        for (e = e->mNextEdge; e != mFirstEdge; e = e->mNextEdge)
        {
            // Get the 3rd point
            Vec3 y2 = inPositions[e->mStartIdx];
 
            // Calculate edges (counter clockwise)
            Vec3 e0 = y1 - y0;
            Vec3 e1 = y2 - y1;
            Vec3 e2 = y0 - y2;
 
            // The best 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 others 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.
            mNormal += LengthSquared(e1) < LengthSquared(e2) ? CrossProduct(e0, e1) : CrossProduct(e2, e0);
 
            // Accumulate centroid
            mCentroid += y2;
            n++;
 
            // Update y1 for next triangle
            y1 = y2;
        }
 
        // Finalize centroid
        mCentroid /= float(n);
    }
 
    template<CVec3 Vec3Type>
    template<class Vec3i>
    inline bool ConvexHullBuilder<Vec3Type>::Face::Triangulate(std::span<const Vec3> inPositions, Array<Vec3i>& outTris) const
    {
        using IndexType = std::remove_cvref_t<decltype(std::declval<Vec3i>()[0])>;
 
        Array<int> poly;
        poly.reserve(32);
 
        // Collect boundary indices
        poly.push_back(mFirstEdge->mStartIdx);
        for (Edge* e = mFirstEdge->mNextEdge; e != mFirstEdge; e = e->mNextEdge)
            poly.push_back(e->mStartIdx);
 
        if (poly.size() < 3)
            return false; // degenerate
 
        if (poly.size() == 3)
        {
            // Single triangle face
            outTris.push_back(Vec3i{
                static_cast<IndexType>(poly[0]),
                static_cast<IndexType>(poly[1]),
                static_cast<IndexType>(poly[2])
                              });
        }
        else
        {
            // Polygon face, needs triangulation
            Triangulation::TriangulatePoly(inPositions, poly, outTris);
        }
 
        return true;
    }
 
    template<CVec3 Vec3Type>
    inline bool ConvexHullBuilder<Vec3Type>::Face::IsFacing(const Vec3& inPosition) const
    {
        JPL_ASSERT(!mRemoved);
        return DotProduct(mNormal, (inPosition - mCentroid)) > 0.0f;
    }
 
    //==========================================================================
    template<CVec3 Vec3Type>
    inline ConvexHullBuilder<Vec3Type>::ConvexHullBuilder(Positions inPositions)
        : mPositions(inPositions)
        , mFaces(GetDefaultMemoryResource())
        , mCoplanarList(GetDefaultMemoryResource())
    {
#ifdef JPL_CONVEX_BUILDER_DEBUG
        mIteration = 0;
 
        // Center the drawing of the first hull around the origin and calculate the delta offset between states
        mOffset = RVec3::sZero();
        if (mPositions.empty())
        {
            // No hull will be generated
            mDelta = Vec3::sZero();
        }
        else
        {
            Vec3 maxv = Vec3::sReplicate(-std::numeric_limits<float>::max()), minv = Vec3::sReplicate(std::numeric_limits<float>::max());
            for (Vec3 v : mPositions)
            {
                minv = Vec3::sMin(minv, v);
                maxv = Vec3::sMax(maxv, v);
                mOffset -= v;
            }
            mOffset /= Real(mPositions.size());
            mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0);
            mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there
        }
#endif
    }
 
    template<CVec3 Vec3Type>
    inline ConvexHullBuilder<Vec3Type>::~ConvexHullBuilder()
    {
        FreeFaces();
    }
 
    template<CVec3 Vec3Type>
    inline ConvexHullBuilder<Vec3Type>::EResult ConvexHullBuilder<Vec3Type>::Initialize(int inMaxVertices, float inTolerance, const char*& outError)
    {
        // Free the faces possibly left over from an earlier hull
        FreeFaces();
 
        // Test that we have at least 3 points
        if (mPositions.size() < 3)
        {
            outError = "Need at least 3 points to make a hull";
            return EResult::TooFewPoints;
        }
 
        // Determine a suitable tolerance for detecting that points are coplanar
        float coplanar_tolerance_sq = DetermineCoplanarDistance();
        coplanar_tolerance_sq *= coplanar_tolerance_sq;
 
        // Increase desired tolerance if accuracy doesn't allow it
        const auto sqrTolerance = inTolerance * inTolerance;
        float tolerance_sq = std::max(coplanar_tolerance_sq, sqrTolerance);
 
        // Find point furthest from the origin
        int idx1 = -1;
        float max_dist_sq = -1.0f;
        for (int i = 0; i < (int)mPositions.size(); ++i)
        {
            float dist_sq = LengthSquared(mPositions[i]);
            if (dist_sq > max_dist_sq)
            {
                max_dist_sq = dist_sq;
                idx1 = i;
            }
        }
        JPL_ASSERT(idx1 >= 0);
 
        // Find point that is furthest away from this point
        int idx2 = -1;
        max_dist_sq = -1.0f;
        for (int i = 0; i < (int)mPositions.size(); ++i)
        {
            if (i != idx1)
            {
                float dist_sq = LengthSquared(mPositions[i] - mPositions[idx1]);
                if (dist_sq > max_dist_sq)
                {
                    max_dist_sq = dist_sq;
                    idx2 = i;
                }
            }
        }
        JPL_ASSERT(idx2 >= 0);
 
        // Find point that forms the biggest triangle
        // TODO: we may not need this, since we actually want small triangles, just not narrow
        int idx3 = -1;
        float best_triangle_area_sq = -1.0f;
        for (int i = 0; i < (int)mPositions.size(); ++i)
        {
            if (i != idx1 && i != idx2)
            {
                float triangle_area_sq = LengthSquared(CrossProduct(mPositions[idx1] - mPositions[i], mPositions[idx2] - mPositions[i]));
                if (triangle_area_sq > best_triangle_area_sq)
                {
                    best_triangle_area_sq = triangle_area_sq;
                    idx3 = i;
                }
            }
        }
 
        JPL_ASSERT(idx3 >= 0);
        if (best_triangle_area_sq < cMinTriangleAreaSq)
        {
            outError = "Could not find a suitable initial triangle because its area was too small";
            return EResult::Degenerate;
        }
 
        // Check if we have only 3 vertices
        if (mPositions.size() == 3)
        {
            // Create two triangles (back to back)
            // TODO: we may only want a single triangle, instead of two faces of same triangle
            Face* t1 = CreateTriangle(idx1, idx2, idx3);
            Face* t2 = CreateTriangle(idx1, idx3, idx2);
 
            // Link faces edges
            // TODO: we may not need to link faces
            sLinkFace(t1->mFirstEdge, t2->mFirstEdge->mNextEdge->mNextEdge);
            sLinkFace(t1->mFirstEdge->mNextEdge, t2->mFirstEdge->mNextEdge);
            sLinkFace(t1->mFirstEdge->mNextEdge->mNextEdge, t2->mFirstEdge);
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
            // Draw current state
            DrawState();
#endif
 
            return EResult::Success;
        }
 
        // Find point that forms the biggest tetrahedron
        Vec3 initial_plane_normal = Normalized(CrossProduct(mPositions[idx2] - mPositions[idx1], mPositions[idx3] - mPositions[idx1]));
        Vec3 initial_plane_centroid = (mPositions[idx1] + mPositions[idx2] + mPositions[idx3]) / 3.0f;
        int idx4 = -1;
        float max_dist = 0.0f;
        for (int i = 0; i < (int)mPositions.size(); ++i)
        {
            if (i != idx1 && i != idx2 && i != idx3)
            {
                float dist = DotProduct(mPositions[i] - initial_plane_centroid, initial_plane_normal);
                if (std::abs(dist) > std::abs(max_dist))
                {
                    max_dist = dist;
                    idx4 = i;
                }
            }
        }
 
        // Check if the hull is coplanar
        if ((max_dist * max_dist) <= 25.0f * coplanar_tolerance_sq)
        {
            // TODO: (may be still useful if we reuse this hull builder for other stuff
#if 1
            JPL_ASSERT(false);
            return EResult::Degenerate;
#else
                // First project all points in 2D space
            Vec3 base1 = GetNormalizedPerpendicular(initial_plane_normal);
            Vec3 base2 = CrossProduct(initial_plane_normal, base1);
            Array<Vec3> positions_2d;
            positions_2d.reserve(mPositions.size());
            for (Vec3 v : mPositions)
                positions_2d.emplace_back(DotProduct(base1, v), DotProduct(base2, v), 0.0f);
 
            // Build hull
            Array<int> edges_2d;
            ConvexHullBuilder2D builder_2d(positions_2d);
            ConvexHullBuilder2D::EResult result = builder_2d.Initialize(idx1, idx2, idx3, inMaxVertices, inTolerance, edges_2d);
 
            // Create faces (back to back)
            Face* f1 = CreateFace();
            Face* f2 = CreateFace();
 
            // Create edges for face 1
            Array<Edge*> edges_f1;
            edges_f1.reserve(edges_2d.size());
            for (int start_idx : edges_2d)
            {
                Edge* edge = new Edge(f1, start_idx);
                if (edges_f1.empty())
                    f1->mFirstEdge = edge;
                else
                    edges_f1.back()->mNextEdge = edge;
                edges_f1.push_back(edge);
            }
            edges_f1.back()->mNextEdge = f1->mFirstEdge;
 
            // Create edges for face 2
            Array<Edge*> edges_f2;
            edges_f2.reserve(edges_2d.size());
            for (int i = (int)edges_2d.size() - 1; i >= 0; --i)
            {
                Edge* edge = new Edge(f2, edges_2d[i]);
                if (edges_f2.empty())
                    f2->mFirstEdge = edge;
                else
                    edges_f2.back()->mNextEdge = edge;
                edges_f2.push_back(edge);
            }
            edges_f2.back()->mNextEdge = f2->mFirstEdge;
 
            // Link edges
            for (size_t i = 0; i < edges_2d.size(); ++i)
                sLinkFace(edges_f1[i], edges_f2[(2 * edges_2d.size() - 2 - i) % edges_2d.size()]);
 
            // Calculate the plane for both faces
            f1->CalculateNormalAndCentroid(mPositions.data());
            f2->mNormal = -f1->mNormal;
            f2->mCentroid = f1->mCentroid;
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
            // Draw current state
            DrawState();
#endif
 
            return result == ConvexHullBuilder2D::EResult::MaxVerticesReached ? EResult::MaxVerticesReached : EResult::Success;
#endif
        }
 
        // Ensure the planes are facing outwards
        if (max_dist < 0.0f)
            std::swap(idx2, idx3);
 
        // Create tetrahedron
        Face* t1 = CreateTriangle(idx1, idx2, idx4);
        Face* t2 = CreateTriangle(idx2, idx3, idx4);
        Face* t3 = CreateTriangle(idx3, idx1, idx4);
        Face* t4 = CreateTriangle(idx1, idx3, idx2);
 
        // Link face edges
        sLinkFace(t1->mFirstEdge, t4->mFirstEdge->mNextEdge->mNextEdge);
        sLinkFace(t1->mFirstEdge->mNextEdge, t2->mFirstEdge->mNextEdge->mNextEdge);
        sLinkFace(t1->mFirstEdge->mNextEdge->mNextEdge, t3->mFirstEdge->mNextEdge);
        sLinkFace(t2->mFirstEdge, t4->mFirstEdge->mNextEdge);
        sLinkFace(t2->mFirstEdge->mNextEdge, t3->mFirstEdge->mNextEdge->mNextEdge);
        sLinkFace(t3->mFirstEdge, t4->mFirstEdge);
 
        // Build the initial conflict lists
        Faces faces({ t1, t2, t3, t4 }, GetDefaultMemoryResource());
        for (int idx = 0; idx < (int)mPositions.size(); ++idx)
            if (idx != idx1 && idx != idx2 && idx != idx3 && idx != idx4)
                AssignPointToFace(idx, faces, tolerance_sq);
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
        // Draw current state including conflict list
        DrawState(true);
 
        // Increment iteration counter
        ++mIteration;
#endif
 
        // Overestimate of the actual amount of vertices we use, for limiting the amount of vertices in the hull
        int num_vertices_used = 4;
 
        // Loop through the remainder of the points and add them
        for (;;)
        {
            // Find the face with the furthest point on it
            Face* face_with_furthest_point = nullptr;
            float furthest_dist_sq = 0.0f;
            for (Face* f : mFaces)
            {
                if (f->mFurthestPointDistanceSq > furthest_dist_sq)
                {
                    furthest_dist_sq = f->mFurthestPointDistanceSq;
                    face_with_furthest_point = f;
                }
            }
 
            int furthest_point_idx;
            if (face_with_furthest_point != nullptr)
            {
                // Take the furthest point
                furthest_point_idx = face_with_furthest_point->mConflictList.back();
                face_with_furthest_point->mConflictList.pop_back();
            }
            else if (!mCoplanarList.empty())
            {
                // Try to assign points to faces (this also recalculates the distance to the hull for the coplanar vertices)
                CoplanarList coplanar(GetDefaultMemoryResource());
                mCoplanarList.swap(coplanar);
                bool added = false;
                for (const Coplanar& c : coplanar)
                    added |= AssignPointToFace(c.mPositionIdx, mFaces, tolerance_sq);
 
                // If we were able to assign a point, loop again to pick it up
                if (added)
                    continue;
 
                // If the coplanar list is empty, there are no points left and we're done
                if (mCoplanarList.empty())
                    break;
 
                do
                {
                    // Find the vertex that is furthest from the hull
                    typename CoplanarList::size_type best_idx = 0;
                    float best_dist_sq = mCoplanarList.front().mDistanceSq;
                    for (typename CoplanarList::size_type idx = 1; idx < mCoplanarList.size(); ++idx)
                    {
                        const Coplanar& c = mCoplanarList[idx];
                        if (c.mDistanceSq > best_dist_sq)
                        {
                            best_idx = idx;
                            best_dist_sq = c.mDistanceSq;
                        }
                    }
 
                    // Swap it to the end
                    std::swap(mCoplanarList[best_idx], mCoplanarList.back());
 
                    // Remove it
                    furthest_point_idx = mCoplanarList.back().mPositionIdx;
                    mCoplanarList.pop_back();
 
                    // Find the face for which the point is furthest away
                    GetFaceForPoint(mPositions[furthest_point_idx], mFaces, face_with_furthest_point, best_dist_sq);
                } while (!mCoplanarList.empty() && face_with_furthest_point == nullptr);
 
                if (face_with_furthest_point == nullptr)
                    break;
            }
            else
            {
                // If there are no more vertices, we're done
                break;
            }
 
            // Check if we have a limit on the max vertices that we should produce
            if (num_vertices_used >= inMaxVertices)
            {
                // Count the actual amount of used vertices (we did not take the removal of any vertices into account)
                num_vertices_used = GetNumVerticesUsed();
 
                // Check if there are too many
                if (num_vertices_used >= inMaxVertices)
                    return EResult::MaxVerticesReached;
            }
 
            // We're about to add another vertex
            ++num_vertices_used;
 
            // Add the point to the hull
            Faces new_faces(GetDefaultMemoryResource());
            AddPoint(face_with_furthest_point, furthest_point_idx, coplanar_tolerance_sq, new_faces);
 
            // Redistribute points on conflict lists belonging to removed faces
            for (const Face* face : mFaces)
                if (face->mRemoved)
                    for (int idx : face->mConflictList)
                        AssignPointToFace(idx, new_faces, tolerance_sq);
 
            // Permanently delete faces that we removed in AddPoint()
            GarbageCollectFaces();
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
            // Draw state at the end of this step including conflict list
            DrawState(true);
 
            // Increment iteration counter
            ++mIteration;
#endif
        }
 
        // Check if we are left with a hull. It is possible that hull building fails if the points are nearly coplanar.
        if (mFaces.size() < 2)
        {
            outError = "Too few faces in hull";
            return EResult::TooFewFaces;
        }
 
        return EResult::Success;
    }
 
    template<CVec3 Vec3Type>
    inline int ConvexHullBuilder<Vec3Type>::GetNumVerticesUsed() const
    {
        std::unordered_set<int> used_verts;
        for (const Face* f : mFaces)
        {
            const Edge* e = f->mFirstEdge;
            do
            {
                used_verts.insert(e->mStartIdx);
                e = e->mNextEdge;
            } while (e != f->mFirstEdge);
        }
        return (int)used_verts.size();
    }
 
    template<CVec3 Vec3Type>
    inline bool ConvexHullBuilder<Vec3Type>::ContainsFace(const Array<int>& inIndices) const
    {
        for (Face* f : mFaces)
        {
            Edge* e = f->mFirstEdge;
            Array<int>::const_iterator index = std::find(inIndices.begin(), inIndices.end(), e->mStartIdx);
            if (index != inIndices.end())
            {
                size_t matches = 0;
 
                do
                {
                    // Check if index matches
                    if (*index != e->mStartIdx)
                        break;
 
                    // Increment number of matches
                    matches++;
 
                    // Next index in list of inIndices
                    index++;
                    if (index == inIndices.end())
                        index = inIndices.begin();
 
                    // Next edge
                    e = e->mNextEdge;
                } while (e != f->mFirstEdge);
 
                if (matches == inIndices.size())
                    return true;
            }
        }
 
        return false;
    }
 
    template<CVec3 Vec3Type>
    template<class Vec3i>
    inline void ConvexHullBuilder<Vec3Type>::GetTriangles(Array<Vec3i>& outTris)
    {
        outTris.clear();
        outTris.reserve(mFaces.size());
 
        for (const Face* f : mFaces)
            f->Triangulate(mPositions, outTris);
    }
 
    template<CVec3 Vec3Type>
    inline float ConvexHullBuilder<Vec3Type>::DetermineCoplanarDistance() const
    {
        auto vec3max = [](const Vec3& a, const Vec3& b)
        {
            return Vec3{ std::max(GetX(a), GetX(b)), std::max(GetY(a), GetY(b)), std::max(GetZ(a), GetZ(b)) };
        };
        auto vec3abs = [](const Vec3& v)
        {
            return Vec3{ std::abs(GetX(v)), std::abs(GetY(v)), std::abs(GetZ(v)) };
        };
 
        // Formula as per: Implementing Quickhull - Dirk Gregorius.
        Vec3 vmax{ 0.0f, 0.0f, 0.0f };
        for (Vec3 v : mPositions)
            vmax = vec3max(vmax, vec3abs(v));
        return 3.0f * std::numeric_limits<float>::epsilon() * (GetX(vmax) + GetY(vmax) + GetZ(vmax));
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::GetFaceForPoint(const Vec3& inPoint, const Faces& inFaces, Face*& outFace, float& outDistSq) const
    {
        outFace = nullptr;
        outDistSq = 0.0f;
 
        for (Face* f : inFaces)
        {
            if (!f->mRemoved)
            {
                // Determine distance to face
                const float dot = DotProduct(f->mNormal, inPoint - f->mCentroid);
                if (dot > 0.0f)
                {
                    const float dist_sq = dot * dot / LengthSquared(f->mNormal);
                    if (dist_sq > outDistSq)
                    {
                        outFace = f;
                        outDistSq = dist_sq;
                    }
                }
            }
        }
    }
 
    template<CVec3 Vec3Type>
    inline float ConvexHullBuilder<Vec3Type>::GetDistanceToEdgeSq(const Vec3& inPoint, const Face* inFace) const
    {
        bool all_inside = true;
        float edge_dist_sq = std::numeric_limits<float>::max();
 
        // Test if it is inside the edges of the polygon
        Edge* edge = inFace->mFirstEdge;
        Vec3 p1 = mPositions[edge->GetPreviousEdge()->mStartIdx];
        do
        {
            Vec3 p2 = mPositions[edge->mStartIdx];
            if (DotProduct(CrossProduct(p2 - p1, inPoint - p1), inFace->mNormal) < 0.0f)
            {
                // It is outside
                all_inside = false;
 
                // Measure distance to this edge
                uint32 s;
                edge_dist_sq = std::min(edge_dist_sq, LengthSquared(GetClosestPointOnLine(p1 - inPoint, p2 - inPoint, s)));
            }
            p1 = p2;
            edge = edge->mNextEdge;
        } while (edge != inFace->mFirstEdge);
 
        return all_inside ? 0.0f : edge_dist_sq;
    }
 
    template<CVec3 Vec3Type>
    inline bool ConvexHullBuilder<Vec3Type>::AssignPointToFace(int inPositionIdx, const Faces& inFaces, float inToleranceSq)
    {
        Vec3 point = mPositions[inPositionIdx];
 
        // Find the face for which the point is furthest away
        Face* best_face;
        float best_dist_sq;
        GetFaceForPoint(point, inFaces, best_face, best_dist_sq);
 
        if (best_face != nullptr)
        {
            // Check if this point is within the tolerance margin to the plane
            if (best_dist_sq <= inToleranceSq)
            {
                // Check distance to edges
                float dist_to_edge_sq = GetDistanceToEdgeSq(point, best_face);
                if (dist_to_edge_sq > inToleranceSq)
                {
                    // Point is outside of the face and too far away to discard
                    mCoplanarList.push_back({ inPositionIdx, dist_to_edge_sq });
                }
            }
            else
            {
                // This point is in front of the face, add it to the conflict list
                if (best_dist_sq > best_face->mFurthestPointDistanceSq)
                {
                    // This point is further away than any others, update the distance and add point as last point
                    best_face->mFurthestPointDistanceSq = best_dist_sq;
                    best_face->mConflictList.push_back(inPositionIdx);
                }
                else
                {
                    // Not the furthest point, add it as the before last point
                    best_face->mConflictList.insert(best_face->mConflictList.begin() + best_face->mConflictList.size() - 1, inPositionIdx);
                }
 
                return true;
            }
        }
 
        return false;
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::AddPoint(Face* inFacingFace, int inIdx, float inCoplanarToleranceSq, Faces& outNewFaces)
    {
        // Get position
        Vec3 pos = mPositions[inIdx];
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
        // Draw point to be added
        DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + pos), Color::sYellow, 0.1f);
        DebugRenderer::sInstance->DrawText3D(cDrawScale * (mOffset + pos), ConvertToString(inIdx), Color::sWhite);
#endif
 
#ifdef JPL_ENABLE_VALIDATION
        // Check if structure is intact
        ValidateFaces();
#endif
 
        // Find edge of convex hull of faces that are not facing the new vertex
        FullEdges edges(GetDefaultMemoryResource());
        FindEdge(inFacingFace, pos, edges);
        JPL_ASSERT(edges.size() >= 3);
 
        // Create new faces
        outNewFaces.reserve(edges.size());
        for (const FullEdge& e : edges)
        {
            JPL_ASSERT(e.mStartIdx != e.mEndIdx);
            Face* f = CreateTriangle(e.mStartIdx, e.mEndIdx, inIdx);
            outNewFaces.push_back(f);
        }
 
        // Link edges
        for (typename Faces::size_type i = 0; i < outNewFaces.size(); ++i)
        {
            sLinkFace(outNewFaces[i]->mFirstEdge, edges[i].mNeighbourEdge);
            sLinkFace(outNewFaces[i]->mFirstEdge->mNextEdge, outNewFaces[(i + 1) % outNewFaces.size()]->mFirstEdge->mNextEdge->mNextEdge);
        }
 
        // Loop on faces that were modified until nothing needs to be checked anymore
        Faces affected_faces = outNewFaces;
        while (!affected_faces.empty())
        {
            // Take the next face
            Face* face = affected_faces.back();
            affected_faces.pop_back();
 
            if (!face->mRemoved)
            {
                // Merge with neighbour if this is a degenerate face
                MergeDegenerateFace(face, affected_faces);
 
                // Merge with coplanar neighbours (or when the neighbour forms a concave edge)
#if 0
                if (!face->mRemoved)
                    MergeCoplanarOrConcaveFaces(face, inCoplanarToleranceSq, affected_faces);
#endif
            }
        }
 
#ifdef JPL_ENABLE_VALIDATION
        // Check if structure is intact
        ValidateFaces();
#endif
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::GarbageCollectFaces()
    {
        for (int i = (int)mFaces.size() - 1; i >= 0; --i)
        {
            Face* f = mFaces[i];
            if (f->mRemoved)
            {
                FreeFace(f);
                mFaces.erase(mFaces.begin() + i);
            }
        }
    }
 
    template<CVec3 Vec3Type>
    inline ConvexHullBuilder<Vec3Type>::Face* ConvexHullBuilder<Vec3Type>::CreateFace()
    {
        // Call provider to create face
        Face* f = DefaultNew<Face>();
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
        // Remember iteration counter
        f->mIteration = mIteration;
#endif
 
        // Add to list
        mFaces.push_back(f);
        return f;
    }
 
    template<CVec3 Vec3Type>
    inline ConvexHullBuilder<Vec3Type>::Face* ConvexHullBuilder<Vec3Type>::CreateTriangle(int inIdx1, int inIdx2, int inIdx3)
    {
        Face* f = CreateFace();
        f->Initialize(inIdx1, inIdx2, inIdx3, mPositions);
        return f;
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::FreeFace(Face* inFace)
    {
        JPL_ASSERT(inFace->mRemoved);
 
#ifdef JPL_ENABLE_VALIDATION
        // Make sure that this face is not connected
        if (Edge* e = inFace->mFirstEdge)
        {
            do
            {
                JPL_ASSERT(e->mNeighbourEdge == nullptr);
                e = e->mNextEdge;
            } while (e != inFace->mFirstEdge);
        }
#endif
 
        // Free the face
        DefaultDelete(inFace);
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::FreeFaces()
    {
        for (Face* f : mFaces)
        {
            DefaultDelete(f);
        }
        mFaces.clear();
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::sLinkFace(Edge* inEdge1, Edge* inEdge2)
    {
        // Check not connected yet
        JPL_ASSERT(inEdge1->mNeighbourEdge == nullptr);
        JPL_ASSERT(inEdge2->mNeighbourEdge == nullptr);
        JPL_ASSERT(inEdge1->mFace != inEdge2->mFace);
 
        // Check vertices match
        JPL_ASSERT(inEdge1->mStartIdx == inEdge2->mNextEdge->mStartIdx);
        JPL_ASSERT(inEdge2->mStartIdx == inEdge1->mNextEdge->mStartIdx);
 
        // Link up
        inEdge1->mNeighbourEdge = inEdge2;
        inEdge2->mNeighbourEdge = inEdge1;
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::sUnlinkFace(Face* inFace)
    {
        // Unlink from neighbours
        Edge* e = inFace->mFirstEdge;
        do
        {
            if (e->mNeighbourEdge != nullptr)
            {
                // Validate that neighbour points to us
                JPL_ASSERT(e->mNeighbourEdge->mNeighbourEdge == e);
 
                // Unlink
                e->mNeighbourEdge->mNeighbourEdge = nullptr;
                e->mNeighbourEdge = nullptr;
            }
            e = e->mNextEdge;
        } while (e != inFace->mFirstEdge);
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::FindEdge(Face* inFacingFace, const Vec3& inVertex, FullEdges& outEdges) const
    {
        // Assert that we were given an empty array
        JPL_ASSERT(outEdges.empty());
 
        // Should start with a facing face
        JPL_ASSERT(inFacingFace->IsFacing(inVertex));
 
        // Flag as removed
        inFacingFace->mRemoved = true;
 
        // Instead of recursing, we build our own stack with the information we need
        struct StackEntry
        {
            Edge* mFirstEdge;
            Edge* mCurrentEdge;
        };
        constexpr int cMaxEdgeLength = 128;
        StackEntry stack[cMaxEdgeLength];
        int cur_stack_pos = 0;
 
        static_assert(alignof(Edge) >= 2, "Need lowest bit to indicate to tell if we completed the loop");
 
        // Start with the face / edge provided
        stack[0].mFirstEdge = inFacingFace->mFirstEdge;
        stack[0].mCurrentEdge = reinterpret_cast<Edge*>(reinterpret_cast<uintptr_t>(inFacingFace->mFirstEdge) | 1); // Set lowest bit of pointer to make it different from the first edge
 
        for (;;)
        {
            StackEntry& cur_entry = stack[cur_stack_pos];
 
            // Next edge
            Edge* raw_e = cur_entry.mCurrentEdge;
            Edge* e = reinterpret_cast<Edge*>(reinterpret_cast<uintptr_t>(raw_e) & ~uintptr_t(1)); // Remove the lowest bit which was used to indicate that this is the first edge we're testing
            cur_entry.mCurrentEdge = e->mNextEdge;
 
            // If we're back at the first edge we've completed the face and we're done
            if (raw_e == cur_entry.mFirstEdge)
            {
                // This face needs to be removed, unlink it now, caller will free
                sUnlinkFace(e->mFace);
 
                // Pop from stack
                if (--cur_stack_pos < 0)
                    break;
            }
            else
            {
                // Visit neighbour face
                Edge* ne = e->mNeighbourEdge;
                if (ne != nullptr)
                {
                    Face* n = ne->mFace;
                    if (!n->mRemoved)
                    {
                        // Check if vertex is on the front side of this face
                        if (n->IsFacing(inVertex))
                        {
                            // Vertex on front, this face needs to be removed
                            n->mRemoved = true;
 
                            // Add element to the stack of elements to visit
                            cur_stack_pos++;
                            JPL_ASSERT(cur_stack_pos < cMaxEdgeLength);
                            StackEntry& new_entry = stack[cur_stack_pos];
                            new_entry.mFirstEdge = ne;
                            new_entry.mCurrentEdge = ne->mNextEdge; // We don't need to test this edge again since we came from it
                        }
                        else
                        {
                            // Vertex behind, keep edge
                            FullEdge full;
                            full.mNeighbourEdge = ne;
                            full.mStartIdx = e->mStartIdx;
                            full.mEndIdx = ne->mStartIdx;
                            outEdges.push_back(full);
                        }
                    }
                }
            }
        }
 
        // Assert that we have a fully connected loop
#ifdef JPL_ENABLE_VALIDATION
        for (int i = 0; i < (int)outEdges.size(); ++i)
            JPL_ASSERT(outEdges[i].mEndIdx == outEdges[(i + 1) % outEdges.size()].mStartIdx);
#endif
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
        // Draw edge of facing faces
        for (int i = 0; i < (int)outEdges.size(); ++i)
            DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]), cDrawScale * (mOffset + mPositions[outEdges[i].mEndIdx]), Color::sWhite, 0.01f);
        DrawState();
#endif
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::MergeFaces(Edge* inEdge)
    {
        // Get the face
        Face* face = inEdge->mFace;
 
        // Find the previous and next edge
        Edge* next_edge = inEdge->mNextEdge;
        Edge* prev_edge = inEdge->GetPreviousEdge();
 
        // Get the other face
        Edge* other_edge = inEdge->mNeighbourEdge;
        Face* other_face = other_edge->mFace;
 
        // Check if attempting to merge with self
        JPL_ASSERT(face != other_face);
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
        DrawWireFace(face, Color::sGreen);
        DrawWireFace(other_face, Color::sRed);
        DrawState();
#endif
 
        // Loop over the edges of the other face and make them belong to inFace
        Edge* edge = other_edge->mNextEdge;
        prev_edge->mNextEdge = edge;
        for (;;)
        {
            edge->mFace = face;
            if (edge->mNextEdge == other_edge)
            {
                // Terminate when we are back at other_edge
                edge->mNextEdge = next_edge;
                break;
            }
            edge = edge->mNextEdge;
        }
 
        // If the first edge happens to be inEdge we need to fix it because this edge is no longer part of the face.
        // Note that we replace it with the first edge of the merged face so that if the MergeFace function is called
        // from a loop that loops around the face that it will still terminate after visiting all edges once.
        if (face->mFirstEdge == inEdge)
            face->mFirstEdge = prev_edge->mNextEdge;
 
        // Free the edges
        delete inEdge;
        delete other_edge;
 
        // Mark the other face as removed
        other_face->mFirstEdge = nullptr;
        other_face->mRemoved = true;
 
        // Recalculate plane
        face->CalculateNormalAndCentroid(mPositions);
 
        // Merge conflict lists
        if (face->mFurthestPointDistanceSq > other_face->mFurthestPointDistanceSq)
        {
            // This face has a point that's further away, make sure it remains the last one as we add the other points to this faces list
            face->mConflictList.insert(face->mConflictList.end() - 1, other_face->mConflictList.begin(), other_face->mConflictList.end());
        }
        else
        {
            // The other face has a point that's furthest away, add that list at the end.
            face->mConflictList.insert(face->mConflictList.end(), other_face->mConflictList.begin(), other_face->mConflictList.end());
            face->mFurthestPointDistanceSq = other_face->mFurthestPointDistanceSq;
        }
        other_face->mConflictList.clear();
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
        DrawWireFace(face, Color::sWhite);
        DrawState();
#endif
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::MergeDegenerateFace(Face* inFace, Faces& ioAffectedFaces)
    {
        // Check area of face
        if (LengthSquared(inFace->mNormal) < cMinTriangleAreaSq)
        {
            // Find longest edge, since this face is a sliver this should keep the face convex
            float max_length_sq = 0.0f;
            Edge* longest_edge = nullptr;
            Edge* e = inFace->mFirstEdge;
            Vec3 p1 = mPositions[e->mStartIdx];
            do
            {
                Edge* next = e->mNextEdge;
                Vec3 p2 = mPositions[next->mStartIdx];
                float length_sq = LengthSquared(p2 - p1);
                if (length_sq >= max_length_sq)
                {
                    max_length_sq = length_sq;
                    longest_edge = e;
                }
                p1 = p2;
                e = next;
            } while (e != inFace->mFirstEdge);
 
            // Merge with face on longest edge
            MergeFaces(longest_edge);
 
            // Remove any invalid edges
            RemoveInvalidEdges(inFace, ioAffectedFaces);
        }
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::MergeCoplanarOrConcaveFaces(Face* inFace, float inCoplanarToleranceSq, Faces& ioAffectedFaces)
    {
        bool merged = false;
 
        Edge* edge = inFace->mFirstEdge;
        do
        {
            // Store next edge since this edge can be removed
            Edge* next_edge = edge->mNextEdge;
 
            // Test if centroid of one face is above plane of the other face by inCoplanarToleranceSq.
            // If so we need to merge other face into inFace.
            const Face* other_face = edge->mNeighbourEdge->mFace;
            const Vec3 delta_centroid = other_face->mCentroid - inFace->mCentroid;
            const float dist_other_face_centroid = DotProduct(inFace->mNormal, delta_centroid);
            const float signed_dist_other_face_centroid_sq = std::abs(dist_other_face_centroid) * dist_other_face_centroid;
            const float dist_face_centroid = -DotProduct(other_face->mNormal, delta_centroid);
            const float signed_dist_face_centroid_sq = std::abs(dist_face_centroid) * dist_face_centroid;
            const float face_normal_len_sq = LengthSquared(inFace->mNormal);
            const float other_face_normal_len_sq = LengthSquared(other_face->mNormal);
            if ((signed_dist_other_face_centroid_sq > -inCoplanarToleranceSq * face_normal_len_sq
                 || signed_dist_face_centroid_sq > -inCoplanarToleranceSq * other_face_normal_len_sq)
                && DotProduct(inFace->mNormal, other_face->mNormal) > 0.0f) // Never merge faces that are back to back
            {
                MergeFaces(edge);
                merged = true;
            }
 
            edge = next_edge;
        } while (edge != inFace->mFirstEdge);
 
        if (merged)
            RemoveInvalidEdges(inFace, ioAffectedFaces);
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::sMarkAffected(Face* inFace, Faces& ioAffectedFaces)
    {
        if (std::find(ioAffectedFaces.begin(), ioAffectedFaces.end(), inFace) == ioAffectedFaces.end())
            ioAffectedFaces.push_back(inFace);
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::RemoveInvalidEdges(Face* inFace, Faces& ioAffectedFaces)
    {
        // This marks that the plane needs to be recalculated (we delay this until the end of the
        // function since we don't use the plane and we want to avoid calculating it multiple times)
        bool recalculate_plane = false;
 
        // We keep going through this loop until no more edges were removed
        bool removed;
        do
        {
            removed = false;
 
            // Loop over all edges in this face
            Edge* edge = inFace->mFirstEdge;
            Face* neighbour_face = edge->mNeighbourEdge->mFace;
            do
            {
                Edge* next_edge = edge->mNextEdge;
                Face* next_neighbour_face = next_edge->mNeighbourEdge->mFace;
 
                if (neighbour_face == inFace)
                {
                    // We only remove 1 edge at a time, check if this edge's next edge is our neighbour.
                    // If this check fails, we will continue to scan along the edge until we find an edge where this is the case.
                    if (edge->mNeighbourEdge == next_edge)
                    {
                        // This edge leads back to the starting point, this means the edge is interior and needs to be removed
#ifdef JPL_CONVEX_BUILDER_DEBUG
                        DrawWireFace(inFace, Color::sBlue);
                        DrawState();
#endif
 
                        // Remove edge
                        Edge* prev_edge = edge->GetPreviousEdge();
                        prev_edge->mNextEdge = next_edge->mNextEdge;
                        if (inFace->mFirstEdge == edge || inFace->mFirstEdge == next_edge)
                            inFace->mFirstEdge = prev_edge;
                        delete edge;
                        delete next_edge;
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
                        DrawWireFace(inFace, Color::sGreen);
                        DrawState();
#endif
 
                        // Check if inFace now has only 2 edges left
                        if (RemoveTwoEdgeFace(inFace, ioAffectedFaces))
                            return; // Bail if face no longer exists
 
                        // Restart the loop
                        recalculate_plane = true;
                        removed = true;
                        break;
                    }
                }
                else if (neighbour_face == next_neighbour_face)
                {
                    // There are two edges that connect to the same face, we will remove the second one
#ifdef JPL_CONVEX_BUILDER_DEBUG
                    DrawWireFace(inFace, Color::sYellow);
                    DrawWireFace(neighbour_face, Color::sRed);
                    DrawState();
#endif
 
                    // First merge the neighbours edges
                    Edge* neighbour_edge = next_edge->mNeighbourEdge;
                    Edge* next_neighbour_edge = neighbour_edge->mNextEdge;
                    if (neighbour_face->mFirstEdge == next_neighbour_edge)
                        neighbour_face->mFirstEdge = neighbour_edge;
                    neighbour_edge->mNextEdge = next_neighbour_edge->mNextEdge;
                    neighbour_edge->mNeighbourEdge = edge;
                    delete next_neighbour_edge;
 
                    // Then merge my own edges
                    if (inFace->mFirstEdge == next_edge)
                        inFace->mFirstEdge = edge;
                    edge->mNextEdge = next_edge->mNextEdge;
                    edge->mNeighbourEdge = neighbour_edge;
                    delete next_edge;
 
#ifdef JPL_CONVEX_BUILDER_DEBUG
                    DrawWireFace(inFace, Color::sYellow);
                    DrawWireFace(neighbour_face, Color::sGreen);
                    DrawState();
#endif
 
                    // Check if neighbour has only 2 edges left
                    if (!RemoveTwoEdgeFace(neighbour_face, ioAffectedFaces))
                    {
                        // No, we need to recalculate its plane
                        neighbour_face->CalculateNormalAndCentroid(mPositions);
 
                        // Mark neighbour face as affected
                        sMarkAffected(neighbour_face, ioAffectedFaces);
                    }
 
                    // Check if inFace now has only 2 edges left
                    if (RemoveTwoEdgeFace(inFace, ioAffectedFaces))
                        return; // Bail if face no longer exists
 
                    // Restart loop
                    recalculate_plane = true;
                    removed = true;
                    break;
                }
 
                // This edge is ok, go to the next edge
                edge = next_edge;
                neighbour_face = next_neighbour_face;
 
            } while (edge != inFace->mFirstEdge);
        } while (removed);
 
        // Recalculate plane?
        if (recalculate_plane)
            inFace->CalculateNormalAndCentroid(mPositions);
    }
 
    template<CVec3 Vec3Type>
    inline bool ConvexHullBuilder<Vec3Type>::RemoveTwoEdgeFace(Face* inFace, Faces& ioAffectedFaces) const
    {
        // Check if this face contains only 2 edges
        Edge* edge = inFace->mFirstEdge;
        Edge* next_edge = edge->mNextEdge;
        JPL_ASSERT(edge != next_edge); // 1 edge faces should not exist
        if (next_edge->mNextEdge == edge)
        {
#ifdef JPL_CONVEX_BUILDER_DEBUG
            DrawWireFace(inFace, Color::sRed);
            DrawState();
#endif
 
            // Schedule both neighbours for re-checking
            Edge* neighbour_edge = edge->mNeighbourEdge;
            Face* neighbour_face = neighbour_edge->mFace;
            Edge* next_neighbour_edge = next_edge->mNeighbourEdge;
            Face* next_neighbour_face = next_neighbour_edge->mFace;
            sMarkAffected(neighbour_face, ioAffectedFaces);
            sMarkAffected(next_neighbour_face, ioAffectedFaces);
 
            // Link my neighbours to each other
            neighbour_edge->mNeighbourEdge = next_neighbour_edge;
            next_neighbour_edge->mNeighbourEdge = neighbour_edge;
 
            // Unlink my edges
            edge->mNeighbourEdge = nullptr;
            next_edge->mNeighbourEdge = nullptr;
 
            // Mark this face as removed
            inFace->mRemoved = true;
 
            return true;
        }
 
        return false;
    }
 
#ifdef JPL_ENABLE_VALIDATION
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::DumpFace(const Face* inFace) const
    {
        std::cout << std::format("f:{}", std::uintptr_t(inFace)) << '\n';
 
        const Edge* e = inFace->mFirstEdge;
        do
        {
            std::cout << std::format("e:{} || i:{} e:{} f:{} ",
                                     std::uintptr_t(e),
                                     e->mStartIdx,
                                     std::uintptr_t(e->mNeighbourEdge),
                                     std::uintptr_t(e->mNeighbourEdge->mFace)) << '\n';
            e = e->mNextEdge;
        } while (e != inFace->mFirstEdge);
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::DumpFaces() const
    {
        std::cout << std::format("Dump Faces:") << '\n';
 
        for (const Face* f : mFaces)
            if (!f->mRemoved)
                DumpFace(f);
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::ValidateFace(const Face* inFace) const
    {
        if (inFace->mRemoved)
        {
            const Edge* e = inFace->mFirstEdge;
            if (e != nullptr)
                do
                {
                    JPL_ASSERT(e->mNeighbourEdge == nullptr);
                    e = e->mNextEdge;
                } while (e != inFace->mFirstEdge);
        }
        else
        {
            int edge_count = 0;
 
            const Edge* e = inFace->mFirstEdge;
            do
            {
                // Count edge
                ++edge_count;
 
                // Validate that adjacent faces are all different
                if (mFaces.size() > 2)
                    for (const Edge* other_edge = e->mNextEdge; other_edge != inFace->mFirstEdge; other_edge = other_edge->mNextEdge)
                        JPL_ASSERT(e->mNeighbourEdge->mFace != other_edge->mNeighbourEdge->mFace);
 
                // Assert that the face is correct
                JPL_ASSERT(e->mFace == inFace);
 
                // Assert that we have a neighbour
                const Edge* nb_edge = e->mNeighbourEdge;
                JPL_ASSERT(nb_edge != nullptr);
                if (nb_edge != nullptr)
                {
                    // Assert that our neighbours edge points to us
                    JPL_ASSERT(nb_edge->mNeighbourEdge == e);
 
                    // Assert that it belongs to a different face
                    JPL_ASSERT(nb_edge->mFace != inFace);
 
                    // Assert that the next edge of the neighbour points to the same vertex as this edge's vertex
                    JPL_ASSERT(nb_edge->mNextEdge->mStartIdx == e->mStartIdx);
 
                    // Assert that my next edge points to the same vertex as my neighbours vertex
                    JPL_ASSERT(e->mNextEdge->mStartIdx == nb_edge->mStartIdx);
                }
                e = e->mNextEdge;
            } while (e != inFace->mFirstEdge);
 
            // Assert that we have 3 or more edges
            JPL_ASSERT(edge_count >= 3);
        }
    }
 
    template<CVec3 Vec3Type>
    inline void ConvexHullBuilder<Vec3Type>::ValidateFaces() const
    {
        for (const Face* f : mFaces)
            ValidateFace(f);
    }
#endif
 
} // namespace JPL