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