1 module gfm.math.quaternion; 2 3 import std.math, 4 std..string; 5 6 import gfm.math.vector, 7 gfm.math.matrix, 8 funcs = gfm.math.funcs; 9 10 /// Quaternion implementation. 11 /// Holds a rotation + angle in a proper but wild space. 12 struct Quaternion(T) 13 { 14 public 15 { 16 union 17 { 18 Vector!(T, 4u) v; 19 struct 20 { 21 T x, y, z, w; 22 } 23 } 24 25 /// Construct a Quaternion from a value. 26 @nogc this(U)(U x) pure nothrow if (isAssignable!U) 27 { 28 opAssign!U(x); 29 } 30 31 /// Constructs a Quaternion from coordinates. 32 /// Warning: order of coordinates is different from storage. 33 @nogc this(T qw, T qx, T qy, T qz) pure nothrow 34 { 35 x = qx; 36 y = qy; 37 z = qz; 38 w = qw; 39 } 40 41 /// Constructs a Quaternion from axis + angle. 42 @nogc static Quaternion fromAxis(Vector!(T, 3) axis, T angle) pure nothrow 43 { 44 Quaternion q = void; 45 axis.normalize(); 46 T cos_a = cos(angle / 2); 47 T sin_a = sin(angle / 2); 48 q.x = sin_a * axis.x; 49 q.y = sin_a * axis.y; 50 q.z = sin_a * axis.z; 51 q.w = cos_a; 52 return q; // should be normalized 53 } 54 55 /// Constructs a Quaternion from Euler angles. 56 /// All paramers given in radians. 57 /// Roll->X axis, Pitch->Y axis, Yaw->Z axis 58 /// See_also: $(LINK https://www.cs.princeton.edu/~gewang/projects/darth/stuff/quat_faq.html) 59 @nogc static Quaternion fromEulerAngles(T roll, T pitch, T yaw) pure nothrow 60 { 61 Quaternion q = void; 62 T sinPitch = sin(pitch / 2); 63 T cosPitch = cos(pitch / 2); 64 T sinYaw = sin(yaw / 2); 65 T cosYaw = cos(yaw / 2); 66 T sinRoll = sin(roll / 2); 67 T cosRoll = cos(roll / 2); 68 T cosPitchCosYaw = cosPitch * cosYaw; 69 T sinPitchSinYaw = sinPitch * sinYaw; 70 q.x = sinRoll * cosPitchCosYaw - cosRoll * sinPitchSinYaw; 71 q.y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; 72 q.z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; 73 q.w = cosRoll * cosPitchCosYaw + sinRoll * sinPitchSinYaw; 74 return q; 75 } 76 77 /// Converts a quaternion to Euler angles. 78 /// TODO: adds a EulerAngles type. 79 /// Returns: A vector which contains roll-pitch-yaw as x-y-z. 80 @nogc vec3!T toEulerAngles() pure const nothrow 81 { 82 mat3!T m = cast(mat3!T)(this); 83 84 T pitch, yaw, roll; 85 T s = sqrt(m.c[0][0] * m.c[0][0] + m.c[1][0] * m.c[1][0]); 86 if (s > T.epsilon) 87 { 88 pitch = - atan2(m.c[2][0], s); 89 yaw = atan2(m.c[1][0], m.c[0][0]); 90 roll = atan2(m.c[2][1], m.c[2][2]); 91 } 92 else 93 { 94 pitch = m.c[2][0] < 0.0f ? PI /2 : -PI / 2; 95 yaw = -atan2(m.c[0][1], m.c[1][1]); 96 roll = 0.0f; 97 } 98 return vec3!T(roll, pitch, yaw); 99 } 100 101 /// Assign from another Quaternion. 102 @nogc ref Quaternion opAssign(U)(U u) pure nothrow if (is(typeof(U._isQuaternion)) && is(U._T : T)) 103 { 104 v = u.v; 105 return this; 106 } 107 108 /// Assign from a vector of 4 elements. 109 @nogc ref Quaternion opAssign(U)(U u) pure nothrow if (is(U : Vector!(T, 4u))) 110 { 111 v = u; 112 return this; 113 } 114 115 /// Converts to a pretty string. 116 string toString() const nothrow 117 { 118 try 119 return format("%s", v); 120 catch (Exception e) 121 assert(false); // should not happen since format is right 122 } 123 124 /// Normalizes a quaternion. 125 @nogc void normalize() pure nothrow 126 { 127 v.normalize(); 128 } 129 130 /// Returns: Normalized quaternion. 131 @nogc Quaternion normalized() pure const nothrow 132 { 133 Quaternion res = void; 134 res.v = v.normalized(); 135 return res; 136 } 137 138 /// Inverses a quaternion in-place. 139 @nogc void inverse() pure nothrow 140 { 141 x = -x; 142 y = -y; 143 z = -z; 144 } 145 146 /// Returns: Inverse of quaternion. 147 @nogc Quaternion inversed() pure const nothrow 148 { 149 Quaternion res = void; 150 res.v = v; 151 res.inverse(); 152 return res; 153 } 154 155 @nogc ref Quaternion opOpAssign(string op, U)(U q) pure nothrow 156 if (is(U : Quaternion) && (op == "*")) 157 { 158 T nx = w * q.x + x * q.w + y * q.z - z * q.y, 159 ny = w * q.y + y * q.w + z * q.x - x * q.z, 160 nz = w * q.z + z * q.w + x * q.y - y * q.x, 161 nw = w * q.w - x * q.x - y * q.y - z * q.z; 162 x = nx; 163 y = ny; 164 z = nz; 165 w = nw; 166 return this; 167 } 168 169 @nogc ref Quaternion opOpAssign(string op, U)(U operand) pure nothrow if (isConvertible!U) 170 { 171 Quaternion conv = operand; 172 return opOpAssign!op(conv); 173 } 174 175 @nogc Quaternion opBinary(string op, U)(U operand) pure const nothrow 176 if (is(U: Quaternion) || (isConvertible!U)) 177 { 178 Quaternion temp = this; 179 return temp.opOpAssign!op(operand); 180 } 181 182 /// Compare two Quaternions. 183 bool opEquals(U)(U other) pure const if (is(U : Quaternion)) 184 { 185 return v == other.v; 186 } 187 188 /// Compare Quaternion and other types. 189 bool opEquals(U)(U other) pure const nothrow if (isConvertible!U) 190 { 191 Quaternion conv = other; 192 return opEquals(conv); 193 } 194 195 /// Convert to a 3x3 rotation matrix. 196 /// TODO: check out why we can't do is(Unqual!U == mat3!T) 197 @nogc U opCast(U)() pure const nothrow if (is(typeof(U._isMatrix)) 198 && is(U._T : _T) 199 && (U._R == 3) && (U._C == 3)) 200 { 201 // do not assume that quaternion is normalized 202 T norm = x*x + y*y + z*z + w*w; 203 T s = (norm > 0) ? 2 / norm : 0; 204 205 T xx = x * x * s, xy = x * y * s, xz = x * z * s, xw = x * w * s, 206 yy = y * y * s, yz = y * z * s, yw = y * w * s, 207 zz = z * z * s, zw = z * w * s; 208 return mat3!(U._T) 209 ( 210 1 - (yy + zz) , (xy - zw) , (xz + yw) , 211 (xy + zw) , 1 - (xx + zz) , (yz - xw) , 212 (xz - yw) , (yz + xw) , 1 - (xx + yy) 213 ); 214 } 215 216 /// Converts a to a 4x4 rotation matrix. 217 /// Bugs: check why we can't do is(Unqual!U == mat4!T) 218 @nogc U opCast(U)() pure const nothrow if (is(typeof(U._isMatrix)) 219 && is(U._T : _T) 220 && (U._R == 4) && (U._C == 4)) 221 { 222 auto m3 = cast(mat3!(U._T))(this); 223 return cast(U)(m3); 224 } 225 226 // Workaround Vector not being constructable through CTFE 227 @nogc static Quaternion identity() pure nothrow @property 228 { 229 Quaternion q; 230 q.x = q.y = q.z = 0; 231 q.w = 1; 232 return q; 233 } 234 } 235 236 private 237 { 238 alias T _T; 239 enum _isQuaternion = true; 240 241 template isAssignable(T) 242 { 243 enum bool isAssignable = 244 is(typeof( 245 { 246 T x; 247 Quaternion v = x; 248 }())); 249 } 250 251 // define types that can be converted to Quaternion, but are not Quaternion 252 template isConvertible(T) 253 { 254 enum bool isConvertible = (!is(T : Quaternion)) && isAssignable!T; 255 } 256 } 257 } 258 259 alias Quaternion!float quatf; 260 alias Quaternion!double quatd; 261 262 /// Linear interpolation, for quaternions. 263 @nogc Quaternion!T lerp(T)(Quaternion!T a, Quaternion!T b, float t) pure nothrow 264 { 265 Quaternion!T res = void; 266 res.v = funcs.lerp(a.v, b.v, t); 267 return res; 268 } 269 270 271 /// Returns: Nlerp of quaternions. 272 /// See_also: $(WEB keithmaggio.wordpress.com/2011/02/15/math-magician-lerp-slerp-and-nlerp/, Math Magician – Lerp, Slerp, and Nlerp) 273 @nogc Quaternion!T Nlerp(T)(Quaternion!T a, Quaternion!T b, float t) pure nothrow 274 { 275 assert(t >= 0 && t <= 1); // else probably doesn't make sense 276 Quaternion!T res = void; 277 res.v = funcs.lerp(a.v, b.v, t); 278 res.v.normalize(); 279 return res; 280 } 281 282 /// Returns: Slerp of quaternions. Slerp is more expensive than Nlerp. 283 /// See_also: "Understanding Slerp, Then Not Using It" 284 @nogc Quaternion!T slerp(T)(Quaternion!T a, Quaternion!T b, T t) pure nothrow 285 { 286 assert(t >= 0 && t <= 1); // else probably doesn't make sense 287 288 Quaternion!T res = void; 289 290 T dotProduct = dot(a.v, b.v); 291 292 // spherical lerp always has 2 potential paths 293 // here we always take the shortest 294 if (dotProduct < 0) 295 { 296 b.v *= -1; 297 dotProduct = dot(a.v, b.v); 298 } 299 300 immutable T threshold = 10 * T.epsilon; // idMath uses 1e-6f for 32-bits float precision 301 if ((1 - dotProduct) > threshold) // if small difference, use lerp 302 return lerp(a, b, t); 303 304 T theta_0 = funcs.safeAcos(dotProduct); // angle between this and other 305 T theta = theta_0 * t; // angle between this and result 306 307 vec3!T v2 = dot(b.v, a.v * dotProduct); 308 v2.normalize(); 309 310 res.v = dot(b.v, a.v * dotProduct); 311 res.v.normalize(); 312 313 res.v = a.v * cos(theta) + res.v * sin(theta); 314 return res; 315 } 316 317 unittest 318 { 319 quatf a = quatf.fromAxis(vec3f(1, 0, 0), 1); 320 quatf b = quatf.fromAxis(vec3f(0, 1, 0), 0); 321 a = a * b; 322 323 quatf c = lerp(a, b, 0.5f); 324 quatf d = Nlerp(a, b, 0.1f); 325 quatf e = slerp(a, b, 0.0f); 326 quatd f = quatd(1.0, 4, 5.0, 6.0); 327 quatf g = quatf.fromEulerAngles(-0.1f, 1.2f, -0.3f); 328 vec3f ga = g.toEulerAngles(); 329 }