Hello, I tried implementing the GJK algorithm for convex collision detection by using this video and this page but my implementation doest work correctly, I assume there is something wrong around the 4th case (Tetrahedron), here is my code:
private static Vector3 Support(List<Vector3> Vertices, Vector3 Direction){
float highest = float.MinValue;
Vector3 support = Vector3.Zero;
for (int i = 0; i < Vertices.Count; ++i) {
Vector3 v = Vertices[i];
float dot = Vector3.Dot(Direction, v);
if (dot > highest) {
highest = dot;
support = v;
}
}
return support;
}
public static bool Collides(CollisionShape Shape1, CollisionShape Shape2){
Vector3 D = Vector3.UnitX;
Vector3 S = Support(Shape1.Vertices, D) - Support(Shape2.Vertices,-D);
List<Vector3> Points = new List<Vector3>();
Points.Add(S);
D = -S;
int Steps = 0;
while(Steps < 50){
Vector3 A = Support(Shape1.Vertices, D) - Support(Shape2.Vertices,-D);
Points.Add(A);
if( Vector3.Dot(Points.Last(), D) < 0 )
return false;
if( ContainsOrigin(Points, ref D) )
return true;
Steps++;
}
return false;
}
private static Vector3 TripleProduct(Vector3 A, Vector3 B, Vector3 C){
return Vector3.Cross( Vector3.Cross(A,B), C );
}
private static bool ContainsOrigin(List<Vector3> Array, ref Vector3 Direction){
if(Array.Count == 4)
return Tetrahedron(Array, ref Direction);
Vector3 A = Array.Last();
Vector3 AO = -A;
if(Array.Count == 3){
Vector3 B = Array[1];
Vector3 C = Array[0];
Vector3 AB = B - A;
Vector3 AC = C - A;
Vector3 ABC = Vector3.Cross(AB, AC);
if( Vector3.Dot(Vector3.Cross(ABC, AC), AO) > 0){
if(Vector3.Dot(AC, AO) > 0){
Array.Remove(B);
Direction = TripleProduct(AC,AO,AC);
}else{
if(Vector3.Dot(AB, AO) > 0){
Array.Remove(C);
Direction = TripleProduct(AB,AO,AB);
}else{
Array.Remove(B);
Array.Remove(C);
Direction = AO;
}
}
}else{
if(Vector3.Dot( Vector3.Cross(ABC, AB), AO) > 0 ){
if(Vector3.Dot(AB, AO) > 0){
Array.Remove(C);
Direction = TripleProduct(AB,AO,AB);
}else{
Array.Remove(B);
Array.Remove(C);
Direction = AO;
}
}else{
if( Vector3.Dot(ABC, AO) > 0){
Direction = ABC;
}else{
Vector3 C0 = Array[0];
Array[0] = Array[1];
Array[1] = C0;
Direction -= ABC;
}
}
}
}else{
Vector3 B = Array[0];
Vector3 AB = B - A;
if(Vector3.Dot(AB, AO) > 0)
Direction = TripleProduct(AB, AO, AB);
else
Direction = AO;
}
return false;
}
private static bool Tetrahedron(List<Vector3> Array, ref Vector3 Direction){
Vector3 AO = -Array.Last();
Vector3 AB = Array[2] - Array.Last();
Vector3 AC = Array[1] - Array.Last();
Vector3 ABC = Vector3.Cross(AB, AC);
//CASE 1
if (Vector3.Dot(AO, ABC) > 0)
{
CheckTetrahedron(Array, AO, AB, AC, ABC, Direction);
}
//CASE 2:
Vector3 AD = Array.First() - Array.Last();
Vector3 ACD = Vector3.Cross(AC, AD);
if (Vector3.Dot(ACD,AO) > 0)
{
//in front of triangle ACD
Array[2] = Array[1];
Array[1] = Array[0];
AB = AC;
AC = AD;
ABC = ACD;
CheckTetrahedron(Array, AO, AB, AC, ABC, Direction);
}
Vector3 ADB = Vector3.Cross(AD, AB);
//same direaction with ao
if (Vector3.Dot(ADB, AO) > 0)
{
//in front of triangle ADB
Array[1] = Array[2];
Array[2] = Array[0];
AC = AB;
AB = AD;
ABC = ADB;
CheckTetrahedron(Array, AO, AB, AC, ABC, Direction);
}
//origin in tetrahedron
return true;
}
private static bool CheckTetrahedron(List<Vector3> Array, Vector3 AO, Vector3 AB, Vector3 AC, Vector3 ABC, Vector3 Direction)
{
Vector3 ab_abc = Vector3.Cross(AB, ABC);
if (Vector3.Dot(AO, ab_abc) > 0)
{
Array.RemoveAt(0);
Direction = Vector3.Cross(Vector3.Cross(AB, AO), AB);
return false;
}
Vector3 acp = Vector3.Cross(ABC, AC);
if (Vector3.Dot(AO,acp) > 0)
{
Array.RemoveAt(0);
Direction = Vector3.Cross(Vector3.Cross(AC, AO), AC);
return false;
}
Array.RemoveAt(Array.Count-1);
Direction = ABC;
return false;
}
Does anyone has a functional, comented 3d implementation I can take a look at or know where the problem is?
Thanks for the help.
