se3: fix inverse

se3.js

@@ -59,6 +59,15 @@ export function rotxyz(x, y, z) {
     return [rotx(x), roty(y), rotz(z)].reduce(product);
 }
 
+export function rotationOnly(m) {
+    return [
+        m[0], m[1], m[2], 0.0,
+        m[4], m[5], m[6], 0.0,
+        m[8], m[9], m[10], 0.0,
+        0, 0, 0, 1,
+    ];
+}
+
 export function translation(x, y, z) {
     return [
         1.0, 0.0, 0.0, 0.0,
@@ -68,16 +77,34 @@ export function translation(x, y, z) {
     ];
 }
 
-export function inverse(m) {
-    // TODO: translation
+export function orientationOnly(m) {
     return [
-        m[0], m[4], m[8], 0.0,
-        m[1], m[5], m[9], 0.0,
-        m[2], m[6], m[10], 0.0,
+        m[0], m[1], m[2], 0,
+        m[4], m[5], m[6], 0,
+        m[8], m[9], m[10], 0,
         0, 0, 0, 1,
     ];
 }
 
+export function inverse(m) {
+    const t = apply(m, [0, 0, 0, 1]);
+    const r = orientationOnly(m);
+    const newR = [
+        m[0], m[4], m[8], 0,
+        m[1], m[5], m[9], 0,
+        m[2], m[6], m[10], 0,
+        0, 0, 0, 1,
+    ];
+    const newT = apply(newR, t);
+    const out = newR;
+
+    out[12] = -newT[0];
+    out[13] = -newT[1];
+    out[14] = -newT[2];
+
+    return out;
+}
+
 export function product(a, b) {
     const c = (i, j) => (
         a[4 * 0 + i] * b[4 * j + 0] +