// Returns a new axis aligned bounding box that contains the old
// bounding box post the given transformation.
internal static Rect3D ComputeTransformedAxisAlignedBoundingBox( /* IN */ ref Rect3D originalBox, /* IN */ ref Matrix3D matrix)
{
if (originalBox.IsEmpty)
{
return originalBox;
}
if (matrix.IsAffine)
{
return ComputeTransformedAxisAlignedBoundingBoxAffine(ref originalBox, ref matrix);
}
else
{
return ComputeTransformedAxisAlignedBoundingBoxNonAffine(ref originalBox, ref matrix);
}
}
/// <summary>
/// Matrix multiplication.
/// </summary>
/// <param name="matrix1">Matrix to multiply.</param>
/// <param name="matrix2">Matrix by which the first matrix is multiplied.</param>
/// <returns>Result of multiplication.</returns>
public static Matrix3D Multiply(Matrix3D matrix1, Matrix3D matrix2)
{
return (matrix1 * matrix2);
}
//------------------------------------------------------
//
// Multiplication
//
//------------------------------------------------------
/// <summary>
/// Matrix multiplication.
/// </summary>
/// <param name="matrix1">Matrix to multiply.</param>
/// <param name="matrix2">Matrix by which the first matrix is multiplied.</param>
/// <returns>Result of multiplication.</returns>
public static Matrix3D operator *(Matrix3D matrix1, Matrix3D matrix2)
{
// Check if multiplying by identity.
if (matrix1.IsDistinguishedIdentity)
return matrix2;
if (matrix2.IsDistinguishedIdentity)
return matrix1;
// Regular 4x4 matrix multiplication.
Matrix3D result = new Matrix3D(
matrix1._m11 * matrix2._m11 + matrix1._m12 * matrix2._m21 +
matrix1._m13 * matrix2._m31 + matrix1._m14 * matrix2._offsetX,
matrix1._m11 * matrix2._m12 + matrix1._m12 * matrix2._m22 +
matrix1._m13 * matrix2._m32 + matrix1._m14 * matrix2._offsetY,
matrix1._m11 * matrix2._m13 + matrix1._m12 * matrix2._m23 +
matrix1._m13 * matrix2._m33 + matrix1._m14 * matrix2._offsetZ,
matrix1._m11 * matrix2._m14 + matrix1._m12 * matrix2._m24 +
matrix1._m13 * matrix2._m34 + matrix1._m14 * matrix2._m44,
matrix1._m21 * matrix2._m11 + matrix1._m22 * matrix2._m21 +
matrix1._m23 * matrix2._m31 + matrix1._m24 * matrix2._offsetX,
matrix1._m21 * matrix2._m12 + matrix1._m22 * matrix2._m22 +
matrix1._m23 * matrix2._m32 + matrix1._m24 * matrix2._offsetY,
matrix1._m21 * matrix2._m13 + matrix1._m22 * matrix2._m23 +
matrix1._m23 * matrix2._m33 + matrix1._m24 * matrix2._offsetZ,
matrix1._m21 * matrix2._m14 + matrix1._m22 * matrix2._m24 +
matrix1._m23 * matrix2._m34 + matrix1._m24 * matrix2._m44,
matrix1._m31 * matrix2._m11 + matrix1._m32 * matrix2._m21 +
matrix1._m33 * matrix2._m31 + matrix1._m34 * matrix2._offsetX,
matrix1._m31 * matrix2._m12 + matrix1._m32 * matrix2._m22 +
matrix1._m33 * matrix2._m32 + matrix1._m34 * matrix2._offsetY,
matrix1._m31 * matrix2._m13 + matrix1._m32 * matrix2._m23 +
matrix1._m33 * matrix2._m33 + matrix1._m34 * matrix2._offsetZ,
matrix1._m31 * matrix2._m14 + matrix1._m32 * matrix2._m24 +
matrix1._m33 * matrix2._m34 + matrix1._m34 * matrix2._m44,
matrix1._offsetX * matrix2._m11 + matrix1._offsetY * matrix2._m21 +
matrix1._offsetZ * matrix2._m31 + matrix1._m44 * matrix2._offsetX,
matrix1._offsetX * matrix2._m12 + matrix1._offsetY * matrix2._m22 +
matrix1._offsetZ * matrix2._m32 + matrix1._m44 * matrix2._offsetY,
matrix1._offsetX * matrix2._m13 + matrix1._offsetY * matrix2._m23 +
matrix1._offsetZ * matrix2._m33 + matrix1._m44 * matrix2._offsetZ,
matrix1._offsetX * matrix2._m14 + matrix1._offsetY * matrix2._m24 +
matrix1._offsetZ * matrix2._m34 + matrix1._m44 * matrix2._m44);
return result;
}
/// <summary>
/// Equals - compares this Matrix3D with the passed in object. In this equality
/// Double.NaN is equal to itself, unlike in numeric equality.
/// Note that double values can acquire error when operated upon, such that
/// an exact comparison between two values which
/// are logically equal may fail.
/// </summary>
/// <returns>
/// bool - true if "value" is equal to "this".
/// </returns>
/// <param name='value'>The Matrix3D to compare to "this"</param>
public bool Equals(Matrix3D value)
{
return Matrix3D.Equals(this, value);
}
/// <summary>
/// Compares two Matrix3D instances for object equality. In this equality
/// Double.NaN is equal to itself, unlike in numeric equality.
/// Note that double values can acquire error when operated upon, such that
/// an exact comparison between two values which
/// are logically equal may fail.
/// </summary>
/// <returns>
/// bool - true if the two Matrix3D instances are exactly equal, false otherwise
/// </returns>
/// <param name='matrix1'>The first Matrix3D to compare</param>
/// <param name='matrix2'>The second Matrix3D to compare</param>
public static bool Equals(Matrix3D matrix1, Matrix3D matrix2)
{
if (matrix1.IsDistinguishedIdentity || matrix2.IsDistinguishedIdentity)
{
return matrix1.IsIdentity == matrix2.IsIdentity;
}
else
{
return matrix1.M11.Equals(matrix2.M11) &&
matrix1.M12.Equals(matrix2.M12) &&
matrix1.M13.Equals(matrix2.M13) &&
matrix1.M14.Equals(matrix2.M14) &&
matrix1.M21.Equals(matrix2.M21) &&
matrix1.M22.Equals(matrix2.M22) &&
matrix1.M23.Equals(matrix2.M23) &&
matrix1.M24.Equals(matrix2.M24) &&
matrix1.M31.Equals(matrix2.M31) &&
matrix1.M32.Equals(matrix2.M32) &&
matrix1.M33.Equals(matrix2.M33) &&
matrix1.M34.Equals(matrix2.M34) &&
matrix1.OffsetX.Equals(matrix2.OffsetX) &&
matrix1.OffsetY.Equals(matrix2.OffsetY) &&
matrix1.OffsetZ.Equals(matrix2.OffsetZ) &&
matrix1.M44.Equals(matrix2.M44);
}
}
/// <summary>
/// Appends the given matrix to the current matrix.
/// </summary>
/// <param name="matrix">Matrix to append.</param>
public void Append(Matrix3D matrix)
{
this *= matrix;
}
// Math operations
/// <summary>
/// Prepends the given matrix to the current matrix.
/// </summary>
/// <param name="matrix">Matrix to prepend.</param>
public void Prepend(Matrix3D matrix)
{
this = matrix * this;
}
private static Matrix3D CreateIdentity()
{
// Don't call this function, use s_identity.
Matrix3D matrix = new Matrix3D(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
matrix.IsDistinguishedIdentity = true;
return matrix;
}
// CTAABB for an affine transforms
internal static Rect3D ComputeTransformedAxisAlignedBoundingBoxAffine(/* IN */ ref Rect3D originalBox, /* IN */ ref Matrix3D matrix)
{
Debug.Assert(matrix.IsAffine);
// Based on Arvo's paper "Transforming Axis-Aligned Bounding Boxes"
// from the original Graphics Gems book. Specifically, this code
// is based on Figure 1 which is for a box stored as min and
// max points. Our bounding boxes are stored as a min point and
// a diagonal so we'll convert when needed. Also, we have row
// vectors.
//
// Mapping Arvo's variables to ours:
// A - the untransformed box (originalBox)
// B - the transformed box (what we return at the end)
// M - the rotation + scale (matrix.Mji)
// T - the translation (matrix.Offset?)
//
// for i = 1 ... 3
// Bmin_i = Bmax_i = T_i
// for j = 1 ... 3
// a = M_ij * Amin_j
// b = M_ij * Amax_j
// Bmin_i += min(a, b)
// Bmax_i += max(a, b)
//
// Matrix3D doesn't have indexers because they're too slow so we'll
// have to unroll the loops. A complete unroll of both loops was
// found to be the fastest.
double oldMaxX = originalBox.X + originalBox.SizeX;
double oldMaxY = originalBox.Y + originalBox.SizeY;
double oldMaxZ = originalBox.Z + originalBox.SizeZ;
// i = 1 (X)
double newMinX = matrix.OffsetX;
double newMaxX = matrix.OffsetX;
{
// i = 1 (X), j = 1 (X)
double a = matrix.M11 * originalBox.X;
double b = matrix.M11 * oldMaxX;
if (b > a)
{
newMinX += a;
newMaxX += b;
}
else
{
newMinX += b;
newMaxX += a;
}
// i = 1 (X), j = 2 (Y)
a = matrix.M21 * originalBox.Y;
b = matrix.M21 * oldMaxY;
if (b > a)
{
newMinX += a;
newMaxX += b;
}
else
{
newMinX += b;
newMaxX += a;
}
// i = 1 (X), j = 3 (Z)
a = matrix.M31 * originalBox.Z;
b = matrix.M31 * oldMaxZ;
if (b > a)
{
newMinX += a;
newMaxX += b;
}
else
{
newMinX += b;
newMaxX += a;
}
}
// i = 2 (Y)
double newMinY = matrix.OffsetY;
double newMaxY = matrix.OffsetY;
{
// i = 2 (Y), j = 1 (X)
double a = matrix.M12 * originalBox.X;
double b = matrix.M12 * oldMaxX;
if (b > a)
{
newMinY += a;
newMaxY += b;
}
else
{
newMinY += b;
newMaxY += a;
}
// i = 2 (Y), j = 2 (Y)
a = matrix.M22 * originalBox.Y;
b = matrix.M22 * oldMaxY;
if (b > a)
{
newMinY += a;
newMaxY += b;
}
else
{
newMinY += b;
newMaxY += a;
}
// i = 2 (Y), j = 3 (Z)
a = matrix.M32 * originalBox.Z;
b = matrix.M32 * oldMaxZ;
if (b > a)
{
newMinY += a;
newMaxY += b;
}
else
{
newMinY += b;
newMaxY += a;
}
}
// i = 3 (Z)
double newMinZ = matrix.OffsetZ;
double newMaxZ = matrix.OffsetZ;
{
// i = 3 (Z), j = 1 (X)
double a = matrix.M13 * originalBox.X;
double b = matrix.M13 * oldMaxX;
if (b > a)
{
newMinZ += a;
newMaxZ += b;
}
else
{
newMinZ += b;
newMaxZ += a;
}
// i = 3 (Z), j = 2 (Y)
a = matrix.M23 * originalBox.Y;
b = matrix.M23 * oldMaxY;
if (b > a)
{
newMinZ += a;
newMaxZ += b;
}
else
{
newMinZ += b;
newMaxZ += a;
}
// i = 3 (Z), j = 3 (Z)
a = matrix.M33 * originalBox.Z;
b = matrix.M33 * oldMaxZ;
if (b > a)
{
newMinZ += a;
newMaxZ += b;
}
else
{
newMinZ += b;
newMaxZ += a;
}
}
return new Rect3D(newMinX, newMinY, newMinZ, newMaxX - newMinX, newMaxY - newMinY, newMaxZ - newMinZ);
}
// CTAABB for non-affine transformations
internal static Rect3D ComputeTransformedAxisAlignedBoundingBoxNonAffine(/* IN */ ref Rect3D originalBox, /* IN */ ref Matrix3D matrix)
{
Debug.Assert(!matrix.IsAffine);
double x1 = originalBox.X;
double y1 = originalBox.Y;
double z1 = originalBox.Z;
double x2 = originalBox.X + originalBox.SizeX;
double y2 = originalBox.Y + originalBox.SizeY;
double z2 = originalBox.Z + originalBox.SizeZ;
Point3D[] points = new Point3D[] {
new Point3D(x1, y1, z1),
new Point3D(x1, y1, z2),
new Point3D(x1, y2, z1),
new Point3D(x1, y2, z2),
new Point3D(x2, y1, z1),
new Point3D(x2, y1, z2),
new Point3D(x2, y2, z1),
new Point3D(x2, y2, z2),
};
matrix.Transform(points);
Point3D p = points[0];
Rect3D newBounds = new Rect3D(p.X, p.Y, p.Z, 0, 0, 0);
// Traverse the entire mesh and compute bounding box.
for (int i = 1; i < points.Length; i++)
{
p = points[i];
AddPointToBounds(ref p, ref newBounds);
}
return newBounds;
}