aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs1162
1 files changed, 581 insertions, 581 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
index 2e3f258..15c7bb8 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
@@ -1,581 +1,581 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software 14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26using MonoXnaCompactMaths; 26using MonoXnaCompactMaths;
27 27
28namespace XnaDevRu.BulletX 28namespace XnaDevRu.BulletX
29{ 29{
30 public static class MathHelper 30 public static class MathHelper
31 { 31 {
32 internal const float Sqrt12 = 0.7071067811865475244008443621048490f; 32 internal const float Sqrt12 = 0.7071067811865475244008443621048490f;
33 internal const float Infinity = 3.402823466e+38f; 33 internal const float Infinity = 3.402823466e+38f;
34 internal const float Epsilon = 1.192092896e-07f; 34 internal const float Epsilon = 1.192092896e-07f;
35 35
36 public static Vector3 MatrixToVector(Matrix m, Vector3 v) 36 public static Vector3 MatrixToVector(Matrix m, Vector3 v)
37 { 37 {
38 return new Vector3( 38 return new Vector3(
39 Vector3.Dot(new Vector3(m.M11, m.M12, m.M13), v) + m.Translation.X, 39 Vector3.Dot(new Vector3(m.M11, m.M12, m.M13), v) + m.Translation.X,
40 Vector3.Dot(new Vector3(m.M21, m.M22, m.M23), v) + m.Translation.Y, 40 Vector3.Dot(new Vector3(m.M21, m.M22, m.M23), v) + m.Translation.Y,
41 Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z 41 Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z
42 ); 42 );
43 } 43 }
44 44
45 internal static int ClosestAxis(Vector4 v) 45 internal static int ClosestAxis(Vector4 v)
46 { 46 {
47 return MaxAxis(Absolute(v)); 47 return MaxAxis(Absolute(v));
48 } 48 }
49 49
50 internal static Vector4 Absolute(Vector4 v) 50 internal static Vector4 Absolute(Vector4 v)
51 { 51 {
52 return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W)); 52 return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W));
53 } 53 }
54 54
55 internal static int MaxAxis(Vector4 v) 55 internal static int MaxAxis(Vector4 v)
56 { 56 {
57 int maxIndex = -1; 57 int maxIndex = -1;
58 float maxVal = float.MinValue; 58 float maxVal = float.MinValue;
59 if (v.X > maxVal) 59 if (v.X > maxVal)
60 { 60 {
61 maxIndex = 0; 61 maxIndex = 0;
62 maxVal = v.X; 62 maxVal = v.X;
63 } 63 }
64 if (v.Y > maxVal) 64 if (v.Y > maxVal)
65 { 65 {
66 maxIndex = 1; 66 maxIndex = 1;
67 maxVal = v.Y; 67 maxVal = v.Y;
68 } 68 }
69 if (v.Z > maxVal) 69 if (v.Z > maxVal)
70 { 70 {
71 maxIndex = 2; 71 maxIndex = 2;
72 maxVal = v.Z; 72 maxVal = v.Z;
73 } 73 }
74 if (v.W > maxVal) 74 if (v.W > maxVal)
75 { 75 {
76 maxIndex = 3; 76 maxIndex = 3;
77 maxVal = v.W; 77 maxVal = v.W;
78 } 78 }
79 79
80 return maxIndex; 80 return maxIndex;
81 } 81 }
82 82
83 internal static int MaxAxis(Vector3 v) 83 internal static int MaxAxis(Vector3 v)
84 { 84 {
85 return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0); 85 return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0);
86 } 86 }
87 87
88 // conservative test for overlap between two aabbs 88 // conservative test for overlap between two aabbs
89 internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB) 89 internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB)
90 { 90 {
91 bool overlap = true; 91 bool overlap = true;
92 overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap; 92 overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap;
93 overlap = (aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z) ? false : overlap; 93 overlap = (aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z) ? false : overlap;
94 overlap = (aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y) ? false : overlap; 94 overlap = (aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y) ? false : overlap;
95 return overlap; 95 return overlap;
96 } 96 }
97 97
98 internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax) 98 internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax)
99 { 99 {
100 Vector3 p1 = vertices[0]; 100 Vector3 p1 = vertices[0];
101 Vector3 p2 = vertices[1]; 101 Vector3 p2 = vertices[1];
102 Vector3 p3 = vertices[2]; 102 Vector3 p3 = vertices[2];
103 103
104 if (Math.Min(Math.Min(p1.X, p2.X), p3.X) > aabbMax.X) return false; 104 if (Math.Min(Math.Min(p1.X, p2.X), p3.X) > aabbMax.X) return false;
105 if (Math.Max(Math.Max(p1.X, p2.X), p3.X) < aabbMin.X) return false; 105 if (Math.Max(Math.Max(p1.X, p2.X), p3.X) < aabbMin.X) return false;
106 106
107 if (Math.Min(Math.Min(p1.Z, p2.Z), p3.Z) > aabbMax.Z) return false; 107 if (Math.Min(Math.Min(p1.Z, p2.Z), p3.Z) > aabbMax.Z) return false;
108 if (Math.Max(Math.Max(p1.Z, p2.Z), p3.Z) < aabbMin.Z) return false; 108 if (Math.Max(Math.Max(p1.Z, p2.Z), p3.Z) < aabbMin.Z) return false;
109 109
110 if (Math.Min(Math.Min(p1.Y, p2.Y), p3.Y) > aabbMax.Y) return false; 110 if (Math.Min(Math.Min(p1.Y, p2.Y), p3.Y) > aabbMax.Y) return false;
111 if (Math.Max(Math.Max(p1.Y, p2.Y), p3.Y) < aabbMin.Y) return false; 111 if (Math.Max(Math.Max(p1.Y, p2.Y), p3.Y) < aabbMin.Y) return false;
112 return true; 112 return true;
113 } 113 }
114 114
115 internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated) 115 internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated)
116 { 116 {
117 float s = 1.0f - rt; 117 float s = 1.0f - rt;
118 interpolated.X = s * vA.X + rt * vB.X; 118 interpolated.X = s * vA.X + rt * vB.X;
119 interpolated.Y = s * vA.Y + rt * vB.Y; 119 interpolated.Y = s * vA.Y + rt * vB.Y;
120 interpolated.Z = s * vA.Z + rt * vB.Z; 120 interpolated.Z = s * vA.Z + rt * vB.Z;
121 } 121 }
122 122
123 internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q) 123 internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q)
124 { 124 {
125 if (Math.Abs(n.Z) > Sqrt12) 125 if (Math.Abs(n.Z) > Sqrt12)
126 { 126 {
127 // choose p in y-z plane 127 // choose p in y-z plane
128 float a = n.Y * n.Y + n.Z * n.Z; 128 float a = n.Y * n.Y + n.Z * n.Z;
129 float k = 1f / (float)Math.Sqrt(a); 129 float k = 1f / (float)Math.Sqrt(a);
130 p.X = 0; 130 p.X = 0;
131 p.Y = -n.Z * k; 131 p.Y = -n.Z * k;
132 p.Z = n.Y * k; 132 p.Z = n.Y * k;
133 // set q = n x p 133 // set q = n x p
134 q.X = a * k; 134 q.X = a * k;
135 q.Y = -n.X * p.Z; 135 q.Y = -n.X * p.Z;
136 q.Z = n.X * p.Y; 136 q.Z = n.X * p.Y;
137 } 137 }
138 else 138 else
139 { 139 {
140 // choose p in x-y plane 140 // choose p in x-y plane
141 float a = n.X * n.X + n.Y * n.Y; 141 float a = n.X * n.X + n.Y * n.Y;
142 float k = 1f / (float)Math.Sqrt(a); 142 float k = 1f / (float)Math.Sqrt(a);
143 p.X = -n.Y * k; 143 p.X = -n.Y * k;
144 p.Y = n.X * k; 144 p.Y = n.X * k;
145 p.Z = 0; 145 p.Z = 0;
146 // set q = n x p 146 // set q = n x p
147 q.X = -n.Z * p.Y; 147 q.X = -n.Z * p.Y;
148 q.Y = n.Z * p.X; 148 q.Y = n.Z * p.X;
149 q.Z = a * k; 149 q.Z = a * k;
150 } 150 }
151 } 151 }
152 152
153 internal static bool RayAabb(Vector3 rayFrom, 153 internal static bool RayAabb(Vector3 rayFrom,
154 Vector3 rayTo, 154 Vector3 rayTo,
155 Vector3 aabbMin, 155 Vector3 aabbMin,
156 Vector3 aabbMax, 156 Vector3 aabbMax,
157 float param, Vector3 normal) 157 float param, Vector3 normal)
158 { 158 {
159 Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f; 159 Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f;
160 Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f; 160 Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f;
161 Vector3 source = rayFrom - aabbCenter; 161 Vector3 source = rayFrom - aabbCenter;
162 Vector3 target = rayTo - aabbCenter; 162 Vector3 target = rayTo - aabbCenter;
163 int sourceOutcode = Outcode(source, aabbHalfExtent); 163 int sourceOutcode = Outcode(source, aabbHalfExtent);
164 int targetOutcode = Outcode(target, aabbHalfExtent); 164 int targetOutcode = Outcode(target, aabbHalfExtent);
165 if ((sourceOutcode & targetOutcode) == 0x0) 165 if ((sourceOutcode & targetOutcode) == 0x0)
166 { 166 {
167 float lambda_enter = 0; 167 float lambda_enter = 0;
168 float lambda_exit = param; 168 float lambda_exit = param;
169 Vector3 r = target - source; 169 Vector3 r = target - source;
170 float normSign = 1; 170 float normSign = 1;
171 Vector3 hitNormal = new Vector3(); 171 Vector3 hitNormal = new Vector3();
172 int bit = 1; 172 int bit = 1;
173 173
174 for (int j = 0; j < 2; j++) 174 for (int j = 0; j < 2; j++)
175 { 175 {
176 { 176 {
177 if ((sourceOutcode & bit) != 0) 177 if ((sourceOutcode & bit) != 0)
178 { 178 {
179 float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; 179 float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X;
180 if (lambda_enter <= lambda) 180 if (lambda_enter <= lambda)
181 { 181 {
182 lambda_enter = lambda; 182 lambda_enter = lambda;
183 hitNormal = new Vector3(); 183 hitNormal = new Vector3();
184 hitNormal.X = normSign; 184 hitNormal.X = normSign;
185 } 185 }
186 } 186 }
187 else if ((targetOutcode & bit) != 0) 187 else if ((targetOutcode & bit) != 0)
188 { 188 {
189 float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; 189 float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X;
190 SetMin(ref lambda_exit, lambda); 190 SetMin(ref lambda_exit, lambda);
191 } 191 }
192 bit <<= 1; 192 bit <<= 1;
193 } 193 }
194 { 194 {
195 if ((sourceOutcode & bit) != 0) 195 if ((sourceOutcode & bit) != 0)
196 { 196 {
197 float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; 197 float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y;
198 if (lambda_enter <= lambda) 198 if (lambda_enter <= lambda)
199 { 199 {
200 lambda_enter = lambda; 200 lambda_enter = lambda;
201 hitNormal = new Vector3(); 201 hitNormal = new Vector3();
202 hitNormal.Y = normSign; 202 hitNormal.Y = normSign;
203 } 203 }
204 } 204 }
205 else if ((targetOutcode & bit) != 0) 205 else if ((targetOutcode & bit) != 0)
206 { 206 {
207 float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; 207 float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y;
208 SetMin(ref lambda_exit, lambda); 208 SetMin(ref lambda_exit, lambda);
209 } 209 }
210 bit <<= 1; 210 bit <<= 1;
211 } 211 }
212 { 212 {
213 if ((sourceOutcode & bit) != 0) 213 if ((sourceOutcode & bit) != 0)
214 { 214 {
215 float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; 215 float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z;
216 if (lambda_enter <= lambda) 216 if (lambda_enter <= lambda)
217 { 217 {
218 lambda_enter = lambda; 218 lambda_enter = lambda;
219 hitNormal = new Vector3(); 219 hitNormal = new Vector3();
220 hitNormal.Z = normSign; 220 hitNormal.Z = normSign;
221 } 221 }
222 } 222 }
223 else if ((targetOutcode & bit) != 0) 223 else if ((targetOutcode & bit) != 0)
224 { 224 {
225 float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; 225 float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z;
226 SetMin(ref lambda_exit, lambda); 226 SetMin(ref lambda_exit, lambda);
227 } 227 }
228 bit <<= 1; 228 bit <<= 1;
229 } 229 }
230 normSign = -1; 230 normSign = -1;
231 } 231 }
232 if (lambda_enter <= lambda_exit) 232 if (lambda_enter <= lambda_exit)
233 { 233 {
234 param = lambda_enter; 234 param = lambda_enter;
235 normal = hitNormal; 235 normal = hitNormal;
236 return true; 236 return true;
237 } 237 }
238 } 238 }
239 return false; 239 return false;
240 } 240 }
241 241
242 internal static void SetMin(ref float a, float b) 242 internal static void SetMin(ref float a, float b)
243 { 243 {
244 if (a > b) 244 if (a > b)
245 a = b; 245 a = b;
246 } 246 }
247 247
248 internal static void SetMax(ref float a, float b) 248 internal static void SetMax(ref float a, float b)
249 { 249 {
250 if (a < b) 250 if (a < b)
251 a = b; 251 a = b;
252 } 252 }
253 253
254 internal static void SetMax(ref Vector3 self, Vector3 other) 254 internal static void SetMax(ref Vector3 self, Vector3 other)
255 { 255 {
256 if (other.X > self.X) 256 if (other.X > self.X)
257 self.X = other.X; 257 self.X = other.X;
258 258
259 if (other.Y > self.Y) 259 if (other.Y > self.Y)
260 self.Y = other.Y; 260 self.Y = other.Y;
261 261
262 if (other.Z > self.Z) 262 if (other.Z > self.Z)
263 self.Z = other.Z; 263 self.Z = other.Z;
264 } 264 }
265 265
266 internal static Vector3 SetMax(Vector3 self, Vector3 other) 266 internal static Vector3 SetMax(Vector3 self, Vector3 other)
267 { 267 {
268 if (other.X > self.X) 268 if (other.X > self.X)
269 self.X = other.X; 269 self.X = other.X;
270 270
271 if (other.Y > self.Y) 271 if (other.Y > self.Y)
272 self.Y = other.Y; 272 self.Y = other.Y;
273 273
274 if (other.Z > self.Z) 274 if (other.Z > self.Z)
275 self.Z = other.Z; 275 self.Z = other.Z;
276 276
277 return self; 277 return self;
278 } 278 }
279 279
280 internal static void SetMin(ref Vector3 self, Vector3 other) 280 internal static void SetMin(ref Vector3 self, Vector3 other)
281 { 281 {
282 if (other.X < self.X) 282 if (other.X < self.X)
283 self.X = other.X; 283 self.X = other.X;
284 284
285 if (other.Y < self.Y) 285 if (other.Y < self.Y)
286 self.Y = other.Y; 286 self.Y = other.Y;
287 287
288 if (other.Z < self.Z) 288 if (other.Z < self.Z)
289 self.Z = other.Z; 289 self.Z = other.Z;
290 } 290 }
291 291
292 internal static Vector3 SetMin(Vector3 self, Vector3 other) 292 internal static Vector3 SetMin(Vector3 self, Vector3 other)
293 { 293 {
294 if (other.X < self.X) 294 if (other.X < self.X)
295 self.X = other.X; 295 self.X = other.X;
296 296
297 if (other.Y < self.Y) 297 if (other.Y < self.Y)
298 self.Y = other.Y; 298 self.Y = other.Y;
299 299
300 if (other.Z < self.Z) 300 if (other.Z < self.Z)
301 self.Z = other.Z; 301 self.Z = other.Z;
302 302
303 return self; 303 return self;
304 } 304 }
305 305
306 internal static int Outcode(Vector3 p, Vector3 halfExtent) 306 internal static int Outcode(Vector3 p, Vector3 halfExtent)
307 { 307 {
308 return (p.X < -halfExtent.X ? 0x01 : 0x0) | 308 return (p.X < -halfExtent.X ? 0x01 : 0x0) |
309 (p.X > halfExtent.X ? 0x08 : 0x0) | 309 (p.X > halfExtent.X ? 0x08 : 0x0) |
310 (p.Y < -halfExtent.Y ? 0x02 : 0x0) | 310 (p.Y < -halfExtent.Y ? 0x02 : 0x0) |
311 (p.Y > halfExtent.Y ? 0x10 : 0x0) | 311 (p.Y > halfExtent.Y ? 0x10 : 0x0) |
312 (p.Z < -halfExtent.Z ? 0x4 : 0x0) | 312 (p.Z < -halfExtent.Z ? 0x4 : 0x0) |
313 (p.Z > halfExtent.Z ? 0x20 : 0x0); 313 (p.Z > halfExtent.Z ? 0x20 : 0x0);
314 } 314 }
315 315
316 internal static Matrix Absolute(Matrix m) 316 internal static Matrix Absolute(Matrix m)
317 { 317 {
318 return new Matrix(Math.Abs(m.M11), Math.Abs(m.M12), Math.Abs(m.M13), Math.Abs(m.M14), 318 return new Matrix(Math.Abs(m.M11), Math.Abs(m.M12), Math.Abs(m.M13), Math.Abs(m.M14),
319 Math.Abs(m.M21), Math.Abs(m.M22), Math.Abs(m.M23), Math.Abs(m.M24), 319 Math.Abs(m.M21), Math.Abs(m.M22), Math.Abs(m.M23), Math.Abs(m.M24),
320 Math.Abs(m.M31), Math.Abs(m.M32), Math.Abs(m.M33), Math.Abs(m.M34), 320 Math.Abs(m.M31), Math.Abs(m.M32), Math.Abs(m.M33), Math.Abs(m.M34),
321 Math.Abs(m.M41), Math.Abs(m.M42), Math.Abs(m.M43), Math.Abs(m.M44)); 321 Math.Abs(m.M41), Math.Abs(m.M42), Math.Abs(m.M43), Math.Abs(m.M44));
322 } 322 }
323 323
324 internal static void SetValueByIndex(ref Vector3 v, int i, float value) 324 internal static void SetValueByIndex(ref Vector3 v, int i, float value)
325 { 325 {
326 if (i == 0) 326 if (i == 0)
327 v.X = value; 327 v.X = value;
328 else if (i == 1) 328 else if (i == 1)
329 v.Y = value; 329 v.Y = value;
330 else 330 else
331 v.Z = value; 331 v.Z = value;
332 } 332 }
333 333
334 internal static float GetValueByIndex(Vector3 v, int i) 334 internal static float GetValueByIndex(Vector3 v, int i)
335 { 335 {
336 if (i == 0) 336 if (i == 0)
337 return v.X; 337 return v.X;
338 else if (i == 1) 338 else if (i == 1)
339 return v.Y; 339 return v.Y;
340 else 340 else
341 return v.Z; 341 return v.Z;
342 } 342 }
343 343
344 internal static Vector3 InvXForm(Matrix m, Vector3 v) 344 internal static Vector3 InvXForm(Matrix m, Vector3 v)
345 { 345 {
346 v -= m.Translation; 346 v -= m.Translation;
347 m.Translation = new Vector3(); 347 m.Translation = new Vector3();
348 return MathHelper.Transform(v, Matrix.Transpose(m)); 348 return MathHelper.Transform(v, Matrix.Transpose(m));
349 } 349 }
350 350
351 internal static Matrix InverseTimes(Matrix m, Matrix t) 351 internal static Matrix InverseTimes(Matrix m, Matrix t)
352 { 352 {
353 Vector3 v = t.Translation - m.Translation; 353 Vector3 v = t.Translation - m.Translation;
354 354
355 Matrix mat = TransposeTimes(m, t); 355 Matrix mat = TransposeTimes(m, t);
356 mat.Translation = Vector3.Transform(v, m); 356 mat.Translation = Vector3.Transform(v, m);
357 return mat; 357 return mat;
358 } 358 }
359 359
360 internal static Matrix TransposeTimes(Matrix mA, Matrix mB) 360 internal static Matrix TransposeTimes(Matrix mA, Matrix mB)
361 { 361 {
362 return new Matrix( 362 return new Matrix(
363 mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31, 363 mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31,
364 mA.M11 * mB.M12 + mA.M21 * mB.M22 + mA.M31 * mB.M32, 364 mA.M11 * mB.M12 + mA.M21 * mB.M22 + mA.M31 * mB.M32,
365 mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33, 365 mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33,
366 0, 366 0,
367 mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31, 367 mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31,
368 mA.M12 * mB.M12 + mA.M22 * mB.M22 + mA.M32 * mB.M32, 368 mA.M12 * mB.M12 + mA.M22 * mB.M22 + mA.M32 * mB.M32,
369 mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33, 369 mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33,
370 0, 370 0,
371 mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31, 371 mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31,
372 mA.M13 * mB.M12 + mA.M23 * mB.M22 + mA.M33 * mB.M32, 372 mA.M13 * mB.M12 + mA.M23 * mB.M22 + mA.M33 * mB.M32,
373 mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33, 373 mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33,
374 0, 0, 0, 0, 1); 374 0, 0, 0, 0, 1);
375 } 375 }
376 376
377 internal static Vector3 GetColumn(Matrix m, int column) 377 internal static Vector3 GetColumn(Matrix m, int column)
378 { 378 {
379 switch (column) 379 switch (column)
380 { 380 {
381 case 1: 381 case 1:
382 return new Vector3(m.M11, m.M21, m.M31); 382 return new Vector3(m.M11, m.M21, m.M31);
383 case 2: 383 case 2:
384 return new Vector3(m.M12, m.M22, m.M32); 384 return new Vector3(m.M12, m.M22, m.M32);
385 case 3: 385 case 3:
386 return new Vector3(m.M13, m.M23, m.M33); 386 return new Vector3(m.M13, m.M23, m.M33);
387 default: 387 default:
388 throw new ArgumentOutOfRangeException("column"); 388 throw new ArgumentOutOfRangeException("column");
389 } 389 }
390 } 390 }
391 391
392 internal static Vector3 GetRow(Matrix m, int row) 392 internal static Vector3 GetRow(Matrix m, int row)
393 { 393 {
394 switch (row) 394 switch (row)
395 { 395 {
396 case 1: 396 case 1:
397 return new Vector3(m.M11, m.M12, m.M13); 397 return new Vector3(m.M11, m.M12, m.M13);
398 case 2: 398 case 2:
399 return new Vector3(m.M21, m.M22, m.M23); 399 return new Vector3(m.M21, m.M22, m.M23);
400 case 3: 400 case 3:
401 return new Vector3(m.M31, m.M32, m.M33); 401 return new Vector3(m.M31, m.M32, m.M33);
402 default: 402 default:
403 throw new ArgumentOutOfRangeException("row"); 403 throw new ArgumentOutOfRangeException("row");
404 } 404 }
405 } 405 }
406 406
407 internal static Quaternion GetRotation(Matrix m) 407 internal static Quaternion GetRotation(Matrix m)
408 { 408 {
409 float trace = m.M11 + m.M22 + m.M33; 409 float trace = m.M11 + m.M22 + m.M33;
410 Quaternion q = new Quaternion(); 410 Quaternion q = new Quaternion();
411 411
412 if (trace > 0) 412 if (trace > 0)
413 { 413 {
414 float s = (float)Math.Sqrt(trace + 1.0f); 414 float s = (float)Math.Sqrt(trace + 1.0f);
415 q.W = s * 0.5f; 415 q.W = s * 0.5f;
416 s = 0.5f / s; 416 s = 0.5f / s;
417 417
418 q.X = (m.M32 - m.M23) * s; 418 q.X = (m.M32 - m.M23) * s;
419 q.Y = (m.M13 - m.M31) * s; 419 q.Y = (m.M13 - m.M31) * s;
420 q.Z = (m.M21 - m.M12) * s; 420 q.Z = (m.M21 - m.M12) * s;
421 } 421 }
422 else 422 else
423 { 423 {
424 int i = m.M11 < m.M22 ? 424 int i = m.M11 < m.M22 ?
425 (m.M22 < m.M33 ? 2 : 1) : 425 (m.M22 < m.M33 ? 2 : 1) :
426 (m.M11 < m.M33 ? 2 : 0); 426 (m.M11 < m.M33 ? 2 : 0);
427 int j = (i + 1) % 3; 427 int j = (i + 1) % 3;
428 int k = (i + 2) % 3; 428 int k = (i + 2) % 3;
429 429
430 float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1.0f); 430 float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1.0f);
431 SetElement(ref q, i, s * 0.5f); 431 SetElement(ref q, i, s * 0.5f);
432 s = 0.5f / s; 432 s = 0.5f / s;
433 433
434 q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s; 434 q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s;
435 SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j)) * s); 435 SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j)) * s);
436 SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s); 436 SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s);
437 } 437 }
438 return q; 438 return q;
439 } 439 }
440 440
441 internal static float SetElement(ref Quaternion q, int index, float value) 441 internal static float SetElement(ref Quaternion q, int index, float value)
442 { 442 {
443 switch (index) 443 switch (index)
444 { 444 {
445 case 0: 445 case 0:
446 q.X = value; break; 446 q.X = value; break;
447 case 1: 447 case 1:
448 q.Y = value; break; 448 q.Y = value; break;
449 case 2: 449 case 2:
450 q.Z = value; break; 450 q.Z = value; break;
451 case 3: 451 case 3:
452 q.W = value; break; 452 q.W = value; break;
453 } 453 }
454 454
455 return 0; 455 return 0;
456 } 456 }
457 457
458 internal static float GetElement(Quaternion q, int index) 458 internal static float GetElement(Quaternion q, int index)
459 { 459 {
460 switch (index) 460 switch (index)
461 { 461 {
462 case 0: 462 case 0:
463 return q.X; 463 return q.X;
464 case 1: 464 case 1:
465 return q.Y; 465 return q.Y;
466 case 2: 466 case 2:
467 return q.Z; 467 return q.Z;
468 default: 468 default:
469 return q.W; 469 return q.W;
470 } 470 }
471 } 471 }
472 472
473 internal static float GetElement(Matrix mat, int index) 473 internal static float GetElement(Matrix mat, int index)
474 { 474 {
475 int row = index % 3; 475 int row = index % 3;
476 int col = index / 3; 476 int col = index / 3;
477 477
478 return GetElement(mat, row, col); 478 return GetElement(mat, row, col);
479 } 479 }
480 480
481 internal static float GetElement(Matrix mat, int row, int col) 481 internal static float GetElement(Matrix mat, int row, int col)
482 { 482 {
483 switch (row) 483 switch (row)
484 { 484 {
485 case 0: 485 case 0:
486 switch (col) 486 switch (col)
487 { 487 {
488 case 0: 488 case 0:
489 return mat.M11; 489 return mat.M11;
490 case 1: 490 case 1:
491 return mat.M12; 491 return mat.M12;
492 case 2: 492 case 2:
493 return mat.M13; 493 return mat.M13;
494 } break; 494 } break;
495 case 1: 495 case 1:
496 switch (col) 496 switch (col)
497 { 497 {
498 case 0: 498 case 0:
499 return mat.M21; 499 return mat.M21;
500 case 1: 500 case 1:
501 return mat.M22; 501 return mat.M22;
502 case 2: 502 case 2:
503 return mat.M23; 503 return mat.M23;
504 } break; 504 } break;
505 case 2: 505 case 2:
506 switch (col) 506 switch (col)
507 { 507 {
508 case 0: 508 case 0:
509 return mat.M31; 509 return mat.M31;
510 case 1: 510 case 1:
511 return mat.M32; 511 return mat.M32;
512 case 2: 512 case 2:
513 return mat.M33; 513 return mat.M33;
514 } break; 514 } break;
515 } 515 }
516 516
517 return 0; 517 return 0;
518 } 518 }
519 519
520 internal static float GetElement(Vector3 v, int index) 520 internal static float GetElement(Vector3 v, int index)
521 { 521 {
522 if (index == 0) 522 if (index == 0)
523 return v.X; 523 return v.X;
524 if (index == 1) 524 if (index == 1)
525 return v.Y; 525 return v.Y;
526 if (index == 2) 526 if (index == 2)
527 return v.Z; 527 return v.Z;
528 528
529 throw new ArgumentOutOfRangeException("index"); 529 throw new ArgumentOutOfRangeException("index");
530 } 530 }
531 531
532 internal static void SetElement(ref Vector3 v, int index, float value) 532 internal static void SetElement(ref Vector3 v, int index, float value)
533 { 533 {
534 if (index == 0) 534 if (index == 0)
535 v.X = value; 535 v.X = value;
536 else if (index == 1) 536 else if (index == 1)
537 v.Y = value; 537 v.Y = value;
538 else if (index == 2) 538 else if (index == 2)
539 v.Z = value; 539 v.Z = value;
540 else 540 else
541 throw new ArgumentOutOfRangeException("index"); 541 throw new ArgumentOutOfRangeException("index");
542 } 542 }
543 543
544 public static Matrix InvertMatrix(Matrix m) 544 public static Matrix InvertMatrix(Matrix m)
545 { 545 {
546 Vector3 pos = m.Translation; 546 Vector3 pos = m.Translation;
547 m.Translation = Vector3.Zero; 547 m.Translation = Vector3.Zero;
548 Matrix inv = Matrix.Transpose(m); 548 Matrix inv = Matrix.Transpose(m);
549 pos = Vector3.Transform(-pos, m); 549 pos = Vector3.Transform(-pos, m);
550 inv.Translation = pos; 550 inv.Translation = pos;
551 return inv; 551 return inv;
552 } 552 }
553 553
554 public static Matrix GetDisplayMatrix(Matrix m) 554 public static Matrix GetDisplayMatrix(Matrix m)
555 { 555 {
556 Matrix displayMatrix = m; 556 Matrix displayMatrix = m;
557 displayMatrix.Translation = Vector3.Zero; 557 displayMatrix.Translation = Vector3.Zero;
558 displayMatrix = Matrix.Transpose(displayMatrix); 558 displayMatrix = Matrix.Transpose(displayMatrix);
559 displayMatrix.Translation = m.Translation; 559 displayMatrix.Translation = m.Translation;
560 return displayMatrix; 560 return displayMatrix;
561 } 561 }
562 562
563 internal static Vector3 Transform(Vector3 position, Matrix matrix) 563 internal static Vector3 Transform(Vector3 position, Matrix matrix)
564 { 564 {
565 Vector3 vector = new Vector3(); 565 Vector3 vector = new Vector3();
566 vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)) + matrix.M41; 566 vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)) + matrix.M41;
567 vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)) + matrix.M42; 567 vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)) + matrix.M42;
568 vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43; 568 vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43;
569 return vector; 569 return vector;
570 } 570 }
571 571
572 internal static Vector3 TransformNormal(Vector3 position, Matrix matrix) 572 internal static Vector3 TransformNormal(Vector3 position, Matrix matrix)
573 { 573 {
574 Vector3 vector = new Vector3(); 574 Vector3 vector = new Vector3();
575 vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)); 575 vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13));
576 vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)); 576 vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23));
577 vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)); 577 vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33));
578 return vector; 578 return vector;
579 } 579 }
580 } 580 }
581} 581}
generated by cgit v1.1 at 2024-08-24 09:04:42 +0000