/// <summary>
///
/// </summary>
/// <param name="szx"></param>
/// <param name="szy"></param>
/// <param name="szz"></param>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="normal"></param>
/// <param name="pos"></param>
/// <returns></returns>
public bool ConvexCastAgainstStatic(ConvexShape shape, Vector3 from, Vector3 to, out Vector3 normal, out Vector3 pos)
{
BoundingBox bbox;
Vector3 sweep = to - from;
AffineTransform identity = AffineTransform.Identity;
RigidTransform transform = new RigidTransform(from);
CHECK(from);
CHECK(to);
normal = Vector3.Zero;
pos = to;
shape.GetSweptLocalBoundingBox(ref transform, ref identity, ref sweep, out bbox);
var candidates = Resources.GetCollisionEntryList();
Space.BroadPhase.QueryAccelerator.BroadPhase.QueryAccelerator.GetEntries(bbox, candidates);
float minT = float.MaxValue;
bool hit = false;
foreach (var candidate in candidates)
{
if (candidate as ConvexCollidable != null)
{
continue;
}
RayHit rayHit;
bool r = candidate.ConvexCast(shape, ref transform, ref sweep, out rayHit);
if (!r)
{
continue;
}
if (minT > rayHit.T)
{
hit = true;
minT = rayHit.T;
normal = rayHit.Normal;
pos = rayHit.Location;
}
}
return(hit);
}
/// <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 = F64.C0
};
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 = Fix64.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, F64.OneThird, 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();
Fix64 radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
tri.MaximumRadius = Fix64.Sqrt(tri.MaximumRadius);
tri.collisionMargin = F64.C0;
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 = F64.C0;
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(hit.T != Fix64.MaxValue);
}
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(false);
}
