clockwerk-opensim – Blame information for rev 3
?pathlinks?
Rev | Author | Line No. | Line |
---|---|---|---|
1 | vero | 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 | |||
28 | using System; |
||
29 | using System.Collections.Generic; |
||
30 | using System.Linq; |
||
31 | using System.Text; |
||
32 | |||
33 | namespace 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 | } |