@woosh/meep-engine
Version:
Pure JavaScript game engine. Fully featured and production ready.
1,650 lines (1,330 loc) • 44.2 kB
JavaScript
import { assert } from "../assert.js";
import Signal from "../events/signal/Signal.js";
import { clamp } from "../math/clamp.js";
import { clamp01 } from "../math/clamp01.js";
import { DEG_TO_RAD } from "../math/DEG_TO_RAD.js";
import { EPSILON } from "../math/EPSILON.js";
import { epsilonEquals } from "../math/epsilonEquals.js";
import { lerp } from "../math/lerp.js";
import { PI2 } from "../math/PI2.js";
import { computeHashFloat } from "../primitives/numbers/computeHashFloat.js";
import { v3_dot } from "./vec3/v3_dot.js";
import { v4_length } from "./vec4/v4_length.js";
import Vector3 from "./Vector3.js";
const scratch_v3_a = new Vector3();
const scratch_v3_b = new Vector3();
const scratch_v3_c = new Vector3();
const sin = Math.sin;
const cos = Math.cos;
/**
* Implementation of a quaternion.
* Represents rotation in 3d space
*
* Iterating through a Quaternion instance will yield its components `(x, y, z, w)` in the corresponding order.
* Note that a quaternion must be {@link normalize}d to properly represent rotation. Check documentation of individual operations when in doubt.
* @see https://en.wikipedia.org/wiki/Quaternion
* @implements Iterable<number>
*
* @author Alex Goldring
* @copyright Company Named Limited (c) 2025
*/
export class Quaternion {
/**
*
* @param {number} [x=0]
* @param {number} [y=0]
* @param {number} [z=0]
* @param {number} [w=1]
* @constructor
*/
constructor(x = 0, y = 0, z = 0, w = 1) {
assert.isNumber(x, 'x');
assert.isNumber(y, 'y');
assert.isNumber(z, 'z');
assert.isNumber(w, 'w');
/**
*
* @type {number}
*/
this.x = x;
/**
*
* @type {number}
*/
this.y = y;
/**
*
* @type {number}
*/
this.z = z;
/**
*
* @type {number}
*/
this.w = w;
/**
* Fires when the value of the quaternion changes
* Signature of the signal data is as follows:
* (new_x, new_y, new_z, new_w, old_x, old_y, old_z, old_w)
* @readonly
* @type {Signal<number, number, number, number, number, number, number, number>}
*/
this.onChanged = new Signal();
}
// Making Quaternion comply to array interface
/**
*
* @return {number}
*/
get 0() {
return this.x;
}
/**
*
* @return {number}
*/
get 1() {
return this.y;
}
/**
*
* @return {number}
*/
get 2() {
return this.z;
}
/**
*
* @return {number}
*/
get 3() {
return this.w;
}
/**
*
* @param {number} v
*/
set 0(v) {
this.x = v;
}
/**
*
* @param {number} v
*/
set 1(v) {
this.y = v;
}
/**
*
* @param {number} v
*/
set 2(v) {
this.z = v;
}
/**
*
* @param {number} v
*/
set 3(v) {
this.w = v;
}
/**
* Making quaternion iterable
* @returns {Generator<number>}
*/
* [Symbol.iterator]() {
yield this.x;
yield this.y;
yield this.z;
yield this.w;
}
/**
* Orient quaternion on a `forward` vector, with the spin matching `up` vector
* Useful for `lookAt` operations, such as for camera or inverse kinematics.
* Normalizes input, meaning input does not have to be normalized.
*
* NOTE: `forward` and `up` vectors being the same is allowed, but you will likely get unexpected rotation along the look axis, so prefer not to do it.
*
* @param {number} fx forward vector
* @param {number} fy forward vector
* @param {number} fz forward vector
* @param {number} ux up vector
* @param {number} uy up vector
* @param {number} uz up vector
* @returns {this}
*/
_lookRotation(
fx, fy, fz,
ux, uy, uz
) {
scratch_v3_a.set(fx, fy, fz);
scratch_v3_a.normalize();
scratch_v3_c._crossVectors(ux, uy, uz, scratch_v3_a.x, scratch_v3_a.y, scratch_v3_a.z);
if (scratch_v3_c.lengthSq() === 0) {
// up and forward are parallel
/*
adjust forward direction slightly
code take from : https://github.com/mrdoob/three.js/blob/88e8954b69377dad4ad1ceaf1a383f3536e88e5a/src/math/Matrix4.js#L304
*/
if (Math.abs(uz) === 1) {
scratch_v3_a.x += 0.001;
} else {
scratch_v3_a.z += 0.001;
}
scratch_v3_a.normalize();
scratch_v3_c._crossVectors(ux, uy, uz, scratch_v3_a.x, scratch_v3_a.y, scratch_v3_a.z);
}
scratch_v3_c.normalize();
scratch_v3_b.crossVectors(scratch_v3_a, scratch_v3_c);
// construct partial transform matrix
const m00 = scratch_v3_c.x;
const m10 = scratch_v3_c.y;
const m20 = scratch_v3_c.z;
const m01 = scratch_v3_b.x;
const m11 = scratch_v3_b.y;
const m21 = scratch_v3_b.z;
const m02 = scratch_v3_a.x;
const m12 = scratch_v3_a.y;
const m22 = scratch_v3_a.z;
return this.__setFromRotationMatrix(
m00, m01, m02,
m10, m11, m12,
m20, m21, m22
);
}
/**
* Orient quaternion to align with the `forward` direction.
* @param {Vector3} forward Does not need to be normalized.
* @param {Vector3} [up=Vector3.up] Does not need to be normalized.
* @returns {this}
*/
lookRotation(forward, up = Vector3.up) {
return this._lookRotation(
forward.x, forward.y, forward.z,
up.x, up.y, up.z
);
}
/**
* Vector dot product in 4 dimensions
* @param {Quaternion} other
* @return {number}
*/
dot(other) {
return this.x * other.x
+ this.y * other.y
+ this.z * other.z
+ this.w * other.w
;
}
/**
* Makes this quaternion into an inverse of the other
* @param {Quaternion} other
* @returns {this}
*/
copyInverse(other) {
this.copy(other);
return this.invert();
}
/**
* Calculates the inverse.
* Correctly handles unnormalized quaternions.
*
* If your quaternion is normalized, you can use {@link conjugate} instead for speed.
* @returns {this}
* @see conjugate
*/
invert() {
const x = this.x;
const y = this.y;
const z = this.z;
const w = this.w;
const dot_product = x * x + y * y + z * z + w * w;
if (dot_product === 0) {
// 0 magnitude, avoid division by 0 and set identity (arbitrage)
return this.set(0, 0, 0, 1);
}
const invDot = 1.0 / dot_product;
const _x = -x * invDot;
const _y = -y * invDot;
const _z = -z * invDot;
const _w = w * invDot;
return this.set(_x, _y, _z, _w);
}
/**
* NOTE: this is the same as {@link invert} if the quaternion is normalized.
* @returns {this}
* @see invert
*/
conjugate() {
return this.set(-this.x, -this.y, -this.z, this.w);
}
/**
* Returns angle between this orientation and another
* @param {Quaternion} other
* @return {number} angle in radians
*/
angleTo(other) {
const x0 = this.x;
const y0 = this.y;
const z0 = this.z;
const w0 = this.w;
const x1 = other.x;
const y1 = other.y;
const z1 = other.z;
const w1 = other.w;
const dot = x0 * x1 + y0 * y1 + z0 * z1 + w0 * w1;
const dot_clamped = clamp(dot, -1, 1);
const dot_abs = Math.abs(dot_clamped);
return Math.acos(dot_abs) * 2;
}
/**
*
* @param {Vector3} axis
* @param {number} angle
* @returns {Quaternion}
*/
static fromAxisAngle(axis, angle) {
const r = new Quaternion();
r.fromAxisAngle(axis, angle);
return r;
}
/**
* Set quaternion from axis + angle definition
* @param {Vector3} axis
* @param {number} angle
* @returns {this}
*/
fromAxisAngle(axis, angle) {
assert.defined(axis, 'axis');
assert.isObject(axis, 'axis');
assert.isNumber(angle, 'angle');
return this._fromAxisAngle(axis.x, axis.y, axis.z, angle);
}
/**
*
* @param {number} axis_x
* @param {number} axis_y
* @param {number} axis_z
* @param {number} angle
* @returns {this}
*/
_fromAxisAngle(axis_x, axis_y, axis_z, angle) {
assert.isNumber(axis_x, 'axis_x');
assert.isNumber(axis_y, 'axis_y');
assert.isNumber(axis_z, 'axis_z');
assert.isNumber(angle, 'angle');
assert.notNaN(axis_x, 'axis_x');
assert.notNaN(axis_y, 'axis_y');
assert.notNaN(axis_z, 'axis_z');
assert.notNaN(angle, 'angle');
const halfAngle = angle * 0.5;
const sinA2 = sin(halfAngle);
const cosA2 = cos(halfAngle);
const qx = axis_x * sinA2;
const qy = axis_y * sinA2;
const qz = axis_z * sinA2;
const qw = cosA2;
//normalize
const length = Math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
const m = 1 / length;
const x = qx * m;
const y = qy * m;
const z = qz * m;
const w = qw * m;
return this.set(x, y, z, w);
}
/**
* Given a direction axis, compute rotation quaternions from current rotation to that axis as swing and twist. Swing moves to a given orientation while without "twisting",
* `twist` just the twist around the given axis, no change in orientation.
* @param {Vector3} axis
* @param {Quaternion} swing Swing quaternion will be written here
* @param {Quaternion} twist Twist quaternion will be written here
* @returns {void}
* @see slerp
*/
computeSwingAndTwist(axis, swing, twist) {
assert.defined(axis, 'axis');
assert.defined(swing, 'swing');
assert.defined(twist, 'twist');
// see https://stackoverflow.com/questions/3684269/component-of-a-quaternion-rotation-around-an-axis
const x = this.x;
const y = this.y;
const z = this.z;
const w = this.w;
// perform projection of rotation onto axis
const d = v3_dot(x, y, z, axis.x, axis.y, axis.z);
const mag2 = axis.x * axis.x + axis.y * axis.y + axis.z * axis.z;
const m = d / mag2;
const px = axis.x * m;
const py = axis.y * m;
const pz = axis.z * m;
if (d < 0) {
// axis points in the opposite direction from calculated twist, invert all components
twist.set(-px, -py, -pz, -w);
} else {
twist.set(px, py, pz, w);
}
twist.normalize();
// rotation * twist.conjugated(), basically undo the twist
swing._multiplyQuaternions(x, y, z, w, -twist.x, -twist.y, -twist.z, twist.w);
}
/**
* Compute rotation (twist) around input axis
* @param {Vector3} axis
* @returns {number} in radians
*/
computeTwistAngle(axis) {
assert.defined(axis, "axis");
const swing = new Quaternion();
const twist = new Quaternion();
this.computeSwingAndTwist(axis, swing, twist);
// Extract the twist angle from quaternion, see {@link #toAxisAngle}
return Math.acos(twist.w) * 2;
}
/**
* Decompose quaternion to the axis of rotation and angle around this axis.
* @param {Vector3} out_axis axis will be written here
* @returns {number} angle in radians
*/
toAxisAngle(out_axis) {
const rad = Math.acos(this.w) * 2.0;
const s = sin(rad * 0.5);
if (Math.abs(s) > EPSILON) {
out_axis.set(
this.x / s,
this.y / s,
this.z / s
);
} else {
// If s is zero, return any axis (no rotation - axis does not matter)
out_axis.set(1, 0, 0);
}
return rad;
}
/**
*
* @returns {this}
*/
normalize() {
let l = this.length();
if (l < EPSILON) {
// Quaternion has close to 0 length
// use identity, avoid division by 0
this.set(0, 0, 0, 1);
} else {
const m = 1 / l;
this.multiplyScalar(m);
}
return this;
}
/**
*
* @param {number} val
* @return {this}
*/
multiplyScalar(val) {
return this.set(
this.x * val,
this.y * val,
this.z * val,
this.w * val
);
}
/**
* @param {Quaternion} other
* @returns {this}
*/
multiply(other) {
return this.multiplyQuaternions(this, other);
}
/**
*
* @param {Quaternion} first
* @param {Quaternion} second
* @returns {this}
*/
multiplyQuaternions(first, second) {
const aX = first.x;
const aY = first.y;
const aZ = first.z;
const aW = first.w;
const bX = second.x;
const bY = second.y;
const bZ = second.z;
const bW = second.w;
return this._multiplyQuaternions(aX, aY, aZ, aW, bX, bY, bZ, bW)
}
/**
*
* @param {number} ax
* @param {number} ay
* @param {number} az
* @param {number} aw
* @param {number} bx
* @param {number} by
* @param {number} bz
* @param {number} bw
* @returns {this}
*/
_multiplyQuaternions(
ax, ay, az, aw,
bx, by, bz, bw
) {
// see http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm
const x = ax * bw + aw * bx + ay * bz - az * by;
const y = ay * bw + aw * by + az * bx - ax * bz;
const z = az * bw + aw * bz + ax * by - ay * bx;
const w = aw * bw - ax * bx - ay * by - az * bz;
return this.set(x, y, z, w);
}
/**
*
* @return {number}
*/
length() {
const x = this.x;
const y = this.y;
const z = this.z;
const w = this.w;
return v4_length(x, y, z, w);
}
/**
*
* @param {Quaternion} other
* @param {number} max_delta in radians
* @returns {this}
*/
rotateTowards(other, max_delta) {
Quaternion.rotateTowards(this, this, other, max_delta);
return this;
}
/**
*
* @param {Vector3} source
* @param {Vector3} target
* @param {Vector3} [up]
* @see lookRotation
*/
lookAt(source, target, up = Vector3.up) {
const forward = scratch_v3_a;
forward.subVectors(target, source);
forward.normalize();
this.lookRotation(forward, up);
}
/**
* @deprecated use {@link fromEulerAnglesXYZ} or others specifically.
*
* @param {number} x
* @param {number} y
* @param {number} z
* @param {String} [order='XYZ'] a combination of capital letters X,Y,Z. Examples: XYZ, YXZ
* @returns {this}
*
* @see fromEulerAnglesXYZ
* @see fromEulerAnglesYXZ
* @see fromEulerAnglesZXY
* @see fromEulerAnglesZYX
* @see fromEulerAnglesYZX
* @see fromEulerAnglesXZY
*/
__setFromEuler(
x,
y,
z,
order = 'XYZ'
) {
// http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m
if (order === 'XYZ') {
this.fromEulerAnglesXYZ(x, y, z);
} else if (order === 'YXZ') {
this.fromEulerAnglesYXZ(x, y, z);
} else if (order === 'ZXY') {
this.fromEulerAnglesZXY(x, y, z);
} else if (order === 'ZYX') {
this.fromEulerAnglesZYX(x, y, z);
} else if (order === 'YZX') {
this.fromEulerAnglesYZX(x, y, z);
} else if (order === 'XZY') {
this.fromEulerAnglesXZY(x, y, z);
} else {
throw new Error(`Invalid order '${order}', bust be 3 capital letters consisting of X,Y and Z`);
}
return this;
}
/**
* @see https://localcoder.org/euler-angle-to-quaternion-then-quaternion-to-euler-angle
* @see https://discourse.mcneel.com/t/what-is-the-right-method-to-convert-quaternion-to-plane-using-rhinocommon/92411/21?page=2
* @param {Vector3} result
*/
toEulerAnglesXYZ(result) {
const x = this.x;
const y = this.y;
const z = this.z;
const w = this.w;
const w2 = w * w;
const x2 = x * x;
const y2 = y * y;
const z2 = z * z;
const r31 = 2 * (x * w - y * z);
const r32 = w2 - x2 - y2 + z2;
const psi = Math.atan2(r31, r32);
const r21 = 2 * (x * z + y * w);
const theta = Math.asin(r21);
const r11 = 2 * (z * w - x * y);
const r12 = w2 + x2 - y2 - z2;
const phi = Math.atan2(r11, r12);
result.set(psi, theta, phi);
}
/**
* Adapted from http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
* @param {Vector3} result
*/
toEulerAnglesYXZ(result) {
const x = this.x;
const y = this.y;
const z = this.z;
const w = this.w;
const z2 = z * z;
const x2 = x * x;
const w2 = w * w;
const y2 = y * y;
const r11 = 2 * (x * z + w * y),
r12 = w2 - x2 - y2 + z2,
r21 = -2 * (y * z - w * x),
r31 = 2 * (x * y + w * z),
r32 = w2 - x2 + y2 - z2;
const psi = Math.atan2(r31, r32);
const theta = Math.asin(r21);
const phi = Math.atan2(r11, r12);
// result.set(psi, theta, phi);
result.set(theta, phi, psi);
}
/**
* Adapted from http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
* @param {Vector3} result
*/
toEulerAnglesZYX(result) {
const x = this.x;
const y = this.y;
const z = this.z;
const w = this.w;
const xx = x * x;
const yy = y * y;
const zz = z * z;
const ww = w * w;
const r11 = 2 * (x * y + w * z),
r12 = ww + xx - yy - zz,
r21 = -2 * (x * z - w * y),
r31 = 2 * (y * z + w * x),
r32 = ww - xx - yy + zz;
const psi = Math.atan2(r31, r32);
const theta = Math.asin(r21);
const phi = Math.atan2(r11, r12);
result.set(psi, theta, phi);
}
/**
* Set rotation from Euler angles in degrees.
*
* Order is explicitly XYZ.
*
* Utility shortcut, same as `fromEulerAnglesXYZ(x * π / 180, y * π / 180, z * π / 180)`
*
* @param {number} [x] angle in degrees
* @param {number} [y] angle in degrees
* @param {number} [z] angle in degrees
* @returns {this}
*
* @see fromEulerAnglesXYZ
*/
fromDegrees(x = 0, y = 0, z = 0) {
return this.fromEulerAnglesXYZ(
x * DEG_TO_RAD,
y * DEG_TO_RAD,
z * DEG_TO_RAD
);
}
/**
* XYZ order
* @source: https://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
* @see http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m
* @see https://github.com/mrdoob/three.js/blob/510705cde208b165fd87946b0f8504a1cd6dbe83/src/math/Quaternion.js#L206
* @param {number} x angle in X axis in radians
* @param {number} y angle in Y axis in radians
* @param {number} z angle in Z axis in radians
* @returns {this}
*/
fromEulerAnglesXYZ(x, y, z) {
const half_x = x * 0.5;
const half_y = y * 0.5;
const half_z = z * 0.5;
const s1 = sin(half_x);
const s2 = sin(half_y);
const s3 = sin(half_z);
const c1 = cos(half_x);
const c2 = cos(half_y);
const c3 = cos(half_z);
const _x = s1 * c2 * c3 + c1 * s2 * s3;
const _y = c1 * s2 * c3 - s1 * c2 * s3;
const _z = c1 * c2 * s3 + s1 * s2 * c3;
const _w = c1 * c2 * c3 - s1 * s2 * s3;
return this.set(_x, _y, _z, _w);
}
/**
* YXZ order
* @source: https://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
* @see http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m
* @see https://github.com/mrdoob/three.js/blob/510705cde208b165fd87946b0f8504a1cd6dbe83/src/math/Quaternion.js#L206
* @param {number} x angle in X axis in radians
* @param {number} y angle in Y axis in radians
* @param {number} z angle in Z axis in radians
* @returns {this}
*/
fromEulerAnglesYXZ(x, y, z) {
const scaled_x = x * 0.5;
const scaled_y = y * 0.5;
const scaled_z = z * 0.5;
const s1 = sin(scaled_x);
const s2 = sin(scaled_y);
const s3 = sin(scaled_z);
const c1 = cos(scaled_x);
const c2 = cos(scaled_y);
const c3 = cos(scaled_z);
const _x = s1 * c2 * c3 + c1 * s2 * s3;
const _y = c1 * s2 * c3 - s1 * c2 * s3;
const _z = c1 * c2 * s3 - s1 * s2 * c3;
const _w = c1 * c2 * c3 + s1 * s2 * s3;
return this.set(_x, _y, _z, _w);
}
/**
* ZXY order
* @source: https://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
* @see http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m
* @see https://github.com/mrdoob/three.js/blob/510705cde208b165fd87946b0f8504a1cd6dbe83/src/math/Quaternion.js#L206
* @param {number} x angle in X axis in radians
* @param {number} y angle in Y axis in radians
* @param {number} z angle in Z axis in radians
* @returns {this}
*/
fromEulerAnglesZXY(x, y, z) {
const scaled_x = x * 0.5;
const scaled_y = y * 0.5;
const scaled_z = z * 0.5;
const s1 = sin(scaled_x);
const s2 = sin(scaled_y);
const s3 = sin(scaled_z);
const c1 = cos(scaled_x);
const c2 = cos(scaled_y);
const c3 = cos(scaled_z);
const _x = s1 * c2 * c3 - c1 * s2 * s3;
const _y = c1 * s2 * c3 + s1 * c2 * s3;
const _z = c1 * c2 * s3 + s1 * s2 * c3;
const _w = c1 * c2 * c3 - s1 * s2 * s3;
return this.set(_x, _y, _z, _w);
}
/**
* ZYX order
* @source: https://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
* @see http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m
* @see https://github.com/mrdoob/three.js/blob/510705cde208b165fd87946b0f8504a1cd6dbe83/src/math/Quaternion.js#L206
* @param {number} x angle in X axis in radians
* @param {number} y angle in Y axis in radians
* @param {number} z angle in Z axis in radians
* @returns {this}
*/
fromEulerAnglesZYX(x, y, z) {
const scaled_x = x * 0.5;
const scaled_y = y * 0.5;
const scaled_z = z * 0.5;
const s1 = sin(scaled_x);
const s2 = sin(scaled_y);
const s3 = sin(scaled_z);
const c1 = cos(scaled_x);
const c2 = cos(scaled_y);
const c3 = cos(scaled_z);
const _x = s1 * c2 * c3 - c1 * s2 * s3;
const _y = c1 * s2 * c3 + s1 * c2 * s3;
const _z = c1 * c2 * s3 - s1 * s2 * c3;
const _w = c1 * c2 * c3 + s1 * s2 * s3;
return this.set(_x, _y, _z, _w);
}
/**
* YZX order
* @source: https://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
* @see http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m
* @see https://github.com/mrdoob/three.js/blob/510705cde208b165fd87946b0f8504a1cd6dbe83/src/math/Quaternion.js#L206
* @param {number} x angle in X axis in radians
* @param {number} y angle in Y axis in radians
* @param {number} z angle in Z axis in radians
* @returns {this}
*/
fromEulerAnglesYZX(x, y, z) {
const scaled_x = x * 0.5;
const scaled_y = y * 0.5;
const scaled_z = z * 0.5;
const s1 = sin(scaled_x);
const s2 = sin(scaled_y);
const s3 = sin(scaled_z);
const c1 = cos(scaled_x);
const c2 = cos(scaled_y);
const c3 = cos(scaled_z);
const _x = s1 * c2 * c3 + c1 * s2 * s3;
const _y = c1 * s2 * c3 + s1 * c2 * s3;
const _z = c1 * c2 * s3 - s1 * s2 * c3;
const _w = c1 * c2 * c3 - s1 * s2 * s3;
return this.set(_x, _y, _z, _w);
}
/**
* XZY order
* @source: https://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
* @see http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m
* @see https://github.com/mrdoob/three.js/blob/510705cde208b165fd87946b0f8504a1cd6dbe83/src/math/Quaternion.js#L206
* @param {number} x angle in X axis in radians
* @param {number} y angle in Y axis in radians
* @param {number} z angle in Z axis in radians
* @returns {this}
*/
fromEulerAnglesXZY(x, y, z) {
const scaled_x = x * 0.5;
const scaled_y = y * 0.5;
const scaled_z = z * 0.5;
const s1 = sin(scaled_x);
const s2 = sin(scaled_y);
const s3 = sin(scaled_z);
const c1 = cos(scaled_x);
const c2 = cos(scaled_y);
const c3 = cos(scaled_z);
const _x = s1 * c2 * c3 - c1 * s2 * s3;
const _y = c1 * s2 * c3 - s1 * c2 * s3;
const _z = c1 * c2 * s3 + s1 * s2 * c3;
const _w = c1 * c2 * c3 + s1 * s2 * s3;
return this.set(_x, _y, _z, _w);
}
/**
* NOTE: Vectors need to be normalized
*
* @param {Vector3} from Must be normalized
* @param {Vector3} to Must be normalized
* @returns {this}
*/
fromUnitVectors(from, to) {
// Based on this blog post: http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors
assert.ok(from.isNormalized(), `from vector is not normalized, length = ${from.length()}`);
assert.ok(to.isNormalized(), `to vector is not normalized, length = ${to.length()}`);
//quat quat::fromtwovectors(vec3 u, vec3 v)
// {
// float m = sqrt(2.f + 2.f * dot(u, v));
// vec3 w = (1.f / m) * cross(u, v);
// return quat(0.5f * m, w.x, w.y, w.z);
// }
const ax = from.x;
const ay = from.y;
const az = from.z;
const bx = to.x;
const by = to.y;
const bz = to.z;
const uv_dot = v3_dot(ax, ay, az, bx, by, bz);
if (uv_dot < -0.9999999) {
//to vector is opposite, produce a reversal quaternion
scratch_v3_a.crossVectors(Vector3.left, from);
if (scratch_v3_a.lengthSqr() < 0.00001) {
scratch_v3_a.crossVectors(Vector3.up, from);
}
scratch_v3_a.normalize();
return this.set(
scratch_v3_a.x,
scratch_v3_a.y,
scratch_v3_a.z,
0
);
}
const m = Math.sqrt(2 + 2 * uv_dot);
const inv_m = 1 / m;
//compute cross product
const crossX = ay * bz - az * by;
const crossY = az * bx - ax * bz;
const crossZ = ax * by - ay * bx;
//compute W vector
const wX = inv_m * crossX;
const wY = inv_m * crossY;
const wZ = inv_m * crossZ;
return this.set(
wX,
wY,
wZ,
0.5 * m
);
}
/**
* @param {number[]|Float32Array} m4x4
* @returns {this}
*/
setFromRotationMatrix(m4x4) {
assert.defined(m4x4, 'm4x4');
assert.isArrayLike(m4x4, 'm4x4');
return this.__setFromRotationMatrix(
m4x4[0], m4x4[4], m4x4[8],
m4x4[1], m4x4[5], m4x4[9],
m4x4[2], m4x4[6], m4x4[10]
);
}
/**
* This algorithm comes from "Quaternion Calculus and Fast Animation",
* Ken Shoemake, 1987 SIGGRAPH course notes
* @see https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Quaternion.h#L813
* @param {number} m11
* @param {number} m12
* @param {number} m13
* @param {number} m21
* @param {number} m22
* @param {number} m23
* @param {number} m31
* @param {number} m32
* @param {number} m33
* @returns {this}
*/
__setFromRotationMatrix(
m11, m12, m13,
m21, m22, m23,
m31, m32, m33
) {
assert.notNaN(m11, 'm11');
assert.notNaN(m12, 'm12');
assert.notNaN(m13, 'm13');
assert.notNaN(m21, 'm21');
assert.notNaN(m22, 'm22');
assert.notNaN(m23, 'm23');
assert.notNaN(m31, 'm31');
assert.notNaN(m32, 'm32');
assert.notNaN(m33, 'm33');
const trace = m11 + m22 + m33;
let x, y, z, w, s;
if (trace > 0) {
s = Math.sqrt(trace + 1.0);
w = 0.5 * s;
s = 0.5 / s;
x = (m32 - m23) * s;
y = (m13 - m31) * s;
z = (m21 - m12) * s;
} else if (m11 > m22 && m11 > m33) {
s = Math.sqrt(1.0 + m11 - m22 - m33);
x = 0.5 * s;
s = 0.5 / s;
w = (m32 - m23) * s;
y = (m12 + m21) * s;
z = (m13 + m31) * s;
} else if (m22 > m33) {
s = Math.sqrt(1.0 + m22 - m11 - m33);
y = 0.5 * s;
s = 0.5 / s;
w = (m13 - m31) * s;
x = (m12 + m21) * s;
z = (m23 + m32) * s;
} else {
s = Math.sqrt(1.0 + m33 - m11 - m22);
z = 0.5 * s;
s = 0.5 / s;
w = (m21 - m12) * s;
x = (m13 + m31) * s;
y = (m23 + m32) * s;
}
return this.set(x, y, z, w);
}
/**
* Linear interpolation
* @param {Quaternion} other
* @param {number} t fractional value between 0 and 1
* @returns {this}
*/
lerp(other, t) {
return this.lerpQuaternions(this, other, t);
}
/**
* Linear interpolation of two quaternions
* Pretty good, not as good as slerp, but close enough for most application
* @param {Quaternion} first
* @param {Quaternion} second
* @param {number} t
* @returns {this}
*/
lerpQuaternions(first, second, t) {
assert.isNumber(t, 't');
assert.greaterThanOrEqual(t, 0, 't >= 0');
assert.lessThanOrEqual(t, 1, 't <= 1');
const x = lerp(first.x, second.x, t);
const y = lerp(first.y, second.y, t);
const z = lerp(first.z, second.z, t);
const w = lerp(first.w, second.w, t);
return this.set(x, y, z, w);
}
/**
* Spherical linear interpolation
* @param {Quaternion} from
* @param {Quaternion} to
* @param {number} t coefficient, how much between the input quaternions? Must be a value between 0 and 1
* @returns {this}
*/
slerpQuaternions(from, to, t) {
assert.isNumber(t, 't');
assert.notNaN(t, 't');
// see https://github.com/glampert/vectormath/blob/7105ef341303fe83b3dacd6883d9333989126069/scalar/quaternion.hpp#L90
const ax = from.x,
ay = from.y,
az = from.z,
aw = from.w;
let bx = to.x,
by = to.y,
bz = to.z,
bw = to.w;
// calc cosine ( dot product )
let cos_angle = ax * bx + ay * by + az * bz + aw * bw;
// adjust signs (if necessary)
if (cos_angle < 0.0) {
cos_angle = -cos_angle;
bx = -bx;
by = -by;
bz = -bz;
bw = -bw;
}
let scale0, scale1;
// calculate coefficients
if ((1.0 - cos_angle) > EPSILON) {
// standard case (slerp)
const angle = Math.acos(cos_angle);
const recip_sin_angle = 1 / sin(angle);
scale0 = sin((1.0 - t) * angle) * recip_sin_angle;
scale1 = sin(t * angle) * recip_sin_angle;
} else {
// "from" and "to" quaternions are very close
// ... so we can do a linear interpolation
scale0 = 1.0 - t;
scale1 = t;
}
// calculate final values
const _x = scale0 * ax + scale1 * bx;
const _y = scale0 * ay + scale1 * by;
const _z = scale0 * az + scale1 * bz;
const _w = scale0 * aw + scale1 * bw;
return this.set(_x, _y, _z, _w);
}
/**
* @see https://github.com/toji/gl-matrix/blob/master/src/gl-matrix/quat.js
* @param {Quaternion} other
* @param {number} t
* @returns {this}
*/
slerp(other, t) {
return this.slerpQuaternions(this, other, t);
}
/**
*
* @param {function(x:number,y:number,z:number,w:number)} handler
* @param {*} [thisArg]
* @returns {this}
*/
process(handler, thisArg) {
assert.isFunction(handler, 'handler');
handler.call(thisArg, this.x, this.y, this.z, this.w);
this.onChanged.add(handler, thisArg);
return this;
}
/**
*
* @param {Quaternion} other
* @returns {this}
*/
copy(other) {
return this.set(other.x, other.y, other.z, other.w);
}
/**
*
* @returns {Quaternion}
*/
clone() {
const result = new Quaternion();
result.copy(this);
return result;
}
/**
* Set current value of the quaternion
* You *MUST* use this method in order for {@link Quaternion#onChanged} signal to be fired
* @param {number} x
* @param {number} y
* @param {number} z
* @param {number} w
* @returns {this}
*/
set(x, y, z, w) {
assert.isNumber(x, 'x');
assert.isNumber(y, 'y');
assert.isNumber(z, 'z');
assert.isNumber(w, 'w');
assert.notNaN(x, 'x');
assert.notNaN(y, 'y');
assert.notNaN(z, 'z');
assert.notNaN(w, 'w');
const _x = this.x;
const _y = this.y;
const _z = this.z;
const _w = this.w;
if (_x !== x || _y !== y || _z !== z || _w !== w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
if (this.onChanged.hasHandlers()) {
this.onChanged.send8(x, y, z, w, _x, _y, _z, _w);
}
}
return this;
}
toJSON() {
return {
x: this.x,
y: this.y,
z: this.z,
w: this.w
};
}
/**
*
* @param obj
* @return {this}
*/
fromJSON(obj) {
return this.set(obj.x, obj.y, obj.z, obj.w);
}
/**
*
* @param {BinaryBuffer} buffer
*/
toBinaryBuffer(buffer) {
buffer.writeFloat64(this.x);
buffer.writeFloat64(this.y);
buffer.writeFloat64(this.z);
buffer.writeFloat64(this.w);
}
/**
*
* @param {BinaryBuffer} buffer
*/
fromBinaryBuffer(buffer) {
const x = buffer.readFloat64();
const y = buffer.readFloat64();
const z = buffer.readFloat64();
const w = buffer.readFloat64();
this.set(x, y, z, w);
}
/**
*
* @param {BinaryBuffer} buffer
*/
toBinaryBufferFloat32(buffer) {
buffer.writeFloat32(this.x);
buffer.writeFloat32(this.y);
buffer.writeFloat32(this.z);
buffer.writeFloat32(this.w);
}
/**
*
* @param {BinaryBuffer} buffer
*/
fromBinaryBufferFloat32(buffer) {
const x = buffer.readFloat32();
const y = buffer.readFloat32();
const z = buffer.readFloat32();
const w = buffer.readFloat32();
this.set(x, y, z, w);
}
/**
*
* @param {number[]} array
* @param {number} [offset]
* @returns {this}
*/
fromArray(array, offset = 0) {
assert.defined(array, "array");
assert.isNonNegativeInteger(offset, "offset");
return this.set(
array[offset],
array[offset + 1],
array[offset + 2],
array[offset + 3]
);
}
/**
*
* @param {number[]} [array]
* @param {number} [offset]
* @returns {number[]}
*/
toArray(array = [], offset = 0) {
assert.defined(array, 'array');
assert.isNonNegativeInteger(offset, "offset");
array[offset] = this.x;
array[offset + 1] = this.y;
array[offset + 2] = this.z;
array[offset + 3] = this.w;
return array;
}
/**
* Strict equality check
* @param {Quaternion} other
* @returns {boolean}
* @see roughlyEquals
*/
equals(other) {
return this.x === other.x
&& this.y === other.y
&& this.z === other.z
&& this.w === other.w
;
}
/**
*
* @returns {number} integer hash
*/
hash() {
// fast and dumb hash, just spread bits a bit and XOR them
return computeHashFloat(this.x)
^ (computeHashFloat(this.y) >> 2)
^ (computeHashFloat(this.z) >> 1)
^ (computeHashFloat(this.w) << 2);
}
/**
* Check for approximate equality between two quaternions
* @param {Quaternion} other
* @param {number} [tolerance]
* @return {boolean}
*/
roughlyEquals(other, tolerance) {
return this._roughlyEquals(
other.x, other.y, other.z, other.w,
tolerance
);
}
/**
*
* @param {number} x
* @param {number} y
* @param {number} z
* @param {number} w
* @param {number} [tolerance] acceptable difference value per coordinate
* @return {boolean}
*/
_roughlyEquals(x, y, z, w, tolerance = EPSILON) {
assert.isNumber(tolerance, 'tolerance');
return epsilonEquals(this.x, x, tolerance)
&& epsilonEquals(this.y, y, tolerance)
&& epsilonEquals(this.z, z, tolerance)
&& epsilonEquals(this.w, w, tolerance);
}
/**
* @deprecated use {@link random} instead
* @param {function():number} random
*/
setRandom(random) {
throw new Error("use .random() instead");
}
/**
* Randomly orient current quaternion
* @param {function():number} [random=Math.random] Random number generator function.
*/
random(random = Math.random) {
assert.isFunction(random, 'random');
// Based on http://planning.cs.uiuc.edu/node198.html
const u1 = random();
assert.isNumber(u1, 'u1'); // validating random roll
const sqrt1u1 = Math.sqrt(1 - u1);
const sqrtu1 = Math.sqrt(u1);
const u2 = PI2 * random();
const u3 = PI2 * random();
return this.set(
sqrt1u1 * cos(u2),
sqrtu1 * sin(u3),
sqrtu1 * cos(u3),
sqrt1u1 * sin(u2),
);
}
toString() {
return `{ x: ${this.x}, y: ${this.y}, z: ${this.z}, w: ${this.w} }`;
}
/**
* Create a randomly oriented quaternion
* @param {function} [random] random number generator function
* @returns {Quaternion}
*/
static random(random = Math.random) {
const result = new Quaternion();
result.random(random);
return result;
}
/**
* Convenience constructor
* Used XYZ angle order (see {@link fromEulerAnglesXYZ})
* @param {number} x in radians
* @param {number} y in radians
* @param {number} z in radians
* @returns {Quaternion}
*/
static fromEulerAngles(x, y, z) {
const r = new Quaternion();
r.fromEulerAnglesXYZ(x, y, z);
return r;
}
/**
* Behaves similarly to Unity's Quaternion `RotateToward` method
* @param {Quaternion} result
* @param {Quaternion} from
* @param {Quaternion} to
* @param {number} max_delta in radians
*/
static rotateTowards(
result,
from,
to,
max_delta
) {
assert.isNumber(max_delta, 'max_delta');
assert.notNaN(max_delta, 'max_delta');
const angle = from.angleTo(to);
if (angle === 0) {
// We're already where we need to be.
// Also - avoid division by 0.
result.copy(to);
} else {
// clamp to 1, to make sure we don't overshoot
const t = clamp01(max_delta / angle);
result.slerpQuaternions(from, to, t);
}
}
}
/**
* @deprecated use `fromArray`
*/
Quaternion.prototype.readFromArray = Quaternion.prototype.fromArray;
/**
* @deprecated use `toArray`
*/
Quaternion.prototype.writeToArray = Quaternion.prototype.toArray;
/**
* @deprecated use `toArray`
*/
Quaternion.prototype.asArray = Quaternion.prototype.toArray;
Quaternion.prototype.fromEulerAngles = Quaternion.prototype.fromEulerAnglesXYZ;
/**
* @readonly
* @type {Quaternion}
*/
Quaternion.identity = Object.freeze(new Quaternion(0, 0, 0, 1));
/**
* Shortcut for type checking
* @readonly
* @type {boolean}
*/
Quaternion.prototype.isQuaternion = true;
/**
* @readonly
* @type {string}
*/
Quaternion.typeName = 'Quaternion';
export default Quaternion;