256 lines
6.5 KiB
Haxe
256 lines
6.5 KiB
Haxe
|
package kha.math;
|
||
|
|
|
||
|
|
import kha.math.Vector3;
|
||
|
|
import kha.math.Matrix4;
|
||
|
|
|
||
|
|
@:structInit
|
||
|
|
class Quaternion {
|
||
|
|
var values: Array<Float>;
|
||
|
|
|
||
|
|
public inline function new(x: Float = 0, y: Float = 0, z: Float = 0, w: Float = 1): Void {
|
||
|
|
values = new Array<Float>();
|
||
|
|
values.push(x);
|
||
|
|
values.push(y);
|
||
|
|
values.push(z);
|
||
|
|
values.push(w);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Axis has to be normalized
|
||
|
|
public inline static function fromAxisAngle(axis: Vector3, radians: Float): Quaternion {
|
||
|
|
var q: Quaternion = new Quaternion();
|
||
|
|
q.w = Math.cos(radians / 2.0);
|
||
|
|
q.x = q.y = q.z = Math.sin(radians / 2.0);
|
||
|
|
q.x *= axis.x;
|
||
|
|
q.y *= axis.y;
|
||
|
|
q.z *= axis.z;
|
||
|
|
return q;
|
||
|
|
}
|
||
|
|
|
||
|
|
public function slerp(t: Float, q: Quaternion) {
|
||
|
|
var epsilon: Float = 0.0005;
|
||
|
|
|
||
|
|
var dot = dot(q);
|
||
|
|
|
||
|
|
if (dot > 1 - epsilon) {
|
||
|
|
var result: Quaternion = q.add((this.sub(q)).scaled(t));
|
||
|
|
result.normalize();
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
if (dot < 0)
|
||
|
|
dot = 0;
|
||
|
|
if (dot > 1)
|
||
|
|
dot = 1;
|
||
|
|
|
||
|
|
var theta0: Float = Math.acos(dot);
|
||
|
|
var theta: Float = theta0 * t;
|
||
|
|
|
||
|
|
var q2: Quaternion = q.sub(scaled(dot));
|
||
|
|
q2.normalize();
|
||
|
|
|
||
|
|
var result: Quaternion = scaled(Math.cos(theta)).add(q2.scaled(Math.sin(theta)));
|
||
|
|
|
||
|
|
result.normalize();
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
// TODO: This should be multiplication
|
||
|
|
public inline function rotated(b: Quaternion): Quaternion {
|
||
|
|
var q: Quaternion = new Quaternion();
|
||
|
|
q.w = w * b.w - x * b.x - y * b.y - z * b.z;
|
||
|
|
q.x = w * b.x + x * b.w + y * b.z - z * b.y;
|
||
|
|
q.y = w * b.y + y * b.w + z * b.x - x * b.z;
|
||
|
|
q.z = w * b.z + z * b.w + x * b.y - y * b.x;
|
||
|
|
q.normalize();
|
||
|
|
return q;
|
||
|
|
}
|
||
|
|
|
||
|
|
public inline function scaled(scale: Float): Quaternion {
|
||
|
|
return new Quaternion(x * scale, y * scale, z * scale, w * scale);
|
||
|
|
}
|
||
|
|
|
||
|
|
public inline function scale(scale: Float) {
|
||
|
|
x = x * scale;
|
||
|
|
y = y * scale;
|
||
|
|
z = z * scale;
|
||
|
|
w = w * scale;
|
||
|
|
}
|
||
|
|
|
||
|
|
public inline function matrix(): Matrix4 {
|
||
|
|
var s: Float = 2.0;
|
||
|
|
|
||
|
|
var xs: Float = x * s;
|
||
|
|
var ys: Float = y * s;
|
||
|
|
var zs: Float = z * s;
|
||
|
|
var wx: Float = w * xs;
|
||
|
|
var wy: Float = w * ys;
|
||
|
|
var wz: Float = w * zs;
|
||
|
|
var xx: Float = x * xs;
|
||
|
|
var xy: Float = x * ys;
|
||
|
|
var xz: Float = x * zs;
|
||
|
|
var yy: Float = y * ys;
|
||
|
|
var yz: Float = y * zs;
|
||
|
|
var zz: Float = z * zs;
|
||
|
|
|
||
|
|
return new Matrix4(1 - (yy + zz), xy - wz, xz + wy, 0, xy + wz, 1 - (xx + zz), yz - wx, 0, xz - wy, yz + wx, 1 - (xx + yy), 0, 0, 0, 0, 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
public inline function get(index: Int): Float {
|
||
|
|
return values[index];
|
||
|
|
}
|
||
|
|
|
||
|
|
public inline function set(index: Int, value: Float): Void {
|
||
|
|
values[index] = value;
|
||
|
|
}
|
||
|
|
|
||
|
|
public var x(get, set): Float;
|
||
|
|
public var y(get, set): Float;
|
||
|
|
public var z(get, set): Float;
|
||
|
|
public var w(get, set): Float;
|
||
|
|
public var length(get, set): Float;
|
||
|
|
|
||
|
|
function get_x(): Float {
|
||
|
|
return values[0];
|
||
|
|
}
|
||
|
|
|
||
|
|
function set_x(value: Float): Float {
|
||
|
|
return values[0] = value;
|
||
|
|
}
|
||
|
|
|
||
|
|
function get_y(): Float {
|
||
|
|
return values[1];
|
||
|
|
}
|
||
|
|
|
||
|
|
function set_y(value: Float): Float {
|
||
|
|
return values[1] = value;
|
||
|
|
}
|
||
|
|
|
||
|
|
function get_z(): Float {
|
||
|
|
return values[2];
|
||
|
|
}
|
||
|
|
|
||
|
|
function set_z(value: Float): Float {
|
||
|
|
return values[2] = value;
|
||
|
|
}
|
||
|
|
|
||
|
|
function get_w(): Float {
|
||
|
|
return values[3];
|
||
|
|
}
|
||
|
|
|
||
|
|
function set_w(value: Float): Float {
|
||
|
|
return values[3] = value;
|
||
|
|
}
|
||
|
|
|
||
|
|
// TODO: Isn't this code wrong? Is wrong in Vector4 for sure! (Missing w in the length)
|
||
|
|
function get_length(): Float {
|
||
|
|
return Math.sqrt(x * x + y * y + z * z + w * w);
|
||
|
|
}
|
||
|
|
|
||
|
|
function set_length(length: Float): Float {
|
||
|
|
if (get_length() == 0)
|
||
|
|
return 0;
|
||
|
|
var mul = length / get_length();
|
||
|
|
x *= mul;
|
||
|
|
y *= mul;
|
||
|
|
z *= mul;
|
||
|
|
return length;
|
||
|
|
}
|
||
|
|
|
||
|
|
// For adding a (scaled) axis-angle representation of a quaternion
|
||
|
|
public inline function addVector(vec: Vector3): Quaternion {
|
||
|
|
var result: Quaternion = new Quaternion(x, y, z, w);
|
||
|
|
var q1: Quaternion = new Quaternion(0, vec.x, vec.y, vec.z);
|
||
|
|
|
||
|
|
q1 = q1.mult(result);
|
||
|
|
|
||
|
|
result.x += q1.x * 0.5;
|
||
|
|
result.y += q1.y * 0.5;
|
||
|
|
result.z += q1.z * 0.5;
|
||
|
|
result.w += q1.w * 0.5;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
public inline function add(q: Quaternion): Quaternion {
|
||
|
|
return new Quaternion(x + q.x, y + q.y, z + q.z, w + q.w);
|
||
|
|
}
|
||
|
|
|
||
|
|
public inline function sub(q: Quaternion): Quaternion {
|
||
|
|
return new Quaternion(x - q.x, y - q.y, z - q.z, w - q.w);
|
||
|
|
}
|
||
|
|
|
||
|
|
// TODO: Check again, but I think the code in Kore is wrong
|
||
|
|
public inline function mult(r: Quaternion): Quaternion {
|
||
|
|
var q: Quaternion = new Quaternion();
|
||
|
|
q.x = w * r.x + x * r.w + y * r.z - z * r.y;
|
||
|
|
q.y = w * r.y - x * r.z + y * r.w + z * r.x;
|
||
|
|
q.z = w * r.z + x * r.y - y * r.x + z * r.w;
|
||
|
|
q.w = w * r.w - x * r.x - y * r.y - z * r.z;
|
||
|
|
return q;
|
||
|
|
}
|
||
|
|
|
||
|
|
public inline function normalize() {
|
||
|
|
scale(1.0 / length);
|
||
|
|
}
|
||
|
|
|
||
|
|
public inline function dot(q: Quaternion) {
|
||
|
|
return x * q.x + y * q.y + z * q.z + w * q.w;
|
||
|
|
}
|
||
|
|
|
||
|
|
// GetEulerAngles extracts Euler angles from the quaternion, in the specified order of
|
||
|
|
// axis rotations and the specified coordinate system. Right-handed coordinate system
|
||
|
|
// is the default, with CCW rotations while looking in the negative axis direction.
|
||
|
|
// Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
|
||
|
|
// rotation a around axis A1
|
||
|
|
// is followed by rotation b around axis A2
|
||
|
|
// is followed by rotation c around axis A3
|
||
|
|
// rotations are CCW or CW (D) in LH or RH coordinate system (S)
|
||
|
|
public static inline var AXIS_X: Int = 0;
|
||
|
|
public static inline var AXIS_Y: Int = 1;
|
||
|
|
public static inline var AXIS_Z: Int = 2;
|
||
|
|
|
||
|
|
public function getEulerAngles(A1: Int, A2: Int, A3: Int, S: Int = 1, D: Int = 1): Vector3 {
|
||
|
|
var result: Vector3 = new Vector3();
|
||
|
|
|
||
|
|
var Q: Array<Float> = new Array<Float>();
|
||
|
|
Q[0] = x;
|
||
|
|
Q[1] = y;
|
||
|
|
Q[2] = z;
|
||
|
|
|
||
|
|
var ww: Float = w * w;
|
||
|
|
|
||
|
|
var Q11: Float = Q[A1] * Q[A1];
|
||
|
|
var Q22: Float = Q[A2] * Q[A2];
|
||
|
|
var Q33: Float = Q[A3] * Q[A3];
|
||
|
|
|
||
|
|
var psign: Float = -1;
|
||
|
|
|
||
|
|
var SingularityRadius: Float = 0.0000001;
|
||
|
|
var PiOver2: Float = Math.PI / 2.0;
|
||
|
|
|
||
|
|
// Determine whether even permutation
|
||
|
|
if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) {
|
||
|
|
psign = 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
var s2: Float = psign * 2.0 * (psign * w * Q[A2] + Q[A1] * Q[A3]);
|
||
|
|
|
||
|
|
if (s2 < -1 + SingularityRadius) { // South pole singularity
|
||
|
|
result.x = 0;
|
||
|
|
result.y = -S * D * PiOver2;
|
||
|
|
result.z = S * D * Math.atan2(2 * (psign * Q[A1] * Q[A2] + w * Q[A3]), ww + Q22 - Q11 - Q33);
|
||
|
|
}
|
||
|
|
else if (s2 > 1 - SingularityRadius) { // North pole singularity
|
||
|
|
result.x = 0;
|
||
|
|
result.y = S * D * PiOver2;
|
||
|
|
result.z = S * D * Math.atan2(2 * (psign * Q[A1] * Q[A2] + w * Q[A3]), ww + Q22 - Q11 - Q33);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
result.x = -S * D * Math.atan2(-2 * (w * Q[A1] - psign * Q[A2] * Q[A3]), ww + Q33 - Q11 - Q22);
|
||
|
|
result.y = S * D * Math.asin(s2);
|
||
|
|
result.z = S * D * Math.atan2(2 * (w * Q[A3] - psign * Q[A1] * Q[A2]), ww + Q11 - Q22 - Q33);
|
||
|
|
}
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
}
|