public static Vector3 GetLocalVector(CoordinateSystemAxis axis, TransformMatrix transformMatrix)
{
if (axis == CoordinateSystemAxis.PositiveRight)
{
return(transformMatrix.GetNormalizedRightAxis());
}
if (axis == CoordinateSystemAxis.NegativeRight)
{
return(-transformMatrix.GetNormalizedRightAxis());
}
if (axis == CoordinateSystemAxis.PositiveUp)
{
return(transformMatrix.GetNormalizedUpAxis());
}
if (axis == CoordinateSystemAxis.NegativeUp)
{
return(-transformMatrix.GetNormalizedUpAxis());
}
if (axis == CoordinateSystemAxis.PositiveLook)
{
return(transformMatrix.GetNormalizedLookAxis());
}
return(-transformMatrix.GetNormalizedLookAxis());
}
private void RecalculatePositiveAndNegativeUpAxes()
{
_localAxes[2] = _transformMatrix.GetNormalizedUpAxis();
_localAxes[3] = -_localAxes[2];
}
public bool Intersects(OrientedBox otherBox)
{
Vector3 thisScale = Scale;
Vector3 otherScale = otherBox.Scale;
// Negative scale causes problems
Scale = thisScale.GetVectorWithPositiveComponents();
otherBox.Scale = otherScale.GetVectorWithPositiveComponents();
TransformMatrix transformMatrix = TransformMatrix;
Vector3 A0 = transformMatrix.GetNormalizedRightAxis();
Vector3 A1 = transformMatrix.GetNormalizedUpAxis();
Vector3 A2 = transformMatrix.GetNormalizedLookAxis();
Vector3[] A = new Vector3[] { A0, A1, A2 };
TransformMatrix otherTransformMatrix = otherBox.TransformMatrix;
Vector3 B0 = otherTransformMatrix.GetNormalizedRightAxis();
Vector3 B1 = otherTransformMatrix.GetNormalizedUpAxis();
Vector3 B2 = otherTransformMatrix.GetNormalizedLookAxis();
Vector3[] B = new Vector3[] { B0, B1, B2 };
// Note: We're using column major matrices.
float[,] R = new float[3, 3];
for (int row = 0; row < 3; ++row)
{
for (int column = 0; column < 3; ++column)
{
R[row, column] = Vector3.Dot(A[row], B[column]);
}
}
Vector3 scaledExtents = ScaledExtents;
Vector3 AEx = new Vector3(scaledExtents.x, scaledExtents.y, scaledExtents.z);
scaledExtents = otherBox.ScaledExtents;
Vector3 BEx = new Vector3(scaledExtents.x, scaledExtents.y, scaledExtents.z);
// Construct absolute rotation error matrix to account for cases when 2 local axes are parallel
const float epsilon = 1e-4f;
float[,] absR = new float[3, 3];
for (int row = 0; row < 3; ++row)
{
for (int column = 0; column < 3; ++column)
{
absR[row, column] = Mathf.Abs(R[row, column]) + epsilon;
}
}
Vector3 trVector = otherBox.Center - Center;
Vector3 t = new Vector3(Vector3.Dot(trVector, A0), Vector3.Dot(trVector, A1), Vector3.Dot(trVector, A2));
// Test extents projection on this box's local axes (A0, A1, A2)
for (int axisIndex = 0; axisIndex < 3; ++axisIndex)
{
float bExtents = BEx[0] * absR[axisIndex, 0] + BEx[1] * absR[axisIndex, 1] + BEx[2] * absR[axisIndex, 2];
if (Mathf.Abs(t[axisIndex]) > AEx[axisIndex] + bExtents)
{
return(false);
}
}
// Test extents projection on the other box's local axes (B0, B1, B2)
for (int axisIndex = 0; axisIndex < 3; ++axisIndex)
{
float aExtents = AEx[0] * absR[0, axisIndex] + AEx[1] * absR[1, axisIndex] + AEx[2] * absR[2, axisIndex];
if (Mathf.Abs(t[0] * R[0, axisIndex] +
t[1] * R[1, axisIndex] +
t[2] * R[2, axisIndex]) > aExtents + BEx[axisIndex])
{
return(false);
}
}
// Test axis A0 x B0
float ra = AEx[1] * absR[2, 0] + AEx[2] * absR[1, 0];
float rb = BEx[1] * absR[0, 2] + BEx[2] * absR[0, 1];
if (Mathf.Abs(t[2] * R[1, 0] - t[1] * R[2, 0]) > ra + rb)
{
return(false);
}
// Test axis A0 x B1
ra = AEx[1] * absR[2, 1] + AEx[2] * absR[1, 1];
rb = BEx[0] * absR[0, 2] + BEx[2] * absR[0, 0];
if (Mathf.Abs(t[2] * R[1, 1] - t[1] * R[2, 1]) > ra + rb)
{
return(false);
}
// Test axis A0 x B2
ra = AEx[1] * absR[2, 2] + AEx[2] * absR[1, 2];
rb = BEx[0] * absR[0, 1] + BEx[1] * absR[0, 0];
if (Mathf.Abs(t[2] * R[1, 2] - t[1] * R[2, 2]) > ra + rb)
{
return(false);
}
// Test axis A1 x B0
ra = AEx[0] * absR[2, 0] + AEx[2] * absR[0, 0];
rb = BEx[1] * absR[1, 2] + BEx[2] * absR[1, 1];
if (Mathf.Abs(t[0] * R[2, 0] - t[2] * R[0, 0]) > ra + rb)
{
return(false);
}
// Test axis A1 x B1
ra = AEx[0] * absR[2, 1] + AEx[2] * absR[0, 1];
rb = BEx[0] * absR[1, 2] + BEx[2] * absR[1, 0];
if (Mathf.Abs(t[0] * R[2, 1] - t[2] * R[0, 1]) > ra + rb)
{
return(false);
}
// Test axis A1 x B2
ra = AEx[0] * absR[2, 2] + AEx[2] * absR[0, 2];
rb = BEx[0] * absR[1, 1] + BEx[1] * absR[1, 0];
if (Mathf.Abs(t[0] * R[2, 2] - t[2] * R[0, 2]) > ra + rb)
{
return(false);
}
// Test axis A2 x B0
ra = AEx[0] * absR[1, 0] + AEx[1] * absR[0, 0];
rb = BEx[1] * absR[2, 2] + BEx[2] * absR[2, 1];
if (Math.Abs(t[1] * R[0, 0] - t[0] * R[1, 0]) > ra + rb)
{
return(false);
}
// Test axis A2 x B1
ra = AEx[0] * absR[1, 1] + AEx[1] * absR[0, 1];
rb = BEx[0] * absR[2, 2] + BEx[2] * absR[2, 0];
if (Math.Abs(t[1] * R[0, 1] - t[0] * R[1, 1]) > ra + rb)
{
return(false);
}
// Test axis A2 x B2
ra = AEx[0] * absR[1, 2] + AEx[1] * absR[0, 2];
rb = BEx[0] * absR[2, 1] + BEx[1] * absR[2, 0];
if (Math.Abs(t[1] * R[0, 2] - t[0] * R[1, 2]) > ra + rb)
{
return(false);
}
Scale = thisScale;
otherBox.Scale = otherScale;
return(true);
}