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));
}
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 float getError(Vec3 pos)
{
if (!this.hasSolution)
{
this.setAta();
this.setAtb();
}
Vec3 atax;
MatUtils.vmul_symmetric(out atax, this.ata, pos);
return(VecUtils.dot(pos, atax) - 2.0f * VecUtils.dot(pos, this.atb)
+ this.data.btb);
}
