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