Expanding Polytope Algorithm in 3D Confusion.

Hello, I am currently trying to implement the Expanding Polytope Algorithm in 3D space using a resulting simplex from the GJK algorithm and I am failing. If someone could please point me in the right direction I would appreciate it.

I have followed the Handmade Hero Twitch stream since the beginning (I admit to missing about 1/4 of the streams) but this is my first time visiting the forums. So I apologize if this post is inappropriate by the fact that it does not directly pertain to the Handmade Hero game. I tried to find a code of conduct for posting on the forums but I did not find one. However, I did notice other posts with general coding questions so I am assuming this is an acceptable post.

Most of my knowledge of the algorithm has come from this tutorial:
http://www.dyn4j.org/2010/05/epa-expanding-polytope-algorithm/

I am also assuming that my implementation of the GJK algorithm works and it follows Casey's version here:
https://mollyrocket.com/849

My main problem is that I am having trouble visualizing my code to find out what is going on. Currently I am trying to debug by displaying the resulting face's normal vector to tell if it is in the correct direction and it seems to only sometimes be in the correct direction depending on the collision point.

Here is the correct normal when standing on a platform with only gravity acting as an external force to the object:

Here is the incorrect normal when i take a few steps to the left:

Here is the current state of my code:

Note: The MinkowskiSupport function is the same support function I used for the GJK algorthm. Both the GJK algorithm and the MinkowskiSupport function appear to work as they should, so I have not posted them to keep the code shorter. Also, v3 is a vector with three float values.

struct SupportPoint
{
	v3 MinkowskiPoint;
	v3 ObjAPoint;
	v3 ObjBPoint;

	SupportPoint & operator=(SupportPoint& B)
	{
		MinkowskiPoint = B.MinkowskiPoint;
		ObjAPoint = B.ObjAPoint;
		ObjBPoint = B.ObjBPoint;
		return *this;
	}

	bool operator==(SupportPoint& B)
	{
		if ((MinkowskiPoint == B.MinkowskiPoint) &&
			(ObjAPoint == B.ObjAPoint) &&
			(ObjBPoint == B.ObjBPoint))
		{
			return true;
		}
		else
		{
			return false;
		}
	}
};

struct Face
{
	SupportPoint Vertices[3];
	v3 Normal;
	float OriginDistance;

	Face & operator=(Face& B)
	{
		Vertices[0] = B.Vertices[0];
		Vertices[1] = B.Vertices[1];
		Vertices[2] = B.Vertices[2];
		Normal = B.Normal;
		OriginDistance = B.OriginDistance;
		return *this;
	}
};

Face AddFaceEPA(SupportPoint &v0, SupportPoint &v1, SupportPoint &v2)
{
	Face Result;
	Result.Vertices[0] = v0;
	Result.Vertices[1] = v1;
	Result.Vertices[2] = v2;
	Result.Normal = CrossProduct(Result.Vertices[1].MinkowskiPoint - Result.Vertices[0].MinkowskiPoint,
		Result.Vertices[2].MinkowskiPoint - Result.Vertices[1].MinkowskiPoint);
	Result.OriginDistance = InnerProduct(Result.Normal,
		-Result.Vertices[0].MinkowskiPoint) / Magnitude(Result.Normal);

	if (Result.OriginDistance < 0) // To fix errors in wrapping order
	{							 
		Result.OriginDistance = -Result.OriginDistance;
		Result.Normal = -Result.Normal;
	}

	return Result;
}

Note: The SimplexPoints parameter of the function is always an array of four SupportPoints given by the GJK algorithm. The two ObjectMap parameters are only used in the MinkowskiSupport function to get the vertices of the colliding objects.

Face EPA(SupportPoint *SimplexPoints, ObjectMap *ObjA, ObjectMap *ObjB)
{

	Face Faces[12]; //Temporary (The objects are boxes that will have no more than 12 faces in this test)
	uint32 FaceCount = 4; // Temporary, SimplexPoints always comes in with 4 vertices
	uint32 FinalFace = 0; // Temporary, Store the final closest face position in the array

	Faces[0] = AddFaceEPA(SimplexPoints[0], SimplexPoints[1], SimplexPoints[2]);
	Faces[1] = AddFaceEPA(SimplexPoints[1], SimplexPoints[2], SimplexPoints[0]);
	Faces[2] = AddFaceEPA(SimplexPoints[2], SimplexPoints[0], SimplexPoints[1]);
	Faces[3] = AddFaceEPA(SimplexPoints[0], SimplexPoints[1], SimplexPoints[2]);

	float FinalDistance = Faces[0].OriginDistance; // A random choice start value

	for (uint32 i = 0; i < FaceCount; i++) // Find the closest face to the origin
	{
		if (Faces[i].OriginDistance < FinalDistance)
		{
			FinalDistance = Faces[i].OriginDistance;
			FinalFace = i;
		}
	}

	while (1)
	{
		SupportPoint NewPoint = MinkowskiSupport(ObjA, ObjB, Faces[FinalFace].Normal);

		for (unsigned int i = 0; i < FaceCount; i++)
		{
			if ((Faces[i].Vertices[0] == NewPoint) || // If the new point is already a part of the 
				(Faces[i].Vertices[1] == NewPoint) || // simplex then return the current closest face.
				(Faces[i].Vertices[2] == NewPoint))
			{
				return Faces[FinalFace];
			}
		}

		float D = InnerProduct(NewPoint.MinkowskiPoint, Faces[FinalFace].Normal);

		if ((D - Faces[FinalFace].OriginDistance) < 0.00001) // 0.00001 is the tolerance
		{
			return Faces[FinalFace];
		}
		else
		{
			// Add 3 new faces using the new point, the last one replacing the old face slot in the array.
			Faces[FaceCount] = AddFaceEPA(NewPoint, Faces[FinalFace].Vertices[1], Faces[FinalFace].Vertices[2]);
			FaceCount++;
			Faces[FaceCount] = AddFaceEPA(Faces[FinalFace].Vertices[0], NewPoint, Faces[FinalFace].Vertices[2]);
			FaceCount++;
			Faces[FinalFace] = AddFaceEPA(Faces[FinalFace].Vertices[0], Faces[FinalFace].Vertices[1], NewPoint);
	
			for (unsigned int i = 0; i < FaceCount; i++)
			{
				if (Faces[i].OriginDistance < FinalDistance) // Find the closest face to the origin
				{
					FinalDistance = Faces[i].OriginDistance;
					FinalFace = i;
				}
			}
		}
	}
}

I apologize if the code is messy, I do not have a lot of programming experience. This is my first significant program that I have made.

Thank you.