aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs204
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
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 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}
generated by cgit v1.1 at 2024-09-03 05:12:10 +0000