diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs | 880 |
1 files changed, 440 insertions, 440 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs index 479c863..f1e0b1e 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs | |||
@@ -1,440 +1,440 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | /// <summary> | 29 | /// <summary> |
30 | /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space | 30 | /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space |
31 | /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked' | 31 | /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked' |
32 | /// Work in progress (is still a Hinge actually) | 32 | /// Work in progress (is still a Hinge actually) |
33 | /// </summary> | 33 | /// </summary> |
34 | public class Generic6DofConstraint : TypedConstraint | 34 | public class Generic6DofConstraint : TypedConstraint |
35 | { | 35 | { |
36 | private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f }; | 36 | private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f }; |
37 | private static readonly int[] _axisA = { 1, 0, 0 }; | 37 | private static readonly int[] _axisA = { 1, 0, 0 }; |
38 | private static readonly int[] _axisB = { 2, 2, 1 }; | 38 | private static readonly int[] _axisB = { 2, 2, 1 }; |
39 | 39 | ||
40 | private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints | 40 | private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints |
41 | private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints | 41 | private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints |
42 | 42 | ||
43 | private Matrix _frameInA; // the constraint space w.r.t body A | 43 | private Matrix _frameInA; // the constraint space w.r.t body A |
44 | private Matrix _frameInB; // the constraint space w.r.t body B | 44 | private Matrix _frameInB; // the constraint space w.r.t body B |
45 | 45 | ||
46 | private float[] _lowerLimit = new float[6]; // the constraint lower limits | 46 | private float[] _lowerLimit = new float[6]; // the constraint lower limits |
47 | private float[] _upperLimit = new float[6]; // the constraint upper limits | 47 | private float[] _upperLimit = new float[6]; // the constraint upper limits |
48 | 48 | ||
49 | private float[] _accumulatedImpulse = new float[6]; | 49 | private float[] _accumulatedImpulse = new float[6]; |
50 | 50 | ||
51 | public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB) | 51 | public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB) |
52 | : base(rbA, rbB) | 52 | : base(rbA, rbB) |
53 | { | 53 | { |
54 | _frameInA = frameInA; | 54 | _frameInA = frameInA; |
55 | _frameInB = frameInB; | 55 | _frameInB = frameInB; |
56 | //free means upper < lower, | 56 | //free means upper < lower, |
57 | //locked means upper == lower | 57 | //locked means upper == lower |
58 | //limited means upper > lower | 58 | //limited means upper > lower |
59 | //so start all locked | 59 | //so start all locked |
60 | for (int i = 0; i < 6; ++i) | 60 | for (int i = 0; i < 6; ++i) |
61 | { | 61 | { |
62 | _lowerLimit[i] = 0.0f; | 62 | _lowerLimit[i] = 0.0f; |
63 | _upperLimit[i] = 0.0f; | 63 | _upperLimit[i] = 0.0f; |
64 | _accumulatedImpulse[i] = 0.0f; | 64 | _accumulatedImpulse[i] = 0.0f; |
65 | } | 65 | } |
66 | } | 66 | } |
67 | 67 | ||
68 | public Generic6DofConstraint() { } | 68 | public Generic6DofConstraint() { } |
69 | 69 | ||
70 | public void UpdateRHS(float timeStep) { } | 70 | public void UpdateRHS(float timeStep) { } |
71 | 71 | ||
72 | public float ComputeAngle(int axis) | 72 | public float ComputeAngle(int axis) |
73 | { | 73 | { |
74 | float angle = 0; | 74 | float angle = 0; |
75 | 75 | ||
76 | switch (axis) | 76 | switch (axis) |
77 | { | 77 | { |
78 | case 0: | 78 | case 0: |
79 | { | 79 | { |
80 | Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform); | 80 | Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform); |
81 | Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); | 81 | Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); |
82 | Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); | 82 | Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); |
83 | 83 | ||
84 | float s = Vector3.Dot(v1, w2); | 84 | float s = Vector3.Dot(v1, w2); |
85 | float c = Vector3.Dot(v1, v2); | 85 | float c = Vector3.Dot(v1, v2); |
86 | 86 | ||
87 | angle = (float)Math.Atan2(s, c); | 87 | angle = (float)Math.Atan2(s, c); |
88 | break; | 88 | break; |
89 | } | 89 | } |
90 | case 1: | 90 | case 1: |
91 | { | 91 | { |
92 | Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform); | 92 | Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform); |
93 | Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); | 93 | Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); |
94 | Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); | 94 | Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); |
95 | 95 | ||
96 | float s = Vector3.Dot(w1, u2); | 96 | float s = Vector3.Dot(w1, u2); |
97 | float c = Vector3.Dot(w1, w2); | 97 | float c = Vector3.Dot(w1, w2); |
98 | 98 | ||
99 | angle = (float)Math.Atan2(s, c); | 99 | angle = (float)Math.Atan2(s, c); |
100 | break; | 100 | break; |
101 | } | 101 | } |
102 | case 2: | 102 | case 2: |
103 | { | 103 | { |
104 | Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform); | 104 | Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform); |
105 | Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); | 105 | Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); |
106 | Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); | 106 | Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); |
107 | 107 | ||
108 | float s = Vector3.Dot(u1, v2); | 108 | float s = Vector3.Dot(u1, v2); |
109 | float c = Vector3.Dot(u1, u2); | 109 | float c = Vector3.Dot(u1, u2); |
110 | 110 | ||
111 | angle = (float)Math.Atan2(s, c); | 111 | angle = (float)Math.Atan2(s, c); |
112 | break; | 112 | break; |
113 | } | 113 | } |
114 | default: BulletDebug.Assert(false); break; | 114 | default: BulletDebug.Assert(false); break; |
115 | } | 115 | } |
116 | 116 | ||
117 | return angle; | 117 | return angle; |
118 | } | 118 | } |
119 | 119 | ||
120 | public void SetLinearLowerLimit(Vector3 linearLower) | 120 | public void SetLinearLowerLimit(Vector3 linearLower) |
121 | { | 121 | { |
122 | _lowerLimit[0] = linearLower.X; | 122 | _lowerLimit[0] = linearLower.X; |
123 | _lowerLimit[1] = linearLower.Y; | 123 | _lowerLimit[1] = linearLower.Y; |
124 | _lowerLimit[2] = linearLower.Z; | 124 | _lowerLimit[2] = linearLower.Z; |
125 | } | 125 | } |
126 | 126 | ||
127 | public void SetLinearUpperLimit(Vector3 linearUpper) | 127 | public void SetLinearUpperLimit(Vector3 linearUpper) |
128 | { | 128 | { |
129 | _upperLimit[0] = linearUpper.X; | 129 | _upperLimit[0] = linearUpper.X; |
130 | _upperLimit[1] = linearUpper.Y; | 130 | _upperLimit[1] = linearUpper.Y; |
131 | _upperLimit[2] = linearUpper.Z; | 131 | _upperLimit[2] = linearUpper.Z; |
132 | } | 132 | } |
133 | 133 | ||
134 | public void SetAngularLowerLimit(Vector3 angularLower) | 134 | public void SetAngularLowerLimit(Vector3 angularLower) |
135 | { | 135 | { |
136 | _lowerLimit[3] = angularLower.X; | 136 | _lowerLimit[3] = angularLower.X; |
137 | _lowerLimit[4] = angularLower.Y; | 137 | _lowerLimit[4] = angularLower.Y; |
138 | _lowerLimit[5] = angularLower.Z; | 138 | _lowerLimit[5] = angularLower.Z; |
139 | } | 139 | } |
140 | 140 | ||
141 | public void SetAngularUpperLimit(Vector3 angularUpper) | 141 | public void SetAngularUpperLimit(Vector3 angularUpper) |
142 | { | 142 | { |
143 | _upperLimit[3] = angularUpper.X; | 143 | _upperLimit[3] = angularUpper.X; |
144 | _upperLimit[4] = angularUpper.Y; | 144 | _upperLimit[4] = angularUpper.Y; |
145 | _upperLimit[5] = angularUpper.Z; | 145 | _upperLimit[5] = angularUpper.Z; |
146 | } | 146 | } |
147 | 147 | ||
148 | //first 3 are linear, next 3 are angular | 148 | //first 3 are linear, next 3 are angular |
149 | public void SetLimit(int axis, float lo, float hi) | 149 | public void SetLimit(int axis, float lo, float hi) |
150 | { | 150 | { |
151 | _lowerLimit[axis] = lo; | 151 | _lowerLimit[axis] = lo; |
152 | _upperLimit[axis] = hi; | 152 | _upperLimit[axis] = hi; |
153 | } | 153 | } |
154 | 154 | ||
155 | //free means upper < lower, | 155 | //free means upper < lower, |
156 | //locked means upper == lower | 156 | //locked means upper == lower |
157 | //limited means upper > lower | 157 | //limited means upper > lower |
158 | //limitIndex: first 3 are linear, next 3 are angular | 158 | //limitIndex: first 3 are linear, next 3 are angular |
159 | public bool IsLimited(int limitIndex) | 159 | public bool IsLimited(int limitIndex) |
160 | { | 160 | { |
161 | return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]); | 161 | return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]); |
162 | } | 162 | } |
163 | 163 | ||
164 | public override void BuildJacobian() | 164 | public override void BuildJacobian() |
165 | { | 165 | { |
166 | Vector3 localNormalInA = new Vector3(0, 0, 0); | 166 | Vector3 localNormalInA = new Vector3(0, 0, 0); |
167 | 167 | ||
168 | Vector3 pivotInA = _frameInA.Translation; | 168 | Vector3 pivotInA = _frameInA.Translation; |
169 | Vector3 pivotInB = _frameInB.Translation; | 169 | Vector3 pivotInB = _frameInB.Translation; |
170 | 170 | ||
171 | Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); | 171 | Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); |
172 | Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); | 172 | Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); |
173 | 173 | ||
174 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | 174 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; |
175 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | 175 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; |
176 | 176 | ||
177 | //linear part | 177 | //linear part |
178 | for (int i = 0; i < 3; i++) | 178 | for (int i = 0; i < 3; i++) |
179 | { | 179 | { |
180 | if (IsLimited(i)) | 180 | if (IsLimited(i)) |
181 | { | 181 | { |
182 | if (i == 0) | 182 | if (i == 0) |
183 | localNormalInA = new Vector3(1, 0, 0); | 183 | localNormalInA = new Vector3(1, 0, 0); |
184 | else if (i == 1) | 184 | else if (i == 1) |
185 | localNormalInA = new Vector3(0, 1, 0); | 185 | localNormalInA = new Vector3(0, 1, 0); |
186 | else | 186 | else |
187 | localNormalInA = new Vector3(0, 0, 1); | 187 | localNormalInA = new Vector3(0, 0, 1); |
188 | 188 | ||
189 | Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); | 189 | Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); |
190 | 190 | ||
191 | // Create linear atom | 191 | // Create linear atom |
192 | _jacLinear[i] = new JacobianEntry( | 192 | _jacLinear[i] = new JacobianEntry( |
193 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | 193 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), |
194 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | 194 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), |
195 | MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, | 195 | MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, |
196 | MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, | 196 | MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, |
197 | normalWorld, | 197 | normalWorld, |
198 | RigidBodyA.InvInertiaDiagLocal, | 198 | RigidBodyA.InvInertiaDiagLocal, |
199 | RigidBodyA.InverseMass, | 199 | RigidBodyA.InverseMass, |
200 | RigidBodyB.InvInertiaDiagLocal, | 200 | RigidBodyB.InvInertiaDiagLocal, |
201 | RigidBodyB.InverseMass); | 201 | RigidBodyB.InverseMass); |
202 | 202 | ||
203 | //optionally disable warmstarting | 203 | //optionally disable warmstarting |
204 | _accumulatedImpulse[i] = 0f; | 204 | _accumulatedImpulse[i] = 0f; |
205 | 205 | ||
206 | // Apply accumulated impulse | 206 | // Apply accumulated impulse |
207 | Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld; | 207 | Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld; |
208 | 208 | ||
209 | RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); | 209 | RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); |
210 | RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); | 210 | RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); |
211 | } | 211 | } |
212 | } | 212 | } |
213 | 213 | ||
214 | // angular part | 214 | // angular part |
215 | for (int i = 0; i < 3; i++) | 215 | for (int i = 0; i < 3; i++) |
216 | { | 216 | { |
217 | if (IsLimited(i + 3)) | 217 | if (IsLimited(i + 3)) |
218 | { | 218 | { |
219 | Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); | 219 | Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); |
220 | Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); | 220 | Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); |
221 | 221 | ||
222 | Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB); | 222 | Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB); |
223 | 223 | ||
224 | // Create angular atom | 224 | // Create angular atom |
225 | _jacAng[i] = new JacobianEntry(axis, | 225 | _jacAng[i] = new JacobianEntry(axis, |
226 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | 226 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), |
227 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | 227 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), |
228 | RigidBodyA.InvInertiaDiagLocal, | 228 | RigidBodyA.InvInertiaDiagLocal, |
229 | RigidBodyB.InvInertiaDiagLocal); | 229 | RigidBodyB.InvInertiaDiagLocal); |
230 | 230 | ||
231 | _accumulatedImpulse[i + 3] = 0f; | 231 | _accumulatedImpulse[i + 3] = 0f; |
232 | 232 | ||
233 | // Apply accumulated impulse | 233 | // Apply accumulated impulse |
234 | Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis; | 234 | Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis; |
235 | 235 | ||
236 | RigidBodyA.ApplyTorqueImpulse(impulse_vector); | 236 | RigidBodyA.ApplyTorqueImpulse(impulse_vector); |
237 | RigidBodyB.ApplyTorqueImpulse(-impulse_vector); | 237 | RigidBodyB.ApplyTorqueImpulse(-impulse_vector); |
238 | } | 238 | } |
239 | } | 239 | } |
240 | } | 240 | } |
241 | 241 | ||
242 | public override void SolveConstraint(float timeStep) | 242 | public override void SolveConstraint(float timeStep) |
243 | { | 243 | { |
244 | float tau = 0.1f; | 244 | float tau = 0.1f; |
245 | float damping = 1.0f; | 245 | float damping = 1.0f; |
246 | 246 | ||
247 | Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); | 247 | Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); |
248 | Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); | 248 | Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); |
249 | 249 | ||
250 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | 250 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; |
251 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | 251 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; |
252 | 252 | ||
253 | Vector3 localNormalInA = new Vector3(); | 253 | Vector3 localNormalInA = new Vector3(); |
254 | 254 | ||
255 | // linear | 255 | // linear |
256 | for (int i = 0; i < 3; i++) | 256 | for (int i = 0; i < 3; i++) |
257 | { | 257 | { |
258 | if (IsLimited(i)) | 258 | if (IsLimited(i)) |
259 | { | 259 | { |
260 | Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); | 260 | Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); |
261 | Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); | 261 | Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); |
262 | 262 | ||
263 | if (i == 0) | 263 | if (i == 0) |
264 | localNormalInA = new Vector3(1, 0, 0); | 264 | localNormalInA = new Vector3(1, 0, 0); |
265 | else if (i == 1) | 265 | else if (i == 1) |
266 | localNormalInA = new Vector3(0, 1, 0); | 266 | localNormalInA = new Vector3(0, 1, 0); |
267 | else | 267 | else |
268 | localNormalInA = new Vector3(0, 0, 1); | 268 | localNormalInA = new Vector3(0, 0, 1); |
269 | 269 | ||
270 | Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); | 270 | Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); |
271 | 271 | ||
272 | float jacDiagABInv = 1f / _jacLinear[i].Diagonal; | 272 | float jacDiagABInv = 1f / _jacLinear[i].Diagonal; |
273 | 273 | ||
274 | //velocity error (first order error) | 274 | //velocity error (first order error) |
275 | float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, | 275 | float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, |
276 | RigidBodyB.LinearVelocity, angvelB); | 276 | RigidBodyB.LinearVelocity, angvelB); |
277 | 277 | ||
278 | //positional error (zeroth order error) | 278 | //positional error (zeroth order error) |
279 | float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld); | 279 | float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld); |
280 | float lo = -1e30f; | 280 | float lo = -1e30f; |
281 | float hi = 1e30f; | 281 | float hi = 1e30f; |
282 | 282 | ||
283 | //handle the limits | 283 | //handle the limits |
284 | if (_lowerLimit[i] < _upperLimit[i]) | 284 | if (_lowerLimit[i] < _upperLimit[i]) |
285 | { | 285 | { |
286 | if (depth > _upperLimit[i]) | 286 | if (depth > _upperLimit[i]) |
287 | { | 287 | { |
288 | depth -= _upperLimit[i]; | 288 | depth -= _upperLimit[i]; |
289 | lo = 0f; | 289 | lo = 0f; |
290 | } | 290 | } |
291 | else | 291 | else |
292 | { | 292 | { |
293 | if (depth < _lowerLimit[i]) | 293 | if (depth < _lowerLimit[i]) |
294 | { | 294 | { |
295 | depth -= _lowerLimit[i]; | 295 | depth -= _lowerLimit[i]; |
296 | hi = 0f; | 296 | hi = 0f; |
297 | } | 297 | } |
298 | else | 298 | else |
299 | { | 299 | { |
300 | continue; | 300 | continue; |
301 | } | 301 | } |
302 | } | 302 | } |
303 | } | 303 | } |
304 | 304 | ||
305 | float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv; | 305 | float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv; |
306 | float oldNormalImpulse = _accumulatedImpulse[i]; | 306 | float oldNormalImpulse = _accumulatedImpulse[i]; |
307 | float sum = oldNormalImpulse + normalImpulse; | 307 | float sum = oldNormalImpulse + normalImpulse; |
308 | _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum; | 308 | _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum; |
309 | normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse; | 309 | normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse; |
310 | 310 | ||
311 | Vector3 impulse_vector = normalWorld * normalImpulse; | 311 | Vector3 impulse_vector = normalWorld * normalImpulse; |
312 | RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); | 312 | RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); |
313 | RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); | 313 | RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); |
314 | } | 314 | } |
315 | } | 315 | } |
316 | 316 | ||
317 | Vector3 axis; | 317 | Vector3 axis; |
318 | float angle; | 318 | float angle; |
319 | Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA; | 319 | Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA; |
320 | Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB; | 320 | Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB; |
321 | 321 | ||
322 | TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle); | 322 | TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle); |
323 | Quaternion diff = new Quaternion(axis, angle); | 323 | Quaternion diff = new Quaternion(axis, angle); |
324 | Matrix diffMat = Matrix.CreateFromQuaternion(diff); | 324 | Matrix diffMat = Matrix.CreateFromQuaternion(diff); |
325 | Vector3 xyz; | 325 | Vector3 xyz; |
326 | // this is not perfect, we can first check which axis are limited, and choose a more appropriate order | 326 | // this is not perfect, we can first check which axis are limited, and choose a more appropriate order |
327 | MatrixToEulerXYZ(diffMat, out xyz); | 327 | MatrixToEulerXYZ(diffMat, out xyz); |
328 | 328 | ||
329 | // angular | 329 | // angular |
330 | for (int i = 0; i < 3; i++) | 330 | for (int i = 0; i < 3; i++) |
331 | { | 331 | { |
332 | if (IsLimited(i + 3)) | 332 | if (IsLimited(i + 3)) |
333 | { | 333 | { |
334 | Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); | 334 | Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); |
335 | Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); | 335 | Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); |
336 | 336 | ||
337 | float jacDiagABInv = 1f / _jacAng[i].Diagonal; | 337 | float jacDiagABInv = 1f / _jacAng[i].Diagonal; |
338 | 338 | ||
339 | //velocity error (first order error) | 339 | //velocity error (first order error) |
340 | float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, | 340 | float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, |
341 | RigidBodyB.LinearVelocity, angvelB); | 341 | RigidBodyB.LinearVelocity, angvelB); |
342 | 342 | ||
343 | //positional error (zeroth order error) | 343 | //positional error (zeroth order error) |
344 | Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); | 344 | Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); |
345 | Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); | 345 | Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); |
346 | 346 | ||
347 | float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB); | 347 | float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB); |
348 | 348 | ||
349 | float lo = -1e30f; | 349 | float lo = -1e30f; |
350 | float hi = 1e30f; | 350 | float hi = 1e30f; |
351 | 351 | ||
352 | //handle the twist limit | 352 | //handle the twist limit |
353 | if (_lowerLimit[i + 3] < _upperLimit[i + 3]) | 353 | if (_lowerLimit[i + 3] < _upperLimit[i + 3]) |
354 | { | 354 | { |
355 | //clamp the values | 355 | //clamp the values |
356 | float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f; | 356 | float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f; |
357 | float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f; | 357 | float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f; |
358 | 358 | ||
359 | float projAngle; | 359 | float projAngle; |
360 | 360 | ||
361 | if (i == 0) | 361 | if (i == 0) |
362 | projAngle = -2f * xyz.Z; | 362 | projAngle = -2f * xyz.Z; |
363 | else if (i == 1) | 363 | else if (i == 1) |
364 | projAngle = -2f * xyz.Y; | 364 | projAngle = -2f * xyz.Y; |
365 | else | 365 | else |
366 | projAngle = -2f * xyz.Z; | 366 | projAngle = -2f * xyz.Z; |
367 | 367 | ||
368 | if (projAngle < loLimit) | 368 | if (projAngle < loLimit) |
369 | { | 369 | { |
370 | hi = 0f; | 370 | hi = 0f; |
371 | rel_pos = loLimit - projAngle; | 371 | rel_pos = loLimit - projAngle; |
372 | } | 372 | } |
373 | else | 373 | else |
374 | { | 374 | { |
375 | if (projAngle > hiLimit) | 375 | if (projAngle > hiLimit) |
376 | { | 376 | { |
377 | lo = 0f; | 377 | lo = 0f; |
378 | rel_pos = (hiLimit - projAngle); | 378 | rel_pos = (hiLimit - projAngle); |
379 | } | 379 | } |
380 | else | 380 | else |
381 | { | 381 | { |
382 | continue; | 382 | continue; |
383 | } | 383 | } |
384 | } | 384 | } |
385 | } | 385 | } |
386 | 386 | ||
387 | //impulse | 387 | //impulse |
388 | 388 | ||
389 | float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv; | 389 | float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv; |
390 | float oldNormalImpulse = _accumulatedImpulse[i + 3]; | 390 | float oldNormalImpulse = _accumulatedImpulse[i + 3]; |
391 | float sum = oldNormalImpulse + normalImpulse; | 391 | float sum = oldNormalImpulse + normalImpulse; |
392 | _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum; | 392 | _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum; |
393 | normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse; | 393 | normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse; |
394 | 394 | ||
395 | Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB); | 395 | Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB); |
396 | Vector3 impulse_vector = axis2 * normalImpulse; | 396 | Vector3 impulse_vector = axis2 * normalImpulse; |
397 | 397 | ||
398 | RigidBodyA.ApplyTorqueImpulse(impulse_vector); | 398 | RigidBodyA.ApplyTorqueImpulse(impulse_vector); |
399 | RigidBodyB.ApplyTorqueImpulse(-impulse_vector); | 399 | RigidBodyB.ApplyTorqueImpulse(-impulse_vector); |
400 | } | 400 | } |
401 | } | 401 | } |
402 | } | 402 | } |
403 | 403 | ||
404 | //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html | 404 | //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html |
405 | private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz) | 405 | private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz) |
406 | { | 406 | { |
407 | // rot = cy*cz -cy*sz sy | 407 | // rot = cy*cz -cy*sz sy |
408 | // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx | 408 | // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx |
409 | // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy | 409 | // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy |
410 | xyz = new Vector3(); | 410 | xyz = new Vector3(); |
411 | 411 | ||
412 | if (MathHelper.GetElement(mat, 2) < 1.0f) | 412 | if (MathHelper.GetElement(mat, 2) < 1.0f) |
413 | { | 413 | { |
414 | if (MathHelper.GetElement(mat, 2) > -1.0f) | 414 | if (MathHelper.GetElement(mat, 2) > -1.0f) |
415 | { | 415 | { |
416 | xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8)); | 416 | xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8)); |
417 | xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2)); | 417 | xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2)); |
418 | xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0)); | 418 | xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0)); |
419 | return true; | 419 | return true; |
420 | } | 420 | } |
421 | else | 421 | else |
422 | { | 422 | { |
423 | // WARNING. Not unique. XA - ZA = -atan2(r10,r11) | 423 | // WARNING. Not unique. XA - ZA = -atan2(r10,r11) |
424 | xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); | 424 | xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); |
425 | xyz.Y = -(float)Math.PI / 2; | 425 | xyz.Y = -(float)Math.PI / 2; |
426 | xyz.Z = 0.0f; | 426 | xyz.Z = 0.0f; |
427 | return false; | 427 | return false; |
428 | } | 428 | } |
429 | } | 429 | } |
430 | else | 430 | else |
431 | { | 431 | { |
432 | // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) | 432 | // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) |
433 | xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); | 433 | xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); |
434 | xyz.Y = (float)Math.PI / 2; | 434 | xyz.Y = (float)Math.PI / 2; |
435 | xyz.Z = 0.0f; | 435 | xyz.Z = 0.0f; |
436 | return false; | 436 | return false; |
437 | } | 437 | } |
438 | } | 438 | } |
439 | } | 439 | } |
440 | } | 440 | } |