GJKClosestPoint.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
 
#include <Jolt/Core/NonCopyable.h>
#include <Jolt/Core/FPException.h>
#include <Jolt/Geometry/ClosestPoint.h>
#include <Jolt/Geometry/ConvexSupport.h>
 
//#define JPH_GJK_DEBUG
#ifdef JPH_GJK_DEBUG
    #include <Jolt/Core/StringTools.h>
    #include <Jolt/Renderer/DebugRenderer.h>
#endif
 
JPH_NAMESPACE_BEGIN
 
class GJKClosestPoint : public NonCopyable
{
private:
    template <bool LastPointPartOfClosestFeature>
    bool        GetClosest(float inPrevVLenSq, Vec3 &outV, float &outVLenSq, uint32 &outSet) const
    {
#ifdef JPH_GJK_DEBUG
        for (int i = 0; i < mNumPoints; ++i)
            Trace("y[%d] = [%s], |y[%d]| = %g", i, ConvertToString(mY[i]).c_str(), i, (double)mY[i].Length());
#endif
 
        uint32 set;
        Vec3 v;
 
        switch (mNumPoints)
        {
        case 1:
            // Single point
            set = 0b0001;
            v = mY[0];
            break;
 
        case 2:
            // Line segment
            v = ClosestPoint::GetClosestPointOnLine(mY[0], mY[1], set);
            break;
 
        case 3:
            // Triangle
            v = ClosestPoint::GetClosestPointOnTriangle<LastPointPartOfClosestFeature>(mY[0], mY[1], mY[2], set);
            break;
 
        case 4:
            // Tetrahedron
            v = ClosestPoint::GetClosestPointOnTetrahedron<LastPointPartOfClosestFeature>(mY[0], mY[1], mY[2], mY[3], set);
            break;
 
        default:
            JPH_ASSERT(false);
            return false;
        }
 
#ifdef JPH_GJK_DEBUG
        Trace("GetClosest: set = 0b%s, v = [%s], |v| = %g", NibbleToBinary(set), ConvertToString(v).c_str(), (double)v.Length());
#endif
 
        float v_len_sq = v.LengthSq();
        if (v_len_sq < inPrevVLenSq) // Note, comparison order important: If v_len_sq is NaN then this expression will be false so we will return false
        {
            // Return closest point
            outV = v;
            outVLenSq = v_len_sq;
            outSet = set;
            return true;
        }
 
        // No better match found
#ifdef JPH_GJK_DEBUG
        Trace("New closer point is further away, failed to converge");
#endif
        return false;
    }
 
    // Get max(|Y_0|^2 .. |Y_n|^2)
    float       GetMaxYLengthSq() const
    {
        float y_len_sq = mY[0].LengthSq();
        for (int i = 1; i < mNumPoints; ++i)
            y_len_sq = max(y_len_sq, mY[i].LengthSq());
        return y_len_sq;
    }
 
    // Remove points that are not in the set, only updates mY
    void        UpdatePointSetY(uint32 inSet)
    {
        int num_points = 0;
        for (int i = 0; i < mNumPoints; ++i)
            if ((inSet & (1 << i)) != 0)
            {
                mY[num_points] = mY[i];
                ++num_points;
            }
        mNumPoints = num_points;
    }
 
    // GCC 11.3 thinks the assignments to mP, mQ and mY below may use uninitialized variables
    JPH_SUPPRESS_WARNING_PUSH
    JPH_GCC_SUPPRESS_WARNING("-Wmaybe-uninitialized")
 
    // Remove points that are not in the set, only updates mP
    void        UpdatePointSetP(uint32 inSet)
    {
        int num_points = 0;
        for (int i = 0; i < mNumPoints; ++i)
            if ((inSet & (1 << i)) != 0)
            {
                mP[num_points] = mP[i];
                ++num_points;
            }
        mNumPoints = num_points;
    }
 
    // Remove points that are not in the set, only updates mP and mQ
    void        UpdatePointSetPQ(uint32 inSet)
    {
        int num_points = 0;
        for (int i = 0; i < mNumPoints; ++i)
            if ((inSet & (1 << i)) != 0)
            {
                mP[num_points] = mP[i];
                mQ[num_points] = mQ[i];
                ++num_points;
            }
        mNumPoints = num_points;
    }
 
    // Remove points that are not in the set, updates mY, mP and mQ
    void        UpdatePointSetYPQ(uint32 inSet)
    {
        int num_points = 0;
        for (int i = 0; i < mNumPoints; ++i)
            if ((inSet & (1 << i)) != 0)
            {
                mY[num_points] = mY[i];
                mP[num_points] = mP[i];
                mQ[num_points] = mQ[i];
                ++num_points;
            }
        mNumPoints = num_points;
    }
 
    JPH_SUPPRESS_WARNING_POP
 
    // Calculate closest points on A and B
    void        CalculatePointAAndB(Vec3 &outPointA, Vec3 &outPointB) const
    {
        switch (mNumPoints)     
        {
        case 1:
            outPointA = mP[0]; 
            outPointB = mQ[0];
            break;
 
        case 2:
            {
                float u, v;
                ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], u, v);
                outPointA = u * mP[0] + v * mP[1];
                outPointB = u * mQ[0] + v * mQ[1];
            }
            break;
 
        case 3:
            {
                float u, v, w;
                ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], u, v, w);
                outPointA = u * mP[0] + v * mP[1] + w * mP[2];
                outPointB = u * mQ[0] + v * mQ[1] + w * mQ[2];
            }
            break;
 
        case 4:
        #ifdef _DEBUG
            memset(&outPointA, 0xcd, sizeof(outPointA));
            memset(&outPointB, 0xcd, sizeof(outPointB));
        #endif
            break;
        }
    }
    
public:
    template <typename A, typename B>
    bool        Intersects(const A &inA, const B &inB, float inTolerance, Vec3 &ioV)
    {
        float tolerance_sq = Square(inTolerance);
 
        // Reset state
        mNumPoints = 0;
 
#ifdef JPH_GJK_DEBUG
        for (int i = 0; i < 4; ++i)
            mY[i] = Vec3::sZero();
#endif
 
        // Previous length^2 of v
        float prev_v_len_sq = FLT_MAX;
 
        for (;;)
        {
#ifdef JPH_GJK_DEBUG
            Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints);
#endif
 
            // Get support points for shape A and B in the direction of v
            Vec3 p = inA.GetSupport(ioV);
            Vec3 q = inB.GetSupport(-ioV);
 
            // Get support point of the minkowski sum A - B of v
            Vec3 w = p - q;
 
            // If the support point sA-B(v) is in the opposite direction as v, then we have found a separating axis and there is no intersection
            if (ioV.Dot(w) < 0.0f)
            {
                // Separating axis found
#ifdef JPH_GJK_DEBUG
                Trace("Seperating axis");
#endif
                return false;
            }
 
            // Store the point for later use
            mY[mNumPoints] = w;
            ++mNumPoints;
 
#ifdef JPH_GJK_DEBUG
            Trace("w = [%s]", ConvertToString(w).c_str());
#endif
 
            // Determine the new closest point
            float v_len_sq;         // Length^2 of v
            uint32 set;             // Set of points that form the new simplex
            if (!GetClosest<true>(prev_v_len_sq, ioV, v_len_sq, set))
                return false;
 
            // If there are 4 points, the origin is inside the tetrahedron and we're done
            if (set == 0xf)
            {
#ifdef JPH_GJK_DEBUG
                Trace("Full simplex");
#endif
                ioV = Vec3::sZero();
                return true;
            }
 
            // If v is very close to zero, we consider this a collision
            if (v_len_sq <= tolerance_sq)
            {
#ifdef JPH_GJK_DEBUG
                Trace("Distance zero");
#endif
                ioV = Vec3::sZero();
                return true;
            }
 
            // If v is very small compared to the length of y, we also consider this a collision
            if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq())
            {
#ifdef JPH_GJK_DEBUG
                Trace("Machine precision reached");
#endif
                ioV = Vec3::sZero();
                return true;
            }
 
            // The next seperation axis to test is the negative of the closest point of the Minkowski sum to the origin
            // Note: This must be done before terminating as converged since the separating axis is -v
            ioV = -ioV;
 
            // If the squared length of v is not changing enough, we've converged and there is no collision
            JPH_ASSERT(prev_v_len_sq >= v_len_sq);
            if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq)
            {
                // v is a separating axis
#ifdef JPH_GJK_DEBUG
                Trace("Converged");
#endif
                return false;
            }
            prev_v_len_sq = v_len_sq;
 
            // Update the points of the simplex
            UpdatePointSetY(set);
        }
    }
 
    template <typename A, typename B>
    float       GetClosestPoints(const A &inA, const B &inB, float inTolerance, float inMaxDistSq, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB)
    {
        float tolerance_sq = Square(inTolerance);
 
        // Reset state
        mNumPoints = 0;
 
#ifdef JPH_GJK_DEBUG
        // Generate the hull of the Minkowski difference for visualization
        MinkowskiDifference diff(inA, inB);
        mGeometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); });
 
        for (int i = 0; i < 4; ++i)
        {
            mY[i] = Vec3::sZero();
            mP[i] = Vec3::sZero();
            mQ[i] = Vec3::sZero();
        }
#endif
 
        // Length^2 of v
        float v_len_sq = ioV.LengthSq();
 
        // Previous length^2 of v
        float prev_v_len_sq = FLT_MAX;
 
        for (;;)
        {
#ifdef JPH_GJK_DEBUG
            Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints);
#endif
 
            // Get support points for shape A and B in the direction of v
            Vec3 p = inA.GetSupport(ioV);
            Vec3 q = inB.GetSupport(-ioV);
 
            // Get support point of the minkowski sum A - B of v
            Vec3 w = p - q;
 
            float dot = ioV.Dot(w);
 
#ifdef JPH_GJK_DEBUG
            // Draw -ioV to show the closest point to the origin from the previous simplex
            DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f);
 
            // Draw ioV to show where we're probing next
            DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + ioV, Color::sCyan, 0.05f);
 
            // Draw w, the support point
            DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + w, Color::sGreen, 0.05f);
            DebugRenderer::sInstance->DrawMarker(mOffset + w, Color::sGreen, 1.0f);
 
            // Draw the simplex and the Minkowski difference around it
            DrawState();
#endif
 
            // Test if we have a separation of more than inMaxDistSq, in which case we terminate early
            if (dot < 0.0f && dot * dot > v_len_sq * inMaxDistSq)
            {
#ifdef JPH_GJK_DEBUG
                Trace("Distance bigger than max");
#endif
#ifdef _DEBUG
                memset(&outPointA, 0xcd, sizeof(outPointA));
                memset(&outPointB, 0xcd, sizeof(outPointB));
#endif
                return FLT_MAX;
            }
 
            // Store the point for later use
            mY[mNumPoints] = w;
            mP[mNumPoints] = p;
            mQ[mNumPoints] = q;
            ++mNumPoints;
 
#ifdef JPH_GJK_DEBUG
            Trace("w = [%s]", ConvertToString(w).c_str());
#endif
 
            uint32 set;
            if (!GetClosest<true>(prev_v_len_sq, ioV, v_len_sq, set))
            {
                --mNumPoints; // Undo add last point
                break;
            }
 
            // If there are 4 points, the origin is inside the tetrahedron and we're done
            if (set == 0xf)
            {
#ifdef JPH_GJK_DEBUG
                Trace("Full simplex");
#endif
                ioV = Vec3::sZero();
                v_len_sq = 0.0f;
                break;
            }
 
            // Update the points of the simplex
            UpdatePointSetYPQ(set);
 
            // If v is very close to zero, we consider this a collision
            if (v_len_sq <= tolerance_sq)
            {
#ifdef JPH_GJK_DEBUG
                Trace("Distance zero");
#endif
                ioV = Vec3::sZero();
                v_len_sq = 0.0f;
                break;
            }
 
            // If v is very small compared to the length of y, we also consider this a collision
#ifdef JPH_GJK_DEBUG
            Trace("Check v small compared to y: %g <= %g", (double)v_len_sq, (double)(FLT_EPSILON * GetMaxYLengthSq()));
#endif
            if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq())
            {
#ifdef JPH_GJK_DEBUG
                Trace("Machine precision reached");
#endif
                ioV = Vec3::sZero();
                v_len_sq = 0.0f;
                break;
            }
 
            // The next seperation axis to test is the negative of the closest point of the Minkowski sum to the origin
            // Note: This must be done before terminating as converged since the separating axis is -v
            ioV = -ioV;
 
            // If the squared length of v is not changing enough, we've converged and there is no collision
#ifdef JPH_GJK_DEBUG
            Trace("Check v not changing enough: %g <= %g", (double)(prev_v_len_sq - v_len_sq), (double)(FLT_EPSILON * prev_v_len_sq));
#endif
            JPH_ASSERT(prev_v_len_sq >= v_len_sq);
            if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq)
            {
                // v is a separating axis
#ifdef JPH_GJK_DEBUG
                Trace("Converged");
#endif
                break;
            }
            prev_v_len_sq = v_len_sq;
        }
 
        // Get the closest points
        CalculatePointAAndB(outPointA, outPointB);
 
#ifdef JPH_GJK_DEBUG
        Trace("Return: v = [%s], |v| = %g", ConvertToString(ioV).c_str(), (double)ioV.Length());
 
        // Draw -ioV to show the closest point to the origin from the previous simplex
        DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f);
 
        // Draw the closest points
        DebugRenderer::sInstance->DrawMarker(mOffset + outPointA, Color::sGreen, 1.0f);
        DebugRenderer::sInstance->DrawMarker(mOffset + outPointB, Color::sPurple, 1.0f);
 
        // Draw the simplex and the Minkowski difference around it
        DrawState();
#endif
 
        JPH_ASSERT(ioV.LengthSq() == v_len_sq);
        return v_len_sq;
    }
 
    void        GetClosestPointsSimplex(Vec3 *outY, Vec3 *outP, Vec3 *outQ, uint &outNumPoints) const
    {
        uint size = sizeof(Vec3) * mNumPoints;
        memcpy(outY, mY, size);
        memcpy(outP, mP, size);
        memcpy(outQ, mQ, size);
        outNumPoints = mNumPoints;
    }
 
    template <typename A>
    bool        CastRay(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inTolerance, const A &inA, float &ioLambda)
    {
        float tolerance_sq = Square(inTolerance);
 
        // Reset state
        mNumPoints = 0;
 
        float lambda = 0.0f;
        Vec3 x = inRayOrigin;
        Vec3 v = x - inA.GetSupport(Vec3::sZero());
        float v_len_sq = FLT_MAX;
        bool allow_restart = false;
                
        for (;;)
        {
#ifdef JPH_GJK_DEBUG
            Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints);
#endif
 
            // Get new support point
            Vec3 p = inA.GetSupport(v);
            Vec3 w = x - p;
 
#ifdef JPH_GJK_DEBUG
            Trace("w = [%s]", ConvertToString(w).c_str());
#endif
 
            float v_dot_w = v.Dot(w);
#ifdef JPH_GJK_DEBUG
            Trace("v . w = %g", (double)v_dot_w);
#endif
            if (v_dot_w > 0.0f)
            {
                // If ray and normal are in the same direction, we've passed A and there's no collision
                float v_dot_r = v.Dot(inRayDirection);
#ifdef JPH_GJK_DEBUG
                Trace("v . r = %g", (double)v_dot_r);
#endif
                if (v_dot_r >= 0.0f)
                    return false;
 
                // Update the lower bound for lambda
                float delta = v_dot_w / v_dot_r;
                float old_lambda = lambda;
                lambda -= delta;
#ifdef JPH_GJK_DEBUG
                Trace("lambda = %g, delta = %g", (double)lambda, (double)delta);
#endif
 
                // If lambda didn't change, we cannot converge any further and we assume a hit
                if (old_lambda == lambda)
                    break;
 
                // If lambda is bigger or equal than max, we don't have a hit
                if (lambda >= ioLambda)
                    return false;
 
                // Update x to new closest point on the ray
                x = inRayOrigin + lambda * inRayDirection;
                
                // We've shifted x, so reset v_len_sq so that it is not used as early out for GetClosest
                v_len_sq = FLT_MAX;
 
                // We allow rebuilding the simplex once after x changes because the simplex was built
                // for another x and numerical round off builds up as you keep adding points to an
                // existing simplex
                allow_restart = true;
            }
 
            // Add p to set P: P = P U {p}
            mP[mNumPoints] = p;
            ++mNumPoints;
 
            // Calculate Y = {x} - P
            for (int i = 0; i < mNumPoints; ++i)
                mY[i] = x - mP[i];
 
            // Determine the new closest point from Y to origin
            bool needs_restart = false;
            uint32 set;                     // Set of points that form the new simplex
            if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
            {
#ifdef JPH_GJK_DEBUG
                Trace("Failed to converge");
#endif
 
                // We failed to converge, restart
                needs_restart = true;
            }
            else if (set == 0xf)
            {
#ifdef JPH_GJK_DEBUG
                Trace("Full simplex");
#endif
 
                // If there are 4 points, x is inside the tetrahedron and we've found a hit
                // Double check if this is indeed the case
                if (v_len_sq <= tolerance_sq)
                    break;
 
                // We failed to converge, restart
                needs_restart = true;
            }
 
            if (needs_restart)
            {
                // Only allow 1 restart, if we still can't get a closest point 
                // we're so close that we return this as a hit
                if (!allow_restart)
                    break;
 
                // If we fail to converge, we start again with the last point as simplex
#ifdef JPH_GJK_DEBUG
                Trace("Restarting");
#endif
                allow_restart = false;
                mP[0] = p;
                mNumPoints = 1;
                v = x - p;
                v_len_sq = FLT_MAX;
                continue;
            }
 
            // Update the points P to form the new simplex
            // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration
            UpdatePointSetP(set);
 
            // Check if x is close enough to inA
            if (v_len_sq <= tolerance_sq)
            {
#ifdef JPH_GJK_DEBUG
                Trace("Converged");
#endif
                break;
            }
        }
 
        // Store hit fraction
        ioLambda = lambda;
        return true;
    }
 
    template <typename A, typename B>
    bool        CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float &ioLambda)
    {
        // Transform the shape to be cast to the starting position
        TransformedConvexObject transformed_a(inStart, inA);
 
        // Calculate the minkowski difference inB - inA
        // inA is moving, so we need to add the back side of inB to the front side of inA
        MinkowskiDifference difference(inB, transformed_a);
 
        // Do a raycast against the Minkowski difference
        return CastRay(Vec3::sZero(), inDirection, inTolerance, difference, ioLambda);
    }
 
    template <typename A, typename B>
    bool        CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outSeparatingAxis)
    {
        float tolerance_sq = Square(inTolerance);
 
        // Calculate how close A and B (without their convex radius) need to be to eachother in order for us to consider this a collision
        float sum_convex_radius = inConvexRadiusA + inConvexRadiusB;
 
        // Transform the shape to be cast to the starting position
        TransformedConvexObject transformed_a(inStart, inA);
 
        // Reset state
        mNumPoints = 0;
 
        float lambda = 0.0f;
        Vec3 x = Vec3::sZero(); // Since A is already transformed we can start the cast from zero
        Vec3 v = -inB.GetSupport(Vec3::sZero()) + transformed_a.GetSupport(Vec3::sZero()); // See CastRay: v = x - inA.GetSupport(Vec3::sZero()) where inA is the Minkowski difference inB - transformed_a (see CastShape above) and x is zero
        float v_len_sq = FLT_MAX;
        bool allow_restart = false;
 
        // Keeps track of separating axis of the previous iteration.
        // Initialized at zero as we don't know if our first v is actually a separating axis.
        Vec3 prev_v = Vec3::sZero();
 
        for (;;)
        {
#ifdef JPH_GJK_DEBUG
            Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints);
#endif
 
            // Calculate the minkowski difference inB - inA
            // inA is moving, so we need to add the back side of inB to the front side of inA
            // Keep the support points on A and B separate so that in the end we can calculate a contact point
            Vec3 p = transformed_a.GetSupport(-v);
            Vec3 q = inB.GetSupport(v);
            Vec3 w = x - (q - p);
 
#ifdef JPH_GJK_DEBUG
            Trace("w = [%s]", ConvertToString(w).c_str());
#endif
 
            // Difference from article to this code:
            // We did not include the convex radius in p and q in order to be able to calculate a good separating axis at the end of the algorithm.
            // However when moving forward along inDirection we do need to take this into account so that we keep A and B separated by the sum of their convex radii.
            // From p we have to subtract: inConvexRadiusA * v / |v|
            // To q we have to add: inConvexRadiusB * v / |v|
            // This means that to w we have to add: -(inConvexRadiusA + inConvexRadiusB) * v / |v|
            // So to v . w we have to add: v . (-(inConvexRadiusA + inConvexRadiusB) * v / |v|) = -(inConvexRadiusA + inConvexRadiusB) * |v|
            float v_dot_w = v.Dot(w) - sum_convex_radius * v.Length();
#ifdef JPH_GJK_DEBUG
            Trace("v . w = %g", (double)v_dot_w);
#endif
            if (v_dot_w > 0.0f)
            {
                // If ray and normal are in the same direction, we've passed A and there's no collision
                float v_dot_r = v.Dot(inDirection);
#ifdef JPH_GJK_DEBUG
                Trace("v . r = %g", (double)v_dot_r);
#endif
                if (v_dot_r >= 0.0f)
                    return false;
 
                // Update the lower bound for lambda
                float delta = v_dot_w / v_dot_r;
                float old_lambda = lambda;
                lambda -= delta;
#ifdef JPH_GJK_DEBUG
                Trace("lambda = %g, delta = %g", (double)lambda, (double)delta);
#endif
 
                // If lambda didn't change, we cannot converge any further and we assume a hit
                if (old_lambda == lambda)
                    break;
 
                // If lambda is bigger or equal than max, we don't have a hit
                if (lambda >= ioLambda)
                    return false;
 
                // Update x to new closest point on the ray
                x = lambda * inDirection;
                
                // We've shifted x, so reset v_len_sq so that it is not used as early out when GetClosest returns false
                v_len_sq = FLT_MAX;
 
                // Now that we've moved, we know that A and B are not intersecting at lambda = 0, so we can update our tolerance to stop iterating
                // as soon as A and B are inConvexRadiusA + inConvexRadiusB apart
                tolerance_sq = Square(inTolerance + sum_convex_radius);
 
                // We allow rebuilding the simplex once after x changes because the simplex was built
                // for another x and numerical round off builds up as you keep adding points to an
                // existing simplex
                allow_restart = true;
            }
 
            // Add p to set P, q to set Q: P = P U {p}, Q = Q U {q}
            mP[mNumPoints] = p;
            mQ[mNumPoints] = q;
            ++mNumPoints;
 
            // Calculate Y = {x} - (Q - P)
            for (int i = 0; i < mNumPoints; ++i)
                mY[i] = x - (mQ[i] - mP[i]);
 
            // Determine the new closest point from Y to origin
            bool needs_restart = false;
            uint32 set;                     // Set of points that form the new simplex
            if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
            {
#ifdef JPH_GJK_DEBUG
                Trace("Failed to converge");
#endif
 
                // We failed to converge, restart
                needs_restart = true;
            }
            else if (set == 0xf)
            {
#ifdef JPH_GJK_DEBUG
                Trace("Full simplex");
#endif
 
                // If there are 4 points, x is inside the tetrahedron and we've found a hit
                // Double check that A and B are indeed touching according to our tolerance
                if (v_len_sq <= tolerance_sq)
                    break;
 
                // We failed to converge, restart
                needs_restart = true;
            }
 
            if (needs_restart)
            {
                // Only allow 1 restart, if we still can't get a closest point 
                // we're so close that we return this as a hit
                if (!allow_restart)
                    break;
 
                // If we fail to converge, we start again with the last point as simplex
#ifdef JPH_GJK_DEBUG
                Trace("Restarting");
#endif
                allow_restart = false;
                mP[0] = p;
                mQ[0] = q;
                mNumPoints = 1;
                v = x - q;
                v_len_sq = FLT_MAX;
                continue;
            }
 
            // Update the points P and Q to form the new simplex
            // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration
            UpdatePointSetPQ(set);
 
            // Check if A and B are touching according to our tolerance
            if (v_len_sq <= tolerance_sq)
            {
#ifdef JPH_GJK_DEBUG
                Trace("Converged");
#endif
                break;
            }
 
            // Store our v to return as separating axis
            prev_v = v;
        }
 
        // Calculate Y = {x} - (Q - P) again so we can calculate the contact points
        for (int i = 0; i < mNumPoints; ++i)
            mY[i] = x - (mQ[i] - mP[i]);
 
        // Calculate the offset we need to apply to A and B to correct for the convex radius
        Vec3 normalized_v = v.NormalizedOr(Vec3::sZero());
        Vec3 convex_radius_a = inConvexRadiusA * normalized_v;
        Vec3 convex_radius_b = inConvexRadiusB * normalized_v;
 
        // Get the contact point
        // Note that A and B will coincide when lambda > 0. In this case we calculate only B as it is more accurate as it contains less terms.
        switch (mNumPoints)     
        {
        case 1:
            outPointB = mQ[0] + convex_radius_b; 
            outPointA = lambda > 0.0f? outPointB : mP[0] - convex_radius_a;
            break;
 
        case 2:
            {
                float bu, bv;
                ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], bu, bv);
                outPointB = bu * mQ[0] + bv * mQ[1] + convex_radius_b;
                outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] - convex_radius_a;
            }
            break;
 
        case 3:
        case 4: // A full simplex, we can't properly determine a contact point! As contact point we take the closest point of the previous iteration.
            {
                float bu, bv, bw;
                ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], bu, bv, bw);
                outPointB = bu * mQ[0] + bv * mQ[1] + bw * mQ[2] + convex_radius_b;
                outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] + bw * mP[2] - convex_radius_a;
            }
            break;
        }
 
        // Store separating axis, in case we have a convex radius we can just return v, 
        // otherwise v will be very small and we resort to returning previous v as an approximation.
        outSeparatingAxis = sum_convex_radius > 0.0f? -v : -prev_v;
 
        // Store hit fraction
        ioLambda = lambda;
        return true;
    }
 
private:
#ifdef JPH_GJK_DEBUG
    void        DrawState()
    {
        RMat44 origin = RMat44::sTranslation(mOffset);
 
        // Draw origin
        DebugRenderer::sInstance->DrawCoordinateSystem(origin, 1.0f);
 
        // Draw the hull
        DebugRenderer::sInstance->DrawGeometry(origin, mGeometry->mBounds.Transformed(origin), mGeometry->mBounds.GetExtent().LengthSq(), Color::sYellow, mGeometry);
 
        // Draw Y
        for (int i = 0; i < mNumPoints; ++i)
        {
            // Draw support point
            RVec3 y_i = origin * mY[i];
            DebugRenderer::sInstance->DrawMarker(y_i, Color::sRed, 1.0f);
            for (int j = i + 1; j < mNumPoints; ++j)
            {
                // Draw edge
                RVec3 y_j = origin * mY[j];
                DebugRenderer::sInstance->DrawLine(y_i, y_j, Color::sRed);
                for (int k = j + 1; k < mNumPoints; ++k)
                {
                    // Make sure triangle faces the origin
                    RVec3 y_k = origin * mY[k];
                    RVec3 center = (y_i + y_j + y_k) / Real(3);
                    RVec3 normal = (y_j - y_i).Cross(y_k - y_i);
                    if (normal.Dot(center) < Real(0))
                        DebugRenderer::sInstance->DrawTriangle(y_i, y_j, y_k, Color::sLightGrey);
                    else
                        DebugRenderer::sInstance->DrawTriangle(y_i, y_k, y_j, Color::sLightGrey);
                }
            }
        }
 
        // Offset to the right
        mOffset += Vec3(mGeometry->mBounds.GetSize().GetX() + 2.0f, 0, 0);
    }
#endif // JPH_GJK_DEBUG
 
    Vec3        mY[4];                      
    Vec3        mP[4];                      
    Vec3        mQ[4];                      
    int         mNumPoints = 0;             
 
#ifdef JPH_GJK_DEBUG
    DebugRenderer::GeometryRef  mGeometry;  
    RVec3       mOffset = RVec3::sZero();   
#endif
};
 
JPH_NAMESPACE_END