Quaternion.cs 7.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209
  1. /* The MIT License
  2. *
  3. * Copyright (c) 2010 Intel Corporation.
  4. * All rights reserved.
  5. *
  6. * Based on the convexdecomposition library from
  7. * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
  8. *
  9. * Permission is hereby granted, free of charge, to any person obtaining a copy
  10. * of this software and associated documentation files (the "Software"), to deal
  11. * in the Software without restriction, including without limitation the rights
  12. * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  13. * copies of the Software, and to permit persons to whom the Software is
  14. * furnished to do so, subject to the following conditions:
  15. *
  16. * The above copyright notice and this permission notice shall be included in
  17. * all copies or substantial portions of the Software.
  18. *
  19. * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  20. * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  21. * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  22. * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  23. * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  24. * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  25. * THE SOFTWARE.
  26. */
  27. using System;
  28. namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
  29. {
  30. public class Quaternion : float4
  31. {
  32. public Quaternion()
  33. {
  34. x = y = z = 0.0f;
  35. w = 1.0f;
  36. }
  37. public Quaternion(float3 v, float t)
  38. {
  39. v = float3.normalize(v);
  40. w = (float)Math.Cos(t / 2.0f);
  41. v = v * (float)Math.Sin(t / 2.0f);
  42. x = v.x;
  43. y = v.y;
  44. z = v.z;
  45. }
  46. public Quaternion(float _x, float _y, float _z, float _w)
  47. {
  48. x = _x;
  49. y = _y;
  50. z = _z;
  51. w = _w;
  52. }
  53. public float angle()
  54. {
  55. return (float)Math.Acos(w) * 2.0f;
  56. }
  57. public float3 axis()
  58. {
  59. float3 a = new float3(x, y, z);
  60. if (Math.Abs(angle()) < 0.0000001f)
  61. return new float3(1f, 0f, 0f);
  62. return a * (1 / (float)Math.Sin(angle() / 2.0f));
  63. }
  64. public float3 xdir()
  65. {
  66. return new float3(1 - 2 * (y * y + z * z), 2 * (x * y + w * z), 2 * (x * z - w * y));
  67. }
  68. public float3 ydir()
  69. {
  70. return new float3(2 * (x * y - w * z), 1 - 2 * (x * x + z * z), 2 * (y * z + w * x));
  71. }
  72. public float3 zdir()
  73. {
  74. return new float3(2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y));
  75. }
  76. public float3x3 getmatrix()
  77. {
  78. return new float3x3(xdir(), ydir(), zdir());
  79. }
  80. public static implicit operator float3x3(Quaternion q)
  81. {
  82. return q.getmatrix();
  83. }
  84. public static Quaternion operator *(Quaternion a, Quaternion b)
  85. {
  86. Quaternion c = new Quaternion();
  87. c.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
  88. c.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
  89. c.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
  90. c.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
  91. return c;
  92. }
  93. public static float3 operator *(Quaternion q, float3 v)
  94. {
  95. // The following is equivalent to:
  96. //return (q.getmatrix() * v);
  97. float qx2 = q.x * q.x;
  98. float qy2 = q.y * q.y;
  99. float qz2 = q.z * q.z;
  100. float qxqy = q.x * q.y;
  101. float qxqz = q.x * q.z;
  102. float qxqw = q.x * q.w;
  103. float qyqz = q.y * q.z;
  104. float qyqw = q.y * q.w;
  105. float qzqw = q.z * q.w;
  106. return new float3((1 - 2 * (qy2 + qz2)) * v.x + (2 * (qxqy - qzqw)) * v.y + (2 * (qxqz + qyqw)) * v.z, (2 * (qxqy + qzqw)) * v.x + (1 - 2 * (qx2 + qz2)) * v.y + (2 * (qyqz - qxqw)) * v.z, (2 * (qxqz - qyqw)) * v.x + (2 * (qyqz + qxqw)) * v.y + (1 - 2 * (qx2 + qy2)) * v.z);
  107. }
  108. public static Quaternion operator +(Quaternion a, Quaternion b)
  109. {
  110. return new Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
  111. }
  112. public static Quaternion operator *(Quaternion a, float b)
  113. {
  114. return new Quaternion(a.x *b, a.y *b, a.z *b, a.w *b);
  115. }
  116. public static Quaternion normalize(Quaternion a)
  117. {
  118. float m = (float)Math.Sqrt(a.w * a.w + a.x * a.x + a.y * a.y + a.z * a.z);
  119. if (m < 0.000000001f)
  120. {
  121. a.w = 1;
  122. a.x = a.y = a.z = 0;
  123. return a;
  124. }
  125. return a * (1f / m);
  126. }
  127. public static float dot(Quaternion a, Quaternion b)
  128. {
  129. return (a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z);
  130. }
  131. public static Quaternion slerp(Quaternion a, Quaternion b, float interp)
  132. {
  133. if (dot(a, b) < 0.0)
  134. {
  135. a.w = -a.w;
  136. a.x = -a.x;
  137. a.y = -a.y;
  138. a.z = -a.z;
  139. }
  140. float d = dot(a, b);
  141. if (d >= 1.0)
  142. {
  143. return a;
  144. }
  145. float theta = (float)Math.Acos(d);
  146. if (theta == 0.0f)
  147. {
  148. return (a);
  149. }
  150. return a * ((float)Math.Sin(theta - interp * theta) / (float)Math.Sin(theta)) + b * ((float)Math.Sin(interp * theta) / (float)Math.Sin(theta));
  151. }
  152. public static Quaternion Interpolate(Quaternion q0, Quaternion q1, float alpha)
  153. {
  154. return slerp(q0, q1, alpha);
  155. }
  156. public static Quaternion Inverse(Quaternion q)
  157. {
  158. return new Quaternion(-q.x, -q.y, -q.z, q.w);
  159. }
  160. public static Quaternion YawPitchRoll(float yaw, float pitch, float roll)
  161. {
  162. roll *= (3.14159264f / 180.0f);
  163. yaw *= (3.14159264f / 180.0f);
  164. pitch *= (3.14159264f / 180.0f);
  165. return new Quaternion(new float3(0.0f, 0.0f, 1.0f), yaw) * new Quaternion(new float3(1.0f, 0.0f, 0.0f), pitch) * new Quaternion(new float3(0.0f, 1.0f, 0.0f), roll);
  166. }
  167. public static float Yaw(Quaternion q)
  168. {
  169. float3 v = q.ydir();
  170. return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f);
  171. }
  172. public static float Pitch(Quaternion q)
  173. {
  174. float3 v = q.ydir();
  175. return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f);
  176. }
  177. public static float Roll(Quaternion q)
  178. {
  179. q = new Quaternion(new float3(0.0f, 0.0f, 1.0f), -Yaw(q) * (3.14159264f / 180.0f)) * q;
  180. q = new Quaternion(new float3(1.0f, 0.0f, 0.0f), -Pitch(q) * (3.14159264f / 180.0f)) * q;
  181. return (float)Math.Atan2(-q.xdir().z, q.xdir().x) * (180.0f / 3.14159264f);
  182. }
  183. }
  184. }