Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @ 2430

Last change on this file since 2430 was 2430, checked in by rgrieder, 16 years ago

Updated to Bullet 2.73 (first part).

  • Property svn:eol-style set to native
File size: 14.6 KB
RevLine 
[1963]1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15/*
162007-09-09
17Refactored by Francisco Le?n
18email: projectileman@yahoo.com
19http://gimpact.sf.net
20*/
21
22
23#include "btGeneric6DofConstraint.h"
24#include "BulletDynamics/Dynamics/btRigidBody.h"
25#include "LinearMath/btTransformUtil.h"
26#include <new>
27
28
29#define GENERIC_D6_DISABLE_WARMSTARTING 1
30
31btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
32btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
33{
34        int i = index%3;
35        int j = index/3;
36        return mat[i][j];
37}
38
39///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
40bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
41bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
42{
43//      // rot =  cy*cz          -cy*sz           sy
44//      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
45//      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
46//
47
48                if (btGetMatrixElem(mat,2) < btScalar(1.0))
49                {
50                        if (btGetMatrixElem(mat,2) > btScalar(-1.0))
51                        {
52                                xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
53                                xyz[1] = btAsin(btGetMatrixElem(mat,2));
54                                xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
55                                return true;
56                        }
57                        else
58                        {
59                                // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
60                                xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
61                                xyz[1] = -SIMD_HALF_PI;
62                                xyz[2] = btScalar(0.0);
63                                return false;
64                        }
65                }
66                else
67                {
68                        // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
69                        xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
70                        xyz[1] = SIMD_HALF_PI;
71                        xyz[2] = 0.0;
72
73                }
74
75
76        return false;
77}
78
79
80
81//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
82
83
84int btRotationalLimitMotor::testLimitValue(btScalar test_value)
85{
86        if(m_loLimit>m_hiLimit)
87        {
88                m_currentLimit = 0;//Free from violation
89                return 0;
90        }
91
92        if (test_value < m_loLimit)
93        {
94                m_currentLimit = 1;//low limit violation
95                m_currentLimitError =  test_value - m_loLimit;
96                return 1;
97        }
98        else if (test_value> m_hiLimit)
99        {
100                m_currentLimit = 2;//High limit violation
101                m_currentLimitError = test_value - m_hiLimit;
102                return 2;
103        };
104
105        m_currentLimit = 0;//Free from violation
106        return 0;
107       
108}
109
110
111btScalar btRotationalLimitMotor::solveAngularLimits(
112                btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
113                btRigidBody * body0, btRigidBody * body1)
114{
115    if (needApplyTorques()==false) return 0.0f;
116
117    btScalar target_velocity = m_targetVelocity;
118    btScalar maxMotorForce = m_maxMotorForce;
119
120        //current error correction
121    if (m_currentLimit!=0)
122    {
123        target_velocity = -m_ERP*m_currentLimitError/(timeStep);
124        maxMotorForce = m_maxLimitForce;
125    }
126
127    maxMotorForce *= timeStep;
128
129    // current velocity difference
130    btVector3 vel_diff = body0->getAngularVelocity();
131    if (body1)
132    {
133        vel_diff -= body1->getAngularVelocity();
134    }
135
136
137
138    btScalar rel_vel = axis.dot(vel_diff);
139
140        // correction velocity
141    btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
142
143
144    if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
145    {
146        return 0.0f;//no need for applying force
147    }
148
149
150        // correction impulse
151    btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
152
153        // clip correction impulse
154    btScalar clippedMotorImpulse;
155
[2430]156    ///@todo: should clip against accumulated impulse
[1963]157    if (unclippedMotorImpulse>0.0f)
158    {
159        clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
160    }
161    else
162    {
163        clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
164    }
165
166
167        // sort with accumulated impulses
168    btScalar    lo = btScalar(-1e30);
169    btScalar    hi = btScalar(1e30);
170
171    btScalar oldaccumImpulse = m_accumulatedImpulse;
172    btScalar sum = oldaccumImpulse + clippedMotorImpulse;
173    m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
174
175    clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
176
177
178
179    btVector3 motorImp = clippedMotorImpulse * axis;
180
181
182    body0->applyTorqueImpulse(motorImp);
183    if (body1) body1->applyTorqueImpulse(-motorImp);
184
185    return clippedMotorImpulse;
186
187
188}
189
190//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
191
192//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
193btScalar btTranslationalLimitMotor::solveLinearAxis(
194                btScalar timeStep,
195        btScalar jacDiagABInv,
196        btRigidBody& body1,const btVector3 &pointInA,
197        btRigidBody& body2,const btVector3 &pointInB,
198        int limit_index,
199        const btVector3 & axis_normal_on_a,
200                const btVector3 & anchorPos)
201{
202
203///find relative velocity
204//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
205//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
206    btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
207    btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
208
209    btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
210    btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
211    btVector3 vel = vel1 - vel2;
212
213    btScalar rel_vel = axis_normal_on_a.dot(vel);
214
215
216
217/// apply displacement correction
218
219//positional error (zeroth order error)
220    btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
221    btScalar    lo = btScalar(-1e30);
222    btScalar    hi = btScalar(1e30);
223
224    btScalar minLimit = m_lowerLimit[limit_index];
225    btScalar maxLimit = m_upperLimit[limit_index];
226
227    //handle the limits
228    if (minLimit < maxLimit)
229    {
230        {
231            if (depth > maxLimit)
232            {
233                depth -= maxLimit;
234                lo = btScalar(0.);
235
236            }
237            else
238            {
239                if (depth < minLimit)
240                {
241                    depth -= minLimit;
242                    hi = btScalar(0.);
243                }
244                else
245                {
246                    return 0.0f;
247                }
248            }
249        }
250    }
251
252    btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
253
254
255
256
257    btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
258    btScalar sum = oldNormalImpulse + normalImpulse;
259    m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
260    normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
261
262    btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
263    body1.applyImpulse( impulse_vector, rel_pos1);
264    body2.applyImpulse(-impulse_vector, rel_pos2);
265    return normalImpulse;
266}
267
268//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
269
270
271btGeneric6DofConstraint::btGeneric6DofConstraint()
272        :btTypedConstraint(D6_CONSTRAINT_TYPE),
273                m_useLinearReferenceFrameA(true)
274{
275}
276
277btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
278        : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
279        , m_frameInA(frameInA)
280        , m_frameInB(frameInB),
281                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
282{
283
284}
285
286
287
288
289
290void btGeneric6DofConstraint::calculateAngleInfo()
291{
292        btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
293
294        matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
295
296
297
298        // in euler angle mode we do not actually constrain the angular velocity
299  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
300  //
301  //    to get                  constrain w2-w1 along           ...not
302  //    ------                  ---------------------           ------
303  //    d(angle[0])/dt = 0      ax[1] x ax[2]                   ax[0]
304  //    d(angle[1])/dt = 0      ax[1]
305  //    d(angle[2])/dt = 0      ax[0] x ax[1]                   ax[2]
306  //
307  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
308  // to prove the result for angle[0], write the expression for angle[0] from
309  // GetInfo1 then take the derivative. to prove this for angle[2] it is
310  // easier to take the euler rate expression for d(angle[2])/dt with respect
311  // to the components of w and set that to 0.
312
313        btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
314        btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
315
316        m_calculatedAxis[1] = axis2.cross(axis0);
317        m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
318        m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
319
320
321//    if(m_debugDrawer)
322//    {
323//
324//      char buff[300];
325//              sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
326//              m_calculatedAxisAngleDiff[0],
327//              m_calculatedAxisAngleDiff[1],
328//              m_calculatedAxisAngleDiff[2]);
329//      m_debugDrawer->reportErrorWarning(buff);
330//    }
331
332}
333
334void btGeneric6DofConstraint::calculateTransforms()
335{
336    m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
337    m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
338
339    calculateAngleInfo();
340}
341
342
343void btGeneric6DofConstraint::buildLinearJacobian(
344    btJacobianEntry & jacLinear,const btVector3 & normalWorld,
345    const btVector3 & pivotAInW,const btVector3 & pivotBInW)
346{
347    new (&jacLinear) btJacobianEntry(
348        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
349        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
350        pivotAInW - m_rbA.getCenterOfMassPosition(),
351        pivotBInW - m_rbB.getCenterOfMassPosition(),
352        normalWorld,
353        m_rbA.getInvInertiaDiagLocal(),
354        m_rbA.getInvMass(),
355        m_rbB.getInvInertiaDiagLocal(),
356        m_rbB.getInvMass());
357
358}
359
360void btGeneric6DofConstraint::buildAngularJacobian(
361    btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
362{
363    new (&jacAngular)   btJacobianEntry(jointAxisW,
364                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
365                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
366                                      m_rbA.getInvInertiaDiagLocal(),
367                                      m_rbB.getInvInertiaDiagLocal());
368
369}
370
371bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
372{
373    btScalar angle = m_calculatedAxisAngleDiff[axis_index];
374
375    //test limits
376    m_angularLimits[axis_index].testLimitValue(angle);
377    return m_angularLimits[axis_index].needApplyTorques();
378}
379
380void btGeneric6DofConstraint::buildJacobian()
381{
382
383        // Clear accumulated impulses for the next simulation step
384    m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
385    int i;
386    for(i = 0; i < 3; i++)
387    {
388        m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
389    }
390    //calculates transform
391    calculateTransforms();
392
393//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
394//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
395        calcAnchorPos();
396        btVector3 pivotAInW = m_AnchorPos;
397        btVector3 pivotBInW = m_AnchorPos;
398
399// not used here
400//    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
401//    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
402
403    btVector3 normalWorld;
404    //linear part
405    for (i=0;i<3;i++)
406    {
407        if (m_linearLimits.isLimited(i))
408        {
409                        if (m_useLinearReferenceFrameA)
410                    normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
411                        else
412                    normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
413
414            buildLinearJacobian(
415                m_jacLinear[i],normalWorld ,
416                pivotAInW,pivotBInW);
417
418        }
419    }
420
421    // angular part
422    for (i=0;i<3;i++)
423    {
424        //calculates error angle
425        if (testAngularLimitMotor(i))
426        {
427            normalWorld = this->getAxis(i);
428            // Create angular atom
429            buildAngularJacobian(m_jacAng[i],normalWorld);
430        }
431    }
432
433
434}
435
436
437void btGeneric6DofConstraint::solveConstraint(btScalar  timeStep)
438{
439    m_timeStep = timeStep;
440
441    //calculateTransforms();
442
443    int i;
444
445    // linear
446
447    btVector3 pointInA = m_calculatedTransformA.getOrigin();
448        btVector3 pointInB = m_calculatedTransformB.getOrigin();
449
450        btScalar jacDiagABInv;
451        btVector3 linear_axis;
452    for (i=0;i<3;i++)
453    {
454        if (m_linearLimits.isLimited(i))
455        {
456            jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
457
458                        if (m_useLinearReferenceFrameA)
459                    linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
460                        else
461                    linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
462
463            m_linearLimits.solveLinearAxis(
464                m_timeStep,
465                jacDiagABInv,
466                m_rbA,pointInA,
467                m_rbB,pointInB,
468                i,linear_axis, m_AnchorPos);
469
470        }
471    }
472
473    // angular
474    btVector3 angular_axis;
475    btScalar angularJacDiagABInv;
476    for (i=0;i<3;i++)
477    {
478        if (m_angularLimits[i].needApplyTorques())
479        {
480
481                        // get axis
482                        angular_axis = getAxis(i);
483
484                        angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
485
486                        m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB);
487        }
488    }
489}
490
491void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
492{
493    (void)timeStep;
494
495}
496
497btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
498{
499    return m_calculatedAxis[axis_index];
500}
501
502btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
503{
504    return m_calculatedAxisAngleDiff[axis_index];
505}
506
507void btGeneric6DofConstraint::calcAnchorPos(void)
508{
509        btScalar imA = m_rbA.getInvMass();
510        btScalar imB = m_rbB.getInvMass();
511        btScalar weight;
512        if(imB == btScalar(0.0))
513        {
514                weight = btScalar(1.0);
515        }
516        else
517        {
518                weight = imA / (imA + imB);
519        }
520        const btVector3& pA = m_calculatedTransformA.getOrigin();
521        const btVector3& pB = m_calculatedTransformB.getOrigin();
522        m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
523        return;
524} // btGeneric6DofConstraint::calcAnchorPos()
525
Note: See TracBrowser for help on using the repository browser.