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