hyperpoint:: ortho_error

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;      
+  }
+
 }