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 = PhysicsResources.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;
PhysicsResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return hit.T != float.MaxValue;
}
PhysicsResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return false;
}
}
