Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/kicklib/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @ 8633

Last change on this file since 8633 was 7983, checked in by rgrieder, 14 years ago

Updated Bullet Physics Engine from v2.74 to v2.77

  • Property svn:eol-style set to native
File size: 30.7 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 "LinearMath/btTransformUtil.h"
26#include <new>
27
28
29
30#define D6_USE_OBSOLETE_METHOD false
31#define D6_USE_FRAME_OFFSET true
32
33
34
35
36
37
38btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
39: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
40, m_frameInA(frameInA)
41, m_frameInB(frameInB),
42m_useLinearReferenceFrameA(useLinearReferenceFrameA),
43m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
44m_flags(0),
45m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
46{
47        calculateTransforms();
48}
49
50
51
52btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
53        : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
54                m_frameInB(frameInB),
55                m_useLinearReferenceFrameA(useLinearReferenceFrameB),
56                m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
57                m_flags(0),
58                m_useSolveConstraintObsolete(false)
59{
60        ///not providing rigidbody A means implicitly using worldspace for body A
61        m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
62        calculateTransforms();
63}
64
65
66
67
68#define GENERIC_D6_DISABLE_WARMSTARTING 1
69
70
71
72btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
73btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
74{
75        int i = index%3;
76        int j = index/3;
77        return mat[i][j];
78}
79
80
81
82///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
83bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
84bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
85{
86        //      // rot =  cy*cz          -cy*sz           sy
87        //      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
88        //      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
89        //
90
91        btScalar fi = btGetMatrixElem(mat,2);
92        if (fi < btScalar(1.0f))
93        {
94                if (fi > btScalar(-1.0f))
95                {
96                        xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
97                        xyz[1] = btAsin(btGetMatrixElem(mat,2));
98                        xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
99                        return true;
100                }
101                else
102                {
103                        // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
104                        xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
105                        xyz[1] = -SIMD_HALF_PI;
106                        xyz[2] = btScalar(0.0);
107                        return false;
108                }
109        }
110        else
111        {
112                // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
113                xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
114                xyz[1] = SIMD_HALF_PI;
115                xyz[2] = 0.0;
116        }
117        return false;
118}
119
120//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
121
122int btRotationalLimitMotor::testLimitValue(btScalar test_value)
123{
124        if(m_loLimit>m_hiLimit)
125        {
126                m_currentLimit = 0;//Free from violation
127                return 0;
128        }
129        if (test_value < m_loLimit)
130        {
131                m_currentLimit = 1;//low limit violation
132                m_currentLimitError =  test_value - m_loLimit;
133                return 1;
134        }
135        else if (test_value> m_hiLimit)
136        {
137                m_currentLimit = 2;//High limit violation
138                m_currentLimitError = test_value - m_hiLimit;
139                return 2;
140        };
141
142        m_currentLimit = 0;//Free from violation
143        return 0;
144
145}
146
147
148
149btScalar btRotationalLimitMotor::solveAngularLimits(
150        btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
151        btRigidBody * body0, btRigidBody * body1 )
152{
153        if (needApplyTorques()==false) return 0.0f;
154
155        btScalar target_velocity = m_targetVelocity;
156        btScalar maxMotorForce = m_maxMotorForce;
157
158        //current error correction
159        if (m_currentLimit!=0)
160        {
161                target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
162                maxMotorForce = m_maxLimitForce;
163        }
164
165        maxMotorForce *= timeStep;
166
167        // current velocity difference
168
169        btVector3 angVelA;
170        body0->internalGetAngularVelocity(angVelA);
171        btVector3 angVelB;
172        body1->internalGetAngularVelocity(angVelB);
173
174        btVector3 vel_diff;
175        vel_diff = angVelA-angVelB;
176
177
178
179        btScalar rel_vel = axis.dot(vel_diff);
180
181        // correction velocity
182        btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
183
184
185        if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
186        {
187                return 0.0f;//no need for applying force
188        }
189
190
191        // correction impulse
192        btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
193
194        // clip correction impulse
195        btScalar clippedMotorImpulse;
196
197        ///@todo: should clip against accumulated impulse
198        if (unclippedMotorImpulse>0.0f)
199        {
200                clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
201        }
202        else
203        {
204                clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
205        }
206
207
208        // sort with accumulated impulses
209        btScalar        lo = btScalar(-BT_LARGE_FLOAT);
210        btScalar        hi = btScalar(BT_LARGE_FLOAT);
211
212        btScalar oldaccumImpulse = m_accumulatedImpulse;
213        btScalar sum = oldaccumImpulse + clippedMotorImpulse;
214        m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
215
216        clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
217
218        btVector3 motorImp = clippedMotorImpulse * axis;
219
220        //body0->applyTorqueImpulse(motorImp);
221        //body1->applyTorqueImpulse(-motorImp);
222
223        body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
224        body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
225
226
227        return clippedMotorImpulse;
228
229
230}
231
232//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
233
234
235
236
237//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
238
239
240int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
241{
242        btScalar loLimit = m_lowerLimit[limitIndex];
243        btScalar hiLimit = m_upperLimit[limitIndex];
244        if(loLimit > hiLimit)
245        {
246                m_currentLimit[limitIndex] = 0;//Free from violation
247                m_currentLimitError[limitIndex] = btScalar(0.f);
248                return 0;
249        }
250
251        if (test_value < loLimit)
252        {
253                m_currentLimit[limitIndex] = 2;//low limit violation
254                m_currentLimitError[limitIndex] =  test_value - loLimit;
255                return 2;
256        }
257        else if (test_value> hiLimit)
258        {
259                m_currentLimit[limitIndex] = 1;//High limit violation
260                m_currentLimitError[limitIndex] = test_value - hiLimit;
261                return 1;
262        };
263
264        m_currentLimit[limitIndex] = 0;//Free from violation
265        m_currentLimitError[limitIndex] = btScalar(0.f);
266        return 0;
267}
268
269
270
271btScalar btTranslationalLimitMotor::solveLinearAxis(
272        btScalar timeStep,
273        btScalar jacDiagABInv,
274        btRigidBody& body1,const btVector3 &pointInA,
275        btRigidBody& body2,const btVector3 &pointInB,
276        int limit_index,
277        const btVector3 & axis_normal_on_a,
278        const btVector3 & anchorPos)
279{
280
281        ///find relative velocity
282        //    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
283        //    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
284        btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
285        btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
286
287        btVector3 vel1;
288        body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
289        btVector3 vel2;
290        body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
291        btVector3 vel = vel1 - vel2;
292
293        btScalar rel_vel = axis_normal_on_a.dot(vel);
294
295
296
297        /// apply displacement correction
298
299        //positional error (zeroth order error)
300        btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
301        btScalar        lo = btScalar(-BT_LARGE_FLOAT);
302        btScalar        hi = btScalar(BT_LARGE_FLOAT);
303
304        btScalar minLimit = m_lowerLimit[limit_index];
305        btScalar maxLimit = m_upperLimit[limit_index];
306
307        //handle the limits
308        if (minLimit < maxLimit)
309        {
310                {
311                        if (depth > maxLimit)
312                        {
313                                depth -= maxLimit;
314                                lo = btScalar(0.);
315
316                        }
317                        else
318                        {
319                                if (depth < minLimit)
320                                {
321                                        depth -= minLimit;
322                                        hi = btScalar(0.);
323                                }
324                                else
325                                {
326                                        return 0.0f;
327                                }
328                        }
329                }
330        }
331
332        btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
333
334
335
336
337        btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
338        btScalar sum = oldNormalImpulse + normalImpulse;
339        m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
340        normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
341
342        btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
343        //body1.applyImpulse( impulse_vector, rel_pos1);
344        //body2.applyImpulse(-impulse_vector, rel_pos2);
345
346        btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
347        btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
348        body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
349        body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
350
351
352
353
354        return normalImpulse;
355}
356
357//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
358
359void btGeneric6DofConstraint::calculateAngleInfo()
360{
361        btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
362        matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
363        // in euler angle mode we do not actually constrain the angular velocity
364        // along the axes axis[0] and axis[2] (although we do use axis[1]) :
365        //
366        //    to get                    constrain w2-w1 along           ...not
367        //    ------                    ---------------------           ------
368        //    d(angle[0])/dt = 0        ax[1] x ax[2]                   ax[0]
369        //    d(angle[1])/dt = 0        ax[1]
370        //    d(angle[2])/dt = 0        ax[0] x ax[1]                   ax[2]
371        //
372        // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
373        // to prove the result for angle[0], write the expression for angle[0] from
374        // GetInfo1 then take the derivative. to prove this for angle[2] it is
375        // easier to take the euler rate expression for d(angle[2])/dt with respect
376        // to the components of w and set that to 0.
377        btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
378        btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
379
380        m_calculatedAxis[1] = axis2.cross(axis0);
381        m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
382        m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
383
384        m_calculatedAxis[0].normalize();
385        m_calculatedAxis[1].normalize();
386        m_calculatedAxis[2].normalize();
387
388}
389
390void btGeneric6DofConstraint::calculateTransforms()
391{
392        calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
393}
394
395void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
396{
397        m_calculatedTransformA = transA * m_frameInA;
398        m_calculatedTransformB = transB * m_frameInB;
399        calculateLinearInfo();
400        calculateAngleInfo();
401        if(m_useOffsetForConstraintFrame)
402        {       //  get weight factors depending on masses
403                btScalar miA = getRigidBodyA().getInvMass();
404                btScalar miB = getRigidBodyB().getInvMass();
405                m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
406                btScalar miS = miA + miB;
407                if(miS > btScalar(0.f))
408                {
409                        m_factA = miB / miS;
410                }
411                else 
412                {
413                        m_factA = btScalar(0.5f);
414                }
415                m_factB = btScalar(1.0f) - m_factA;
416        }
417}
418
419
420
421void btGeneric6DofConstraint::buildLinearJacobian(
422        btJacobianEntry & jacLinear,const btVector3 & normalWorld,
423        const btVector3 & pivotAInW,const btVector3 & pivotBInW)
424{
425        new (&jacLinear) btJacobianEntry(
426        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
427        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
428        pivotAInW - m_rbA.getCenterOfMassPosition(),
429        pivotBInW - m_rbB.getCenterOfMassPosition(),
430        normalWorld,
431        m_rbA.getInvInertiaDiagLocal(),
432        m_rbA.getInvMass(),
433        m_rbB.getInvInertiaDiagLocal(),
434        m_rbB.getInvMass());
435}
436
437
438
439void btGeneric6DofConstraint::buildAngularJacobian(
440        btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
441{
442         new (&jacAngular)      btJacobianEntry(jointAxisW,
443                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
444                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
445                                      m_rbA.getInvInertiaDiagLocal(),
446                                      m_rbB.getInvInertiaDiagLocal());
447
448}
449
450
451
452bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
453{
454        btScalar angle = m_calculatedAxisAngleDiff[axis_index];
455        angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
456        m_angularLimits[axis_index].m_currentPosition = angle;
457        //test limits
458        m_angularLimits[axis_index].testLimitValue(angle);
459        return m_angularLimits[axis_index].needApplyTorques();
460}
461
462
463
464void btGeneric6DofConstraint::buildJacobian()
465{
466#ifndef __SPU__
467        if (m_useSolveConstraintObsolete)
468        {
469
470                // Clear accumulated impulses for the next simulation step
471                m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
472                int i;
473                for(i = 0; i < 3; i++)
474                {
475                        m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
476                }
477                //calculates transform
478                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
479
480                //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
481                //  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
482                calcAnchorPos();
483                btVector3 pivotAInW = m_AnchorPos;
484                btVector3 pivotBInW = m_AnchorPos;
485
486                // not used here
487                //    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
488                //    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
489
490                btVector3 normalWorld;
491                //linear part
492                for (i=0;i<3;i++)
493                {
494                        if (m_linearLimits.isLimited(i))
495                        {
496                                if (m_useLinearReferenceFrameA)
497                                        normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
498                                else
499                                        normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
500
501                                buildLinearJacobian(
502                                        m_jacLinear[i],normalWorld ,
503                                        pivotAInW,pivotBInW);
504
505                        }
506                }
507
508                // angular part
509                for (i=0;i<3;i++)
510                {
511                        //calculates error angle
512                        if (testAngularLimitMotor(i))
513                        {
514                                normalWorld = this->getAxis(i);
515                                // Create angular atom
516                                buildAngularJacobian(m_jacAng[i],normalWorld);
517                        }
518                }
519
520        }
521#endif //__SPU__
522
523}
524
525
526void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
527{
528        if (m_useSolveConstraintObsolete)
529        {
530                info->m_numConstraintRows = 0;
531                info->nub = 0;
532        } else
533        {
534                //prepare constraint
535                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
536                info->m_numConstraintRows = 0;
537                info->nub = 6;
538                int i;
539                //test linear limits
540                for(i = 0; i < 3; i++)
541                {
542                        if(m_linearLimits.needApplyForce(i))
543                        {
544                                info->m_numConstraintRows++;
545                                info->nub--;
546                        }
547                }
548                //test angular limits
549                for (i=0;i<3 ;i++ )
550                {
551                        if(testAngularLimitMotor(i))
552                        {
553                                info->m_numConstraintRows++;
554                                info->nub--;
555                        }
556                }
557        }
558}
559
560void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
561{
562        if (m_useSolveConstraintObsolete)
563        {
564                info->m_numConstraintRows = 0;
565                info->nub = 0;
566        } else
567        {
568                //pre-allocate all 6
569                info->m_numConstraintRows = 6;
570                info->nub = 0;
571        }
572}
573
574
575void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
576{
577        btAssert(!m_useSolveConstraintObsolete);
578
579        const btTransform& transA = m_rbA.getCenterOfMassTransform();
580        const btTransform& transB = m_rbB.getCenterOfMassTransform();
581        const btVector3& linVelA = m_rbA.getLinearVelocity();
582        const btVector3& linVelB = m_rbB.getLinearVelocity();
583        const btVector3& angVelA = m_rbA.getAngularVelocity();
584        const btVector3& angVelB = m_rbB.getAngularVelocity();
585
586        if(m_useOffsetForConstraintFrame)
587        { // for stability better to solve angular limits first
588                int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
589                setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
590        }
591        else
592        { // leave old version for compatibility
593                int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
594                setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
595        }
596
597}
598
599
600void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
601{
602       
603        btAssert(!m_useSolveConstraintObsolete);
604        //prepare constraint
605        calculateTransforms(transA,transB);
606
607        int i;
608        for (i=0;i<3 ;i++ )
609        {
610                testAngularLimitMotor(i);
611        }
612
613        if(m_useOffsetForConstraintFrame)
614        { // for stability better to solve angular limits first
615                int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
616                setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
617        }
618        else
619        { // leave old version for compatibility
620                int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
621                setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
622        }
623}
624
625
626
627int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
628{
629//      int row = 0;
630        //solve linear limits
631        btRotationalLimitMotor limot;
632        for (int i=0;i<3 ;i++ )
633        {
634                if(m_linearLimits.needApplyForce(i))
635                { // re-use rotational motor code
636                        limot.m_bounce = btScalar(0.f);
637                        limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
638                        limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
639                        limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
640                        limot.m_damping  = m_linearLimits.m_damping;
641                        limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
642                        limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
643                        limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
644                        limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
645                        limot.m_maxLimitForce  = btScalar(0.f);
646                        limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
647                        limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
648                        btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
649                        int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
650                        limot.m_normalCFM       = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
651                        limot.m_stopCFM         = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
652                        limot.m_stopERP         = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
653                        if(m_useOffsetForConstraintFrame)
654                        {
655                                int indx1 = (i + 1) % 3;
656                                int indx2 = (i + 2) % 3;
657                                int rotAllowed = 1; // rotations around orthos to current axis
658                                if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
659                                {
660                                        rotAllowed = 0;
661                                }
662                                row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
663                        }
664                        else
665                        {
666                                row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
667                        }
668                }
669        }
670        return row;
671}
672
673
674
675int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
676{
677        btGeneric6DofConstraint * d6constraint = this;
678        int row = row_offset;
679        //solve angular limits
680        for (int i=0;i<3 ;i++ )
681        {
682                if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
683                {
684                        btVector3 axis = d6constraint->getAxis(i);
685                        int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
686                        if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
687                        {
688                                m_angularLimits[i].m_normalCFM = info->cfm[0];
689                        }
690                        if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
691                        {
692                                m_angularLimits[i].m_stopCFM = info->cfm[0];
693                        }
694                        if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
695                        {
696                                m_angularLimits[i].m_stopERP = info->erp;
697                        }
698                        row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
699                                                                                                transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
700                }
701        }
702
703        return row;
704}
705
706
707
708
709void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
710{
711        (void)timeStep;
712
713}
714
715
716
717btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
718{
719        return m_calculatedAxis[axis_index];
720}
721
722
723btScalar        btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
724{
725        return m_calculatedLinearDiff[axisIndex];
726}
727
728
729btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
730{
731        return m_calculatedAxisAngleDiff[axisIndex];
732}
733
734
735
736void btGeneric6DofConstraint::calcAnchorPos(void)
737{
738        btScalar imA = m_rbA.getInvMass();
739        btScalar imB = m_rbB.getInvMass();
740        btScalar weight;
741        if(imB == btScalar(0.0))
742        {
743                weight = btScalar(1.0);
744        }
745        else
746        {
747                weight = imA / (imA + imB);
748        }
749        const btVector3& pA = m_calculatedTransformA.getOrigin();
750        const btVector3& pB = m_calculatedTransformB.getOrigin();
751        m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
752        return;
753}
754
755
756
757void btGeneric6DofConstraint::calculateLinearInfo()
758{
759        m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
760        m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
761        for(int i = 0; i < 3; i++)
762        {
763                m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
764                m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
765        }
766}
767
768
769
770int btGeneric6DofConstraint::get_limit_motor_info2(
771        btRotationalLimitMotor * limot,
772        const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
773        btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
774{
775    int srow = row * info->rowskip;
776    int powered = limot->m_enableMotor;
777    int limit = limot->m_currentLimit;
778    if (powered || limit)
779    {   // if the joint is powered, or has joint limits, add in the extra row
780        btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
781        btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
782        J1[srow+0] = ax1[0];
783        J1[srow+1] = ax1[1];
784        J1[srow+2] = ax1[2];
785        if(rotational)
786        {
787            J2[srow+0] = -ax1[0];
788            J2[srow+1] = -ax1[1];
789            J2[srow+2] = -ax1[2];
790        }
791        if((!rotational))
792        {
793                        if (m_useOffsetForConstraintFrame)
794                        {
795                                btVector3 tmpA, tmpB, relA, relB;
796                                // get vector from bodyB to frameB in WCS
797                                relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
798                                // get its projection to constraint axis
799                                btVector3 projB = ax1 * relB.dot(ax1);
800                                // get vector directed from bodyB to constraint axis (and orthogonal to it)
801                                btVector3 orthoB = relB - projB;
802                                // same for bodyA
803                                relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
804                                btVector3 projA = ax1 * relA.dot(ax1);
805                                btVector3 orthoA = relA - projA;
806                                // get desired offset between frames A and B along constraint axis
807                                btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
808                                // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
809                                btVector3 totalDist = projA + ax1 * desiredOffs - projB;
810                                // get offset vectors relA and relB
811                                relA = orthoA + totalDist * m_factA;
812                                relB = orthoB - totalDist * m_factB;
813                                tmpA = relA.cross(ax1);
814                                tmpB = relB.cross(ax1);
815                                if(m_hasStaticBody && (!rotAllowed))
816                                {
817                                        tmpA *= m_factA;
818                                        tmpB *= m_factB;
819                                }
820                                int i;
821                                for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
822                                for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
823                        } else
824                        {
825                                btVector3 ltd;  // Linear Torque Decoupling vector
826                                btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
827                                ltd = c.cross(ax1);
828                                info->m_J1angularAxis[srow+0] = ltd[0];
829                                info->m_J1angularAxis[srow+1] = ltd[1];
830                                info->m_J1angularAxis[srow+2] = ltd[2];
831
832                                c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
833                                ltd = -c.cross(ax1);
834                                info->m_J2angularAxis[srow+0] = ltd[0];
835                                info->m_J2angularAxis[srow+1] = ltd[1];
836                                info->m_J2angularAxis[srow+2] = ltd[2];
837                        }
838        }
839        // if we're limited low and high simultaneously, the joint motor is
840        // ineffective
841        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
842        info->m_constraintError[srow] = btScalar(0.f);
843        if (powered)
844        {
845                        info->cfm[srow] = limot->m_normalCFM;
846            if(!limit)
847            {
848                                btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
849
850                                btScalar mot_fact = getMotorFactor(     limot->m_currentPosition, 
851                                                                                                        limot->m_loLimit,
852                                                                                                        limot->m_hiLimit, 
853                                                                                                        tag_vel, 
854                                                                                                        info->fps * limot->m_stopERP);
855                                info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
856                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
857                info->m_upperLimit[srow] = limot->m_maxMotorForce;
858            }
859        }
860        if(limit)
861        {
862            btScalar k = info->fps * limot->m_stopERP;
863                        if(!rotational)
864                        {
865                                info->m_constraintError[srow] += k * limot->m_currentLimitError;
866                        }
867                        else
868                        {
869                                info->m_constraintError[srow] += -k * limot->m_currentLimitError;
870                        }
871                        info->cfm[srow] = limot->m_stopCFM;
872            if (limot->m_loLimit == limot->m_hiLimit)
873            {   // limited low and high simultaneously
874                info->m_lowerLimit[srow] = -SIMD_INFINITY;
875                info->m_upperLimit[srow] = SIMD_INFINITY;
876            }
877            else
878            {
879                if (limit == 1)
880                {
881                    info->m_lowerLimit[srow] = 0;
882                    info->m_upperLimit[srow] = SIMD_INFINITY;
883                }
884                else
885                {
886                    info->m_lowerLimit[srow] = -SIMD_INFINITY;
887                    info->m_upperLimit[srow] = 0;
888                }
889                // deal with bounce
890                if (limot->m_bounce > 0)
891                {
892                    // calculate joint velocity
893                    btScalar vel;
894                    if (rotational)
895                    {
896                        vel = angVelA.dot(ax1);
897//make sure that if no body -> angVelB == zero vec
898//                        if (body1)
899                            vel -= angVelB.dot(ax1);
900                    }
901                    else
902                    {
903                        vel = linVelA.dot(ax1);
904//make sure that if no body -> angVelB == zero vec
905//                        if (body1)
906                            vel -= linVelB.dot(ax1);
907                    }
908                    // only apply bounce if the velocity is incoming, and if the
909                    // resulting c[] exceeds what we already have.
910                    if (limit == 1)
911                    {
912                        if (vel < 0)
913                        {
914                            btScalar newc = -limot->m_bounce* vel;
915                            if (newc > info->m_constraintError[srow]) 
916                                                                info->m_constraintError[srow] = newc;
917                        }
918                    }
919                    else
920                    {
921                        if (vel > 0)
922                        {
923                            btScalar newc = -limot->m_bounce * vel;
924                            if (newc < info->m_constraintError[srow]) 
925                                                                info->m_constraintError[srow] = newc;
926                        }
927                    }
928                }
929            }
930        }
931        return 1;
932    }
933    else return 0;
934}
935
936
937
938
939
940
941        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
942        ///If no axis is provided, it uses the default axis for this constraint.
943void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
944{
945        if((axis >= 0) && (axis < 3))
946        {
947                switch(num)
948                {
949                        case BT_CONSTRAINT_STOP_ERP : 
950                                m_linearLimits.m_stopERP[axis] = value;
951                                m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
952                                break;
953                        case BT_CONSTRAINT_STOP_CFM : 
954                                m_linearLimits.m_stopCFM[axis] = value;
955                                m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
956                                break;
957                        case BT_CONSTRAINT_CFM : 
958                                m_linearLimits.m_normalCFM[axis] = value;
959                                m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
960                                break;
961                        default : 
962                                btAssertConstrParams(0);
963                }
964        }
965        else if((axis >=3) && (axis < 6))
966        {
967                switch(num)
968                {
969                        case BT_CONSTRAINT_STOP_ERP : 
970                                m_angularLimits[axis - 3].m_stopERP = value;
971                                m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
972                                break;
973                        case BT_CONSTRAINT_STOP_CFM : 
974                                m_angularLimits[axis - 3].m_stopCFM = value;
975                                m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
976                                break;
977                        case BT_CONSTRAINT_CFM : 
978                                m_angularLimits[axis - 3].m_normalCFM = value;
979                                m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
980                                break;
981                        default : 
982                                btAssertConstrParams(0);
983                }
984        }
985        else
986        {
987                btAssertConstrParams(0);
988        }
989}
990
991        ///return the local value of parameter
992btScalar btGeneric6DofConstraint::getParam(int num, int axis) const 
993{
994        btScalar retVal = 0;
995        if((axis >= 0) && (axis < 3))
996        {
997                switch(num)
998                {
999                        case BT_CONSTRAINT_STOP_ERP : 
1000                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1001                                retVal = m_linearLimits.m_stopERP[axis];
1002                                break;
1003                        case BT_CONSTRAINT_STOP_CFM : 
1004                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1005                                retVal = m_linearLimits.m_stopCFM[axis];
1006                                break;
1007                        case BT_CONSTRAINT_CFM : 
1008                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1009                                retVal = m_linearLimits.m_normalCFM[axis];
1010                                break;
1011                        default : 
1012                                btAssertConstrParams(0);
1013                }
1014        }
1015        else if((axis >=3) && (axis < 6))
1016        {
1017                switch(num)
1018                {
1019                        case BT_CONSTRAINT_STOP_ERP : 
1020                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1021                                retVal = m_angularLimits[axis - 3].m_stopERP;
1022                                break;
1023                        case BT_CONSTRAINT_STOP_CFM : 
1024                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1025                                retVal = m_angularLimits[axis - 3].m_stopCFM;
1026                                break;
1027                        case BT_CONSTRAINT_CFM : 
1028                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1029                                retVal = m_angularLimits[axis - 3].m_normalCFM;
1030                                break;
1031                        default : 
1032                                btAssertConstrParams(0);
1033                }
1034        }
1035        else
1036        {
1037                btAssertConstrParams(0);
1038        }
1039        return retVal;
1040}
Note: See TracBrowser for help on using the repository browser.