static void CheckWide <TShape, TShapeWide>(ref RigidPoses poses, ref TShapeWide shapeWide, ref Vector3 origin, ref Vector3 direction, bool intersected, float t, ref Vector3 normal)
where TShape : IConvexShape where TShapeWide : IShapeWide <TShape>
{
RayWide rayWide;
Vector3Wide.Broadcast(origin, out rayWide.Origin);
Vector3Wide.Broadcast(direction, out rayWide.Direction);
shapeWide.RayTest(ref poses, ref rayWide, out var intersectedWide, out var tWide, out var normalWide);
if (intersectedWide[0] < 0 != intersected)
{
Console.WriteLine($"Wide ray boolean result disagrees with scalar ray.");
}
if (intersected && intersectedWide[0] < 0)
{
if (Math.Abs(tWide[0] - t) > 1e-7f)
{
Console.WriteLine("Wide ray t disagrees with scalar ray.");
}
if (Math.Abs(normalWide.X[0] - normal.X) > 1e-7f ||
Math.Abs(normalWide.Y[0] - normal.Y) > 1e-7f ||
Math.Abs(normalWide.Z[0] - normal.Z) > 1e-7f)
{
Console.WriteLine("Wide ray normal disagrees with scalar ray.");
}
}
}
public void GatherCollidableBundle(int collidablesStartIndex, int count, out Vector <int> shapeIndices, out Vector <float> maximumExpansion, out RigidPoses poses, out BodyVelocities velocities)
{
Bodies.GatherDataForBounds(ref BodyIndices[collidablesStartIndex], count, out poses, out velocities, out shapeIndices, out maximumExpansion);
}
