Instantly share code, notes, and snippets.

Embed
What would you like to do?
Ray/Capsule intersection test
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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment