aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs168
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 {