Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btSliderConstraint.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 Added by Roman Ponomarev (rponom@gmail.com)
18 April 04, 2008
19 */
20
21
22
23 #include "btSliderConstraint.h"
24 #include "BulletDynamics/Dynamics/btRigidBody.h"
25 #include "LinearMath/btTransformUtil.h"
26 #include <new>
27
28 #define USE_OFFSET_FOR_CONSTANT_FRAME true
29
30 void btSliderConstraint::initParams()
31 {
32     m_lowerLinLimit = btScalar(1.0);
33     m_upperLinLimit = btScalar(-1.0);
34     m_lowerAngLimit = btScalar(0.);
35     m_upperAngLimit = btScalar(0.);
36         m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
37         m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
38         m_dampingDirLin = btScalar(0.);
39         m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM;
40         m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
41         m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
42         m_dampingDirAng = btScalar(0.);
43         m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM;
44         m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
45         m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
46         m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
47         m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM;
48         m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
49         m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
50         m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
51         m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM;
52         m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
53         m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
54         m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
55         m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM;
56         m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
57         m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
58         m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
59         m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
60
61         m_poweredLinMotor = false;
62     m_targetLinMotorVelocity = btScalar(0.);
63     m_maxLinMotorForce = btScalar(0.);
64         m_accumulatedLinMotorImpulse = btScalar(0.0);
65
66         m_poweredAngMotor = false;
67     m_targetAngMotorVelocity = btScalar(0.);
68     m_maxAngMotorForce = btScalar(0.);
69         m_accumulatedAngMotorImpulse = btScalar(0.0);
70
71         m_flags = 0;
72         m_flags = 0;
73
74         m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
75
76         calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
77 }
78
79
80
81
82
83 btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
84         : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
85                 m_useSolveConstraintObsolete(false),
86                 m_frameInA(frameInA),
87         m_frameInB(frameInB),
88                 m_useLinearReferenceFrameA(useLinearReferenceFrameA)
89 {
90         initParams();
91 }
92
93
94
95 btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
96         : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
97                 m_useSolveConstraintObsolete(false),
98                 m_frameInB(frameInB),
99                 m_useLinearReferenceFrameA(useLinearReferenceFrameA)
100 {
101         ///not providing rigidbody A means implicitly using worldspace for body A
102         m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
103 //      m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
104
105         initParams();
106 }
107
108
109
110
111
112
113 void btSliderConstraint::getInfo1(btConstraintInfo1* info)
114 {
115         if (m_useSolveConstraintObsolete)
116         {
117                 info->m_numConstraintRows = 0;
118                 info->nub = 0;
119         }
120         else
121         {
122                 info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
123                 info->nub = 2; 
124                 //prepare constraint
125                 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
126                 testAngLimits();
127                 testLinLimits();
128                 if(getSolveLinLimit() || getPoweredLinMotor())
129                 {
130                         info->m_numConstraintRows++; // limit 3rd linear as well
131                         info->nub--; 
132                 }
133                 if(getSolveAngLimit() || getPoweredAngMotor())
134                 {
135                         info->m_numConstraintRows++; // limit 3rd angular as well
136                         info->nub--; 
137                 }
138         }
139 }
140
141 void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
142 {
143
144         info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
145         info->nub = 0; 
146 }
147
148 void btSliderConstraint::getInfo2(btConstraintInfo2* info)
149 {
150         getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
151 }
152
153
154
155
156
157
158
159 void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
160 {
161         if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
162         {
163                 m_calculatedTransformA = transA * m_frameInA;
164                 m_calculatedTransformB = transB * m_frameInB;
165         }
166         else
167         {
168                 m_calculatedTransformA = transB * m_frameInB;
169                 m_calculatedTransformB = transA * m_frameInA;
170         }
171         m_realPivotAInW = m_calculatedTransformA.getOrigin();
172         m_realPivotBInW = m_calculatedTransformB.getOrigin();
173         m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
174         if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
175         {
176                 m_delta = m_realPivotBInW - m_realPivotAInW;
177         }
178         else
179         {
180                 m_delta = m_realPivotAInW - m_realPivotBInW;
181         }
182         m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
183     btVector3 normalWorld;
184     int i;
185     //linear part
186     for(i = 0; i < 3; i++)
187     {
188                 normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
189                 m_depth[i] = m_delta.dot(normalWorld);
190     }
191 }
192  
193
194
195 void btSliderConstraint::testLinLimits(void)
196 {
197         m_solveLinLim = false;
198         m_linPos = m_depth[0];
199         if(m_lowerLinLimit <= m_upperLinLimit)
200         {
201                 if(m_depth[0] > m_upperLinLimit)
202                 {
203                         m_depth[0] -= m_upperLinLimit;
204                         m_solveLinLim = true;
205                 }
206                 else if(m_depth[0] < m_lowerLinLimit)
207                 {
208                         m_depth[0] -= m_lowerLinLimit;
209                         m_solveLinLim = true;
210                 }
211                 else
212                 {
213                         m_depth[0] = btScalar(0.);
214                 }
215         }
216         else
217         {
218                 m_depth[0] = btScalar(0.);
219         }
220 }
221
222
223
224 void btSliderConstraint::testAngLimits(void)
225 {
226         m_angDepth = btScalar(0.);
227         m_solveAngLim = false;
228         if(m_lowerAngLimit <= m_upperAngLimit)
229         {
230                 const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
231                 const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
232                 const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
233 //              btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
234                 btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));  
235                 rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
236                 m_angPos = rot;
237                 if(rot < m_lowerAngLimit)
238                 {
239                         m_angDepth = rot - m_lowerAngLimit;
240                         m_solveAngLim = true;
241                 } 
242                 else if(rot > m_upperAngLimit)
243                 {
244                         m_angDepth = rot - m_upperAngLimit;
245                         m_solveAngLim = true;
246                 }
247         }
248 }
249
250 btVector3 btSliderConstraint::getAncorInA(void)
251 {
252         btVector3 ancorInA;
253         ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
254         ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
255         return ancorInA;
256 }
257
258
259
260 btVector3 btSliderConstraint::getAncorInB(void)
261 {
262         btVector3 ancorInB;
263         ancorInB = m_frameInB.getOrigin();
264         return ancorInB;
265 }
266
267
268 void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
269 {
270         const btTransform& trA = getCalculatedTransformA();
271         const btTransform& trB = getCalculatedTransformB();
272         
273         btAssert(!m_useSolveConstraintObsolete);
274         int i, s = info->rowskip;
275         
276         btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
277         
278         // difference between frames in WCS
279         btVector3 ofs = trB.getOrigin() - trA.getOrigin();
280         // now get weight factors depending on masses
281         btScalar miA = rbAinvMass;
282         btScalar miB = rbBinvMass;
283         bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
284         btScalar miS = miA + miB;
285         btScalar factA, factB;
286         if(miS > btScalar(0.f))
287         {
288                 factA = miB / miS;
289         }
290         else 
291         {
292                 factA = btScalar(0.5f);
293         }
294         factB = btScalar(1.0f) - factA;
295         btVector3 ax1, p, q;
296         btVector3 ax1A = trA.getBasis().getColumn(0);
297         btVector3 ax1B = trB.getBasis().getColumn(0);
298         if(m_useOffsetForConstraintFrame)
299         {
300                 // get the desired direction of slider axis
301                 // as weighted sum of X-orthos of frameA and frameB in WCS
302                 ax1 = ax1A * factA + ax1B * factB;
303                 ax1.normalize();
304                 // construct two orthos to slider axis
305                 btPlaneSpace1 (ax1, p, q);
306         }
307         else
308         { // old way - use frameA
309                 ax1 = trA.getBasis().getColumn(0);
310                 // get 2 orthos to slider axis (Y, Z)
311                 p = trA.getBasis().getColumn(1);
312                 q = trA.getBasis().getColumn(2);
313         }
314         // make rotations around these orthos equal
315         // the slider axis should be the only unconstrained
316         // rotational axis, the angular velocity of the two bodies perpendicular to
317         // the slider axis should be equal. thus the constraint equations are
318         //    p*w1 - p*w2 = 0
319         //    q*w1 - q*w2 = 0
320         // where p and q are unit vectors normal to the slider axis, and w1 and w2
321         // are the angular velocity vectors of the two bodies.
322         info->m_J1angularAxis[0] = p[0];
323         info->m_J1angularAxis[1] = p[1];
324         info->m_J1angularAxis[2] = p[2];
325         info->m_J1angularAxis[s+0] = q[0];
326         info->m_J1angularAxis[s+1] = q[1];
327         info->m_J1angularAxis[s+2] = q[2];
328
329         info->m_J2angularAxis[0] = -p[0];
330         info->m_J2angularAxis[1] = -p[1];
331         info->m_J2angularAxis[2] = -p[2];
332         info->m_J2angularAxis[s+0] = -q[0];
333         info->m_J2angularAxis[s+1] = -q[1];
334         info->m_J2angularAxis[s+2] = -q[2];
335         // compute the right hand side of the constraint equation. set relative
336         // body velocities along p and q to bring the slider back into alignment.
337         // if ax1A,ax1B are the unit length slider axes as computed from bodyA and
338         // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
339         // if "theta" is the angle between ax1 and ax2, we need an angular velocity
340         // along u to cover angle erp*theta in one step :
341         //   |angular_velocity| = angle/time = erp*theta / stepsize
342         //                      = (erp*fps) * theta
343         //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
344         //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
345         // ...as ax1 and ax2 are unit length. if theta is smallish,
346         // theta ~= sin(theta), so
347         //    angular_velocity  = (erp*fps) * (ax1 x ax2)
348         // ax1 x ax2 is in the plane space of ax1, so we project the angular
349         // velocity to p and q to find the right hand side.
350 //      btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
351         btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
352         btScalar k = info->fps * currERP;
353
354         btVector3 u = ax1A.cross(ax1B);
355         info->m_constraintError[0] = k * u.dot(p);
356         info->m_constraintError[s] = k * u.dot(q);
357         if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
358         {
359                 info->cfm[0] = m_cfmOrthoAng;
360                 info->cfm[s] = m_cfmOrthoAng;
361         }
362
363         int nrow = 1; // last filled row
364         int srow;
365         btScalar limit_err;
366         int limit;
367         int powered;
368
369         // next two rows. 
370         // we want: velA + wA x relA == velB + wB x relB ... but this would
371         // result in three equations, so we project along two orthos to the slider axis
372
373         btTransform bodyA_trans = transA;
374         btTransform bodyB_trans = transB;
375         nrow++;
376         int s2 = nrow * s;
377         nrow++;
378         int s3 = nrow * s;
379         btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
380         if(m_useOffsetForConstraintFrame)
381         {
382                 // get vector from bodyB to frameB in WCS
383                 relB = trB.getOrigin() - bodyB_trans.getOrigin();
384                 // get its projection to slider axis
385                 btVector3 projB = ax1 * relB.dot(ax1);
386                 // get vector directed from bodyB to slider axis (and orthogonal to it)
387                 btVector3 orthoB = relB - projB;
388                 // same for bodyA
389                 relA = trA.getOrigin() - bodyA_trans.getOrigin();
390                 btVector3 projA = ax1 * relA.dot(ax1);
391                 btVector3 orthoA = relA - projA;
392                 // get desired offset between frames A and B along slider axis
393                 btScalar sliderOffs = m_linPos - m_depth[0];
394                 // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
395                 btVector3 totalDist = projA + ax1 * sliderOffs - projB;
396                 // get offset vectors relA and relB
397                 relA = orthoA + totalDist * factA;
398                 relB = orthoB - totalDist * factB;
399                 // now choose average ortho to slider axis
400                 p = orthoB * factA + orthoA * factB;
401                 btScalar len2 = p.length2();
402                 if(len2 > SIMD_EPSILON)
403                 {
404                         p /= btSqrt(len2);
405                 }
406                 else
407                 {
408                         p = trA.getBasis().getColumn(1);
409                 }
410                 // make one more ortho
411                 q = ax1.cross(p);
412                 // fill two rows
413                 tmpA = relA.cross(p);
414                 tmpB = relB.cross(p);
415                 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
416                 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
417                 tmpA = relA.cross(q);
418                 tmpB = relB.cross(q);
419                 if(hasStaticBody && getSolveAngLimit())
420                 { // to make constraint between static and dynamic objects more rigid
421                         // remove wA (or wB) from equation if angular limit is hit
422                         tmpB *= factB;
423                         tmpA *= factA;
424                 }
425                 for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
426                 for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
427                 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
428                 for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
429                 for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
430                 for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
431         }
432         else
433         {       // old way - maybe incorrect if bodies are not on the slider axis
434                 // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
435                 c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
436                 btVector3 tmp = c.cross(p);
437                 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
438                 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
439                 tmp = c.cross(q);
440                 for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
441                 for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
442
443                 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
444                 for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
445                 for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
446                 for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
447         }
448         // compute two elements of right hand side
449
450         //      k = info->fps * info->erp * getSoftnessOrthoLin();
451         currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
452         k = info->fps * currERP;
453
454         btScalar rhs = k * p.dot(ofs);
455         info->m_constraintError[s2] = rhs;
456         rhs = k * q.dot(ofs);
457         info->m_constraintError[s3] = rhs;
458         if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
459         {
460                 info->cfm[s2] = m_cfmOrthoLin;
461                 info->cfm[s3] = m_cfmOrthoLin;
462         }
463
464
465         // check linear limits
466         limit_err = btScalar(0.0);
467         limit = 0;
468         if(getSolveLinLimit())
469         {
470                 limit_err = getLinDepth() *  signFact;
471                 limit = (limit_err > btScalar(0.0)) ? 2 : 1;
472         }
473         powered = 0;
474         if(getPoweredLinMotor())
475         {
476                 powered = 1;
477         }
478         // if the slider has joint limits or motor, add in the extra row
479         if (limit || powered) 
480         {
481                 nrow++;
482                 srow = nrow * info->rowskip;
483                 info->m_J1linearAxis[srow+0] = ax1[0];
484                 info->m_J1linearAxis[srow+1] = ax1[1];
485                 info->m_J1linearAxis[srow+2] = ax1[2];
486                 info->m_J2linearAxis[srow+0] = -ax1[0];
487                 info->m_J2linearAxis[srow+1] = -ax1[1];
488                 info->m_J2linearAxis[srow+2] = -ax1[2];
489                 // linear torque decoupling step:
490                 //
491                 // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
492                 // do not create a torque couple. in other words, the points that the
493                 // constraint force is applied at must lie along the same ax1 axis.
494                 // a torque couple will result in limited slider-jointed free
495                 // bodies from gaining angular momentum.
496                 if(m_useOffsetForConstraintFrame)
497                 {
498                         // this is needed only when bodyA and bodyB are both dynamic.
499                         if(!hasStaticBody)
500                         {
501                                 tmpA = relA.cross(ax1);
502                                 tmpB = relB.cross(ax1);
503                                 info->m_J1angularAxis[srow+0] = tmpA[0];
504                                 info->m_J1angularAxis[srow+1] = tmpA[1];
505                                 info->m_J1angularAxis[srow+2] = tmpA[2];
506                                 info->m_J2angularAxis[srow+0] = -tmpB[0];
507                                 info->m_J2angularAxis[srow+1] = -tmpB[1];
508                                 info->m_J2angularAxis[srow+2] = -tmpB[2];
509                         }
510                 }
511                 else
512                 { // The old way. May be incorrect if bodies are not on the slider axis
513                         btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
514                         ltd = c.cross(ax1);
515                         info->m_J1angularAxis[srow+0] = factA*ltd[0];
516                         info->m_J1angularAxis[srow+1] = factA*ltd[1];
517                         info->m_J1angularAxis[srow+2] = factA*ltd[2];
518                         info->m_J2angularAxis[srow+0] = factB*ltd[0];
519                         info->m_J2angularAxis[srow+1] = factB*ltd[1];
520                         info->m_J2angularAxis[srow+2] = factB*ltd[2];
521                 }
522                 // right-hand part
523                 btScalar lostop = getLowerLinLimit();
524                 btScalar histop = getUpperLinLimit();
525                 if(limit && (lostop == histop))
526                 {  // the joint motor is ineffective
527                         powered = 0;
528                 }
529                 info->m_constraintError[srow] = 0.;
530                 info->m_lowerLimit[srow] = 0.;
531                 info->m_upperLimit[srow] = 0.;
532                 currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
533                 if(powered)
534                 {
535                         if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
536                         {
537                                 info->cfm[srow] = m_cfmDirLin;
538                         }
539                         btScalar tag_vel = getTargetLinMotorVelocity();
540                         btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
541                         info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
542                         info->m_lowerLimit[srow] += -getMaxLinMotorForce() / info->fps;
543                         info->m_upperLimit[srow] += getMaxLinMotorForce() / info->fps;
544                 }
545                 if(limit)
546                 {
547                         k = info->fps * currERP;
548                         info->m_constraintError[srow] += k * limit_err;
549                         if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
550                         {
551                                 info->cfm[srow] = m_cfmLimLin;
552                         }
553                         if(lostop == histop) 
554                         {       // limited low and high simultaneously
555                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
556                                 info->m_upperLimit[srow] = SIMD_INFINITY;
557                         }
558                         else if(limit == 1) 
559                         { // low limit
560                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
561                                 info->m_upperLimit[srow] = 0;
562                         }
563                         else 
564                         { // high limit
565                                 info->m_lowerLimit[srow] = 0;
566                                 info->m_upperLimit[srow] = SIMD_INFINITY;
567                         }
568                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
569                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
570                         if(bounce > btScalar(0.0))
571                         {
572                                 btScalar vel = linVelA.dot(ax1);
573                                 vel -= linVelB.dot(ax1);
574                                 vel *= signFact;
575                                 // only apply bounce if the velocity is incoming, and if the
576                                 // resulting c[] exceeds what we already have.
577                                 if(limit == 1)
578                                 {       // low limit
579                                         if(vel < 0)
580                                         {
581                                                 btScalar newc = -bounce * vel;
582                                                 if (newc > info->m_constraintError[srow])
583                                                 {
584                                                         info->m_constraintError[srow] = newc;
585                                                 }
586                                         }
587                                 }
588                                 else
589                                 { // high limit - all those computations are reversed
590                                         if(vel > 0)
591                                         {
592                                                 btScalar newc = -bounce * vel;
593                                                 if(newc < info->m_constraintError[srow]) 
594                                                 {
595                                                         info->m_constraintError[srow] = newc;
596                                                 }
597                                         }
598                                 }
599                         }
600                         info->m_constraintError[srow] *= getSoftnessLimLin();
601                 } // if(limit)
602         } // if linear limit
603         // check angular limits
604         limit_err = btScalar(0.0);
605         limit = 0;
606         if(getSolveAngLimit())
607         {
608                 limit_err = getAngDepth();
609                 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
610         }
611         // if the slider has joint limits, add in the extra row
612         powered = 0;
613         if(getPoweredAngMotor())
614         {
615                 powered = 1;
616         }
617         if(limit || powered) 
618         {
619                 nrow++;
620                 srow = nrow * info->rowskip;
621                 info->m_J1angularAxis[srow+0] = ax1[0];
622                 info->m_J1angularAxis[srow+1] = ax1[1];
623                 info->m_J1angularAxis[srow+2] = ax1[2];
624
625                 info->m_J2angularAxis[srow+0] = -ax1[0];
626                 info->m_J2angularAxis[srow+1] = -ax1[1];
627                 info->m_J2angularAxis[srow+2] = -ax1[2];
628
629                 btScalar lostop = getLowerAngLimit();
630                 btScalar histop = getUpperAngLimit();
631                 if(limit && (lostop == histop))
632                 {  // the joint motor is ineffective
633                         powered = 0;
634                 }
635                 currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
636                 if(powered)
637                 {
638                         if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
639                         {
640                                 info->cfm[srow] = m_cfmDirAng;
641                         }
642                         btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
643                         info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
644                         info->m_lowerLimit[srow] = -getMaxAngMotorForce() / info->fps;
645                         info->m_upperLimit[srow] = getMaxAngMotorForce() / info->fps;
646                 }
647                 if(limit)
648                 {
649                         k = info->fps * currERP;
650                         info->m_constraintError[srow] += k * limit_err;
651                         if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
652                         {
653                                 info->cfm[srow] = m_cfmLimAng;
654                         }
655                         if(lostop == histop) 
656                         {
657                                 // limited low and high simultaneously
658                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
659                                 info->m_upperLimit[srow] = SIMD_INFINITY;
660                         }
661                         else if(limit == 1) 
662                         { // low limit
663                                 info->m_lowerLimit[srow] = 0;
664                                 info->m_upperLimit[srow] = SIMD_INFINITY;
665                         }
666                         else 
667                         { // high limit
668                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
669                                 info->m_upperLimit[srow] = 0;
670                         }
671                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
672                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
673                         if(bounce > btScalar(0.0))
674                         {
675                                 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
676                                 vel -= m_rbB.getAngularVelocity().dot(ax1);
677                                 // only apply bounce if the velocity is incoming, and if the
678                                 // resulting c[] exceeds what we already have.
679                                 if(limit == 1)
680                                 {       // low limit
681                                         if(vel < 0)
682                                         {
683                                                 btScalar newc = -bounce * vel;
684                                                 if(newc > info->m_constraintError[srow])
685                                                 {
686                                                         info->m_constraintError[srow] = newc;
687                                                 }
688                                         }
689                                 }
690                                 else
691                                 {       // high limit - all those computations are reversed
692                                         if(vel > 0)
693                                         {
694                                                 btScalar newc = -bounce * vel;
695                                                 if(newc < info->m_constraintError[srow])
696                                                 {
697                                                         info->m_constraintError[srow] = newc;
698                                                 }
699                                         }
700                                 }
701                         }
702                         info->m_constraintError[srow] *= getSoftnessLimAng();
703                 } // if(limit)
704         } // if angular limit or powered
705 }
706
707
708 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
709 ///If no axis is provided, it uses the default axis for this constraint.
710 void btSliderConstraint::setParam(int num, btScalar value, int axis)
711 {
712         switch(num)
713         {
714         case BT_CONSTRAINT_STOP_ERP :
715                 if(axis < 1)
716                 {
717                         m_softnessLimLin = value;
718                         m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
719                 }
720                 else if(axis < 3)
721                 {
722                         m_softnessOrthoLin = value;
723                         m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
724                 }
725                 else if(axis == 3)
726                 {
727                         m_softnessLimAng = value;
728                         m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
729                 }
730                 else if(axis < 6)
731                 {
732                         m_softnessOrthoAng = value;
733                         m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
734                 }
735                 else
736                 {
737                         btAssertConstrParams(0);
738                 }
739                 break;
740         case BT_CONSTRAINT_CFM :
741                 if(axis < 1)
742                 {
743                         m_cfmDirLin = value;
744                         m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
745                 }
746                 else if(axis == 3)
747                 {
748                         m_cfmDirAng = value;
749                         m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
750                 }
751                 else
752                 {
753                         btAssertConstrParams(0);
754                 }
755                 break;
756         case BT_CONSTRAINT_STOP_CFM :
757                 if(axis < 1)
758                 {
759                         m_cfmLimLin = value;
760                         m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
761                 }
762                 else if(axis < 3)
763                 {
764                         m_cfmOrthoLin = value;
765                         m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
766                 }
767                 else if(axis == 3)
768                 {
769                         m_cfmLimAng = value;
770                         m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
771                 }
772                 else if(axis < 6)
773                 {
774                         m_cfmOrthoAng = value;
775                         m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
776                 }
777                 else
778                 {
779                         btAssertConstrParams(0);
780                 }
781                 break;
782         }
783 }
784
785 ///return the local value of parameter
786 btScalar btSliderConstraint::getParam(int num, int axis) const 
787 {
788         btScalar retVal(SIMD_INFINITY);
789         switch(num)
790         {
791         case BT_CONSTRAINT_STOP_ERP :
792                 if(axis < 1)
793                 {
794                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
795                         retVal = m_softnessLimLin;
796                 }
797                 else if(axis < 3)
798                 {
799                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
800                         retVal = m_softnessOrthoLin;
801                 }
802                 else if(axis == 3)
803                 {
804                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
805                         retVal = m_softnessLimAng;
806                 }
807                 else if(axis < 6)
808                 {
809                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
810                         retVal = m_softnessOrthoAng;
811                 }
812                 else
813                 {
814                         btAssertConstrParams(0);
815                 }
816                 break;
817         case BT_CONSTRAINT_CFM :
818                 if(axis < 1)
819                 {
820                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
821                         retVal = m_cfmDirLin;
822                 }
823                 else if(axis == 3)
824                 {
825                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
826                         retVal = m_cfmDirAng;
827                 }
828                 else
829                 {
830                         btAssertConstrParams(0);
831                 }
832                 break;
833         case BT_CONSTRAINT_STOP_CFM :
834                 if(axis < 1)
835                 {
836                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
837                         retVal = m_cfmLimLin;
838                 }
839                 else if(axis < 3)
840                 {
841                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
842                         retVal = m_cfmOrthoLin;
843                 }
844                 else if(axis == 3)
845                 {
846                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
847                         retVal = m_cfmLimAng;
848                 }
849                 else if(axis < 6)
850                 {
851                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
852                         retVal = m_cfmOrthoAng;
853                 }
854                 else
855                 {
856                         btAssertConstrParams(0);
857                 }
858                 break;
859         }
860         return retVal;
861 }
862
863
864