Sign in to follow this  

Problem with my GJK implementation

This topic is 387 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

If you intended to correct an error in the post then please contact us.

Recommended Posts

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.

Share this post


Link to post
Share on other sites

Yes, I have a functional implementation in 3D. Link for download:

 

http://www.bulletphysics.org/Bullet/phpBB3/download/file.php?id=1477

 

Erin Catto's GDC presentation is also very intuitive to understand, you should take look at it.

 

BTW, you're not implementing GJK. The algorithm you showed doesn't find the closest points between two hulls, it just tells you whether they're overlapping or not. It is also called SAT-GJK. This wasn't very helpfull for me in the context of physics engines.

Edited by Irlan Robson

Share this post


Link to post
Share on other sites

GJK finds the closest points and with some effort you can find the closest features (see b3GJKFeaturePair.cpp). SAT-GJK  simply performs a boolean query and can become a little hard to debug because it doesn't have enough bookeeping for visualizing what leads to the final results (closest points). 

 

Of course the performance depends of implementation, but generally your algorithm should be faster and easier to implement since it doesn't keep track of the barycentric coordinates for each simplex vertex.

Share this post


Link to post
Share on other sites

GJK is used frequently in collision detection and resolution, for example. It is a must have when implementing a robust physics engine nowadays. Here is a fresh resource showing its usabiliy: 

 

http://box2d.org/files/GDC2015/DirkGregorius_Contacts.pdf

 

No SAT-GJK for you. You can copy GJK and simplify it. But again I'm not sure if is worth it :) .

Share this post


Link to post
Share on other sites

GJK is used frequently in collision detection and resolution, for example. It is a must have when implementing a robust physics engine nowadays. Here is a fresh resource showing its usabiliy: 

 

http://box2d.org/files/GDC2015/DirkGregorius_Contacts.pdf

 

No SAT-GJK for you. You can copy GJK and simplify it. But again I'm not sure if is worth it :) .

Why do you think it wont be worth it?It would be too time consuming? Do you know of any other algorithm to achieve this? Also, do you recommend me to instead use a physics library like bullet? 

Share this post


Link to post
Share on other sites

Because once you have a true, reliably, and optimized GJK implementation SAT-GJK becomes an unnecessary optimization.

 

Yes, alternatively there is SAT, which runs extremely fast on some shape pairs like a sphere or a capsule vs. a box for example. All of this is explained in Dirk's presentation in my last post. If you choose to go with SAT be prepared to learn some new topics.

 

No problem with using Bullet for collision detection, but you've already done a great job implementing SAT-GJK, so why not stick with this and only use Bullet if you need more than collision detection? :)  

Share this post


Link to post
Share on other sites

Actually Zaphyk I have implemented this optimization some years ago and found a copy in my old files. Be aware that this is totally not recommended for anything but studying due to the unecessary optimizations or readability. However at the time it just worked  :wink:. 

 

CGJK.h

CGJK.cpp

 

Hope that helps. 

Edited by Irlan Robson

Share this post


Link to post
Share on other sites

I've used SAT-GJK combined with EPA (Expanding polytope algorithm) for generating accurate contacts with success. It's actually not as slow as some would have you believe, I can easily simulate a few thousand objects in a large pile on one thread at 60fps with a brute-force broadphase. The main bottleneck is usually constraint solving, not collision. My implementation handles any type of convex shape, given a support function, and can refine the contact results to be within a user-defined epsilon of the actual contacts.

 

My implementations of these algorithms are pretty well commented/flexible and has been rewritten several times for performance/clarity. You can find it here (as part of om::physics::collision, sorry I don't have a direct link). (GJKSolver/EPASolver classes + supporting classes)

Share this post


Link to post
Share on other sites

Actually Zaphyk I have implemented this optimization some years ago and found a copy in my old files. Be aware that this is totally not recommended for anything but studying due to the unecessary optimizations or readability. However at the time it just worked  :wink:. 

 

CGJK.h

CGJK.cpp

 

Hope that helps. 

Thanks man, this really helps, can you please send me the CShapeInstance class so I can check the GetSupport method?

Share this post


Link to post
Share on other sites

The support function does exactly the same what the support function in your original post does but additionally returns the vertex index. 

Oh thanks, I just ported your implementation and it works! But I have some collision it's not detecting, I would like to know if it is a problem with my port or if you had this issue in your implementaion.

Share this post


Link to post
Share on other sites

No issue with my implementation at that time, but I remember only tested collisions between a box, sphere, and capsule. That code also assumes the shapes are convex and in the same space.

 

It can be many things. You definetly need to debug your code visually. One way is render the resulting tetrahedron in the case or overlap and check if the origin is really inside it or perhaps record all simplices built during the iterations and draw them. With the true GJK is even easier to debug because each iteration new closest points are found and they must become closer and closer after each iteration. Otherwise there is a bug in the code.

Edited by Irlan Robson

Share this post


Link to post
Share on other sites

This topic is 387 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

If you intended to correct an error in the post then please contact us.

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

Sign in to follow this