diff options
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs | 204 |
1 files changed, 102 insertions, 102 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs index 031faab..ccf92a0 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs | |||
@@ -1,102 +1,102 @@ | |||
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 | ||
22 | using System; | 22 | using System; |
23 | using System.Collections.Generic; | 23 | using System.Collections.Generic; |
24 | using System.Text; | 24 | using System.Text; |
25 | using MonoXnaCompactMaths; | 25 | using MonoXnaCompactMaths; |
26 | 26 | ||
27 | namespace XnaDevRu.BulletX | 27 | namespace XnaDevRu.BulletX |
28 | { | 28 | { |
29 | public static class TransformUtil | 29 | public static class TransformUtil |
30 | { | 30 | { |
31 | //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2; | 31 | //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2; |
32 | const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f; | 32 | const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f; |
33 | 33 | ||
34 | public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform) | 34 | public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform) |
35 | { | 35 | { |
36 | predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep; | 36 | predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep; |
37 | //exponential map | 37 | //exponential map |
38 | Vector3 axis; | 38 | Vector3 axis; |
39 | float angle = angularVelocity.Length(); | 39 | float angle = angularVelocity.Length(); |
40 | //limit the angular motion | 40 | //limit the angular motion |
41 | if (angle * timeStep > AngularMotionTreshold) | 41 | if (angle * timeStep > AngularMotionTreshold) |
42 | { | 42 | { |
43 | angle = AngularMotionTreshold / timeStep; | 43 | angle = AngularMotionTreshold / timeStep; |
44 | } | 44 | } |
45 | 45 | ||
46 | if (angle < 0.001f) | 46 | if (angle < 0.001f) |
47 | { | 47 | { |
48 | // use Taylor's expansions of sync function | 48 | // use Taylor's expansions of sync function |
49 | axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle); | 49 | axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle); |
50 | } | 50 | } |
51 | else | 51 | else |
52 | { | 52 | { |
53 | // sync(fAngle) = sin(c*fAngle)/t | 53 | // sync(fAngle) = sin(c*fAngle)/t |
54 | axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle); | 54 | axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle); |
55 | } | 55 | } |
56 | Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f)); | 56 | Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f)); |
57 | Quaternion ornA = MatrixOperations.GetRotation(currentTransform); | 57 | Quaternion ornA = MatrixOperations.GetRotation(currentTransform); |
58 | 58 | ||
59 | Quaternion predictedOrn = dorn * ornA; | 59 | Quaternion predictedOrn = dorn * ornA; |
60 | predictedOrn.Normalize(); | 60 | predictedOrn.Normalize(); |
61 | 61 | ||
62 | MatrixOperations.SetRotation(ref predictedTransform, predictedOrn); | 62 | MatrixOperations.SetRotation(ref predictedTransform, predictedOrn); |
63 | 63 | ||
64 | Matrix test = Matrix.CreateFromQuaternion(predictedOrn); | 64 | Matrix test = Matrix.CreateFromQuaternion(predictedOrn); |
65 | } | 65 | } |
66 | 66 | ||
67 | public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity) | 67 | public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity) |
68 | { | 68 | { |
69 | linearVelocity = (transformB.Translation - transformA.Translation) / timeStep; | 69 | linearVelocity = (transformB.Translation - transformA.Translation) / timeStep; |
70 | Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); | 70 | Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); |
71 | Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat); | 71 | Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat); |
72 | 72 | ||
73 | Vector3 axis; | 73 | Vector3 axis; |
74 | float angle = 2 * (float)Math.Acos(dorn.W); | 74 | float angle = 2 * (float)Math.Acos(dorn.W); |
75 | axis = new Vector3(dorn.X, dorn.Y, dorn.Z); | 75 | axis = new Vector3(dorn.X, dorn.Y, dorn.Z); |
76 | //axis[3] = 0.f; | 76 | //axis[3] = 0.f; |
77 | //check for axis length | 77 | //check for axis length |
78 | float len = axis.LengthSquared(); | 78 | float len = axis.LengthSquared(); |
79 | if (len < MathHelper.Epsilon * MathHelper.Epsilon) | 79 | if (len < MathHelper.Epsilon * MathHelper.Epsilon) |
80 | axis = new Vector3(1f, 0f, 0f); | 80 | axis = new Vector3(1f, 0f, 0f); |
81 | else | 81 | else |
82 | axis /= (float)Math.Sqrt(len); | 82 | axis /= (float)Math.Sqrt(len); |
83 | 83 | ||
84 | angularVelocity = axis * angle / timeStep; | 84 | angularVelocity = axis * angle / timeStep; |
85 | } | 85 | } |
86 | 86 | ||
87 | public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle) | 87 | public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle) |
88 | { | 88 | { |
89 | Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); | 89 | Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); |
90 | Quaternion dorn = MathHelper.GetRotation(dmat); | 90 | Quaternion dorn = MathHelper.GetRotation(dmat); |
91 | 91 | ||
92 | angle = 2f * (float)Math.Acos(dorn.W); | 92 | angle = 2f * (float)Math.Acos(dorn.W); |
93 | axis = new Vector3(dorn.X, dorn.Y, dorn.Z); | 93 | axis = new Vector3(dorn.X, dorn.Y, dorn.Z); |
94 | //check for axis length | 94 | //check for axis length |
95 | float len = axis.LengthSquared(); | 95 | float len = axis.LengthSquared(); |
96 | if (len < MathHelper.Epsilon * MathHelper.Epsilon) | 96 | if (len < MathHelper.Epsilon * MathHelper.Epsilon) |
97 | axis = new Vector3(1f, 0f, 0f); | 97 | axis = new Vector3(1f, 0f, 0f); |
98 | else | 98 | else |
99 | axis /= (float)Math.Sqrt(len); | 99 | axis /= (float)Math.Sqrt(len); |
100 | } | 100 | } |
101 | } | 101 | } |
102 | } | 102 | } |