124 lines
3.1 KiB
TypeScript
124 lines
3.1 KiB
TypeScript
import {Mat4} from './linalg';
|
|
import * as se3 from '../se3';
|
|
|
|
export interface Quat {
|
|
x: number,
|
|
y: number,
|
|
z: number,
|
|
w: number,
|
|
}
|
|
|
|
export function mat2Quat(m: Mat4) : Quat {
|
|
const q : Quat = {};
|
|
|
|
if (m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] > 0.0) {
|
|
const t = + m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0;
|
|
const s = 0.5 / Math.sqrt(t);
|
|
q.w = s * t;
|
|
q.z = (m[1 * 4 + 0] - m[0 * 4 + 1]) * s;
|
|
q.y = (m[0 * 4 + 2] - m[2 * 4 + 0]) * s;
|
|
q.x = (m[2 * 4 + 1] - m[1 * 4 + 2]) * s;
|
|
} else if (m[0 * 4 + 0] > m[1 * 4 + 1] && m[0 * 4 + 0] > m[2 * 4 + 2]) {
|
|
const t = + m[0 * 4 + 0] - m[1 * 4 + 1] - m[2 * 4 + 2] + 1.0;
|
|
const s = 0.5 / Math.sqrt(t);
|
|
q.x = s * t;
|
|
q.y = (m[1 * 4 + 0] + m[0 * 4 + 1]) * s;
|
|
q.z = (m[0 * 4 + 2] + m[2 * 4 + 0]) * s;
|
|
q.w = (m[2 * 4 + 1] - m[1 * 4 + 2]) * s;
|
|
} else if (m[1 * 4 + 1] > m[2 * 4 + 2]) {
|
|
const t = - m[0 * 4 + 0] + m[1 * 4 + 1] - m[2 * 4 + 2] + 1.0;
|
|
const s = 0.5 / Math.sqrt(t);
|
|
q.y = s * t;
|
|
q.x = (m[1 * 4 + 0] + m[0 * 4 + 1]) * s;
|
|
q.w = (m[0 * 4 + 2] - m[2 * 4 + 0]) * s;
|
|
q.z = (m[2 * 4 + 1] + m[1 * 4 + 2]) * s;
|
|
} else {
|
|
const t = - m[0 * 4 + 0] - m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0;
|
|
const s = 0.5 / Math.sqrt(t);
|
|
q.z = s * t;
|
|
q.w = (m[1 * 4 + 0] - m[0 * 4 + 1]) * s;
|
|
q.x = (m[0 * 4 + 2] + m[2 * 4 + 0]) * s;
|
|
q.y = (m[2 * 4 + 1] + m[1 * 4 + 2]) * s;
|
|
}
|
|
|
|
return q;
|
|
}
|
|
|
|
export function quat2Mat(q: Quat): Mat4 {
|
|
const m: Mat4 = se3.identity();
|
|
|
|
const x2 = q.x + q.x;
|
|
const y2 = q.y + q.y;
|
|
const z2 = q.z + q.z;
|
|
{
|
|
const xx2 = q.x * x2;
|
|
const yy2 = q.y * y2;
|
|
const zz2 = q.z * z2;
|
|
m[0 * 4 + 0] = 1.0 - yy2 - zz2;
|
|
m[1 * 4 + 1] = 1.0 - xx2 - zz2;
|
|
m[2 * 4 + 2] = 1.0 - xx2 - yy2;
|
|
}
|
|
{
|
|
const yz2 = q.y * z2;
|
|
const wx2 = q.w * x2;
|
|
m[1 * 4 + 2] = yz2 - wx2;
|
|
m[2 * 4 + 1] = yz2 + wx2;
|
|
}
|
|
{
|
|
const xy2 = q.x * y2;
|
|
const wz2 = q.w * z2;
|
|
m[0 * 4 + 1] = xy2 - wz2;
|
|
m[1 * 4 + 0] = xy2 + wz2;
|
|
}
|
|
{
|
|
const xz2 = q.x * z2;
|
|
const wy2 = q.w * y2;
|
|
m[2 * 4 + 0] = xz2 - wy2;
|
|
m[0 * 4 + 2] = xz2 + wy2;
|
|
}
|
|
|
|
return m;
|
|
}
|
|
|
|
export function normalize(q: Quat) : Quat {
|
|
const n = norm(q);
|
|
if (n < 1e-10) {
|
|
return {x: 0, y: 0, z: 0, w: 1};
|
|
}
|
|
return {
|
|
x: q.x / n,
|
|
y: q.y / n,
|
|
z: q.z / n,
|
|
w: q.w / n,
|
|
};
|
|
}
|
|
|
|
export function diff(q0: Quat, q1: Quat) {
|
|
return {
|
|
x: q0.x - q1.x,
|
|
y: q0.y - q1.y,
|
|
z: q0.z - q1.z,
|
|
w: q0.w - q1.w,
|
|
};
|
|
}
|
|
export function norm(q: Quat) {
|
|
return Math.sqrt(q.x**2 + q.y**2 + q.z**2 + q.w**2);
|
|
}
|
|
|
|
export function add(q0: Quat, q1: Quat) {
|
|
return {
|
|
x: q0.x + q1.x,
|
|
y: q0.y + q1.y,
|
|
z: q0.z + q1.z,
|
|
w: q0.w + q1.w,
|
|
};
|
|
}
|
|
|
|
export function scale(q: Quat, a: number): Quat {
|
|
return {
|
|
x: a * q.x,
|
|
y: a * q.y,
|
|
z: a * q.z,
|
|
w: a * q.w,
|
|
};
|
|
} |