Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btHingeConstraint.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16
17 #include "btHingeConstraint.h"
18 #include "BulletDynamics/Dynamics/btRigidBody.h"
19 #include "LinearMath/btTransformUtil.h"
20 #include "LinearMath/btMinMax.h"
21 #include <new>
22 #include "btSolverBody.h"
23
24
25
26 //#define HINGE_USE_OBSOLETE_SOLVER false
27 #define HINGE_USE_OBSOLETE_SOLVER false
28
29 #define HINGE_USE_FRAME_OFFSET true
30
31 #ifndef __SPU__
32
33
34
35
36
37 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
38                                                                          const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
39                                                                          :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
40 #ifdef _BT_USE_CENTER_LIMIT_
41                                                                          m_limit(),
42 #endif
43                                                                          m_angularOnly(false),
44                                                                          m_enableAngularMotor(false),
45                                                                          m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
46                                                                          m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
47                                                                          m_useReferenceFrameA(useReferenceFrameA),
48                                                                          m_flags(0),
49                                                                          m_normalCFM(0),
50                                                                          m_normalERP(0),
51                                                                          m_stopCFM(0),
52                                                                          m_stopERP(0)
53 {
54         m_rbAFrame.getOrigin() = pivotInA;
55         
56         // since no frame is given, assume this to be zero angle and just pick rb transform axis
57         btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
58
59         btVector3 rbAxisA2;
60         btScalar projection = axisInA.dot(rbAxisA1);
61         if (projection >= 1.0f - SIMD_EPSILON) {
62                 rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
63                 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
64         } else if (projection <= -1.0f + SIMD_EPSILON) {
65                 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
66                 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);      
67         } else {
68                 rbAxisA2 = axisInA.cross(rbAxisA1);
69                 rbAxisA1 = rbAxisA2.cross(axisInA);
70         }
71
72         m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
73                                                                         rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
74                                                                         rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
75
76         btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
77         btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
78         btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1);  
79         
80         m_rbBFrame.getOrigin() = pivotInB;
81         m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
82                                                                         rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
83                                                                         rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
84         
85 #ifndef _BT_USE_CENTER_LIMIT_
86         //start with free
87         m_lowerLimit = btScalar(1.0f);
88         m_upperLimit = btScalar(-1.0f);
89         m_biasFactor = 0.3f;
90         m_relaxationFactor = 1.0f;
91         m_limitSoftness = 0.9f;
92         m_solveLimit = false;
93 #endif
94         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
95 }
96
97
98
99 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
100 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
101 #ifdef _BT_USE_CENTER_LIMIT_
102 m_limit(),
103 #endif
104 m_angularOnly(false), m_enableAngularMotor(false), 
105 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
106 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
107 m_useReferenceFrameA(useReferenceFrameA),
108 m_flags(0),
109 m_normalCFM(0),
110 m_normalERP(0),
111 m_stopCFM(0),
112 m_stopERP(0)
113 {
114
115         // since no frame is given, assume this to be zero angle and just pick rb transform axis
116         // fixed axis in worldspace
117         btVector3 rbAxisA1, rbAxisA2;
118         btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
119
120         m_rbAFrame.getOrigin() = pivotInA;
121         m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
122                                                                         rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
123                                                                         rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
124
125         btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
126
127         btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
128         btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
129         btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
130
131
132         m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
133         m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
134                                                                         rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
135                                                                         rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
136         
137 #ifndef _BT_USE_CENTER_LIMIT_
138         //start with free
139         m_lowerLimit = btScalar(1.0f);
140         m_upperLimit = btScalar(-1.0f);
141         m_biasFactor = 0.3f;
142         m_relaxationFactor = 1.0f;
143         m_limitSoftness = 0.9f;
144         m_solveLimit = false;
145 #endif
146         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
147 }
148
149
150
151 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
152                                                                      const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
153 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
154 #ifdef _BT_USE_CENTER_LIMIT_
155 m_limit(),
156 #endif
157 m_angularOnly(false),
158 m_enableAngularMotor(false),
159 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
160 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
161 m_useReferenceFrameA(useReferenceFrameA),
162 m_flags(0),
163 m_normalCFM(0),
164 m_normalERP(0),
165 m_stopCFM(0),
166 m_stopERP(0)
167 {
168 #ifndef _BT_USE_CENTER_LIMIT_
169         //start with free
170         m_lowerLimit = btScalar(1.0f);
171         m_upperLimit = btScalar(-1.0f);
172         m_biasFactor = 0.3f;
173         m_relaxationFactor = 1.0f;
174         m_limitSoftness = 0.9f;
175         m_solveLimit = false;
176 #endif
177         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
178 }                       
179
180
181
182 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
183 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
184 #ifdef _BT_USE_CENTER_LIMIT_
185 m_limit(),
186 #endif
187 m_angularOnly(false),
188 m_enableAngularMotor(false),
189 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
190 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
191 m_useReferenceFrameA(useReferenceFrameA),
192 m_flags(0),
193 m_normalCFM(0),
194 m_normalERP(0),
195 m_stopCFM(0),
196 m_stopERP(0)
197 {
198         ///not providing rigidbody B means implicitly using worldspace for body B
199
200         m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
201 #ifndef _BT_USE_CENTER_LIMIT_
202         //start with free
203         m_lowerLimit = btScalar(1.0f);
204         m_upperLimit = btScalar(-1.0f);
205         m_biasFactor = 0.3f;
206         m_relaxationFactor = 1.0f;
207         m_limitSoftness = 0.9f;
208         m_solveLimit = false;
209 #endif
210         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
211 }
212
213
214
215 void    btHingeConstraint::buildJacobian()
216 {
217         if (m_useSolveConstraintObsolete)
218         {
219                 m_appliedImpulse = btScalar(0.);
220                 m_accMotorImpulse = btScalar(0.);
221
222                 if (!m_angularOnly)
223                 {
224                         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
225                         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
226                         btVector3 relPos = pivotBInW - pivotAInW;
227
228                         btVector3 normal[3];
229                         if (relPos.length2() > SIMD_EPSILON)
230                         {
231                                 normal[0] = relPos.normalized();
232                         }
233                         else
234                         {
235                                 normal[0].setValue(btScalar(1.0),0,0);
236                         }
237
238                         btPlaneSpace1(normal[0], normal[1], normal[2]);
239
240                         for (int i=0;i<3;i++)
241                         {
242                                 new (&m_jac[i]) btJacobianEntry(
243                                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
244                                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
245                                 pivotAInW - m_rbA.getCenterOfMassPosition(),
246                                 pivotBInW - m_rbB.getCenterOfMassPosition(),
247                                 normal[i],
248                                 m_rbA.getInvInertiaDiagLocal(),
249                                 m_rbA.getInvMass(),
250                                 m_rbB.getInvInertiaDiagLocal(),
251                                 m_rbB.getInvMass());
252                         }
253                 }
254
255                 //calculate two perpendicular jointAxis, orthogonal to hingeAxis
256                 //these two jointAxis require equal angular velocities for both bodies
257
258                 //this is unused for now, it's a todo
259                 btVector3 jointAxis0local;
260                 btVector3 jointAxis1local;
261                 
262                 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
263
264                 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
265                 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
266                 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
267                         
268                 new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
269                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
270                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
271                         m_rbA.getInvInertiaDiagLocal(),
272                         m_rbB.getInvInertiaDiagLocal());
273
274                 new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
275                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
276                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
277                         m_rbA.getInvInertiaDiagLocal(),
278                         m_rbB.getInvInertiaDiagLocal());
279
280                 new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
281                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
282                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
283                         m_rbA.getInvInertiaDiagLocal(),
284                         m_rbB.getInvInertiaDiagLocal());
285
286                         // clear accumulator
287                         m_accLimitImpulse = btScalar(0.);
288
289                         // test angular limit
290                         testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
291
292                 //Compute K = J*W*J' for hinge axis
293                 btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
294                 m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
295                                                          getRigidBodyB().computeAngularImpulseDenominator(axisA));
296
297         }
298 }
299
300
301 #endif //__SPU__
302
303
304 static inline btScalar btNormalizeAnglePositive(btScalar angle)
305 {
306   return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI));
307 }
308
309
310
311 static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
312 {
313         btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
314         btNormalizeAnglePositive(accAngle)));
315         return result;
316 }
317
318 static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
319 {
320         btScalar tol(0.3);
321         btScalar result = btShortestAngularDistance(accAngle, curAngle);
322
323           if (btFabs(result) > tol)
324                 return curAngle;
325           else
326                 return accAngle + result;
327
328         return curAngle;
329 }
330
331
332 btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
333 {
334         btScalar hingeAngle = getHingeAngle();
335         m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
336         return m_accumulatedAngle;
337 }
338 void    btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
339 {
340         m_accumulatedAngle  = accAngle;
341 }
342
343 void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
344 {
345         //update m_accumulatedAngle
346         btScalar curHingeAngle = getHingeAngle();
347         m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
348
349         btHingeConstraint::getInfo1(info);
350         
351 }
352
353
354 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
355 {
356
357
358         if (m_useSolveConstraintObsolete)
359         {
360                 info->m_numConstraintRows = 0;
361                 info->nub = 0;
362         }
363         else
364         {
365                 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
366                 info->nub = 1; 
367                 //always add the row, to avoid computation (data is not available yet)
368                 //prepare constraint
369                 testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
370                 if(getSolveLimit() || getEnableAngularMotor())
371                 {
372                         info->m_numConstraintRows++; // limit 3rd anguar as well
373                         info->nub--; 
374                 }
375
376         }
377 }
378
379 void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
380 {
381         if (m_useSolveConstraintObsolete)
382         {
383                 info->m_numConstraintRows = 0;
384                 info->nub = 0;
385         }
386         else
387         {
388                 //always add the 'limit' row, to avoid computation (data is not available yet)
389                 info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
390                 info->nub = 0; 
391         }
392 }
393
394 void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
395 {
396         if(m_useOffsetForConstraintFrame)
397         {
398                 getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
399         }
400         else
401         {
402                 getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
403         }
404 }
405
406
407 void    btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
408 {
409         ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
410         testLimit(transA,transB);
411
412         getInfo2Internal(info,transA,transB,angVelA,angVelB);
413 }
414
415
416 void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
417 {
418
419         btAssert(!m_useSolveConstraintObsolete);
420         int i, skip = info->rowskip;
421         // transforms in world space
422         btTransform trA = transA*m_rbAFrame;
423         btTransform trB = transB*m_rbBFrame;
424         // pivot point
425         btVector3 pivotAInW = trA.getOrigin();
426         btVector3 pivotBInW = trB.getOrigin();
427 #if 0
428         if (0)
429         {
430                 for (i=0;i<6;i++)
431                 {
432                         info->m_J1linearAxis[i*skip]=0;
433                         info->m_J1linearAxis[i*skip+1]=0;
434                         info->m_J1linearAxis[i*skip+2]=0;
435
436                         info->m_J1angularAxis[i*skip]=0;
437                         info->m_J1angularAxis[i*skip+1]=0;
438                         info->m_J1angularAxis[i*skip+2]=0;
439
440                         info->m_J2linearAxis[i*skip]=0;
441                         info->m_J2linearAxis[i*skip+1]=0;
442                         info->m_J2linearAxis[i*skip+2]=0;
443
444                         info->m_J2angularAxis[i*skip]=0;
445                         info->m_J2angularAxis[i*skip+1]=0;
446                         info->m_J2angularAxis[i*skip+2]=0;
447
448                         info->m_constraintError[i*skip]=0.f;
449                 }
450         }
451 #endif //#if 0
452         // linear (all fixed)
453
454         if (!m_angularOnly)
455         {
456                 info->m_J1linearAxis[0] = 1;
457                 info->m_J1linearAxis[skip + 1] = 1;
458                 info->m_J1linearAxis[2 * skip + 2] = 1;
459
460                 info->m_J2linearAxis[0] = -1;
461                 info->m_J2linearAxis[skip + 1] = -1;
462                 info->m_J2linearAxis[2 * skip + 2] = -1;
463         }       
464
465
466
467
468         btVector3 a1 = pivotAInW - transA.getOrigin();
469         {
470                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
471                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
472                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
473                 btVector3 a1neg = -a1;
474                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
475         }
476         btVector3 a2 = pivotBInW - transB.getOrigin();
477         {
478                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
479                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
480                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
481                 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
482         }
483         // linear RHS
484         btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
485
486     btScalar k = info->fps * normalErp;
487         if (!m_angularOnly)
488         {
489                 for(i = 0; i < 3; i++)
490                 {
491                         info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
492                 }
493         }
494         // make rotations around X and Y equal
495         // the hinge axis should be the only unconstrained
496         // rotational axis, the angular velocity of the two bodies perpendicular to
497         // the hinge axis should be equal. thus the constraint equations are
498         //    p*w1 - p*w2 = 0
499         //    q*w1 - q*w2 = 0
500         // where p and q are unit vectors normal to the hinge axis, and w1 and w2
501         // are the angular velocity vectors of the two bodies.
502         // get hinge axis (Z)
503         btVector3 ax1 = trA.getBasis().getColumn(2);
504         // get 2 orthos to hinge axis (X, Y)
505         btVector3 p = trA.getBasis().getColumn(0);
506         btVector3 q = trA.getBasis().getColumn(1);
507         // set the two hinge angular rows 
508     int s3 = 3 * info->rowskip;
509     int s4 = 4 * info->rowskip;
510
511         info->m_J1angularAxis[s3 + 0] = p[0];
512         info->m_J1angularAxis[s3 + 1] = p[1];
513         info->m_J1angularAxis[s3 + 2] = p[2];
514         info->m_J1angularAxis[s4 + 0] = q[0];
515         info->m_J1angularAxis[s4 + 1] = q[1];
516         info->m_J1angularAxis[s4 + 2] = q[2];
517
518         info->m_J2angularAxis[s3 + 0] = -p[0];
519         info->m_J2angularAxis[s3 + 1] = -p[1];
520         info->m_J2angularAxis[s3 + 2] = -p[2];
521         info->m_J2angularAxis[s4 + 0] = -q[0];
522         info->m_J2angularAxis[s4 + 1] = -q[1];
523         info->m_J2angularAxis[s4 + 2] = -q[2];
524     // compute the right hand side of the constraint equation. set relative
525     // body velocities along p and q to bring the hinge back into alignment.
526     // if ax1,ax2 are the unit length hinge axes as computed from body1 and
527     // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
528     // if `theta' is the angle between ax1 and ax2, we need an angular velocity
529     // along u to cover angle erp*theta in one step :
530     //   |angular_velocity| = angle/time = erp*theta / stepsize
531     //                      = (erp*fps) * theta
532     //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
533     //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
534     // ...as ax1 and ax2 are unit length. if theta is smallish,
535     // theta ~= sin(theta), so
536     //    angular_velocity  = (erp*fps) * (ax1 x ax2)
537     // ax1 x ax2 is in the plane space of ax1, so we project the angular
538     // velocity to p and q to find the right hand side.
539     btVector3 ax2 = trB.getBasis().getColumn(2);
540         btVector3 u = ax1.cross(ax2);
541         info->m_constraintError[s3] = k * u.dot(p);
542         info->m_constraintError[s4] = k * u.dot(q);
543         // check angular limits
544         int nrow = 4; // last filled row
545         int srow;
546         btScalar limit_err = btScalar(0.0);
547         int limit = 0;
548         if(getSolveLimit())
549         {
550 #ifdef  _BT_USE_CENTER_LIMIT_
551         limit_err = m_limit.getCorrection() * m_referenceSign;
552 #else
553         limit_err = m_correction * m_referenceSign;
554 #endif
555         limit = (limit_err > btScalar(0.0)) ? 1 : 2;
556
557         }
558         // if the hinge has joint limits or motor, add in the extra row
559         int powered = 0;
560         if(getEnableAngularMotor())
561         {
562                 powered = 1;
563         }
564         if(limit || powered) 
565         {
566                 nrow++;
567                 srow = nrow * info->rowskip;
568                 info->m_J1angularAxis[srow+0] = ax1[0];
569                 info->m_J1angularAxis[srow+1] = ax1[1];
570                 info->m_J1angularAxis[srow+2] = ax1[2];
571
572                 info->m_J2angularAxis[srow+0] = -ax1[0];
573                 info->m_J2angularAxis[srow+1] = -ax1[1];
574                 info->m_J2angularAxis[srow+2] = -ax1[2];
575
576                 btScalar lostop = getLowerLimit();
577                 btScalar histop = getUpperLimit();
578                 if(limit && (lostop == histop))
579                 {  // the joint motor is ineffective
580                         powered = 0;
581                 }
582                 info->m_constraintError[srow] = btScalar(0.0f);
583                 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
584                 if(powered)
585                 {
586                         if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
587                         {
588                                 info->cfm[srow] = m_normalCFM;
589                         }
590                         btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
591                         info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
592                         info->m_lowerLimit[srow] = - m_maxMotorImpulse;
593                         info->m_upperLimit[srow] =   m_maxMotorImpulse;
594                 }
595                 if(limit)
596                 {
597                         k = info->fps * currERP;
598                         info->m_constraintError[srow] += k * limit_err;
599                         if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
600                         {
601                                 info->cfm[srow] = m_stopCFM;
602                         }
603                         if(lostop == histop) 
604                         {
605                                 // limited low and high simultaneously
606                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
607                                 info->m_upperLimit[srow] = SIMD_INFINITY;
608                         }
609                         else if(limit == 1) 
610                         { // low limit
611                                 info->m_lowerLimit[srow] = 0;
612                                 info->m_upperLimit[srow] = SIMD_INFINITY;
613                         }
614                         else 
615                         { // high limit
616                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
617                                 info->m_upperLimit[srow] = 0;
618                         }
619                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
620 #ifdef  _BT_USE_CENTER_LIMIT_
621                         btScalar bounce = m_limit.getRelaxationFactor();
622 #else
623                         btScalar bounce = m_relaxationFactor;
624 #endif
625                         if(bounce > btScalar(0.0))
626                         {
627                                 btScalar vel = angVelA.dot(ax1);
628                                 vel -= angVelB.dot(ax1);
629                                 // only apply bounce if the velocity is incoming, and if the
630                                 // resulting c[] exceeds what we already have.
631                                 if(limit == 1)
632                                 {       // low limit
633                                         if(vel < 0)
634                                         {
635                                                 btScalar newc = -bounce * vel;
636                                                 if(newc > info->m_constraintError[srow])
637                                                 {
638                                                         info->m_constraintError[srow] = newc;
639                                                 }
640                                         }
641                                 }
642                                 else
643                                 {       // high limit - all those computations are reversed
644                                         if(vel > 0)
645                                         {
646                                                 btScalar newc = -bounce * vel;
647                                                 if(newc < info->m_constraintError[srow])
648                                                 {
649                                                         info->m_constraintError[srow] = newc;
650                                                 }
651                                         }
652                                 }
653                         }
654 #ifdef  _BT_USE_CENTER_LIMIT_
655                         info->m_constraintError[srow] *= m_limit.getBiasFactor();
656 #else
657                         info->m_constraintError[srow] *= m_biasFactor;
658 #endif
659                 } // if(limit)
660         } // if angular limit or powered
661 }
662
663
664 void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
665 {
666         m_rbAFrame = frameA;
667         m_rbBFrame = frameB;
668         buildJacobian();
669 }
670
671
672 void    btHingeConstraint::updateRHS(btScalar   timeStep)
673 {
674         (void)timeStep;
675
676 }
677
678
679
680
681 btScalar btHingeConstraint::getHingeAngle()
682 {
683         return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
684 }
685
686 btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
687 {
688         const btVector3 refAxis0  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
689         const btVector3 refAxis1  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
690         const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
691 //      btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
692         btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
693         return m_referenceSign * angle;
694 }
695
696
697
698 void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
699 {
700         // Compute limit information
701         m_hingeAngle = getHingeAngle(transA,transB);
702 #ifdef  _BT_USE_CENTER_LIMIT_
703         m_limit.test(m_hingeAngle);
704 #else
705         m_correction = btScalar(0.);
706         m_limitSign = btScalar(0.);
707         m_solveLimit = false;
708         if (m_lowerLimit <= m_upperLimit)
709         {
710                 m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
711                 if (m_hingeAngle <= m_lowerLimit)
712                 {
713                         m_correction = (m_lowerLimit - m_hingeAngle);
714                         m_limitSign = 1.0f;
715                         m_solveLimit = true;
716                 } 
717                 else if (m_hingeAngle >= m_upperLimit)
718                 {
719                         m_correction = m_upperLimit - m_hingeAngle;
720                         m_limitSign = -1.0f;
721                         m_solveLimit = true;
722                 }
723         }
724 #endif
725         return;
726 }
727
728
729 static btVector3 vHinge(0, 0, btScalar(1));
730
731 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
732 {
733         // convert target from body to constraint space
734         btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
735         qConstraint.normalize();
736
737         // extract "pure" hinge component
738         btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
739         btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
740         btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
741         qHinge.normalize();
742
743         // compute angular target, clamped to limits
744         btScalar targetAngle = qHinge.getAngle();
745         if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
746         {
747                 qHinge = -(qHinge);
748                 targetAngle = qHinge.getAngle();
749         }
750         if (qHinge.getZ() < 0)
751                 targetAngle = -targetAngle;
752
753         setMotorTarget(targetAngle, dt);
754 }
755
756 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
757 {
758 #ifdef  _BT_USE_CENTER_LIMIT_
759         m_limit.fit(targetAngle);
760 #else
761         if (m_lowerLimit < m_upperLimit)
762         {
763                 if (targetAngle < m_lowerLimit)
764                         targetAngle = m_lowerLimit;
765                 else if (targetAngle > m_upperLimit)
766                         targetAngle = m_upperLimit;
767         }
768 #endif
769         // compute angular velocity
770         btScalar curAngle  = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
771         btScalar dAngle = targetAngle - curAngle;
772         m_motorTargetVelocity = dAngle / dt;
773 }
774
775
776
777 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
778 {
779         btAssert(!m_useSolveConstraintObsolete);
780         int i, s = info->rowskip;
781         // transforms in world space
782         btTransform trA = transA*m_rbAFrame;
783         btTransform trB = transB*m_rbBFrame;
784         // pivot point
785 //      btVector3 pivotAInW = trA.getOrigin();
786 //      btVector3 pivotBInW = trB.getOrigin();
787 #if 1
788         // difference between frames in WCS
789         btVector3 ofs = trB.getOrigin() - trA.getOrigin();
790         // now get weight factors depending on masses
791         btScalar miA = getRigidBodyA().getInvMass();
792         btScalar miB = getRigidBodyB().getInvMass();
793         bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
794         btScalar miS = miA + miB;
795         btScalar factA, factB;
796         if(miS > btScalar(0.f))
797         {
798                 factA = miB / miS;
799         }
800         else 
801         {
802                 factA = btScalar(0.5f);
803         }
804         factB = btScalar(1.0f) - factA;
805         // get the desired direction of hinge axis
806         // as weighted sum of Z-orthos of frameA and frameB in WCS
807         btVector3 ax1A = trA.getBasis().getColumn(2);
808         btVector3 ax1B = trB.getBasis().getColumn(2);
809         btVector3 ax1 = ax1A * factA + ax1B * factB;
810         ax1.normalize();
811         // fill first 3 rows 
812         // we want: velA + wA x relA == velB + wB x relB
813         btTransform bodyA_trans = transA;
814         btTransform bodyB_trans = transB;
815         int s0 = 0;
816         int s1 = s;
817         int s2 = s * 2;
818         int nrow = 2; // last filled row
819         btVector3 tmpA, tmpB, relA, relB, p, q;
820         // get vector from bodyB to frameB in WCS
821         relB = trB.getOrigin() - bodyB_trans.getOrigin();
822         // get its projection to hinge axis
823         btVector3 projB = ax1 * relB.dot(ax1);
824         // get vector directed from bodyB to hinge axis (and orthogonal to it)
825         btVector3 orthoB = relB - projB;
826         // same for bodyA
827         relA = trA.getOrigin() - bodyA_trans.getOrigin();
828         btVector3 projA = ax1 * relA.dot(ax1);
829         btVector3 orthoA = relA - projA;
830         btVector3 totalDist = projA - projB;
831         // get offset vectors relA and relB
832         relA = orthoA + totalDist * factA;
833         relB = orthoB - totalDist * factB;
834         // now choose average ortho to hinge axis
835         p = orthoB * factA + orthoA * factB;
836         btScalar len2 = p.length2();
837         if(len2 > SIMD_EPSILON)
838         {
839                 p /= btSqrt(len2);
840         }
841         else
842         {
843                 p = trA.getBasis().getColumn(1);
844         }
845         // make one more ortho
846         q = ax1.cross(p);
847         // fill three rows
848         tmpA = relA.cross(p);
849         tmpB = relB.cross(p);
850     for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
851     for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
852         tmpA = relA.cross(q);
853         tmpB = relB.cross(q);
854         if(hasStaticBody && getSolveLimit())
855         { // to make constraint between static and dynamic objects more rigid
856                 // remove wA (or wB) from equation if angular limit is hit
857                 tmpB *= factB;
858                 tmpA *= factA;
859         }
860         for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
861     for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
862         tmpA = relA.cross(ax1);
863         tmpB = relB.cross(ax1);
864         if(hasStaticBody)
865         { // to make constraint between static and dynamic objects more rigid
866                 // remove wA (or wB) from equation
867                 tmpB *= factB;
868                 tmpA *= factA;
869         }
870         for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
871     for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
872
873         btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
874         btScalar k = info->fps * normalErp;
875
876         if (!m_angularOnly)
877         {
878                 for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
879                 for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
880                 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
881
882                 for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i];
883                 for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i];
884                 for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i];
885
886         // compute three elements of right hand side
887         
888                 btScalar rhs = k * p.dot(ofs);
889                 info->m_constraintError[s0] = rhs;
890                 rhs = k * q.dot(ofs);
891                 info->m_constraintError[s1] = rhs;
892                 rhs = k * ax1.dot(ofs);
893                 info->m_constraintError[s2] = rhs;
894         }
895         // the hinge axis should be the only unconstrained
896         // rotational axis, the angular velocity of the two bodies perpendicular to
897         // the hinge axis should be equal. thus the constraint equations are
898         //    p*w1 - p*w2 = 0
899         //    q*w1 - q*w2 = 0
900         // where p and q are unit vectors normal to the hinge axis, and w1 and w2
901         // are the angular velocity vectors of the two bodies.
902         int s3 = 3 * s;
903         int s4 = 4 * s;
904         info->m_J1angularAxis[s3 + 0] = p[0];
905         info->m_J1angularAxis[s3 + 1] = p[1];
906         info->m_J1angularAxis[s3 + 2] = p[2];
907         info->m_J1angularAxis[s4 + 0] = q[0];
908         info->m_J1angularAxis[s4 + 1] = q[1];
909         info->m_J1angularAxis[s4 + 2] = q[2];
910
911         info->m_J2angularAxis[s3 + 0] = -p[0];
912         info->m_J2angularAxis[s3 + 1] = -p[1];
913         info->m_J2angularAxis[s3 + 2] = -p[2];
914         info->m_J2angularAxis[s4 + 0] = -q[0];
915         info->m_J2angularAxis[s4 + 1] = -q[1];
916         info->m_J2angularAxis[s4 + 2] = -q[2];
917         // compute the right hand side of the constraint equation. set relative
918         // body velocities along p and q to bring the hinge back into alignment.
919         // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
920         // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
921         // if "theta" is the angle between ax1 and ax2, we need an angular velocity
922         // along u to cover angle erp*theta in one step :
923         //   |angular_velocity| = angle/time = erp*theta / stepsize
924         //                      = (erp*fps) * theta
925         //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
926         //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
927         // ...as ax1 and ax2 are unit length. if theta is smallish,
928         // theta ~= sin(theta), so
929         //    angular_velocity  = (erp*fps) * (ax1 x ax2)
930         // ax1 x ax2 is in the plane space of ax1, so we project the angular
931         // velocity to p and q to find the right hand side.
932         k = info->fps * normalErp;//??
933
934         btVector3 u = ax1A.cross(ax1B);
935         info->m_constraintError[s3] = k * u.dot(p);
936         info->m_constraintError[s4] = k * u.dot(q);
937 #endif
938         // check angular limits
939         nrow = 4; // last filled row
940         int srow;
941         btScalar limit_err = btScalar(0.0);
942         int limit = 0;
943         if(getSolveLimit())
944         {
945 #ifdef  _BT_USE_CENTER_LIMIT_
946         limit_err = m_limit.getCorrection() * m_referenceSign;
947 #else
948         limit_err = m_correction * m_referenceSign;
949 #endif
950         limit = (limit_err > btScalar(0.0)) ? 1 : 2;
951
952         }
953         // if the hinge has joint limits or motor, add in the extra row
954         int powered = 0;
955         if(getEnableAngularMotor())
956         {
957                 powered = 1;
958         }
959         if(limit || powered) 
960         {
961                 nrow++;
962                 srow = nrow * info->rowskip;
963                 info->m_J1angularAxis[srow+0] = ax1[0];
964                 info->m_J1angularAxis[srow+1] = ax1[1];
965                 info->m_J1angularAxis[srow+2] = ax1[2];
966
967                 info->m_J2angularAxis[srow+0] = -ax1[0];
968                 info->m_J2angularAxis[srow+1] = -ax1[1];
969                 info->m_J2angularAxis[srow+2] = -ax1[2];
970
971                 btScalar lostop = getLowerLimit();
972                 btScalar histop = getUpperLimit();
973                 if(limit && (lostop == histop))
974                 {  // the joint motor is ineffective
975                         powered = 0;
976                 }
977                 info->m_constraintError[srow] = btScalar(0.0f);
978                 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
979                 if(powered)
980                 {
981                         if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
982                         {
983                                 info->cfm[srow] = m_normalCFM;
984                         }
985                         btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
986                         info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
987                         info->m_lowerLimit[srow] = - m_maxMotorImpulse;
988                         info->m_upperLimit[srow] =   m_maxMotorImpulse;
989                 }
990                 if(limit)
991                 {
992                         k = info->fps * currERP;
993                         info->m_constraintError[srow] += k * limit_err;
994                         if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
995                         {
996                                 info->cfm[srow] = m_stopCFM;
997                         }
998                         if(lostop == histop) 
999                         {
1000                                 // limited low and high simultaneously
1001                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
1002                                 info->m_upperLimit[srow] = SIMD_INFINITY;
1003                         }
1004                         else if(limit == 1) 
1005                         { // low limit
1006                                 info->m_lowerLimit[srow] = 0;
1007                                 info->m_upperLimit[srow] = SIMD_INFINITY;
1008                         }
1009                         else 
1010                         { // high limit
1011                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
1012                                 info->m_upperLimit[srow] = 0;
1013                         }
1014                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
1015 #ifdef  _BT_USE_CENTER_LIMIT_
1016                         btScalar bounce = m_limit.getRelaxationFactor();
1017 #else
1018                         btScalar bounce = m_relaxationFactor;
1019 #endif
1020                         if(bounce > btScalar(0.0))
1021                         {
1022                                 btScalar vel = angVelA.dot(ax1);
1023                                 vel -= angVelB.dot(ax1);
1024                                 // only apply bounce if the velocity is incoming, and if the
1025                                 // resulting c[] exceeds what we already have.
1026                                 if(limit == 1)
1027                                 {       // low limit
1028                                         if(vel < 0)
1029                                         {
1030                                                 btScalar newc = -bounce * vel;
1031                                                 if(newc > info->m_constraintError[srow])
1032                                                 {
1033                                                         info->m_constraintError[srow] = newc;
1034                                                 }
1035                                         }
1036                                 }
1037                                 else
1038                                 {       // high limit - all those computations are reversed
1039                                         if(vel > 0)
1040                                         {
1041                                                 btScalar newc = -bounce * vel;
1042                                                 if(newc < info->m_constraintError[srow])
1043                                                 {
1044                                                         info->m_constraintError[srow] = newc;
1045                                                 }
1046                                         }
1047                                 }
1048                         }
1049 #ifdef  _BT_USE_CENTER_LIMIT_
1050                         info->m_constraintError[srow] *= m_limit.getBiasFactor();
1051 #else
1052                         info->m_constraintError[srow] *= m_biasFactor;
1053 #endif
1054                 } // if(limit)
1055         } // if angular limit or powered
1056 }
1057
1058
1059 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
1060 ///If no axis is provided, it uses the default axis for this constraint.
1061 void btHingeConstraint::setParam(int num, btScalar value, int axis)
1062 {
1063         if((axis == -1) || (axis == 5))
1064         {
1065                 switch(num)
1066                 {       
1067                         case BT_CONSTRAINT_STOP_ERP :
1068                                 m_stopERP = value;
1069                                 m_flags |= BT_HINGE_FLAGS_ERP_STOP;
1070                                 break;
1071                         case BT_CONSTRAINT_STOP_CFM :
1072                                 m_stopCFM = value;
1073                                 m_flags |= BT_HINGE_FLAGS_CFM_STOP;
1074                                 break;
1075                         case BT_CONSTRAINT_CFM :
1076                                 m_normalCFM = value;
1077                                 m_flags |= BT_HINGE_FLAGS_CFM_NORM;
1078                                 break;
1079                         case BT_CONSTRAINT_ERP:
1080                                 m_normalERP = value;
1081                                 m_flags |= BT_HINGE_FLAGS_ERP_NORM;
1082                                 break;
1083                         default : 
1084                                 btAssertConstrParams(0);
1085                 }
1086         }
1087         else
1088         {
1089                 btAssertConstrParams(0);
1090         }
1091 }
1092
1093 ///return the local value of parameter
1094 btScalar btHingeConstraint::getParam(int num, int axis) const 
1095 {
1096         btScalar retVal = 0;
1097         if((axis == -1) || (axis == 5))
1098         {
1099                 switch(num)
1100                 {       
1101                         case BT_CONSTRAINT_STOP_ERP :
1102                                 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
1103                                 retVal = m_stopERP;
1104                                 break;
1105                         case BT_CONSTRAINT_STOP_CFM :
1106                                 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
1107                                 retVal = m_stopCFM;
1108                                 break;
1109                         case BT_CONSTRAINT_CFM :
1110                                 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
1111                                 retVal = m_normalCFM;
1112                                 break;
1113                         case BT_CONSTRAINT_ERP:
1114                                 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
1115                                 retVal = m_normalERP;
1116                                 break;
1117                         default : 
1118                                 btAssertConstrParams(0);
1119                 }
1120         }
1121         else
1122         {
1123                 btAssertConstrParams(0);
1124         }
1125         return retVal;
1126 }
1127
1128