From 29f5e84312b0de3238b17980abaed9575ba17a7c Mon Sep 17 00:00:00 2001
From: Paul Mathieu <paul@ponteilla.net>
Date: Thu, 13 Feb 2025 22:13:16 +0100
Subject: [PATCH] More math

---
 common/se3.js      | 36 ++++++++++++++++++++++++++++++++++++
 skycraft/linalg.ts |  6 +++++-
 2 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/common/se3.js b/common/se3.js
index a46513f..f4440c0 100644
--- a/common/se3.js
+++ b/common/se3.js
@@ -86,6 +86,42 @@ export function orientationOnly(m) {
     ];
 }
 
+export function fromBases(x, y, z) {
+    return [
+        x[0], x[1], x[2], 0,
+        y[0], y[1], y[2], 0,
+        z[0], z[1], z[2], 0,
+        0, 0, 0, 1,
+    ];
+}
+
+export function positionOnly(m) {
+    return [
+        1, 0, 0, 0,
+        0, 1, 0, 0,
+        0, 0, 1, 0,
+        m[12], m[13], m[14], m[15],
+    ]
+}
+
+export function setOrientation(m, o) {
+    return [
+        o[0], o[1], o[2], m[3],
+        o[4], o[5], o[6], m[7],
+        o[8], o[9], o[10], m[11],
+        m[12], m[13], m[14], m[15],
+    ];
+}
+
+export function setPosition(m, x) {
+    return [
+        m[0], m[1], m[2], m[3],
+        m[4], m[5], m[6], m[7],
+        m[8], m[9], m[10], m[11],
+        x[0], x[1], x[2], 1,
+    ];
+}
+
 export function inverse(m) {
     const t = apply(m, [0, 0, 0, 1]);
     const r = orientationOnly(m);
diff --git a/skycraft/linalg.ts b/skycraft/linalg.ts
index 32b4ca3..7eb6cc7 100644
--- a/skycraft/linalg.ts
+++ b/skycraft/linalg.ts
@@ -38,4 +38,8 @@ export function scale(a: Vec3, s: number) : Vec3 {
         a[1] * s,
         a[2] * s,
     ];
-}
\ No newline at end of file
+}
+
+export function normalize(a: Vec3) : Vec3 {
+    return scale(a, 1/norm(a));
+}