/BulletNoXna/BulletNoXna/LinearMath/Quaternion.cs

https://bitbucket.org/cjrgaming/bullet-noxna · C# · 678 lines · 550 code · 104 blank · 24 comment · 33 complexity · 3ce21abe54ed774fec7e945f9c977d1d MD5 · raw file 

    

  1. #region License
    2. 

  2. /*
    3. 

  3. MIT License
    4. 

  4. Copyright © 2006 The Mono.Xna Team
    5. 

    6. 

  6. All rights reserved.
    7. 

    8. 

  8. Permission is hereby granted, free of charge, to any person obtaining a copy
    9. 

  9. of this software and associated documentation files (the "Software"), to deal
    10. 

  10. in the Software without restriction, including without limitation the rights
    11. 

  11. to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    12. 

  12. copies of the Software, and to permit persons to whom the Software is
    13. 

  13. furnished to do so, subject to the following conditions:
    14. 

    15. 

  15. The above copyright notice and this permission notice shall be included in all
    16. 

  16. copies or substantial portions of the Software.
    17. 

    18. 

  18. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    19. 

  19. IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    20. 

  20. FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    21. 

  21. AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    22. 

  22. LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    23. 

  23. OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    24. 

  24. SOFTWARE.
    25. 

  25. */
    26. 

  26. #endregion License
    27. 

  27. 

  28. using System;
    29. 

  29. using System.ComponentModel;
    30. 

  30. using System.Diagnostics;
    31. 

  31. using System.Text;
    32. 

  32. 

  33. namespace BulletXNA.LinearMath
    34. 

  34. {
    35. 

  35.     [Serializable]
    36. 

  36.     public struct Quaternion : IEquatable<Quaternion>
    37. 

  37.     {
    38. 

  38.         public float X;
    39. 

  39.         public float Y;
    40. 

  40.         public float Z;
    41. 

  41.         public float W;
    42. 

  42.         static Quaternion identity = new Quaternion(0, 0, 0, 1);
    43. 

  43. 

  44.         
    45. 

  45.         public Quaternion(float x, float y, float z, float w)
    46. 

  46.         {
    47. 

  47.             this.X = x;
    48. 

  48.             this.Y = y;
    49. 

  49.             this.Z = z;
    50. 

  50.             this.W = w;
    51. 

  51.         }
    52. 

  52.         
    53. 

  53.         
    54. 

  54.         public Quaternion(Vector3 vectorPart, float scalarPart)
    55. 

  55.         {
    56. 

  56.             this.X = vectorPart.X;
    57. 

  57.             this.Y = vectorPart.Y;
    58. 

  58.             this.Z = vectorPart.Z;
    59. 

  59.             this.W = scalarPart;
    60. 

  60.         }
    61. 

  61. 

  62.         public static Quaternion Identity
    63. 

  63.         {
    64. 

  64.             get{ return identity; }
    65. 

  65.         }
    66. 

  66. 

  67. 

  68.         public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2)
    69. 

  69.         {
    70. 

  70.             return new Quaternion(quaternion1.X + quaternion2.X, quaternion1.Y + quaternion2.Y, quaternion1.Z + quaternion2.Z, quaternion1.W + quaternion2.W);
    71. 

  71.         }
    72. 

  72. 

  73. 

  74.         public static void Add(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
    75. 

  75.         {
    76. 

  76.             result.W = quaternion1.W + quaternion2.W;
    77. 

  77.             result.X = quaternion1.X + quaternion2.X;
    78. 

  78.             result.Y = quaternion1.Y + quaternion2.Y;
    79. 

  79.             result.Z = quaternion1.Z + quaternion2.Z;
    80. 

  80.         }
    81. 

  81. 

  82.         public static Quaternion Concatenate(Quaternion value1, Quaternion value2)
    83. 

  83.         {
    84. 

  84.             Quaternion quaternion;
    85. 

  85.             quaternion.X = ((value2.X * value1.W) + (value1.X * value2.W)) + (value2.Y * value1.Z) - (value2.Z * value1.Y);
    86. 

  86.             quaternion.Y = ((value2.Y * value1.W) + (value1.Y * value2.W)) + (value2.Z * value1.X) - (value2.X * value1.Z);
    87. 

  87.             quaternion.Z = ((value2.Z * value1.W) + (value1.Z * value2.W)) + (value2.X * value1.Y) - (value2.Y * value1.X);
    88. 

  88.             quaternion.W = (value2.W * value1.W) - ((value2.X * value1.X) + (value2.Y * value1.Y)) + (value2.Z * value1.Z);
    89. 

  89.             return quaternion;
    90. 

  90.         }
    91. 

  91. 

  92.         public void Conjugate()
    93. 

  93.         {
    94. 

  94.             this.X = -this.X;
    95. 

  95.             this.Y = -this.Y;
    96. 

  96.             this.Z = -this.Z;
    97. 

  97.         }
    98. 

  98. 

  99.         public static Quaternion Conjugate(Quaternion value)
    100. 

  100.         {
    101. 

  101.             Quaternion quaternion;
    102. 

  102.             quaternion.X = -value.X;
    103. 

  103.             quaternion.Y = -value.Y;
    104. 

  104.             quaternion.Z = -value.Z;
    105. 

  105.             quaternion.W = value.W;
    106. 

  106.             return quaternion;
    107. 

  107.         }
    108. 

  108. 

  109.         public static void Conjugate(ref Quaternion value, out Quaternion result)
    110. 

  110.         {
    111. 

  111.             result.X = -value.X;
    112. 

  112.             result.Y = -value.Y;
    113. 

  113.             result.Z = -value.Z;
    114. 

  114.             result.W = value.W;
    115. 

  115.         }
    116. 

  116. 

  117.         public static void Concatenate(ref Quaternion value1, ref Quaternion value2, out Quaternion result)
    118. 

  118.         {
    119. 

  119.             result.X = ((value2.X * value1.W) + (value1.X * value2.W)) + (value2.Y * value1.Z) - (value2.Z * value1.Y);
    120. 

  120.             result.Y = ((value2.Y * value1.W) + (value1.Y * value2.W)) + (value2.Z * value1.X) - (value2.X * value1.Z);
    121. 

  121.             result.Z = ((value2.Z * value1.W) + (value1.Z * value2.W)) + (value2.X * value1.Y) - (value2.Y * value1.X);
    122. 

  122.             result.W = (value2.W * value1.W) - ((value2.X * value1.X) + (value2.Y * value1.Y)) + (value2.Z * value1.Z);
    123. 

  123.         }
    124. 

  124. 

  125.         public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll)
    126. 

  126.         {
    127. 

  127.             Quaternion quaternion;
    128. 

  128.             quaternion.X = (float)(((Math.Cos((yaw * 0.5)) * Math.Sin((pitch * 0.5))) * Math.Cos((roll * 0.5))) + ((Math.Sin((yaw * 0.5)) * Math.Cos((pitch * 0.5))) * Math.Sin((roll * 0.5))));
    129. 

  129.             quaternion.Y = (float)(((Math.Sin((yaw * 0.5)) * Math.Cos((pitch * 0.5))) * Math.Cos((roll * 0.5))) - ((Math.Cos((yaw * 0.5)) * Math.Sin((pitch * 0.5))) * Math.Sin((roll * 0.5))));
    130. 

  130.             quaternion.Z = (float)(((Math.Cos((yaw * 0.5)) * Math.Cos((pitch * 0.5))) * Math.Sin((roll * 0.5))) - ((Math.Sin((yaw * 0.5)) * Math.Sin((pitch * 0.5))) * Math.Cos((roll * 0.5))));
    131. 

  131.             quaternion.W = (float)(((Math.Cos((yaw * 0.5)) * Math.Cos((pitch * 0.5))) * Math.Cos((roll * 0.5))) + ((Math.Sin((yaw * 0.5)) * Math.Sin((pitch * 0.5))) * Math.Sin((roll * 0.5))));
    132. 

  132.             return quaternion;
    133. 

  133.         }
    134. 

  134. 

  135.         public static void CreateFromYawPitchRoll(float yaw, float pitch, float roll, out Quaternion result)
    136. 

  136.         {
    137. 

  137.             result.X = (float)(((Math.Cos((yaw * 0.5)) * Math.Sin((pitch * 0.5))) * Math.Cos((roll * 0.5))) + ((Math.Sin((yaw * 0.5)) * Math.Cos((pitch * 0.5))) * Math.Sin((roll * 0.5))));
    138. 

  138.             result.Y = (float)(((Math.Sin((yaw * 0.5)) * Math.Cos((pitch * 0.5))) * Math.Cos((roll * 0.5))) - ((Math.Cos((yaw * 0.5)) * Math.Sin((pitch * 0.5))) * Math.Sin((roll * 0.5))));
    139. 

  139.             result.Z = (float)(((Math.Cos((yaw * 0.5)) * Math.Cos((pitch * 0.5))) * Math.Sin((roll * 0.5))) - ((Math.Sin((yaw * 0.5)) * Math.Sin((pitch * 0.5))) * Math.Cos((roll * 0.5))));
    140. 

  140.             result.W = (float)(((Math.Cos((yaw * 0.5)) * Math.Cos((pitch * 0.5))) * Math.Cos((roll * 0.5))) + ((Math.Sin((yaw * 0.5)) * Math.Sin((pitch * 0.5))) * Math.Sin((roll * 0.5))));
    141. 

  141.         }
    142. 

  142. 

  143.         public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle)
    144. 

  144.         {
    145. 

  145.             double sin_a = Math.Sin(angle / 2.0);
    146. 

  146.             return new Quaternion((float)(axis.X * sin_a),(float)(axis.Y * sin_a),(float)(axis.Z * sin_a),(float)Math.Cos(angle / 2.0));
    147. 

  147.         }
    148. 

  148. 

  149. 

  150.         public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Quaternion result)
    151. 

  151.         {
    152. 

  152.             double sin_a = Math.Sin(angle / 2.0);
    153. 

  153.             result.X = (float)(axis.X * sin_a);
    154. 

  154.             result.Y = (float)(axis.Y * sin_a);
    155. 

  155.             result.Z = (float)(axis.Z * sin_a);
    156. 

  156.             result.W = (float)Math.Cos(angle / 2.0);
    157. 

  157.         }
    158. 

  158. 

  159. 

  160.         public static Quaternion CreateFromRotationMatrix(Matrix matrix)
    161. 

  161.         {
    162. 

  162.             Quaternion result;
    163. 

  163.             if ((matrix._basis[0, 0] + matrix._basis[1, 1] + matrix._basis[2, 2]) > 0.0F)
    164. 

  164.             {
    165. 

  165.                 double M1 = System.Math.Sqrt((matrix._basis[0, 0] + matrix._basis[1, 1] + matrix._basis[2, 2] + 1.0));
    166. 

  166.                 result.W = (float)(M1 * 0.5);
    167. 

  167.                 M1 = 0.5 / M1;
    168. 

  168.                 result.X = (float)((matrix._basis[1, 2] - matrix._basis[2, 1]) * M1);
    169. 

  169.                 result.Y = (float)((matrix._basis[2, 0] - matrix._basis[0, 2]) * M1);
    170. 

  170.                 result.Z = (float)((matrix._basis[0, 1] - matrix._basis[1, 0]) * M1);
    171. 

  171.                 return result;
    172. 

  172.             }
    173. 

  173.             if ((matrix._basis[0, 0] >= matrix._basis[1, 1]) && (matrix._basis[0, 0] >= matrix._basis[2, 2]))
    174. 

  174.             {
    175. 

  175.                 double M2 = System.Math.Sqrt((1.0 + matrix._basis[0, 0] - matrix._basis[1, 1] - matrix._basis[2, 2]));
    176. 

  176.                 double M3 = 0.5 / M2;
    177. 

  177.                 result.X = (float)(0.5 * M2);
    178. 

  178.                 result.Y = (float)((matrix._basis[0, 1] + matrix._basis[1, 0]) * M3);
    179. 

  179.                 result.Z = (float)((matrix._basis[0, 2] + matrix._basis[2, 0]) * M3);
    180. 

  180.                 result.W = (float)((matrix._basis[1, 2] - matrix._basis[2, 1]) * M3);
    181. 

  181.                 return result;
    182. 

  182.             }
    183. 

  183.             if (matrix._basis[1, 1] > matrix._basis[2, 2])
    184. 

  184.             {
    185. 

  185.                 double M4 = System.Math.Sqrt((double)(1.0 + matrix._basis[1, 1] - matrix._basis[0, 0] - matrix._basis[2, 2]));
    186. 

  186.                 double M5 = 0.5 / M4;
    187. 

  187.                 result.X = (float)((matrix._basis[1, 0] + matrix._basis[0, 1]) * M5);
    188. 

  188.                 result.Y = (float)(0.5 * M4);
    189. 

  189.                 result.Z = (float)((matrix._basis[2, 1] + matrix._basis[1, 2]) * M5);
    190. 

  190.                 result.W = (float)((matrix._basis[2, 0] - matrix._basis[0, 2]) * M5);
    191. 

  191.                 return result;
    192. 

  192.             }
    193. 

  193.             double M6 = System.Math.Sqrt((1.0 + matrix._basis[2, 2] - matrix._basis[0, 0] - matrix._basis[1, 1]));
    194. 

  194.             double M7 = 0.5 / M6;
    195. 

  195.             result.X = (float)((matrix._basis[2, 0] + matrix._basis[0, 2]) * M7);
    196. 

  196.             result.Y = (float)((matrix._basis[2, 1] + matrix._basis[1, 2]) * M7);
    197. 

  197.             result.Z = (float)(0.5 * M6);
    198. 

  198.             result.W = (float)((matrix._basis[0, 1] - matrix._basis[1, 0]) * M7);
    199. 

  199.             return result;
    200. 

  200.         }
    201. 

  201. 

  202. 

  203.         public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result)
    204. 

  204.         {
    205. 

  205.             if ((matrix._basis[0, 0] + matrix._basis[1, 1] + matrix._basis[2, 2]) > 0.0F)
    206. 

  206.             {
    207. 

  207.                 double M1 = System.Math.Sqrt((matrix._basis[0, 0] + matrix._basis[1, 1] + matrix._basis[2, 2] + 1.0));
    208. 

  208.                 result.W = (float)(M1 * 0.5);
    209. 

  209.                 M1 = 0.5 / M1;
    210. 

  210.                 result.X = (float)((matrix._basis[1, 2] - matrix._basis[2, 1]) * M1);
    211. 

  211.                 result.Y = (float)((matrix._basis[2, 0] - matrix._basis[0, 2]) * M1);
    212. 

  212.                 result.Z = (float)((matrix._basis[0, 1] - matrix._basis[1, 0]) * M1);
    213. 

  213.                 return;
    214. 

  214.             }
    215. 

  215.             if ((matrix._basis[0, 0] >= matrix._basis[1, 1]) && (matrix._basis[0, 0] >= matrix._basis[2, 2]))
    216. 

  216.             {
    217. 

  217.                 double M2 = System.Math.Sqrt((1.0 + matrix._basis[0, 0] - matrix._basis[1, 1] - matrix._basis[2, 2]));
    218. 

  218.                 double M3 = 0.5 / M2;
    219. 

  219.                 result.X = (float)(0.5 * M2);
    220. 

  220.                 result.Y = (float)((matrix._basis[0, 1] + matrix._basis[1, 0]) * M3);
    221. 

  221.                 result.Z = (float)((matrix._basis[0, 2] + matrix._basis[2, 0]) * M3);
    222. 

  222.                 result.W = (float)((matrix._basis[1, 2] - matrix._basis[2, 1]) * M3);
    223. 

  223.                 return;
    224. 

  224.             }
    225. 

  225.             if (matrix._basis[1, 1] > matrix._basis[2, 2])
    226. 

  226.             {
    227. 

  227.                 double M4 = System.Math.Sqrt((1.0 + matrix._basis[1, 1] - matrix._basis[0, 0] - matrix._basis[2, 2]));
    228. 

  228.                 double M5 = 0.5 / M4;
    229. 

  229.                 result.X = (float)((matrix._basis[1, 0] + matrix._basis[0, 1]) * M5);
    230. 

  230.                 result.Y = (float)(0.5 * M4);
    231. 

  231.                 result.Z = (float)((matrix._basis[2, 1] + matrix._basis[1, 2]) * M5);
    232. 

  232.                 result.W = (float)((matrix._basis[2, 0] - matrix._basis[0, 2]) * M5);
    233. 

  233.                 return;
    234. 

  234.             }
    235. 

  235.             double M6 = System.Math.Sqrt((1.0 + matrix._basis[2, 2] - matrix._basis[0, 0] - matrix._basis[1, 1]));
    236. 

  236.             double M7 = 0.5 / M6;
    237. 

  237.             result.X = (float)((matrix._basis[2, 0] + matrix._basis[0, 2]) * M7);
    238. 

  238.             result.Y = (float)((matrix._basis[2, 1] + matrix._basis[1, 2]) * M7);
    239. 

  239.             result.Z = (float)(0.5 * M6);
    240. 

  240.             result.W = (float)((matrix._basis[0, 1] - matrix._basis[1, 0]) * M7);
    241. 

  241.         }
    242. 

  242. 

  243. 

  244.         public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2)
    245. 

  245.         {
    246. 

  246.             Quaternion result;
    247. 

  247. 

  248.             double w5 = 1.0 / ((quaternion2.X * quaternion2.X) + (quaternion2.Y * quaternion2.Y) + (quaternion2.Z * quaternion2.Z) + (quaternion2.W * quaternion2.W));
    249. 

  249.             double w4 = -quaternion2.X * w5;
    250. 

  250.             double w3 = -quaternion2.Y * w5;
    251. 

  251.             double w2 = -quaternion2.Z * w5;
    252. 

  252.             double w1 = quaternion2.W * w5;
    253. 

  253. 

  254.             result.X = (float)((quaternion1.X * w1) + (w4 * quaternion1.W) + ((quaternion1.Y * w2) - (quaternion1.Z * w3)));
    255. 

  255.             result.Y = (float)((quaternion1.Y * w1) + (w3 * quaternion1.W) + ((quaternion1.Z * w4) - (quaternion1.X * w2)));
    256. 

  256.             result.Z = (float)((quaternion1.Z * w1) + (w2 * quaternion1.W) + ((quaternion1.X * w3) - (quaternion1.Y * w4)));
    257. 

  257.             result.W = (float)((quaternion1.W * quaternion2.W * w5) - ((quaternion1.X * w4) + (quaternion1.Y * w3) + (quaternion1.Z * w2)));
    258. 

  258.             return result;
    259. 

  259.         }
    260. 

  260. 

  261. 

  262.         public static void Divide(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
    263. 

  263.         {
    264. 

  264.             double w5 = 1.0 / ((quaternion2.X * quaternion2.X) + (quaternion2.Y * quaternion2.Y) + (quaternion2.Z * quaternion2.Z) + (quaternion2.W * quaternion2.W));
    265. 

  265.             double w4 = -quaternion2.X * w5;
    266. 

  266.             double w3 = -quaternion2.Y * w5;
    267. 

  267.             double w2 = -quaternion2.Z * w5;
    268. 

  268.             double w1 = quaternion2.W * w5;
    269. 

  269. 

  270.             result.X = (float)((quaternion1.X * w1) + (w4 * quaternion1.W) + ((quaternion1.Y * w2) - (quaternion1.Z * w3)));
    271. 

  271.             result.Y = (float)((quaternion1.Y * w1) + (w3 * quaternion1.W) + ((quaternion1.Z * w4) - (quaternion1.X * w2)));
    272. 

  272.             result.Z = (float)((quaternion1.Z * w1) + (w2 * quaternion1.W) + ((quaternion1.X * w3) - (quaternion1.Y * w4)));
    273. 

  273.             result.W = (float)((quaternion1.W * quaternion2.W * w5) - ((quaternion1.X * w4) + (quaternion1.Y * w3) + (quaternion1.Z * w2)));
    274. 

  274.         }
    275. 

  275. 

  276. 

  277.         public static float Dot(Quaternion quaternion1, Quaternion quaternion2)
    278. 

  278.         {
    279. 

  279.             return (quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) + (quaternion1.Z * quaternion2.Z) + (quaternion1.W * quaternion2.W);
    280. 

  280.         }
    281. 

  281. 

  282. 

  283.         public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result)
    284. 

  284.         {
    285. 

  285.            result = (quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) + (quaternion1.Z * quaternion2.Z) + (quaternion1.W * quaternion2.W);
    286. 

  286.         }
    287. 

  287. 

  288. 

  289.         public override bool Equals(object obj)
    290. 

  290.         {
    291. 

  291.             return (obj is Quaternion) ? this == (Quaternion)obj : false;
    292. 

  292.         }
    293. 

  293. 

  294. 

  295.         public bool Equals(Quaternion other)
    296. 

  296.         {
    297. 

  297.             if ((X == other.X) && (Y == other.Y) && (Z == other.Z))
    298. 

  298.                 return W == other.W;
    299. 

  299.             return false;   
    300. 

  300.         }
    301. 

  301. 

  302. 

  303.         public override int GetHashCode()
    304. 

  304.         {
    305. 

  305.             return X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode();
    306. 

  306.         }
    307. 

  307. 

  308. 

  309.         public static Quaternion Inverse(Quaternion quaternion)
    310. 

  310.         {
    311. 

  311.             Quaternion result;
    312. 

  312.             double m1 = 1.0 / ((quaternion.X * quaternion.X) + (quaternion.Y * quaternion.Y) + (quaternion.Z * quaternion.Z) + (quaternion.W * quaternion.W));
    313. 

  313.             result.X = (float)(-quaternion.X * m1);
    314. 

  314.             result.Y = (float)(-quaternion.Y * m1);
    315. 

  315.             result.Z = (float)(-quaternion.Z * m1);
    316. 

  316.             result.W = (float)(quaternion.W * m1);
    317. 

  317.             return result;
    318. 

  318.         }
    319. 

  319. 

  320. 

  321.         public static void Inverse(ref Quaternion quaternion, out Quaternion result)
    322. 

  322.         {
    323. 

  323.             double m1 = 1.0 / ((quaternion.X * quaternion.X) + (quaternion.Y * quaternion.Y) + (quaternion.Z * quaternion.Z) + (quaternion.W * quaternion.W));
    324. 

  324.             result.X = (float)(-quaternion.X * m1);
    325. 

  325.             result.Y = (float)(-quaternion.Y * m1);
    326. 

  326.             result.Z = (float)(-quaternion.Z * m1);
    327. 

  327.             result.W = (float)(quaternion.W * m1);
    328. 

  328.         }
    329. 

  329. 

  330. 

  331.         public float Length()
    332. 

  332.         {
    333. 

  333.             return (float)System.Math.Sqrt(((X * X) + (Y * Y) + (Z * Z) + (W * W)));
    334. 

  334.         }
    335. 

  335. 

  336. 

  337.         public float LengthSquared()
    338. 

  338.         {
    339. 

  339.             return (X * X) + (Y * Y) + (Z * Z) + (W * W);
    340. 

  340.         }
    341. 

  341. 

  342. 

  343.         public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
    344. 

  344.         {
    345. 

  345.             Quaternion result;
    346. 

  346.             double f2 = 1.0 - amount;
    347. 

  347.             double tempX, tempY, tempZ, tempW;
    348. 

  348.             if (((quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) + (quaternion1.Z * quaternion2.Z) + (quaternion1.W * quaternion2.W)) >= 0.0F)
    349. 

  349.             {
    350. 

  350.                 tempX = (f2 * quaternion1.X) + (amount * quaternion2.X);
    351. 

  351.                 tempY = (f2 * quaternion1.Y) + (amount * quaternion2.Y);
    352. 

  352.                 tempZ = (f2 * quaternion1.Z) + (amount * quaternion2.Z);
    353. 

  353.                 tempW = (f2 * quaternion1.W) + (amount * quaternion2.W);
    354. 

  354.             }
    355. 

  355.             else
    356. 

  356.             {
    357. 

  357.                 tempX = (f2 * quaternion1.X) - (amount * quaternion2.X);
    358. 

  358.                 tempY = (f2 * quaternion1.Y) - (amount * quaternion2.Y);
    359. 

  359.                 tempZ = (f2 * quaternion1.Z) - (amount * quaternion2.Z);
    360. 

  360.                 tempW = (f2 * quaternion1.W) - (amount * quaternion2.W);
    361. 

  361.             }
    362. 

  362.             double f4 = (tempX * tempX) + (tempY * tempY) + (tempZ * tempZ) + (tempW * tempW);
    363. 

  363.             double f3 = 1.0 / System.Math.Sqrt(f4);
    364. 

  364.             result.X = (float)(tempX * f3);
    365. 

  365.             result.Y = (float)(tempY * f3);
    366. 

  366.             result.Z = (float)(tempZ * f3);
    367. 

  367.             result.W = (float)(tempW * f3);
    368. 

  368. 

  369.             return result;
    370. 

  370.         }
    371. 

  371. 

  372. 

  373.         public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
    374. 

  374.         {
    375. 

  375.             double m2 = 1.0 - amount;
    376. 

  376.             double tempX, tempY, tempZ, tempW;
    377. 

  377.             if (((quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) + (quaternion1.Z * quaternion2.Z) + (quaternion1.W * quaternion2.W)) >= 0.0F)
    378. 

  378.             {
    379. 

  379.                 tempX = ((m2 * quaternion1.X) + (amount * quaternion2.X));
    380. 

  380.                 tempY = ((m2 * quaternion1.Y) + (amount * quaternion2.Y));
    381. 

  381.                 tempZ = ((m2 * quaternion1.Z) + (amount * quaternion2.Z));
    382. 

  382.                 tempW = ((m2 * quaternion1.W) + (amount * quaternion2.W));
    383. 

  383.             }
    384. 

  384.             else
    385. 

  385.             {
    386. 

  386.                 tempX = ((m2 * quaternion1.X) - (amount * quaternion2.X));
    387. 

  387.                 tempY = ((m2 * quaternion1.Y) - (amount * quaternion2.Y));
    388. 

  388.                 tempZ = ((m2 * quaternion1.Z) - (amount * quaternion2.Z));
    389. 

  389.                 tempW = ((m2 * quaternion1.W) - (amount * quaternion2.W));
    390. 

  390.             }
    391. 

  391.             double m4 = (tempX * tempX) + (tempY * tempY) + (tempZ * tempZ) + (tempW * tempW);
    392. 

  392.             double m3 = 1.0 / System.Math.Sqrt(m4);
    393. 

  393.             result.X = (float)(tempX * m3);
    394. 

  394.             result.Y = (float)(tempY * m3);
    395. 

  395.             result.Z = (float)(tempZ * m3);
    396. 

  396.             result.W = (float)(tempW * m3);
    397. 

  397.         }
    398. 

  398. 

  399. 

  400.         public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float t)
    401. 

  401.         {
    402. 

  402. 

  403.             float theta = Angle(quaternion1, quaternion2);
    404. 

  404.             if(theta != 0)
    405. 

  405.             {
    406. 

  406.                 double d = 1.0/Math.Sin(theta);
    407. 

  407.                 double s0 = Math.Sin((1.0 - t)*theta);
    408. 

  408.                 double s1 = Math.Sin(t*theta);
    409. 

  409.                 if(Quaternion.Dot(quaternion1, quaternion2) < 0)
    410. 

  410.                 {
    411. 

  411.                     return new Quaternion((float)((quaternion1.X * s0 + -quaternion2.X * s1) *d),
    412. 

  412.                         (float)((quaternion1.Y * s0 + -quaternion2.Y * s1) *d),
    413. 

  413.                         (float)((quaternion1.Z * s0 + -quaternion2.Z * s1) *d),
    414. 

  414.                         (float)((quaternion1.W * s0 + -quaternion2.W * s1) *d));
    415. 

  415.                 }
    416. 

  416.                 else
    417. 

  417.                 {
    418. 

  418.                     return new Quaternion((float)((quaternion1.X * s0 + quaternion2.X * s1) * d),
    419. 

  419.                         (float)((quaternion1.Y * s0 + quaternion2.Y * s1) * d),
    420. 

  420.                         (float)((quaternion1.Z * s0 + quaternion2.Z * s1) * d),
    421. 

  421.                         (float)((quaternion1.W * s0 + quaternion2.W * s1) * d));
    422. 

  422.                 }
    423. 

  423.             }
    424. 

  424. 

  425.             return quaternion1;
    426. 

  426.         }
    427. 

  427. 

  428.         public static float Angle(Quaternion p, Quaternion q) 
    429. 

  429. 	    {
    430. 

  430. 		    double s = Math.Sqrt(p.LengthSquared() * q.LengthSquared());
    431. 

  431. 		    Debug.Assert(s != 0.0);
    432. 

  432. 		    return (float)Math.Acos(Quaternion.Dot(p, q) / s);
    433. 

  433. 	    }
    434. 

  434. 

  435.         public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float t, out Quaternion result)
    436. 

  436.         {
    437. 

  437.             float theta = Angle(quaternion1, quaternion2);
    438. 

  438.             if (theta != 0)
    439. 

  439.             {
    440. 

  440.                 double d = 1.0 / Math.Sin(theta);
    441. 

  441.                 double s0 = Math.Sin((1.0 - t) * theta);
    442. 

  442.                 double s1 = Math.Sin(t * theta);
    443. 

  443.                 if (Quaternion.Dot(quaternion1, quaternion2) < 0)
    444. 

  444.                 {
    445. 

  445.                     result = new Quaternion((float)((quaternion1.X * s0 + -quaternion2.X * s1) * d),
    446. 

  446.                         (float)((quaternion1.Y * s0 + -quaternion2.Y * s1) * d),
    447. 

  447.                         (float)((quaternion1.Z * s0 + -quaternion2.Z * s1) * d),
    448. 

  448.                         (float)((quaternion1.W * s0 + -quaternion2.W * s1) * d));
    449. 

  449.                 }
    450. 

  450.                 else
    451. 

  451.                 {
    452. 

  452.                     result =  new Quaternion((float)((quaternion1.X * s0 + quaternion2.X * s1) * d),
    453. 

  453.                         (float)((quaternion1.Y * s0 + quaternion2.Y * s1) * d),
    454. 

  454.                         (float)((quaternion1.Z * s0 + quaternion2.Z * s1) * d),
    455. 

  455.                         (float)((quaternion1.W * s0 + quaternion2.W * s1) * d));
    456. 

  456.                 }
    457. 

  457.             }
    458. 

  458.             else
    459. 

  459.             {
    460. 

  460.                 result = quaternion1;
    461. 

  461.             }
    462. 

  462.         }
    463. 

  463. 

  464. 

  465.         public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2)
    466. 

  466.         {
    467. 

  467.             return new Quaternion(quaternion1.X - quaternion2.X, quaternion1.Y - quaternion2.Y, quaternion1.Z - quaternion2.Z, quaternion1.W - quaternion2.W);
    468. 

  468.         }
    469. 

  469. 

  470. 

  471.         public static void Subtract(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
    472. 

  472.         {
    473. 

  473.             result.X = quaternion1.X - quaternion2.X;
    474. 

  474.             result.Y = quaternion1.Y - quaternion2.Y;
    475. 

  475.             result.Z = quaternion1.Z - quaternion2.Z;
    476. 

  476.             result.W = quaternion1.W - quaternion2.W;
    477. 

  477.         }
    478. 

  478. 

  479. 

  480.         public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2)
    481. 

  481.         {
    482. 

  482.             Quaternion result;
    483. 

  483.             double f12 = (quaternion1.Y * quaternion2.Z) - (quaternion1.Z * quaternion2.Y);
    484. 

  484.             double f11 = (quaternion1.Z * quaternion2.X) - (quaternion1.X * quaternion2.Z);
    485. 

  485.             double f10 = (quaternion1.X * quaternion2.Y) - (quaternion1.Y * quaternion2.X);
    486. 

  486.             double f9 = (quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) + (quaternion1.Z * quaternion2.Z);
    487. 

  487.             result.X = (float)((quaternion1.X * quaternion2.W) + (quaternion2.X * quaternion1.W) + f12);
    488. 

  488.             result.Y = (float)((quaternion1.Y * quaternion2.W) + (quaternion2.Y * quaternion1.W) + f11);
    489. 

  489.             result.Z = (float)((quaternion1.Z * quaternion2.W) + (quaternion2.Z * quaternion1.W) + f10);
    490. 

  490.             result.W = (float)((quaternion1.W * quaternion2.W) - f9);
    491. 

  491.             return result;
    492. 

  492.         }
    493. 

  493. 

  494. 

  495.         public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor)
    496. 

  496.         {
    497. 

  497.             Quaternion result;
    498. 

  498.             result.X = quaternion1.X * scaleFactor;
    499. 

  499.             result.Y = quaternion1.Y * scaleFactor;
    500. 

  500.             result.Z = quaternion1.Z * scaleFactor;
    501. 

  501.             result.W = quaternion1.W * scaleFactor;
    502. 

  502.             return result;
    503. 

  503.         }
    504. 

  504. 

  505. 

  506.         public static void Multiply(ref Quaternion quaternion1, float scaleFactor, out Quaternion result)
    507. 

  507.         {
    508. 

  508.             result.X = quaternion1.X * scaleFactor;
    509. 

  509.             result.Y = quaternion1.Y * scaleFactor;
    510. 

  510.             result.Z = quaternion1.Z * scaleFactor;
    511. 

  511.             result.W = quaternion1.W * scaleFactor;
    512. 

  512.         }
    513. 

  513. 

  514. 

  515.         public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
    516. 

  516.         {
    517. 

  517.             double f12 = (quaternion1.Y * quaternion2.Z) - (quaternion1.Z * quaternion2.Y);
    518. 

  518.             double f11 = (quaternion1.Z * quaternion2.X) - (quaternion1.X * quaternion2.Z);
    519. 

  519.             double f10 = (quaternion1.X * quaternion2.Y) - (quaternion1.Y * quaternion2.X);
    520. 

  520.             double f9 = (quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) + (quaternion1.Z * quaternion2.Z);
    521. 

  521.             result.X = (float)((quaternion1.X * quaternion2.W) + (quaternion2.X * quaternion1.W) + f12);
    522. 

  522.             result.Y = (float)((quaternion1.Y * quaternion2.W) + (quaternion2.Y * quaternion1.W) + f11);
    523. 

  523.             result.Z = (float)((quaternion1.Z * quaternion2.W) + (quaternion2.Z * quaternion1.W) + f10);
    524. 

  524.             result.W = (float)((quaternion1.W * quaternion2.W) - f9);
    525. 

  525.         }
    526. 

  526. 

  527. 

  528.         public static Quaternion Negate(Quaternion quaternion)
    529. 

  529.         {
    530. 

  530.             Quaternion result;
    531. 

  531.             result.X = -quaternion.X;
    532. 

  532.             result.Y = -quaternion.Y;
    533. 

  533.             result.Z = -quaternion.Z;
    534. 

  534.             result.W = -quaternion.W;
    535. 

  535.             return result;
    536. 

  536.         }
    537. 

  537. 

  538. 

  539.         public static void Negate(ref Quaternion quaternion, out Quaternion result)
    540. 

  540.         {
    541. 

  541.             result.X = -quaternion.X;
    542. 

  542.             result.Y = -quaternion.Y;
    543. 

  543.             result.Z = -quaternion.Z;
    544. 

  544.             result.W = -quaternion.W;
    545. 

  545.         }
    546. 

  546. 

  547. 

  548.         public void Normalize()
    549. 

  549.         {
    550. 

  550.             double f1 = (1.0 / System.Math.Sqrt((double)((this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z) + (this.W * this.W))));
    551. 

  551.             X = (float)(X*f1);
    552. 

  552.             Y = (float)(Y * f1);
    553. 

  553.             Z = (float)(Z * f1);
    554. 

  554.             W = (float)(W * f1);
    555. 

  555.         }
    556. 

  556. 

  557. 

  558.         public static Quaternion Normalize(Quaternion quaternion)
    559. 

  559.         {
    560. 

  560.             Quaternion result;
    561. 

  561.             double f1 = (1.0 / System.Math.Sqrt((double)((quaternion.X * quaternion.X) + (quaternion.Y * quaternion.Y) + (quaternion.Z * quaternion.Z) + (quaternion.W * quaternion.W))));
    562. 

  562.             result.X = (float)(quaternion.X * f1);
    563. 

  563.             result.Y = (float)(quaternion.Y * f1);
    564. 

  564.             result.Z = (float)(quaternion.Z * f1);
    565. 

  565.             result.W = (float)(quaternion.W * f1);
    566. 

  566.             return result;
    567. 

  567.         }
    568. 

  568. 

  569. 

  570.         public static void Normalize(ref Quaternion quaternion, out Quaternion result)
    571. 

  571.         {
    572. 

  572.             double f1 = (1.0 / System.Math.Sqrt((double)((quaternion.X * quaternion.X) + (quaternion.Y * quaternion.Y) + (quaternion.Z * quaternion.Z) + (quaternion.W * quaternion.W))));
    573. 

  573.             result.X = (float)(quaternion.X * f1);
    574. 

  574.             result.Y = (float)(quaternion.Y * f1);
    575. 

  575.             result.Z = (float)(quaternion.Z * f1);
    576. 

  576.             result.W = (float)(quaternion.W * f1);
    577. 

  577.         }
    578. 

  578. 

  579. 

  580.         public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2)
    581. 

  581.         {
    582. 

  582.             return new Quaternion(quaternion1.X + quaternion2.X, quaternion1.Y + quaternion2.Y, quaternion1.Z + quaternion2.Z, quaternion1.W + quaternion2.W);
    583. 

  583.         }
    584. 

  584. 

  585. 

  586.         public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2)
    587. 

  587.         {
    588. 

  588.             Quaternion result;
    589. 

  589. 

  590.             double w5 = 1.0 / ((quaternion2.X * quaternion2.X) + (quaternion2.Y * quaternion2.Y) + (quaternion2.Z * quaternion2.Z) + (quaternion2.W * quaternion2.W));
    591. 

  591.             double w4 = -quaternion2.X * w5;
    592. 

  592.             double w3 = -quaternion2.Y * w5;
    593. 

  593.             double w2 = -quaternion2.Z * w5;
    594. 

  594.             double w1 = quaternion2.W * w5;
    595. 

  595. 

  596.             result.X = (float)((quaternion1.X * w1) + (w4 * quaternion1.W) + ((quaternion1.Y * w2) - (quaternion1.Z * w3)));
    597. 

  597.             result.Y = (float)((quaternion1.Y * w1) + (w3 * quaternion1.W) + ((quaternion1.Z * w4) - (quaternion1.X * w2)));
    598. 

  598.             result.Z = (float)((quaternion1.Z * w1) + (w2 * quaternion1.W) + ((quaternion1.X * w3) - (quaternion1.Y * w4)));
    599. 

  599.             result.W = (float)((quaternion1.W * quaternion2.W * w5) - ((quaternion1.X * w4) + (quaternion1.Y * w3) + (quaternion1.Z * w2)));
    600. 

  600.             return result;
    601. 

  601.         }
    602. 

  602. 

  603. 

  604.         public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2)
    605. 

  605.         {
    606. 

  606.             return quaternion1.X == quaternion2.X
    607. 

  607.                 && quaternion1.Y == quaternion2.Y
    608. 

  608.                 && quaternion1.Z == quaternion2.Z
    609. 

  609.                 && quaternion1.W == quaternion2.W;
    610. 

  610.         }
    611. 

  611. 

  612. 

  613.         public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2)
    614. 

  614.         {
    615. 

  615.             return !(quaternion1 == quaternion2);
    616. 

  616.         }
    617. 

  617. 

  618. 

  619.         public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2)
    620. 

  620.         {
    621. 

  621.             Quaternion result;
    622. 

  622.             double f12 = (quaternion1.Y * quaternion2.Z) - (quaternion1.Z * quaternion2.Y);
    623. 

  623.             double f11 = (quaternion1.Z * quaternion2.X) - (quaternion1.X * quaternion2.Z);
    624. 

  624.             double f10 = (quaternion1.X * quaternion2.Y) - (quaternion1.Y * quaternion2.X);
    625. 

  625.             double f9 = (quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) + (quaternion1.Z * quaternion2.Z);
    626. 

  626.             result.X = (float)((quaternion1.X * quaternion2.W) + (quaternion2.X * quaternion1.W) + f12);
    627. 

  627.             result.Y = (float)((quaternion1.Y * quaternion2.W) + (quaternion2.Y * quaternion1.W) + f11);
    628. 

  628.             result.Z = (float)((quaternion1.Z * quaternion2.W) + (quaternion2.Z * quaternion1.W) + f10);
    629. 

  629.             result.W = (float)((quaternion1.W * quaternion2.W) - f9);
    630. 

  630.             return result;
    631. 

  631.         }
    632. 

  632. 

  633. 

  634.         public static Quaternion operator *(Quaternion quaternion1, float scaleFactor)
    635. 

  635.         {
    636. 

  636.             Quaternion result;
    637. 

  637.             result.X = quaternion1.X * scaleFactor;
    638. 

  638.             result.Y = quaternion1.Y * scaleFactor;
    639. 

  639.             result.Z = quaternion1.Z * scaleFactor;
    640. 

  640.             result.W = quaternion1.W * scaleFactor;
    641. 

  641.             return result;
    642. 

  642.         }
    643. 

  643. 

  644. 

  645.         public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2)
    646. 

  646.         {
    647. 

  647.             return new Quaternion(quaternion1.X - quaternion2.X, quaternion1.Y - quaternion2.Y, quaternion1.Z - quaternion2.Z, quaternion1.W - quaternion2.W);
    648. 

  648.         }
    649. 

  649. 

  650. 

  651.         public static Quaternion operator -(Quaternion quaternion)
    652. 

  652.         {
    653. 

  653.             Quaternion q1;
    654. 

  654.             q1.X = -quaternion.X;
    655. 

  655.             q1.Y = -quaternion.Y;
    656. 

  656.             q1.Z = -quaternion.Z;
    657. 

  657.             q1.W = -quaternion.W;
    658. 

  658.             return q1;
    659. 

  659.         }
    660. 

  660. 

  661. 

  662.         public override string ToString()
    663. 

  663.         {
    664. 

  664.             StringBuilder sb = new StringBuilder(32);
    665. 

  665.             sb.Append("{X:");
    666. 

  666.             sb.Append(this.X);
    667. 

  667.             sb.Append(" Y:");
    668. 

  668.             sb.Append(this.Y);
    669. 

  669.             sb.Append(" Z:");
    670. 

  670.             sb.Append(this.Z);
    671. 

  671.             sb.Append(" W:");
    672. 

  672.             sb.Append(this.W);
    673. 

  673.             sb.Append("}");
    674. 

  674.             return sb.ToString();
    675. 

  675.         }
    676. 

  676. 

  677.     }
    678. 

  678. }
    679.