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 unsafe override void Initialize(ContentArchive content, Camera camera)
{
{
SphereTriangleTester tester;
SphereWide sphere = default;
sphere.Broadcast(new Sphere(0.5f));
TriangleWide triangle = default;
var a = new Vector3(0, 0, 0);
var b = new Vector3(1, 0, 0);
var c = new Vector3(0, 0, 1);
//var center = (a + b + c) / 3f;
//a -= center;
//b -= center;
//c -= center;
triangle.Broadcast(new Triangle(a, b, c));
var margin = new Vector <float>(1f);
Vector3Wide.Broadcast(new Vector3(1, -1, 0), out var offsetB);
QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), out var orientationB);
tester.Test(ref sphere, ref triangle, ref margin, ref offsetB, ref orientationB, Vector <float> .Count, out var manifold);
}
{
CapsuleTriangleTester tester;
CapsuleWide capsule = default;
capsule.Broadcast(new Capsule(0.5f, 0.5f));
TriangleWide triangle = default;
var a = new Vector3(0, 0, 0);
var b = new Vector3(1, 0, 0);
var c = new Vector3(0, 0, 1);
//var center = (a + b + c) / 3f;
//a -= center;
//b -= center;
//c -= center;
triangle.Broadcast(new Triangle(a, b, c));
var margin = new Vector <float>(2f);
Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB);
QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA);
QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB);
tester.Test(ref capsule, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector <float> .Count, out var manifold);
}
{
BoxTriangleTester tester;
BoxWide shape = default;
shape.Broadcast(new Box(1f, 1f, 1f));
TriangleWide triangle = default;
var a = new Vector3(0, 0, 0);
var b = new Vector3(1, 0, 0);
var c = new Vector3(0, 0, 1);
//var center = (a + b + c) / 3f;
//a -= center;
//b -= center;
//c -= center;
triangle.Broadcast(new Triangle(a, b, c));
var margin = new Vector <float>(2f);
Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB);
QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA);
QuaternionWide.Broadcast(QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB);
tester.Test(ref shape, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector <float> .Count, out var manifold);
}
{
TrianglePairTester tester;
TriangleWide a = default, b = default;
public static void Test <TShape, TShapeWide, TTester>() where TShape : IConvexShape where TTester : IRayTester <TShape> where TShapeWide : IShapeWide <TShape>
{
const int shapeIterations = 1000;
const int transformIterations = 100;
const int outsideToInsideRays = 100;
const int insideRays = 10;
const int outsideRays = 100;
const int outwardPointingRays = 100;
const float volumeInnerMargin = 1e-4f;
const float positionBoundsSpan = 100;
const float positionMin = positionBoundsSpan * -0.5f;
const float outsideMinimumDistance = 0.02f;
const float outsideDistanceSpan = 1000;
const float tangentMinimumDistance = 0.02f;
const float tangentDistanceSpan = 10;
const float tangentCentralExclusionMin = 0.01f;
const float tangentCentralExclusionSpan = 10;
const float tangentSourceSpanMin = 0.01f;
const float tangentSourceSpanSpan = 1000f;
const float outwardPointingSpan = 1000f;
var tester = default(TTester);
Random random = new Random(5);
TShapeWide shapeWide = default;
for (int shapeIteration = 0; shapeIteration < shapeIterations; ++shapeIteration)
{
tester.GetRandomShape(random, out var shape);
shapeWide.Broadcast(shape);
for (int transformIteration = 0; transformIteration < transformIterations; ++transformIteration)
{
RigidPose pose;
pose.Position = new Vector3(positionMin) + positionBoundsSpan * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
GetUnitQuaternion(random, out pose.Orientation);
Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation);
RigidPoses poses;
Vector3Wide.Broadcast(pose.Position, out poses.Position);
QuaternionWide.Broadcast(pose.Orientation, out poses.Orientation);
for (int rayIndex = 0; rayIndex < outsideToInsideRays; ++rayIndex)
{
tester.GetSurface(random, ref shape, out var pointOnSurface, out var normal);
var localSourcePoint = pointOnSurface + normal * (outsideMinimumDistance + (float)random.NextDouble() * outsideDistanceSpan);
tester.GetPointInVolume(random, volumeInnerMargin, ref shape, out var localTargetPoint);
Matrix3x3.Transform(localSourcePoint, orientation, out var sourcePoint);
sourcePoint += pose.Position;
var directionScale = (0.01f + 2 * (float)random.NextDouble());
var localDirection = (localTargetPoint - localSourcePoint) * directionScale;
Matrix3x3.Transform(localDirection, orientation, out var direction);
bool intersected;
if (intersected = shape.RayTest(pose, sourcePoint, direction, out var t, out var rayTestedNormal))
{
//If the ray start is outside the shape and the target point is inside, then the ray impact should exist on the surface of the shape.
var hitLocation = sourcePoint + t * direction;
var localHitLocation = hitLocation - pose.Position;
Matrix3x3.TransformTranspose(localHitLocation, orientation, out localHitLocation);
if (!tester.PointIsOnSurface(ref shape, ref localHitLocation))
{
Console.WriteLine("Outside->inside ray detected non-surface impact.");
}
}
public static void Test()
{
var random = new Random(4);
var timer = new Stopwatch();
var symmetricVectorSandwichTime = 0.0;
var symmetricWideVectorSandwichTime = 0.0;
var triangularWideVectorSandwichTime = 0.0;
var symmetricWide2x3SandwichTime = 0.0;
var triangularWide2x3SandwichTime = 0.0;
var symmetricSkewSandwichTime = 0.0;
var symmetricWideSkewSandwichTime = 0.0;
var triangularWideSkewSandwichTime = 0.0;
var symmetricRotationSandwichTime = 0.0;
var symmetricWideRotationSandwichTime = 0.0;
var triangularWideRotationSandwichTime = 0.0;
var symmetricInvertTime = 0.0;
var symmetricWideInvertTime = 0.0;
var triangularWideInvertTime = 0.0;
for (int i = 0; i < 1000; ++i)
{
var axis = Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1));
Vector3Wide.Broadcast(axis, out var axisWide);
var rotation = Matrix3x3.CreateFromAxisAngle(axis, (float)random.NextDouble());
Matrix3x3Wide rotationWide;
Vector3Wide.Broadcast(rotation.X, out rotationWide.X);
Vector3Wide.Broadcast(rotation.Y, out rotationWide.Y);
Vector3Wide.Broadcast(rotation.Z, out rotationWide.Z);
var m2x3Wide = new Matrix2x3Wide()
{
X = axisWide, Y = new Vector3Wide {
X = -axisWide.Y, Y = axisWide.Z, Z = axisWide.X
}
};
var triangular = new Symmetric3x3
{
XX = (float)random.NextDouble() * 2 + 1,
YX = (float)random.NextDouble() * 1 + 1,
YY = (float)random.NextDouble() * 2 + 1,
ZX = (float)random.NextDouble() * 1 + 1,
ZY = (float)random.NextDouble() * 1 + 1,
ZZ = (float)random.NextDouble() * 2 + 1,
};
Symmetric3x3Wide triangularWide;
triangularWide.XX = new Vector <float>(triangular.XX);
triangularWide.YX = new Vector <float>(triangular.YX);
triangularWide.YY = new Vector <float>(triangular.YY);
triangularWide.ZX = new Vector <float>(triangular.ZX);
triangularWide.ZY = new Vector <float>(triangular.ZY);
triangularWide.ZZ = new Vector <float>(triangular.ZZ);
var symmetric = new Matrix3x3
{
X = new Vector3(triangular.XX, triangular.YX, triangular.ZX),
Y = new Vector3(triangular.YX, triangular.YY, triangular.ZY),
Z = new Vector3(triangular.ZX, triangular.ZY, triangular.ZZ),
};
Matrix3x3Wide symmetricWide;
Vector3Wide.Broadcast(symmetric.X, out symmetricWide.X);
Vector3Wide.Broadcast(symmetric.Y, out symmetricWide.Y);
Vector3Wide.Broadcast(symmetric.Z, out symmetricWide.Z);
var symmetricVectorSandwich = new SymmetricVectorSandwich()
{
v = axis, symmetric = symmetric
};
var symmetricWideVectorSandwich = new SymmetricWideVectorSandwich()
{
v = axisWide, symmetric = symmetricWide
};
var triangularWideVectorSandwich = new TriangularWideVectorSandwich()
{
v = axisWide, triangular = triangularWide
};
var symmetricWide2x3Sandwich = new SymmetricWide2x3Sandwich()
{
m = m2x3Wide, symmetric = symmetricWide
};
var triangularWide2x3Sandwich = new TriangularWide2x3Sandwich()
{
m = m2x3Wide, triangular = triangularWide
};
var symmetricSkewSandwich = new SymmetricSkewSandwich()
{
v = axis, symmetric = symmetric
};
var symmetricWideSkewSandwich = new SymmetricWideSkewSandwich()
{
v = axisWide, symmetric = symmetricWide
};
var triangularWideSkewSandwich = new TriangularWideSkewSandwich()
{
v = axisWide, triangular = triangularWide
};
var symmetricSandwich = new SymmetricRotationSandwich()
{
rotation = rotation, symmetric = symmetric
};
var symmetricWideSandwich = new SymmetricRotationSandwichWide()
{
rotation = rotationWide, symmetric = symmetricWide
};
var triangularWideSandwich = new TriangularRotationSandwichWide()
{
rotation = rotationWide, triangular = triangularWide
};
var symmetricInvert = new SymmetricInvert()
{
symmetric = symmetric
};
var symmetricWideInvert = new SymmetricInvertWide()
{
symmetric = symmetricWide
};
var triangularWideInvert = new TriangularInvertWide()
{
triangular = triangularWide
};
const int innerIterations = 100000;
symmetricVectorSandwichTime += TimeTest(innerIterations, ref symmetricVectorSandwich);
symmetricWideVectorSandwichTime += TimeTest(innerIterations, ref symmetricWideVectorSandwich);
triangularWideVectorSandwichTime += TimeTest(innerIterations, ref triangularWideVectorSandwich);
symmetricWide2x3SandwichTime += TimeTest(innerIterations, ref symmetricWide2x3Sandwich);
triangularWide2x3SandwichTime += TimeTest(innerIterations, ref triangularWide2x3Sandwich);
symmetricSkewSandwichTime += TimeTest(innerIterations, ref symmetricSkewSandwich);
symmetricWideSkewSandwichTime += TimeTest(innerIterations, ref symmetricWideSkewSandwich);
triangularWideSkewSandwichTime += TimeTest(innerIterations, ref triangularWideSkewSandwich);
symmetricRotationSandwichTime += TimeTest(innerIterations, ref symmetricSandwich);
symmetricWideRotationSandwichTime += TimeTest(innerIterations, ref symmetricWideSandwich);
triangularWideRotationSandwichTime += TimeTest(innerIterations, ref triangularWideSandwich);
symmetricInvertTime += TimeTest(innerIterations, ref symmetricInvert);
symmetricWideInvertTime += TimeTest(innerIterations, ref symmetricWideInvert);
triangularWideInvertTime += TimeTest(innerIterations, ref triangularWideInvert);
Compare(symmetricVectorSandwich.result, ref symmetricWideVectorSandwich.result);
Compare(symmetricVectorSandwich.result, ref triangularWideVectorSandwich.result);
Compare(ref symmetricWide2x3Sandwich.result, ref triangularWide2x3Sandwich.result);
Compare(ref symmetricSkewSandwich.result, ref symmetricWideSkewSandwich.result);
Compare(ref symmetricSkewSandwich.result, ref triangularWideSkewSandwich.result);
Compare(ref symmetricSandwich.result, ref symmetricWideSandwich.result);
Compare(ref symmetricSandwich.result, ref triangularWideSandwich.result);
Compare(ref symmetricInvert.result, ref symmetricWideInvert.result);
Compare(ref symmetricInvert.result, ref triangularWideInvert.result);
}
Console.WriteLine($"Symmetric vector sandwich: {symmetricVectorSandwichTime}");
Console.WriteLine($"Symmetric wide vector sandwich: {symmetricWideVectorSandwichTime}");
Console.WriteLine($"Triangular wide vector sandwich: {triangularWideVectorSandwichTime}");
Console.WriteLine($"Symmetric wide 2x3 sandwich: {symmetricWide2x3SandwichTime}");
Console.WriteLine($"Triangular wide 2x3 sandwich: {triangularWide2x3SandwichTime}");
Console.WriteLine($"Symmetric skew sandwich: {symmetricSkewSandwichTime}");
Console.WriteLine($"Symmetric wide skew sandwich: {symmetricWideSkewSandwichTime}");
Console.WriteLine($"Triangular wide skew sandwich: {triangularWideSkewSandwichTime}");
Console.WriteLine($"Symmetric rotation sandwich: {symmetricRotationSandwichTime}");
Console.WriteLine($"Symmetric wide rotation sandwich: {symmetricWideRotationSandwichTime}");
Console.WriteLine($"Triangular wide rotation sandwich: {triangularWideRotationSandwichTime}");
Console.WriteLine($"Symmetric invert: {symmetricInvertTime}");
Console.WriteLine($"Symmetric wide invert: {symmetricWideInvertTime}");
Console.WriteLine($"Triangular wide invert: {triangularWideInvertTime}");
}