public override bool ConvexCast(ConvexShape castShape, ref MathExtensions.RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { return MPRToolbox.Sweep(castShape, Shape, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref worldTransform, out hit); }
public override bool ConvexCast(ConvexShape castShape, ref MathExtensions.RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { hit = new RayHit(); BoundingBox boundingBox; Toolbox.GetExpandedBoundingBox(ref castShape, ref startingTransform, ref sweep, out boundingBox); var tri = Resources.GetTriangle(); var hitElements = Resources.GetIntList(); if (triangleMesh.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { triangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, 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; Resources.GiveBack(tri); Resources.GiveBack(hitElements); return hit.T != float.MaxValue; } Resources.GiveBack(tri); Resources.GiveBack(hitElements); return false; }