Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @ 12177

Last change on this file since 12177 was 12177, checked in by siramesh, 5 years ago

Super Orxo Bros Final (Sidharth Ramesh, Nisa Balta, Jeff Ren)

File size: 31.6 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
716void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
717{
718        m_frameInA = frameA;
719        m_frameInB = frameB;
720        buildJacobian();
721        calculateTransforms();
722}
723
724
725
726btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
727{
728        return m_calculatedAxis[axis_index];
729}
730
731
732btScalar        btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
733{
734        return m_calculatedLinearDiff[axisIndex];
735}
736
737
738btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
739{
740        return m_calculatedAxisAngleDiff[axisIndex];
741}
742
743
744
745void btGeneric6DofConstraint::calcAnchorPos(void)
746{
747        btScalar imA = m_rbA.getInvMass();
748        btScalar imB = m_rbB.getInvMass();
749        btScalar weight;
750        if(imB == btScalar(0.0))
751        {
752                weight = btScalar(1.0);
753        }
754        else
755        {
756                weight = imA / (imA + imB);
757        }
758        const btVector3& pA = m_calculatedTransformA.getOrigin();
759        const btVector3& pB = m_calculatedTransformB.getOrigin();
760        m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
761        return;
762}
763
764
765
766void btGeneric6DofConstraint::calculateLinearInfo()
767{
768        m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
769        m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
770        for(int i = 0; i < 3; i++)
771        {
772                m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
773                m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
774        }
775}
776
777
778
779int btGeneric6DofConstraint::get_limit_motor_info2(
780        btRotationalLimitMotor * limot,
781        const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
782        btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
783{
784    int srow = row * info->rowskip;
785    int powered = limot->m_enableMotor;
786    int limit = limot->m_currentLimit;
787    if (powered || limit)
788    {   // if the joint is powered, or has joint limits, add in the extra row
789        btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
790        btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
791        J1[srow+0] = ax1[0];
792        J1[srow+1] = ax1[1];
793        J1[srow+2] = ax1[2];
794        if(rotational)
795        {
796            J2[srow+0] = -ax1[0];
797            J2[srow+1] = -ax1[1];
798            J2[srow+2] = -ax1[2];
799        }
800        if((!rotational))
801        {
802                        if (m_useOffsetForConstraintFrame)
803                        {
804                                btVector3 tmpA, tmpB, relA, relB;
805                                // get vector from bodyB to frameB in WCS
806                                relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
807                                // get its projection to constraint axis
808                                btVector3 projB = ax1 * relB.dot(ax1);
809                                // get vector directed from bodyB to constraint axis (and orthogonal to it)
810                                btVector3 orthoB = relB - projB;
811                                // same for bodyA
812                                relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
813                                btVector3 projA = ax1 * relA.dot(ax1);
814                                btVector3 orthoA = relA - projA;
815                                // get desired offset between frames A and B along constraint axis
816                                btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
817                                // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
818                                btVector3 totalDist = projA + ax1 * desiredOffs - projB;
819                                // get offset vectors relA and relB
820                                relA = orthoA + totalDist * m_factA;
821                                relB = orthoB - totalDist * m_factB;
822                                tmpA = relA.cross(ax1);
823                                tmpB = relB.cross(ax1);
824                                if(m_hasStaticBody && (!rotAllowed))
825                                {
826                                        tmpA *= m_factA;
827                                        tmpB *= m_factB;
828                                }
829                                int i;
830                                for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
831                                for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
832                        } else
833                        {
834                                btVector3 ltd;  // Linear Torque Decoupling vector
835                                btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
836                                ltd = c.cross(ax1);
837                                info->m_J1angularAxis[srow+0] = ltd[0];
838                                info->m_J1angularAxis[srow+1] = ltd[1];
839                                info->m_J1angularAxis[srow+2] = ltd[2];
840
841                                c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
842                                ltd = -c.cross(ax1);
843                                info->m_J2angularAxis[srow+0] = ltd[0];
844                                info->m_J2angularAxis[srow+1] = ltd[1];
845                                info->m_J2angularAxis[srow+2] = ltd[2];
846                        }
847        }
848        // if we're limited low and high simultaneously, the joint motor is
849        // ineffective
850        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
851        info->m_constraintError[srow] = btScalar(0.f);
852        if (powered)
853        {
854                        info->cfm[srow] = limot->m_normalCFM;
855            if(!limit)
856            {
857                                btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
858
859                                btScalar mot_fact = getMotorFactor(     limot->m_currentPosition, 
860                                                                                                        limot->m_loLimit,
861                                                                                                        limot->m_hiLimit, 
862                                                                                                        tag_vel, 
863                                                                                                        info->fps * limot->m_stopERP);
864                                info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
865                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
866                info->m_upperLimit[srow] = limot->m_maxMotorForce;
867            }
868        }
869        if(limit)
870        {
871            btScalar k = info->fps * limot->m_stopERP;
872                        if(!rotational)
873                        {
874                                info->m_constraintError[srow] += k * limot->m_currentLimitError;
875                        }
876                        else
877                        {
878                                info->m_constraintError[srow] += -k * limot->m_currentLimitError;
879                        }
880                        info->cfm[srow] = limot->m_stopCFM;
881            if (limot->m_loLimit == limot->m_hiLimit)
882            {   // limited low and high simultaneously
883                info->m_lowerLimit[srow] = -SIMD_INFINITY;
884                info->m_upperLimit[srow] = SIMD_INFINITY;
885            }
886            else
887            {
888                if (limit == 1)
889                {
890                    info->m_lowerLimit[srow] = 0;
891                    info->m_upperLimit[srow] = SIMD_INFINITY;
892                }
893                else
894                {
895                    info->m_lowerLimit[srow] = -SIMD_INFINITY;
896                    info->m_upperLimit[srow] = 0;
897                }
898                // deal with bounce
899                if (limot->m_bounce > 0)
900                {
901                    // calculate joint velocity
902                    btScalar vel;
903                    if (rotational)
904                    {
905                        vel = angVelA.dot(ax1);
906//make sure that if no body -> angVelB == zero vec
907//                        if (body1)
908                            vel -= angVelB.dot(ax1);
909                    }
910                    else
911                    {
912                        vel = linVelA.dot(ax1);
913//make sure that if no body -> angVelB == zero vec
914//                        if (body1)
915                            vel -= linVelB.dot(ax1);
916                    }
917                    // only apply bounce if the velocity is incoming, and if the
918                    // resulting c[] exceeds what we already have.
919                    if (limit == 1)
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                    else
929                    {
930                        if (vel > 0)
931                        {
932                            btScalar newc = -limot->m_bounce * vel;
933                            if (newc < info->m_constraintError[srow]) 
934                                                                info->m_constraintError[srow] = newc;
935                        }
936                    }
937                }
938            }
939        }
940        return 1;
941    }
942    else return 0;
943}
944
945
946
947
948
949
950        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
951        ///If no axis is provided, it uses the default axis for this constraint.
952void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
953{
954        if((axis >= 0) && (axis < 3))
955        {
956                switch(num)
957                {
958                        case BT_CONSTRAINT_STOP_ERP : 
959                                m_linearLimits.m_stopERP[axis] = value;
960                                m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
961                                break;
962                        case BT_CONSTRAINT_STOP_CFM : 
963                                m_linearLimits.m_stopCFM[axis] = value;
964                                m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
965                                break;
966                        case BT_CONSTRAINT_CFM : 
967                                m_linearLimits.m_normalCFM[axis] = value;
968                                m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
969                                break;
970                        default : 
971                                btAssertConstrParams(0);
972                }
973        }
974        else if((axis >=3) && (axis < 6))
975        {
976                switch(num)
977                {
978                        case BT_CONSTRAINT_STOP_ERP : 
979                                m_angularLimits[axis - 3].m_stopERP = value;
980                                m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
981                                break;
982                        case BT_CONSTRAINT_STOP_CFM : 
983                                m_angularLimits[axis - 3].m_stopCFM = value;
984                                m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
985                                break;
986                        case BT_CONSTRAINT_CFM : 
987                                m_angularLimits[axis - 3].m_normalCFM = value;
988                                m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
989                                break;
990                        default : 
991                                btAssertConstrParams(0);
992                }
993        }
994        else
995        {
996                btAssertConstrParams(0);
997        }
998}
999
1000        ///return the local value of parameter
1001btScalar btGeneric6DofConstraint::getParam(int num, int axis) const 
1002{
1003        btScalar retVal = 0;
1004        if((axis >= 0) && (axis < 3))
1005        {
1006                switch(num)
1007                {
1008                        case BT_CONSTRAINT_STOP_ERP : 
1009                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1010                                retVal = m_linearLimits.m_stopERP[axis];
1011                                break;
1012                        case BT_CONSTRAINT_STOP_CFM : 
1013                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1014                                retVal = m_linearLimits.m_stopCFM[axis];
1015                                break;
1016                        case BT_CONSTRAINT_CFM : 
1017                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1018                                retVal = m_linearLimits.m_normalCFM[axis];
1019                                break;
1020                        default : 
1021                                btAssertConstrParams(0);
1022                }
1023        }
1024        else if((axis >=3) && (axis < 6))
1025        {
1026                switch(num)
1027                {
1028                        case BT_CONSTRAINT_STOP_ERP : 
1029                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1030                                retVal = m_angularLimits[axis - 3].m_stopERP;
1031                                break;
1032                        case BT_CONSTRAINT_STOP_CFM : 
1033                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1034                                retVal = m_angularLimits[axis - 3].m_stopCFM;
1035                                break;
1036                        case BT_CONSTRAINT_CFM : 
1037                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1038                                retVal = m_angularLimits[axis - 3].m_normalCFM;
1039                                break;
1040                        default : 
1041                                btAssertConstrParams(0);
1042                }
1043        }
1044        else
1045        {
1046                btAssertConstrParams(0);
1047        }
1048        return retVal;
1049}
1050
1051 
1052
1053void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
1054{
1055        btVector3 zAxis = axis1.normalized();
1056        btVector3 yAxis = axis2.normalized();
1057        btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
1058       
1059        btTransform frameInW;
1060        frameInW.setIdentity();
1061        frameInW.getBasis().setValue(   xAxis[0], yAxis[0], zAxis[0],   
1062                                        xAxis[1], yAxis[1], zAxis[1],
1063                                       xAxis[2], yAxis[2], zAxis[2]);
1064       
1065        // now get constraint frame in local coordinate systems
1066        m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
1067        m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
1068       
1069        calculateTransforms();
1070}
Note: See TracBrowser for help on using the repository browser.