← Back to branch summary

~siretart/ubuntu/utopic/blender/libav10

« back to all changes in this revision

Viewing changes to extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

  • Committer: Package Import Robot
  • Author(s): Matteo F. Vescovi
  • Date: 2012-07-23 08:54:18 UTC
  • mfrom: (14.2.16 sid)
  • mto: (14.2.19 sid)
  • mto: This revision was merged to the branch mainline in revision 42.
  • Revision ID: package-import@ubuntu.com-20120723085418-9foz30v6afaf5ffs
Tags: 2.63a-2
http://bugs.debian.org/658075
* debian/: Cycles support added (Closes: #658075)
  For now, this top feature has been enabled only
  on [any-amd64 any-i386] architectures because
  of OpenImageIO failing on all others
* debian/: scripts installation path changed
  from /usr/lib to /usr/share:
  + debian/patches/: patchset re-worked for path changing
  + debian/control: "Breaks" field added on yafaray-exporter

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
29
29
#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
30
30
 
31
31
 
32
 
 
33
 
btConeTwistConstraint::btConeTwistConstraint()
34
 
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
35
 
m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
 
32
SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
36
33
{
 
34
        btVector3 vec = axis * invInertiaWorld;
 
35
        return axis.dot(vec);
37
36
}
38
37
 
39
38
 
 
39
 
 
40
 
40
41
btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
41
42
                                                                                         const btTransform& rbAFrame,const btTransform& rbBFrame)
42
43
                                                                                         :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
67
68
        setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
68
69
        m_damping = btScalar(0.01);
69
70
        m_fixThresh = CONETWIST_DEF_FIX_THRESH;
 
71
        m_flags = 0;
 
72
        m_linCFM = btScalar(0.f);
 
73
        m_linERP = btScalar(0.7f);
 
74
        m_angCFM = btScalar(0.f);
70
75
}
71
76
 
72
77
 
73
 
 
74
 
 
75
78
void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
76
79
{
77
80
        if (m_useSolveConstraintObsolete)
83
86
        {
84
87
                info->m_numConstraintRows = 3;
85
88
                info->nub = 3;
86
 
                calcAngleInfo2();
 
89
                calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
87
90
                if(m_solveSwingLimit)
88
91
                {
89
92
                        info->m_numConstraintRows++;
101
104
                }
102
105
        }
103
106
}
 
107
 
 
108
void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
 
109
{
 
110
        //always reserve 6 rows: object transform is not available on SPU
 
111
        info->m_numConstraintRows = 6;
 
112
        info->nub = 0;
 
113
                
 
114
}
104
115
        
105
116
 
106
 
 
107
117
void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
108
118
{
 
119
        getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
 
120
}
 
121
 
 
122
void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
 
123
{
 
124
        calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
 
125
        
109
126
        btAssert(!m_useSolveConstraintObsolete);
110
 
        //retrieve matrices
111
 
        btTransform body0_trans;
112
 
        body0_trans = m_rbA.getCenterOfMassTransform();
113
 
    btTransform body1_trans;
114
 
        body1_trans = m_rbB.getCenterOfMassTransform();
115
127
    // set jacobian
116
128
    info->m_J1linearAxis[0] = 1;
117
129
    info->m_J1linearAxis[info->rowskip+1] = 1;
118
130
    info->m_J1linearAxis[2*info->rowskip+2] = 1;
119
 
        btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
 
131
        btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
120
132
        {
121
133
                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
122
134
                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
124
136
                btVector3 a1neg = -a1;
125
137
                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
126
138
        }
127
 
        btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
 
139
        btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
128
140
        {
129
141
                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
130
142
                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
132
144
                a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
133
145
        }
134
146
    // set right hand side
135
 
    btScalar k = info->fps * info->erp;
 
147
        btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
 
148
    btScalar k = info->fps * linERP;
136
149
    int j;
137
150
        for (j=0; j<3; j++)
138
151
    {
139
 
        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
 
152
        info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
140
153
                info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
141
154
                info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
 
155
                if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
 
156
                {
 
157
                        info->cfm[j*info->rowskip] = m_linCFM;
 
158
                }
142
159
    }
143
160
        int row = 3;
144
161
    int srow = row * info->rowskip;
150
167
                btScalar *J2 = info->m_J2angularAxis;
151
168
                if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
152
169
                {
153
 
                        btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
 
170
                        btTransform trA = transA*m_rbAFrame;
154
171
                        btVector3 p = trA.getBasis().getColumn(1);
155
172
                        btVector3 q = trA.getBasis().getColumn(2);
156
173
                        int srow1 = srow + info->rowskip;
187
204
                        btScalar k = info->fps * m_biasFactor;
188
205
 
189
206
                        info->m_constraintError[srow] = k * m_swingCorrection;
190
 
                        info->cfm[srow] = 0.0f;
 
207
                        if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
 
208
                        {
 
209
                                info->cfm[srow] = m_angCFM;
 
210
                        }
191
211
                        // m_swingCorrection is always positive or 0
192
212
                        info->m_lowerLimit[srow] = 0;
193
213
                        info->m_upperLimit[srow] = SIMD_INFINITY;
207
227
                J2[srow+2] = -ax1[2];
208
228
                btScalar k = info->fps * m_biasFactor;
209
229
                info->m_constraintError[srow] = k * m_twistCorrection;
210
 
                info->cfm[srow] = 0.0f;
 
230
                if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
 
231
                {
 
232
                        info->cfm[srow] = m_angCFM;
 
233
                }
211
234
                if(m_twistSpan > 0.0f)
212
235
                {
213
236
 
275
298
                        }
276
299
                }
277
300
 
278
 
                calcAngleInfo2();
 
301
                calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
279
302
        }
280
303
}
281
304
 
282
305
 
283
306
 
284
 
void    btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
 
307
void    btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar   timeStep)
285
308
{
 
309
        #ifndef __SPU__
286
310
        if (m_useSolveConstraintObsolete)
287
311
        {
288
312
                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
297
321
                        btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
298
322
 
299
323
                        btVector3 vel1;
300
 
                        bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
 
324
                        bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
301
325
                        btVector3 vel2;
302
 
                        bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
 
326
                        bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
303
327
                        btVector3 vel = vel1 - vel2;
304
328
 
305
329
                        for (int i=0;i<3;i++)
316
340
                                
317
341
                                btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
318
342
                                btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
319
 
                                bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
320
 
                                bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
 
343
                                bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
 
344
                                bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
321
345
                
322
346
                        }
323
347
                }
328
352
                        // compute current and predicted transforms
329
353
                        btTransform trACur = m_rbA.getCenterOfMassTransform();
330
354
                        btTransform trBCur = m_rbB.getCenterOfMassTransform();
331
 
                        btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
332
 
                        btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
 
355
                        btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
 
356
                        btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
333
357
                        btTransform trAPred; trAPred.setIdentity(); 
334
358
                        btVector3 zerovec(0,0,0);
335
359
                        btTransformUtil::integrateTransform(
403
427
                                btScalar  impulseMag  = impulse.length();
404
428
                                btVector3 impulseAxis =  impulse / impulseMag;
405
429
 
406
 
                                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
407
 
                                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
 
430
                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
 
431
                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
408
432
 
409
433
                        }
410
434
                }
411
435
                else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
412
436
                {
413
 
                        btVector3 angVelA; bodyA.getAngularVelocity(angVelA);
414
 
                        btVector3 angVelB; bodyB.getAngularVelocity(angVelB);
 
437
                        btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
 
438
                        btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
415
439
                        btVector3 relVel = angVelB - angVelA;
416
440
                        if (relVel.length2() > SIMD_EPSILON)
417
441
                        {
423
447
 
424
448
                                btScalar  impulseMag  = impulse.length();
425
449
                                btVector3 impulseAxis = impulse / impulseMag;
426
 
                                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
427
 
                                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
 
450
                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
 
451
                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
428
452
                        }
429
453
                }
430
454
 
432
456
                {
433
457
                        ///solve angular part
434
458
                        btVector3 angVelA;
435
 
                        bodyA.getAngularVelocity(angVelA);
 
459
                        bodyA.internalGetAngularVelocity(angVelA);
436
460
                        btVector3 angVelB;
437
 
                        bodyB.getAngularVelocity(angVelB);
 
461
                        bodyB.internalGetAngularVelocity(angVelB);
438
462
 
439
463
                        // solve swing limit
440
464
                        if (m_solveSwingLimit)
463
487
                                impulseMag = impulse.length();
464
488
                                btVector3 noTwistSwingAxis = impulse / impulseMag;
465
489
 
466
 
                                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
467
 
                                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
 
490
                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
 
491
                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
468
492
                        }
469
493
 
470
494
 
484
508
 
485
509
                                btVector3 impulse = m_twistAxis * impulseMag;
486
510
 
487
 
                                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
488
 
                                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
 
511
                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
 
512
                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
489
513
                        }               
490
514
                }
491
515
        }
492
 
 
 
516
#else
 
517
btAssert(0);
 
518
#endif //__SPU__
493
519
}
494
520
 
495
521
 
496
522
 
 
523
 
497
524
void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
498
525
{
499
526
        (void)timeStep;
501
528
}
502
529
 
503
530
 
504
 
 
 
531
#ifndef __SPU__
505
532
void btConeTwistConstraint::calcAngleInfo()
506
533
{
507
534
        m_swingCorrection = btScalar(0.);
587
614
                }
588
615
        }
589
616
}
590
 
 
 
617
#endif //__SPU__
591
618
 
592
619
static btVector3 vTwist(1,0,0); // twist axis in constraint's space
593
620
 
594
621
 
595
622
 
596
 
void btConeTwistConstraint::calcAngleInfo2()
 
623
void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
597
624
{
598
625
        m_swingCorrection = btScalar(0.);
599
626
        m_twistLimitSign = btScalar(0.);
601
628
        m_solveSwingLimit = false;
602
629
        // compute rotation of A wrt B (in constraint space)
603
630
        if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
604
 
        {       // it is assumed that setMotorTarget() was already called 
 
631
        {       // it is assumed that setMotorTarget() was alredy called 
605
632
                // and motor target m_qTarget is within constraint limits
606
633
                // TODO : split rotation to pure swing and pure twist
607
634
                // compute desired transforms in world
608
635
                btTransform trPose(m_qTarget);
609
 
                btTransform trA = getRigidBodyA().getCenterOfMassTransform() * m_rbAFrame;
610
 
                btTransform trB = getRigidBodyB().getCenterOfMassTransform() * m_rbBFrame;
 
636
                btTransform trA = transA * m_rbAFrame;
 
637
                btTransform trB = transB * m_rbBFrame;
611
638
                btTransform trDeltaAB = trB * trPose * trA.inverse();
612
639
                btQuaternion qDeltaAB = trDeltaAB.getRotation();
613
640
                btVector3 swingAxis =   btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
624
651
 
625
652
        {
626
653
                // compute rotation of A wrt B (in constraint space)
627
 
                btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
628
 
                btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
 
654
                btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
 
655
                btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
629
656
                btQuaternion qAB = qB.inverse() * qA;
630
657
                // split rotation into cone and twist
631
658
                // (all this is done from B's perspective. Maybe I should be averaging axes...)
664
691
                                m_twistAxisA.setValue(0,0,0);
665
692
 
666
693
                                m_kSwing =  btScalar(1.) /
667
 
                                        (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
668
 
                                         getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
 
694
                                        (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
 
695
                                         computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
669
696
                        }
670
697
                }
671
698
                else
673
700
                        // you haven't set any limits;
674
701
                        // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
675
702
                        // anyway, we have either hinge or fixed joint
676
 
                        btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
677
 
                        btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
678
 
                        btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
679
 
                        btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
 
703
                        btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
 
704
                        btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
 
705
                        btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
 
706
                        btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
680
707
                        btVector3 target;
681
708
                        btScalar x = ivB.dot(ivA);
682
709
                        btScalar y = ivB.dot(jvA);
767
794
                                m_twistAxis = quatRotate(qB, -twistAxis);
768
795
 
769
796
                                m_kTwist = btScalar(1.) /
770
 
                                        (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
771
 
                                         getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
 
797
                                        (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
 
798
                                         computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
772
799
                        }
773
800
 
774
801
                        if (m_solveSwingLimit)
1004
1031
        }
1005
1032
}
1006
1033
 
1007
 
 
 
1034
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
 
1035
///If no axis is provided, it uses the default axis for this constraint.
 
1036
void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
 
1037
{
 
1038
        switch(num)
 
1039
        {
 
1040
                case BT_CONSTRAINT_ERP :
 
1041
                case BT_CONSTRAINT_STOP_ERP :
 
1042
                        if((axis >= 0) && (axis < 3)) 
 
1043
                        {
 
1044
                                m_linERP = value;
 
1045
                                m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
 
1046
                        }
 
1047
                        else
 
1048
                        {
 
1049
                                m_biasFactor = value;
 
1050
                        }
 
1051
                        break;
 
1052
                case BT_CONSTRAINT_CFM :
 
1053
                case BT_CONSTRAINT_STOP_CFM :
 
1054
                        if((axis >= 0) && (axis < 3)) 
 
1055
                        {
 
1056
                                m_linCFM = value;
 
1057
                                m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
 
1058
                        }
 
1059
                        else
 
1060
                        {
 
1061
                                m_angCFM = value;
 
1062
                                m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
 
1063
                        }
 
1064
                        break;
 
1065
                default:
 
1066
                        btAssertConstrParams(0);
 
1067
                        break;
 
1068
        }
 
1069
}
 
1070
 
 
1071
///return the local value of parameter
 
1072
btScalar btConeTwistConstraint::getParam(int num, int axis) const 
 
1073
{
 
1074
        btScalar retVal = 0;
 
1075
        switch(num)
 
1076
        {
 
1077
                case BT_CONSTRAINT_ERP :
 
1078
                case BT_CONSTRAINT_STOP_ERP :
 
1079
                        if((axis >= 0) && (axis < 3)) 
 
1080
                        {
 
1081
                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
 
1082
                                retVal = m_linERP;
 
1083
                        }
 
1084
                        else if((axis >= 3) && (axis < 6)) 
 
1085
                        {
 
1086
                                retVal = m_biasFactor;
 
1087
                        }
 
1088
                        else
 
1089
                        {
 
1090
                                btAssertConstrParams(0);
 
1091
                        }
 
1092
                        break;
 
1093
                case BT_CONSTRAINT_CFM :
 
1094
                case BT_CONSTRAINT_STOP_CFM :
 
1095
                        if((axis >= 0) && (axis < 3)) 
 
1096
                        {
 
1097
                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
 
1098
                                retVal = m_linCFM;
 
1099
                        }
 
1100
                        else if((axis >= 3) && (axis < 6)) 
 
1101
                        {
 
1102
                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
 
1103
                                retVal = m_angCFM;
 
1104
                        }
 
1105
                        else
 
1106
                        {
 
1107
                                btAssertConstrParams(0);
 
1108
                        }
 
1109
                        break;
 
1110
                default : 
 
1111
                        btAssertConstrParams(0);
 
1112
        }
 
1113
        return retVal;
 
1114
}
 
1115
 
 
1116
 
 
1117
void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
 
1118
{
 
1119
        m_rbAFrame = frameA;
 
1120
        m_rbBFrame = frameB;
 
1121
        buildJacobian();
 
1122
        //calculateTransforms();
 
1123
}
 
1124
 
 
1125
 
1008
1126
 
1009
1127