Sign in to follow this  
Dark_Bob

Help with my Swept Sphere Collision Detection

Recommended Posts

Dark_Bob    122
Hi, I'm a little stumped because I've written this collision detection and response and it seems to work but just every so often fail. I've yet to figure out exactly why it fails, I know it's not very efficient at the moment but then I'm just trying to get it to work well and can worry about efficiency later. Any help you can offer will be greatly appreciated. I'm sorry about the amount of code, I just wish collision detection didn't seem to required so much of it in the first place ;) Basically I first check the sphere against the plane, if this doesn't work I check whether the sphere straddles the plane and try to correct it (though I would have thought this shouldn't happen (floating point error has messed with my logic before and now its an easy scape goat)). After both of these have been tried, I check for edge collisions. Pack all theinfo into a struct and send it back easy (or so you'd think)... (PS: If you could help get it so I can drop the RAY_OFFSET I'd appreciate that it causes ugly juddering in the corners.)

#define EPSILON 0.001f

typedef struct
{
	// Passed info
	enum COLLISION_TYPE { RAY, SPHERE, ELIPSOID };
	COLLISION_TYPE type;
	float radius;
	CVector ellipsoid;
	// Returned info
	UID id;
	bool bCollisionFound;
	CVector planeNormal;
	CVector collisionPoint;
	float planeDist;
} SCollisionInfo;

CVector ClosestPointOnLine(CVector p, CVector a, CVector b)
{
	CVector result;
	CVector c,V;
	float t,d;

	// Determine t (the length of the vector from ‘a’ to ‘p’)

	// Get the lines from point a to the point p and the line a to b
	c = p - a;
	V = b - a;

	// d = length of V

	d = sqrt( pow(V.x,2) + pow(V.y,2) + pow(V.z,2));

	// normalize V

	V.Normalize();

	// Get the magnitude of c in the direction V
	t = V.DotProduct(c);

	// Check to see if ‘t’ is beyond the extents of the line segment

	if (t < 0.0f)	{ return (a); }
	if (t > d)	  {	return (b); }

	// Return the point between ‘a’ and ‘b’

	V *= t;

	result = a + V;

	return result;
}

CVector ClosestPointOnTriangle(CVector p, CVector a, CVector b, CVector c)
{
	CVector Rab, Rca, Rbc, result;
	float dab, dca, dbc, min;

	Rab = ClosestPointOnLine(p, a, b);
	Rbc = ClosestPointOnLine(p, b, c);
	Rca = ClosestPointOnLine(p, c, a);

	dab = pow((p.x-Rab.x),2) + pow((p.y-Rab.y),2) + pow((p.z-Rab.z),2);
	dbc = pow((p.x-Rbc.x),2) + pow((p.y-Rbc.y),2) + pow((p.z-Rbc.z),2);
	dca = pow((p.x-Rca.x),2) + pow((p.y-Rca.y),2) + pow((p.z-Rca.z),2);

	min = dab;
	result = Rab;

	if (dbc < min)
	{
		min = dbc;
		result = Rbc;
	}

	if (dca < min)
		result = Rca;

	return (result);
}

bool CheckPointInTriangle(CVector point ,CVector p1, CVector p2, CVector p3)
{
	//point.z = p1.z = p2.z = p3.z = 0.f;

	CVector v1 = p1-point;
	CVector v2 = p2-point;
	CVector v3 = p3-point;

	v1.Normalize();
	v2.Normalize();
	v3.Normalize();

	float angle = acos(v1.DotProduct(v2));
	angle += acos(v2.DotProduct(v3));
	angle += acos(v3.DotProduct(v1));

	return (fabs(angle-2*PI) <= EPSILON);
}

float intersectRayPlane(CVector rayOrigin, CVector rayVector, CVector pOrigin, CVector pNormal)
{
	// The ray vector must be normalised
	float d = - (pNormal.DotProduct(pOrigin));
	float numer = pNormal.DotProduct(rayOrigin) + d; // This is the signed distance to the plane from the rayOrigin
	float denom = pNormal.DotProduct(rayVector);
  
	if (denom > 0)  // normal is orthogonal to vector, cant intersect
		return (-1.0f);
	return -(numer / denom);
}

float IntersectRaySphere(CVector p, CVector normalizedRay, CVector s, float radius)
{	
   CVector diff = s-p;
   // c is the distance to the sphere origin from the point
   float c = diff.Length();
   // v is the magnitude of the normalised ray in the direction of the sphere origin squared
   float v = diff.DotProduct(normalizedRay);
   float d = radius*radius - (c*c - v*v);

   // If there was no intersection, return -1
   if (d < 0.0) return (-1.0f);

   // Return the distance to the [first] intersecting point
   return (v - sqrt(d));
}

int ClassifyPoint(CVector point, CVector pN, CVector pO)
{
	CVector dir;
	float d;

	dir = pO - point;

	d = dir.DotProduct(pN);

	if (d<-0.001f)
		return 1;	// FRONT
	else
	if (d>0.001f)
		return 0;	//BACKSIDE

	return 1; // Hmmm...
}

SCollisionInfo SphereTriangleIntersection(CVector orig, CVector ray, float radius,
                   CVector vert0, CVector vert2, CVector vert1)
{
	SCollisionInfo colInfo;

	CVector v1 = vert1 - vert0;
	CVector v2 = vert2 - vert0;

	CVector pNormal = v1.CrossProduct(v2);
	pNormal.Normalize();

	CVector origin = orig, tRay, tVert0 = vert0, tVert1 = vert1, tVert2 = vert2;

	origin -= pNormal*radius;

	colInfo.bCollisionFound = false;

	// This will return what percentage of the ray's magnitude is the distance 
	// to the plane 1.0f is on the plane, 1.5 is 1.5 times the rays length
	float distanceToPlane = intersectRayPlane(origin, ray, vert0, pNormal);

	if ((distanceToPlane > 0.f)&&(distanceToPlane < 1.f))
	//if ((ClassifyPoint(orig, vert0, pNormal))&&(distanceToPlane < 1.f))
	{
		tRay = ray * distanceToPlane;
		origin += tRay;

		colInfo.bCollisionFound = CheckPointInTriangle(origin , tVert0, tVert1, tVert2);
		colInfo.planeDist = tRay.Length()-RAY_OFFSET;
		colInfo.planeNormal = pNormal;
	}

	if ((!colInfo.bCollisionFound)&&
		(ClassifyPoint(orig, pNormal, vert0))&&(!ClassifyPoint(orig-pNormal*radius, pNormal, vert0)))
	{
		tRay = ray * distanceToPlane;
		origin += tRay;

		colInfo.bCollisionFound = CheckPointInTriangle(origin , tVert0, tVert1, tVert2);
		colInfo.planeDist = tRay.Length()-RAY_OFFSET;
		colInfo.planeNormal = pNormal;
	}

	if (!colInfo.bCollisionFound)
	{
		float rayMagnitude = ray.Length();
		ray.Normalize();

		CVector normalizedVelocity = ray;

		CVector closestPoint = ClosestPointOnTriangle(orig, vert0, vert1, vert2);	

		float distToSphereIntersection = IntersectRaySphere(closestPoint, -normalizedVelocity, orig, radius);

		// we cannot know if the ray will actually hit the sphere so we need this check
		//if ((distToSphereIntersection > 0)&&(distToSphereIntersection < radius)) {
		if ((distToSphereIntersection > 0)&&(distToSphereIntersection < rayMagnitude)) {
			// calculate true sphere intersection point
			//CVector normal = orig-closestPoint;
			CVector normal = orig-(closestPoint+(-normalizedVelocity*distToSphereIntersection));
			normal.Normalize();
			colInfo.bCollisionFound = true;
			colInfo.collisionPoint = closestPoint;
			colInfo.planeNormal = normal;
			colInfo.planeDist = distToSphereIntersection-RAY_OFFSET;
		}
	}

	return colInfo;
}

Then there was the response to consider, basically I measure the magnitude of the vector that describes the desired position (hereafter refered to as the velocity of simplicity's sake) that is beyond the collision point along the collision plane normal and subtract this from the original velocity to get the new position with a little bit of sliding.

colInfo.bCollisionFound = true;

CVector orig(objs[i]->xForm.pos);
CVector velocity(objs[i]->velocity);

while (colInfo.bCollisionFound)
{
	colInfo.bCollisionFound = false;

	level->CheckCollision(&orig, &velocity, colInfo);

	if (colInfo.bCollisionFound)
	{
		// Get the normalised velocity
		CVector normalisedVel = velocity;
		normalisedVel.Normalize();

		// The section of the velocity vector past the collision
		velocity = normalisedVel*(velocity.Length()-colInfo.planeDist);
		orig += normalisedVel*colInfo.planeDist;

		// Get the magnitude of the velocity vector 
		// along the negative normal that is past the collision point
		CVector velNormalMagnitude = -colInfo.planeNormal*(colInfo.planeNormal.DotProduct(velocity));

		// Adjust the player velocity vector by the magnitude of the velocity vector 
		// along the negative normal that is past the collision point
		objs[i]->velocity += velNormalMagnitude;

		// Adjust the velocity vector by the magnitude of the velocity vector along 
		// the negative normal (This should now be perpendicular to the collision 
		// plane)
		velocity += velNormalMagnitude;
	}
}


If you've gotten this far thanks for taking the time to read through. If you can help please do, if I can return the favour in any way I will. If you use this source to create a collision detection system of your own at least have the decency to credit me and if you use my code, fix the bug and don't tell me I hope the guilt gets the better of you in the end. Thanks, Andy

Share this post


Link to post
Share on other sites
jyk    2094
From your code it looks like you're working from Kasper Fauerby's paper, correct? I implemented a system based on that paper. I also added some stuff to deal with jiggling in obtuse corners and getting stuck in acute corners.

I added static collision resolution at the end of each step to deal with any weird circumstances or floating point error. That involved some code that isn't in the article, i.e. finding the closest point between a point (the center of the sphere) and a triangle. For that I used the algorithm from Dave Eberly's book 'Game Engine Design'.

Anyway, don't have time to look at the code, but do you have any sort of console or real-time error logging? I found it was very helpful to post messages about what was going on, i.e. 'velocity very small', 'velocity parallel to edge', 'a < epsilon in FindRoots()', that sort of thing. It might help you track down the source of the problem.

If floating point error is leaving you intersecting a triangle at the end of the step, you may be falling through it next time - that's my best guess about the problem you're having.

If you really get stuck, let me know and I'll be glad to share my code.

Share this post


Link to post
Share on other sites
wendigo23    512
I was just doing this a week or two ago and was running into problems where it would fail. Took me a day or two to understand why. The method you are using (I did look at it line by line) and that I was using (that I was basing on some papers I found on the web) is inherently flawed. It does not work when approaching edges from an angle. The issue arises from the step where you clamp the point, if outside the triangle, to be on one of the edges. You move the point in a line straight toward the edge, but look at a sphere, it is round. The point you find that way is not the intersection point if the sphere is moving at an angle, it also needs to move slightly to the side along the edge.

Personally, I decided to throw swept-sphere/triangle out the window and just go with the much simpler static sphere/triangle. Sure, you have to worry about tunneling (my objects don't move that fast, so I don't worry about it) but it is a much simpler test, and the method I use to react to a colission is also much much simpler (just push the sphere out of the triangle).

Share this post


Link to post
Share on other sites
jyk    2094
Quote:
The point you find that way is not the intersection point if the sphere is moving at an angle, it also needs to move slightly to the side along the edge.


Are you (the OP) using Paul Nettle's original paper? Or the updated Kasper Fauerby paper (which is what I used)? From what I understand, the original paper does have some issues.

Share this post


Link to post
Share on other sites
Dark_Bob    122
Thanks jyk and thank you wendigo23. I'll start by addressing wendigo23's comments.

Quote:
Original post by wendigo23
The method you are using (I did look at it line by line) and that I was using (that I was basing on some papers I found on the web) is inherently flawed. It does not work when approaching edges from an angle.


You need to use a sperate check for edge collisions. One simple method (that I used) is to fire a ray along the reversed velocity vector from the closest point on the edge of the triangle and see if this intersects the sphere of your object.

Quote:
Original post by wendigo23
Personally, I decided to throw swept-sphere/triangle out the window ....(my objects don't move that fast, so I don't worry about it)


I want my game objects to move very fast, so I'm keen to get this working. Thanks for the advice though.

Quote:
Original post by jyk
From your code it looks like you're working from Kasper Fauerby's paper, correct?


Sort of, I had a few ideas on how to do swept sphere CD and started going about it my way which was fine till until I implememented it and found that I had completely neglected any form of edge detection. Thats when I started looking on the web for some good solutions and found Kasper Fauerby's paper. If you look at the last function in the first set of code called SphereTriangleIntersection(..) you'll see the main code that (attempts to) detect the collision.

Quote:
Original post by jyk
I implemented a system based on that paper. I also added some stuff to deal with jiggling in obtuse corners and getting stuck in acute corners.


I haven't even begun to seriously worry about fixing the asthetics such as jiggling in the corners but I noticed that that was only really introduced when I added in the RAY_OFFEST but this seems kind of important because without it my dude often just falls straight through the floor and goes through walls quite a bit.

Quote:
Original post by jyk
I added static collision resolution at the end of each step to deal with any weird circumstances or floating point error. That involved some code that isn't in the article, i.e. finding the closest point between a point (the center of the sphere) and a triangle. For that I used the algorithm from Dave Eberly's book 'Game Engine Design'.

If floating point error is leaving you intersecting a triangle at the end of the step, you may be falling through it next time - that's my best guess about the problem you're having.


That last comment is exactly what I think is causing the problem. I have a seperate check before the edge collision where I check if the sphere stradles the plane and if it does I check if its inside the triangle and I push it out infront of the plane again. Though I have a feeling that this might be where the problem is but I'm at a bit of a loss as to what to do.

Quote:
Original post by jyk
Anyway, don't have time to look at the code, but do you have any sort of console or real-time error logging? I found it was very helpful to post messages about what was going on, i.e. 'velocity very small', 'velocity parallel to edge', 'a < epsilon in FindRoots()', that sort of thing. It might help you track down the source of the problem.

If you really get stuck, let me know and I'll be glad to share my code.


I've got a translucent console that displays debug info and a log but the fail case seems kind of random and was reduced by slightly reducing the distance to plane which is what makes me think its the floating point error thats giving me the issues. I first had problems with it when I couldn't figure out why my camera wobbled. I'm not familiar with the algorithm from Dave Eberly's book 'Game Engine Design' and if you wouldn't mind sharing your code I'd really appreciate it (It's always easier to get something to work when you've got a working example to look at and follow through:).

Thanks again for your help,
Andy

Share this post


Link to post
Share on other sites
SimmerD    1210
I implemented swept sphere collision based on Igor's code from flipcode last year. I had similar 'thru the floor' problems. The solution was to ensure that the object never got closer than 'epsilon' to any ray or face or edge.

The think I don't handle yet and that can still screw things up is when multiple triangles are collided with in the same frame ( room interior corners ). Moving epsilon away from just one feature is not enough. You need to be epsilon away from all features to avoid problems.

Share this post


Link to post
Share on other sites

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