/// <summary>
/// Caculates the matrix required to transfer any point from one <see cref="SeeingSharp.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>
/// Creates a new frustum relaying on perspective camera parameters
/// </summary>
/// <param name="cameraPos">The camera pos.</param>
/// <param name="lookDir">The look dir.</param>
/// <param name="upDir">Up dir.</param>
/// <param name="fov">The fov.</param>
/// <param name="znear">The znear.</param>
/// <param name="zfar">The zfar.</param>
/// <param name="aspect">The aspect.</param>
/// <returns>The bouding frustum calculated from perspective camera</returns>
public static BoundingFrustum FromCamera(Vector3 cameraPos, Vector3 lookDir, Vector3 upDir, float fov, float znear, float zfar, float aspect)
{
//http://knol.google.com/k/view-frustum
lookDir = Vector3.Normalize(lookDir);
upDir = Vector3.Normalize(upDir);
Vector3 nearCenter = cameraPos + lookDir * znear;
Vector3 farCenter = cameraPos + lookDir * zfar;
float nearHalfHeight = (float)(znear * Math.Tan(fov / 2f));
float farHalfHeight = (float)(zfar * Math.Tan(fov / 2f));
float nearHalfWidth = nearHalfHeight * aspect;
float farHalfWidth = farHalfHeight * aspect;
Vector3 rightDir = Vector3.Normalize(Vector3.Cross(upDir, lookDir));
Vector3 Near1 = nearCenter - nearHalfHeight * upDir + nearHalfWidth * rightDir;
Vector3 Near2 = nearCenter + nearHalfHeight * upDir + nearHalfWidth * rightDir;
Vector3 Near3 = nearCenter + nearHalfHeight * upDir - nearHalfWidth * rightDir;
Vector3 Near4 = nearCenter - nearHalfHeight * upDir - nearHalfWidth * rightDir;
Vector3 Far1 = farCenter - farHalfHeight * upDir + farHalfWidth * rightDir;
Vector3 Far2 = farCenter + farHalfHeight * upDir + farHalfWidth * rightDir;
Vector3 Far3 = farCenter + farHalfHeight * upDir - farHalfWidth * rightDir;
Vector3 Far4 = farCenter - farHalfHeight * upDir - farHalfWidth * rightDir;
var result = new BoundingFrustum();
result.pNear = new Plane(Near1, Near2, Near3);
result.pFar = new Plane(Far3, Far2, Far1);
result.pLeft = new Plane(Near4, Near3, Far3);
result.pRight = new Plane(Far1, Far2, Near2);
result.pTop = new Plane(Near2, Far2, Far3);
result.pBottom = new Plane(Far4, Far1, Near1);
result.pNear.Normalize();
result.pFar.Normalize();
result.pLeft.Normalize();
result.pRight.Normalize();
result.pTop.Normalize();
result.pBottom.Normalize();
result.pMatrix = Matrix4x4Ex.CreateLookAtLH(cameraPos, cameraPos + lookDir * 10, upDir) * Matrix4x4Ex.CreatePerspectiveFovLH(fov, aspect, znear, zfar);
return(result);
}
/// <summary>
/// Check the intersection between an <see cref="SeeingSharp.OrientedBoundingBox"/> and <see cref="SeeingSharp.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="SeeingSharp.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 = 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 - 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 = Vector3Ex.GetValue(SizeA, 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(Vector3Ex.GetValue(vSepA, 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 = Vector3Ex.GetValue(SizeB, 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 =
Vector3Ex.GetValue(SizeA, i1) * Matrix4x4Ex.GetValue(AR, i2, k) +
Vector3Ex.GetValue(SizeA, i2) * Matrix4x4Ex.GetValue(AR, i1, k);
ExtentB =
Vector3Ex.GetValue(SizeB, k1) * Matrix4x4Ex.GetValue(AR, i, k2) +
Vector3Ex.GetValue(SizeB, k2) * Matrix4x4Ex.GetValue(AR, i, k1);
Separation = Math.Abs(
Vector3Ex.GetValue(vSepA, i2) * Matrix4x4Ex.GetValue(R, i1, k) -
Vector3Ex.GetValue(vSepA, 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="SeeingSharp.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="SeeingSharp.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 = 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 - 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 = Vector3Ex.GetValue(SizeA, 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(Vector3Ex.GetValue(vSepA, 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 = Vector3Ex.GetValue(SizeB, 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 =
Vector3Ex.GetValue(SizeA, i1) * Matrix4x4Ex.GetValue(AR, i2, k) +
Vector3Ex.GetValue(SizeA, i2) * Matrix4x4Ex.GetValue(AR, i1, k);
ExtentB =
Vector3Ex.GetValue(SizeB, k1) * Matrix4x4Ex.GetValue(AR, i, k2) +
Vector3Ex.GetValue(SizeB, k2) * Matrix4x4Ex.GetValue(AR, i, k1);
Separation = Math.Abs(
Vector3Ex.GetValue(vSepA, i2) * Matrix4x4Ex.GetValue(R, i1, k) -
Vector3Ex.GetValue(vSepA, i1) * Matrix4x4Ex.GetValue(R, i2, k));
if (Separation > ExtentA + ExtentB)
{
return(ContainmentType.Disjoint);
}
}
}
// No separating axis found, the boxes overlap
return(ContainmentType.Intersects);
}
