public void Invert_Static()
{
var mat = new double4x4(1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 1, 0, 0, 0, 1);
var actual = mat.Invert();
Assert.Equal(new double4x4(1, 0, 0, -1, 0, 1, 0, -1, 0, 0, 1, -1, 0, 0, 0, 1), actual);
}
/// <summary>
/// Generates a new oriented bounding box from a given set of vertices or points
/// </summary>
/// <param name="vertices"></param>
public OBBd(double3[] vertices)
{
var verticesList = vertices.ToList();
if (verticesList.Any(pt => pt.IsInfinity) || verticesList.Any(pt => pt.IsNaN))
{
Max = double3.PositiveInfinity;
Min = double3.NegativeInfinity;
Rotation = double4x4.Identity;
Translation = double3.Zero;
Size = double3.Zero;
return;
}
Translation = M.CalculateCentroid(vertices);
var covarianceMatrix = M.CreateCovarianceMatrix(Translation, vertices);
var eigen = M.EigenFromCovarianceMat(covarianceMatrix);
Rotation = eigen.RotationMatrix;
var changeBasis = Rotation.Invert();
Min = double3.One * double.MaxValue;
Max = double3.One * double.MinValue;
Size = double3.One;
for (var i = 0; i < vertices.Length; i++)
{
// translate every point in first quadrant of [0, 0, 0] coordinate system
var currentPointTranslated = vertices[i] - Translation;
var currentPointTranslatedAndRotated = changeBasis * currentPointTranslated;
var pt = currentPointTranslatedAndRotated;
// check min and max points
Min = new double3(
Min.x > pt.x ? pt.x : Min.x,
Min.y > pt.y ? pt.y : Min.y,
Min.z > pt.z ? pt.z : Min.z);
Max = new double3(
Max.x < pt.x ? pt.x : Max.x,
Max.y < pt.y ? pt.y : Max.y,
Max.z < pt.z ? pt.z : Max.z);
}
Max = (Rotation * Max) + Translation;
Min = (Rotation * Min) + Translation;
Size = Max - Min;
}
/// <summary>
/// Transform a Normal by the given Matrix
/// </summary>
/// <param name="norm">The normal to transform</param>
/// <param name="mat">The desired transformation</param>
/// <returns>
/// The transformed normal
/// </returns>
/// <remarks>
/// This calculates the inverse of the given matrix, use TransformNormalInverse if you
/// already have the inverse to avoid this extra calculation
/// </remarks>
public static double3 TransformNormal(double3 norm, double4x4 mat)
{
mat.Invert();
return(TransformNormalInverse(norm, mat));
}