public static double Det(this SymmetricMatrix3 @this)
{
var mx = @this.YY * @this.ZZ - @this.YZ * @this.YZ;
var my = @this.XY * @this.ZZ - @this.YZ * @this.ZX;
var mz = @this.XY * @this.YZ - @this.YY * @this.ZX;
return(@this.XX * mx - @this.XY * my + @this.ZX * mz);
}
// finds least-squares solution to a system of orthogonality equations (i.e. result is fitted to be orthogonal to vectors)
public static void FitOrthogonal(IEnumerable <Vector3> vectors, out Vector3 result, out double residual)
{
var mat = SymmetricMatrix3.TensorSquare(vectors.First());
foreach (var vector in vectors.Skip(1))
{
mat += SymmetricMatrix3.TensorSquare(vector);
}
mat.ComputeLowestEigendata(out residual, out result);
}
public static double LowestEigenvalue(this SymmetricMatrix3 @this)
{
var b = @this.Trace();
var c =
[email protected] * @this.ZZ + @this.YZ * @this.YZ
- @this.XX * @this.ZZ + @this.ZX * @this.ZX
- @this.XX * @this.YY + @this.XY * @this.XY;
var d = @this.Det();
return(CubicEquation.Solve(-1, b, c, d).Min());
}
public void Eigenvectors_and_eigenvalues_of_satisfy_defining_equations(
[Values(1, 0.577, 0.577, 0.577, 0.577, 0.577, 0.577)] double xx,
[Values(1, 2.718, 2.718, 0.577, 0.577, 0.577, 0.577)] double yy,
[Values(1, 3.141, 3.141, 3.141, 0.577, 3.141, 0.577)] double zz,
[Values(0, 0, 1.618, 1e-40, 1e-40, 1e-60, 1e-60)] double xy,
[Values(0, 0, 2.665, 2e-40, 2e-40, 2e-60, 2e-60)] double yz,
[Values(0, 0, 4.669, 3e-40, 3e-40, 3e-60, 3e-60)] double zx)
{
var mat = new SymmetricMatrix3(xx, yy, zz, xy, yz, zx);
var loVal = mat.LowestEigenvalue();
var loVec = mat.LowestEigenvector();
Expect((mat - SymmetricMatrix3.Scalar(loVal)).Det(), Is.EqualTo(0).Within(_tolerance));
Expect(Vector3.Distance(mat * loVec, loVal * loVec), Is.LessThan(_tolerance));
}
private static Vector3 Eigenvector(this SymmetricMatrix3 @this, double eigenvalue)
{
var rowX = new Vector3(@this.XX - eigenvalue, @this.XY, @this.ZX);
var rowY = new Vector3(@this.XY, @this.YY - eigenvalue, @this.YZ);
var rowZ = new Vector3(@this.ZX, @this.YZ, @this.ZZ - eigenvalue);
var normX2 = rowX.Norm2;
var normY2 = rowY.Norm2;
var normZ2 = rowZ.Norm2;
Vector3 row1, row2, row3;
if (normX2 > normY2)
{
if (normX2 > normZ2)
{
row1 = rowX / Math.Sqrt(normX2);
row2 = rowY;
row3 = rowZ;
}
else
{
row1 = rowZ / Math.Sqrt(normZ2);
row2 = rowX;
row3 = rowY;
}
}
else
{
if (normY2 > normZ2)
{
row1 = rowY / Math.Sqrt(normY2);
row2 = rowX;
row3 = rowZ;
}
else
{
row1 = rowZ / Math.Sqrt(normZ2);
row2 = rowY;
row3 = rowX;
}
}
var cross2 = row1.Cross(row2);
var cross3 = row1.Cross(row3);
var norm22 = cross2.Norm2;
var norm32 = cross3.Norm2;
if (norm22 > norm32)
{
if (norm22 > BasicMath.Epsilon)
{
return(cross2 / Math.Sqrt(norm22));
}
else
{
return(Vector3Utils.VectorOrthogonalTo(row1).Normalized);
}
}
else
{
if (norm32 > BasicMath.Epsilon)
{
return(cross3 / Math.Sqrt(norm32));
}
else
{
return(Vector3Utils.VectorOrthogonalTo(row1).Normalized);
}
}
}
public static double Trace(this SymmetricMatrix3 @this)
{
return(@this.XX + @this.YY + @this.ZZ);
}
public static Vector3 RowZ(this SymmetricMatrix3 @this)
{
return(new Vector3(@this.ZX, @this.YZ, @this.ZZ));
}
public static Vector3 RowY(this SymmetricMatrix3 @this)
{
return(new Vector3(@this.XY, @this.YY, @this.YZ));
}
public static Vector3 RowX(this SymmetricMatrix3 @this)
{
return(new Vector3(@this.XX, @this.XY, @this.ZX));
}
public static void ComputeLowestEigendata(this SymmetricMatrix3 @this, out double eigenvalue, out Vector3 eigenvector)
{
eigenvalue = @this.LowestEigenvalue();
eigenvector = @this.Eigenvector(eigenvalue);
}
public static Vector3 LowestEigenvector(this SymmetricMatrix3 @this)
{
return(@this.Eigenvector(@this.LowestEigenvalue()));
}