///<summary>
/// Sweeps two shapes against another.
///</summary>
///<param name="shapeA">First shape being swept.</param>
///<param name="shapeB">Second shape being swept.</param>
///<param name="sweepA">Sweep vector for the first shape.</param>
///<param name="sweepB">Sweep vector for the second shape.</param>
///<param name="transformA">Transform to apply to the first shape.</param>
///<param name="transformB">Transform to apply to the second shape.</param>
///<param name="hit">Hit data of the sweep test, if any.</param>
///<returns>Whether or not the swept shapes hit each other..</returns>
public static bool ConvexCast(ConvexShape shapeA, ConvexShape shapeB, ref Vector3f sweepA, ref Vector3f sweepB, ref RigidTransform transformA, ref RigidTransform transformB,
out RayHit hit)
{
//Put the velocity into shapeA's local space.
Vector3f velocityWorld;
Vector3f.Subtract(ref sweepB, ref sweepA, out velocityWorld);
Quaternion conjugateOrientationA;
Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
Vector3f rayDirection;
Vector3f.Transform(ref velocityWorld, ref conjugateOrientationA, out rayDirection);
//Transform b into a's local space.
RigidTransform localTransformB;
Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation);
Vector3f.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
Vector3f.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
Vector3f w, p;
hit.T = 0;
hit.Location = Vector3f.Zero; //The ray starts at the origin.
hit.Normal = Toolbox.ZeroVector;
Vector3f v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
do
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref v, ref localTransformB, out p);
Vector3f.Subtract(ref hit.Location, ref p, out w);
Vector3f.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3f.Dot(ref v, ref rayDirection, out vdir);
if (vdir >= 0)
{
hit = new RayHit();
return(false);
}
hit.T = hit.T - vw / vdir;
if (hit.T > 1)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
hit = new RayHit();
return(false);
}
//Shift the ray up.
Vector3f.Multiply(ref rayDirection, hit.T, out hit.Location);
//The ray origin is the origin! Don't need to add any ray position.
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
//Could measure the progress of the ray. If it's too little, could early out.
//Not used by default since it's biased towards precision over performance.
} while (v.LengthSquared >= MathHelper.Epsilon * simplex.GetErrorTolerance(ref Toolbox.ZeroVector));
//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.
//Transform the hit data into world space.
Vector3f.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
Vector3f.Multiply(ref velocityWorld, hit.T, out hit.Location);
Vector3f.Add(ref hit.Location, ref transformA.Position, out hit.Location);
return(true);
}
///<summary>
/// Casts a fat (sphere expanded) ray against the shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="radius">Radius of the ray.</param>
///<param name="shape">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 SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3f.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Vector3f.Transform(ref ray.Position, ref conjugate, out ray.Position);
Vector3f.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
Vector3f w, p;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
Vector3f 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 >= MathHelper.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);
Vector3f contribution;
MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution);
Vector3f.Add(ref p, ref contribution, out p);
Vector3f.Subtract(ref hit.Location, ref p, out w);
Vector3f.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3f.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.
Vector3f.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3f.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.
Vector3f.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
Vector3f.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3f.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return(true);
}
//TODO: Consider changing the termination epsilons on these casts. Epsilon * Modifier is okay, but there might be better options.
///<summary>
/// Tests a ray against a convex shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="shape">Shape to test.</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 ray cast, if any.</param>
///<returns>Whether or not the ray hit the shape.</returns>
public static bool RayCast(Ray ray, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3f.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Vector3f.Transform(ref ray.Position, ref conjugate, out ray.Position);
Vector3f.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
Vector3f extremePointToRayOrigin, extremePoint;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
Vector3f closestOffset = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, closestPointDotDirection;
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 (closestOffset.LengthSquared >= MathHelper.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.GetLocalExtremePoint(closestOffset, out extremePoint);
Vector3f.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin);
Vector3f.Dot(ref closestOffset, ref extremePointToRayOrigin, out vw);
//If the closest offset and the extreme point->ray origin direction point the same way,
//then we might be able to conservatively advance the point towards the surface.
if (vw > 0)
{
Vector3f.Dot(ref closestOffset, ref ray.Direction, out closestPointDotDirection);
if (closestPointDotDirection >= 0)
{
hit = new RayHit();
return(false);
}
hit.T = hit.T - vw / closestPointDotDirection;
if (hit.T > maximumLength)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
hit = new RayHit();
return(false);
}
//Shift the ray up.
Vector3f.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3f.Add(ref hit.Location, ref ray.Position, out hit.Location);
hit.Normal = closestOffset;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref extremePoint, ref hit.Location, out shiftedSimplex);
//Compute the offset from the simplex surface to the origin.
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out closestOffset);
}
//Transform the hit data into world space.
Vector3f.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
Vector3f.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3f.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return(true);
}