public static Vector3 Normal(this BoundingCapsule c, ref Vector3 position)
{
Vector3 ba = c.PointB - c.PointA;
Vector3 pa = position - c.PointA;
float h = Math.Min(Math.Max(Vector3.Dot(pa, ba) / Vector3.Dot(ba, ba), 0.0f), 1.0f);
return((pa - h * ba) / c.Radius);
}
public static bool Intersects(this BoundingSphere sph, ref BoundingCapsule capsule, out Vector3 norm)
{
var dist = LineMath.PointSegmentDistance(ref sph.Center, ref capsule.PointA, ref capsule.PointB);
var rads = sph.Radius + capsule.Radius;
if (dist <= rads)
{
norm = LineMath.PointSegmentNormal(ref sph.Center, ref capsule.PointA, ref capsule.PointB);
return(true);
}
else
{
norm = Vector3.Up;
return(false);
}
}
public static bool Intersects(this Ray r, ref BoundingCapsule capsule, out float dist)
{
// intersect capsule : http://www.iquilezles.org/www/articles/intersectors/intersectors.htm
Vector3 ba = capsule.PointB - capsule.PointA;
Vector3 oa = r.Position - capsule.PointA;
float baba = Vector3.Dot(ba, ba);
float bard = Vector3.Dot(ba, r.Direction);
float baoa = Vector3.Dot(ba, oa);
float rdoa = Vector3.Dot(r.Direction, oa);
float oaoa = Vector3.Dot(oa, oa);
float r2 = capsule.Radius * capsule.Radius;
float a = baba - bard * bard;
float b = baba * rdoa - baoa * bard;
float c = baba * oaoa - baoa * baoa - r2 * baba;
float h = b * b - a * c;
if (h >= 0.0f)
{
float t = (-b - (float)Math.Sqrt(h)) / a;
float y = baoa + t * bard;
// body
if (y > 0.0f && y < baba)
{
dist = t;
return(true);
}
// caps
Vector3 oc = (y <= 0.0f) ? oa : r.Position - capsule.PointB;
b = Vector3.Dot(r.Direction, oc);
c = Vector3.Dot(oc, oc) - r2;
h = b * b - c;
if (h > 0.0f)
{
dist = -b - (float)Math.Sqrt(h);
return(true);
}
}
dist = -1.0f;
return(false);
}