public static float calcError(Mat3 A, Vector3 x, Vector3 b)
{
Vector3 vtmp;
MatUtils.vmul(out vtmp, A, x);
vtmp = b - vtmp;
return(Vector3.Dot(vtmp, vtmp));
}
private static float calcError(Mat3 A, Vec3 x, Vec3 b)
{
Vec3 vtmp;
MatUtils.vmul(out vtmp, A, x);
VecUtils.sub(out vtmp, b, vtmp);
return(VecUtils.dot(vtmp, vtmp));
}
public static float calcError(SMat3 origA, Vector3 x, Vector3 b)
{
Mat3 A = new Mat3();
Vector3 vtmp;
A.setSymmetric(origA);
MatUtils.vmul(out vtmp, A, x);
vtmp = b - vtmp;
return(Vector3.Dot(vtmp, vtmp));
}
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 solveLeastSquares(Mat3 a, Vector3 b, Vector3 x, float svd_tol, int svd_sweeps, float pinv_tol)
{
Mat3 at;
SMat3 ata;
Vector3 atb;
MatUtils.transpose(out at, a);
MatUtils.mmul_ata(out ata, a);
MatUtils.vmul(out atb, at, b);
return(solveSymmetric(ata, atb, x, svd_tol, svd_sweeps, pinv_tol));
}
public static float solveSymmetric(SMat3 A, Vector3 b, Vector3 x, float svd_tol, int svd_sweeps, float pinv_tol)
{
Mat3 pinv;
Mat3 V = new Mat3();
SMat3 VTAV = new SMat3();
getSymmetricSvd(A, VTAV, V, svd_tol, svd_sweeps);
pseudoinverse(out pinv, VTAV, V, pinv_tol);
MatUtils.vmul(out x, pinv, b);
return(calcError(A, x, b));
}
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));
}
