110401(DTXMania089)/SlimDXc_Jun2010(VC++2008)/source/math/Quaternion.cpp

   1 #include "stdafx.h"\r
   2 /*\r
   3 * Copyright (c) 2007-2010 SlimDX Group\r
   4 * \r
   5 * Permission is hereby granted, free of charge, to any person obtaining a copy\r
   6 * of this software and associated documentation files (the "Software"), to deal\r
   7 * in the Software without restriction, including without limitation the rights\r
   8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell\r
   9 * copies of the Software, and to permit persons to whom the Software is\r
  10 * furnished to do so, subject to the following conditions:\r
  11 * \r
  12 * The above copyright notice and this permission notice shall be included in\r
  13 * all copies or substantial portions of the Software.\r
  14 * \r
  15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r
  16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r
  17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\r
  18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r
  19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\r
  20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN\r
  21 * THE SOFTWARE.\r
  22 */\r
  23 \r
  24 #include <d3dx9.h>\r
  25 \r
  26 #include "Matrix.h"\r
  27 #include "Quaternion.h"\r
  28 #include "Vector3.h"\r
  29 \r
  30 using namespace System;\r
  31 using namespace System::Globalization;\r
  32 \r
  33 namespace SlimDX\r
  34 {\r
  35         Quaternion::Quaternion(float x, float y, float z, float w)\r
  36         {\r
  37                 X = x;\r
  38                 Y = y;\r
  39                 Z = z;\r
  40                 W = w;\r
  41         }\r
  42 \r
  43         Quaternion::Quaternion( Vector3 value, float w )\r
  44         {\r
  45                 X = value.X;\r
  46                 Y = value.Y;\r
  47                 Z = value.Z;\r
  48                 W = w;\r
  49         }\r
  50 \r
  51         Quaternion Quaternion::Identity::get()\r
  52         {\r
  53                 Quaternion result;\r
  54                 result.X = 0.0f;\r
  55                 result.Y = 0.0f;\r
  56                 result.Z = 0.0f;\r
  57                 result.W = 1.0f;\r
  58                 return result;\r
  59         }\r
  60 \r
  61         bool Quaternion::IsIdentity::get()\r
  62         {\r
  63                 if( X != 0.0f || Y != 0.0f || Z != 0.0f )\r
  64                         return false;\r
  65 \r
  66                 return (W == 1.0f);\r
  67         }\r
  68 \r
  69         Vector3 Quaternion::Axis::get()\r
  70         {\r
  71                 pin_ptr<Quaternion> pinThis = this;\r
  72                 float angle;\r
  73                 Vector3 axis;\r
  74 \r
  75                 D3DXQuaternionToAxisAngle( (D3DXQUATERNION*) pinThis, (D3DXVECTOR3*) &axis, &angle );\r
  76                 return axis;\r
  77         }\r
  78 \r
  79         float Quaternion::Angle::get()\r
  80         {\r
  81                 pin_ptr<Quaternion> pinThis = this;\r
  82                 float angle;\r
  83                 Vector3 axis;\r
  84 \r
  85                 D3DXQuaternionToAxisAngle( (D3DXQUATERNION*) pinThis, (D3DXVECTOR3*) &axis, &angle );\r
  86                 return angle;\r
  87         }\r
  88 \r
  89         float Quaternion::Length()\r
  90         {\r
  91                 return static_cast<float>( Math::Sqrt( (X * X) + (Y * Y) + (Z * Z) + (W * W) ) );\r
  92         }\r
  93         \r
  94         float Quaternion::LengthSquared()\r
  95         {\r
  96                 return (X * X) + (Y * Y) + (Z * Z) + (W * W);\r
  97         }\r
  98 \r
  99         void Quaternion::Normalize()\r
 100         {\r
 101                 float length = 1.0f / Length();\r
 102                 X *= length;\r
 103                 Y *= length;\r
 104                 Z *= length;\r
 105                 W *= length;\r
 106         }\r
 107 \r
 108         void Quaternion::Conjugate()\r
 109         {\r
 110                 X = -X;\r
 111                 Y = -Y;\r
 112                 Z = -Z;\r
 113         }\r
 114 \r
 115         void Quaternion::Invert()\r
 116         {\r
 117                 float lengthSq = 1.0f / ( (X * X) + (Y * Y) + (Z * Z) + (W * W) );\r
 118                 X = -X * lengthSq;\r
 119                 Y = -Y * lengthSq;\r
 120                 Z = -Z * lengthSq;\r
 121                 W = W * lengthSq;\r
 122         }\r
 123 \r
 124         Quaternion Quaternion::Add( Quaternion left, Quaternion right )\r
 125         {\r
 126                 Quaternion result;\r
 127                 result.X = left.X + right.X;\r
 128                 result.Y = left.Y + right.Y;\r
 129                 result.Z = left.Z + right.Z;\r
 130                 result.W = left.W + right.W;\r
 131                 return result;\r
 132         }\r
 133 \r
 134         void Quaternion::Add( Quaternion% left, Quaternion% right, [Out] Quaternion% result )\r
 135         {\r
 136                 Quaternion r;\r
 137                 r.X = left.X + right.X;\r
 138                 r.Y = left.Y + right.Y;\r
 139                 r.Z = left.Z + right.Z;\r
 140                 r.W = left.W + right.W;\r
 141 \r
 142                 result = r;\r
 143         }\r
 144 \r
 145         Quaternion Quaternion::Barycentric( Quaternion q1, Quaternion q2, Quaternion q3, float f, float g )\r
 146         {\r
 147                 Quaternion result;\r
 148 \r
 149                 D3DXQuaternionBaryCentric( (D3DXQUATERNION*) &result, (D3DXQUATERNION*) &q1,\r
 150                         (D3DXQUATERNION*) &q2, (D3DXQUATERNION*) &q3, f, g );\r
 151 \r
 152                 return result;\r
 153         }\r
 154 \r
 155         void Quaternion::Barycentric( Quaternion% q1, Quaternion% q2, Quaternion% q3, float f, float g, [Out] Quaternion% result )\r
 156         {\r
 157                 pin_ptr<Quaternion> pin1 = &q1;\r
 158                 pin_ptr<Quaternion> pin2 = &q2;\r
 159                 pin_ptr<Quaternion> pin3 = &q3;\r
 160                 pin_ptr<Quaternion> pinResult = &result;\r
 161 \r
 162                 D3DXQuaternionBaryCentric( (D3DXQUATERNION*) pinResult, (D3DXQUATERNION*) pin1,\r
 163                         (D3DXQUATERNION*) pin2, (D3DXQUATERNION*) pin3, f, g );\r
 164         }\r
 165 \r
 166         Quaternion Quaternion::Conjugate( Quaternion quat )\r
 167         {\r
 168                 Quaternion result;\r
 169                 result.X = -quat.X;\r
 170                 result.Y = -quat.Y;\r
 171                 result.Z = -quat.Z;\r
 172                 result.W = quat.W;\r
 173                 return result;\r
 174         }\r
 175 \r
 176         void Quaternion::Conjugate( Quaternion% quat, [Out] Quaternion% result )\r
 177         {\r
 178                 result.X = -quat.X;\r
 179                 result.Y = -quat.Y;\r
 180                 result.Z = -quat.Z;\r
 181                 result.W = quat.W;\r
 182         }\r
 183 \r
 184         Quaternion Quaternion::Divide( Quaternion left, Quaternion right )\r
 185         {\r
 186                 Quaternion result;\r
 187                 result.X = left.X / right.X;\r
 188                 result.Y = left.Y / right.Y;\r
 189                 result.Z = left.Z / right.Z;\r
 190                 result.W = left.W / right.W;\r
 191                 return result;\r
 192         }\r
 193 \r
 194         void Quaternion::Divide( Quaternion% left, Quaternion% right, [Out] Quaternion% result )\r
 195         {\r
 196                 result.X = left.X / right.X;\r
 197                 result.Y = left.Y / right.Y;\r
 198                 result.Z = left.Z / right.Z;\r
 199                 result.W = left.W / right.W;\r
 200         }\r
 201         \r
 202         float Quaternion::Dot( Quaternion left, Quaternion right )\r
 203         {\r
 204                 return (left.X * right.X) + (left.Y * right.Y) + (left.Z * right.Z) + (left.W * right.W); \r
 205         }\r
 206 \r
 207         Quaternion Quaternion::Exponential( Quaternion quat )\r
 208         {\r
 209                 Quaternion result;\r
 210                 D3DXQuaternionExp( (D3DXQUATERNION*) &result, (D3DXQUATERNION*) &quat );\r
 211                 return result;\r
 212         }\r
 213 \r
 214         void Quaternion::Exponential( Quaternion% quat, [Out] Quaternion% result )\r
 215         {\r
 216                 pin_ptr<Quaternion> pinQuat = &quat;\r
 217                 pin_ptr<Quaternion> pinResult = &result;\r
 218 \r
 219                 D3DXQuaternionExp( (D3DXQUATERNION*) pinResult, (D3DXQUATERNION*) pinQuat );\r
 220         }\r
 221 \r
 222         Quaternion Quaternion::Invert( Quaternion quaternion )\r
 223         {\r
 224                 Quaternion result;\r
 225                 float lengthSq = 1.0f / ( (quaternion.X * quaternion.X) + (quaternion.Y * quaternion.Y) + (quaternion.Z * quaternion.Z) + (quaternion.W * quaternion.W) );\r
 226 \r
 227                 result.X = -quaternion.X * lengthSq;\r
 228                 result.Y = -quaternion.Y * lengthSq;\r
 229                 result.Z = -quaternion.Z * lengthSq;\r
 230                 result.W = quaternion.W * lengthSq;\r
 231 \r
 232                 return result;\r
 233         }\r
 234 \r
 235         void Quaternion::Invert( Quaternion% quaternion, [Out] Quaternion% result )\r
 236         {\r
 237                 float lengthSq = 1.0f / ( (quaternion.X * quaternion.X) + (quaternion.Y * quaternion.Y) + (quaternion.Z * quaternion.Z) + (quaternion.W * quaternion.W) );\r
 238 \r
 239                 result.X = -quaternion.X * lengthSq;\r
 240                 result.Y = -quaternion.Y * lengthSq;\r
 241                 result.Z = -quaternion.Z * lengthSq;\r
 242                 result.W = quaternion.W * lengthSq;\r
 243         }\r
 244 \r
 245         Quaternion Quaternion::Lerp( Quaternion left, Quaternion right, float amount )\r
 246         {\r
 247                 Quaternion result;\r
 248                 float inverse = 1.0f - amount;\r
 249                 float dot = (left.X * right.X) + (left.Y * right.Y) + (left.Z * right.Z) + (left.W * right.W); \r
 250 \r
 251                 if( dot >= 0.0f )\r
 252                 {\r
 253                         result.X = (inverse * left.X) + (amount * right.X);\r
 254                         result.Y = (inverse * left.Y) + (amount * right.Y);\r
 255                         result.Z = (inverse * left.Z) + (amount * right.Z);\r
 256                         result.W = (inverse * left.W) + (amount * right.W);\r
 257                 }\r
 258                 else\r
 259                 {\r
 260                         result.X = (inverse * left.X) - (amount * right.X);\r
 261                         result.Y = (inverse * left.Y) - (amount * right.Y);\r
 262                         result.Z = (inverse * left.Z) - (amount * right.Z);\r
 263                         result.W = (inverse * left.W) - (amount * right.W);\r
 264                 }\r
 265 \r
 266                 float invLength = 1.0f / result.Length();\r
 267 \r
 268                 result.X *= invLength;\r
 269                 result.Y *= invLength;\r
 270                 result.Z *= invLength;\r
 271                 result.W *= invLength;\r
 272 \r
 273                 return result;\r
 274         }\r
 275 \r
 276         void Quaternion::Lerp( Quaternion% left, Quaternion% right, float amount, [Out] Quaternion% result )\r
 277         {\r
 278                 float inverse = 1.0f - amount;\r
 279                 float dot = (left.X * right.X) + (left.Y * right.Y) + (left.Z * right.Z) + (left.W * right.W); \r
 280 \r
 281                 if( dot >= 0.0f )\r
 282                 {\r
 283                         result.X = (inverse * left.X) + (amount * right.X);\r
 284                         result.Y = (inverse * left.Y) + (amount * right.Y);\r
 285                         result.Z = (inverse * left.Z) + (amount * right.Z);\r
 286                         result.W = (inverse * left.W) + (amount * right.W);\r
 287                 }\r
 288                 else\r
 289                 {\r
 290                         result.X = (inverse * left.X) - (amount * right.X);\r
 291                         result.Y = (inverse * left.Y) - (amount * right.Y);\r
 292                         result.Z = (inverse * left.Z) - (amount * right.Z);\r
 293                         result.W = (inverse * left.W) - (amount * right.W);\r
 294                 }\r
 295 \r
 296                 float invLength = 1.0f / result.Length();\r
 297 \r
 298                 result.X *= invLength;\r
 299                 result.Y *= invLength;\r
 300                 result.Z *= invLength;\r
 301                 result.W *= invLength;\r
 302         }\r
 303 \r
 304         Quaternion Quaternion::Logarithm( Quaternion quat )\r
 305         {\r
 306                 Quaternion result;\r
 307                 D3DXQuaternionLn( (D3DXQUATERNION*) &result, (D3DXQUATERNION*) &quat );\r
 308                 return result;\r
 309         }\r
 310 \r
 311         void Quaternion::Logarithm( Quaternion% quat, [Out] Quaternion% result )\r
 312         {\r
 313                 pin_ptr<Quaternion> pinQuat = &quat;\r
 314                 pin_ptr<Quaternion> pinResult = &result;\r
 315 \r
 316                 D3DXQuaternionLn( (D3DXQUATERNION*) pinResult, (D3DXQUATERNION*) pinQuat );\r
 317         }\r
 318 \r
 319         Quaternion Quaternion::Multiply( Quaternion left, Quaternion right )\r
 320         {\r
 321                 Quaternion quaternion;\r
 322                 float lx = left.X;\r
 323         float ly = left.Y;\r
 324         float lz = left.Z;\r
 325         float lw = left.W;\r
 326         float rx = right.X;\r
 327         float ry = right.Y;\r
 328         float rz = right.Z;\r
 329         float rw = right.W;\r
 330 \r
 331         quaternion.X = (rx * lw + lx * rw + ry * lz) - (rz * ly);\r
 332         quaternion.Y = (ry * lw + ly * rw + rz * lx) - (rx * lz);\r
 333         quaternion.Z = (rz * lw + lz * rw + rx * ly) - (ry * lx);\r
 334         quaternion.W = (rw * lw) - (rx * lx + ry * ly + rz * lz);\r
 335 \r
 336                 return quaternion;\r
 337         }\r
 338 \r
 339         void Quaternion::Multiply( Quaternion% left, Quaternion% right, [Out] Quaternion% result )\r
 340         {\r
 341                 float lx = left.X;\r
 342         float ly = left.Y;\r
 343         float lz = left.Z;\r
 344         float lw = left.W;\r
 345         float rx = right.X;\r
 346         float ry = right.Y;\r
 347         float rz = right.Z;\r
 348         float rw = right.W;\r
 349 \r
 350         result.X = (rx * lw + lx * rw + ry * lz) - (rz * ly);\r
 351         result.Y = (ry * lw + ly * rw + rz * lx) - (rx * lz);\r
 352         result.Z = (rz * lw + lz * rw + rx * ly) - (ry * lx);\r
 353         result.W = (rw * lw) - (rx * lx + ry * ly + rz * lz);\r
 354         }\r
 355 \r
 356         Quaternion Quaternion::Multiply( Quaternion quaternion, float scale )\r
 357         {\r
 358                 Quaternion result;\r
 359                 result.X = quaternion.X * scale;\r
 360                 result.Y = quaternion.Y * scale;\r
 361                 result.Z = quaternion.Z * scale;\r
 362                 result.W = quaternion.W * scale;\r
 363                 return result;\r
 364         }\r
 365 \r
 366         void Quaternion::Multiply( Quaternion% quaternion, float scale, [Out] Quaternion% result )\r
 367         {\r
 368                 result.X = quaternion.X * scale;\r
 369                 result.Y = quaternion.Y * scale;\r
 370                 result.Z = quaternion.Z * scale;\r
 371                 result.W = quaternion.W * scale;\r
 372         }\r
 373 \r
 374         Quaternion Quaternion::Negate( Quaternion quat )\r
 375         {\r
 376                 Quaternion result;\r
 377                 result.X = -quat.X;\r
 378                 result.Y = -quat.Y;\r
 379                 result.Z = -quat.Z;\r
 380                 result.W = -quat.W;\r
 381                 return result;\r
 382         }\r
 383 \r
 384         void Quaternion::Negate( Quaternion% quat, [Out] Quaternion% result )\r
 385         {\r
 386                 result.X = -quat.X;\r
 387                 result.Y = -quat.Y;\r
 388                 result.Z = -quat.Z;\r
 389                 result.W = -quat.W;\r
 390         }\r
 391 \r
 392         Quaternion Quaternion::Normalize( Quaternion quat )\r
 393         {\r
 394                 quat.Normalize();\r
 395                 return quat;\r
 396         }\r
 397 \r
 398         void Quaternion::Normalize( Quaternion% quat, [Out] Quaternion% result )\r
 399         {\r
 400                 float length = 1.0f / quat.Length();\r
 401                 result.X = quat.X * length;\r
 402                 result.Y = quat.Y * length;\r
 403                 result.Z = quat.Z * length;\r
 404                 result.W = quat.W * length;\r
 405         }\r
 406 \r
 407         Quaternion Quaternion::RotationAxis( Vector3 axis, float angle )\r
 408         {\r
 409                 Quaternion result;\r
 410 \r
 411                 Vector3::Normalize( axis, axis );\r
 412 \r
 413                 float half = angle * 0.5f;\r
 414                 float sin = static_cast<float>( Math::Sin( static_cast<double>( half ) ) );\r
 415                 float cos = static_cast<float>( Math::Cos( static_cast<double>( half ) ) );\r
 416 \r
 417                 result.X = axis.X * sin;\r
 418                 result.Y = axis.Y * sin;\r
 419                 result.Z = axis.Z * sin;\r
 420                 result.W = cos;\r
 421 \r
 422                 return result;\r
 423         }\r
 424 \r
 425         void Quaternion::RotationAxis( Vector3% axis, float angle, [Out] Quaternion% result )\r
 426         {\r
 427                 Vector3::Normalize( axis, axis );\r
 428 \r
 429                 float half = angle * 0.5f;\r
 430                 float sin = static_cast<float>( Math::Sin( static_cast<double>( half ) ) );\r
 431                 float cos = static_cast<float>( Math::Cos( static_cast<double>( half ) ) );\r
 432 \r
 433                 result.X = axis.X * sin;\r
 434                 result.Y = axis.Y * sin;\r
 435                 result.Z = axis.Z * sin;\r
 436                 result.W = cos;\r
 437         }\r
 438         \r
 439         Quaternion Quaternion::RotationMatrix( Matrix matrix )\r
 440         {\r
 441                 Quaternion result;\r
 442                 float scale = matrix.M11 + matrix.M22 + matrix.M33;\r
 443 \r
 444                 if( scale > 0.0f )\r
 445                 {\r
 446                         float sqrt = static_cast<float>( Math::Sqrt( static_cast<double>(scale + 1.0f) ) );\r
 447 \r
 448                         result.W = sqrt * 0.5f;\r
 449                         sqrt = 0.5f / sqrt;\r
 450 \r
 451                         result.X = (matrix.M23 - matrix.M32) * sqrt;\r
 452                         result.Y = (matrix.M31 - matrix.M13) * sqrt;\r
 453                         result.Z = (matrix.M12 - matrix.M21) * sqrt;\r
 454 \r
 455                         return result;\r
 456                 }\r
 457 \r
 458                 if( (matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33) )\r
 459                 {\r
 460                         float sqrt = static_cast<float>( Math::Sqrt( static_cast<double>(1.0f + matrix.M11 - matrix.M22 - matrix.M33) ) );\r
 461                         float half = 0.5f / sqrt;\r
 462 \r
 463                         result.X = 0.5f * sqrt;\r
 464                         result.Y = (matrix.M12 + matrix.M21) * half;\r
 465                         result.Z = (matrix.M13 + matrix.M31) * half;\r
 466                         result.W = (matrix.M23 - matrix.M32) * half;\r
 467 \r
 468                         return result;\r
 469                 }\r
 470 \r
 471                 if( matrix.M22 > matrix.M33 )\r
 472                 {\r
 473                         float sqrt = static_cast<float>( Math::Sqrt( static_cast<double>(1.0f + matrix.M22 - matrix.M11 - matrix.M33) ) );\r
 474                         float half = 0.5f / sqrt;\r
 475 \r
 476                         result.X = (matrix.M21 + matrix.M12) * half;\r
 477                         result.Y = 0.5f * sqrt;\r
 478                         result.Z = (matrix.M32 + matrix.M23) * half;\r
 479                         result.W = (matrix.M31 - matrix.M13) * half;\r
 480 \r
 481                         return result;\r
 482                 }\r
 483 \r
 484                 float sqrt = static_cast<float>( Math::Sqrt( static_cast<double>(1.0f + matrix.M33 - matrix.M11 - matrix.M22) ) );\r
 485                 float half = 0.5f / sqrt;\r
 486 \r
 487                 result.X = (matrix.M31 + matrix.M13) * half;\r
 488                 result.Y = (matrix.M32 + matrix.M23) * half;\r
 489                 result.Z = 0.5f * sqrt;\r
 490                 result.W = (matrix.M12 - matrix.M21) * half;\r
 491 \r
 492                 return result;\r
 493         }\r
 494 \r
 495         void Quaternion::RotationMatrix( Matrix% matrix, [Out] Quaternion% result )\r
 496         {\r
 497                 float scale = matrix.M11 + matrix.M22 + matrix.M33;\r
 498 \r
 499                 if( scale > 0.0f )\r
 500                 {\r
 501                         float sqrt = static_cast<float>( Math::Sqrt( static_cast<double>(scale + 1.0f) ) );\r
 502 \r
 503                         result.W = sqrt * 0.5f;\r
 504                         sqrt = 0.5f / sqrt;\r
 505 \r
 506                         result.X = (matrix.M23 - matrix.M32) * sqrt;\r
 507                         result.Y = (matrix.M31 - matrix.M13) * sqrt;\r
 508                         result.Z = (matrix.M12 - matrix.M21) * sqrt;\r
 509                         return;\r
 510                 }\r
 511 \r
 512                 if( (matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33) )\r
 513                 {\r
 514                         float sqrt = static_cast<float>( Math::Sqrt( static_cast<double>(1.0f + matrix.M11 - matrix.M22 - matrix.M33) ) );\r
 515                         float half = 0.5f / sqrt;\r
 516 \r
 517                         result.X = 0.5f * sqrt;\r
 518                         result.Y = (matrix.M12 + matrix.M21) * half;\r
 519                         result.Z = (matrix.M13 + matrix.M31) * half;\r
 520                         result.W = (matrix.M23 - matrix.M32) * half;\r
 521                         return;\r
 522                 }\r
 523 \r
 524                 if( matrix.M22 > matrix.M33 )\r
 525                 {\r
 526                         float sqrt = static_cast<float>( Math::Sqrt( static_cast<double>(1.0f + matrix.M22 - matrix.M11 - matrix.M33) ) );\r
 527                         float half = 0.5f / sqrt;\r
 528 \r
 529                         result.X = (matrix.M21 + matrix.M12) * half;\r
 530                         result.Y = 0.5f * sqrt;\r
 531                         result.Z = (matrix.M32 + matrix.M23) * half;\r
 532                         result.W = (matrix.M31 - matrix.M13) * half;\r
 533                         return;\r
 534                 }\r
 535 \r
 536                 float sqrt = static_cast<float>( Math::Sqrt( static_cast<double>(1.0f + matrix.M33 - matrix.M11 - matrix.M22) ) );\r
 537                 float half = 0.5f / sqrt;\r
 538 \r
 539                 result.X = (matrix.M31 + matrix.M13) * half;\r
 540                 result.Y = (matrix.M32 + matrix.M23) * half;\r
 541                 result.Z = 0.5f * sqrt;\r
 542                 result.W = (matrix.M12 - matrix.M21) * half;\r
 543         }\r
 544 \r
 545         Quaternion Quaternion::RotationYawPitchRoll( float yaw, float pitch, float roll )\r
 546         {\r
 547                 Quaternion result;\r
 548 \r
 549                 float halfRoll = roll * 0.5f;\r
 550                 float sinRoll = static_cast<float>( Math::Sin( static_cast<double>( halfRoll ) ) );\r
 551                 float cosRoll = static_cast<float>( Math::Cos( static_cast<double>( halfRoll ) ) );\r
 552                 float halfPitch = pitch * 0.5f;\r
 553                 float sinPitch = static_cast<float>( Math::Sin( static_cast<double>( halfPitch ) ) );\r
 554                 float cosPitch = static_cast<float>( Math::Cos( static_cast<double>( halfPitch ) ) );\r
 555                 float halfYaw = yaw * 0.5f;\r
 556                 float sinYaw = static_cast<float>( Math::Sin( static_cast<double>( halfYaw ) ) );\r
 557                 float cosYaw = static_cast<float>( Math::Cos( static_cast<double>( halfYaw ) ) );\r
 558 \r
 559                 result.X = (cosYaw * sinPitch * cosRoll) + (sinYaw * cosPitch * sinRoll);\r
 560                 result.Y = (sinYaw * cosPitch * cosRoll) - (cosYaw * sinPitch * sinRoll);\r
 561                 result.Z = (cosYaw * cosPitch * sinRoll) - (sinYaw * sinPitch * cosRoll);\r
 562                 result.W = (cosYaw * cosPitch * cosRoll) + (sinYaw * sinPitch * sinRoll);\r
 563 \r
 564                 return result;\r
 565         }\r
 566 \r
 567         void Quaternion::RotationYawPitchRoll( float yaw, float pitch, float roll, [Out] Quaternion% result )\r
 568         {\r
 569                 float halfRoll = roll * 0.5f;\r
 570                 float sinRoll = static_cast<float>( Math::Sin( static_cast<double>( halfRoll ) ) );\r
 571                 float cosRoll = static_cast<float>( Math::Cos( static_cast<double>( halfRoll ) ) );\r
 572                 float halfPitch = pitch * 0.5f;\r
 573                 float sinPitch = static_cast<float>( Math::Sin( static_cast<double>( halfPitch ) ) );\r
 574                 float cosPitch = static_cast<float>( Math::Cos( static_cast<double>( halfPitch ) ) );\r
 575                 float halfYaw = yaw * 0.5f;\r
 576                 float sinYaw = static_cast<float>( Math::Sin( static_cast<double>( halfYaw ) ) );\r
 577                 float cosYaw = static_cast<float>( Math::Cos( static_cast<double>( halfYaw ) ) );\r
 578 \r
 579                 result.X = (cosYaw * sinPitch * cosRoll) + (sinYaw * cosPitch * sinRoll);\r
 580                 result.Y = (sinYaw * cosPitch * cosRoll) - (cosYaw * sinPitch * sinRoll);\r
 581                 result.Z = (cosYaw * cosPitch * sinRoll) - (sinYaw * sinPitch * cosRoll);\r
 582                 result.W = (cosYaw * cosPitch * cosRoll) + (sinYaw * sinPitch * sinRoll);\r
 583         }\r
 584 \r
 585         Quaternion Quaternion::Slerp( Quaternion q1, Quaternion q2, float t )\r
 586         {\r
 587                 Quaternion result;\r
 588 \r
 589                 float opposite;\r
 590                 float inverse;\r
 591                 float dot = (q1.X * q2.X) + (q1.Y * q2.Y) + (q1.Z * q2.Z) + (q1.W * q2.W);\r
 592                 bool flag = false;\r
 593 \r
 594                 if( dot < 0.0f )\r
 595                 {\r
 596                         flag = true;\r
 597                         dot = -dot;\r
 598                 }\r
 599 \r
 600                 if( dot > 0.999999f )\r
 601                 {\r
 602                         inverse = 1.0f - t;\r
 603                         opposite = flag ? -t : t;\r
 604                 }\r
 605                 else\r
 606                 {\r
 607                         float acos = static_cast<float>( Math::Acos( static_cast<double>( dot ) ) );\r
 608                         float invSin = static_cast<float>( ( 1.0f / Math::Sin( static_cast<double>( acos ) ) ) );\r
 609 \r
 610                         inverse = ( static_cast<float>( Math::Sin( static_cast<double>( (1.0f - t) * acos ) ) ) ) * invSin;\r
 611                         opposite = flag ? ( ( static_cast<float>( -Math::Sin( static_cast<double>( t * acos ) ) ) ) * invSin ) : ( ( static_cast<float>( Math::Sin( static_cast<double>( t * acos ) ) ) ) * invSin );\r
 612                 }\r
 613 \r
 614                 result.X = (inverse * q1.X) + (opposite * q2.X);\r
 615                 result.Y = (inverse * q1.Y) + (opposite * q2.Y);\r
 616                 result.Z = (inverse * q1.Z) + (opposite * q2.Z);\r
 617                 result.W = (inverse * q1.W) + (opposite * q2.W);\r
 618 \r
 619                 return result;\r
 620         }\r
 621 \r
 622         void Quaternion::Slerp( Quaternion% q1, Quaternion% q2, float t, [Out] Quaternion% result )\r
 623         {\r
 624                 float opposite;\r
 625                 float inverse;\r
 626                 float dot = (q1.X * q2.X) + (q1.Y * q2.Y) + (q1.Z * q2.Z) + (q1.W * q2.W);\r
 627                 bool flag = false;\r
 628 \r
 629                 if( dot < 0.0f )\r
 630                 {\r
 631                         flag = true;\r
 632                         dot = -dot;\r
 633                 }\r
 634 \r
 635                 if( dot > 0.999999f )\r
 636                 {\r
 637                         inverse = 1.0f - t;\r
 638                         opposite = flag ? -t : t;\r
 639                 }\r
 640                 else\r
 641                 {\r
 642                         float acos = static_cast<float>( Math::Acos( static_cast<double>( dot ) ) );\r
 643                         float invSin = static_cast<float>( ( 1.0f / Math::Sin( static_cast<double>( acos ) ) ) );\r
 644 \r
 645                         inverse = ( static_cast<float>( Math::Sin( static_cast<double>( (1.0f - t) * acos ) ) ) ) * invSin;\r
 646                         opposite = flag ? ( ( static_cast<float>( -Math::Sin( static_cast<double>( t * acos ) ) ) ) * invSin ) : ( ( static_cast<float>( Math::Sin( static_cast<double>( t * acos ) ) ) ) * invSin );\r
 647                 }\r
 648 \r
 649                 result.X = (inverse * q1.X) + (opposite * q2.X);\r
 650                 result.Y = (inverse * q1.Y) + (opposite * q2.Y);\r
 651                 result.Z = (inverse * q1.Z) + (opposite * q2.Z);\r
 652                 result.W = (inverse * q1.W) + (opposite * q2.W);\r
 653         }\r
 654 \r
 655         Quaternion Quaternion::Squad( Quaternion q1, Quaternion a, Quaternion b, Quaternion c, float t )\r
 656         {\r
 657                 Quaternion result;\r
 658 \r
 659                 D3DXQuaternionSquad( (D3DXQUATERNION*) &result, (D3DXQUATERNION*) &q1, (D3DXQUATERNION*) &a,\r
 660                         (D3DXQUATERNION*) &b, (D3DXQUATERNION*) &c, t );\r
 661 \r
 662                 return result;\r
 663         }\r
 664 \r
 665         void Quaternion::Squad( Quaternion% q1, Quaternion% a, Quaternion% b, Quaternion% c, float t, [Out] Quaternion% result )\r
 666         {\r
 667                 pin_ptr<Quaternion> pin1 = &q1;\r
 668                 pin_ptr<Quaternion> pinA = &a;\r
 669                 pin_ptr<Quaternion> pinB = &b;\r
 670                 pin_ptr<Quaternion> pinC = &c;\r
 671                 pin_ptr<Quaternion> pinResult = &result;\r
 672 \r
 673                 D3DXQuaternionSquad( (D3DXQUATERNION*) pinResult, (D3DXQUATERNION*) pin1, (D3DXQUATERNION*) pinA,\r
 674                         (D3DXQUATERNION*) pinB, (D3DXQUATERNION*) pinC, t );\r
 675         }\r
 676 \r
 677         array<Quaternion>^ Quaternion::SquadSetup( Quaternion source1, Quaternion source2, Quaternion source3, Quaternion source4 )\r
 678         {\r
 679                 Quaternion result1;\r
 680                 Quaternion result2;\r
 681                 Quaternion result3;\r
 682                 array<Quaternion>^ results = gcnew array<Quaternion>( 3 );\r
 683 \r
 684                 D3DXQuaternionSquadSetup( (D3DXQUATERNION*) &result1, (D3DXQUATERNION*) &result2, (D3DXQUATERNION*) &result3,\r
 685                         (D3DXQUATERNION*) &source1, (D3DXQUATERNION*) &source2, (D3DXQUATERNION*) &source3, (D3DXQUATERNION*) &source4 );\r
 686 \r
 687                 results[0] = result1;\r
 688                 results[1] = result2;\r
 689                 results[2] = result3;\r
 690                 return results;\r
 691         }\r
 692 \r
 693         Quaternion Quaternion::Subtract( Quaternion left, Quaternion right )\r
 694         {\r
 695                 Quaternion result;\r
 696                 result.X = left.X - right.X;\r
 697                 result.Y = left.Y - right.Y;\r
 698                 result.Z = left.Z - right.Z;\r
 699                 result.W = left.W - right.W;\r
 700                 return result;\r
 701         }\r
 702 \r
 703         void Quaternion::Subtract( Quaternion% left, Quaternion% right, [Out] Quaternion% result )\r
 704         {\r
 705                 result.X = left.X - right.X;\r
 706                 result.Y = left.Y - right.Y;\r
 707                 result.Z = left.Z - right.Z;\r
 708                 result.W = left.W - right.W;\r
 709         }\r
 710 \r
 711         Quaternion Quaternion::operator * (Quaternion left, Quaternion right)\r
 712         {\r
 713                 Quaternion quaternion;\r
 714                 float lx = left.X;\r
 715         float ly = left.Y;\r
 716         float lz = left.Z;\r
 717         float lw = left.W;\r
 718         float rx = right.X;\r
 719         float ry = right.Y;\r
 720         float rz = right.Z;\r
 721         float rw = right.W;\r
 722 \r
 723         quaternion.X = (rx * lw + lx * rw + ry * lz) - (rz * ly);\r
 724         quaternion.Y = (ry * lw + ly * rw + rz * lx) - (rx * lz);\r
 725         quaternion.Z = (rz * lw + lz * rw + rx * ly) - (ry * lx);\r
 726         quaternion.W = (rw * lw) - (rx * lx + ry * ly + rz * lz);\r
 727 \r
 728                 return quaternion;\r
 729         }\r
 730 \r
 731         Quaternion Quaternion::operator * (Quaternion quaternion, float scale)\r
 732         {\r
 733                 Quaternion result;\r
 734                 result.X = quaternion.X * scale;\r
 735                 result.Y = quaternion.Y * scale;\r
 736                 result.Z = quaternion.Z * scale;\r
 737                 result.W = quaternion.W * scale;\r
 738                 return result;\r
 739         }\r
 740 \r
 741         Quaternion Quaternion::operator * (float scale, Quaternion quaternion)\r
 742         {\r
 743                 Quaternion result;\r
 744                 result.X = quaternion.X * scale;\r
 745                 result.Y = quaternion.Y * scale;\r
 746                 result.Z = quaternion.Z * scale;\r
 747                 result.W = quaternion.W * scale;\r
 748                 return result;\r
 749         }\r
 750 \r
 751         Quaternion Quaternion::operator / (Quaternion lhs, float rhs)\r
 752         {\r
 753                 Quaternion result;\r
 754                 result.X = lhs.X / rhs;\r
 755                 result.Y = lhs.Y / rhs;\r
 756                 result.Z = lhs.Z / rhs;\r
 757                 result.W = lhs.W / rhs;\r
 758                 return result;\r
 759         }\r
 760 \r
 761         Quaternion Quaternion::operator + (Quaternion lhs, Quaternion rhs)\r
 762         {\r
 763                 Quaternion result;\r
 764                 result.X = lhs.X + rhs.X;\r
 765                 result.Y = lhs.Y + rhs.Y;\r
 766                 result.Z = lhs.Z + rhs.Z;\r
 767                 result.W = lhs.W + rhs.W;\r
 768                 return result;\r
 769         }\r
 770 \r
 771         Quaternion Quaternion::operator - (Quaternion lhs, Quaternion rhs)\r
 772         {\r
 773                 Quaternion result;\r
 774                 result.X = lhs.X - rhs.X;\r
 775                 result.Y = lhs.Y - rhs.Y;\r
 776                 result.Z = lhs.Z - rhs.Z;\r
 777                 result.W = lhs.W - rhs.W;\r
 778                 return result;\r
 779         }\r
 780 \r
 781         Quaternion Quaternion::operator - (Quaternion quaternion)\r
 782         {\r
 783                 Quaternion result;\r
 784                 result.X = -quaternion.X;\r
 785                 result.Y = -quaternion.Y;\r
 786                 result.Z = -quaternion.Z;\r
 787                 result.W = -quaternion.W;\r
 788                 return result;\r
 789         }\r
 790 \r
 791         bool Quaternion::operator == ( Quaternion left, Quaternion right )\r
 792         {\r
 793                 return Quaternion::Equals( left, right );\r
 794         }\r
 795 \r
 796         bool Quaternion::operator != ( Quaternion left, Quaternion right )\r
 797         {\r
 798                 return !Quaternion::Equals( left, right );\r
 799         }\r
 800 \r
 801         String^ Quaternion::ToString()\r
 802         {\r
 803                 return String::Format( CultureInfo::CurrentCulture, "X:{0} Y:{1} Z:{2} W:{3}", X.ToString(CultureInfo::CurrentCulture), \r
 804                         Y.ToString(CultureInfo::CurrentCulture), Z.ToString(CultureInfo::CurrentCulture),\r
 805                         W.ToString(CultureInfo::CurrentCulture) );\r
 806         }\r
 807 \r
 808         int Quaternion::GetHashCode()\r
 809         {\r
 810                 return X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode();\r
 811         }\r
 812 \r
 813         bool Quaternion::Equals( Object^ value )\r
 814         {\r
 815                 if( value == nullptr )\r
 816                         return false;\r
 817 \r
 818                 if( value->GetType() != GetType() )\r
 819                         return false;\r
 820 \r
 821                 return Equals( safe_cast<Quaternion>( value ) );\r
 822         }\r
 823 \r
 824         bool Quaternion::Equals( Quaternion value )\r
 825         {\r
 826                 return ( X == value.X && Y == value.Y && Z == value.Z && W == value.W );\r
 827         }\r
 828 \r
 829         bool Quaternion::Equals( Quaternion% value1, Quaternion% value2 )\r
 830         {\r
 831                 return ( value1.X == value2.X && value1.Y == value2.Y && value1.Z == value2.Z && value1.W == value2.W );\r
 832         }\r
 833 }\r