aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs232
1 files changed, 116 insertions, 116 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
index 04e42d9..93a99f2 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
@@ -1,116 +1,116 @@
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;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 internal static class MatrixOperations 29 internal static class MatrixOperations
30 { 30 {
31 public static void SetRotation(ref Matrix m, Quaternion q) 31 public static void SetRotation(ref Matrix m, Quaternion q)
32 { 32 {
33 float d = q.LengthSquared(); 33 float d = q.LengthSquared();
34 BulletDebug.Assert(d != 0); 34 BulletDebug.Assert(d != 0);
35 float s = 2f / d; 35 float s = 2f / d;
36 float xs = q.X * s, ys = q.Y * s, zs = q.Z * s; 36 float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
37 float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs; 37 float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
38 float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs; 38 float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
39 float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs; 39 float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
40 m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0, 40 m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0,
41 xy + wz, 1 - (xx + zz), yz - wx, 0, 41 xy + wz, 1 - (xx + zz), yz - wx, 0,
42 xz - wy, yz + wx, 1 - (xx + yy), 0, 42 xz - wy, yz + wx, 1 - (xx + yy), 0,
43 m.M41, m.M42, m.M43, 1); 43 m.M41, m.M42, m.M43, 1);
44 } 44 }
45 45
46 public static Quaternion GetRotation(Matrix m) 46 public static Quaternion GetRotation(Matrix m)
47 { 47 {
48 Quaternion q = new Quaternion(); 48 Quaternion q = new Quaternion();
49 49
50 float trace = m.M11 + m.M22 + m.M33; 50 float trace = m.M11 + m.M22 + m.M33;
51 51
52 if (trace > 0) 52 if (trace > 0)
53 { 53 {
54 float s = (float)Math.Sqrt(trace + 1); 54 float s = (float)Math.Sqrt(trace + 1);
55 q.W = s * 0.5f; 55 q.W = s * 0.5f;
56 s = 0.5f / s; 56 s = 0.5f / s;
57 57
58 q.X = (m.M32 - m.M23) * s; 58 q.X = (m.M32 - m.M23) * s;
59 q.Y = (m.M13 - m.M31) * s; 59 q.Y = (m.M13 - m.M31) * s;
60 q.Z = (m.M21 - m.M12) * s; 60 q.Z = (m.M21 - m.M12) * s;
61 } 61 }
62 else 62 else
63 { 63 {
64 int i = m.M11 < m.M22 ? 64 int i = m.M11 < m.M22 ?
65 (m.M22 < m.M33 ? 2 : 1) : 65 (m.M22 < m.M33 ? 2 : 1) :
66 (m.M11 < m.M33 ? 2 : 0); 66 (m.M11 < m.M33 ? 2 : 0);
67 int j = (i + 1) % 3; 67 int j = (i + 1) % 3;
68 int k = (i + 2) % 3; 68 int k = (i + 2) % 3;
69 69
70 float s = (float)Math.Sqrt(MathHelper.GetElement(m, i, i) - MathHelper.GetElement(m, j, j) - MathHelper.GetElement(m, k, k) + 1); 70 float s = (float)Math.Sqrt(MathHelper.GetElement(m, i, i) - MathHelper.GetElement(m, j, j) - MathHelper.GetElement(m, k, k) + 1);
71 MathHelper.SetElement(ref q, i, s * 0.5f); 71 MathHelper.SetElement(ref q, i, s * 0.5f);
72 s = 0.5f / s; 72 s = 0.5f / s;
73 73
74 q.W = (MathHelper.GetElement(m, k, j) - MathHelper.GetElement(m, j, k)) * s; 74 q.W = (MathHelper.GetElement(m, k, j) - MathHelper.GetElement(m, j, k)) * s;
75 MathHelper.SetElement(ref q, j, (MathHelper.GetElement(m, j, i) + MathHelper.GetElement(m, i, j)) * s); 75 MathHelper.SetElement(ref q, j, (MathHelper.GetElement(m, j, i) + MathHelper.GetElement(m, i, j)) * s);
76 MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s); 76 MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s);
77 } 77 }
78 78
79 return q; 79 return q;
80 } 80 }
81 81
82 public static Matrix Scaled(Matrix m, Vector3 v) 82 public static Matrix Scaled(Matrix m, Vector3 v)
83 { 83 {
84 return new Matrix( m.M11 * v.X, m.M12 * v.Y, m.M13 * v.Z, 0, 84 return new Matrix( m.M11 * v.X, m.M12 * v.Y, m.M13 * v.Z, 0,
85 m.M21 * v.X, m.M22 * v.Y, m.M23 * v.Z, 0, 85 m.M21 * v.X, m.M22 * v.Y, m.M23 * v.Z, 0,
86 m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0, 86 m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0,
87 0, 0, 0, 1); 87 0, 0, 0, 1);
88 } 88 }
89 89
90 public static Matrix Multiply(Matrix a, Matrix b) 90 public static Matrix Multiply(Matrix a, Matrix b)
91 { 91 {
92 /*return btMatrix3x3( 92 /*return btMatrix3x3(
93 m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]), 93 m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
94 m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]), 94 m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
95 m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/ 95 m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/
96 return new Matrix( 96 return new Matrix(
97 Dot(b, 0, MathHelper.GetRow(a, 1)), Dot(b, 1, MathHelper.GetRow(a, 1)), Dot(b, 2, MathHelper.GetRow(a, 1)), 0, 97 Dot(b, 0, MathHelper.GetRow(a, 1)), Dot(b, 1, MathHelper.GetRow(a, 1)), Dot(b, 2, MathHelper.GetRow(a, 1)), 0,
98 Dot(b, 0, MathHelper.GetRow(a, 2)), Dot(b, 1, MathHelper.GetRow(a, 2)), Dot(b, 2, MathHelper.GetRow(a, 2)), 0, 98 Dot(b, 0, MathHelper.GetRow(a, 2)), Dot(b, 1, MathHelper.GetRow(a, 2)), Dot(b, 2, MathHelper.GetRow(a, 2)), 0,
99 Dot(b, 0, MathHelper.GetRow(a, 3)), Dot(b, 1, MathHelper.GetRow(a, 3)), Dot(b, 2, MathHelper.GetRow(a, 3)), 0, 99 Dot(b, 0, MathHelper.GetRow(a, 3)), Dot(b, 1, MathHelper.GetRow(a, 3)), Dot(b, 2, MathHelper.GetRow(a, 3)), 0,
100 0, 0, 0, 1); 100 0, 0, 0, 1);
101 } 101 }
102 102
103 public static float Dot(Matrix m, int c, Vector3 v) 103 public static float Dot(Matrix m, int c, Vector3 v)
104 { 104 {
105 return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z; 105 return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z;
106 } 106 }
107 107
108 public static Matrix Transpose(Matrix m) 108 public static Matrix Transpose(Matrix m)
109 { 109 {
110 return new Matrix( m.M11, m.M21, m.M31, 0, 110 return new Matrix( m.M11, m.M21, m.M31, 0,
111 m.M12, m.M22, m.M32, 0, 111 m.M12, m.M22, m.M32, 0,
112 m.M13, m.M23, m.M33, 0, 112 m.M13, m.M23, m.M33, 0,
113 0, 0, 0, 1); 113 0, 0, 0, 1);
114 } 114 }
115 } 115 }
116} 116}
generated by cgit v1.1 at 2024-12-15 10:32:54 +0000