pol/wmc
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

se3: fix inverse

Browse Source
This commit is contained in:
Paul Mathieu 2022-10-18 13:49:35 -07:00
parent 5fefa04891
commit 60a2e8f19e
2 changed files with 34 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
se3.js
View File

@ -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] +