» Useless Snippet #5: Ray/Capsule intersection test

Goal: Check if a ray intersects a capsule.


Restrictions:


1. Ray is defined by a point and a unit vector (Origin, Direction)
2. Capsule is defined by two points and a radius (A, B, r)
3. If there is an intersection, we must calculate the actual points of impact as well as the normals at those points


Theory
A picture is like a thousand words, they say, so here is the general case of our problem.

Ray/Capsule Intersection test (General case)

The problem we are going to solve is to find whether points P1 and P2 exist, and if they do, what is their value and the normals of the capsule at those points. From now on we’ll be treating them as one point, which we will call P. As it will become evident later on, the equations that arise are quadratic and they will let us calculate both of them at once.

Observation #1: Point P is on the ray. In other words, the following equations are true:

Point P on ray (O, d)

Observation #2: If there is an intersection between the ray and the infinite cylinder defined by (A, B), there exist a point K on AB for which the following equations are true:

Equations for K

Equation (I) says that point K is the projection of P on the AB line, and as a result AK is perpendicular to KP. Equation (II) gives us the magnitude of KP, which is equal to the radius of the capsule, because K is on the AB line and P is on the capsule.

Observation #3: Since K is on the AB line, it can be expressed by the following equations:

Point K on line (A, B)

Substituting equations (1) – (6) into equation (I), reordering and solving for t’ gives the following relation between t’ and t:

Substituting equations (1) – (7) info equation (II), squaring, reordering and solving for t gives the following quadratic equation:

If equation (8) has solutions, those correspond to the distances from the ray’s origin to points P1 and P2 on the capsule. Otherwise the ray doesn’t intersect the infinite cylinder defined by the line AB, and as a result it doesn’t hit the capsule.
Using equation (7) we can calculate 2 values for t’ (one for each t we got from equ. (8)), which correspond to K1 and K2 respectively. In order for the intersection points P1 and P2 to be valid (in other words, to be on the cylindrical part of the capsule), the two t’ values should be in the [0, 1] range. This is because the AB vector isn’t normalized. If your capsule definition is different, such as (A, d, length), then t’ should be in the [0, length] range. If t’ is less than 0 then there might be an intersection with the sphere around point A with radius equal to the capsule’s radius. If t’ is greater than 1, then the potential intersection would be with the sphere (B, r). In those cases, a simple ray/sphere intersection test would be able to calculate the correct point and normal on the sphere.

Let’s take for example the case shown below.

Ray/Capsule intersection (negative t')

Point K1, as it is calculated using the above equations, has a negative t’ (it is behind A). As you can see, there is an intersection with the infinite cylinder (E1) but it’s not on the capsule. Calculating the intersection between the ray and the sphere (A, r) gives us the true intersection point P1, between the ray and the capsule.

Now lets see some code.

Structs and helper functions

// Very simple Vector3f class supporting only the operations needed by the code below.
class CVector3f
{
public:
	float x, y, z;
 
	CVector3f()
	{}
 
	CVector3f(float xx, float yy, float zz) : x(xx), y(yy), z(zz)
	{}
 
	~CVector3f()
	{}
 
	CVector3f operator + (const CVector3f& other) const
	{
		return CVector3f(x + other.x, y + other.y, z + other.z);
	}
 
	CVector3f operator - (const CVector3f& other) const
	{
		return CVector3f(x - other.x, y - other.y, z - other.z);
	}
 
	CVector3f operator * (float scale) const
	{
		return CVector3f(x * scale, y * scale, z * scale);
	}
 
	float Dot(const CVector3f& other) const
	{
		return x * other.x + y * other.y + z * other.z;
	}
 
	void Normalize(void)
	{
		float mag_sqr = x * x + y * y + z * z;
		if(mag_sqr > 1e-12f)
		{
			float inv_mag = 1.0f / sqrtf(mag_sqr);
			x *= inv_mag;
			y *= inv_mag;
			z *= inv_mag;
		}
		else
		{
			x = 0.0f;
			y = 0.0f;
			z = 0.0f;
		}
	}
};
 
// A ray based on restriction #1
struct Ray
{
	CVector3f m_Origin;
	CVector3f m_Direction;
};
 
// A capsule based on restriction #2
struct Capsule
{
	CVector3f m_A;
	CVector3f m_B;
	float m_Radius;
};
 
// A simple sphere (helper struct)
struct Sphere
{
	CVector3f m_Center;
	float m_Radius;
};
 
// NOTE: This function doesn't calculate the normal because it's easily derived for a sphere (p - center).
bool IntersectRaySphere(const Ray& ray, const Sphere& sphere, float& tmin, float& tmax)
{
	CVector3f CO = ray.m_Origin - sphere.m_Center;
 
	float a = ray.m_Direction.Dot(ray.m_Direction);
	float b = 2.0f * CO.Dot(ray.m_Direction);
	float c = CO.Dot(CO) - (sphere.m_Radius * sphere.m_Radius);
 
	float discriminant = b * b - 4.0f * a * c;
	if(discriminant < 0.0f) 		return false; 	tmin = (-b - sqrtf(discriminant)) / (2.0f * a); 	tmax = (-b + sqrtf(discriminant)) / (2.0f * a); 	if(tmin > tmax)
	{
		float temp = tmin;
		tmin = tmax;
		tmax = temp;
	}
 
	return true;
}

IntersectRayCapsule()

bool IntersectRayCapsule(const Ray& ray, const Capsule& capsule, CVector3f& p1, CVector3f& p2, CVector3f& n1, CVector3f& n2)
{
	// Substituting equ. (1) - (6) to equ. (I) and solving for t' gives:
	//
	// t' = (t * dot(AB, d) + dot(AB, AO)) / dot(AB, AB); (7) or
	// t' = t * m + n where 
	// m = dot(AB, d) / dot(AB, AB) and 
	// n = dot(AB, AO) / dot(AB, AB)
	//
	CVector3f AB = capsule.m_B - capsule.m_A;
	CVector3f AO = ray.m_Origin - capsule.m_A;
 
	float AB_dot_d = AB.Dot(ray.m_Direction);
	float AB_dot_AO = AB.Dot(AO);
	float AB_dot_AB = AB.Dot(AB);
 
	float m = AB_dot_d / AB_dot_AB;
	float n = AB_dot_AO / AB_dot_AB;
 
	// Substituting (7) into (II) and solving for t gives:
	//
	// dot(Q, Q)*t^2 + 2*dot(Q, R)*t + (dot(R, R) - r^2) = 0
	// where
	// Q = d - AB * m
	// R = AO - AB * n
	CVector3f Q = ray.m_Direction - (AB * m);
	CVector3f R = AO - (AB * n);
 
	float a = Q.Dot(Q);
	float b = 2.0f * Q.Dot(R);
	float c = R.Dot(R) - (capsule.m_Radius * capsule.m_Radius);
 
	if(a == 0.0f)
	{
		// Special case: AB and ray direction are parallel. If there is an intersection it will be on the end spheres...
		// NOTE: Why is that?
		// Q = d - AB * m =>
		// Q = d - AB * (|AB|*|d|*cos(AB,d) / |AB|^2) => |d| == 1.0
		// Q = d - AB * (|AB|*cos(AB,d)/|AB|^2) =>
		// Q = d - AB * cos(AB, d) / |AB| =>
		// Q = d - unit(AB) * cos(AB, d)
		//
		// |Q| == 0 means Q = (0, 0, 0) or d = unit(AB) * cos(AB,d)
		// both d and unit(AB) are unit vectors, so cos(AB, d) = 1 => AB and d are parallel.
		// 
		Sphere sphereA, sphereB;
		sphereA.m_Center = capsule.m_A;
		sphereA.m_Radius = capsule.m_Radius;
		sphereB.m_Center = capsule.m_B;
		sphereB.m_Radius = capsule.m_Radius;
 
		float atmin, atmax, btmin, btmax;
		if(	!IntersectRaySphere(ray, sphereA, atmin, atmax) ||
			!IntersectRaySphere(ray, sphereB, btmin, btmax))
		{
			// No intersection with one of the spheres means no intersection at all...
			return false;
		}
 
		if(atmin < btmin) 		{ 			p1 = ray.m_Origin + (ray.m_Direction * atmin); 			n1 = p1 - capsule.m_A; 			n1.Normalize(); 		} 		else 		{ 			p1 = ray.m_Origin + (ray.m_Direction * btmin); 			n1 = p1 - capsule.m_B; 			n1.Normalize(); 		} 		if(atmax > btmax)
		{
			p2 = ray.m_Origin + (ray.m_Direction * atmax);
			n2 = p2 - capsule.m_A;
			n2.Normalize();
		}
		else
		{
			p2 = ray.m_Origin + (ray.m_Direction * btmax);
			n2 = p2 - capsule.m_B;
			n2.Normalize();
		}
 
		return true;
	}
 
	float discriminant = b * b - 4.0f * a * c;
	if(discriminant < 0.0f) 	{ 		// The ray doesn't hit the infinite cylinder defined by (A, B). 		// No intersection. 		return false; 	} 	float tmin = (-b - sqrtf(discriminant)) / (2.0f * a); 	float tmax = (-b + sqrtf(discriminant)) / (2.0f * a); 	if(tmin > tmax)
	{
		float temp = tmin;
		tmin = tmax;
		tmax = temp;
	}
 
	// Now check to see if K1 and K2 are inside the line segment defined by A,B
	float t_k1 = tmin * m + n;
	if(t_k1 < 0.0f) 	{ 		// On sphere (A, r)... 		Sphere s; 		s.m_Center = capsule.m_A; 		s.m_Radius = capsule.m_Radius; 		float stmin, stmax; 		if(IntersectRaySphere(ray, s, stmin, stmax)) 		{ 			p1 = ray.m_Origin + (ray.m_Direction * stmin); 			n1 = p1 - capsule.m_A; 			n1.Normalize(); 		} 		else  			return false; 	} 	else if(t_k1 > 1.0f)
	{
		// On sphere (B, r)...
		Sphere s;
		s.m_Center = capsule.m_B;
		s.m_Radius = capsule.m_Radius;
 
		float stmin, stmax;
		if(IntersectRaySphere(ray, s, stmin, stmax))
		{
			p1 = ray.m_Origin + (ray.m_Direction * stmin);
			n1 = p1 - capsule.m_B;
			n1.Normalize();
		}
		else 
			return false;
	}
	else
	{
		// On the cylinder...
		p1 = ray.m_Origin + (ray.m_Direction * tmin);
 
		CVector3f k1 = capsule.m_A + AB * t_k1;
		n1 = p1 - k1;
		n1.Normalize();
	}
 
	float t_k2 = tmax * m + n;
	if(t_k2 < 0.0f) 	{ 		// On sphere (A, r)... 		Sphere s; 		s.m_Center = capsule.m_A; 		s.m_Radius = capsule.m_Radius; 		float stmin, stmax; 		if(IntersectRaySphere(ray, s, stmin, stmax)) 		{ 			p2 = ray.m_Origin + (ray.m_Direction * stmax); 			n2 = p2 - capsule.m_A; 			n2.Normalize(); 		} 		else  			return false; 	} 	else if(t_k2 > 1.0f)
	{
		// On sphere (B, r)...
		Sphere s;
		s.m_Center = capsule.m_B;
		s.m_Radius = capsule.m_Radius;
 
		float stmin, stmax;
		if(IntersectRaySphere(ray, s, stmin, stmax))
		{
			p2 = ray.m_Origin + (ray.m_Direction * stmax);
			n2 = p2 - capsule.m_B;
			n2.Normalize();
		}
		else 
			return false;
	}
	else
	{
		p2 = ray.m_Origin + (ray.m_Direction * tmax);
 
		CVector3f k2 = capsule.m_A + AB * t_k2;
		n2 = p2 - k2;
		n2.Normalize();
	}
 
	return true;
}

NOTE: This is definitely not the most optimal ray/capsule intersection test on the internet. No speed comparisons or SSE optimizations in this post, sorry!

Ok. The code above should be straightforward, given the theoretical explanation earlier. There’s one special case though, which we haven’t covered previously. When the ray is parallel to the capsule’s axis, vector Q is zero, which means both the t^2 and t factors are zero, and equation (8) either doesn’t have any solution or it has infinite solutions, depending on the quadratic constant. Instead of explicitly checking for the two rays being parallel, we do it through vector Q.

|Q| = 0 => d || AB

In this case, if the ray intersects the capsule, it would be at the two spheres (A, r) and (B, r).

Calculating normals should be straightforward. If the intersection point is on the cylindrical part of the capsule, the normal is equal to unit(KP). If the intersection point is on one of the end spheres, the normal is equal to unit(CP) where C is the corresponding sphere center.

Finally, notice that the code treat all rays the same. This means that even if the ray has its origin inside the capsule, we’ll still get two intersection points. The difference is that one of them would be behind the ray’s origin. Note that, it should always be P1 because it’s the one with the minimum/negative t.

That’s all. Thanks for reading.

Tags:

2 Responses to “Useless Snippet #5: Ray/Capsule intersection test”

  • Shpek Says:

    Hm… I think that you should check if tmax is >= 0. Otherwise you might get false hits from capsules behind the ray origin.

  • JD Says:

    Hi,
    Thanks for the correction. You are right, I should also check for that case. I’ll try editing the code sometime today.

Leave a Reply


four × 4 =