435 lines
12 KiB
TypeScript
435 lines
12 KiB
TypeScript
import type { Vector3 } from "./Vector3";
|
|
import type { XMLNode } from 'xmlbuilder';
|
|
|
|
export class Quaternion
|
|
{
|
|
public x = 0;
|
|
public y = 0;
|
|
public z = 0;
|
|
public w = 1.0;
|
|
|
|
public constructor(buf?: Buffer | number[] | Quaternion | number, pos?: number, z?: number, w?: number)
|
|
{
|
|
if (typeof buf === 'number' && typeof pos === 'number' && typeof z === 'number' && typeof w === 'number')
|
|
{
|
|
this.x = buf;
|
|
this.y = pos;
|
|
this.z = z;
|
|
this.w = w;
|
|
}
|
|
else if (buf instanceof Quaternion)
|
|
{
|
|
this.x = buf.x;
|
|
this.y = buf.y;
|
|
this.z = buf.z;
|
|
this.w = buf.w;
|
|
}
|
|
else
|
|
{
|
|
if (buf !== undefined && pos !== undefined && buf instanceof Buffer)
|
|
{
|
|
this.x = buf.readFloatLE(pos);
|
|
this.y = buf.readFloatLE(pos + 4);
|
|
this.z = buf.readFloatLE(pos + 8);
|
|
const xyzsum = 1.0 - this.x * this.x - this.y * this.y - this.z * this.z;
|
|
this.w = xyzsum > 0.0 ? Math.sqrt(xyzsum) : 0;
|
|
}
|
|
else if (buf !== undefined && Array.isArray(buf) && buf.length > 3)
|
|
{
|
|
[this.x, this.y, this.z, this.w] = buf;
|
|
}
|
|
}
|
|
}
|
|
|
|
public static getXML(doc: XMLNode, v?: Quaternion): void
|
|
{
|
|
if (v === undefined)
|
|
{
|
|
v = Quaternion.getIdentity();
|
|
}
|
|
doc.ele('X', v.x);
|
|
doc.ele('Y', v.y);
|
|
doc.ele('Z', v.z);
|
|
doc.ele('W', v.w);
|
|
}
|
|
|
|
public static fromXMLJS(obj: any, param: string): Quaternion | false
|
|
{
|
|
if (!obj[param])
|
|
{
|
|
return false;
|
|
}
|
|
let value = obj[param];
|
|
if (Array.isArray(value) && value.length > 0)
|
|
{
|
|
value = value[0];
|
|
}
|
|
if (typeof value === 'object')
|
|
{
|
|
if (value.X !== undefined && value.Y !== undefined && value.Z !== undefined && value.W !== undefined)
|
|
{
|
|
let x = value.X;
|
|
let y = value.Y;
|
|
let z = value.Z;
|
|
let w = value.W;
|
|
if (Array.isArray(x) && x.length > 0)
|
|
{
|
|
x = x[0];
|
|
}
|
|
if (Array.isArray(y) && y.length > 0)
|
|
{
|
|
y = y[0];
|
|
}
|
|
if (Array.isArray(z) && z.length > 0)
|
|
{
|
|
z = z[0];
|
|
}
|
|
if (Array.isArray(w) && w.length > 0)
|
|
{
|
|
w = w[0];
|
|
}
|
|
return new Quaternion([Number(x), Number(y), Number(z), Number(w)]);
|
|
}
|
|
return false;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
public static fromAxis(axis: Vector3, angle: number): Quaternion
|
|
{
|
|
const halfAngle = angle / 2;
|
|
const sinHalfAngle = Math.sin(halfAngle);
|
|
const cosHalfAngle = Math.cos(halfAngle);
|
|
|
|
const normalizedAxis = axis.copy().normalize();
|
|
|
|
const axisLengthSquared = axis.x * axis.x + axis.y * axis.y + axis.z * axis.z;
|
|
|
|
if (axisLengthSquared === 0)
|
|
{
|
|
return Quaternion.getIdentity();
|
|
}
|
|
|
|
return new Quaternion([
|
|
normalizedAxis.x * sinHalfAngle,
|
|
normalizedAxis.y * sinHalfAngle,
|
|
normalizedAxis.z * sinHalfAngle,
|
|
cosHalfAngle,
|
|
]);
|
|
}
|
|
|
|
public static getIdentity(): Quaternion
|
|
{
|
|
return new Quaternion();
|
|
}
|
|
|
|
public writeToBuffer(buf: Buffer, pos: number): void
|
|
{
|
|
const q = this.normalize();
|
|
buf.writeFloatLE(q.x, pos);
|
|
buf.writeFloatLE(q.y, pos + 4);
|
|
buf.writeFloatLE(q.z, pos + 8);
|
|
}
|
|
|
|
public toString(): string
|
|
{
|
|
return '<' + this.x + ', ' + this.y + ', ' + this.z + ', ' + this.w + '>';
|
|
}
|
|
|
|
public getBuffer(): Buffer
|
|
{
|
|
const j = Buffer.allocUnsafe(12);
|
|
this.writeToBuffer(j, 0);
|
|
return j;
|
|
}
|
|
|
|
public angleBetween(b: Quaternion): number
|
|
{
|
|
const aa = this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;
|
|
const bb = b.x * b.x + b.y * b.y + b.z * b.z + b.w * b.w;
|
|
const aa_bb = aa * bb;
|
|
if (aa_bb === 0)
|
|
{
|
|
return 0.0;
|
|
}
|
|
const ab = this.x * b.x + this.y * b.y + this.z * b.z + this.w * b.w;
|
|
const quotient = (ab * ab) / aa_bb;
|
|
if (quotient >= 1.0)
|
|
{
|
|
return 0.0;
|
|
}
|
|
return Math.acos(2 * quotient - 1);
|
|
}
|
|
|
|
public dot(q: Quaternion): number
|
|
{
|
|
return this.x * q.x + this.y * q.y + this.z * q.z + this.w * q.w;
|
|
}
|
|
|
|
public sum(q: Quaternion): Quaternion
|
|
{
|
|
return new Quaternion([
|
|
this.x + q.x,
|
|
this.y + q.y,
|
|
this.z + q.z,
|
|
this.w + q.w,
|
|
]);
|
|
}
|
|
|
|
public product(q: Quaternion): Quaternion
|
|
{
|
|
return new Quaternion([
|
|
this.x * q.x,
|
|
this.y * q.y,
|
|
this.z * q.z,
|
|
this.w * q.w,
|
|
]);
|
|
}
|
|
|
|
public cross(q: Quaternion): Quaternion
|
|
{
|
|
// Cross product of vector parts; scalar part is zero
|
|
return new Quaternion([
|
|
this.y * q.z - this.z * q.y,
|
|
this.z * q.x - this.x * q.z,
|
|
this.x * q.y - this.y * q.x,
|
|
0,
|
|
]);
|
|
}
|
|
|
|
public shortMix(q: Quaternion, t: number): Quaternion
|
|
{
|
|
if (t <= 0.0)
|
|
{
|
|
return new Quaternion(this);
|
|
}
|
|
else if (t >= 1.0)
|
|
{
|
|
return new Quaternion(q);
|
|
}
|
|
|
|
let cosHalfTheta = this.dot(q);
|
|
|
|
if (cosHalfTheta < 0)
|
|
{
|
|
q = q.negate();
|
|
cosHalfTheta = -cosHalfTheta;
|
|
}
|
|
|
|
let k0 = 0;
|
|
let k1 = 0;
|
|
|
|
if (cosHalfTheta > 0.9999)
|
|
{
|
|
// Quaternions are very close; use linear interpolation
|
|
k0 = 1 - t;
|
|
k1 = t;
|
|
}
|
|
else
|
|
{
|
|
const sinHalfTheta = Math.sqrt(1.0 - cosHalfTheta * cosHalfTheta);
|
|
const halfTheta = Math.atan2(sinHalfTheta, cosHalfTheta);
|
|
const oneOverSinHalfTheta = 1.0 / sinHalfTheta;
|
|
|
|
k0 = Math.sin((1 - t) * halfTheta) * oneOverSinHalfTheta;
|
|
k1 = Math.sin(t * halfTheta) * oneOverSinHalfTheta;
|
|
}
|
|
|
|
return new Quaternion([
|
|
this.x * k0 + q.x * k1,
|
|
this.y * k0 + q.y * k1,
|
|
this.z * k0 + q.z * k1,
|
|
this.w * k0 + q.w * k1,
|
|
]).normalize();
|
|
}
|
|
|
|
public mix(q: Quaternion, t: number): Quaternion
|
|
{
|
|
const cosHalfTheta = this.dot(q);
|
|
|
|
if (Math.abs(cosHalfTheta) >= 1.0)
|
|
{
|
|
// Quaternions are very close; return this quaternion
|
|
return new Quaternion(this);
|
|
}
|
|
|
|
const halfTheta = Math.acos(cosHalfTheta);
|
|
const sinHalfTheta = Math.sqrt(1.0 - cosHalfTheta * cosHalfTheta);
|
|
|
|
if (Math.abs(sinHalfTheta) < 0.001)
|
|
{
|
|
// Quaternions are too close; use linear interpolation
|
|
return new Quaternion([
|
|
this.x * (1 - t) + q.x * t,
|
|
this.y * (1 - t) + q.y * t,
|
|
this.z * (1 - t) + q.z * t,
|
|
this.w * (1 - t) + q.w * t,
|
|
]).normalize();
|
|
}
|
|
|
|
const ratioA = Math.sin((1 - t) * halfTheta) / sinHalfTheta;
|
|
const ratioB = Math.sin(t * halfTheta) / sinHalfTheta;
|
|
|
|
return new Quaternion([
|
|
this.x * ratioA + q.x * ratioB,
|
|
this.y * ratioA + q.y * ratioB,
|
|
this.z * ratioA + q.z * ratioB,
|
|
this.w * ratioA + q.w * ratioB,
|
|
]);
|
|
}
|
|
|
|
public copy(): Quaternion
|
|
{
|
|
return new Quaternion(this);
|
|
}
|
|
|
|
public roll(): number
|
|
{
|
|
// Rotation around the x-axis
|
|
return Math.atan2(
|
|
2 * (this.w * this.x + this.y * this.z),
|
|
1 - 2 * (this.x * this.x + this.y * this.y)
|
|
);
|
|
}
|
|
|
|
public pitch(): number
|
|
{
|
|
// Rotation around the y-axis
|
|
const sinp = 2 * (this.w * this.y - this.z * this.x);
|
|
if (Math.abs(sinp) >= 1)
|
|
{
|
|
return (Math.PI / 2) * Math.sign(sinp); // Use 90 degrees if out of range
|
|
}
|
|
else
|
|
{
|
|
return Math.asin(sinp);
|
|
}
|
|
}
|
|
|
|
public yaw(): number
|
|
{
|
|
// Rotation around the z-axis
|
|
return Math.atan2(
|
|
2 * (this.w * this.z + this.x * this.y),
|
|
1 - 2 * (this.y * this.y + this.z * this.z)
|
|
);
|
|
}
|
|
|
|
public equals(q: Quaternion, epsilon = Number.EPSILON): boolean
|
|
{
|
|
return (
|
|
Math.abs(this.x - q.x) < epsilon &&
|
|
Math.abs(this.y - q.y) < epsilon &&
|
|
Math.abs(this.z - q.z) < epsilon &&
|
|
Math.abs(this.w - q.w) < epsilon
|
|
);
|
|
}
|
|
|
|
public inverse(): Quaternion
|
|
{
|
|
const lengthSq = this.lengthSquared();
|
|
if (lengthSq === 0)
|
|
{
|
|
throw new Error('Cannot invert a quaternion with zero length');
|
|
}
|
|
return this.conjugate().scale(1 / lengthSq);
|
|
}
|
|
|
|
public conjugate(): Quaternion
|
|
{
|
|
return new Quaternion([-this.x, -this.y, -this.z, this.w]);
|
|
}
|
|
|
|
public length(): number
|
|
{
|
|
return Math.sqrt(this.lengthSquared());
|
|
}
|
|
|
|
public lengthSquared(): number
|
|
{
|
|
return this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;
|
|
}
|
|
|
|
public normalize(): Quaternion
|
|
{
|
|
const len = this.length();
|
|
if (len === 0)
|
|
{
|
|
return Quaternion.getIdentity();
|
|
}
|
|
else
|
|
{
|
|
return new Quaternion([
|
|
this.x / len,
|
|
this.y / len,
|
|
this.z / len,
|
|
this.w / len,
|
|
]);
|
|
}
|
|
}
|
|
|
|
public add(q: Quaternion): Quaternion
|
|
{
|
|
return new Quaternion([
|
|
this.x + q.x,
|
|
this.y + q.y,
|
|
this.z + q.z,
|
|
this.w + q.w,
|
|
]);
|
|
}
|
|
|
|
public multiply(value: number | Quaternion): Quaternion
|
|
{
|
|
if (typeof value === 'number')
|
|
{
|
|
// Scalar multiplication
|
|
return new Quaternion([
|
|
this.x * value,
|
|
this.y * value,
|
|
this.z * value,
|
|
this.w * value,
|
|
]);
|
|
}
|
|
else
|
|
{
|
|
// Quaternion multiplication
|
|
const x1 = this.x,
|
|
y1 = this.y,
|
|
z1 = this.z,
|
|
w1 = this.w;
|
|
const x2 = value.x,
|
|
y2 = value.y,
|
|
z2 = value.z,
|
|
w2 = value.w;
|
|
return new Quaternion([
|
|
w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
|
|
w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
|
|
w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2,
|
|
w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
|
|
]);
|
|
}
|
|
}
|
|
|
|
public negate(): Quaternion
|
|
{
|
|
return new Quaternion([-this.x, -this.y, -this.z, -this.w]);
|
|
}
|
|
|
|
public scale(scalar: number): Quaternion
|
|
{
|
|
return new Quaternion([
|
|
this.x * scalar,
|
|
this.y * scalar,
|
|
this.z * scalar,
|
|
this.w * scalar,
|
|
]);
|
|
}
|
|
|
|
public calculateW(): Quaternion
|
|
{
|
|
const t = 1.0 - (this.x * this.x + this.y * this.y + this.z * this.z);
|
|
const w = t < 0 ? 0 : Math.sqrt(t);
|
|
return new Quaternion([this.x, this.y, this.z, w]);
|
|
}
|
|
}
|