public static void Rotate01(SMat3 vtav, Mat3 v)
{
if (vtav.m01 == 0)
{
return;
}
double c = 0, s = 0;
Schur2.Rot01(vtav, c, s);
Givens.Rot01Post(v, c, s);
}
public static void Rotate12(SMat3 vtav, Mat3 v)
{
if (vtav.m12 == 0)
{
return;
}
double c = 0, s = 0;
Schur2.Rot12(vtav, c, s);
Givens.Rot12Post(v, c, s);
}
public static SMat3 MmulAta(Mat3 a)
{
var mat3 = new SMat3();
mat3.SetSymmetric(a.m00 * a.m00 + a.m10 * a.m10 + a.m20 * a.m20,
a.m00 * a.m01 + a.m10 * a.m11 + a.m20 * a.m21,
a.m00 * a.m02 + a.m10 * a.m12 + a.m20 * a.m22,
a.m01 * a.m01 + a.m11 * a.m11 + a.m21 * a.m21,
a.m01 * a.m02 + a.m11 * a.m12 + a.m21 * a.m22,
a.m02 * a.m02 + a.m12 * a.m12 + a.m22 * a.m22);
return(mat3);
}
public static void GetSymmetricSvd(SMat3 a, SMat3 vtav, Mat3 v, double tol, int max_sweeps)
{
vtav.SetSymmetric(a);
v.Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
double delta = tol * MatUtils.Norm(vtav);
for (int i = 0; i < max_sweeps && MatUtils.off(vtav) > delta; ++i)
{
Rotate01(vtav, v);
Rotate02(vtav, v);
Rotate12(vtav, v);
}
}
public static void getSymmetricSvd(SMat3 a, SMat3 vtav, Mat3 v, float tol, int max_sweeps)
{
vtav.setSymmetric(a);
v.set(1, 0, 0, 0, 1, 0, 0, 0, 1);
float delta = tol * MatUtils.fnorm(vtav);
for (int i = 0; i < max_sweeps && MatUtils.off(vtav) > delta; ++i)
{
rotate01(vtav, v);
rotate02(vtav, v);
rotate12(vtav, v);
}
}
public static void rotate12(out SMat3 vtav, out Mat3 v, SMat3 ivtav, Mat3 iv)
{
vtav = ivtav;
v = iv;
if (vtav.m12 == 0)
{
return;
}
float c, s;
rot12(out vtav, out c, out s, vtav, c, s);
Givens.rot12_post(out v, c, s, v);
}
public static void pseudoinverse(out Mat3 Out, SMat3 d, Mat3 v, float tol)
{
float d0 = pinv(d.m00, tol), d1 = pinv(d.m11, tol), d2 = pinv(d.m22, tol);
Out = new Mat3();
Out.set(v.m00 * d0 * v.m00 + v.m01 * d1 * v.m01 + v.m02 * d2 * v.m02,
v.m00 * d0 * v.m10 + v.m01 * d1 * v.m11 + v.m02 * d2 * v.m12,
v.m00 * d0 * v.m20 + v.m01 * d1 * v.m21 + v.m02 * d2 * v.m22,
v.m10 * d0 * v.m00 + v.m11 * d1 * v.m01 + v.m12 * d2 * v.m02,
v.m10 * d0 * v.m10 + v.m11 * d1 * v.m11 + v.m12 * d2 * v.m12,
v.m10 * d0 * v.m20 + v.m11 * d1 * v.m21 + v.m12 * d2 * v.m22,
v.m20 * d0 * v.m00 + v.m21 * d1 * v.m01 + v.m22 * d2 * v.m02,
v.m20 * d0 * v.m10 + v.m21 * d1 * v.m11 + v.m22 * d2 * v.m12,
v.m20 * d0 * v.m20 + v.m21 * d1 * v.m21 + v.m22 * d2 * v.m22);
}
public static void getSymmetricSvd(SMat3 a, out SMat3 vtav, out Mat3 v,
float tol, int max_sweeps)
{
vtav = new SMat3();
v = new Mat3();
vtav.setSymmetric(a);
v.set(1, 0, 0, 0, 1, 0, 0, 0, 1);
float delta = tol * MatUtils.fnorm(vtav);
for (int i = 0; i < max_sweeps && MatUtils.off(vtav) > delta; ++i)
{
Schur2.rotate01(out vtav, out v, vtav, v);
Schur2.rotate02(out vtav, out v, vtav, v);
Schur2.rotate12(out vtav, out v, vtav, v);
}
}
public static Mat3 PseudoInverse(SMat3 d, Mat3 v, double tol)
{
double d0 = PinV(d.m00, tol), d1 = PinV(d.m11, tol), d2 = PinV(d.m22, tol);
var mat3 = new Mat3();
mat3.Set(v.m00 * d0 * v.m00 + v.m01 * d1 * v.m01 + v.m02 * d2 * v.m02,
v.m00 * d0 * v.m10 + v.m01 * d1 * v.m11 + v.m02 * d2 * v.m12,
v.m00 * d0 * v.m20 + v.m01 * d1 * v.m21 + v.m02 * d2 * v.m22,
v.m10 * d0 * v.m00 + v.m11 * d1 * v.m01 + v.m12 * d2 * v.m02,
v.m10 * d0 * v.m10 + v.m11 * d1 * v.m11 + v.m12 * d2 * v.m12,
v.m10 * d0 * v.m20 + v.m11 * d1 * v.m21 + v.m12 * d2 * v.m22,
v.m20 * d0 * v.m00 + v.m21 * d1 * v.m01 + v.m22 * d2 * v.m02,
v.m20 * d0 * v.m10 + v.m21 * d1 * v.m11 + v.m22 * d2 * v.m12,
v.m20 * d0 * v.m20 + v.m21 * d1 * v.m21 + v.m22 * d2 * v.m22);
return(mat3);
}
public void setSymmetric(SMat3 rhs)
{
}
public static double Norm(SMat3 a)
{
return(Math.Sqrt((a.m00 * a.m00) + (a.m01 * a.m01) + (a.m02 * a.m02)
+ (a.m01 * a.m01) + (a.m11 * a.m11) + (a.m12 * a.m12)
+ (a.m02 * a.m02) + (a.m12 * a.m12) + (a.m22 * a.m22)));
}
public static double off(SMat3 a)
{
return(Math.Sqrt(2 * ((a.m01 * a.m01) + (a.m02 * a.m02) + (a.m12 * a.m12))));
}
public static void rot12(out SMat3 m, out float c, out float s, SMat3 im, float Ic, float Is)
{
m = im; c = Ic; s = Is;
calcSymmetricGivensCoefficients(m.m11, m.m12, m.m22, out c, out s);
float cc = c * c;
float ss = s * s;
float mix = 2.0f * c * s * m.m12;
m.setSymmetric(m.m00, c * m.m01 - s * m.m02, s * m.m01 + c * m.m02,
cc * m.m11 - mix + ss * m.m22, 0, ss * m.m11 + mix + cc * m.m22);
}
public void setSymmetric(SMat3 rhs)
{
this.setSymmetric(rhs.m00, rhs.m01, rhs.m02, rhs.m11, rhs.m12, rhs.m22);
}
public QefSolver()
{
data = new QefData();
ata = new SMat3();
atb = new Vec3();
massPoint = new Vec3();
x = new Vec3();
hasSolution = false;
}
private SMat3(SMat3 rhs)
{
this.SetSymmetric(rhs);
}
public static void rotate12(out SMat3 vtav, out Mat3 v, SMat3 ivtav, Mat3 iv)
{
vtav = ivtav;
v = iv;
if (vtav.m12 == 0)
return;
float c, s;
rot12(out vtav, out c, out s, vtav, c, s);
Givens.rot12_post(out v, c, s, v);
}
private static float calcError(SMat3 origA, Vec3 x, Vec3 b)
{
Mat3 A = new Mat3();
Vec3 vtmp;
A.setSymmetric(origA);
MatUtils.vmul(out vtmp, A, x);
VecUtils.sub(out vtmp, b, vtmp);
return VecUtils.dot(vtmp, vtmp);
}
public static float solveSymmetric(SMat3 A, Vec3 b, out Vec3 x,
float svd_tol, int svd_sweeps, float pinv_tol)
{
Mat3 mtmp, pinv, V;
SMat3 VTAV;
getSymmetricSvd(A, out VTAV, out V, svd_tol, svd_sweeps);
psuedoinverse(out pinv, VTAV, V, pinv_tol);
MatUtils.vmul(out x, pinv, b);
return calcError(A, x, b);
}
public static void psuedoinverse(out Mat3 m, SMat3 d, Mat3 v, float tol)
{
m = new Mat3();
float d0 = pinv(d.m00, tol), d1 = pinv (d.m11, tol), d2 = pinv(d.m22, tol);
m.set(v.m00 * d0 * v.m00 + v.m01 * d1 * v.m01 + v.m02 * d2 * v.m02,
v.m00 * d0 * v.m10 + v.m01 * d1 * v.m11 + v.m02 * d2 * v.m12,
v.m00 * d0 * v.m20 + v.m01 * d1 * v.m21 + v.m02 * d2 * v.m22,
v.m10 * d0 * v.m00 + v.m11 * d1 * v.m01 + v.m12 * d2 * v.m02,
v.m10 * d0 * v.m10 + v.m11 * d1 * v.m11 + v.m12 * d2 * v.m12,
v.m10 * d0 * v.m20 + v.m11 * d1 * v.m21 + v.m12 * d2 * v.m22,
v.m20 * d0 * v.m00 + v.m21 * d1 * v.m01 + v.m22 * d2 * v.m02,
v.m20 * d0 * v.m10 + v.m21 * d1 * v.m11 + v.m22 * d2 * v.m12,
v.m20 * d0 * v.m20 + v.m21 * d1 * v.m21 + v.m22 * d2 * v.m22);
}
public static void getSymmetricSvd(SMat3 a, out SMat3 vtav, out Mat3 v,
float tol, int max_sweeps)
{
vtav = new SMat3();
v = new Mat3();
vtav.setSymmetric(a);
v.set(1, 0, 0, 0, 1, 0, 0, 0, 1);
float delta = tol * MatUtils.fnorm(vtav);
for (int i = 0; i < max_sweeps && MatUtils.off(vtav) > delta; ++i)
{
Schur2.rotate01(out vtav, out v, vtav, v);
Schur2.rotate02(out vtav, out v, vtav, v);
Schur2.rotate12(out vtav, out v, vtav, v);
}
}
public static float fnorm(SMat3 a)
{
return(Mathf.Sqrt((a.m00 * a.m00) + (a.m01 * a.m01) + (a.m02 * a.m02)
+ (a.m01 * a.m01) + (a.m11 * a.m11) + (a.m12 * a.m12)
+ (a.m02 * a.m02) + (a.m12 * a.m12) + (a.m22 * a.m22)));
}
public static float off(SMat3 a)
{
return(Mathf.Sqrt(2 * ((a.m01 * a.m01) + (a.m02 * a.m02) + (a.m12 * a.m12))));
}
public void setSymmetric(SMat3 rhs)
{
setSymmetric(rhs.m00, rhs.m01, rhs.m02, rhs.m11, rhs.m12, rhs.m22);
}
public static float fnorm(SMat3 a)
{
return Mathf.Sqrt((a.m00 * a.m00) + (a.m01 * a.m01) + (a.m02 * a.m02)
+ (a.m01 * a.m01) + (a.m11 * a.m11) + (a.m12 * a.m12)
+ (a.m02 * a.m02) + (a.m12 * a.m12) + (a.m22 * a.m22));
}
private SMat3(SMat3 rhs)
{
this.setSymmetric(rhs);
}
public void SetSymmetric(SMat3 rhs)
{
this.SetSymmetric(rhs.m00, rhs.m01, rhs.m02, rhs.m11, rhs.m12, rhs.m22);
}
public static void mmul_ata(out SMat3 mout, Mat3 a)
{
mout = new SMat3(a.m00 * a.m00 + a.m10 * a.m10 + a.m20 * a.m20,
a.m00 * a.m01 + a.m10 * a.m11 + a.m20 * a.m21,
a.m00 * a.m02 + a.m10 * a.m12 + a.m20 * a.m22,
a.m01 * a.m01 + a.m11 * a.m11 + a.m21 * a.m21,
a.m01 * a.m02 + a.m11 * a.m12 + a.m21 * a.m22,
a.m02 * a.m02 + a.m12 * a.m12 + a.m22 * a.m22);
}
public void SetSymmetric(SMat3 a)
{
SetSymmetric(a.m00, a.m01, a.m02, a.m11, a.m12, a.m22);
}
public static float off(SMat3 a)
{
return Mathf.Sqrt(2.0f * ((a.m01 * a.m01) + (a.m02 * a.m02) + (a.m12 * a.m12)));
}
SMat3(SMat3 rhs)
{
setSymmetric(rhs);
}
public static void vmul_symmetric(out Vec3 vout, SMat3 a, Vec3 v)
{
vout = new Vec3();
vout.x = (a.m00 * v.x) + (a.m01 * v.y) + (a.m02 * v.z);
vout.y = (a.m01 * v.x) + (a.m11 * v.y) + (a.m12 * v.z);
vout.z = (a.m02 * v.x) + (a.m12 * v.y) + (a.m22 * v.z);
}