public static Matrix3 T(this Matrix3 A)
{
var C = Matrix3.Zero;
C.Row(0).Copy(A.Column(0));
C.Row(1).Copy(A.Column(1));
C.Row(2).Copy(A.Column(2));
return(C);
}
/// <summary>
///
/// </summary>
/// <param name="former"></param>
/// <param name="latter"></param>
/// <returns></returns>
public override Manifold Collide(Box former, Box latter)
{
// https://gamedev.stackexchange.com/questions/112883/simple-3d-obb-collision-directx9-c
// https://github.com/RandyGaul/qu3e/blob/master/src/collision/q3Collide.cpp
Matrix3 formerTransform = former.transform;
Matrix3 latterTransform = latter.transform;
Vector3 formerSize = former.size;
Vector3 latterSize = latter.size;
// latter's frame in former space
Matrix3 C = Matrix3.Transpose(formerTransform) * latterTransform;
Matrix3 absoluteC = Matrix3.Zero;
bool parallel = false;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
float value = Math.Abs(C[i, j]);
if (value + float.Epsilon > 1)
{
parallel = true;
}
}
}
// vector from the center of former to the center of latter in former space
Vector3 difference = Vector3.Zero; // Matrix3.Transform(formerTransform, latter.Position - former.Position);
// query states
float s;
float aMax = float.NegativeInfinity;
float bMax = float.NegativeInfinity;
float eMax = float.NegativeInfinity;
int aAxis = ~0;
int bAxis = ~0;
int eAxis = ~0;
Vector3 nA = Vector3.Zero;
Vector3 nB = Vector3.Zero;
Vector3 nC = Vector3.Zero;
// former's X axis
s = Math.Abs(difference.X) - (formerSize.X + Vector3.Dot(absoluteC.Column(0), latterSize));
if (TrackFaceAxis(ref aAxis, 0, s, ref aMax, formerTransform.Row(0), ref nA))
{
return(null);
}
// former's Y axis
s = Math.Abs(difference.Y) - (formerSize.Y + Vector3.Dot(absoluteC.Column(0), latterSize));
if (TrackFaceAxis(ref aAxis, 1, s, ref aMax, formerTransform.Row(1), ref nA))
{
return(null);
}
// former's Z axis
s = Math.Abs(difference.Z) - (formerSize.Z + Vector3.Dot(absoluteC.Column(0), latterSize));
if (TrackFaceAxis(ref aAxis, 2, s, ref aMax, formerTransform.Row(2), ref nA))
{
return(null);
}
// latter's X axis
s = Math.Abs(Vector3.Dot(difference, C.Row(0))) - (latterSize.X + Vector3.Dot(absoluteC.Row(0), formerSize));
if (TrackFaceAxis(ref bAxis, 3, s, ref bMax, latterTransform.Row(0), ref nB))
{
return(null);
}
// latter's Y axis
s = Math.Abs(Vector3.Dot(difference, C.Row(1))) - (latterSize.Y + Vector3.Dot(absoluteC.Row(1), formerSize));
if (TrackFaceAxis(ref bAxis, 4, s, ref bMax, latterTransform.Row(1), ref nB))
{
return(null);
}
// latter's Z axis
s = Math.Abs(Vector3.Dot(difference, C.Row(2))) - (latterSize.Z + Vector3.Dot(absoluteC.Row(2), formerSize));
if (TrackFaceAxis(ref bAxis, 5, s, ref bMax, latterTransform.Row(2), ref nB))
{
return(null);
}
if (!parallel)
{
}
throw new NotImplementedException();
}
/// <summary> /// Gets the slice position from the image orientation to patient matrix direction and origin. /// </summary> /// <param name="direction">The image orientation to patient matrix.</param> /// <param name="origin">The origin 3-dimensional point.</param> /// <returns>The position of the slice.</returns> private static double GetSlicePosition(Matrix3 direction, Point3D origin) => Point3D.DotProd(direction.Column(2), origin);