se3: fix inverse

gl.js

@@ -46,4 +46,4 @@ export async function loadTexture(gl, url) {
     gl.generateMipmap(gl.TEXTURE_2D);
 
     return texture;
-}
+}

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) {
+export function orientationOnly(m) {
-    // TODO: translation
     return [
-        m[0], m[4], m[8], 0.0,
+        m[0], m[1], m[2], 0,
-        m[1], m[5], m[9], 0.0,
+        m[4], m[5], m[6], 0,
-        m[2], m[6], m[10], 0.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] +
@@ -109,4 +136,4 @@ export function perspective(fov, aspectRatio, near, far) {
         0.0, 0.0, (near + far) * rangeInv, -1,
         0.0, 0.0, near * far * rangeInv * 2, 0,
     ];
-};
+};