///<summary>
/// Gets the point on the simplex that is closest to the origin.
///</summary>
///<param name="simplex">Simplex to test.</param>
///<param name="point">Closest point on the simplex.</param>
///<returns>Whether or not the simplex contains the origin.</returns>
public bool GetPointClosestToOrigin(ref RaySimplex simplex, out Vector3 point)
{
//This method finds the closest point on the simplex to the origin.
//Barycentric coordinates are assigned to the MinimumNormCoordinates as necessary to perform the inclusion calculation.
//If the simplex is a tetrahedron and found to be overlapping the origin, the function returns true to tell the caller to terminate.
//Elements of the simplex that are not used to determine the point of minimum norm are removed from the simplex.
switch (State)
{
case SimplexState.Point:
point = A;
break;
case SimplexState.Segment:
GetPointOnSegmentClosestToOrigin(ref simplex, out point);
break;
case SimplexState.Triangle:
GetPointOnTriangleClosestToOrigin(ref simplex, out point);
break;
case SimplexState.Tetrahedron:
return GetPointOnTetrahedronClosestToOrigin(ref simplex, out point);
default:
point = Toolbox.ZeroVector;
break;
}
return false;
}
///<summary>
/// Finds the point on the segment to the origin.
///</summary>
///<param name="simplex">Simplex to test.</param>
///<param name="point">Closest point.</param>
public void GetPointOnSegmentClosestToOrigin(ref RaySimplex simplex, out Vector3 point)
{
Vector3 segmentDisplacement;
Vector3.Subtract(ref B, ref A, out segmentDisplacement);
float dotA;
Vector3.Dot(ref segmentDisplacement, ref A, out dotA);
if (dotA > 0)
{
//'Behind' segment. This can't happen in a boolean version,
//but with closest points warmstarting or raycasts, it will.
simplex.State = SimplexState.Point;
point = A;
return;
}
float dotB;
Vector3.Dot(ref segmentDisplacement, ref B, out dotB);
if (dotB > 0)
{
//Inside segment.
float V = -dotA / segmentDisplacement.LengthSquared();
Vector3.Multiply(ref segmentDisplacement, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//It should be possible in the warmstarted closest point calculation/raycasting to be outside B.
//It is not possible in a 'boolean' GJK, where it early outs as soon as a separating axis is found.
//Outside B.
//Remove current A; we're becoming a point.
simplex.A = simplex.B;
simplex.State = SimplexState.Point;
point = A;
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="point">Point to add.</param>
///<param name="hitLocation">Current ray hit location.</param>
///<param name="shiftedSimplex">Simplex shifted with the hit location.</param>
public void AddNewSimplexPoint(ref Vector3 point, ref Vector3 hitLocation, out RaySimplex shiftedSimplex)
{
shiftedSimplex = new RaySimplex();
switch (State)
{
case SimplexState.Empty:
State = SimplexState.Point;
A = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
break;
case SimplexState.Point:
State = SimplexState.Segment;
B = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
break;
case SimplexState.Segment:
State = SimplexState.Triangle;
C = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
Vector3.Subtract(ref hitLocation, ref C, out shiftedSimplex.C);
break;
case SimplexState.Triangle:
State = SimplexState.Tetrahedron;
D = point;
Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
Vector3.Subtract(ref hitLocation, ref C, out shiftedSimplex.C);
Vector3.Subtract(ref hitLocation, ref D, out shiftedSimplex.D);
break;
}
shiftedSimplex.State = State;
}
///<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.
Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position);
Quaternion.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.
Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return true;
}
///<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 Vector3 sweepA, ref Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB,
out RayHit hit)
{
//Put the velocity into shapeA's local space.
Vector3 velocityWorld;
Vector3.Subtract(ref sweepB, ref sweepA, out velocityWorld);
Quaternion conjugateOrientationA;
Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
Vector3 rayDirection;
Quaternion.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);
Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
Quaternion.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
Vector3 w, p;
hit.T = 0;
hit.Location = Vector3.Zero; //The ray starts at the origin.
hit.Normal = Toolbox.ZeroVector;
Vector3 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);
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 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.
Vector3.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() >= Toolbox.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.
Quaternion.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
Vector3.Multiply(ref velocityWorld, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref transformA.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.
Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position);
Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
Vector3 extremePointToRayOrigin, extremePoint;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
Vector3 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() >= 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.GetLocalExtremePoint(closestOffset, out extremePoint);
Vector3.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin);
Vector3.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)
{
Vector3.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.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.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.
Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return true;
}
private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C,
ref Vector3 simplexA, ref Vector3 simplexB, ref Vector3 simplexC,
ref Vector3 otherPoint, out RaySimplex simplex, out Vector3 point)
{
//Note that there may be some extra terms that can be removed from this process.
//Some conditions could use less parameters, since it is known that the origin
//is not 'behind' BC or AC.
simplex = new RaySimplex();
point = new Vector3();
Vector3 ab, ac;
Vector3.Subtract(ref B, ref A, out ab);
Vector3.Subtract(ref C, ref A, out ac);
Vector3 normal;
Vector3.Cross(ref ab, ref ac, out normal);
float AdotN, ADdotN;
Vector3 AD;
Vector3.Subtract(ref otherPoint, ref A, out AD);
Vector3.Dot(ref A, ref normal, out AdotN);
Vector3.Dot(ref AD, ref normal, out ADdotN);
//If (-A * N) * (AD * N) < 0, D and O are on opposite sides of the triangle.
if (AdotN * ADdotN >= 0)
{
//The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector,
//just use -A.
//Same for B->, C->P...
//Check to see if it's outside A.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A.
float AdotAB, AdotAC;
Vector3.Dot(ref ab, ref A, out AdotAB);
Vector3.Dot(ref ac, ref A, out AdotAC);
AdotAB = -AdotAB;
AdotAC = -AdotAC;
if (AdotAC <= 0f && AdotAB <= 0)
{
//It is A!
simplex.State = SimplexState.Point;
simplex.A = simplexA;
point = A;
return true;
}
//Check to see if it's outside B.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B.
float BdotAB, BdotAC;
Vector3.Dot(ref ab, ref B, out BdotAB);
Vector3.Dot(ref ac, ref B, out BdotAC);
BdotAB = -BdotAB;
BdotAC = -BdotAC;
if (BdotAB >= 0f && BdotAC <= BdotAB)
{
//It is B!
simplex.State = SimplexState.Point;
simplex.A = simplexB;
point = B;
return true;
}
//Check to see if it's outside AB.
float vc = AdotAB * BdotAC - BdotAB * AdotAC;
if (vc <= 0 && AdotAB > 0 && BdotAB < 0) //Note > and < instead of => <=; avoids possibly division by zero
{
simplex.State = SimplexState.Segment;
simplex.A = simplexA;
simplex.B = simplexB;
float V = AdotAB / (AdotAB - BdotAB);
Vector3.Multiply(ref ab, V, out point);
Vector3.Add(ref point, ref A, out point);
return true;
}
//Check to see if it's outside C.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C.
float CdotAB, CdotAC;
Vector3.Dot(ref ab, ref C, out CdotAB);
Vector3.Dot(ref ac, ref C, out CdotAC);
CdotAB = -CdotAB;
CdotAC = -CdotAC;
if (CdotAC >= 0f && CdotAB <= CdotAC)
{
//It is C!
simplex.State = SimplexState.Point;
simplex.A = simplexC;
point = C;
return true;
}
//Check if it's outside AC.
//float AdotAB, AdotAC;
//Vector3.Dot(ref ab, ref A, out AdotAB);
//Vector3.Dot(ref ac, ref A, out AdotAC);
//AdotAB = -AdotAB;
//AdotAC = -AdotAC;
float vb = CdotAB * AdotAC - AdotAB * CdotAC;
if (vb <= 0f && AdotAC > 0f && CdotAC < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator
{
simplex.State = SimplexState.Segment;
simplex.A = simplexA;
simplex.B = simplexC;
float V = AdotAC / (AdotAC - CdotAC);
Vector3.Multiply(ref ac, V, out point);
Vector3.Add(ref point, ref A, out point);
return true;
}
//Check if it's outside BC.
//float BdotAB, BdotAC;
//Vector3.Dot(ref ab, ref B, out BdotAB);
//Vector3.Dot(ref ac, ref B, out BdotAC);
//BdotAB = -BdotAB;
//BdotAC = -BdotAC;
float va = BdotAB * CdotAC - CdotAB * BdotAC;
float d3d4;
float d6d5;
if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
simplex.State = SimplexState.Segment;
simplex.A = simplexB;
simplex.B = simplexC;
float V = d3d4 / (d3d4 + d6d5);
Vector3 bc;
Vector3.Subtract(ref C, ref B, out bc);
Vector3.Multiply(ref bc, V, out point);
Vector3.Add(ref point, ref B, out point);
return true;
}
//On the face of the triangle.
simplex.State = SimplexState.Triangle;
simplex.A = simplexA;
simplex.B = simplexB;
simplex.C = simplexC;
float denom = 1f / (va + vb + vc);
float w = vc * denom;
float v = vb * denom;
Vector3.Multiply(ref ab, v, out point);
Vector3 acw;
Vector3.Multiply(ref ac, w, out acw);
Vector3.Add(ref A, ref point, out point);
Vector3.Add(ref point, ref acw, out point);
return true;
}
return false;
}
///<summary>
/// Gets the point closest to the origin on the tetrahedron.
///</summary>
///<param name="simplex">Simplex to test.</param>
///<param name="point">Closest point.</param>
///<returns>Whether or not the tetrahedron encloses the origin.</returns>
public bool GetPointOnTetrahedronClosestToOrigin(ref RaySimplex simplex, out Vector3 point)
{
//Thanks to the fact that D is new and that we know that the origin is within the extruded
//triangular prism of ABC (and on the "D" side of ABC),
//we can immediately ignore voronoi regions:
//A, B, C, AC, AB, BC, ABC
//and only consider:
//D, DA, DB, DC, DAC, DCB, DBA
//There is some overlap of calculations in this method, since DAC, DCB, and DBA are tested fully.
//When this method is being called, we don't care about the state of 'this' simplex. It's just a temporary shifted simplex.
//The one that needs to be updated is the simplex being passed in.
var minimumSimplex = new RaySimplex();
point = new Vector3();
float minimumDistance = float.MaxValue;
RaySimplex candidate;
float candidateDistance;
Vector3 candidatePoint;
if (TryTetrahedronTriangle(ref A, ref C, ref D,
ref simplex.A, ref simplex.C, ref simplex.D,
ref B, out candidate, out candidatePoint))
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidatePoint.LengthSquared();
}
if (TryTetrahedronTriangle(ref C, ref B, ref D,
ref simplex.C, ref simplex.B, ref simplex.D,
ref A, out candidate, out candidatePoint) &&
(candidateDistance = candidatePoint.LengthSquared()) < minimumDistance)
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidateDistance;
}
if (TryTetrahedronTriangle(ref B, ref A, ref D,
ref simplex.B, ref simplex.A, ref simplex.D,
ref C, out candidate, out candidatePoint) &&
(candidateDistance = candidatePoint.LengthSquared()) < minimumDistance)
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidateDistance;
}
if (TryTetrahedronTriangle(ref A, ref B, ref C,
ref simplex.A, ref simplex.B, ref simplex.C,
ref D, out candidate, out candidatePoint) &&
(candidateDistance = candidatePoint.LengthSquared()) < minimumDistance)
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidateDistance;
}
if (minimumDistance < float.MaxValue)
{
simplex = minimumSimplex;
return false;
}
return true;
}
///<summary>
/// Gets the point on the triangle that is closest to the origin.
///</summary>
///<param name="simplex">Simplex to test.</param>
///<param name="point">Closest point to origin.</param>
public void GetPointOnTriangleClosestToOrigin(ref RaySimplex simplex, out Vector3 point)
{
Vector3 ab, ac;
Vector3.Subtract(ref B, ref A, out ab);
Vector3.Subtract(ref C, ref A, out ac);
//The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector,
//just use -A.
//Same for B->P, C->P...
//Check to see if it's outside A.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A.
float AdotAB, AdotAC;
Vector3.Dot(ref ab, ref A, out AdotAB);
Vector3.Dot(ref ac, ref A, out AdotAC);
AdotAB = -AdotAB;
AdotAC = -AdotAC;
if (AdotAC <= 0f && AdotAB <= 0)
{
//It is A!
simplex.State = SimplexState.Point;
point = A;
return;
}
//Check to see if it's outside B.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B.
float BdotAB, BdotAC;
Vector3.Dot(ref ab, ref B, out BdotAB);
Vector3.Dot(ref ac, ref B, out BdotAC);
BdotAB = -BdotAB;
BdotAC = -BdotAC;
if (BdotAB >= 0f && BdotAC <= BdotAB)
{
//It is B!
simplex.State = SimplexState.Point;
simplex.A = simplex.B;
point = B;
return;
}
//Check to see if it's outside AB.
float vc = AdotAB * BdotAC - BdotAB * AdotAC;
if (vc <= 0 && AdotAB > 0 && BdotAB < 0)//Note > and < instead of => <=; avoids possibly division by zero
{
simplex.State = SimplexState.Segment;
float V = AdotAB / (AdotAB - BdotAB);
Vector3.Multiply(ref ab, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//Check to see if it's outside C.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C.
float CdotAB, CdotAC;
Vector3.Dot(ref ab, ref C, out CdotAB);
Vector3.Dot(ref ac, ref C, out CdotAC);
CdotAB = -CdotAB;
CdotAC = -CdotAC;
if (CdotAC >= 0f && CdotAB <= CdotAC)
{
//It is C!
simplex.State = SimplexState.Point;
simplex.A = simplex.C;
point = A;
return;
}
//Check if it's outside AC.
//float AdotAB, AdotAC;
//Vector3.Dot(ref ab, ref A, out AdotAB);
//Vector3.Dot(ref ac, ref A, out AdotAC);
//AdotAB = -AdotAB;
//AdotAC = -AdotAC;
float vb = CdotAB * AdotAC - AdotAB * CdotAC;
if (vb <= 0f && AdotAC > 0f && CdotAC < 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
//Get rid of B. Compress C into B.
simplex.State = SimplexState.Segment;
simplex.B = simplex.C;
float V = AdotAC / (AdotAC - CdotAC);
Vector3.Multiply(ref ac, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//Check if it's outside BC.
//float BdotAB, BdotAC;
//Vector3.Dot(ref ab, ref B, out BdotAB);
//Vector3.Dot(ref ac, ref B, out BdotAC);
//BdotAB = -BdotAB;
//BdotAC = -BdotAC;
float va = BdotAB * CdotAC - CdotAB * BdotAC;
float d3d4;
float d6d5;
if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
//Throw away A. C->A.
//TODO: Does B->A, C->B work better?
simplex.State = SimplexState.Segment;
simplex.A = simplex.C;
float U = d3d4 / (d3d4 + d6d5);
Vector3 bc;
Vector3.Subtract(ref C, ref B, out bc);
Vector3.Multiply(ref bc, U, out point);
Vector3.Add(ref point, ref B, out point);
return;
}
//On the face of the triangle.
float denom = 1f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;
Vector3.Multiply(ref ab, v, out point);
Vector3 acw;
Vector3.Multiply(ref ac, w, out acw);
Vector3.Add(ref A, ref point, out point);
Vector3.Add(ref point, ref acw, out point);
}
private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C,
ref Vector3 simplexA, ref Vector3 simplexB, ref Vector3 simplexC,
ref Vector3 otherPoint, out RaySimplex simplex, out Vector3 point)
{
//Note that there may be some extra terms that can be removed from this process.
//Some conditions could use less parameters, since it is known that the origin
//is not 'behind' BC or AC.
simplex = new RaySimplex();
point = new Vector3();
Vector3 ab, ac;
Vector3.Subtract(ref B, ref A, out ab);
Vector3.Subtract(ref C, ref A, out ac);
Vector3 normal;
Vector3.Cross(ref ab, ref ac, out normal);
float AdotN, ADdotN;
Vector3 AD;
Vector3.Subtract(ref otherPoint, ref A, out AD);
Vector3.Dot(ref A, ref normal, out AdotN);
Vector3.Dot(ref AD, ref normal, out ADdotN);
//If (-A * N) * (AD * N) < 0, D and O are on opposite sides of the triangle.
if (AdotN * ADdotN >= 0)
{
//The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector,
//just use -A.
//Same for B->, C->P...
//Check to see if it's outside A.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A.
float AdotAB, AdotAC;
Vector3.Dot(ref ab, ref A, out AdotAB);
Vector3.Dot(ref ac, ref A, out AdotAC);
AdotAB = -AdotAB;
AdotAC = -AdotAC;
if (AdotAC <= 0f && AdotAB <= 0)
{
//It is A!
simplex.State = SimplexState.Point;
simplex.A = simplexA;
point = A;
return(true);
}
//Check to see if it's outside B.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B.
float BdotAB, BdotAC;
Vector3.Dot(ref ab, ref B, out BdotAB);
Vector3.Dot(ref ac, ref B, out BdotAC);
BdotAB = -BdotAB;
BdotAC = -BdotAC;
if (BdotAB >= 0f && BdotAC <= BdotAB)
{
//It is B!
simplex.State = SimplexState.Point;
simplex.A = simplexB;
point = B;
return(true);
}
//Check to see if it's outside AB.
float vc = AdotAB * BdotAC - BdotAB * AdotAC;
if (vc <= 0 && AdotAB > 0 && BdotAB < 0) //Note > and < instead of => <=; avoids possibly division by zero
{
simplex.State = SimplexState.Segment;
simplex.A = simplexA;
simplex.B = simplexB;
float V = AdotAB / (AdotAB - BdotAB);
Vector3.Multiply(ref ab, V, out point);
Vector3.Add(ref point, ref A, out point);
return(true);
}
//Check to see if it's outside C.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C.
float CdotAB, CdotAC;
Vector3.Dot(ref ab, ref C, out CdotAB);
Vector3.Dot(ref ac, ref C, out CdotAC);
CdotAB = -CdotAB;
CdotAC = -CdotAC;
if (CdotAC >= 0f && CdotAB <= CdotAC)
{
//It is C!
simplex.State = SimplexState.Point;
simplex.A = simplexC;
point = C;
return(true);
}
//Check if it's outside AC.
//float AdotAB, AdotAC;
//Vector3.Dot(ref ab, ref A, out AdotAB);
//Vector3.Dot(ref ac, ref A, out AdotAC);
//AdotAB = -AdotAB;
//AdotAC = -AdotAC;
float vb = CdotAB * AdotAC - AdotAB * CdotAC;
if (vb <= 0f && AdotAC > 0f && CdotAC < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator
{
simplex.State = SimplexState.Segment;
simplex.A = simplexA;
simplex.B = simplexC;
float V = AdotAC / (AdotAC - CdotAC);
Vector3.Multiply(ref ac, V, out point);
Vector3.Add(ref point, ref A, out point);
return(true);
}
//Check if it's outside BC.
//float BdotAB, BdotAC;
//Vector3.Dot(ref ab, ref B, out BdotAB);
//Vector3.Dot(ref ac, ref B, out BdotAC);
//BdotAB = -BdotAB;
//BdotAC = -BdotAC;
float va = BdotAB * CdotAC - CdotAB * BdotAC;
float d3d4;
float d6d5;
if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
simplex.State = SimplexState.Segment;
simplex.A = simplexB;
simplex.B = simplexC;
float V = d3d4 / (d3d4 + d6d5);
Vector3 bc;
Vector3.Subtract(ref C, ref B, out bc);
Vector3.Multiply(ref bc, V, out point);
Vector3.Add(ref point, ref B, out point);
return(true);
}
//On the face of the triangle.
simplex.State = SimplexState.Triangle;
simplex.A = simplexA;
simplex.B = simplexB;
simplex.C = simplexC;
float denom = 1f / (va + vb + vc);
float w = vc * denom;
float v = vb * denom;
Vector3.Multiply(ref ab, v, out point);
Vector3 acw;
Vector3.Multiply(ref ac, w, out acw);
Vector3.Add(ref A, ref point, out point);
Vector3.Add(ref point, ref acw, out point);
return(true);
}
return(false);
}
///<summary>
/// Gets the point closest to the origin on the tetrahedron.
///</summary>
///<param name="simplex">Simplex to test.</param>
///<param name="point">Closest point.</param>
///<returns>Whether or not the tetrahedron encloses the origin.</returns>
public bool GetPointOnTetrahedronClosestToOrigin(ref RaySimplex simplex, out Vector3 point)
{
//Thanks to the fact that D is new and that we know that the origin is within the extruded
//triangular prism of ABC (and on the "D" side of ABC),
//we can immediately ignore voronoi regions:
//A, B, C, AC, AB, BC, ABC
//and only consider:
//D, DA, DB, DC, DAC, DCB, DBA
//There is some overlap of calculations in this method, since DAC, DCB, and DBA are tested fully.
//When this method is being called, we don't care about the state of 'this' simplex. It's just a temporary shifted simplex.
//The one that needs to be updated is the simplex being passed in.
var minimumSimplex = new RaySimplex();
point = new Vector3();
float minimumDistance = float.MaxValue;
RaySimplex candidate;
float candidateDistance;
Vector3 candidatePoint;
if (TryTetrahedronTriangle(ref A, ref C, ref D,
ref simplex.A, ref simplex.C, ref simplex.D,
ref B, out candidate, out candidatePoint))
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidatePoint.LengthSquared();
}
if (TryTetrahedronTriangle(ref C, ref B, ref D,
ref simplex.C, ref simplex.B, ref simplex.D,
ref A, out candidate, out candidatePoint) &&
(candidateDistance = candidatePoint.LengthSquared()) < minimumDistance)
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidateDistance;
}
if (TryTetrahedronTriangle(ref B, ref A, ref D,
ref simplex.B, ref simplex.A, ref simplex.D,
ref C, out candidate, out candidatePoint) &&
(candidateDistance = candidatePoint.LengthSquared()) < minimumDistance)
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidateDistance;
}
if (TryTetrahedronTriangle(ref A, ref B, ref C,
ref simplex.A, ref simplex.B, ref simplex.C,
ref D, out candidate, out candidatePoint) &&
(candidateDistance = candidatePoint.LengthSquared()) < minimumDistance)
{
point = candidatePoint;
minimumSimplex = candidate;
minimumDistance = candidateDistance;
}
if (minimumDistance < float.MaxValue)
{
simplex = minimumSimplex;
return(false);
}
return(true);
}
///<summary>
/// Gets the point on the triangle that is closest to the origin.
///</summary>
///<param name="simplex">Simplex to test.</param>
///<param name="point">Closest point to origin.</param>
public void GetPointOnTriangleClosestToOrigin(ref RaySimplex simplex, out Vector3 point)
{
Vector3 ab, ac;
Vector3.Subtract(ref B, ref A, out ab);
Vector3.Subtract(ref C, ref A, out ac);
//The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector,
//just use -A.
//Same for B->P, C->P...
//Check to see if it's outside A.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A.
float AdotAB, AdotAC;
Vector3.Dot(ref ab, ref A, out AdotAB);
Vector3.Dot(ref ac, ref A, out AdotAC);
AdotAB = -AdotAB;
AdotAC = -AdotAC;
if (AdotAC <= 0f && AdotAB <= 0)
{
//It is A!
simplex.State = SimplexState.Point;
point = A;
return;
}
//Check to see if it's outside B.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B.
float BdotAB, BdotAC;
Vector3.Dot(ref ab, ref B, out BdotAB);
Vector3.Dot(ref ac, ref B, out BdotAC);
BdotAB = -BdotAB;
BdotAC = -BdotAC;
if (BdotAB >= 0f && BdotAC <= BdotAB)
{
//It is B!
simplex.State = SimplexState.Point;
simplex.A = simplex.B;
point = B;
return;
}
//Check to see if it's outside AB.
float vc = AdotAB * BdotAC - BdotAB * AdotAC;
if (vc <= 0 && AdotAB > 0 && BdotAB < 0)//Note > and < instead of => <=; avoids possibly division by zero
{
simplex.State = SimplexState.Segment;
float V = AdotAB / (AdotAB - BdotAB);
Vector3.Multiply(ref ab, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//Check to see if it's outside C.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C.
float CdotAB, CdotAC;
Vector3.Dot(ref ab, ref C, out CdotAB);
Vector3.Dot(ref ac, ref C, out CdotAC);
CdotAB = -CdotAB;
CdotAC = -CdotAC;
if (CdotAC >= 0f && CdotAB <= CdotAC)
{
//It is C!
simplex.State = SimplexState.Point;
simplex.A = simplex.C;
point = A;
return;
}
//Check if it's outside AC.
//float AdotAB, AdotAC;
//Vector3.Dot(ref ab, ref A, out AdotAB);
//Vector3.Dot(ref ac, ref A, out AdotAC);
//AdotAB = -AdotAB;
//AdotAC = -AdotAC;
float vb = CdotAB * AdotAC - AdotAB * CdotAC;
if (vb <= 0f && AdotAC > 0f && CdotAC < 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
//Get rid of B. Compress C into B.
simplex.State = SimplexState.Segment;
simplex.B = simplex.C;
float V = AdotAC / (AdotAC - CdotAC);
Vector3.Multiply(ref ac, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//Check if it's outside BC.
//float BdotAB, BdotAC;
//Vector3.Dot(ref ab, ref B, out BdotAB);
//Vector3.Dot(ref ac, ref B, out BdotAC);
//BdotAB = -BdotAB;
//BdotAC = -BdotAC;
float va = BdotAB * CdotAC - CdotAB * BdotAC;
float d3d4;
float d6d5;
if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
//Throw away A. C->A.
//TODO: Does B->A, C->B work better?
simplex.State = SimplexState.Segment;
simplex.A = simplex.C;
float U = d3d4 / (d3d4 + d6d5);
Vector3 bc;
Vector3.Subtract(ref C, ref B, out bc);
Vector3.Multiply(ref bc, U, out point);
Vector3.Add(ref point, ref B, out point);
return;
}
//On the face of the triangle.
float denom = 1f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;
Vector3.Multiply(ref ab, v, out point);
Vector3 acw;
Vector3.Multiply(ref ac, w, out acw);
Vector3.Add(ref A, ref point, out point);
Vector3.Add(ref point, ref acw, out point);
}
///<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.
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>
/// 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 Vector3 sweepA, ref Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB,
out RayHit hit)
{
//Put the velocity into shapeA's local space.
Vector3 velocityWorld;
Vector3.Subtract(ref sweepB, ref sweepA, out velocityWorld);
Quaternion conjugateOrientationA;
Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
Vector3 rayDirection;
Vector3.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);
Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
Vector3.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
Vector3 w, p;
hit.T = 0;
hit.Location = Vector3.Zero; //The ray starts at the origin.
hit.Normal = Toolbox.ZeroVector;
Vector3 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);
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 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.
Vector3.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() >= Toolbox.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.
Vector3.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
Vector3.Multiply(ref velocityWorld, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref transformA.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.
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 extremePointToRayOrigin, extremePoint;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
Vector3 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() >= 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.GetLocalExtremePoint(closestOffset, out extremePoint);
Vector3.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin);
Vector3.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)
{
Vector3.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.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.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.
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);
}