diff options
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs | 168 |
1 files changed, 84 insertions, 84 deletions
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs index 58f4f58..85a8cf1 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs | |||
@@ -1,21 +1,21 @@ | |||
1 | /* The MIT License | 1 | /* The MIT License |
2 | * | 2 | * |
3 | * Copyright (c) 2010 Intel Corporation. | 3 | * Copyright (c) 2010 Intel Corporation. |
4 | * All rights reserved. | 4 | * All rights reserved. |
5 | * | 5 | * |
6 | * Based on the convexdecomposition library from | 6 | * Based on the convexdecomposition library from |
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | 7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. |
8 | * | 8 | * |
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | 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 | 10 | * of this software and associated documentation files (the "Software"), to deal |
11 | * in the Software without restriction, including without limitation the rights | 11 | * in the Software without restriction, including without limitation the rights |
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | 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 | 13 | * copies of the Software, and to permit persons to whom the Software is |
14 | * furnished to do so, subject to the following conditions: | 14 | * furnished to do so, subject to the following conditions: |
15 | * | 15 | * |
16 | * The above copyright notice and this permission notice shall be included in | 16 | * The above copyright notice and this permission notice shall be included in |
17 | * all copies or substantial portions of the Software. | 17 | * all copies or substantial portions of the Software. |
18 | * | 18 | * |
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | 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, | 20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | 21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
@@ -127,88 +127,88 @@ namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet | |||
127 | } | 127 | } |
128 | 128 | ||
129 | public static float4x4 Inverse(float4x4 m) | 129 | public static float4x4 Inverse(float4x4 m) |
130 | { | 130 | { |
131 | float4x4 d = new float4x4(); | 131 | float4x4 d = new float4x4(); |
132 | //float dst = d.x.x; | 132 | //float dst = d.x.x; |
133 | float[] tmp = new float[12]; // temp array for pairs | 133 | float[] tmp = new float[12]; // temp array for pairs |
134 | float[] src = new float[16]; // array of transpose source matrix | 134 | float[] src = new float[16]; // array of transpose source matrix |
135 | float det; // determinant | 135 | float det; // determinant |
136 | // transpose matrix | 136 | // transpose matrix |
137 | for (int i = 0; i < 4; i++) | 137 | for (int i = 0; i < 4; i++) |
138 | { | 138 | { |
139 | src[i] = m[i].x; | 139 | src[i] = m[i].x; |
140 | src[i + 4] = m[i].y; | 140 | src[i + 4] = m[i].y; |
141 | src[i + 8] = m[i].z; | 141 | src[i + 8] = m[i].z; |
142 | src[i + 12] = m[i].w; | 142 | src[i + 12] = m[i].w; |
143 | } | 143 | } |
144 | // calculate pairs for first 8 elements (cofactors) | 144 | // calculate pairs for first 8 elements (cofactors) |
145 | tmp[0] = src[10] * src[15]; | 145 | tmp[0] = src[10] * src[15]; |
146 | tmp[1] = src[11] * src[14]; | 146 | tmp[1] = src[11] * src[14]; |
147 | tmp[2] = src[9] * src[15]; | 147 | tmp[2] = src[9] * src[15]; |
148 | tmp[3] = src[11] * src[13]; | 148 | tmp[3] = src[11] * src[13]; |
149 | tmp[4] = src[9] * src[14]; | 149 | tmp[4] = src[9] * src[14]; |
150 | tmp[5] = src[10] * src[13]; | 150 | tmp[5] = src[10] * src[13]; |
151 | tmp[6] = src[8] * src[15]; | 151 | tmp[6] = src[8] * src[15]; |
152 | tmp[7] = src[11] * src[12]; | 152 | tmp[7] = src[11] * src[12]; |
153 | tmp[8] = src[8] * src[14]; | 153 | tmp[8] = src[8] * src[14]; |
154 | tmp[9] = src[10] * src[12]; | 154 | tmp[9] = src[10] * src[12]; |
155 | tmp[10] = src[8] * src[13]; | 155 | tmp[10] = src[8] * src[13]; |
156 | tmp[11] = src[9] * src[12]; | 156 | tmp[11] = src[9] * src[12]; |
157 | // calculate first 8 elements (cofactors) | 157 | // calculate first 8 elements (cofactors) |
158 | d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 173 | d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2]; |
174 | // calculate pairs for second 8 elements (cofactors) | 174 | // calculate pairs for second 8 elements (cofactors) |
175 | tmp[0] = src[2]*src[7]; | 175 | tmp[0] = src[2]*src[7]; |
176 | tmp[1] = src[3]*src[6]; | 176 | tmp[1] = src[3]*src[6]; |
177 | tmp[2] = src[1]*src[7]; | 177 | tmp[2] = src[1]*src[7]; |
178 | tmp[3] = src[3]*src[5]; | 178 | tmp[3] = src[3]*src[5]; |
179 | tmp[4] = src[1]*src[6]; | 179 | tmp[4] = src[1]*src[6]; |
180 | tmp[5] = src[2]*src[5]; | 180 | tmp[5] = src[2]*src[5]; |
181 | tmp[6] = src[0]*src[7]; | 181 | tmp[6] = src[0]*src[7]; |
182 | tmp[7] = src[3]*src[4]; | 182 | tmp[7] = src[3]*src[4]; |
183 | tmp[8] = src[0]*src[6]; | 183 | tmp[8] = src[0]*src[6]; |
184 | tmp[9] = src[2]*src[4]; | 184 | tmp[9] = src[2]*src[4]; |
185 | tmp[10] = src[0]*src[5]; | 185 | tmp[10] = src[0]*src[5]; |
186 | tmp[11] = src[1]*src[4]; | 186 | tmp[11] = src[1]*src[4]; |
187 | // calculate second 8 elements (cofactors) | 187 | // calculate second 8 elements (cofactors) |
188 | d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 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]; | 203 | d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8]; |
204 | // calculate determinant | 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; | 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 | 206 | // calculate matrix inverse |
207 | det = 1/det; | 207 | det = 1/det; |
208 | for (int j = 0; j < 4; j++) | 208 | for (int j = 0; j < 4; j++) |
209 | d[j] *= det; | 209 | d[j] *= det; |
210 | return d; | 210 | return d; |
211 | } | 211 | } |
212 | 212 | ||
213 | public static float4x4 MatrixRigidInverse(float4x4 m) | 213 | public static float4x4 MatrixRigidInverse(float4x4 m) |
214 | { | 214 | { |