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