/Utilities/Datatypes/Quaternion.cs
C# | 979 lines | 548 code | 90 blank | 341 comment | 20 complexity | 904220030f204be120611f8cd6a1a4ba MD5 | raw file
1using System; 2using System.Diagnostics; 3using System.Globalization; 4using System.IO; 5using System.Runtime.InteropServices; 6using Delta.Utilities.Helpers; 7using NUnit.Framework; 8 9 10namespace Delta.Utilities.Datatypes 11{ 12 /// <summary> 13 /// Quaternion, contains X, Y, Z and W values. Mostly used for cameras. 14 /// Note: Sometimes used as a replacement for Vector4 because we only have 15 /// Point for Vector2 and Vector for Vector3. 16 /// </summary> 17 [Serializable] 18 [StructLayout(LayoutKind.Explicit)] 19 [DebuggerDisplay("Quaternion(Vector=({Vector.X}, {Vector.Y}, {Vector.Z})" + 20 ", W={W})")] 21 public struct Quaternion : ISaveLoadBinary, IEquatable<Quaternion> 22 { 23 #region Constants 24 /// <summary> 25 /// Represents the size in bytes of a Quaternion (4 * 4 = 16 bytes). 26 /// </summary> 27 public const int DataSize = 4 * 4; 28 #endregion 29 30 #region Static 31 /// <summary> 32 /// Returns a identity quaternion (all values 0, except W is 1) 33 /// </summary> 34 public static readonly Quaternion Identity = 35 new Quaternion(0, 0, 0, 1); 36 #endregion 37 38 #region Framework Union Defines (Public) 39 #endregion 40 41 #region Public 42 /// <summary> 43 /// X value of the Vector. 44 /// </summary> 45 [FieldOffset(0)] 46 public float X; 47 48 /// <summary> 49 /// Y value of the Vector. 50 /// </summary> 51 [FieldOffset(4)] 52 public float Y; 53 54 /// <summary> 55 /// Z value of the Vector. 56 /// </summary> 57 [FieldOffset(8)] 58 public float Z; 59 60 /// <summary> 61 /// W value of the Vector. 62 /// </summary> 63 [FieldOffset(12)] 64 public float W; 65 66 /// <summary> 67 /// We can also use X, Y, Z as a vector (same data) 68 /// </summary> 69 [FieldOffset(0)] 70 public Vector Vector; 71 #endregion 72 73 #region Constructor 74 /// <summary> 75 /// Create quaternion 76 /// </summary> 77 /// <param name="scalarPart">scalarPart</param> 78 /// <param name="vectorPart">vectorPart</param> 79 public Quaternion(Vector vectorPart, float scalarPart) 80 : this() 81 { 82 Vector = vectorPart; 83 W = scalarPart; 84 } 85 86 /// <summary> 87 /// Create quaternion 88 /// </summary> 89 /// <param name="setW">setW</param> 90 /// <param name="setX">setX</param> 91 /// <param name="setY">setY</param> 92 /// <param name="setZ">setZ</param> 93 public Quaternion(float setX, float setY, float setZ, float setW) 94 : this() 95 { 96 X = setX; 97 Y = setY; 98 Z = setZ; 99 W = setW; 100 } 101 #endregion 102 103 #region Equals 104 /// <summary> 105 /// Check if another quaternion has the same values. 106 /// </summary> 107 /// <param name="other">Other quaternion to compare against.</param> 108 /// <returns>True if the other quaternion has the same values.</returns> 109 public bool Equals(Quaternion other) 110 { 111 return this == other; 112 } 113 114 /// <summary> 115 /// Check if another object is an quaternion and has the same values. 116 /// </summary> 117 /// <param name="obj">Other object to compare against.</param> 118 /// <returns>True if the other quaternion has the same values.</returns> 119 public override bool Equals(object obj) 120 { 121 return obj is Quaternion && 122 Equals((Quaternion)obj); 123 } 124 #endregion 125 126 #region GetHashCode 127 /// <summary> 128 /// Get hash code 129 /// </summary> 130 /// <returns>Hash code</returns> 131 public override int GetHashCode() 132 { 133 return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^ 134 W.GetHashCode(); 135 } 136 #endregion 137 138 #region ToString 139 /// <summary> 140 /// To string 141 /// </summary> 142 /// <returns>string</returns> 143 public override string ToString() 144 { 145 CultureInfo culture = CultureInfo.CurrentCulture; 146 return "{X:" + X.ToString(culture) + 147 " Y:" + Y.ToString(culture) + 148 " Z:" + Z.ToString(culture) + 149 " W:" + W.ToString(culture) + "}"; 150 } 151 #endregion 152 153 #region Conjugate 154 /// <summary> 155 /// Conjugate 156 /// </summary> 157 public void Conjugate() 158 { 159 X = -X; 160 Y = -Y; 161 Z = -Z; 162 } 163 #endregion 164 165 #region CreateFromAxisAngle 166 /// <summary> 167 /// Create from axis angle 168 /// </summary> 169 /// <param name="angle">angle</param> 170 /// <param name="axis">axis</param> 171 /// <returns>Quaternion with the rotation around the axis</returns> 172 public static Quaternion CreateFromAxisAngle(Vector axis, float angle) 173 { 174 // Create the resulting quaternion by applying the angle 175 return new Quaternion( 176 MathHelper.Sin(angle * 0.5f) * axis, 177 MathHelper.Cos(angle * 0.5f)); 178 } 179 #endregion 180 181 #region CreateFromYawPitchRoll 182 /// <summary> 183 /// Create from yaw pitch roll 184 /// </summary> 185 /// <param name="pitch">pitch</param> 186 /// <param name="roll">roll</param> 187 /// <param name="yaw">yaw</param> 188 /// <returns>Quaternion with the rotation around yaw, pitch, roll</returns> 189 public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, 190 float roll) 191 { 192 //Note: Still needs to be refactored! 193 yaw *= 0.5f; 194 pitch *= 0.5f; 195 roll *= 0.5f; 196 float rollSin = MathHelper.Sin(roll); 197 float rollCos = MathHelper.Cos(roll); 198 float pitchSin = MathHelper.Sin(pitch); 199 float pitchCos = MathHelper.Cos(pitch); 200 float yawSin = MathHelper.Sin(yaw); 201 float yawCos = MathHelper.Cos(yaw); 202 203 // create the resulting quaternion 204 Quaternion result = new Quaternion( 205 // X 206 ((yawCos * pitchSin) * rollCos) + ((yawSin * pitchCos) * rollSin), 207 // Y 208 ((yawCos * pitchCos) * rollSin) - ((yawSin * pitchSin) * rollCos), 209 // Z 210 ((yawSin * pitchCos) * rollCos) - ((yawCos * pitchSin) * rollSin), 211 // W 212 ((yawCos * pitchCos) * rollCos) + ((yawSin * pitchSin) * rollSin)); 213 return result; 214 } 215 #endregion 216 217 #region Concatenate 218 /// <summary> 219 /// Concatenate 220 /// </summary> 221 /// <param name="value1">Value 1</param> 222 /// <param name="value2">Value 2</param> 223 /// <returns>Concatenated quaternion</returns> 224 public static Quaternion Concatenate(Quaternion value1, Quaternion value2) 225 { 226 float x1 = value1.X; 227 float y1 = value1.Y; 228 float z1 = value1.Z; 229 float w1 = value1.W; 230 231 float x2 = value2.X; 232 float y2 = value2.Y; 233 float z2 = value2.Z; 234 float w2 = value2.W; 235 236 float num12 = (y2 * z1) - (z2 * y1); 237 float num11 = (z2 * x1) - (x2 * z1); 238 float num10 = (x2 * y1) - (y2 * x1); 239 float num9 = (x2 * x1) + (y2 * y1) + (z2 * z1); 240 241 // create the resulting quaternion 242 return new Quaternion( 243 // X 244 (x2 * w1) + (x1 * w2) + num12, 245 // Y 246 (y2 * w1) + (y1 * w2) + num11, 247 // Z 248 (z2 * w1) + (z1 * w2) + num10, 249 // W 250 (w2 * w1) - num9); 251 } 252 #endregion 253 254 #region CreateFromRotationMatrix 255 /// <summary> 256 /// Create from rotation matrix 257 /// </summary> 258 /// <param name="matrix">matrix</param> 259 /// <returns>Quaternion</returns> 260 public static Quaternion CreateFromRotationMatrix(Matrix matrix) 261 { 262 float num8 = matrix.M11 + matrix.M22 + matrix.M33; 263 Quaternion quaternion = new Quaternion(); 264 if (num8 > 0f) 265 { 266 float num = MathHelper.Sqrt(num8 + 1f); 267 quaternion.W = num * 0.5f; 268 num = 0.5f / num; 269 quaternion.X = (matrix.M23 - matrix.M32) * num; 270 quaternion.Y = (matrix.M31 - matrix.M13) * num; 271 quaternion.Z = (matrix.M12 - matrix.M21) * num; 272 return quaternion; 273 } 274 275 if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33)) 276 { 277 float num7 = MathHelper.Sqrt(1f + matrix.M11 - matrix.M22 - matrix.M33); 278 float num4 = 0.5f / num7; 279 quaternion.X = 0.5f * num7; 280 quaternion.Y = (matrix.M12 + matrix.M21) * num4; 281 quaternion.Z = (matrix.M13 + matrix.M31) * num4; 282 quaternion.W = (matrix.M23 - matrix.M32) * num4; 283 return quaternion; 284 } 285 286 if (matrix.M22 > matrix.M33) 287 { 288 float num6 = MathHelper.Sqrt(1f + matrix.M22 - matrix.M11 - matrix.M33); 289 float num3 = 0.5f / num6; 290 quaternion.X = (matrix.M21 + matrix.M12) * num3; 291 quaternion.Y = 0.5f * num6; 292 quaternion.Z = (matrix.M32 + matrix.M23) * num3; 293 quaternion.W = (matrix.M31 - matrix.M13) * num3; 294 return quaternion; 295 } 296 297 float num5 = MathHelper.Sqrt(1f + matrix.M33 - matrix.M11 - matrix.M22); 298 float num2 = 0.5f / num5; 299 quaternion.X = (matrix.M31 + matrix.M13) * num2; 300 quaternion.Y = (matrix.M32 + matrix.M23) * num2; 301 quaternion.Z = 0.5f * num5; 302 quaternion.W = (matrix.M12 - matrix.M21) * num2; 303 return quaternion; 304 } 305 #endregion 306 307 #region Length 308 /// <summary> 309 /// Length, also called Magnitude for Quaternions sometimes. 310 /// </summary> 311 /// <returns>Length</returns> 312 public float Length() 313 { 314 return MathHelper.Sqrt(X * X + Y * Y + Z * Z + W * W); 315 } 316 #endregion 317 318 #region LengthSquared 319 /// <summary> 320 /// Length squared, also called SquareMagnitude for Quaternions sometimes. 321 /// </summary> 322 /// <returns>Squared length</returns> 323 public float LengthSquared() 324 { 325 return X * X + Y * Y + Z * Z + W * W; 326 } 327 #endregion 328 329 #region Normalize 330 /// <summary> 331 /// Normalize this quaternion. 332 /// </summary> 333 public void Normalize() 334 { 335 float length = Length(); 336 if (length < float.Epsilon || 337 MathHelper.Abs(length - 1.0f) < float.Epsilon) 338 { 339 return; 340 } 341 342 float invertedLength = 1.0f / length; 343 Vector *= invertedLength; 344 W *= invertedLength; 345 } 346 #endregion 347 348 #region Invert 349 /// <summary> 350 /// Invert this quaternion 351 /// </summary> 352 public void Invert() 353 { 354 float lengthSquared = LengthSquared(); 355 if (lengthSquared < float.Epsilon) 356 { 357 return; 358 } 359 360 float invertedLengthSquared = -1.0f / lengthSquared; 361 Vector *= invertedLengthSquared; 362 W *= -invertedLengthSquared; 363 } 364 #endregion 365 366 #region Lerp 367 /// <summary> 368 /// Used to interpolate between two quaternions. This is not just 369 /// quat1*amount+quat2*(1-amount) because we need to rotate correctly. 370 /// </summary> 371 /// <param name="quat1">Value 1</param> 372 /// <param name="quat2">Value 2</param> 373 /// <param name="amount">Interpolation amount</param> 374 /// <returns>Interpolated quaternion</returns> 375 public static Quaternion Lerp(Quaternion quat1, Quaternion quat2, 376 float amount) 377 { 378 float cos = Dot(quat1, quat2); 379 float sin = MathHelper.Sqrt(MathHelper.Abs(1.0f - cos * cos)); 380 if (MathHelper.Abs(sin) < float.Epsilon) 381 { 382 return quat1; 383 } 384 385 float angle = MathHelper.Atan(sin, cos); 386 float invertedSin = 1.0f / sin; 387 float c0 = MathHelper.Sin((1 - amount) * angle) * invertedSin; 388 float c1 = MathHelper.Sin(amount * angle) * invertedSin; 389 return (quat1 * c0) + (quat2 * c1); 390 } 391 #endregion 392 393 #region Dot 394 /// <summary> 395 /// Dot product of two quaternions 396 /// </summary> 397 /// <param name="quaternion1">Quaternion 1</param> 398 /// <param name="quaternion2">Quaternion 2</param> 399 /// <returns>Dot product result</returns> 400 public static float Dot(Quaternion quaternion1, Quaternion quaternion2) 401 { 402 return (quaternion1.X * quaternion2.X) + 403 (quaternion1.Y * quaternion2.Y) + 404 (quaternion1.Z * quaternion2.Z) + 405 (quaternion1.W * quaternion2.W); 406 } 407 #endregion 408 409 #region Operators 410 411 #region Equation 412 /// <summary> 413 /// Operator for equality 414 /// </summary> 415 /// <param name="quaternion1">Quaternion 1</param> 416 /// <param name="quaternion2">Quaternion 2</param> 417 /// <returns>True if both quaternions are equal</returns> 418 public static bool operator ==(Quaternion quaternion1, 419 Quaternion quaternion2) 420 { 421 return (quaternion1.X == quaternion2.X) && 422 (quaternion1.Y == quaternion2.Y) && 423 (quaternion1.Z == quaternion2.Z) && 424 (quaternion1.W == quaternion2.W); 425 } 426 427 /// <summary> 428 /// Operator for inequality 429 /// </summary> 430 /// <param name="quaternion1">Quaternion 1</param> 431 /// <param name="quaternion2">Quaternion 2</param> 432 /// <returns>True if both quaternions are not equal</returns> 433 public static bool operator !=(Quaternion quaternion1, 434 Quaternion quaternion2) 435 { 436 return (quaternion1.X != quaternion2.X) || 437 (quaternion1.Y != quaternion2.Y) || 438 (quaternion1.Z != quaternion2.Z) || 439 (quaternion1.W != quaternion2.W); 440 } 441 #endregion 442 443 #region Negation 444 /// <summary> 445 /// Operator for unary negation 446 /// </summary> 447 /// <param name="value">Quaternion to negate</param> 448 /// <returns>Negated value</returns> 449 public static Quaternion operator -(Quaternion value) 450 { 451 return new Quaternion(-value.Vector, -value.W); 452 } 453 #endregion 454 455 #region Addition 456 /// <summary> 457 /// Operator for addition 458 /// </summary> 459 /// <param name="value1">Quaternion 1</param> 460 /// <param name="value2">Quaternion 2</param> 461 /// <returns>Added quaternion</returns> 462 public static Quaternion operator +(Quaternion value1, Quaternion value2) 463 { 464 return new Quaternion(value1.Vector + value2.Vector, 465 value1.W + value2.W); 466 } 467 #endregion 468 469 #region Multiply 470 /// <summary> 471 /// Operator for multiply 472 /// </summary> 473 /// <param name="value1">Quaternion 1</param> 474 /// <param name="value2">Quaternion 2</param> 475 /// <returns>Multiplied quaternion</returns> 476 public static Quaternion operator *(Quaternion value1, Quaternion value2) 477 { 478 return new Quaternion(Vector.Cross(value1.Vector, value2.Vector) + 479 (value1.W * value2.Vector) + (value1.Vector * value2.W), 480 (value1.W * value2.W) - Vector.Dot(value1.Vector, value2.Vector)); 481 } 482 483 /// <summary> 484 /// Operator for multiply 485 /// </summary> 486 /// <param name="value">Quaternion to multiply with</param> 487 /// <param name="scalar">Scalar to multiply with</param> 488 /// <returns>Multiplied quaternion</returns> 489 public static Quaternion operator *(Quaternion value, float scalar) 490 { 491 return new Quaternion(value.Vector * scalar, value.W * scalar); 492 } 493 494 /// <summary> 495 /// Operator for multiply 496 /// </summary> 497 /// <param name="scalar">Scalar to multiply with</param> 498 /// <param name="value">Quaternion to multiply with</param> 499 /// <returns>Multiplied quaternion</returns> 500 public static Quaternion operator *(float scalar, Quaternion value) 501 { 502 return new Quaternion(value.Vector * scalar, value.W * scalar); 503 } 504 #endregion 505 506 #region Division 507 /// <summary> 508 /// Operator for division 509 /// </summary> 510 /// <param name="value1">Quaternion 1</param> 511 /// <param name="value2">Quaternion 2</param> 512 /// <returns>Divided quaternion</returns> 513 public static Quaternion operator /(Quaternion value1, Quaternion value2) 514 { 515 value2.Invert(); 516 return value1 * value2; 517 } 518 519 /// <summary> 520 /// Operator for division 521 /// </summary> 522 /// <param name="value">Quaternion</param> 523 /// <param name="scalar">Scalar to divide through</param> 524 /// <returns>Divided quaternion</returns> 525 public static Quaternion operator /(Quaternion value, float scalar) 526 { 527 float invertedScalar = 1.0f / scalar; 528 return new Quaternion(value.Vector * invertedScalar, 529 value.W * invertedScalar); 530 } 531 #endregion 532 533 #region Cast operators 534 /// <summary> 535 /// Operator to explicitly convert a quaternion to a matrix. 536 /// </summary> 537 /// <param name="quat">Quaternion to convert</param> 538 /// <returns>Matrix from quaternion</returns> 539 public static explicit operator Matrix(Quaternion quat) 540 { 541 Matrix ret = Matrix.Identity; 542 // A cast to a matrix only works with normalized quaternions! 543 quat.Normalize(); 544 545 float xs = 2.0f * quat.X; 546 float ys = 2.0f * quat.Y; 547 float zs = 2.0f * quat.Z; 548 549 float wx = quat.W * xs; 550 float wy = quat.W * ys; 551 float wz = quat.W * zs; 552 float xx = quat.X * xs; 553 float xy = quat.X * ys; 554 float xz = quat.X * zs; 555 float yy = quat.Y * ys; 556 float yz = quat.Y * zs; 557 float zz = quat.Z * zs; 558 559 ret.M11 = 1.0f - yy - zz; 560 ret.M21 = xy - wz; 561 ret.M31 = xz + wy; 562 ret.M41 = 0.0f; 563 564 ret.M12 = xy + wz; 565 ret.M22 = 1.0f - xx - zz; 566 ret.M32 = yz - wx; 567 ret.M42 = 0.0f; 568 569 ret.M13 = xz - wy; 570 ret.M23 = yz + wx; 571 ret.M33 = 1.0f - xx - yy; 572 ret.M43 = 0.0f; 573 574 return ret; 575 } 576 #endregion 577 578 #endregion 579 580 #region ISaveLoadBinary Methods 581 582 #region Save 583 /// <summary> 584 /// Saves the point to a stream. 585 /// </summary> 586 /// <param name="writer">The stream that will be used.</param> 587 public void Save(BinaryWriter writer) 588 { 589 writer.Write(X); 590 writer.Write(Y); 591 writer.Write(Z); 592 writer.Write(W); 593 } 594 #endregion 595 596 #region Load 597 /// <summary> 598 /// Load the point values from a stream. 599 /// </summary> 600 /// <param name="reader">The stream that will be used.</param> 601 public void Load(BinaryReader reader) 602 { 603 X = reader.ReadSingle(); 604 Y = reader.ReadSingle(); 605 Z = reader.ReadSingle(); 606 W = reader.ReadSingle(); 607 } 608 #endregion 609 610 #endregion 611 612 /// <summary> 613 /// Tests 614 /// </summary> 615 internal class QuaternionTests 616 { 617 #region Helpers 618 /// <summary> 619 /// Length test 620 /// 621 /// Calculation: 622 /// 10 * 10 = 100 623 /// + 40 * 40 = 1600 624 /// + 4 * 4 = 16 625 /// + 2 * 2 = 4 626 /// ---------------- 627 /// _____ 628 /// -/1720' 629 /// 41.47288 630 /// </summary> 631 public static void Length() 632 { 633 Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f); 634 Assert.NearlyEqual(quat.Length(), 41.47288f); 635 } 636 #endregion 637 638 #region Equality (Static) 639 /// <summary> 640 /// Equality test 641 /// </summary> 642 [Test] 643 public static void Equality() 644 { 645 Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f); 646 Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f); 647 Quaternion quat2 = new Quaternion(new Vector(5f, 0f, 43f), 21f); 648 649 // using the Equals methods 650 Assert.True(quat.Equals(quat)); 651 Assert.True(quat.Equals(quat1)); 652 Assert.True(quat2.Equals(quat2)); 653 Assert.True(quat.Equals((object)quat)); 654 655 Assert.False(quat.Equals(quat2)); 656 Assert.False(quat.Equals(1234)); 657 658 // using the operators 659 Assert.True(quat1 == quat); 660 Assert.True(quat == quat1); 661 Assert.True(quat1 != quat2); 662 Assert.False(quat1 == quat2); 663 } 664 #endregion 665 666 #region ArithmeticOperators (Static) 667 /// <summary> 668 /// ArithmeticOperators test 669 /// </summary> 670 [Test] 671 public static void ArithmeticOperators() 672 { 673 Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f); 674 Quaternion quat2 = new Quaternion(new Vector(5f, 1f, 43f), 21f); 675 676 // Multiply with scalar 677 Quaternion result = quat1 * 3f; 678 Assert.NearlyEqual(result.X, 30f); 679 Assert.NearlyEqual(result.Y, 120f); 680 Assert.NearlyEqual(result.Z, 12f); 681 Assert.NearlyEqual(result.W, 6f); 682 result = 3f * quat1; 683 Assert.NearlyEqual(result.X, 30f); 684 Assert.NearlyEqual(result.Y, 120f); 685 Assert.NearlyEqual(result.Z, 12f); 686 Assert.NearlyEqual(result.W, 6f); 687 688 // Multiply two quaternions 689 // vec1 = Cross(Vector1, Vector2) 690 // vec1.X = (40 * 43) - ( 4 * 1) = 1716 691 // vec1.Y = ( 4 * 5) - (10 * 43) = -410 692 // vec1.Z = (10 * 1) - (40 * 5) = -190 693 // vec2 = (2 * {5, 1, 43}) = {10, 1, 86} 694 // vec3 = (21 * {10, 40, 4}) = {210, 840, 84} 695 // X = vec1.X + vec2.X + vec3.X = 1936 696 // Y = vec1.Y + vec2.Y + vec3.Y = 432 697 // Z = vec1.Z + vec2.Z + vec3.Z = -20 698 699 // W = (W1 * W2) - Dot(Vector1, Vector2) 700 // W = 42 - (10 * 5 + 40 * 1 + 4 * 43) = -220 701 result = quat1 * quat2; 702 Assert.NearlyEqual(result.X, 1936f); 703 Assert.NearlyEqual(result.Y, 432f); 704 Assert.NearlyEqual(result.Z, -20f); 705 Assert.NearlyEqual(result.W, -220); 706 707 // Negation of a quaternion 708 result = -quat1; 709 Assert.NearlyEqual(result.X, -10f); 710 Assert.NearlyEqual(result.Y, -40f); 711 Assert.NearlyEqual(result.Z, -4f); 712 Assert.NearlyEqual(result.W, -2f); 713 714 // Addition of two quternions 715 result = quat1 + quat2; 716 Assert.NearlyEqual(result.X, 15f); 717 Assert.NearlyEqual(result.Y, 41f); 718 Assert.NearlyEqual(result.Z, 47f); 719 Assert.NearlyEqual(result.W, 23f); 720 721 // Division with scalar 722 // invScalar = 1 / 4 = 0.25 723 // XYZ = {10, 40, 4} * invScalar = {2.5, 10, 1} 724 // W = 2 * invScalar = 0.5 725 result = quat1 / 4f; 726 Assert.NearlyEqual(result.X, 2.5f); 727 Assert.NearlyEqual(result.Y, 10f); 728 Assert.NearlyEqual(result.Z, 1f); 729 Assert.NearlyEqual(result.W, 0.5f); 730 } 731 #endregion 732 733 #region LengthSquared (Static) 734 /// <summary> 735 /// LengthSquared test 736 /// 737 /// Calculation: 738 /// 10 * 10 = 100 739 /// + 40 * 40 = 1600 740 /// + 4 * 4 = 16 741 /// + 2 * 2 = 4 742 /// ---------------- 743 /// 1720 744 /// </summary> 745 [Test] 746 public static void LengthSquared() 747 { 748 Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f); 749 Assert.NearlyEqual(quat.LengthSquared(), 1720); 750 } 751 #endregion 752 753 #region Normalize (Static) 754 /// <summary> 755 /// Normalize test 756 /// 757 /// Calculation: 758 /// length = 41.47288 (see test above) 759 /// invLength = 1 / 41.47288 = 0.02411 760 /// X = 10 * invLength = 0.2411 761 /// Y = 40 * invLength = 0.9644 762 /// Z = 4 * invLength = 0.09644 763 /// W = 2 * invLength = 0.04822 764 /// </summary> 765 [Test] 766 public static void Normalize() 767 { 768 Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f); 769 quat.Normalize(); 770 771 Assert.NearlyEqual(quat.X, 0.2411f); 772 Assert.NearlyEqual(quat.Y, 0.9644f); 773 Assert.NearlyEqual(quat.Z, 0.09644f); 774 Assert.NearlyEqual(quat.W, 0.04822f); 775 } 776 #endregion 777 778 #region Invert (Static) 779 /// <summary> 780 /// Invert test 781 /// 782 /// Calculation: 783 /// length = 1720 (see test above) 784 /// 785 /// invLength = -1 / 1720 = -0.0005814 786 /// X = 10 * invLength = -0.005814 787 /// Y = 40 * invLength = -0.023256 788 /// Z = 4 * invLength = -0.0023256 789 /// W = 2 * -invLength = 0.0011628 790 /// </summary> 791 [Test] 792 public static void Invert() 793 { 794 Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f); 795 quat.Invert(); 796 797 Assert.NearlyEqual(quat.X, -0.005814f); 798 Assert.NearlyEqual(quat.Y, -0.023256f); 799 Assert.NearlyEqual(quat.Z, -0.0023256f); 800 Assert.NearlyEqual(quat.W, 0.0011628f); 801 } 802 #endregion 803 804 #region Dot (Static) 805 /// <summary> 806 /// Dot test 807 /// 808 /// Calculation: 809 /// (x1 * x2) + (y1 * y2) + (z1 * z2) + (w1 * w2) 810 /// 10 * 5 = 50 811 /// + 40 * 2 = 80 812 /// + 4 * 50 = 200 813 /// + 2 * 12 = 24 814 /// --------------- 815 /// 354 816 /// </summary> 817 [Test] 818 public static void Dot() 819 { 820 Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f); 821 Quaternion quat2 = new Quaternion(new Vector(5f, 2f, 50f), 12f); 822 float dot = Quaternion.Dot(quat1, quat2); 823 824 Assert.NearlyEqual(dot, 354f); 825 } 826 #endregion 827 828 #region Lerp (Static) 829 /// <summary> 830 /// Lerp test, will just interpolate two quaternions. 831 /// </summary> 832 [Test] 833 public static void Lerp() 834 { 835 Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f); 836 Quaternion quat2 = new Quaternion(new Vector(5f, 2f, 50f), 12f); 837 838 Quaternion lerp = Quaternion.Lerp(quat1, quat2, 0.5f); 839 840 Assert.NearlyEqual(lerp.X, 0.01621542f); 841 Assert.NearlyEqual(lerp.Y, 0.04540319f); 842 Assert.NearlyEqual(lerp.Z, 0.05837553f); 843 Assert.NearlyEqual(lerp.W, 0.0151344f); 844 } 845 #endregion 846 847 #region Conjugate (Static) 848 /// <summary> 849 /// Conjugate test 850 /// </summary> 851 [Test] 852 public static void Conjugate() 853 { 854 Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f); 855 quat.Conjugate(); 856 857 Assert.NearlyEqual(quat.X, -10f); 858 Assert.NearlyEqual(quat.Y, -40f); 859 Assert.NearlyEqual(quat.Z, -4f); 860 Assert.NearlyEqual(quat.W, 2f); 861 } 862 #endregion 863 864 #region CreateFromRotationMatrix (Static) 865 /// <summary> 866 /// CreateFromRotationMatrix test 867 /// </summary> 868 [Test] 869 public static void CreateFromRotationMatrix() 870 { 871 Matrix rotMatrixX = Matrix.CreateRotationX(4f); 872 Matrix rotMatrixY = Matrix.CreateRotationY(10f); 873 Matrix rotMatrixZ = Matrix.CreateRotationZ(2f); 874 Matrix rotMatrixCombined = Matrix.Identity; 875 Matrix.Multiply(ref rotMatrixX, ref rotMatrixY, 876 ref rotMatrixCombined); 877 Matrix.Multiply(ref rotMatrixCombined, ref rotMatrixZ, 878 ref rotMatrixCombined); 879 880 Quaternion quat = 881 Quaternion.CreateFromRotationMatrix(rotMatrixCombined); 882 883 Assert.NearlyEqual(quat.X, 0.03324125f); 884 Assert.NearlyEqual(quat.Y, 0.08769615f); 885 Assert.NearlyEqual(quat.Z, 0.01433417f); 886 Assert.NearlyEqual(quat.W, 0.9954893f); 887 } 888 #endregion 889 890 #region CreateFromYawPitchRoll (Static) 891 /// <summary> 892 /// CreateFromYawPitchRoll test 893 /// 894 /// Calculation: 895 /// yaw = 40 * 0.5 = 20 896 /// pitch = 2 * 0.5 = 1 897 /// roll = 238 * 0.5 = 119 898 /// 899 /// rollSin = Sin(roll) = 0.342020 900 /// rollCos = Cos(roll) = 0.939692 901 /// pitchSin = Sin(pitch) = 0.017452 902 /// pitchCos = Cos(pitch) = 0.999847 903 /// yawSin = Sin(yaw) = 0.874619 904 /// yawCos = Cos(yaw) = -0.484809 905 /// 906 /// X = ((yawCos * pitchSin) * rollCos) + 907 /// ((yawSin * pitchCos) * rollSin) 908 /// = ((-0.484809 * 0.017452) * 0.939692) + 909 /// ((0.874619 * 0.999847) * 0.342020) 910 /// = 0.291140794875046 911 /// 912 /// Y = ((yawSin * pitchCos) * rollCos) - 913 /// ((yawCos * pitchSin) * rollSin) 914 /// = ((0.874619 * 0.999847) * 0.939692) - 915 /// ((-0.484809 * 0.017452) * 0.342020) 916 /// = 0.824640523317155 917 /// 918 /// Z = ((yawCos * pitchCos) * rollSin) - 919 /// ((yawSin * pitchSin) * rollCos) 920 /// = ((-0.484809 * 0.999847) * 0.342020) - 921 /// ((0.874619 * 0.017452) * 0.939692) 922 /// = -0.180132323055428 923 /// 924 /// W = ((yawCos * pitchCos) * rollCos) + 925 /// ((yawSin * pitchSin) * rollSin) 926 /// = ((-0.484809 * 0.999847) * 0.939692) + 927 /// ((0.874619 * 0.017452) * 0.342020) 928 /// = -0.450280894197248 929 /// </summary> 930 [Test] 931 public static void CreateFromYawPitchRoll() 932 { 933 Quaternion quat = Quaternion.CreateFromYawPitchRoll(40f, 2f, 238f); 934 935 Assert.NearlyEqual(quat.X, 0.291140f); 936 Assert.NearlyEqual(quat.Y, 0.824640f); 937 Assert.NearlyEqual(quat.Z, -0.180132f); 938 Assert.NearlyEqual(quat.W, -0.450280f); 939 } 940 #endregion 941 942 #region CreateFromAxisAngle (Static) 943 /// <summary> 944 /// CreateFromAxisAngle test 945 /// 946 /// Calculation: 947 /// sinAngle = Sin(4.5) = 0.0784590957278449 948 /// X = 45 * sinAngle = 3.53065 949 /// Y = 2 * sinAngle = 0.1569 950 /// Z = 14 * sinAngle = 1.09842 951 /// W = Cos(4.5) = 0.99691 952 /// </summary> 953 [Test] 954 public static void CreateFromAxisAngle() 955 { 956 Quaternion quat = Quaternion.CreateFromAxisAngle( 957 new Vector(45f, 2f, 14f), 9f); 958 959 Assert.NearlyEqual(quat.X, 3.53065f); 960 Assert.NearlyEqual(quat.Y, 0.1569f); 961 Assert.NearlyEqual(quat.Z, 1.09842f); 962 Assert.NearlyEqual(quat.W, 0.99691f); 963 } 964 #endregion 965 966 #region SizeOf 967 /// <summary> 968 /// Checks if the size of Point is exactly 8 bytes (2 floats: X and Y) 969 /// </summary> 970 [Test] 971 public void SizeOf() 972 { 973 // Quaternion has 4 floats: X, Y, Z, and W 974 Assert.Equal(4 * 4, Marshal.SizeOf(typeof(Quaternion))); 975 } 976 #endregion 977 } 978 } 979}