public static bool TestRaySphereCollision(Math.Vector3 origin,
Math.Vector3 direction, Math.Vector3 spherePosition, float radius, out float distance)
{
Math.Vector3 sphereToOrigin = origin - spherePosition;
float b = 2 * (sphereToOrigin.Dot(direction));
float c = sphereToOrigin.Dot(sphereToOrigin) - radius * radius;
float d = b * b - 4 * c;
if (d < 0)
{
distance = 0;
return(false);
}
float dsqrt = (float)System.Math.Sqrt(d);
float q;
if (b < 0)
{
q = (-b - dsqrt) * 0.5f;
}
else
{
q = (-b + dsqrt) * 0.5f;
}
float t0 = q;
float t1 = c / q;
if (t0 > t1)
{
float tmp = t0;
t0 = t1;
t1 = tmp;
}
if (t1 < 0)
{
distance = 0;
return(false);
}
if (t0 < 0)
{
distance = t1;
}
else
{
distance = t1;
}
return(true);
}
public static bool TestRayTriangleCollision(Math.Vector3 origin,
Math.Vector3 direction, Math.Vector3 v1,
Math.Vector3 v2, Math.Vector3 v3,
out float distance, out Math.Vector3?collisionPoint)
{
distance = 0;
collisionPoint = null;
const float EPSILON = 0.00001f;
Math.Vector3 edge1 = v2 - v1;
Math.Vector3 edge2 = v3 - v1;
Math.Vector3 pvec = direction.Cross(edge2);
float det = edge1.Dot(pvec);
if (det > -EPSILON && det < EPSILON)
{
return(false);
}
float inv_det = 1.0f / det;
Math.Vector3 tvec = origin - v1;
float u = tvec.Dot(pvec) * inv_det;
if (u < 0.0f || u > 1.0f)
{
return(false);
}
Math.Vector3 qvec = tvec.Cross(edge1);
float v = direction.Dot(qvec) * inv_det;
if (v < 0.0f || u + v > 1.0f)
{
return(false);
}
// pack up the results
distance = edge2.Dot(qvec) * inv_det;
collisionPoint = v1 + edge1 * u + edge2 * v;
return(true);
}