liberty.math.quaternion source code

1 /**
2  * Copyright:       Copyright (C) 2018 Gabriel Gheorghe, All Rights Reserved
3  * Authors:         $(Gabriel Gheorghe)
4  * License:         $(LINK2 https://www.gnu.org/licenses/gpl-3.0.txt, GNU GENERAL PUBLIC LICENSE Version 3, 29 June 2007)
5  * Source:			$(LINK2 https://github.com/GabyForceQ/LibertyEngine/blob/master/source/liberty/math/quaternion.d, _quaternion.d)
6  * Documentation:
7  * Coverage:
8  */
9 module liberty.math.quaternion;
10 import liberty.math.vector : Vector;
11 import liberty.math.matrix : Matrix, isMatrixInstance;
12 ///
13 struct Quaternion(T) {
14 	///
15 	alias type = T;
16 	union {
17 		Vector!(T, 4u) v;
18 		struct {
19 			T x, y, z, w;
20 		}
21 	}
22 	/// Constructs a Quaternion from a value.
23 	this(U)(U x) pure nothrow @safe @nogc if (isAssignable!U) {
24 		opAssign!U(x);
25 	}
26 	/// Constructs a Quaternion from coordinates.
27 	this(T qw, T qx, T qy, T qz) pure nothrow @safe @nogc {
28 		x = qx;
29 		y = qy;
30 		z = qz;
31 		w = qw;
32 	}
33 	/// Constructs a Quaternion from axis + angle.
34 	static Quaternion fromAxis(Vector!(T, 3) axis, T angle) pure nothrow @safe @nogc {
35 		import liberty.math.functions : sin, cos;
36 		Quaternion q = void;
37 		axis.normalize();
38 		T cos_a = cos(angle / 2);
39 		T sin_a = sin(angle / 2);
40 		q.x = sin_a * axis.x;
41 		q.y = sin_a * axis.y;
42 		q.z = sin_a * axis.z;
43 		q.w = cos_a;
44 		return q;
45 	}
46 	/// Constructs a Quaternion from Euler angles.
47 	static Quaternion fromEulerAngles(T roll, T pitch, T yaw) pure nothrow @safe @nogc {
48 		import liberty.math.functions : sin, cos;
49 		Quaternion q = void;
50 		T sinPitch = sin(pitch / 2);
51 		T cosPitch = cos(pitch / 2);
52 		T sinYaw = sin(yaw / 2);
53 		T cosYaw = cos(yaw / 2);
54 		T sinRoll = sin(roll / 2);
55 		T cosRoll = cos(roll / 2);
56 		T cosPitchCosYaw = cosPitch * cosYaw;
57 		T sinPitchSinYaw = sinPitch * sinYaw;
58 		q.x = sinRoll * cosPitchCosYaw    - cosRoll * sinPitchSinYaw;
59 		q.y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
60 		q.z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
61 		q.w = cosRoll * cosPitchCosYaw    + sinRoll * sinPitchSinYaw;
62 		return q;
63 	}
64 	/// Converts a quaternion to Euler angles.
65 	Vector!(T, 3) toEulerAngles() pure nothrow const @safe @nogc {
66 		import liberty.math.functions : atan2, sqrt, PI;
67 		Matrix!(T, 3) m = cast(Matrix!(T, 3))(this);
68 		T pitch, yaw, roll;
69 		T s = sqrt(m.c[0][0] * m.c[0][0] + m.c[1][0] * m.c[1][0]);
70 		if (s > T.epsilon) {
71 			pitch = - atan2(m.c[2][0], s);
72 			yaw = atan2(m.c[1][0], m.c[0][0]);
73 			roll = atan2(m.c[2][1], m.c[2][2]);
74 		} else  {
75 			pitch = m.c[2][0] < 0.0f ? PI /2 : -PI / 2;
76 			yaw = -atan2(m.c[0][1], m.c[1][1]);
77 			roll = 0.0f;
78 		}
79 		return Vector!(T, 3)(roll, pitch, yaw);
80 	}
81 	/// Assign from another Quaternion.
82 	ref Quaternion opAssign(U)(U u) pure nothrow @safe @nogc if (isQuaternionInstance!U && is(U._T : T)) {
83 		v = u.v;
84 		return this;
85 	}
86 	/// Assign from a vector of 4 elements.
87 	ref Quaternion opAssign(U)(U u) pure nothrow @safe @nogc if (is(U : Vector!(T, 4u))) {
88 		v = u;
89 		return this;
90 	}
91 	/// Converts to a string.
92 	string toString() const nothrow {
93 		import std.string : format;
94 		try {
95 			return format("%s", v);
96 		} catch (Exception e) {
97 			assert(0);
98 		}
99 	}
100 	/// Normalizes a quaternion.
101 	void normalize() pure nothrow @safe @nogc {
102 		v.normalize();
103 	}
104 	/// Returns normalized quaternion.
105 	Quaternion normalized() pure nothrow const @safe @nogc {
106 		Quaternion ret = void;
107 		ret.v = v.normalized();
108 		return ret;
109 	}
110 	/// Inverses a quaternion in-place.
111 	void inverse() pure nothrow @safe @nogc {
112 		x = -x;
113 		y = -y;
114 		z = -z;
115 	}
116 	/// Returns inverse of quaternion.
117 	Quaternion inversed() pure nothrow const @safe @nogc {
118 		Quaternion ret = void;
119 		ret.v = v;
120 		ret.inverse();
121 		return ret;
122 	}
123 	///
124 	ref Quaternion opOpAssign(string op, U)(U q) pure nothrow @safe @nogc if (is(U : Quaternion) && (op == "*")) {
125 		T nx = w * q.x + x * q.w + y * q.z - z * q.y,
126 		ny = w * q.y + y * q.w + z * q.x - x * q.z,
127 		nz = w * q.z + z * q.w + x * q.y - y * q.x,
128 		nw = w * q.w - x * q.x - y * q.y - z * q.z;
129 		x = nx;
130 		y = ny;
131 		z = nz;
132 		w = nw;
133 		return this;
134 	}
135 	///
136 	ref Quaternion opOpAssign(string op, U)(U operand) pure nothrow @safe @nogc if (isConvertible!U) {
137 		Quaternion conv = operand;
138 		return opOpAssign!op(conv);
139 	}
140 	///
141 	Quaternion opBinary(string op, U)(U operand) pure nothrow const @safe @nogc if (is(U: Quaternion) || (isConvertible!U)) {
142 		Quaternion temp = this;
143 		return temp.opOpAssign!op(operand);
144 	}
145 	/// Compare two Quaternions.
146 	bool opEquals(U)(U other) pure const @safe @nogc if (is(U : Quaternion)) {
147 		return v == other.v;
148 	}
149 	/// Compare Quaternion and other types.
150 	bool opEquals(U)(U other) pure nothrow const @safe @nogc if (isConvertible!U) {
151 		Quaternion conv = other;
152 		return opEquals(conv);
153 	}
154 	/// Convert to a 3x3 rotation matrix.
155 	U opCast(U)() pure nothrow const @safe @nogc if (isMatrixInstance!U && is(U.type : type) && (U.rowCount == 3) && (U.columnCount == 3)) {
156 		T norm = x*x + y*y + z*z + w*w;
157 		T s = (norm > 0) ? 2 / norm : 0;
158 		T xx = x * x * s, xy = x * y * s, xz = x * z * s, xw = x * w * s,
159 		yy = y * y * s, yz = y * z * s, yw = y * w * s,
160 		zz = z * z * s, zw = z * w * s;
161 		return Matrix!(U.type, 3)(1 - (yy + zz), (xy - zw), (xz + yw), (xy + zw), 1 - (xx + zz), (yz - xw), (xz - yw), (yz + xw), 1 - (xx + yy));
162 	}
163 	/// Converts a to a 4x4 rotation matrix.
164 	U opCast(U)() pure nothrow const @safe @nogc if (isMatrixInstance!U && is(U.type : type) && (U.rowCount == 4) && (U.columnCount == 4)) {
165 		auto m3 = cast(Matrix!(U.type, 3))(this);
166 		return cast(U)(m3);
167 	}
168 	///
169 	static Quaternion identity() pure nothrow @safe @nogc @property {
170 		Quaternion q = void;
171 		q.x = q.y = q.z = 0;
172 		q.w = 1;
173 		return q;
174 	}
175 	private enum bool isAssignable(T) = is(typeof({ T x; Quaternion v = x; }()));
176 	private enum bool isConvertible(T) = (!is(T : Quaternion)) && isAssignable!T;
177 }
178 ///
179 template isQuaternionInstance(U) {
180 	private static void isQuaternion(T)(Quaternion!T x) {}
181 	enum bool isQuaternionInstance = is(typeof(isQuaternion(U.init)));
182 }
183 ///
184 alias QuaternionF = Quaternion!float;
185 ///
186 alias QuaternionD = Quaternion!double;
187 /// Returns linear interpolation for quaternions.
188 Quaternion!T lerp(T)(Quaternion!T a, Quaternion!T b, float t) pure nothrow @safe @nogc {
189 	Quaternion!T ret;
190 	ret.v = funcs.lerp(a.v, b.v, t);
191 	return ret;
192 }
193 /// Returns nlerp of quaternions.
194 Quaternion!T nlerp(T)(Quaternion!T a, Quaternion!T b, float t) pure nothrow @safe @nogc
195 in {
196 	assert(t >= 0 && t <= 1);
197 } do {
198 	Quaternion!T ret;
199 	ret.v = funcs.lerp(a.v, b.v, t);
200 	ret.v.normalize();
201 	return ret;
202 }
203 /// Returns slerp of quaternions.
204 Quaternion!T slerp(T)(Quaternion!T a, Quaternion!T b, T t) pure nothrow @safe @nogc
205 in {
206 	assert(t >= 0 && t <= 1);
207 } do {
208 	Quaternion!T ret;
209 	T dotProduct = dot(a.v, b.v);
210 	if (dotProduct < 0) {
211 		b.v *= -1;
212 		dotProduct = dot(a.v, b.v);
213 	}
214 	immutable T threshold = 10 * T.epsilon;
215 	if ((1 - dotProduct) > threshold) {
216 		return lerp(a, b, t);
217 	}
218 	T theta_0 = funcs.safeAcos(dotProduct);
219 	T theta = theta_0 * t;
220 	vec3!T v2 = dot(b.v, a.v * dotProduct);
221 	v2.normalize();
222 	ret.v = dot(b.v, a.v * dotProduct);
223 	ret.v.normalize();
224 	ret.v = a.v * cos(theta) + ret.v * sin(theta);
225 	return ret;
226 }