public static SphereCast ( BEPUutilities.Ray ray, float radius, |
||
ray | BEPUutilities.Ray | Ray to test against the shape. |
radius | float | Radius of the ray. |
shape | Shape to test against. | |
shapeTransform | Transform to apply to the shape for the test. | |
maximumLength | float | Maximum length of the ray in units of the ray direction's length. |
hit | Hit data of the sphere cast, if any. | |
return | bool |
public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Vector3.Transform(ref ray.Position, ref conjugate, out ray.Position);
Vector3.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
Vector3 w, p;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
Vector3 v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return false;
}
shape.GetLocalExtremePointWithoutMargin(ref v, out p);
Vector3 contribution;
MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution);
Vector3.Add(ref p, ref contribution, out p);
Vector3.Subtract(ref hit.Location, ref p, out w);
Vector3.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3.Dot(ref v, ref ray.Direction, out vdir);
hit.T = hit.T - vw / vdir;
if (vdir >= 0)
{
//We would have to back up!
return false;
}
if (hit.T > maximumLength)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
return false;
}
//Shift the ray up.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
}
//Transform the hit data into world space.
Vector3.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
Vector3.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return true;
}
///<summary> /// Casts a fat (sphere expanded) ray against the shape. If the raycast appears to be stuck in the shape, the cast will be attempted /// with a smaller ray (scaled by the MotionSettings.CoreShapeScaling each time). ///</summary> ///<param name="ray">Ray to test against the shape.</param> ///<param name="radius">Radius of the ray.</param> ///<param name="target">Shape to test against.</param> ///<param name="shapeTransform">Transform to apply to the shape for the test.</param> ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param> ///<param name="hit">Hit data of the sphere cast, if any.</param> ///<returns>Whether or not the sphere cast hit the shape.</returns> public static bool CCDSphereCast(Ray ray, float radius, ConvexShape target, ref RigidTransform shapeTransform, float maximumLength, out RayHit hit) { int iterations = 0; while (true) { if (GJKToolbox.SphereCast(ray, radius, target, ref shapeTransform, maximumLength, out hit) && hit.T > 0) { //The ray cast isn't embedded in the shape, and it's less than maximum length away! return(true); } if (hit.T > maximumLength || hit.T < 0) { return(false); //Failure showed it was too far, or behind. } radius *= MotionSettings.CoreShapeScaling; iterations++; if (iterations > 3) //Limit could be configurable. { //It's iterated too much, let's just do a last ditch attempt using a raycast and hope that can help. return(GJKToolbox.RayCast(ray, target, ref shapeTransform, maximumLength, out hit) && hit.T > 0); } } }