/// <summary>
/// Caculates the matrix required to transfer any point from one <see cref="OrientedBoundingBox"/> local corrdinates to another.
/// </summary>
/// <param name="A">The source OrientedBoundingBox.</param>
/// <param name="B">The target OrientedBoundingBox.</param>
/// <param name="NoMatrixScaleApplied">
/// If true, the method will use a fast algorithm which is inapplicable if a scale is applied to the transformation matrix of the OrientedBoundingBox.
/// </param>
/// <returns></returns>
public static Matrix4x4 GetBoxToBoxMatrix(ref OrientedBoundingBox A, ref OrientedBoundingBox B, bool NoMatrixScaleApplied = false)
{
Matrix4x4 AtoB_Matrix;
// Calculate B to A transformation matrix
if (NoMatrixScaleApplied)
{
var RotA = GetRows(ref A.Transformation);
var RotB = GetRows(ref B.Transformation);
AtoB_Matrix = new Matrix4x4();
int i, k;
for (i = 0; i < 3; i++)
{
for (k = 0; k < 3; k++)
{
Matrix4x4Ex.SetValue(AtoB_Matrix, i, k, Vector3.Dot(RotB[i], RotA[k]));
}
}
var v = B.Center - A.Center;
AtoB_Matrix.M41 = Vector3.Dot(v, RotA[0]);
AtoB_Matrix.M42 = Vector3.Dot(v, RotA[1]);
AtoB_Matrix.M43 = Vector3.Dot(v, RotA[2]);
AtoB_Matrix.M44 = 1;
}
else
{
Matrix4x4 AInvMat;
Matrix4x4.Invert(A.Transformation, out AInvMat);
AtoB_Matrix = B.Transformation * AInvMat;
}
return(AtoB_Matrix);
}
/// <summary>
/// Check the intersection between an <see cref="OrientedBoundingBox"/> and <see cref="BoundingBox"/>
/// </summary>
/// <param name="box">The BoundingBox to test.</param>
/// <returns>The type of containment the two objects have.</returns>
/// <remarks>
/// For accuracy, The transformation matrix for the <see cref="OrientedBoundingBox"/> must not have any scaling applied to it.
/// Anyway, scaling using Scale method will keep this method accurate.
/// </remarks>
public ContainmentType Contains(ref BoundingBox box)
{
var cornersCheck = this.Contains(box.GetCorners());
if (cornersCheck != ContainmentType.Disjoint)
{
return(cornersCheck);
}
var boxCenter = box.Minimum + (box.Maximum - box.Minimum) / 2f;
var boxExtents = box.Maximum - boxCenter;
var SizeA = Extents;
var SizeB = boxExtents;
var RotA = GetRows(ref Transformation);
float ExtentA, ExtentB, Separation;
int i, k;
Matrix4x4 R; // Rotation from B to A
Matrix4x4.Invert(Transformation, out R);
var AR = new Matrix4x4(); // absolute values of R matrix, to use with box extents
for (i = 0; i < 3; i++)
{
for (k = 0; k < 3; k++)
{
Matrix4x4Ex.SetValue(AR, i, k, Math.Abs(Matrix4x4Ex.GetValue(R, i, k)));
}
}
// Vector separating the centers of Box B and of Box A
var vSepWS = boxCenter - this.Center;
// Rotated into Box A's coordinates
var vSepA = new Vector3(Vector3.Dot(vSepWS, RotA[0]), Vector3.Dot(vSepWS, RotA[1]), Vector3.Dot(vSepWS, RotA[2]));
// Test if any of A's basis vectors separate the box
for (i = 0; i < 3; i++)
{
ExtentA = SizeA.GetValue(i);
ExtentB = Vector3.Dot(SizeB, new Vector3(
Matrix4x4Ex.GetValue(AR, i, 0),
Matrix4x4Ex.GetValue(AR, i, 1),
Matrix4x4Ex.GetValue(AR, i, 2)));
Separation = Math.Abs(vSepA.GetValue(i));
if (Separation > ExtentA + ExtentB)
{
return(ContainmentType.Disjoint);
}
}
// Test if any of B's basis vectors separate the box
for (k = 0; k < 3; k++)
{
ExtentA = Vector3.Dot(SizeA, new Vector3(
Matrix4x4Ex.GetValue(AR, 0, k),
Matrix4x4Ex.GetValue(AR, 1, k),
Matrix4x4Ex.GetValue(AR, 2, k)));
ExtentB = SizeB.GetValue(k);
Separation = Math.Abs(Vector3.Dot(vSepA, new Vector3(
Matrix4x4Ex.GetValue(R, 0, k),
Matrix4x4Ex.GetValue(R, 1, k),
Matrix4x4Ex.GetValue(R, 2, k))));
if (Separation > ExtentA + ExtentB)
{
return(ContainmentType.Disjoint);
}
}
// Now test Cross Products of each basis vector combination ( A[i], B[k] )
for (i = 0; i < 3; i++)
{
for (k = 0; k < 3; k++)
{
int i1 = (i + 1) % 3, i2 = (i + 2) % 3;
int k1 = (k + 1) % 3, k2 = (k + 2) % 3;
ExtentA =
SizeA.GetValue(i1) * Matrix4x4Ex.GetValue(AR, i2, k) +
SizeA.GetValue(i2) * Matrix4x4Ex.GetValue(AR, i1, k);
ExtentB =
SizeB.GetValue(k1) * Matrix4x4Ex.GetValue(AR, i, k2) +
SizeB.GetValue(k2) * Matrix4x4Ex.GetValue(AR, i, k1);
Separation = Math.Abs(
vSepA.GetValue(i2) * Matrix4x4Ex.GetValue(R, i1, k) -
vSepA.GetValue(i1) * Matrix4x4Ex.GetValue(R, i2, k));
if (Separation > ExtentA + ExtentB)
{
return(ContainmentType.Disjoint);
}
}
}
// No separating axis found, the boxes overlap
return(ContainmentType.Intersects);
}
/// <summary>
/// Check the intersection between two <see cref="OrientedBoundingBox"/>
/// </summary>
/// <param name="obb">The OrientedBoundingBoxs to test.</param>
/// <returns>The type of containment the two objects have.</returns>
/// <remarks>
/// For accuracy, The transformation matrix for both <see cref="OrientedBoundingBox"/> must not have any scaling applied to it.
/// Anyway, scaling using Scale method will keep this method accurate.
/// </remarks>
public ContainmentType Contains(ref OrientedBoundingBox obb)
{
var cornersCheck = this.Contains(obb.GetCorners());
if (cornersCheck != ContainmentType.Disjoint)
{
return(cornersCheck);
}
//http://www.3dkingdoms.com/weekly/bbox.cpp
var SizeA = Extents;
var SizeB = obb.Extents;
var RotA = GetRows(ref Transformation);
var RotB = GetRows(ref obb.Transformation);
var R = new Matrix4x4(); // Rotation from B to A
var AR = new Matrix4x4(); // absolute values of R matrix, to use with box extents
float ExtentA, ExtentB, Separation;
int i, k;
// Calculate B to A rotation matrix
for (i = 0; i < 3; i++)
{
for (k = 0; k < 3; k++)
{
Matrix4x4Ex.SetValue(R, i, k, Vector3.Dot(RotA[i], RotB[k]));
Matrix4x4Ex.SetValue(AR, i, k, Math.Abs(Matrix4x4Ex.GetValue(R, i, k)));
}
}
// Vector separating the centers of Box B and of Box A
var vSepWS = obb.Center - this.Center;
// Rotated into Box A's coordinates
var vSepA = new Vector3(Vector3.Dot(vSepWS, RotA[0]), Vector3.Dot(vSepWS, RotA[1]), Vector3.Dot(vSepWS, RotA[2]));
// Test if any of A's basis vectors separate the box
for (i = 0; i < 3; i++)
{
ExtentA = SizeA.GetValue(i);
ExtentB = Vector3.Dot(SizeB, new Vector3(
Matrix4x4Ex.GetValue(AR, i, 0),
Matrix4x4Ex.GetValue(AR, i, 1),
Matrix4x4Ex.GetValue(AR, i, 2)));
Separation = Math.Abs(vSepA.GetValue(i));
if (Separation > ExtentA + ExtentB)
{
return(ContainmentType.Disjoint);
}
}
// Test if any of B's basis vectors separate the box
for (k = 0; k < 3; k++)
{
ExtentA = Vector3.Dot(SizeA, new Vector3(
Matrix4x4Ex.GetValue(AR, 0, k),
Matrix4x4Ex.GetValue(AR, 1, k),
Matrix4x4Ex.GetValue(AR, 2, k)));
ExtentB = SizeB.GetValue(k);
Separation = Math.Abs(Vector3.Dot(vSepA, new Vector3(
Matrix4x4Ex.GetValue(R, 0, k),
Matrix4x4Ex.GetValue(R, 1, k),
Matrix4x4Ex.GetValue(R, 2, k))));
if (Separation > ExtentA + ExtentB)
{
return(ContainmentType.Disjoint);
}
}
// Now test Cross Products of each basis vector combination ( A[i], B[k] )
for (i = 0; i < 3; i++)
{
for (k = 0; k < 3; k++)
{
int i1 = (i + 1) % 3, i2 = (i + 2) % 3;
int k1 = (k + 1) % 3, k2 = (k + 2) % 3;
ExtentA =
SizeA.GetValue(i1) * Matrix4x4Ex.GetValue(AR, i2, k) +
SizeA.GetValue(i2) * Matrix4x4Ex.GetValue(AR, i1, k);
ExtentB =
SizeB.GetValue(k1) * Matrix4x4Ex.GetValue(AR, i, k2) +
SizeB.GetValue(k2) * Matrix4x4Ex.GetValue(AR, i, k1);
Separation = Math.Abs(
vSepA.GetValue(i2) * Matrix4x4Ex.GetValue(R, i1, k) -
vSepA.GetValue(i1) * Matrix4x4Ex.GetValue(R, i2, k));
if (Separation > ExtentA + ExtentB)
{
return(ContainmentType.Disjoint);
}
}
}
// No separating axis found, the boxes overlap
return(ContainmentType.Intersects);
}
