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); }