/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//If the convex cast is inside the mesh and the mesh is solid, it should return t = 0.
var ray = new Ray()
{
Position = startingTransform.Position, Direction = Toolbox.UpVector
};
if (Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
hit = new RayHit()
{
Location = startingTransform.Position, Normal = new Vector3(), T = 0
};
return(true);
}
}
hit = new RayHit();
BoundingBox boundingBox;
var transform = new AffineTransform {
Translation = worldTransform.Position
};
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform);
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox);
var tri = PhysicsThreadResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref transform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref transform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref transform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform {
Orientation = Quaternion.Identity, Position = center
};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = 0;
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(hit.T != float.MaxValue);
}
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(false);
}
///<summary>
/// Constructs a convex shape entry with identity transformation.
///</summary>
///<param name="shape">Shape of the entry.</param>
public ConvexShapeEntry(ConvexShape shape)
{
Transform = RigidTransform.Identity;
CollisionShape = shape;
}
///<summary>
/// Initializes the pair tester.
///</summary>
///<param name="convex">Convex shape to use.</param>
public override void Initialize(ConvexShape convex)
{
this.sphere = (SphereShape)convex;
}
/// <summary>
/// Constructs a convex shape entry.
/// </summary>
/// <param name="orientation">Local orientation of the entry.</param>
/// <param name="shape">Shape of the entry.</param>
public ConvexShapeEntry(Quaternion orientation, ConvexShape shape)
{
Transform = new RigidTransform(orientation);
CollisionShape = shape;
}
/// <summary>
/// Constructs a convex shape entry.
/// </summary>
/// <param name="transform">Local transform of the entry.</param>
/// <param name="shape">Shape of the entry.</param>
public ConvexShapeEntry(RigidTransform transform, ConvexShape shape)
{
Transform = transform;
CollisionShape = shape;
}
/// <summary> /// Sweeps a convex shape against the entry. /// </summary> /// <param name="castShape">Swept shape.</param> /// <param name="startingTransform">Beginning location and orientation of the cast shape.</param> /// <param name="sweep">Sweep motion to apply to the cast shape.</param> /// <param name="hit">Hit data of the cast on the entry, if any.</param> /// <returns>Whether or not the cast hit the entry.</returns> public abstract bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit);
/// <summary>
/// Constructs a convex shape entry.
/// </summary>
/// <param name="position">Local position of the entry.</param>
/// <param name="shape">Shape of the entry.</param>
public ConvexShapeEntry(Vector3 position, ConvexShape shape)
{
Transform = new RigidTransform(position);
CollisionShape = shape;
}
///<summary>
/// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA, without a margin.
///</summary>
///<param name="shapeA">First shape.</param>
///<param name="shapeB">Second shape.</param>
///<param name="direction">Extreme point direction in local space.</param>
///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
///<param name="extremePoint">The extreme point in the local space of A.</param>
public static void GetLocalMinkowskiExtremePointWithoutMargin(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint)
{
//Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
Vector3 extremePointB;
Vector3 negativeN;
Vector3.Negate(ref direction, out negativeN);
shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out extremePointB);
Vector3.Subtract(ref extremePoint, ref extremePointB, out extremePoint);
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="shapeA">First shape in the pair.</param>
///<param name="shapeB">Second shape in the pair.</param>
///<param name="iterationCount">Current iteration count.</param>
///<param name="closestPoint">Current point on simplex closest to origin.</param>
///<returns>Whether or not GJK should exit due to a lack of progression.</returns>
public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint)
{
Vector3 negativeDirection;
Vector3.Negate(ref closestPoint, out negativeDirection);
Vector3 sa, sb;
shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
Vector3 S;
Vector3.Subtract(ref sa, ref sb, out S);
//If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
float dotS;
Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S
float distanceToClosest = closestPoint.LengthSquared();
float progression = dotS + distanceToClosest;
//It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
//Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
//During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
//After a bunch of iterations, the system lets it pick the 'better' one.
if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance)
{
return(true);
}
if (distanceToClosest < previousDistanceToClosest)
{
previousDistanceToClosest = distanceToClosest;
}
//If "A" is the new point always, then the switch statement can be removed
//in favor of just pushing three points up.
switch (State)
{
case SimplexState.Point:
if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Segment;
B = S;
SimplexA.B = sa;
SimplexB.B = sb;
return(false);
case SimplexState.Segment:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Triangle;
C = S;
SimplexA.C = sa;
SimplexB.C = sb;
return(false);
case SimplexState.Triangle:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Tetrahedron;
D = S;
SimplexA.D = sa;
SimplexB.D = sb;
return(false);
}
return(false);
}
///<summary>
/// Initializes the pair tester.
///</summary>
///<param name="convex">Convex shape to use.</param>
public override void Initialize(ConvexShape convex)
{
this.convex = convex;
}
///<summary>
/// Constructs a new entry with identity orientation.
///</summary>
///<param name="shape">Shape of the entry.</param>
public OrientedConvexShapeEntry(ConvexShape shape)
{
Orientation = Quaternion.Identity;
CollisionShape = shape;
}
///<summary>
/// Constructs a new entry.
///</summary>
///<param name="orientation">Orientation of the entry.</param>
///<param name="shape">Shape of the entry.</param>
public OrientedConvexShapeEntry(Quaternion orientation, ConvexShape shape)
{
Orientation = orientation;
CollisionShape = shape;
}