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