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