float4x4.cs 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284
  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. using System.Collections.Generic;
  29. using System.Linq;
  30. using System.Text;
  31. namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
  32. {
  33. public class float4x4
  34. {
  35. public float4 x = new float4();
  36. public float4 y = new float4();
  37. public float4 z = new float4();
  38. public float4 w = new float4();
  39. public float4x4()
  40. {
  41. }
  42. public float4x4(float4 _x, float4 _y, float4 _z, float4 _w)
  43. {
  44. x = new float4(_x);
  45. y = new float4(_y);
  46. z = new float4(_z);
  47. w = new float4(_w);
  48. }
  49. public float4x4(
  50. float m00, float m01, float m02, float m03,
  51. float m10, float m11, float m12, float m13,
  52. float m20, float m21, float m22, float m23,
  53. float m30, float m31, float m32, float m33)
  54. {
  55. x = new float4(m00, m01, m02, m03);
  56. y = new float4(m10, m11, m12, m13);
  57. z = new float4(m20, m21, m22, m23);
  58. w = new float4(m30, m31, m32, m33);
  59. }
  60. public float4x4(float4x4 m)
  61. {
  62. x = new float4(m.x);
  63. y = new float4(m.y);
  64. z = new float4(m.z);
  65. w = new float4(m.w);
  66. }
  67. public float4 this[int i]
  68. {
  69. get
  70. {
  71. switch (i)
  72. {
  73. case 0: return x;
  74. case 1: return y;
  75. case 2: return z;
  76. case 3: return w;
  77. }
  78. throw new ArgumentOutOfRangeException();
  79. }
  80. set
  81. {
  82. switch (i)
  83. {
  84. case 0: x = value; return;
  85. case 1: y = value; return;
  86. case 2: z = value; return;
  87. case 3: w = value; return;
  88. }
  89. throw new ArgumentOutOfRangeException();
  90. }
  91. }
  92. public override int GetHashCode()
  93. {
  94. return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode();
  95. }
  96. public override bool Equals(object obj)
  97. {
  98. float4x4 m = obj as float4x4;
  99. if (m == null)
  100. return false;
  101. return this == m;
  102. }
  103. public static float4x4 operator *(float4x4 a, float4x4 b)
  104. {
  105. return new float4x4(a.x * b, a.y * b, a.z * b, a.w * b);
  106. }
  107. public static bool operator ==(float4x4 a, float4x4 b)
  108. {
  109. return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w);
  110. }
  111. public static bool operator !=(float4x4 a, float4x4 b)
  112. {
  113. return !(a == b);
  114. }
  115. public static float4x4 Inverse(float4x4 m)
  116. {
  117. float4x4 d = new float4x4();
  118. //float dst = d.x.x;
  119. float[] tmp = new float[12]; // temp array for pairs
  120. float[] src = new float[16]; // array of transpose source matrix
  121. float det; // determinant
  122. // transpose matrix
  123. for (int i = 0; i < 4; i++)
  124. {
  125. src[i] = m[i].x;
  126. src[i + 4] = m[i].y;
  127. src[i + 8] = m[i].z;
  128. src[i + 12] = m[i].w;
  129. }
  130. // calculate pairs for first 8 elements (cofactors)
  131. tmp[0] = src[10] * src[15];
  132. tmp[1] = src[11] * src[14];
  133. tmp[2] = src[9] * src[15];
  134. tmp[3] = src[11] * src[13];
  135. tmp[4] = src[9] * src[14];
  136. tmp[5] = src[10] * src[13];
  137. tmp[6] = src[8] * src[15];
  138. tmp[7] = src[11] * src[12];
  139. tmp[8] = src[8] * src[14];
  140. tmp[9] = src[10] * src[12];
  141. tmp[10] = src[8] * src[13];
  142. tmp[11] = src[9] * src[12];
  143. // calculate first 8 elements (cofactors)
  144. d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
  145. d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
  146. d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
  147. d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
  148. d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
  149. d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
  150. d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
  151. d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
  152. d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
  153. d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
  154. d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
  155. d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
  156. d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
  157. d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
  158. d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
  159. d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
  160. // calculate pairs for second 8 elements (cofactors)
  161. tmp[0] = src[2]*src[7];
  162. tmp[1] = src[3]*src[6];
  163. tmp[2] = src[1]*src[7];
  164. tmp[3] = src[3]*src[5];
  165. tmp[4] = src[1]*src[6];
  166. tmp[5] = src[2]*src[5];
  167. tmp[6] = src[0]*src[7];
  168. tmp[7] = src[3]*src[4];
  169. tmp[8] = src[0]*src[6];
  170. tmp[9] = src[2]*src[4];
  171. tmp[10] = src[0]*src[5];
  172. tmp[11] = src[1]*src[4];
  173. // calculate second 8 elements (cofactors)
  174. d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
  175. d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
  176. d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
  177. d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
  178. d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
  179. d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
  180. d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
  181. d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
  182. d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
  183. d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
  184. d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
  185. d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
  186. d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
  187. d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
  188. d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
  189. d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
  190. // calculate determinant
  191. det = src[0] * d.x.x + src[1] * d.x.y + src[2] * d.x.z + src[3] * d.x.w;
  192. // calculate matrix inverse
  193. det = 1/det;
  194. for (int j = 0; j < 4; j++)
  195. d[j] *= det;
  196. return d;
  197. }
  198. public static float4x4 MatrixRigidInverse(float4x4 m)
  199. {
  200. float4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
  201. float4x4 rot = new float4x4(m);
  202. rot.w = new float4(0f, 0f, 0f, 1f);
  203. return trans_inverse * MatrixTranspose(rot);
  204. }
  205. public static float4x4 MatrixTranspose(float4x4 m)
  206. {
  207. return new float4x4(m.x.x, m.y.x, m.z.x, m.w.x, m.x.y, m.y.y, m.z.y, m.w.y, m.x.z, m.y.z, m.z.z, m.w.z, m.x.w, m.y.w, m.z.w, m.w.w);
  208. }
  209. public static float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf)
  210. {
  211. float h = 1.0f / (float)Math.Tan(fovy / 2.0f); // view space height
  212. float w = h / aspect; // view space width
  213. return new float4x4(w, 0, 0, 0, 0, h, 0, 0, 0, 0, zf / (zn - zf), -1, 0, 0, zn * zf / (zn - zf), 0);
  214. }
  215. public static float4x4 MatrixTranslation(float3 t)
  216. {
  217. return new float4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, t.x, t.y, t.z, 1);
  218. }
  219. public static float4x4 MatrixRotationZ(float angle_radians)
  220. {
  221. float s = (float)Math.Sin(angle_radians);
  222. float c = (float)Math.Cos(angle_radians);
  223. return new float4x4(c, s, 0, 0, -s, c, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
  224. }
  225. public static float4x4 MatrixLookAt(float3 eye, float3 at, float3 up)
  226. {
  227. float4x4 m = new float4x4();
  228. m.w.w = 1.0f;
  229. m.w.setxyz(eye);
  230. m.z.setxyz(float3.normalize(eye - at));
  231. m.x.setxyz(float3.normalize(float3.cross(up, m.z.xyz())));
  232. m.y.setxyz(float3.cross(m.z.xyz(), m.x.xyz()));
  233. return MatrixRigidInverse(m);
  234. }
  235. public static float4x4 MatrixFromQuatVec(Quaternion q, float3 v)
  236. {
  237. // builds a 4x4 transformation matrix based on orientation q and translation v
  238. float qx2 = q.x * q.x;
  239. float qy2 = q.y * q.y;
  240. float qz2 = q.z * q.z;
  241. float qxqy = q.x * q.y;
  242. float qxqz = q.x * q.z;
  243. float qxqw = q.x * q.w;
  244. float qyqz = q.y * q.z;
  245. float qyqw = q.y * q.w;
  246. float qzqw = q.z * q.w;
  247. return new float4x4(
  248. 1 - 2 * (qy2 + qz2),
  249. 2 * (qxqy + qzqw),
  250. 2 * (qxqz - qyqw),
  251. 0,
  252. 2 * (qxqy - qzqw),
  253. 1 - 2 * (qx2 + qz2),
  254. 2 * (qyqz + qxqw),
  255. 0,
  256. 2 * (qxqz + qyqw),
  257. 2 * (qyqz - qxqw),
  258. 1 - 2 * (qx2 + qy2),
  259. 0,
  260. v.x,
  261. v.y,
  262. v.z,
  263. 1.0f);
  264. }
  265. }
  266. }