/// <summary> /// Repeatedly multiplying rotations together can lead to round-off error /// which causes the rotation to become invalid. This function uses the /// SVD to force orthonormality, giving the closest valid rotation matrix /// in a Mahalanobis distance sense. Call it every now and then if you're /// worried about errors accumulating. /// </summary> public void Normalise() { // Doing SVD and setting all singular values to 1 gives the // rotation matrix closest to M in the Mahalanobis distance sense. SVD svd = new SVD(this); // Only update if the SVD was successful (sometimes fails to converge // if already within precision of solution) if (!svd.Valid()) { Debug.WriteLine("VW::RotationMatrix::Normalise(): SVD failed."); } else { //this = svd.U() * svd.V().Transpose(); } }