LNXSDK/leenkx/Sources/iron/math/Quat.hx

package iron.math;

import kha.FastFloat;

class Quat {

	public var x: FastFloat;
	public var y: FastFloat;
	public var z: FastFloat;
	public var w: FastFloat;

	static var helpVec0 = new Vec4();
	static var helpVec1 = new Vec4();
	static var helpVec2 = new Vec4();
	static var helpMat = Mat4.identity();
	static var xAxis = Vec4.xAxis();
	static var yAxis = Vec4.yAxis();
	
	static inline var SQRT2: FastFloat = 1.4142135623730951;

	public inline function new(x: FastFloat = 0.0, y: FastFloat = 0.0, z: FastFloat = 0.0, w: FastFloat = 1.0) {
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
	}

	public inline function set(x: FastFloat, y: FastFloat, z: FastFloat, w: FastFloat): Quat {
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
		return this;
	}

	public inline function add(q: Quat): Quat {
		this.x += q.x;
		this.y += q.y;
		this.z += q.z;
		this.w += q.w;
		return this;
	}

	public inline function addquat(a: Quat, b: Quat): Quat {
		this.x = a.x + b.x;
		this.y = a.y + b.y;
		this.z = a.z + b.z;
		this.w = a.w + b.w;
		return this;
	}

	public inline function sub(q: Quat): Quat {
		this.x -= q.x;
		this.y -= q.y;
		this.z -= q.z;
		this.w -= q.w;
		return this;
	}

	public inline function subquat(a: Quat, b: Quat): Quat {
		this.x = a.x - b.x;
		this.y = a.y - b.y;
		this.z = a.z - b.z;
		this.w = a.w - b.w;
		return this;
	}

	public inline function fromAxisAngle(axis: Vec4, angle: FastFloat): Quat {
		//var s: FastFloat = Math.sin(angle * 0.5);
		//x = axis.x * s;
		//y = axis.y * s;
		//z = axis.z * s;
		//w = Math.cos(angle * 0.5);
		//return normalize();
		// Normalize the axis vector first
		var axisLen = Math.sqrt(axis.x * axis.x + axis.y * axis.y + axis.z * axis.z);
		if (axisLen > 0.00001) {
			var aL = 1.0 / axisLen;
			var nX = axis.x * aL;
			var nY = axis.y * aL;
			var nZ = axis.z * aL;
			var halfAngle = angle * 0.5;
			var s: FastFloat = Math.sin(halfAngle);
			x = nX * s;
			y = nY * s;
			z = nZ * s;
			w = Math.cos(halfAngle);
		} else {
			x = 0.0;
			y = 0.0;
			z = 0.0;
			w = 1.0;
		}
		return this;
	}

	public inline function toAxisAngle(axis: Vec4): FastFloat {
		normalize();
		var angle = 2 * Math.acos(w);
		var s = Math.sqrt(1 - w * w);
		if (s < 0.001) {
			axis.x = this.x;
			axis.y = this.y;
			axis.z = this.z;
		}
		else {
			axis.x = this.x / s;
			axis.y = this.y / s;
			axis.z = this.z / s;
		}
		return angle;
	}

	public inline function fromMat(m: Mat4): Quat {
		helpMat.setFrom(m);
		helpMat.toRotation();
		return fromRotationMat(helpMat);
	}

	public inline function fromRotationMat(m: Mat4): Quat {
		// Assumes the upper 3x3 is a pure rotation matrix
		var m11 = m._00; var m12 = m._10; var m13 = m._20;
		var m21 = m._01; var m22 = m._11; var m23 = m._21;
		var m31 = m._02; var m32 = m._12; var m33 = m._22;
		var tr = m11 + m22 + m33;
		var s = 0.0;

		if (tr > 0) {
			s = 0.5 / Math.sqrt(tr + 1.0);
			this.w = 0.25 / s;
			this.x = (m32 - m23) * s;
			this.y = (m13 - m31) * s;
			this.z = (m21 - m12) * s;
		}
		else if (m11 > m22 && m11 > m33) {
			s = 2.0 * Math.sqrt(1.0 + m11 - m22 - m33);
			this.w = (m32 - m23) / s;
			this.x = 0.25 * s;
			this.y = (m12 + m21) / s;
			this.z = (m13 + m31) / s;
		}
		else if (m22 > m33) {
			s = 2.0 * Math.sqrt(1.0 + m22 - m11 - m33);
			this.w = (m13 - m31) / s;
			this.x = (m12 + m21) / s;
			this.y = 0.25 * s;
			this.z = (m23 + m32) / s;
		}
		else {
			s = 2.0 * Math.sqrt(1.0 + m33 - m11 - m22);
			this.w = (m21 - m12) / s;
			this.x = (m13 + m31) / s;
			this.y = (m23 + m32) / s;
			this.z = 0.25 * s;
		}
		return this;
	}

	// Multiply this quaternion by float
	public inline function scale(scale: FastFloat): Quat {
		this.x *= scale;
		this.y *= scale;
		this.z *= scale;
		this.w *= scale;
		return this;
	}

	public inline function scalequat(q: Quat, scale: FastFloat): Quat {
		q.x *= scale;
		q.y *= scale;
		q.z *= scale;
		q.w *= scale;
		return q;
	}

	/**
		Multiply this quaternion by another.
		@param	q The quaternion to multiply this one with.
		@return	This quaternion.
	**/
	public inline function mult(q: Quat): Quat {
		return multquats(this, q);
	}

	/**
		Multiply two other quaternions and store the result in this one.
		@param	q1 The first operand.
		@param	q2 The second operand.
		@return	This quaternion.
	**/
	public inline function multquats(q1: Quat, q2: Quat): Quat {
		var q1x = q1.x; var q1y = q1.y; var q1z = q1.z; var q1w = q1.w;
		var q2x = q2.x; var q2y = q2.y; var q2z = q2.z; var q2w = q2.w;
		x = q1x * q2w + q1w * q2x + q1y * q2z - q1z * q2y;
		y = q1w * q2y - q1x * q2z + q1y * q2w + q1z * q2x;
		z = q1w * q2z + q1x * q2y - q1y * q2x + q1z * q2w;
		w = q1w * q2w - q1x * q2x - q1y * q2y - q1z * q2z;
		return this;
	}

	public inline function module(): FastFloat {
		return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
	}

	/**
		Scale this quaternion to have a magnitude of 1.
		@return	This quaternion.
	**/
	public inline function normalize(): Quat {
		var l = Math.sqrt(x * x + y * y + z * z + w * w);
		if (l == 0.0) {
			x = 0;
			y = 0;
			z = 0;
			w = 0;
		}
		else {
			l = 1.0 / l;
			x *= l;
			y *= l;
			z *= l;
			w *= l;
		}
		return this;
	}

	/**
		Invert the given quaternion and store the result in this one.
		@param	q Quaternion to invert.
		@return	This quaternion.
	**/
	public inline function inverse(q: Quat): Quat {
		var sqsum = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
		sqsum = -1 / sqsum;
		x = q.x * sqsum;
		y = q.y * sqsum;
		z = q.z * sqsum;
		w = -q.w * sqsum;
		return this;
	}

	/**
		Copy the rotation of another quaternion to this one.
		@param	q A quaternion to copy.
		@return	This quaternion.
	**/
	public inline function setFrom(q: Quat): Quat {
		x = q.x;
		y = q.y;
		z = q.z;
		w = q.w;
		return this;
	}

	/**
		Convert this quaternion to a YZX Euler (note: XZY in blender order terms).
		@return	A new YZX Euler that represents the same rotation as this
				quaternion.
	**/
	public inline function getEuler(): Vec4 {
		var a = -2 * (x * z - w * y);
		var b =  w *  w + x * x - y * y - z * z;
		var c =  2 * (x * y + w * z);
		var d = -2 * (y * z - w * x);
		var e =  w *  w - x * x + y * y - z * z;
		return new Vec4(Math.atan2(d, e), Math.atan2(a, b), Math.asin(c));
	}

	/**
		Set this quaternion to the rotation represented by a YZX Euler (XZY in blender terms).
		@param	x The Euler's x component.
		@param	y The Euler's y component.
		@param	z The Euler's z component.
		@return	This quaternion.
	**/
	public inline function fromEuler(x: FastFloat, y: FastFloat, z: FastFloat): Quat {
		var f = x / 2;
		var c1 = Math.cos(f);
		var s1 = Math.sin(f);
		f = y / 2;
		var c2 = Math.cos(f);
		var s2 = Math.sin(f);
		f = z / 2;
		var c3 = Math.cos(f);
		var s3 = Math.sin(f);
		// YZX
		this.x = s1 * c2 * c3 + c1 * s2 * s3;
		this.y = c1 * s2 * c3 + s1 * c2 * s3;
		this.z = c1 * c2 * s3 - s1 * s2 * c3;
		this.w = c1 * c2 * c3 - s1 * s2 * s3;
		return this;
	}

	/**
		Convert this quaternion to an Euler of arbitrary order.
		@param	the order of the euler to obtain
			(in blender order, opposite from mathematical order)
			can be "XYZ", "XZY", "YXZ", "YZX", "ZXY", or "ZYX".
		@return	A new YZX Euler that represents the same rotation as this
				quaternion.
	**/
	// this method use matrices as a middle ground
	// (and is copied from blender's internal code in mathutils)
	// note: there are two possible eulers for the same rotation, blender defines the 'best' as the one with the smallest sum of absolute components
	//	should we actually make that choice, or is just getting one of them randomly good?
	// note2: it seems that this engine transforms a vector by using vector×matrix instead of matrix×vector, meaning that the outer transformations are on the RIGHT.
	//	(…Except for quaternions, where the outer quaternions are on the LEFT.)
	//	anywho, the way the elements of the matrix are ordered makes sense (first digit-> row ID, second digit->column ID) in this system.
	public inline function toEulerOrdered(p: String): Vec4{
		// normalize quat ?

		var q0: FastFloat = SQRT2 * this.w;
		var q1: FastFloat = SQRT2 * this.x;
		var q2: FastFloat = SQRT2 * this.y;
		var q3: FastFloat = SQRT2 * this.z;

		var qda: FastFloat = q0 * q1;
		var qdb: FastFloat = q0 * q2;
		var qdc: FastFloat = q0 * q3;
		var qaa: FastFloat = q1 * q1;
		var qab: FastFloat = q1 * q2;
		var qac: FastFloat = q1 * q3;
		var qbb: FastFloat = q2 * q2;
		var qbc: FastFloat = q2 * q3;
		var qcc: FastFloat = q3 * q3;

		var m = new Mat3(
			// OK, *this* matrix is transposed with respect to what leenkx expects.
			// it is transposed again in the next step though

			(1.0 - qbb - qcc),
			(qdc + qab),
			(-qdb + qac),

			(-qdc + qab),
			(1.0 - qaa - qcc),
			(qda + qbc),

			(qdb + qac),
			(-qda + qbc),
			(1.0 - qaa - qbb)
		);

		// now define what is necessary to perform look-ups in that matrix
		var ml: Array<Array<FastFloat>> = [[m._00, m._10, m._20],
		                                   [m._01, m._11, m._21],
		                                   [m._02, m._12, m._22]];
		var eull: Array<FastFloat> = [0, 0, 0];

		var i: Int = p.charCodeAt(0) - "X".charCodeAt(0);
		var j: Int = p.charCodeAt(1) - "X".charCodeAt(0);
		var k: Int = p.charCodeAt(2) - "X".charCodeAt(0);

		// now the dumber version (isolating code)
		if (p.charAt(0) == "X") i = 0;
		else if (p.charAt(0) == "Y") i = 1;
		else i = 2;
		if (p.charAt(1) == "X") j = 0;
		else if (p.charAt(1) == "Y") j = 1;
		else j = 2;
		if (p.charAt(2) == "X") k = 0;
		else if (p.charAt(2) == "Y") k = 1;
		else k = 2;

		var cy: FastFloat = Math.sqrt(ml[i][i] * ml[i][i] + ml[i][j] * ml[i][j]);

		var eul1 = new Vec4();

		if (cy > 16.0 * 1e-3) {
			eull[i] = Math.atan2(ml[j][k], ml[k][k]);
			eull[j] = Math.atan2(-ml[i][k], cy);
			eull[k] = Math.atan2(ml[i][j], ml[i][i]);
		}
		else {
			eull[i] = Math.atan2(-ml[k][j], ml[j][j]);
			eull[j] = Math.atan2(-ml[i][k], cy);
			eull[k] = 0; // 2 * Math.PI;
		}
		eul1.x = eull[0];
		eul1.y = eull[1];
		eul1.z = eull[2];

		if (p == "XZY" || p == "YXZ" || p == "ZYX") {
			eul1.x *= -1;
			eul1.y *= -1;
			eul1.z *= -1;
		}
		return eul1;
	}

	/**
		Set this quaternion to the rotation represented by an Euler.
		@param	x The Euler's x component.
		@param	y The Euler's y component.
		@param	z The Euler's z component.
		@param	order: the (blender) order of the euler
			(which is the OPPOSITE of the mathematical order)
			can be "XYZ", "XZY", "YXZ", "YZX", "ZXY", or "ZYX".
		@return	This quaternion.
	**/
	public inline function fromEulerOrdered(e: Vec4, order: String): Quat {
		
		var mappedAngles = new Vec4();
		switch (order) {
			case "XYZ":
				mappedAngles.set(e.x, e.y, e.z);
			case "XZY":
				mappedAngles.set(e.x, e.z, e.y);
			case "YXZ": 
				mappedAngles.set(e.y, e.x, e.z);
			case "YZX": 
				mappedAngles.set(e.y, e.z, e.x);
			case "ZXY": 
				mappedAngles.set(e.z, e.x, e.y);
			case "ZYX": 
				mappedAngles.set(e.z, e.y, e.x);
		}
		var c1 = Math.cos(mappedAngles.x / 2);
		var c2 = Math.cos(mappedAngles.y / 2);
		var c3 = Math.cos(mappedAngles.z / 2);
		var s1 = Math.sin(mappedAngles.x / 2);
		var s2 = Math.sin(mappedAngles.y / 2);
		var s3 = Math.sin(mappedAngles.z / 2);
		var qx = new Quat(s1, 0, 0, c1);
		var qy = new Quat(0, s2, 0, c2);
		var qz = new Quat(0, 0, s3, c3);

		// Original multiplication sequence (implements reverse of 'order')
		if (order.charAt(2) == 'X')
			this.setFrom(qx);
		else if (order.charAt(2) == 'Y')
			this.setFrom(qy);
		else
			this.setFrom(qz);
		if (order.charAt(1) == 'X')
			this.mult(qx);
		else if (order.charAt(1) == 'Y')
			this.mult(qy);
		else
			this.mult(qz);
		if (order.charAt(0) == 'X')
			this.mult(qx);
		else if (order.charAt(0) == 'Y')
			this.mult(qy);
		else
			this.mult(qz);

		// TO DO quick fix somethings wrong..
		this.x = -this.x;
		this.y = -this.y;
		this.z = -this.z;
		this.w = -this.w;
		
		return this;
	}

	/**
		Linearly interpolate between two other quaterions, and store the
		result in this one. This is not a so-called slerp operation.
		@param	from The quaterion to interpolate from.
		@param	to The quaterion to interpolate to.
		@param	s The amount to interpolate, with 0 being `from` and 1 being
				`to`, and 0.5 being half way between the two.
		@return	This quaternion.
	**/
	public inline function lerp(from: Quat, to: Quat, s: FastFloat): Quat {
		var fromx = from.x;
		var fromy = from.y;
		var fromz = from.z;
		var fromw = from.w;
		var dot: FastFloat = from.dot(to);
		if (dot < 0.0) {
			fromx = -fromx;
			fromy = -fromy;
			fromz = -fromz;
			fromw = -fromw;
		}
		x = fromx + (to.x - fromx) * s;
		y = fromy + (to.y - fromy) * s;
		z = fromz + (to.z - fromz) * s;
		w = fromw + (to.w - fromw) * s;
		return normalize();
	}

	// Slerp is shorthand for spherical linear interpolation
	public inline function slerp(from: Quat, to: Quat, t: FastFloat): Quat {
		var epsilon: Float = 0.0005;

		var dot = from.dot(to);
		if (dot > 1 - epsilon) {
			var result: Quat = to.add((from.sub(to)).scale(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: Quat = to.sub(scale(dot));
		q2.normalize();
		var result: Quat = scale(Math.cos(theta)).add(q2.scale(Math.sin(theta)));
		result.normalize();
		return result;
	}

	/**
		Find the dot product of this quaternion with another.
		@param	q The other quaternion.
		@return	The dot product.
	**/
	public inline function dot(q: Quat): FastFloat {
		return (x * q.x) + (y * q.y) + (z * q.z) + (w * q.w);
	}

	public inline function fromTo(v1: Vec4, v2: Vec4): Quat {
		// Rotation formed by direction vectors
		// v1 and v2 should be normalized first
		var a = helpVec0;
		var dot = v1.dot(v2);
		if (dot < -0.999999) {
			a.crossvecs(xAxis, v1);
			if (a.length() < 0.000001) a.crossvecs(yAxis, v1);
			a.normalize();
			fromAxisAngle(a, Math.PI);
		}
		else if (dot > 0.999999) {
			set(0, 0, 0, 1);
		}
		else {
			a.crossvecs(v1, v2);
			set(a.x, a.y, a.z, 1 + dot);
			normalize();
		}
		return this;
	}

	public function toString(): String {
		return this.x + ", " + this.y + ", " + this.z + ", " + this.w;
	}
}