/// <summary>
/// Precomputes the transform to bring triangles from their native local space to the local space of the convex.
/// </summary>
/// <param name="convexInverseWorldTransform">Inverse of the world transform of the convex shape.</param>
/// <param name="fromMeshLocalToConvexLocal">Transform to apply to native local triangles to bring them into the local space of the convex.</param>
protected override void PrecomputeTriangleTransform(ref AffineTransform convexInverseWorldTransform, out AffineTransform fromMeshLocalToConvexLocal)
{
//StaticMeshes only have transformable mesh data.
var data = ((TransformableMeshData)mesh.Mesh.Data);
AffineTransform.Multiply(ref data.worldTransform, ref convexInverseWorldTransform, out fromMeshLocalToConvexLocal);
}
/// <summary>
/// Precomputes the transform to bring triangles from their native local space to the local space of the convex.
/// </summary>
/// <param name="convexInverseWorldTransform">Inverse of the world transform of the convex shape.</param>
/// <param name="fromMeshLocalToConvexLocal">Transform to apply to native local triangles to bring them into the local space of the convex.</param>
protected override void PrecomputeTriangleTransform(ref AffineTransform convexInverseWorldTransform,
out AffineTransform fromMeshLocalToConvexLocal)
{
//InstancedMeshShapes don't have a shape-level transform. The instance transform is all there is.
AffineTransform.Multiply(ref mesh.worldTransform, ref convexInverseWorldTransform,
out fromMeshLocalToConvexLocal);
}
//Transform the convex into the space of something else.
/// <summary>
/// Gets the bounding box of the convex shape transformed first into world space, and then into the local space of another affine transform.
/// </summary>
/// <param name="shapeTransform">Transform to use to put the shape into world space.</param>
/// <param name="spaceTransform">Used as the frame of reference to compute the bounding box.
/// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space.</param>
/// <param name="boundingBox">Bounding box in the local space.</param>
public void GetLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
//TODO: This method peforms quite a few sqrts because the collision margin can get scaled, and so cannot be applied as a final step.
//There should be a better way to do this.
//Additionally, this bounding box is not consistent in all cases with the post-add version. Adding the collision margin at the end can
//slightly overestimate the size of a margin expanded shape at the corners, which is fine (and actually important for the box-box special case).
//Move forward into convex's space, backwards into the new space's local space.
AffineTransform transform;
AffineTransform.Invert(ref spaceTransform, out transform);
AffineTransform.Multiply(ref shapeTransform, ref transform, out transform);
//Sample the local directions from the orientation matrix, implicitly transposed.
Vector3 right;
var direction = new Vector3(transform.LinearTransform.M11, transform.LinearTransform.M21, transform.LinearTransform.M31);
GetLocalExtremePoint(direction, out right);
Vector3 left;
direction = new Vector3(-transform.LinearTransform.M11, -transform.LinearTransform.M21, -transform.LinearTransform.M31);
GetLocalExtremePoint(direction, out left);
Vector3 up;
direction = new Vector3(transform.LinearTransform.M12, transform.LinearTransform.M22, transform.LinearTransform.M32);
GetLocalExtremePoint(direction, out up);
Vector3 down;
direction = new Vector3(-transform.LinearTransform.M12, -transform.LinearTransform.M22, -transform.LinearTransform.M32);
GetLocalExtremePoint(direction, out down);
Vector3 backward;
direction = new Vector3(transform.LinearTransform.M13, transform.LinearTransform.M23, transform.LinearTransform.M33);
GetLocalExtremePoint(direction, out backward);
Vector3 forward;
direction = new Vector3(-transform.LinearTransform.M13, -transform.LinearTransform.M23, -transform.LinearTransform.M33);
GetLocalExtremePoint(direction, out forward);
//This could be optimized. Unnecessary transformation information gets computed.
Matrix3X3.Transform(ref right, ref transform.LinearTransform, out right);
Matrix3X3.Transform(ref left, ref transform.LinearTransform, out left);
Matrix3X3.Transform(ref up, ref transform.LinearTransform, out up);
Matrix3X3.Transform(ref down, ref transform.LinearTransform, out down);
Matrix3X3.Transform(ref backward, ref transform.LinearTransform, out backward);
Matrix3X3.Transform(ref forward, ref transform.LinearTransform, out forward);
//These right/up/backward represent the extreme points in world space along the world space axes.
boundingBox.Max.X = transform.Translation.X + right.X;
boundingBox.Max.Y = transform.Translation.Y + up.Y;
boundingBox.Max.Z = transform.Translation.Z + backward.Z;
boundingBox.Min.X = transform.Translation.X + left.X;
boundingBox.Min.Y = transform.Translation.Y + down.Y;
boundingBox.Min.Z = transform.Translation.Z + forward.Z;
}
//Transform the convex into the space of something else.
/// <summary>
/// Gets the bounding box of the convex shape transformed first into world space, and then into the local space of another affine transform.
/// </summary>
/// <param name="shapeTransform">Transform to use to put the shape into world space.</param>
/// <param name="spaceTransform">Used as the frame of reference to compute the bounding box.
/// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space.</param>
/// <param name="boundingBox">Bounding box in the local space.</param>
public void GetLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
//TODO: This method peforms quite a few sqrts because the collision margin can get scaled, and so cannot be applied as a final step.
//There should be a better way to do this. At the very least, it should be possible to avoid the 6 square roots involved currently.
//If this shows a a bottleneck, it might be best to virtualize this function and implement a per-shape variant.
//Also... It might be better just to have the internal function be a GetBoundingBox that takes an AffineTransform, and an outer function
//does the local space fiddling.
//Move forward into convex's space, backwards into the new space's local space.
AffineTransform transform;
AffineTransform.Invert(ref spaceTransform, out transform);
AffineTransform.Multiply(ref shapeTransform, ref transform, out transform);
//Sample the local directions from the orientation matrix, implicitly transposed.
Vector3 right;
var direction = new Vector3(transform.LinearTransform.M11, transform.LinearTransform.M21, transform.LinearTransform.M31);
GetLocalExtremePoint(direction, out right);
Vector3 left;
direction = new Vector3(-transform.LinearTransform.M11, -transform.LinearTransform.M21, -transform.LinearTransform.M31);
GetLocalExtremePoint(direction, out left);
Vector3 up;
direction = new Vector3(transform.LinearTransform.M12, transform.LinearTransform.M22, transform.LinearTransform.M32);
GetLocalExtremePoint(direction, out up);
Vector3 down;
direction = new Vector3(-transform.LinearTransform.M12, -transform.LinearTransform.M22, -transform.LinearTransform.M32);
GetLocalExtremePoint(direction, out down);
Vector3 backward;
direction = new Vector3(transform.LinearTransform.M13, transform.LinearTransform.M23, transform.LinearTransform.M33);
GetLocalExtremePoint(direction, out backward);
Vector3 forward;
direction = new Vector3(-transform.LinearTransform.M13, -transform.LinearTransform.M23, -transform.LinearTransform.M33);
GetLocalExtremePoint(direction, out forward);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
Vector3 positive, negative;
TransformLocalExtremePoints(ref right, ref up, ref backward, ref transform.LinearTransform, out positive);
TransformLocalExtremePoints(ref left, ref down, ref forward, ref transform.LinearTransform, out negative);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
boundingBox.Max.X = transform.Translation.X + positive.X;
boundingBox.Max.Y = transform.Translation.Y + positive.Y;
boundingBox.Max.Z = transform.Translation.Z + positive.Z;
boundingBox.Min.X = transform.Translation.X + negative.X;
boundingBox.Min.Y = transform.Translation.Y + negative.Y;
boundingBox.Min.Z = transform.Translation.Z + negative.Z;
}
/// <summary>
/// Precomputes the transform to bring triangles from their native local space to the local space of the convex.
/// </summary>
/// <param name="convexInverseWorldTransform">Inverse of the world transform of the convex shape.</param>
/// <param name="fromMeshLocalToConvexLocal">Transform to apply to native local triangles to bring them into the local space of the convex.</param>
protected override void PrecomputeTriangleTransform(ref AffineTransform convexInverseWorldTransform, out AffineTransform fromMeshLocalToConvexLocal)
{
//MobileMeshes only have TransformableMeshData sources.
var data = ((TransformableMeshData)mesh.Shape.TriangleMesh.Data);
//The mobile mesh has a shape-based transform followed by the rigid body transform.
AffineTransform mobileMeshWorldTransform;
AffineTransform.CreateFromRigidTransform(ref mesh.worldTransform, out mobileMeshWorldTransform);
AffineTransform combinedMobileMeshWorldTransform;
AffineTransform.Multiply(ref data.worldTransform, ref mobileMeshWorldTransform, out combinedMobileMeshWorldTransform);
AffineTransform.Multiply(ref combinedMobileMeshWorldTransform, ref convexInverseWorldTransform, out fromMeshLocalToConvexLocal);
}
//public static float TestScalarMultiply(int iterationCount)
//{
// bAffineTransform m1 = bAffineTransform.Identity;
// bAffineTransform m2 = bAffineTransform.Identity;
// float accumulator = 0;
// for (int i = 0; i < iterationCount; ++i)
// {
// bAffineTransform r0, r1;
// bAffineTransform.Multiply(ref m1, ref m2, out r0);
// bAffineTransform.Multiply(ref r0, ref m2, out r1);
// bAffineTransform.Multiply(ref r1, ref m2, out r0);
// bAffineTransform.Multiply(ref r0, ref m2, out r1);
// bAffineTransform.Multiply(ref r1, ref m2, out r0);
// bAffineTransform.Multiply(ref r0, ref m2, out r1);
// bAffineTransform.Multiply(ref r1, ref m2, out r0);
// bAffineTransform.Multiply(ref r0, ref m2, out r1);
// bAffineTransform.Multiply(ref r1, ref m2, out r0);
// bAffineTransform.Multiply(ref r0, ref m2, out r1);
// accumulator += 0.000001f * r1.Translation.X;
// }
// return accumulator;
//}
public static float TestSIMDMultiply(int iterationCount)
{
AffineTransform m1 = AffineTransform.Identity;
AffineTransform m2 = AffineTransform.Identity;
float accumulator = 0;
for (int i = 0; i < iterationCount; ++i)
{
AffineTransform r0, r1;
AffineTransform.Multiply(m1, m2, out r0);
AffineTransform.Multiply(r0, m2, out r1);
AffineTransform.Multiply(r1, m2, out r0);
AffineTransform.Multiply(r0, m2, out r1);
AffineTransform.Multiply(r1, m2, out r0);
AffineTransform.Multiply(r0, m2, out r1);
AffineTransform.Multiply(r1, m2, out r0);
AffineTransform.Multiply(r0, m2, out r1);
AffineTransform.Multiply(r1, m2, out r0);
AffineTransform.Multiply(r0, m2, out r1);
accumulator += 0.000001f * r1.Translation.X;
}
return(accumulator);
}
/// <summary>
/// Gets the bounding box of the mesh transformed first into world space, and then into the local space of another affine transform.
/// </summary>
/// <param name="shapeTransform">Transform to use to put the shape into world space.</param>
/// <param name="spaceTransform">Used as the frame of reference to compute the bounding box.
/// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space.</param>
/// <param name="boundingBox">Bounding box in the local space.</param>
public void GetLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
//TODO: This method peforms quite a few sqrts because the collision margin can get scaled, and so cannot be applied as a final step.
//There should be a better way to do this.
//Additionally, this bounding box is not consistent in all cases with the post-add version. Adding the collision margin at the end can
//slightly overestimate the size of a margin expanded shape at the corners, which is fine (and actually important for the box-box special case).
//Move forward into convex's space, backwards into the new space's local space.
AffineTransform transform;
AffineTransform.Invert(ref spaceTransform, out transform);
AffineTransform.Multiply(ref shapeTransform, ref transform, out transform);
GetBoundingBox(ref transform.LinearTransform, out boundingBox);
boundingBox.Max.X += transform.Translation.X;
boundingBox.Max.Y += transform.Translation.Y;
boundingBox.Max.Z += transform.Translation.Z;
boundingBox.Min.X += transform.Translation.X;
boundingBox.Min.Y += transform.Translation.Y;
boundingBox.Min.Z += transform.Translation.Z;
}
/// <summary>
/// Precomputes the transform to bring triangles from their native local space to the local space of the convex.
/// </summary>
/// <param name="convexInverseWorldTransform">Inverse of the world transform of the convex shape.</param>
/// <param name="fromMeshLocalToConvexLocal">Transform to apply to native local triangles to bring them into the local space of the convex.</param>
protected override void PrecomputeTriangleTransform(ref AffineTransform convexInverseWorldTransform, out AffineTransform fromMeshLocalToConvexLocal)
{
AffineTransform.Multiply(ref terrain.worldTransform, ref convexInverseWorldTransform, out fromMeshLocalToConvexLocal);
}
public unsafe static void TestMultiplyCorrectness()
{
const int iterationCount = 100000;
Random random = new Random(5);
for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex)
{
AffineTransform simdA, simdB;
bAffineTransform scalarA, scalarB;
var simdPointerA = (float *)&simdA;
var scalarPointerA = (float *)&scalarA;
var simdPointerB = (float *)&simdB;
var scalarPointerB = (float *)&scalarB;
for (int i = 0; i < 12; ++i)
{
scalarPointerA[i] = simdPointerA[i] = (float)(random.NextDouble() * 4 - 2);
scalarPointerB[i] = simdPointerB[i] = (float)(random.NextDouble() * 4 - 2);
}
AffineTransform simdResult;
AffineTransform.Multiply(ref simdA, ref simdB, out simdResult);
bAffineTransform scalarResult;
bAffineTransform.Multiply(ref scalarA, ref scalarB, out scalarResult);
var simdPointerResult = (float *)&simdResult;
var scalarPointerResult = (float *)&scalarResult;
for (int i = 0; i < 12; ++i)
{
const float threshold = 1e-5f;
var simdScalarError = Math.Abs(simdPointerResult[i] - scalarPointerResult[i]);
if (simdScalarError > threshold)
{
Console.WriteLine($"Excess error for {i}");
}
}
}
for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex)
{
AffineTransform simdA, simdB;
bAffineTransform scalarA, scalarB;
var simdPointerA = (float *)&simdA;
var scalarPointerA = (float *)&scalarA;
var simdPointerB = (float *)&simdB;
var scalarPointerB = (float *)&scalarB;
for (int i = 0; i < 12; ++i)
{
scalarPointerA[i] = simdPointerA[i] = (float)(random.NextDouble() * 4 - 2);
scalarPointerB[i] = simdPointerB[i] = (float)(random.NextDouble() * 4 - 2);
}
AffineTransform.Multiply(ref simdA, ref simdB, out simdA);
bAffineTransform.Multiply(ref scalarA, ref scalarB, out scalarA);
for (int i = 0; i < 12; ++i)
{
const float threshold = 1e-5f;
var simdScalarError = Math.Abs(simdPointerA[i] - scalarPointerA[i]);
if (simdScalarError > threshold)
{
Console.WriteLine($"Excess error for {i}");
}
}
}
for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex)
{
AffineTransform simdA, simdB;
bAffineTransform scalarA, scalarB;
var simdPointerA = (float *)&simdA;
var scalarPointerA = (float *)&scalarA;
var simdPointerB = (float *)&simdB;
var scalarPointerB = (float *)&scalarB;
for (int i = 0; i < 12; ++i)
{
scalarPointerA[i] = simdPointerA[i] = (float)(random.NextDouble() * 4 - 2);
scalarPointerB[i] = simdPointerB[i] = (float)(random.NextDouble() * 4 - 2);
}
AffineTransform.Multiply(ref simdA, ref simdB, out simdB);
bAffineTransform.Multiply(ref scalarA, ref scalarB, out scalarB);
for (int i = 0; i < 12; ++i)
{
const float threshold = 1e-5f;
var simdScalarError = Math.Abs(simdPointerB[i] - scalarPointerB[i]);
if (simdScalarError > threshold)
{
Console.WriteLine($"Excess error for {i}");
}
}
}
for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex)
{
AffineTransform simd;
bAffineTransform scalar;
var simdPointer = (float *)&simd;
var scalarPointer = (float *)&scalar;
for (int i = 0; i < 12; ++i)
{
scalarPointer[i] = simdPointer[i] = (float)(random.NextDouble() * 4 - 2);
}
AffineTransform.Multiply(ref simd, ref simd, out simd);
bAffineTransform.Multiply(ref scalar, ref scalar, out scalar);
for (int i = 0; i < 12; ++i)
{
const float threshold = 1e-5f;
var simdScalarError = Math.Abs(simdPointer[i] - scalarPointer[i]);
if (simdScalarError > threshold)
{
Console.WriteLine($"Excess error for {i}");
}
}
}
}
