Ray/Capsule intersection test

RayCapsuleTest.cpp

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-6f)
		{
			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;
		}
	}
};

struct Ray
{
	CVector3f m_Origin;
	CVector3f m_Direction;
};

struct Capsule
{
	CVector3f m_A;
	CVector3f m_B;
	float m_Radius;
};

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;
}

bool IntersectRayCapsule(const Ray& ray, const Capsule& capsule, CVector3f& p1, CVector3f& p2, CVector3f& n1, CVector3f& n2)
{
	// http://pastebin.com/2XrrNcxb

	// 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;
}