private Simplex GJK(Prism prismA, Prism prismB, Vector3 dir)
{
Simplex s = new Simplex();
//First point on the edge of the minkowski difference. 1-Simplex.\
s.Add(GetSupport(prismA, prismB, dir));
//Compute negative direction of d
directionVector = -dir;
while (true)
{
//Add the point to the simplex. 2-Simplex.
s.Add(GetSupport(prismA, prismB, directionVector));
if (Vector3.Dot(s.GetLast(), directionVector) <= 0)
{
//point does not pass origin so do not add it.
return(null);
}
else
{
//CheckOrigin automatically updates the directionVector on every call. If it doesnt that means we found the origin.
if (CheckOrigin(s, directionVector))
{
return(s);
}
}
}
}
private static bool ContainsOrigin(Simplex s, ref Fix64Vector2 dir)
{
var a = s.GetLast();
var AO = -a;
var b = s.GetB();
var AB = b - a;
if (s.Count() == 3)
{
var c = s.GetC();
var AC = c - a;
var abPerp = CalcNomal(AC, AB, AB);
var acPerp = CalcNomal(AB, AC, AC);
if (Fix64Vector2.Dot(abPerp, AO) > Fix64.Zero)
{
s.Remove(c);
dir = abPerp;
}
else
{
if (Fix64Vector2.Dot(acPerp, AO) > Fix64.Zero)
{
s.Remove(b);
dir = acPerp;
}
else
{
return(true);
}
}
}
else
{
var abPerp = CalcNomal(AB, AO, AB);
dir = abPerp;
}
return(false);
}
private Simplex GJK(Prism prismA, Prism prismB, Vector3 dir)
{
Simplex s = new Simplex
{
direction = dir
};
//First point on the edge of the minkowski difference. 1-Simplex.
s.Add(GetSupport(prismA, prismB, dir));
//s.points.ForEach(x => print(x));
//Point in opposite direction
s.direction = -s.direction;
while (true)
{
//Add the point to the simplex. 2-Simplex.
s.Add(GetSupport(prismA, prismB, s.direction));
if (Vector3.Dot(s.GetLast(), s.direction) <= 0)
{
//point does not pass origin so do not add it.
return(null);
}
else
{
//CheckOrigin automatically updates the direction parameter on every call. If it doesnt that means we found the origin.
if (CheckOrigin(s))
{
print("found origin");
return(s);
}
Debug.Log(prismA.name + " " + prismB.name);
}
}
}
public static bool GJK(MGFObject a, MGFObject b)
{
Simplex s = new Simplex();
Fix64Vector2 dir = a.GetPos() - b.GetPos();
s.Add(Support(a, b, dir));
dir = -dir;
while (true)
{
s.Add(Support(a, b, dir));
if (Fix64Vector2.Dot(s.GetLast(), dir) <= Fix64.Zero)
{
return(false);
}
else
{
if (ContainsOrigin(s, ref dir))
{
return(true);
}
}
}
}
private bool CheckOrigin(Simplex s)
{
// Count of points in simplex.
var simplexCount = s.numPoints;
// Get first point in simplex
var A = s.GetLast();
// Negate A
var A0 = -A;
if (simplexCount == 2)
{
// 2 points is a line
var B = s.points[0];
// Find AB
var AB = B - A;
// Find perpendicular of AB
var abPerp = TripleProduct(AB, A0, AB);
// set the new direction to perpendicular of AB so we can find another point along it and form a 3-Simplex (Triangle)
s.direction = abPerp;
if (s.direction.sqrMagnitude <= Mathf.Pow(10, 6))
{
s.direction = new Vector3(AB.z, 0.0f, -AB.x);
}
}
else if (simplexCount == 3)
{
// 3 points is a triangle.
// Find the rest of the points in the simplex.
var B = s.points[1];
var C = s.points[0];
// Find edges of triangle.
var AB = B - A;
var AC = C - A;
// Find each edge's perpendicular.
/* var abPerp = TripleProduct(AC, AB, AB);
* var acPerp = TripleProduct(AB, AC, AC);
*/
Vector3 acPerp = new Vector3();
float dot = AB.x * AC.z - AC.x * AB.z;
acPerp.x = -AC.z * dot;
acPerp.z = AC.x * dot;
// Check for origin with perp. of AB
if (Vector3.Dot(acPerp, A0) >= 0f)
{
Debug.Log("removing B" + B);
s.Remove(B);
// Set the new direction to perpendicular of AB so we can find a point that works, unlike C
s.direction = acPerp;
}
else
{
Vector3 abPerp = new Vector3();
abPerp.x = AB.z * dot;
abPerp.z = -AB.x * dot;
// Check for origin with perp. of AC
if (Vector3.Dot(abPerp, A0) >= 0f)
{
Debug.Log("removing C" + C);
s.Remove(C);
// Set the new direction to perpendicular of AC so we can find a point that works, unlike B.
s.direction = abPerp;
}
else
{
// Origin Found
return(true);
}
}
}
return(false);
}
private bool CheckOrigin(Simplex s, Vector3 dir)
{
// Returns true if we have a collision, false if we dont
// If we dont, updates the direction vector so that GJK can search for a new point to query.
// Count of points in simplex.
var simplexCount = s.pointCount;
// A is the last point in our simplex
var A = s.GetLast();
// Negate A
var A0 = -A;
if (simplexCount == 2)
{
// 2 points is a line
var B = s.GetLast();
// Find AB
var AB = B - A;
// Find perpendicular of AB
var abPerp = TripleProduct(AB, A0, AB);
// set the new direction to perpendicular of AB so we can find another point along it and form a 3-Simplex (Triangle)
directionVector = abPerp;
}
else if (simplexCount == 3)
{
// 3 points is a triangle.
// Find the rest of the points in the simplex.
var B = s.points[1];
var C = s.points[0];
// Find edges of triangle.
var AB = B - A;
var AC = C - A;
// Find each edge's perpendicular.
var abPerp = TripleProduct(AC, AB, AB);
var acPerp = TripleProduct(AB, AC, AC);
// Check for origin with perp. of AB
if (Vector3.Dot(abPerp, A0) > 0)
{
// remove point c
s.Remove(C);
// set the new direction to perpendicular of AB so we can find a point that works, unlike C
directionVector = abPerp;
return(false);
}
else
{
// Check for origin with perp. of AC
if (Vector3.Dot(acPerp, A0) > 0)
{
// Remove B from the simplex.
s.Remove(B);
// Set the new direction to perpendicular of AC so we can find a point that works, unlike B.
directionVector = acPerp;
return(false);
}
else
{
// Origin Found
return(true);
}
}
}
return(false);
}