Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @ 7898

Last change on this file since 7898 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).
  • Property svn:eol-style set to native
File size: 24.5 KB
Line 
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#include "btGeneric6DofConstraint.h"
23#include "BulletDynamics/Dynamics/btRigidBody.h"
24#include "LinearMath/btTransformUtil.h"
25#include <new>
26
27
28#define D6_USE_OBSOLETE_METHOD false
29//-----------------------------------------------------------------------------
30
31btGeneric6DofConstraint::btGeneric6DofConstraint()
32:btTypedConstraint(D6_CONSTRAINT_TYPE),
33m_useLinearReferenceFrameA(true),
34m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
35{
36}
37
38//-----------------------------------------------------------------------------
39
40btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
41: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
42, m_frameInA(frameInA)
43, m_frameInB(frameInB),
44m_useLinearReferenceFrameA(useLinearReferenceFrameA),
45m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
46{
47
48}
49//-----------------------------------------------------------------------------
50
51
52#define GENERIC_D6_DISABLE_WARMSTARTING 1
53
54//-----------------------------------------------------------------------------
55
56btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
57btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
58{
59        int i = index%3;
60        int j = index/3;
61        return mat[i][j];
62}
63
64//-----------------------------------------------------------------------------
65
66///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
67bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
68bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
69{
70        //      // rot =  cy*cz          -cy*sz           sy
71        //      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
72        //      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
73        //
74
75        btScalar fi = btGetMatrixElem(mat,2);
76        if (fi < btScalar(1.0f))
77        {
78                if (fi > btScalar(-1.0f))
79                {
80                        xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
81                        xyz[1] = btAsin(btGetMatrixElem(mat,2));
82                        xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
83                        return true;
84                }
85                else
86                {
87                        // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
88                        xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
89                        xyz[1] = -SIMD_HALF_PI;
90                        xyz[2] = btScalar(0.0);
91                        return false;
92                }
93        }
94        else
95        {
96                // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
97                xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
98                xyz[1] = SIMD_HALF_PI;
99                xyz[2] = 0.0;
100        }
101        return false;
102}
103
104//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
105
106int btRotationalLimitMotor::testLimitValue(btScalar test_value)
107{
108        if(m_loLimit>m_hiLimit)
109        {
110                m_currentLimit = 0;//Free from violation
111                return 0;
112        }
113
114        if (test_value < m_loLimit)
115        {
116                m_currentLimit = 1;//low limit violation
117                m_currentLimitError =  test_value - m_loLimit;
118                return 1;
119        }
120        else if (test_value> m_hiLimit)
121        {
122                m_currentLimit = 2;//High limit violation
123                m_currentLimitError = test_value - m_hiLimit;
124                return 2;
125        };
126
127        m_currentLimit = 0;//Free from violation
128        return 0;
129
130}
131
132//-----------------------------------------------------------------------------
133
134btScalar btRotationalLimitMotor::solveAngularLimits(
135        btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
136        btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
137{
138        if (needApplyTorques()==false) return 0.0f;
139
140        btScalar target_velocity = m_targetVelocity;
141        btScalar maxMotorForce = m_maxMotorForce;
142
143        //current error correction
144        if (m_currentLimit!=0)
145        {
146                target_velocity = -m_ERP*m_currentLimitError/(timeStep);
147                maxMotorForce = m_maxLimitForce;
148        }
149
150        maxMotorForce *= timeStep;
151
152        // current velocity difference
153
154        btVector3 angVelA;
155        bodyA.getAngularVelocity(angVelA);
156        btVector3 angVelB;
157        bodyB.getAngularVelocity(angVelB);
158
159        btVector3 vel_diff;
160        vel_diff = angVelA-angVelB;
161
162
163
164        btScalar rel_vel = axis.dot(vel_diff);
165
166        // correction velocity
167        btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
168
169
170        if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
171        {
172                return 0.0f;//no need for applying force
173        }
174
175
176        // correction impulse
177        btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
178
179        // clip correction impulse
180        btScalar clippedMotorImpulse;
181
182        ///@todo: should clip against accumulated impulse
183        if (unclippedMotorImpulse>0.0f)
184        {
185                clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
186        }
187        else
188        {
189                clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
190        }
191
192
193        // sort with accumulated impulses
194        btScalar        lo = btScalar(-1e30);
195        btScalar        hi = btScalar(1e30);
196
197        btScalar oldaccumImpulse = m_accumulatedImpulse;
198        btScalar sum = oldaccumImpulse + clippedMotorImpulse;
199        m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
200
201        clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
202
203        btVector3 motorImp = clippedMotorImpulse * axis;
204
205        //body0->applyTorqueImpulse(motorImp);
206        //body1->applyTorqueImpulse(-motorImp);
207
208        bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
209        bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
210
211
212        return clippedMotorImpulse;
213
214
215}
216
217//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
218
219
220
221
222//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
223
224
225int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
226{
227        btScalar loLimit = m_lowerLimit[limitIndex];
228        btScalar hiLimit = m_upperLimit[limitIndex];
229        if(loLimit > hiLimit)
230        {
231                m_currentLimit[limitIndex] = 0;//Free from violation
232                m_currentLimitError[limitIndex] = btScalar(0.f);
233                return 0;
234        }
235
236        if (test_value < loLimit)
237        {
238                m_currentLimit[limitIndex] = 2;//low limit violation
239                m_currentLimitError[limitIndex] =  test_value - loLimit;
240                return 2;
241        }
242        else if (test_value> hiLimit)
243        {
244                m_currentLimit[limitIndex] = 1;//High limit violation
245                m_currentLimitError[limitIndex] = test_value - hiLimit;
246                return 1;
247        };
248
249        m_currentLimit[limitIndex] = 0;//Free from violation
250        m_currentLimitError[limitIndex] = btScalar(0.f);
251        return 0;
252} // btTranslationalLimitMotor::testLimitValue()
253
254//-----------------------------------------------------------------------------
255
256btScalar btTranslationalLimitMotor::solveLinearAxis(
257        btScalar timeStep,
258        btScalar jacDiagABInv,
259        btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
260        btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
261        int limit_index,
262        const btVector3 & axis_normal_on_a,
263        const btVector3 & anchorPos)
264{
265
266        ///find relative velocity
267        //    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
268        //    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
269        btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
270        btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
271
272        btVector3 vel1;
273        bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
274        btVector3 vel2;
275        bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
276        btVector3 vel = vel1 - vel2;
277
278        btScalar rel_vel = axis_normal_on_a.dot(vel);
279
280
281
282        /// apply displacement correction
283
284        //positional error (zeroth order error)
285        btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
286        btScalar        lo = btScalar(-1e30);
287        btScalar        hi = btScalar(1e30);
288
289        btScalar minLimit = m_lowerLimit[limit_index];
290        btScalar maxLimit = m_upperLimit[limit_index];
291
292        //handle the limits
293        if (minLimit < maxLimit)
294        {
295                {
296                        if (depth > maxLimit)
297                        {
298                                depth -= maxLimit;
299                                lo = btScalar(0.);
300
301                        }
302                        else
303                        {
304                                if (depth < minLimit)
305                                {
306                                        depth -= minLimit;
307                                        hi = btScalar(0.);
308                                }
309                                else
310                                {
311                                        return 0.0f;
312                                }
313                        }
314                }
315        }
316
317        btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
318
319
320
321
322        btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
323        btScalar sum = oldNormalImpulse + normalImpulse;
324        m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
325        normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
326
327        btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
328        //body1.applyImpulse( impulse_vector, rel_pos1);
329        //body2.applyImpulse(-impulse_vector, rel_pos2);
330
331        btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
332        btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
333        bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
334        bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
335
336
337
338
339        return normalImpulse;
340}
341
342//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
343
344void btGeneric6DofConstraint::calculateAngleInfo()
345{
346        btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
347        matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
348        // in euler angle mode we do not actually constrain the angular velocity
349        // along the axes axis[0] and axis[2] (although we do use axis[1]) :
350        //
351        //    to get                    constrain w2-w1 along           ...not
352        //    ------                    ---------------------           ------
353        //    d(angle[0])/dt = 0        ax[1] x ax[2]                   ax[0]
354        //    d(angle[1])/dt = 0        ax[1]
355        //    d(angle[2])/dt = 0        ax[0] x ax[1]                   ax[2]
356        //
357        // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
358        // to prove the result for angle[0], write the expression for angle[0] from
359        // GetInfo1 then take the derivative. to prove this for angle[2] it is
360        // easier to take the euler rate expression for d(angle[2])/dt with respect
361        // to the components of w and set that to 0.
362        btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
363        btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
364
365        m_calculatedAxis[1] = axis2.cross(axis0);
366        m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
367        m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
368
369        m_calculatedAxis[0].normalize();
370        m_calculatedAxis[1].normalize();
371        m_calculatedAxis[2].normalize();
372
373}
374
375//-----------------------------------------------------------------------------
376
377void btGeneric6DofConstraint::calculateTransforms()
378{
379        m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
380        m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
381        calculateLinearInfo();
382        calculateAngleInfo();
383}
384
385//-----------------------------------------------------------------------------
386
387void btGeneric6DofConstraint::buildLinearJacobian(
388        btJacobianEntry & jacLinear,const btVector3 & normalWorld,
389        const btVector3 & pivotAInW,const btVector3 & pivotBInW)
390{
391        new (&jacLinear) btJacobianEntry(
392        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
393        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
394        pivotAInW - m_rbA.getCenterOfMassPosition(),
395        pivotBInW - m_rbB.getCenterOfMassPosition(),
396        normalWorld,
397        m_rbA.getInvInertiaDiagLocal(),
398        m_rbA.getInvMass(),
399        m_rbB.getInvInertiaDiagLocal(),
400        m_rbB.getInvMass());
401}
402
403//-----------------------------------------------------------------------------
404
405void btGeneric6DofConstraint::buildAngularJacobian(
406        btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
407{
408         new (&jacAngular)      btJacobianEntry(jointAxisW,
409                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
410                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
411                                      m_rbA.getInvInertiaDiagLocal(),
412                                      m_rbB.getInvInertiaDiagLocal());
413
414}
415
416//-----------------------------------------------------------------------------
417
418bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
419{
420        btScalar angle = m_calculatedAxisAngleDiff[axis_index];
421        //test limits
422        m_angularLimits[axis_index].testLimitValue(angle);
423        return m_angularLimits[axis_index].needApplyTorques();
424}
425
426//-----------------------------------------------------------------------------
427
428void btGeneric6DofConstraint::buildJacobian()
429{
430        if (m_useSolveConstraintObsolete)
431        {
432
433                // Clear accumulated impulses for the next simulation step
434                m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
435                int i;
436                for(i = 0; i < 3; i++)
437                {
438                        m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
439                }
440                //calculates transform
441                calculateTransforms();
442
443                //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
444                //  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
445                calcAnchorPos();
446                btVector3 pivotAInW = m_AnchorPos;
447                btVector3 pivotBInW = m_AnchorPos;
448
449                // not used here
450                //    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
451                //    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
452
453                btVector3 normalWorld;
454                //linear part
455                for (i=0;i<3;i++)
456                {
457                        if (m_linearLimits.isLimited(i))
458                        {
459                                if (m_useLinearReferenceFrameA)
460                                        normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
461                                else
462                                        normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
463
464                                buildLinearJacobian(
465                                        m_jacLinear[i],normalWorld ,
466                                        pivotAInW,pivotBInW);
467
468                        }
469                }
470
471                // angular part
472                for (i=0;i<3;i++)
473                {
474                        //calculates error angle
475                        if (testAngularLimitMotor(i))
476                        {
477                                normalWorld = this->getAxis(i);
478                                // Create angular atom
479                                buildAngularJacobian(m_jacAng[i],normalWorld);
480                        }
481                }
482
483        }
484}
485
486//-----------------------------------------------------------------------------
487
488void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
489{
490        if (m_useSolveConstraintObsolete)
491        {
492                info->m_numConstraintRows = 0;
493                info->nub = 0;
494        } else
495        {
496                //prepare constraint
497                calculateTransforms();
498                info->m_numConstraintRows = 0;
499                info->nub = 6;
500                int i;
501                //test linear limits
502                for(i = 0; i < 3; i++)
503                {
504                        if(m_linearLimits.needApplyForce(i))
505                        {
506                                info->m_numConstraintRows++;
507                                info->nub--;
508                        }
509                }
510                //test angular limits
511                for (i=0;i<3 ;i++ )
512                {
513                        if(testAngularLimitMotor(i))
514                        {
515                                info->m_numConstraintRows++;
516                                info->nub--;
517                        }
518                }
519        }
520}
521
522//-----------------------------------------------------------------------------
523
524void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
525{
526        btAssert(!m_useSolveConstraintObsolete);
527        int row = setLinearLimits(info);
528        setAngularLimits(info, row);
529}
530
531//-----------------------------------------------------------------------------
532
533int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
534{
535        btGeneric6DofConstraint * d6constraint = this;
536        int row = 0;
537        //solve linear limits
538        btRotationalLimitMotor limot;
539        for (int i=0;i<3 ;i++ )
540        {
541                if(m_linearLimits.needApplyForce(i))
542                { // re-use rotational motor code
543                        limot.m_bounce = btScalar(0.f);
544                        limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
545                        limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
546                        limot.m_damping  = m_linearLimits.m_damping;
547                        limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
548                        limot.m_ERP  = m_linearLimits.m_restitution;
549                        limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
550                        limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
551                        limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
552                        limot.m_maxLimitForce  = btScalar(0.f);
553                        limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
554                        limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
555                        btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
556                        row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
557                }
558        }
559        return row;
560}
561
562//-----------------------------------------------------------------------------
563
564int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
565{
566        btGeneric6DofConstraint * d6constraint = this;
567        int row = row_offset;
568        //solve angular limits
569        for (int i=0;i<3 ;i++ )
570        {
571                if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
572                {
573                        btVector3 axis = d6constraint->getAxis(i);
574                        row += get_limit_motor_info2(
575                                d6constraint->getRotationalLimitMotor(i),
576                                &m_rbA,
577                                &m_rbB,
578                                info,row,axis,1);
579                }
580        }
581
582        return row;
583}
584
585//-----------------------------------------------------------------------------
586
587void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar  timeStep)
588{
589        if (m_useSolveConstraintObsolete)
590        {
591
592
593                m_timeStep = timeStep;
594
595                //calculateTransforms();
596
597                int i;
598
599                // linear
600
601                btVector3 pointInA = m_calculatedTransformA.getOrigin();
602                btVector3 pointInB = m_calculatedTransformB.getOrigin();
603
604                btScalar jacDiagABInv;
605                btVector3 linear_axis;
606                for (i=0;i<3;i++)
607                {
608                        if (m_linearLimits.isLimited(i))
609                        {
610                                jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
611
612                                if (m_useLinearReferenceFrameA)
613                                        linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
614                                else
615                                        linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
616
617                                m_linearLimits.solveLinearAxis(
618                                        m_timeStep,
619                                        jacDiagABInv,
620                                        m_rbA,bodyA,pointInA,
621                                        m_rbB,bodyB,pointInB,
622                                        i,linear_axis, m_AnchorPos);
623
624                        }
625                }
626
627                // angular
628                btVector3 angular_axis;
629                btScalar angularJacDiagABInv;
630                for (i=0;i<3;i++)
631                {
632                        if (m_angularLimits[i].needApplyTorques())
633                        {
634
635                                // get axis
636                                angular_axis = getAxis(i);
637
638                                angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
639
640                                m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
641                        }
642                }
643        }
644}
645
646//-----------------------------------------------------------------------------
647
648void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
649{
650        (void)timeStep;
651
652}
653
654//-----------------------------------------------------------------------------
655
656btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
657{
658        return m_calculatedAxis[axis_index];
659}
660
661//-----------------------------------------------------------------------------
662
663btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
664{
665        return m_calculatedAxisAngleDiff[axis_index];
666}
667
668//-----------------------------------------------------------------------------
669
670void btGeneric6DofConstraint::calcAnchorPos(void)
671{
672        btScalar imA = m_rbA.getInvMass();
673        btScalar imB = m_rbB.getInvMass();
674        btScalar weight;
675        if(imB == btScalar(0.0))
676        {
677                weight = btScalar(1.0);
678        }
679        else
680        {
681                weight = imA / (imA + imB);
682        }
683        const btVector3& pA = m_calculatedTransformA.getOrigin();
684        const btVector3& pB = m_calculatedTransformB.getOrigin();
685        m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
686        return;
687} // btGeneric6DofConstraint::calcAnchorPos()
688
689//-----------------------------------------------------------------------------
690
691void btGeneric6DofConstraint::calculateLinearInfo()
692{
693        m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
694        m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
695        for(int i = 0; i < 3; i++)
696        {
697                m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
698        }
699} // btGeneric6DofConstraint::calculateLinearInfo()
700
701//-----------------------------------------------------------------------------
702
703int btGeneric6DofConstraint::get_limit_motor_info2(
704        btRotationalLimitMotor * limot,
705        btRigidBody * body0, btRigidBody * body1,
706        btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
707{
708    int srow = row * info->rowskip;
709    int powered = limot->m_enableMotor;
710    int limit = limot->m_currentLimit;
711    if (powered || limit)
712    {   // if the joint is powered, or has joint limits, add in the extra row
713        btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
714        btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
715        J1[srow+0] = ax1[0];
716        J1[srow+1] = ax1[1];
717        J1[srow+2] = ax1[2];
718        if(rotational)
719        {
720            J2[srow+0] = -ax1[0];
721            J2[srow+1] = -ax1[1];
722            J2[srow+2] = -ax1[2];
723        }
724        if((!rotational) && limit)
725        {
726                        btVector3 ltd;  // Linear Torque Decoupling vector
727                        btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
728                        ltd = c.cross(ax1);
729            info->m_J1angularAxis[srow+0] = ltd[0];
730            info->m_J1angularAxis[srow+1] = ltd[1];
731            info->m_J1angularAxis[srow+2] = ltd[2];
732
733                        c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
734                        ltd = -c.cross(ax1);
735                        info->m_J2angularAxis[srow+0] = ltd[0];
736            info->m_J2angularAxis[srow+1] = ltd[1];
737            info->m_J2angularAxis[srow+2] = ltd[2];
738        }
739        // if we're limited low and high simultaneously, the joint motor is
740        // ineffective
741        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
742        info->m_constraintError[srow] = btScalar(0.f);
743        if (powered)
744        {
745            info->cfm[srow] = 0.0f;
746            if(!limit)
747            {
748                info->m_constraintError[srow] += limot->m_targetVelocity;
749                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
750                info->m_upperLimit[srow] = limot->m_maxMotorForce;
751            }
752        }
753        if(limit)
754        {
755            btScalar k = info->fps * limot->m_ERP;
756                        if(!rotational)
757                        {
758                                info->m_constraintError[srow] += k * limot->m_currentLimitError;
759                        }
760                        else
761                        {
762                                info->m_constraintError[srow] += -k * limot->m_currentLimitError;
763                        }
764            info->cfm[srow] = 0.0f;
765            if (limot->m_loLimit == limot->m_hiLimit)
766            {   // limited low and high simultaneously
767                info->m_lowerLimit[srow] = -SIMD_INFINITY;
768                info->m_upperLimit[srow] = SIMD_INFINITY;
769            }
770            else
771            {
772                if (limit == 1)
773                {
774                    info->m_lowerLimit[srow] = 0;
775                    info->m_upperLimit[srow] = SIMD_INFINITY;
776                }
777                else
778                {
779                    info->m_lowerLimit[srow] = -SIMD_INFINITY;
780                    info->m_upperLimit[srow] = 0;
781                }
782                // deal with bounce
783                if (limot->m_bounce > 0)
784                {
785                    // calculate joint velocity
786                    btScalar vel;
787                    if (rotational)
788                    {
789                        vel = body0->getAngularVelocity().dot(ax1);
790                        if (body1)
791                            vel -= body1->getAngularVelocity().dot(ax1);
792                    }
793                    else
794                    {
795                        vel = body0->getLinearVelocity().dot(ax1);
796                        if (body1)
797                            vel -= body1->getLinearVelocity().dot(ax1);
798                    }
799                    // only apply bounce if the velocity is incoming, and if the
800                    // resulting c[] exceeds what we already have.
801                    if (limit == 1)
802                    {
803                        if (vel < 0)
804                        {
805                            btScalar newc = -limot->m_bounce* vel;
806                            if (newc > info->m_constraintError[srow]) 
807                                                                info->m_constraintError[srow] = newc;
808                        }
809                    }
810                    else
811                    {
812                        if (vel > 0)
813                        {
814                            btScalar newc = -limot->m_bounce * vel;
815                            if (newc < info->m_constraintError[srow]) 
816                                                                info->m_constraintError[srow] = newc;
817                        }
818                    }
819                }
820            }
821        }
822        return 1;
823    }
824    else return 0;
825}
826
827//-----------------------------------------------------------------------------
828//-----------------------------------------------------------------------------
829//-----------------------------------------------------------------------------
Note: See TracBrowser for help on using the repository browser.