public static bool Sweep(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 sweepA, ref Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB, out RayHit hit)
{
//Put the relative velocity into shape's local space.
Vector3 velocityWorld;
//note the order of subtraction. It 'should' be B-A, but the ray direction the algorithm works with is actually OPPOSITE.
Vector3.Subtract(ref sweepA, ref sweepB, out velocityWorld);
Quaternion conjugateOrientationA;
Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
Vector3 localDirection;
Vector3.Transform(ref velocityWorld, ref conjugateOrientationA, out localDirection);
//Sweeping two objects against each other is very similar to the local surface cast.
//The ray starts at the origin and goes in the sweep direction.
//However, unlike the local surface cast, the origin may start outside of the minkowski difference.
//Additionally, the method can early out if the length traversed by the ray is found to be longer than the maximum length, or if the origin is found to be outside the minkowski difference.
//The support points of the minkowski difference are also modified. By default, the minkowski difference should very rarely contain the origin.
//Sweep tests aren't very useful if the objects are intersecting!
//However, in order for the local surface cast to actually find a proper result (assuming there is a hit at all), the origin must be inside the minkowski difference.
//So, expand the minkowski difference using the sweep direction with magnitude sufficient to fully include the plane defined by the origin and the sweep direction.
//If there's going to be a hit, then the origin will be within this expanded shape.
//If the sweep direction is found to be negative, the ray can be thought of as pointing away from the shape.
//However, the minkowski difference may contain the shape. In this case, the time of impact is zero.
//If the swept (with sweep = 0 in case of incorrect direction) minkowski difference does not contain the shape, then the raycast cannot begin and we also know that the shapes will not intersect.
//If the sweep amount is nonnegative and the minkowski difference contains the shape, then the normal raycasting process can continue.
//Perform the usual local raycast, but use the swept minkowski difference.
//Once the surface is found, the final t parameter of the ray is equal to the sweep distance minus the local raycast computed t parameter.
RigidTransform localTransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localTransformB);
//First: Compute the sweep amount along the sweep direction.
//This sweep amount needs to expand the minkowski difference to fully intersect the plane defined by the sweep direction and origin.
float rayLengthSquared = localDirection.LengthSquared();
float sweepLength;
if (rayLengthSquared > Toolbox.Epsilon * .01f)
{
Vector3.Dot(ref localTransformB.Position, ref localDirection, out sweepLength);
sweepLength /= rayLengthSquared;
//Scale the sweep length by the margins. Divide by the length to pull the margin into terms of the length of the ray.
sweepLength += (shapeA.maximumRadius + shapeB.maximumRadius) / (float)Math.Sqrt(rayLengthSquared);
}
else
{
rayLengthSquared = 0;
sweepLength = 0;
}
//If the sweep direction is found to be negative, the ray can be thought of as pointing away from the shape.
//Do not sweep backward.
bool negativeLength;
if (negativeLength = sweepLength < 0)
sweepLength = 0;
Vector3 sweep;
Vector3.Multiply(ref localDirection, sweepLength, out sweep);
//Check to see if the origin is contained within the swept shape.
if (!AreSweptShapesIntersecting(shapeA, shapeB, ref sweep, ref localTransformB, out hit.Location)) //Computes a hit location to be used if the early-outs due to being in contact.
{
//The origin is not contained within the sweep volume. The raycast definitely misses.
hit.T = float.MaxValue;
hit.Normal = new Vector3();
hit.Location = new Vector3();
return false;
}
if (negativeLength)
{
//The origin is contained, but we shouldn't continue.
//The ray is facing backwards. The time of impact would be 0.
hit.T = 0;
Vector3.Normalize(ref localDirection, out hit.Normal);
Vector3.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
//hit.Location = hit.T * localDirection;
Vector3.Transform(ref hit.Location, ref transformA.Orientation, out hit.Location);
Vector3.Add(ref hit.Location, ref transformA.Position, out hit.Location);
hit.Location += sweepA * hit.T;
return true;
}
//OKAY! We've finally finished all the pre-testing. Cast the ray!
if (LocalSweepCast(shapeA, shapeB, sweepLength, rayLengthSquared, ref localDirection, ref sweep, ref localTransformB, out hit))
{
//Compute the actual hit location on the minkowski surface.
Vector3 minkowskiRayHit = -hit.T * localDirection;
//TODO: This uses MPR to identify a witness point on shape A.
//It's a very roundabout way to do it. There should be a much simpler/faster way to compute the witness point directly, or with a little sampling.
GetLocalPosition(shapeA, shapeB, ref localTransformB, ref minkowskiRayHit, out hit.Location);
//The hit location is still in local space, so transform it into world space using A's transform.
RigidTransform.Transform(ref hit.Location, ref transformA, out hit.Location);
Vector3.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
//Push the world space hit location relative to object A along A's sweep direction.
Vector3 temp;
Vector3.Multiply(ref sweepA, hit.T, out temp);
Vector3.Add(ref temp, ref hit.Location, out hit.Location);
return true;
}
return false;
}
