diff --git a/hyperpoint.cpp b/hyperpoint.cpp index 1f474b5c..36f00257 100644 --- a/hyperpoint.cpp +++ b/hyperpoint.cpp @@ -809,4 +809,19 @@ transmatrix spin_towards(const transmatrix Position, const hyperpoint goal, int return T; } +ld ortho_error(transmatrix T) { + + ld err = 0; + + for(int x=0; x<3; x++) for(int y=0; y<3; y++) { + ld s = 0; + for(int z=0; z<3; z++) s += T[z][x] * T[z][y]; + + s -= (x==y); + err += s*s; + } + + return err; + } + }