- SGMatrix<T> m0(q0, v0);
-
- // Check the tqo forms of the inverse for that kind of special matrix
- SGMatrix<T> m1, m2;
- invert(m1, m0);
- m2 = transNeg(m0);
+ SGMatrix<T> m0 = SGMatrix<T>::unit();
+ m0.postMultTranslate(v0);
+ m0.postMultRotate(q0);
+
+ // Check the three forms of the inverse for that kind of special matrix
+ SGMatrix<T> m1 = SGMatrix<T>::unit();
+ m1.preMultTranslate(-v0);
+ m1.preMultRotate(inverse(q0));
+
+ SGMatrix<T> m2, m3;
+ invert(m2, m0);
+ m3 = transNeg(m0);