2edf28c6bac60b5fe57ca4d435d84a0010fbfcbc
[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         //test limits
424         m_angularLimits[axis_index].testLimitValue(angle);
425         return m_angularLimits[axis_index].needApplyTorques();
426 }
427
428
429
430 void btGeneric6DofConstraint::buildJacobian()
431 {
432         if (m_useSolveConstraintObsolete)
433         {
434
435                 // Clear accumulated impulses for the next simulation step
436                 m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
437                 int i;
438                 for(i = 0; i < 3; i++)
439                 {
440                         m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
441                 }
442                 //calculates transform
443                 calculateTransforms();
444
445                 //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
446                 //  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
447                 calcAnchorPos();
448                 btVector3 pivotAInW = m_AnchorPos;
449                 btVector3 pivotBInW = m_AnchorPos;
450
451                 // not used here
452                 //    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
453                 //    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
454
455                 btVector3 normalWorld;
456                 //linear part
457                 for (i=0;i<3;i++)
458                 {
459                         if (m_linearLimits.isLimited(i))
460                         {
461                                 if (m_useLinearReferenceFrameA)
462                                         normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
463                                 else
464                                         normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
465
466                                 buildLinearJacobian(
467                                         m_jacLinear[i],normalWorld ,
468                                         pivotAInW,pivotBInW);
469
470                         }
471                 }
472
473                 // angular part
474                 for (i=0;i<3;i++)
475                 {
476                         //calculates error angle
477                         if (testAngularLimitMotor(i))
478                         {
479                                 normalWorld = this->getAxis(i);
480                                 // Create angular atom
481                                 buildAngularJacobian(m_jacAng[i],normalWorld);
482                         }
483                 }
484
485         }
486 }
487
488
489
490 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
491 {
492         if (m_useSolveConstraintObsolete)
493         {
494                 info->m_numConstraintRows = 0;
495                 info->nub = 0;
496         } else
497         {
498                 //prepare constraint
499                 calculateTransforms();
500                 info->m_numConstraintRows = 0;
501                 info->nub = 6;
502                 int i;
503                 //test linear limits
504                 for(i = 0; i < 3; i++)
505                 {
506                         if(m_linearLimits.needApplyForce(i))
507                         {
508                                 info->m_numConstraintRows++;
509                                 info->nub--;
510                         }
511                 }
512                 //test angular limits
513                 for (i=0;i<3 ;i++ )
514                 {
515                         if(testAngularLimitMotor(i))
516                         {
517                                 info->m_numConstraintRows++;
518                                 info->nub--;
519                         }
520                 }
521         }
522 }
523
524
525
526 void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
527 {
528         btAssert(!m_useSolveConstraintObsolete);
529         int row = setLinearLimits(info);
530         setAngularLimits(info, row);
531 }
532
533
534
535 int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
536 {
537         btGeneric6DofConstraint * d6constraint = this;
538         int row = 0;
539         //solve linear limits
540         btRotationalLimitMotor limot;
541         for (int i=0;i<3 ;i++ )
542         {
543                 if(m_linearLimits.needApplyForce(i))
544                 { // re-use rotational motor code
545                         limot.m_bounce = btScalar(0.f);
546                         limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
547                         limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
548                         limot.m_damping  = m_linearLimits.m_damping;
549                         limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
550                         limot.m_ERP  = m_linearLimits.m_restitution;
551                         limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
552                         limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
553                         limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
554                         limot.m_maxLimitForce  = btScalar(0.f);
555                         limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
556                         limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
557                         btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
558                         row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
559                 }
560         }
561         return row;
562 }
563
564
565
566 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
567 {
568         btGeneric6DofConstraint * d6constraint = this;
569         int row = row_offset;
570         //solve angular limits
571         for (int i=0;i<3 ;i++ )
572         {
573                 if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
574                 {
575                         btVector3 axis = d6constraint->getAxis(i);
576                         row += get_limit_motor_info2(
577                                 d6constraint->getRotationalLimitMotor(i),
578                                 &m_rbA,
579                                 &m_rbB,
580                                 info,row,axis,1);
581                 }
582         }
583
584         return row;
585 }
586
587
588
589 void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar  timeStep)
590 {
591         if (m_useSolveConstraintObsolete)
592         {
593
594
595                 m_timeStep = timeStep;
596
597                 //calculateTransforms();
598
599                 int i;
600
601                 // linear
602
603                 btVector3 pointInA = m_calculatedTransformA.getOrigin();
604                 btVector3 pointInB = m_calculatedTransformB.getOrigin();
605
606                 btScalar jacDiagABInv;
607                 btVector3 linear_axis;
608                 for (i=0;i<3;i++)
609                 {
610                         if (m_linearLimits.isLimited(i))
611                         {
612                                 jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
613
614                                 if (m_useLinearReferenceFrameA)
615                                         linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
616                                 else
617                                         linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
618
619                                 m_linearLimits.solveLinearAxis(
620                                         m_timeStep,
621                                         jacDiagABInv,
622                                         m_rbA,bodyA,pointInA,
623                                         m_rbB,bodyB,pointInB,
624                                         i,linear_axis, m_AnchorPos);
625
626                         }
627                 }
628
629                 // angular
630                 btVector3 angular_axis;
631                 btScalar angularJacDiagABInv;
632                 for (i=0;i<3;i++)
633                 {
634                         if (m_angularLimits[i].needApplyTorques())
635                         {
636
637                                 // get axis
638                                 angular_axis = getAxis(i);
639
640                                 angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
641
642                                 m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
643                         }
644                 }
645         }
646 }
647
648
649
650 void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
651 {
652         (void)timeStep;
653
654 }
655
656
657
658 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
659 {
660         return m_calculatedAxis[axis_index];
661 }
662
663
664
665 btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
666 {
667         return m_calculatedAxisAngleDiff[axis_index];
668 }
669
670
671
672 void btGeneric6DofConstraint::calcAnchorPos(void)
673 {
674         btScalar imA = m_rbA.getInvMass();
675         btScalar imB = m_rbB.getInvMass();
676         btScalar weight;
677         if(imB == btScalar(0.0))
678         {
679                 weight = btScalar(1.0);
680         }
681         else
682         {
683                 weight = imA / (imA + imB);
684         }
685         const btVector3& pA = m_calculatedTransformA.getOrigin();
686         const btVector3& pB = m_calculatedTransformB.getOrigin();
687         m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
688         return;
689 }
690
691
692
693 void btGeneric6DofConstraint::calculateLinearInfo()
694 {
695         m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
696         m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
697         for(int i = 0; i < 3; i++)
698         {
699                 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
700         }
701 }
702
703
704
705 int btGeneric6DofConstraint::get_limit_motor_info2(
706         btRotationalLimitMotor * limot,
707         btRigidBody * body0, btRigidBody * body1,
708         btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
709 {
710     int srow = row * info->rowskip;
711     int powered = limot->m_enableMotor;
712     int limit = limot->m_currentLimit;
713     if (powered || limit)
714     {   // if the joint is powered, or has joint limits, add in the extra row
715         btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
716         btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
717         J1[srow+0] = ax1[0];
718         J1[srow+1] = ax1[1];
719         J1[srow+2] = ax1[2];
720         if(rotational)
721         {
722             J2[srow+0] = -ax1[0];
723             J2[srow+1] = -ax1[1];
724             J2[srow+2] = -ax1[2];
725         }
726         if((!rotational) && limit)
727         {
728                         btVector3 ltd;  // Linear Torque Decoupling vector
729                         btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
730                         ltd = c.cross(ax1);
731             info->m_J1angularAxis[srow+0] = ltd[0];
732             info->m_J1angularAxis[srow+1] = ltd[1];
733             info->m_J1angularAxis[srow+2] = ltd[2];
734
735                         c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
736                         ltd = -c.cross(ax1);
737                         info->m_J2angularAxis[srow+0] = ltd[0];
738             info->m_J2angularAxis[srow+1] = ltd[1];
739             info->m_J2angularAxis[srow+2] = ltd[2];
740         }
741         // if we're limited low and high simultaneously, the joint motor is
742         // ineffective
743         if (limit && (limot->m_loLimit == limot->m_hiLimit)) 
744                         powered = 0;
745
746         info->m_constraintError[srow] = btScalar(0.f);
747         if (powered)
748         {
749             info->cfm[srow] = 0.0f;
750             if(!limit)
751             {
752                 info->m_constraintError[srow] += limot->m_targetVelocity;
753                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
754                 info->m_upperLimit[srow] = limot->m_maxMotorForce;
755             }
756         }
757         if(limit)
758         {
759             btScalar k = info->fps * limot->m_ERP;
760                         if(!rotational)
761                         {
762                                 info->m_constraintError[srow] += k * limot->m_currentLimitError;
763                         }
764                         else
765                         {
766                                 info->m_constraintError[srow] += -k * limot->m_currentLimitError;
767                         }
768             info->cfm[srow] = 0.0f;
769             if (limot->m_loLimit == limot->m_hiLimit)
770             {   // limited low and high simultaneously
771                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
772                 info->m_upperLimit[srow] = SIMD_INFINITY;
773             }
774             else
775             {
776                 if (limit == 1)
777                 {
778                     info->m_lowerLimit[srow] = 0;
779                     info->m_upperLimit[srow] = SIMD_INFINITY;
780                 }
781                 else
782                 {
783                     info->m_lowerLimit[srow] = -SIMD_INFINITY;
784                     info->m_upperLimit[srow] = 0;
785                 }
786                 // deal with bounce
787                 if (limot->m_bounce > 0)
788                 {
789                     // calculate joint velocity
790                     btScalar vel;
791                     if (rotational)
792                     {
793                         vel = body0->getAngularVelocity().dot(ax1);
794                         if (body1)
795                             vel -= body1->getAngularVelocity().dot(ax1);
796                     }
797                     else
798                     {
799                         vel = body0->getLinearVelocity().dot(ax1);
800                         if (body1)
801                             vel -= body1->getLinearVelocity().dot(ax1);
802                     }
803                     // only apply bounce if the velocity is incoming, and if the
804                     // resulting c[] exceeds what we already have.
805                     if (limit == 1)
806                     {
807                         if (vel < 0)
808                         {
809                             btScalar newc = -limot->m_bounce* vel;
810                             if (newc > info->m_constraintError[srow]) 
811                                                                 info->m_constraintError[srow] = newc;
812                         }
813                     }
814                     else
815                     {
816                         if (vel > 0)
817                         {
818                             btScalar newc = -limot->m_bounce * vel;
819                             if (newc < info->m_constraintError[srow]) 
820                                                                 info->m_constraintError[srow] = newc;
821                         }
822                     }
823                 }
824             }
825         }
826         return 1;
827     }
828     else return 0;
829 }
830
831
832
833
834 btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
835         : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
836 {
837         for(int i = 0; i < 6; i++)
838         {
839                 m_springEnabled[i] = false;
840                 m_equilibriumPoint[i] = btScalar(0.f);
841                 m_springStiffness[i] = btScalar(0.f);
842         }
843 }
844
845
846 void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
847 {
848         btAssert((index >= 0) && (index < 6));
849         m_springEnabled[index] = onOff;
850         if(index < 3)
851         {
852                 m_linearLimits.m_enableMotor[index] = onOff;
853         }
854         else
855         {
856                 m_angularLimits[index - 3].m_enableMotor = onOff;
857         }
858 }
859
860
861
862 void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
863 {
864         btAssert((index >= 0) && (index < 6));
865         m_springStiffness[index] = stiffness;
866 }
867
868
869 void btGeneric6DofSpringConstraint::setEquilibriumPoint()
870 {
871         calculateTransforms();
872         for(int i = 0; i < 3; i++)
873         {
874                 m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
875         }
876         for(int i = 0; i < 3; i++)
877         {
878                 m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
879         }
880 }
881
882
883
884 void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
885 {
886         btAssert((index >= 0) && (index < 6));
887         calculateTransforms();
888         if(index < 3)
889         {
890                 m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
891         }
892         else
893         {
894                 m_equilibriumPoint[index + 3] = m_calculatedAxisAngleDiff[index];
895         }
896 }
897
898
899
900 void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
901 {
902         // it is assumed that calculateTransforms() have been called before this call
903         int i;
904         btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
905         for(i = 0; i < 3; i++)
906         {
907                 if(m_springEnabled[i])
908                 {
909                         // get current position of constraint
910                         btScalar currPos = m_calculatedLinearDiff[i];
911                         // calculate difference
912                         btScalar delta = currPos - m_equilibriumPoint[i];
913                         // spring force is (delta * m_stiffness) according to Hooke's Law
914                         btScalar force = delta * m_springStiffness[i];
915                         m_linearLimits.m_targetVelocity[i] = force  * info->fps;
916                         m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps;
917                 }
918         }
919         for(i = 0; i < 3; i++)
920         {
921                 if(m_springEnabled[i + 3])
922                 {
923                         // get current position of constraint
924                         btScalar currPos = m_calculatedAxisAngleDiff[i];
925                         // calculate difference
926                         btScalar delta = currPos - m_equilibriumPoint[i+3];
927                         // spring force is (-delta * m_stiffness) according to Hooke's Law
928                         btScalar force = -delta * m_springStiffness[i+3];
929                         m_angularLimits[i].m_targetVelocity = force  * info->fps;
930                         m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
931                 }
932         }
933 }
934
935
936 void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
937 {
938         // this will be called by constraint solver at the constraint setup stage
939         // set current motor parameters
940         internalUpdateSprings(info);
941         // do the rest of job for constraint setup
942         btGeneric6DofConstraint::getInfo2(info);
943 }
944
945
946
947