cairnc/sat_blog.cpp

## sat_blog.cpp
void satCollideReference(const SatShape *a, Transform xfA, const SatShape *b, Transform xfB, SatResult *res)
{
    ASSERT(b->numFaces != 2);
    Transform bToA = xfA.inverse().mul(xfB);
    Transform aToB = bToA.inverse();

    res->vert = -1;
    res->face = -1;
    res->support = INFINITY;
    // Test MD against planes of a
    for (size_t i = 0; i < a->numFaces; i++)
    {
        Plane plane = a->facePlanes[i];
        Vec3 normal = -aToB.rotate(plane.normal);
        float planeDist = plane.dist - plane.normal.dot(bToA.p);
        float supp;
        size_t vertex;
        getSupport(normal, b->vertPos, b->numVerts, &supp, &vertex);
        supp += planeDist;
        if (supp < res->support)
        {
            res->type = SatResult::FACE_VERT;
            res->support = supp;
            res->vert = vertex;
            res->face = i;
            res->mtv = plane.normal;
            if (supp < 0)
            {
                res->mtv = xfA.rotate(res->mtv);
                return;
            }
        }
    }
    // Test MD against planes of b
    for (size_t i = 0; i < b->numFaces; i++)
    {
        Plane plane = b->facePlanes[i];
        Vec3 normal = -bToA.rotate(plane.normal);
        float planeDist = plane.dist - normal.dot(bToA.p);
        float supp;
        size_t vertex;
        getSupport(normal, a->vertPos, a->numVerts, &supp, &vertex);
        supp += planeDist;
        if (supp < res->support)
        {
            res->support = supp;
            res->vert = vertex;
            res->face = i;
            res->type = SatResult::VERT_FACE;
            res->mtv = normal;
            if (supp < 0)
            {
                res->mtv = xfA.rotate(res->mtv);
                return;
            }
        }
    }

    //drawSphere(sphereOrigin,0.99f, ColorWhite, 20, 20);

    // Test MD against edge cross products
    for(size_t j = 0; j < b->numEdges; j++)
    {
        SatShape::Edge eB = b->edges[j];
        // it is faster to pre transform all geometry but
        // we don't because satCollideLocal does not
        Vec3 C   = -bToA.rotate(b->facePlanes[eB.f0].normal);
        Vec3 D   = -bToA.rotate(b->facePlanes[eB.f1].normal);
        Vec3 bv0 = -bToA.mul(b->vertPos[eB.v0]);
        Vec3 bv1 = -bToA.mul(b->vertPos[eB.v1]);
        Vec3 D_x_C = bv1 - bv0;

        for(size_t i = 0; i < a->numEdges; i++)
        {
            SatShape::Edge eA = a->edges[i];
            Vec3 A  = a->facePlanes[eA.f0].normal;
            Vec3 B  = a->facePlanes[eA.f1].normal;
            Vec3 av0 = a->vertPos[eA.v0];
            Vec3 av1 = a->vertPos[eA.v1];
            Vec3 B_x_A = av1 - av0;

            float CBA = C.dot(B_x_A);
            float DBA = D.dot(B_x_A);
            float ADC = A.dot(D_x_C);
            float BDC = B.dot(D_x_C);

            //if (i == 0) drawArcBetween(sphereOrigin, C, D, 1.0f, COLOR_BLUE, true);

            float eps = -0.0001f;
            if(CBA*DBA < eps && ADC*BDC < eps && CBA*BDC < eps)
            {
                // Cast the ray CD against the plane containing the tri ABO
                // (C+(D-C)*t) dot (BxA) == 0
                // C dot BxA + (D-C) dot BxA*t == 0
                // CBA + (D dot BxA - C dot BxA) t == 0
                // t = -CBA/(DBA-CBA)
                float t = -CBA / (DBA-CBA);
                Vec3 normal = Vec3::lerp(t, C, D).normalized();
                float support = normal.dot(av0+bv0);

                //drawPoint(sphereOrigin+normal,COLOR_RED);
                //if (support < res->support-3*SOLVER_DELTA_SLOP)
                if (support < res->support)
                {
                    res->mtv = normal;
                    res->support = support;
                    res->type = SatResult::EDGE_EDGE;
                    res->edge1 = eA;
                    res->edge2 = eB;

                    if (support < 0)
                    {
                        //drawArrowTo( tB.mul( av0),  tB.mul(av1), COLOR_RED, 1.0f);
                        //drawArrowTo( tB.mul( -bv0), tB.mul(-bv1), COLOR_RED, 1.0f);
                        //drawArcBetween(g_sphereOrigin, C, D, 1.0f, COLOR_BLUE, true);
                        //drawArcBetween(g_sphereOrigin, A, B, 1.0f, COLOR_GREEN, true);
                        //drawPoint(g_sphereOrigin+normal, COLOR_PURPLE);

                        res->mtv = xfA.rotate(res->mtv);
                        return;
                    }
                }

            }
        }
    }
    //drawPointEx(sphereOrigin+tB.inverse().rotate(res->mtv)), COLOR_PURPLE, 1.5f);

    res->mtv = xfA.rotate(res->mtv);
}


void satCollideGraph(const SatShape *a, Transform xfA, const SatShape *b, Transform xfB, SatResult *res)
{
    ASSERT(b->numFaces != 2);
    ASSERT(a->vertEdges);

    res->support = INFINITY;

    //Vec3 sphereOrigin = {3,0,0};
    //for (int i = 0; i < b->numEdges; i++) drawArcBetween(sphereOrigin, b->facePlanes[b->edges[i].f0].normal, b->facePlanes[b->edges[i].f1].normal, 1.0f, COLOR_BLUE, false);
    //drawSphere(sphereOrigin, 0.99f, ColorWhite, 20, 20);

    uint8_t bFaceToVertexRegionA[SAT_MAX];
    memset(bFaceToVertexRegionA, 0xff, sizeof(bFaceToVertexRegionA[0])*b->numFaces);

    // For a face afi this map is the min support so far of all the arcs that intersected with arcs connecting afi
    float aFaceMaxDot[SAT_MAX];
    uint8_t aFaceToVertexRegionB[SAT_MAX];
    memset(aFaceToVertexRegionB, 0xff, sizeof(aFaceToVertexRegionB[0])*a->numFaces);
    memset(aFaceMaxDot, 0xfe, sizeof(aFaceMaxDot[0])*a->numFaces);

    // as a float the bit pattern 0xfefefefe is -1.6947395e+38

    Transform bToA = xfA.inverse().mul(xfB);
    Transform aToB = bToA.inverse();

    // Find the vertex region that contains the first face of B
    // This is the face we will start our traversal from.
    {
        size_t faceIndex = 0;
        Plane plane = b->facePlanes[faceIndex];
        Vec3 normal = -bToA.rotate(plane.normal);
        float planeDist = plane.dist - normal.dot(bToA.p);
        float supp;
        size_t vertex;
        getSupport(normal, a->vertPos, a->numVerts, &supp, &vertex);
        supp += planeDist;
        if (supp < res->support)
        {
            res->support = supp;
            res->vert = vertex;
            res->face = faceIndex;
            res->type = SatResult::VERT_FACE;
            res->mtv = normal;
            if (supp < 0)
            {
                res->mtv = xfA.rotate(res->mtv);
                return;
            }
        }

        bFaceToVertexRegionA[faceIndex] = (uint8_t)vertex;
    }

    for(int i = 0; i < b->numEdges; i++)
    {
        SatShape::Edge eB = b->edges[i];
        Vec3 rayOrigin = -bToA.rotate(b->facePlanes[eB.f0].normal);
        Vec3 rayEnd = -bToA.rotate(b->facePlanes[eB.f1].normal);
        Vec3 rayDir = rayEnd - rayOrigin;

        int curVertA = bFaceToVertexRegionA[eB.f0];
        //ASSERT(curVertA != 0xff);

        // verts of -B in A
        Vec3 bVert0inA = -bToA.mul(b->vertPos[eB.v0]);
        Vec3 bVert1inA = -bToA.mul(b->vertPos[eB.v1]);

        //drawPoint(sphereOrigin+rayOrigin, COLOR_ORANGE);
        //drawArcBetween(sphereOrigin, rayOrigin, rayMid, 1.0f, COLOR_GREEN, true);
        //drawArcBetween(sphereOrigin, rayMid, rayEnd, 1.0f, COLOR_RED, true);

        // Traverse the arc from f0 to f1
        for (int it = 0; it < 32; it++)
        {
            // We are in the vertex region of curVertA
            // The vertex region is bounded by vert.num planes, it forms a convex linear cone.
            // To find which plane we exit we raycast against the planes.
            SatShape::EdgeList vert = a->verts[curVertA];
            SatShape::Edge eA = { 0 };
            float tMin = INFINITY;
            for (size_t j = 0; j < vert.num; j++)
            {
                SatShape::Edge portal = a->vertEdges[vert.first + j];

                // this can be precomputed and laid out as SoA for SIMD
                Vec3 portalNormal = a->vertPos[curVertA] - a->vertPos[portal.v1]; // <- this normal can be innacurate if hull is bad

                //drawArrow(sphereOrigin+ Vec3::lerp(0.5f, b->facePlanes[b->edges[portal.e].f0].normal, b->facePlanes[b->edges[portal.e].f1].normal).normalize(), portalNormal, COLOR_RED, 1.0f);

                float raySlope = portalNormal.dot(rayDir);
                if (raySlope < 0)
                {
                    float tHit = -rayOrigin.dot(portalNormal) / raySlope;
                    if (tHit < tMin)
                    {
                        eA = portal;
                        tMin = tHit;
                    }
                }
            }

            //ASSERT(tMin >= 0);

            if(tMin < 1)
            {
                // We hit a plane before we reached the end of the arc.
                // This means we are changing vertex regions.

                curVertA = eA.v1;
                Vec3 normal =(rayOrigin+rayDir*tMin).normalized();
                float support = normal.dot(bVert0inA + a->vertPos[curVertA]);
                if (support < res->support)
                {
                    res->mtv = normal;
                    res->type = SatResult::EDGE_EDGE;
                    res->support = support;
                    res->edge1 = eA;
                    res->edge2 = eB;

                    if (support < 0)
                    {
                        // shapes aren't overlapping.

                        //drawPoint(vecAdd(sphereOrigin, normal), COLOR_PURPLE);
                        //SatVertex v = b->verts[curVertB];
                        //for(int j = 0; j < v.numPortals; j++)
                        //{
                        //    SatEdge e = b->edges[b->vertEdges[v.firstPortal + j].e];
                        //    drawArcBetween(sphereOrigin, b->facePlanes[e.f0].normal, b->facePlanes[e.f1].normal, 1.0f, COLOR_YELLOW, true);
                        //}

                        res->mtv = xfA.rotate(res->mtv);
                        return;
                    }
                }


                // relax both faces of A connected to the arc we intersected

                // There might be a way to do less work here

                // f0
                {
                    size_t fa = eA.f0;
                    Plane aPlane = a->facePlanes[fa];
                    float dot_b0 = aPlane.normal.dot(bVert0inA);
                    float dot_b1 = aPlane.normal.dot(bVert1inA);
                    if (dot_b0 > aFaceMaxDot[fa])
                    {
                        aFaceMaxDot[fa] = dot_b0;
                        aFaceToVertexRegionB[fa] = eB.v0;
                    }
                    if (dot_b1 > aFaceMaxDot[fa])
                    {
                        aFaceMaxDot[fa] = dot_b1;
                        aFaceToVertexRegionB[fa] = eB.v1;
                    }
                }

                // f1
                {
                    size_t fa = eA.f1;
                    Plane aPlane = a->facePlanes[fa];
                    float dot_b0 = aPlane.normal.dot(bVert0inA);
                    float dot_b1 = aPlane.normal.dot(bVert1inA);
                    if (dot_b0 > aFaceMaxDot[fa])
                    {
                        aFaceMaxDot[fa] = dot_b0;
                        aFaceToVertexRegionB[fa] = eB.v0;
                    }
                    if (dot_b1 > aFaceMaxDot[fa])
                    {
                        aFaceMaxDot[fa] = dot_b1;
                        aFaceToVertexRegionB[fa] = eB.v1;
                    }
                }


                // Now we keep continue starting from the intersection point we just hit.
                rayOrigin = normal;
                rayDir = rayEnd - rayOrigin;

                //drawPoint(vecAdd(sphereOrigin, normal), COLOR_RED);
            }
            else
            {
                // We didn't hit anything which means f1 is in the region curVertA
                break;
            }
        }

        {
            bFaceToVertexRegionA[eB.f1] = curVertA;

            // Find the plane of f1 in A space
            Plane bPlane = b->facePlanes[eB.f1];
            Vec3 bPlaneNormalInA = -bToA.rotate(bPlane.normal);
            float bPlaneDistInA = bPlane.dist - bPlaneNormalInA.dot(bToA.p);

            float support = rayEnd.dot(a->vertPos[curVertA]) + bPlaneDistInA;
            if(support < res->support)
            {
                res->mtv = rayEnd;
                res->type = SatResult::VERT_FACE;
                res->support = support;
                res->vert = curVertA;
                res->face = eB.f1;

                if (support < 0)
                {
                    // shapes aren't overlapping.

                    //drawPoint(sphereOrigin + rayEnd, COLOR_PURPLE);

                    //SatVertex v = b->verts[curVertB];
                    //for(int j = 0; j < v.numPortals; j++)
                    //{
                    //    SatEdge e = b->edges[b->vertEdges[v.firstPortal + j].e];
                    //    drawArcBetween(sphereOrigin, b->facePlanes[e.f0].normal, b->facePlanes[e.f1].normal, 1.0f, COLOR_YELLOW, true);
                    //}

                    res->mtv = xfA.rotate(res->mtv);
                    return;
                }
            }

        }
    }


    // Color any remaining faces of A that haven't been had their arcs intersected
    uint8_t queue[SAT_MAX];
    size_t queueLo = 0;
    size_t queueHi = 0;
    for (size_t i = 0; i < a->numFaces; i++)
    {
        if (aFaceToVertexRegionB[i] != 0xff)
        {
            queue[queueHi++] = i;
        }
    }

    while (queueLo < queueHi)
    {
        size_t f = queue[queueLo++];
        uint8_t region = aFaceToVertexRegionB[f];

        Vec3 bVertInA = -bToA.mul(b->vertPos[region]);

        SatShape::EdgeList face = a->faces[f];
        for (size_t i = 0; i < face.num; i++)
        {
            SatShape::Edge nbr = a->faceEdges[face.first + i];
            if (aFaceToVertexRegionB[nbr.f1] == 0xff)
            {
                queue[queueHi++] = nbr.f1;
                aFaceToVertexRegionB[nbr.f1] = region;
                aFaceMaxDot[nbr.f1] = a->facePlanes[nbr.f1].normal.dot(bVertInA);
            }
        }
    }

    for (size_t faceA = 0; faceA < a->numFaces; faceA++)
    {
        size_t vertB = aFaceToVertexRegionB[faceA];
        ASSERT(vertB != 0xff);
        Plane aPlane = a->facePlanes[faceA];

        float supp = aPlane.dist + aFaceMaxDot[faceA];
        if (supp < res->support)
        {
            res->mtv = aPlane.normal;
            res->type = SatResult::FACE_VERT;
            res->support = supp;
            res->vert = vertB;
            res->face = faceA;

            if (supp < 0)
            {
                res->mtv = xfA.rotate(res->mtv);
                return;
            }
        }


    }

    res->mtv = xfA.rotate(res->mtv);
}
	void satCollideReference(const SatShape a, Transform xfA, const SatShape b, Transform xfB, SatResult *res)
	{
	ASSERT(b->numFaces != 2);
	Transform bToA = xfA.inverse().mul(xfB);
	Transform aToB = bToA.inverse();

	res->vert = -1;
	res->face = -1;
	res->support = INFINITY;
	// Test MD against planes of a
	for (size_t i = 0; i < a->numFaces; i++)
	{
	Plane plane = a->facePlanes[i];
	Vec3 normal = -aToB.rotate(plane.normal);
	float planeDist = plane.dist - plane.normal.dot(bToA.p);
	float supp;
	size_t vertex;
	getSupport(normal, b->vertPos, b->numVerts, &supp, &vertex);
	supp += planeDist;
	if (supp < res->support)
	{
	res->type = SatResult::FACE_VERT;
	res->support = supp;
	res->vert = vertex;
	res->face = i;
	res->mtv = plane.normal;
	if (supp < 0)
	{
	res->mtv = xfA.rotate(res->mtv);
	return;
	}
	}
	}
	// Test MD against planes of b
	for (size_t i = 0; i < b->numFaces; i++)
	{
	Plane plane = b->facePlanes[i];
	Vec3 normal = -bToA.rotate(plane.normal);
	float planeDist = plane.dist - normal.dot(bToA.p);
	float supp;
	size_t vertex;
	getSupport(normal, a->vertPos, a->numVerts, &supp, &vertex);
	supp += planeDist;
	if (supp < res->support)
	{
	res->support = supp;
	res->vert = vertex;
	res->face = i;
	res->type = SatResult::VERT_FACE;
	res->mtv = normal;
	if (supp < 0)
	{
	res->mtv = xfA.rotate(res->mtv);
	return;
	}
	}
	}

	//drawSphere(sphereOrigin,0.99f, ColorWhite, 20, 20);

	// Test MD against edge cross products
	for(size_t j = 0; j < b->numEdges; j++)
	{
	SatShape::Edge eB = b->edges[j];
	// it is faster to pre transform all geometry but
	// we don't because satCollideLocal does not
	Vec3 C = -bToA.rotate(b->facePlanes[eB.f0].normal);
	Vec3 D = -bToA.rotate(b->facePlanes[eB.f1].normal);
	Vec3 bv0 = -bToA.mul(b->vertPos[eB.v0]);
	Vec3 bv1 = -bToA.mul(b->vertPos[eB.v1]);
	Vec3 D_x_C = bv1 - bv0;

	for(size_t i = 0; i < a->numEdges; i++)
	{
	SatShape::Edge eA = a->edges[i];
	Vec3 A = a->facePlanes[eA.f0].normal;
	Vec3 B = a->facePlanes[eA.f1].normal;
	Vec3 av0 = a->vertPos[eA.v0];
	Vec3 av1 = a->vertPos[eA.v1];
	Vec3 B_x_A = av1 - av0;

	float CBA = C.dot(B_x_A);
	float DBA = D.dot(B_x_A);
	float ADC = A.dot(D_x_C);
	float BDC = B.dot(D_x_C);

	//if (i == 0) drawArcBetween(sphereOrigin, C, D, 1.0f, COLOR_BLUE, true);

	float eps = -0.0001f;
	if(CBADBA < eps && ADCBDC < eps && CBA*BDC < eps)
	{
	// Cast the ray CD against the plane containing the tri ABO
	// (C+(D-C)*t) dot (BxA) == 0
	// C dot BxA + (D-C) dot BxA*t == 0
	// CBA + (D dot BxA - C dot BxA) t == 0
	// t = -CBA/(DBA-CBA)
	float t = -CBA / (DBA-CBA);
	Vec3 normal = Vec3::lerp(t, C, D).normalized();
	float support = normal.dot(av0+bv0);

	//drawPoint(sphereOrigin+normal,COLOR_RED);
	//if (support < res->support-3*SOLVER_DELTA_SLOP)
	if (support < res->support)
	{
	res->mtv = normal;
	res->support = support;
	res->type = SatResult::EDGE_EDGE;
	res->edge1 = eA;
	res->edge2 = eB;

	if (support < 0)
	{
	//drawArrowTo( tB.mul( av0), tB.mul(av1), COLOR_RED, 1.0f);
	//drawArrowTo( tB.mul( -bv0), tB.mul(-bv1), COLOR_RED, 1.0f);
	//drawArcBetween(g_sphereOrigin, C, D, 1.0f, COLOR_BLUE, true);
	//drawArcBetween(g_sphereOrigin, A, B, 1.0f, COLOR_GREEN, true);
	//drawPoint(g_sphereOrigin+normal, COLOR_PURPLE);

	res->mtv = xfA.rotate(res->mtv);
	return;
	}
	}

	}
	}
	}
	//drawPointEx(sphereOrigin+tB.inverse().rotate(res->mtv)), COLOR_PURPLE, 1.5f);

	res->mtv = xfA.rotate(res->mtv);
	}




	void satCollideGraph(const SatShape a, Transform xfA, const SatShape b, Transform xfB, SatResult *res)
	{
	ASSERT(b->numFaces != 2);
	ASSERT(a->vertEdges);

	res->support = INFINITY;

	//Vec3 sphereOrigin = {3,0,0};
	//for (int i = 0; i < b->numEdges; i++) drawArcBetween(sphereOrigin, b->facePlanes[b->edges[i].f0].normal, b->facePlanes[b->edges[i].f1].normal, 1.0f, COLOR_BLUE, false);
	//drawSphere(sphereOrigin, 0.99f, ColorWhite, 20, 20);

	uint8_t bFaceToVertexRegionA[SAT_MAX];
	memset(bFaceToVertexRegionA, 0xff, sizeof(bFaceToVertexRegionA[0])*b->numFaces);

	// For a face afi this map is the min support so far of all the arcs that intersected with arcs connecting afi
	float aFaceMaxDot[SAT_MAX];
	uint8_t aFaceToVertexRegionB[SAT_MAX];
	memset(aFaceToVertexRegionB, 0xff, sizeof(aFaceToVertexRegionB[0])*a->numFaces);
	memset(aFaceMaxDot, 0xfe, sizeof(aFaceMaxDot[0])*a->numFaces);

	// as a float the bit pattern 0xfefefefe is -1.6947395e+38

	Transform bToA = xfA.inverse().mul(xfB);
	Transform aToB = bToA.inverse();

	// Find the vertex region that contains the first face of B
	// This is the face we will start our traversal from.
	{
	size_t faceIndex = 0;
	Plane plane = b->facePlanes[faceIndex];
	Vec3 normal = -bToA.rotate(plane.normal);
	float planeDist = plane.dist - normal.dot(bToA.p);
	float supp;
	size_t vertex;
	getSupport(normal, a->vertPos, a->numVerts, &supp, &vertex);
	supp += planeDist;
	if (supp < res->support)
	{
	res->support = supp;
	res->vert = vertex;
	res->face = faceIndex;
	res->type = SatResult::VERT_FACE;
	res->mtv = normal;
	if (supp < 0)
	{
	res->mtv = xfA.rotate(res->mtv);
	return;
	}
	}

	bFaceToVertexRegionA[faceIndex] = (uint8_t)vertex;
	}

	for(int i = 0; i < b->numEdges; i++)
	{
	SatShape::Edge eB = b->edges[i];
	Vec3 rayOrigin = -bToA.rotate(b->facePlanes[eB.f0].normal);
	Vec3 rayEnd = -bToA.rotate(b->facePlanes[eB.f1].normal);
	Vec3 rayDir = rayEnd - rayOrigin;

	int curVertA = bFaceToVertexRegionA[eB.f0];
	//ASSERT(curVertA != 0xff);

	// verts of -B in A
	Vec3 bVert0inA = -bToA.mul(b->vertPos[eB.v0]);
	Vec3 bVert1inA = -bToA.mul(b->vertPos[eB.v1]);

	//drawPoint(sphereOrigin+rayOrigin, COLOR_ORANGE);
	//drawArcBetween(sphereOrigin, rayOrigin, rayMid, 1.0f, COLOR_GREEN, true);
	//drawArcBetween(sphereOrigin, rayMid, rayEnd, 1.0f, COLOR_RED, true);

	// Traverse the arc from f0 to f1
	for (int it = 0; it < 32; it++)
	{
	// We are in the vertex region of curVertA
	// The vertex region is bounded by vert.num planes, it forms a convex linear cone.
	// To find which plane we exit we raycast against the planes.
	SatShape::EdgeList vert = a->verts[curVertA];
	SatShape::Edge eA = { 0 };
	float tMin = INFINITY;
	for (size_t j = 0; j < vert.num; j++)
	{
	SatShape::Edge portal = a->vertEdges[vert.first + j];

	// this can be precomputed and laid out as SoA for SIMD
	Vec3 portalNormal = a->vertPos[curVertA] - a->vertPos[portal.v1]; // <- this normal can be innacurate if hull is bad

	//drawArrow(sphereOrigin+ Vec3::lerp(0.5f, b->facePlanes[b->edges[portal.e].f0].normal, b->facePlanes[b->edges[portal.e].f1].normal).normalize(), portalNormal, COLOR_RED, 1.0f);

	float raySlope = portalNormal.dot(rayDir);
	if (raySlope < 0)
	{
	float tHit = -rayOrigin.dot(portalNormal) / raySlope;
	if (tHit < tMin)
	{
	eA = portal;
	tMin = tHit;
	}
	}
	}

	//ASSERT(tMin >= 0);

	if(tMin < 1)
	{
	// We hit a plane before we reached the end of the arc.
	// This means we are changing vertex regions.

	curVertA = eA.v1;
	Vec3 normal =(rayOrigin+rayDir*tMin).normalized();
	float support = normal.dot(bVert0inA + a->vertPos[curVertA]);
	if (support < res->support)
	{
	res->mtv = normal;
	res->type = SatResult::EDGE_EDGE;
	res->support = support;
	res->edge1 = eA;
	res->edge2 = eB;

	if (support < 0)
	{
	// shapes aren't overlapping.

	//drawPoint(vecAdd(sphereOrigin, normal), COLOR_PURPLE);
	//SatVertex v = b->verts[curVertB];
	//for(int j = 0; j < v.numPortals; j++)
	//{
	// SatEdge e = b->edges[b->vertEdges[v.firstPortal + j].e];
	// drawArcBetween(sphereOrigin, b->facePlanes[e.f0].normal, b->facePlanes[e.f1].normal, 1.0f, COLOR_YELLOW, true);
	//}

	res->mtv = xfA.rotate(res->mtv);
	return;
	}
	}


	// relax both faces of A connected to the arc we intersected

	// There might be a way to do less work here

	// f0
	{
	size_t fa = eA.f0;
	Plane aPlane = a->facePlanes[fa];
	float dot_b0 = aPlane.normal.dot(bVert0inA);
	float dot_b1 = aPlane.normal.dot(bVert1inA);
	if (dot_b0 > aFaceMaxDot[fa])
	{
	aFaceMaxDot[fa] = dot_b0;
	aFaceToVertexRegionB[fa] = eB.v0;
	}
	if (dot_b1 > aFaceMaxDot[fa])
	{
	aFaceMaxDot[fa] = dot_b1;
	aFaceToVertexRegionB[fa] = eB.v1;
	}
	}

	// f1
	{
	size_t fa = eA.f1;
	Plane aPlane = a->facePlanes[fa];
	float dot_b0 = aPlane.normal.dot(bVert0inA);
	float dot_b1 = aPlane.normal.dot(bVert1inA);
	if (dot_b0 > aFaceMaxDot[fa])
	{
	aFaceMaxDot[fa] = dot_b0;
	aFaceToVertexRegionB[fa] = eB.v0;
	}
	if (dot_b1 > aFaceMaxDot[fa])
	{
	aFaceMaxDot[fa] = dot_b1;
	aFaceToVertexRegionB[fa] = eB.v1;
	}
	}


	// Now we keep continue starting from the intersection point we just hit.
	rayOrigin = normal;
	rayDir = rayEnd - rayOrigin;

	//drawPoint(vecAdd(sphereOrigin, normal), COLOR_RED);
	}
	else
	{
	// We didn't hit anything which means f1 is in the region curVertA
	break;
	}
	}

	{
	bFaceToVertexRegionA[eB.f1] = curVertA;

	// Find the plane of f1 in A space
	Plane bPlane = b->facePlanes[eB.f1];
	Vec3 bPlaneNormalInA = -bToA.rotate(bPlane.normal);
	float bPlaneDistInA = bPlane.dist - bPlaneNormalInA.dot(bToA.p);

	float support = rayEnd.dot(a->vertPos[curVertA]) + bPlaneDistInA;
	if(support < res->support)
	{
	res->mtv = rayEnd;
	res->type = SatResult::VERT_FACE;
	res->support = support;
	res->vert = curVertA;
	res->face = eB.f1;

	if (support < 0)
	{
	// shapes aren't overlapping.

	//drawPoint(sphereOrigin + rayEnd, COLOR_PURPLE);

	//SatVertex v = b->verts[curVertB];
	//for(int j = 0; j < v.numPortals; j++)
	//{
	// SatEdge e = b->edges[b->vertEdges[v.firstPortal + j].e];
	// drawArcBetween(sphereOrigin, b->facePlanes[e.f0].normal, b->facePlanes[e.f1].normal, 1.0f, COLOR_YELLOW, true);
	//}

	res->mtv = xfA.rotate(res->mtv);
	return;
	}
	}

	}
	}


	// Color any remaining faces of A that haven't been had their arcs intersected
	uint8_t queue[SAT_MAX];
	size_t queueLo = 0;
	size_t queueHi = 0;
	for (size_t i = 0; i < a->numFaces; i++)
	{
	if (aFaceToVertexRegionB[i] != 0xff)
	{
	queue[queueHi++] = i;
	}
	}

	while (queueLo < queueHi)
	{
	size_t f = queue[queueLo++];
	uint8_t region = aFaceToVertexRegionB[f];

	Vec3 bVertInA = -bToA.mul(b->vertPos[region]);

	SatShape::EdgeList face = a->faces[f];
	for (size_t i = 0; i < face.num; i++)
	{
	SatShape::Edge nbr = a->faceEdges[face.first + i];
	if (aFaceToVertexRegionB[nbr.f1] == 0xff)
	{
	queue[queueHi++] = nbr.f1;
	aFaceToVertexRegionB[nbr.f1] = region;
	aFaceMaxDot[nbr.f1] = a->facePlanes[nbr.f1].normal.dot(bVertInA);
	}
	}
	}

	for (size_t faceA = 0; faceA < a->numFaces; faceA++)
	{
	size_t vertB = aFaceToVertexRegionB[faceA];
	ASSERT(vertB != 0xff);
	Plane aPlane = a->facePlanes[faceA];

	float supp = aPlane.dist + aFaceMaxDot[faceA];
	if (supp < res->support)
	{
	res->mtv = aPlane.normal;
	res->type = SatResult::FACE_VERT;
	res->support = supp;
	res->vert = vertB;
	res->face = faceA;

	if (supp < 0)
	{
	res->mtv = xfA.rotate(res->mtv);
	return;
	}
	}


	}

	res->mtv = xfA.rotate(res->mtv);
	}
No results found