1 /** 2 dlsl.quternion 3 4 5 Authors: Peter Particle ( based on gl3n by David Herberth ) 6 License: MIT 7 8 Note: All methods marked with pure are weakly pure since, they all access an instance member. 9 All static methods are strongly pure. 10 */ 11 12 module dlsl.quaternion; 13 14 import std.conv : to; 15 import std.math : asin, atan2, cos, sin, sqrt; 16 17 import dlsl.matrix; 18 public import dlsl.vector; 19 20 /// Pre-defined quaternion of type float. 21 alias Quaternion!( float ) quat; 22 23 24 /// If T is a quaternion, this evaluates to true, otherwise false. 25 template isQuaternion( T ) { enum isQuaternion = is( typeof( isQuaternionImpl( T.init ))); } 26 private void isQuaternionImpl( T )( Quaternion!( T ) q ) {} 27 28 29 /// Base template for all quaternion-types. 30 /// Params: 31 /// type = all values get stored as this type 32 struct Quaternion( type ) { 33 34 nothrow @nogc: 35 36 /// Returns the current quternion formatted as string, useful for printing 37 char[] toString( char[] buffer ) { 38 assert( buffer.length >= 64, "At least 64 chars buffer capacity required!" ); 39 return vector.toString( buffer ); 40 } 41 42 43 pure nothrow @nogc: 44 45 /// Returns a pointer to the vector in memory 46 auto ptr() { 47 return vector.data.ptr; 48 } 49 50 51 pure nothrow @nogc @safe: 52 53 Vector!( type, 4 ) vector; 54 55 alias vector this; /// The Quaternion can be treated as a vector, which cen be treated as an array 56 alias type valueType; /// Holds the internal type of the vector. 57 58 59 /// Returns an identity vector ( x=0, y=0, z=0, w=1 ). 60 static @property Quaternion identity() { 61 return Quaternion( 0, 0, 0, 1 ); 62 } 63 64 65 /// Construct from four values of type $( I valueType ), last one the w coordinate 66 this( T )( T x, T y, T z, T w ) if( is( T : type )) { 67 vector.x = x; 68 vector.y = y; 69 vector.z = z; 70 vector.w = w; 71 } 72 73 /// Construct from a 3d vector / array imaginary part and a real value w of type $( I valueType ) 74 this()( Vector!( valueType, 3 ) vec, valueType w ) { 75 vector[0..3] = vec[]; 76 vector.w = w; 77 } 78 79 /// Construct from 4d vector / array, w or index 3 respectively being the quternions w ccord 80 this()( valueType[ 4 ] array ) { 81 vector = array; 82 } 83 84 /// Construct / convert from 3x3 matrix 85 this()( const ref Matrix!( valueType, 3, 3 ) mat ) { 86 valueType trace = mat[0][0] + mat[1][1] + mat[2][2]; 87 if( trace > 0 ) { 88 real s = 0.5 / sqrt( trace + 1.0f ); 89 x = to!valueType(( mat[2][1] - mat[1][2] ) * s ); 90 y = to!valueType(( mat[0][2] - mat[2][0] ) * s ); 91 z = to!valueType(( mat[1][0] - mat[0][1] ) * s ); 92 w = to!valueType( 0.25 / s ); 93 } else if(( mat[0][0] > mat[1][1] ) && ( mat[0][0] > mat[2][2] )) { 94 real s = 2.0 * sqrt( 1.0 + mat[0][0] - mat[1][1] - mat[2][2] ); 95 x = to!valueType( 0.25f * s ); 96 y = to!valueType(( mat[0][1] + mat[1][0] ) / s ); 97 z = to!valueType(( mat[0][2] + mat[2][0] ) / s ); 98 w = to!valueType(( mat[2][1] - mat[1][2] ) / s ); 99 } else if( mat[1][1] > mat[2][2] ) { 100 real s = 2.0 * sqrt( 1 + mat[1][1] - mat[0][0] - mat[2][2] ); 101 x = to!valueType(( mat[0][1] + mat[1][0] ) / s ); 102 y = to!valueType( 0.25f * s ); 103 z = to!valueType(( mat[1][2] + mat[2][1] ) / s ); 104 w = to!valueType(( mat[0][2] - mat[2][0] ) / s ); 105 } else { 106 real s = 2.0 * sqrt( 1 + mat[2][2] - mat[0][0] - mat[1][1] ); 107 x = to!valueType(( mat[0][2] + mat[2][0] ) / s ); 108 y = to!valueType(( mat[1][2] + mat[2][1] ) / s ); 109 z = to!valueType( 0.25f * s ); 110 w = to!valueType(( mat[1][0] - mat[0][1] ) / s ); 111 } 112 } 113 114 115 116 ////////////// 117 // Rotation // 118 ////////////// 119 120 /// Returns a vector with applied rotation around the x-axis. 121 static Quaternion rotationX( real angle ) { 122 Quaternion result; 123 124 angle /= 2; 125 result.x = to!valueType( sin( angle )); 126 result.y = 0; 127 result.z = 0; 128 result.w = to!valueType( cos( angle )); 129 130 return result; 131 } 132 133 /// Returns a vector with applied rotation around the y-axis. 134 static Quaternion rotationY( real angle ) { 135 Quaternion result; 136 137 angle /= 2; 138 result.x = 0; 139 result.y = to!valueType( sin( angle )); 140 result.z = 0; 141 result.w = to!valueType( cos( angle )); 142 143 return result; 144 } 145 146 /// Returns a vector with applied rotation around the z-axis. 147 static Quaternion rotationZ( real angle ) { 148 Quaternion result; 149 150 angle /= 2; 151 result.x = 0; 152 result.y = 0; 153 result.z = to!valueType( sin( angle )); 154 result.w = to!valueType( cos( angle )); 155 156 return result; 157 } 158 159 /// Returns a vector with applied rotation around an axis. 160 static Quaternion rotation( real angle, Vector!( valueType, 3 ) axis ) { 161 if( angle == 0 ) { 162 return Quaternion.identity; 163 } 164 Quaternion result; 165 166 angle /= 2; 167 valueType sin_angle = to!valueType( sin( angle )); 168 169 result.x = axis.x * sin_angle; 170 result.y = axis.y * sin_angle; 171 result.z = axis.z * sin_angle; 172 result.w = to!valueType( cos( angle )); 173 174 return result; 175 } 176 177 /// Creates a vector from an euler rotation. 178 static Quaternion rotation( real roll, real pitch, real yaw ) { 179 Quaternion result; 180 181 auto cr = cos( roll / 2.0 ); 182 auto cp = cos( pitch / 2.0 ); 183 auto cy = cos( yaw / 2.0 ); 184 auto sr = sin( roll / 2.0 ); 185 auto sp = sin( pitch / 2.0 ); 186 auto sy = sin( yaw / 2.0 ); 187 188 result.x = sr * cp * cy - cr * sp * sy; 189 result.y = cr * sp * cy + sr * cp * sy; 190 result.z = cr * cp * sy - sr * sp * cy; 191 result.w = cr * cp * cy + sr * sp * sy; 192 193 return result; 194 } 195 196 unittest { 197 enum startPitch = 0.1; 198 enum startYaw = -0.2; 199 enum startRoll = 0.6; 200 201 auto q = quat.euler_rotation( startRoll, startPitch, startYaw ); 202 203 assert( almostEqual( q.pitch, startPitch )); 204 assert( almostEqual( q.yaw, startYaw )); 205 assert( almostEqual( q.roll, startRoll )); 206 } 207 208 /// Rotates the current vector around the x-axis and returns $( I this ). 209 Quaternion rotateX( real angle ) { 210 this = rotationX( angle ) * this; 211 return this; 212 } 213 214 /// Rotates the current vector around the y-axis and returns $( I this ). 215 Quaternion rotateY( real angle ) { 216 this = rotationY( angle ) * this; 217 return this; 218 } 219 220 /// Rotates the current vector around the z-axis and returns $( I this ). 221 Quaternion rotateZ( real angle ) { 222 this = rotationZ( angle ) * this; 223 return this; 224 } 225 226 /// Rotates the current vector around an axis and returns $( I this ). 227 Quaternion rotate( real angle, Vector!( valueType, 3 ) axis ) { 228 this = rotation( angle, axis ) * this; 229 return this; 230 } 231 232 /// Applies an euler rotation to the current vector and returns $( I this ). 233 Quaternion rotate( real heading, real attitude, real bank ) { 234 this = rotation( heading, attitude, bank ) * this; 235 return this; 236 } 237 238 unittest { 239 assert( quat.xrotation( PI ).vector[1..4] == [1.0f, 0.0f, 0.0f] ); 240 assert( quat.yrotation( PI ).vector[1..4] == [0.0f, 1.0f, 0.0f] ); 241 assert( quat.zrotation( PI ).vector[1..4] == [0.0f, 0.0f, 1.0f] ); 242 assert(( quat.xrotation( PI ).w == quat.yrotation( PI ).w ) && ( quat.yrotation( PI ).w == quat.zrotation( PI ).w )); 243 //assert( quat.rotatex( PI ).w == to!( quat.valueType )( cos( PI )) ); 244 assert( quat.xrotation( PI ).vector == quat.identity.rotatex( PI ).vector ); 245 assert( quat.yrotation( PI ).vector == quat.identity.rotatey( PI ).vector ); 246 assert( quat.zrotation( PI ).vector == quat.identity.rotatez( PI ).vector ); 247 248 assert( quat.axis_rotation( PI, vec3( 1.0f, 1.0f, 1.0f )).vector[1..4] == [1.0f, 1.0f, 1.0f] ); 249 assert( quat.axis_rotation( PI, vec3( 1.0f, 1.0f, 1.0f )).w == quat.xrotation( PI ).w ); 250 assert( quat.axis_rotation( PI, vec3( 1.0f, 1.0f, 1.0f )).vector == 251 quat.identity.rotate_axis( PI, vec3( 1.0f, 1.0f, 1.0f )).vector ); 252 253 quat q1 = quat.euler_rotation( PI, PI, PI ); 254 assert(( q1.x > -1e-16 ) && ( q1.x < 1e-16 )); 255 assert(( q1.y > -1e-16 ) && ( q1.y < 1e-16 )); 256 assert(( q1.z > -1e-16 ) && ( q1.z < 1e-16 )); 257 //assert( q1.w == -1.0f ); 258 assert( quat.euler_rotation( PI, PI, PI ).vector == quat.identity.rotate_euler( PI, PI, PI ).vector ); 259 } 260 261 262 263 /////////////// 264 // Operators // 265 /////////////// 266 267 Quaternion opBinary( string op )( Quaternion q ) const if(( op == "+" ) || ( op == "-" )) { 268 Quaternion result; 269 270 mixin( "result.x = x" ~ op ~ "q.x;" ); 271 mixin( "result.y = y" ~ op ~ "q.y;" ); 272 mixin( "result.z = z" ~ op ~ "q.z;" ); 273 mixin( "result.w = w" ~ op ~ "q.w;" ); 274 275 return result; 276 } 277 278 Quaternion opBinary( string op : "*" )( Quaternion q ) const { 279 Quaternion result; 280 281 result.x = x * q.w + y * q.z - z * q.y + w * q.x; 282 result.y = -x * q.z + y * q.w + z * q.x + w * q.y; 283 result.z = x * q.y - y * q.x + z * q.w + w * q.z; 284 result.w = -x * q.x - y * q.y - z * q.z + w * q.w; 285 286 return result; 287 } 288 289 Vector!( valueType, 3 ) opBinary( string op : "*" )( Vector!( valueType, 3 ) v ) const { 290 Vector!( valueType, 3 ) result; 291 292 valueType w_w = w ^^ 2; 293 valueType w_2 = w * 2; 294 valueType wx2 = w_2 * x; 295 valueType wy2 = w_2 * y; 296 valueType wz2 = w_2 * z; 297 valueType x_x = x ^^ 2; 298 valueType x_2 = x * 2; 299 valueType xy2 = x_2 * y; 300 valueType xz2 = x_2 * z; 301 valueType y_y = y ^^ 2; 302 valueType yz2 = 2 * y * z; 303 valueType z_z = z * z; 304 305 result.x = w_w * v.x + wy2 * v.z - wz2 * v.y + x_x * v.x + 306 xy2 * v.y + xz2 * v.z - z_z * v.x - y_y * v.x; 307 result.y = xy2 * v.x + y_y * v.y + yz2 * v.z + wz2 * v.x - 308 z_z * v.y + w_w * v.y - wx2 * v.z - x_x * v.y; 309 result.z = xz2 * v.x + yz2 * v.y + z_z * v.z - wy2 * v.x - 310 y_y * v.z + wx2 * v.y - x_x * v.z + w_w * v.z; 311 312 return result; 313 } 314 315 Quaternion opBinary( string op : "*" )( valueType s ) const { 316 return Quaternion( s * x, s * y, s * z, s * w ); 317 } 318 319 auto opBinaryRight( string op, T )( T q ) const if( !isQuaternion!T ) { 320 return this.opBinary!( op )( q ); 321 } 322 323 void opOpAssign( string op : "*" )( Quaternion q ) { 324 valueType w2 = -x * q.x - y * q.y - z * q.z + w * q.w; 325 valueType x2 = x * q.w + y * q.z - z * q.y + w * q.x; 326 valueType y2 = -x * q.z + y * q.w + z * q.x + w * q.y; 327 valueType z2 = x * q.y - y * q.x + z * q.w + w * q.z; 328 w = w2; x = x2; y = y2; z = z2; 329 } 330 331 void opOpAssign( string op : "*" )( valueType q ) { 332 vector[0] *= q; 333 vector[1] *= q; 334 vector[2] *= q; 335 vector[3] *= q; 336 } 337 338 void opOpAssign( string op )( Quaternion q ) if(( op == "+" ) || ( op == "-" )) { 339 mixin( "w = w" ~ op ~ "q.w;" ); 340 mixin( "x = x" ~ op ~ "q.x;" ); 341 mixin( "y = y" ~ op ~ "q.y;" ); 342 mixin( "z = z" ~ op ~ "q.z;" ); 343 } 344 345 346 347 unittest { 348 quat q1 = quat.identity; 349 quat q2 = quat( 3.0f, 0.0f, 1.0f, 2.0f ); 350 quat q3 = quat( 3.4f, 0.1f, 1.2f, 2.3f ); 351 352 assert(( q1 * q1 ).vector == q1.vector ); 353 assert(( q1 * q2 ).vector == q2.vector ); 354 assert(( q2 * q1 ).vector == q2.vector ); 355 quat q4 = q3 * q2; 356 assert(( q2 * q3 ).vector != q4.vector ); 357 q3 *= q2; 358 assert( q4.vector == q3.vector ); 359 assert( almostEqual( q4.x, 0.4f )); 360 assert( almostEqual( q4.y, 6.8f )); 361 assert( almostEqual( q4.z, 13.8f )); 362 assert( almostEqual( q4.w, 4.4f )); 363 364 quat q5 = quat( 1.0f, 2.0f, 3.0f, 4.0f ); 365 quat q6 = quat( 3.0f, 1.0f, 6.0f, 2.0f ); 366 367 assert(( q5 - q6 ).vector == [-2.0f, 1.0f, -3.0f, 2.0f] ); 368 assert(( q5 + q6 ).vector == [4.0f, 3.0f, 9.0f, 6.0f] ); 369 assert(( q6 - q5 ).vector == [2.0f, -1.0f, 3.0f, -2.0f] ); 370 assert(( q6 + q5 ).vector == [4.0f, 3.0f, 9.0f, 6.0f] ); 371 q5 += q6; 372 assert( q5.vector == [4.0f, 3.0f, 9.0f, 6.0f] ); 373 q6 -= q6; 374 assert( q6.vector == [0.0f, 0.0f, 0.0f, 0.0f] ); 375 376 quat q7 = quat( 2.0f, 2.0f, 2.0f, 2.0f ); 377 assert(( q7 * 2 ).vector == [4.0f, 4.0f, 4.0f, 4.0f] ); 378 assert(( 2 * q7 ).vector == ( q7 * 2 ).vector ); 379 q7 *= 2; 380 assert( q7.vector == [4.0f, 4.0f, 4.0f, 4.0f] ); 381 382 vec3 v1 = vec3( 1.0f, 2.0f, 3.0f ); 383 assert(( q1 * v1 ) == v1 ); 384 assert(( v1 * q1 ) == ( q1 * v1 ) ); 385 assert(( q2 * v1 ) == [-2.0f, 36.0f, 38.0f] ); 386 } 387 388 int opCmp( ref const Quaternion q ) const { 389 foreach( i, a; vector ) { 390 if( a < q.vector[i] ) { 391 return -1; 392 } else if( a > q.vector[i] ) { 393 return 1; 394 } 395 } 396 return 0; // Quaternions are the same 397 } 398 399 bool opEquals( const Quaternion q ) const { 400 return vector == q.vector; 401 } 402 403 bool opCast( T : bool )() const { 404 return isFinite( this ); 405 } 406 407 unittest { 408 assert( quat( 1.0f, 2.0f, 3.0f, 4.0f ) == quat( 1.0f, 2.0f, 3.0f, 4.0f )); 409 assert( quat( 1.0f, 2.0f, 3.0f, 4.0f ) != quat( 1.0f, 2.0f, 3.0f, 3.0f )); 410 411 assert( !( quat( float.nan, float.nan, float.nan, float.nan )) ); 412 if( quat( 1.0f, 1.0f, 1.0f, 1.0f )) { } 413 else { assert( false ); } 414 } 415 } 416 417 418 419 ///////////////////////////////// 420 // free functions akin to glsl // 421 ///////////////////////////////// 422 423 pure nothrow @nogc @safe: 424 425 /// Returns true if all values are not nan and finite, otherwise false. 426 bool isFinite( Q )( const ref Q q ) if( isQuaternion!Q ) { 427 import std.math : isNaN, isInfinity; 428 foreach( q; vector ) { 429 if( isNaN( q ) || isInfinity( q )) { 430 return false; 431 } 432 } 433 return true; 434 } 435 436 437 unittest { 438 quat q1 = quat( 0.0f, 0.0f, 0.0f, 1.0f ); 439 assert( q1.vector == [0.0f, 0.0f, 0.0f, 1.0f] ); 440 assert( q1.vector == quat( 0.0f, 0.0f, 0.0f, 1.0f ).vector ); 441 assert( q1.vector == quat( 0.0f, vec3( 0.0f, 0.0f, 1.0f )).vector ); 442 assert( q1.vector == quat( vec4( 0.0f, 0.0f, 0.0f, 1.0f )).vector ); 443 444 assert( q1.isFinite ); 445 q1.x = float.infinity; 446 assert( !q1.isFinite ); 447 q1.x = float.nan; 448 assert( !q1.isFinite ); 449 q1.x = 0.0f; 450 assert( q1.isFinite ); 451 } 452 453 454 /// Returns an inverted copy of the current vector 455 Q invert( Q )( const ref Q q ) if ( isQuaternion!Q ) { 456 return Q( -q.x, -q.y, -q.z, q.w ); 457 } alias conjugate = invert; 458 459 460 unittest { 461 quat q1 = quat( 1.0f, 1.0f, 1.0f, 1.0f ); 462 463 assert( q1.length == 2.0f ); 464 assert( q1.lengthSquared == 4.0f ); 465 assert( q1.length == quat( 0.0f, 0.0f, 2.0f, 0.0f ).length ); 466 467 quat q2 = quat.identity; 468 assert( q2.vector == [ 0.0f, 0.0f, 0.0f, 1.0f ] ); 469 assert( q2.x == 0.0f ); 470 assert( q2.y == 0.0f ); 471 assert( q2.z == 0.0f ); 472 assert( q2.w == 1.0f ); 473 474 assert( q1.invert.vector == [ -1.0f, -1.0f, -1.0f, 1.0f ] ); 475 q1 = q1.invert; 476 assert( q1.vector == [ 1.0f, 1.0f, 1.0f, 1.0f ] ); 477 478 q1.make_identity(); 479 assert( q1.vector == q2.vector ); 480 481 } 482 483 484 /// Convenience functions returning the vector as corresponding matrices 485 /// Params: 486 /// q = convert quternion 487 /// Returns: mat3 / mat3x3, mat4 / mat4x4, mat3x4, mat4x3 488 import dlsl.matrix; 489 auto toMat3( Q )( Q q ) if( isQuaternion!Q && is( Q.valueType : float )) { return mat3( q ); } 490 auto toMat4( Q )( Q q ) if( isQuaternion!Q && is( Q.valueType : float )) { return mat4( q ); } 491 auto toMat3x3( Q )( Q q ) if( isQuaternion!Q && is( Q.valueType : float )) { return mat3x3( q ); } 492 auto toMat3x4( Q )( Q q ) if( isQuaternion!Q && is( Q.valueType : float )) { return mat3x4( q ); } 493 auto toMat4x3( Q )( Q q ) if( isQuaternion!Q && is( Q.valueType : float )) { return mat4x3( q ); } 494 auto toMat4x4( Q )( Q q ) if( isQuaternion!Q && is( Q.valueType : float )) { return mat4x4( q ); } 495 496 497 unittest { 498 quat q1 = quat( 4.0f, 1.0f, 2.0f, 3.0f ); 499 500 assert( q1.to_matrix!( 3, 3 ) == [[-25.0f, -20.0f, 22.0f], [28.0f, -19.0f, 4.0f], [-10.0f, 20.0f, -9.0f]] ); 501 assert( q1.to_matrix!( 4, 4 ) == [[-25.0f, -20.0f, 22.0f, 0.0f], 502 [28.0f, -19.0f, 4.0f, 0.0f], 503 [-10.0f, 20.0f, -9.0f, 0.0f], 504 [0.0f, 0.0f, 0.0f, 1.0f]] ); 505 assert( quat.identity.to_matrix!( 3, 3 ) == Matrix!( valueType, 3, 3 ).identity ); 506 assert( q1.vector == quat.from_matrix( q1.to_matrix!( 3, 3 )).vector ); 507 508 assert( quat( 1.0f, 0.0f, 0.0f, 0.0f ).vector == quat.from_matrix( mat3.identity ).vector ); 509 510 quat q2 = quat.from_matrix( mat3( 1.0f, 3.0f, 2.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f )); 511 assert( q2.x == 0.0f ); 512 assert( almostEqual( q2.y, 0.7071067f )); 513 assert( almostEqual( q2.z, -1.060660 )); 514 assert( almostEqual( q2.w, 0.7071067f )); 515 } 516 517 518 519 /// lenghtSquared, length and normalize through vector 520 unittest { 521 quat q1 = quat( 1.0f, 2.0f, 3.0f, 4.0f ); 522 quat q2 = quat( 1.0f, 2.0f, 3.0f, 4.0f ); 523 524 q1.normalize(); 525 assert( q1.vector == q2.normalized.vector ); 526 //assert( q1.vector == q1.normalized.vector ); 527 assert( almostEqual( q1.length, 1.0 )); 528 } 529 530 531 /// Returns the yaw 532 Q.valueType yaw( Q )( const ref Q q ) if ( isQuaternion!Q ) { 533 return atan2( to!( Q.valueType )( 2.0*( w*z + x*y )), to!( Q.valueType )( 1.0 - 2.0*( y*y + z*z ))); 534 } 535 536 537 /// Returns the pitch 538 Q.valueType pitch( Q )( const ref Q q ) if ( isQuaternion!Q ) { 539 return asin( to!( Q.valueType )( 2.0*( w*y - z*x ))); 540 } 541 542 543 /// Returns the roll 544 Q.valueType roll( Q )( const ref Q q ) if ( isQuaternion!Q ) { 545 return atan2( to!( Q.valueType )( 2.0*( w*x + y*z )), to!( Q.valueType )( 1.0 - 2.0*( x*x + y*y ))); 546 } 547 548 549 unittest { 550 quat q1 = quat.identity; 551 assert( q1.pitch == 0.0f ); 552 assert( q1.yaw == 0.0f ); 553 assert( q1.roll == 0.0f ); 554 555 quat q2 = quat( 1.0f, 1.0f, 1.0f, 1.0f ); 556 assert( almostEqual( q2.yaw, q2.roll )); 557 assert( almostEqual( q2.yaw, 1.570796f )); 558 assert( q2.pitch == 0.0f ); 559 560 quat q3 = quat( 0.1f, 1.9f, 2.1f, 1.3f ); 561 assert( almostEqual( q3.yaw, 2.4382f )); 562 assert( isNaN( q3.pitch )); 563 assert( almostEqual( q3.roll, 1.67719f )); 564 565 assert(almostEqual(quat(0.0f, 0.0f, 0.0f, 0.0f), quat(0.0f, 0.0f, 0.0f, 0.0f))); 566 assert(almostEqual(quat(0.0f, 0.0f, 0.0f, 0.0f), quat(0.000001f, 0.000001f, 0.000001f, 0.000001f))); 567 568 }