btSliderConstraint.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose,
00008 including commercial applications, and to alter it and redistribute it freely,
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 /*
00017 Added by Roman Ponomarev (rponom@gmail.com)
00018 April 04, 2008
00019 */
00020 
00021 
00022 
00023 #include "btSliderConstraint.h"
00024 #include "BulletDynamics/Dynamics/btRigidBody.h"
00025 #include "LinearMath/btTransformUtil.h"
00026 #include <new>
00027 
00028 #define USE_OFFSET_FOR_CONSTANT_FRAME true
00029 
00030 void btSliderConstraint::initParams()
00031 {
00032     m_lowerLinLimit = btScalar(1.0);
00033     m_upperLinLimit = btScalar(-1.0);
00034     m_lowerAngLimit = btScalar(0.);
00035     m_upperAngLimit = btScalar(0.);
00036         m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00037         m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00038         m_dampingDirLin = btScalar(0.);
00039         m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM;
00040         m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00041         m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00042         m_dampingDirAng = btScalar(0.);
00043         m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM;
00044         m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00045         m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00046         m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
00047         m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM;
00048         m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00049         m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00050         m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
00051         m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM;
00052         m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00053         m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00054         m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
00055         m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM;
00056         m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00057         m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00058         m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
00059         m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
00060 
00061         m_poweredLinMotor = false;
00062     m_targetLinMotorVelocity = btScalar(0.);
00063     m_maxLinMotorForce = btScalar(0.);
00064         m_accumulatedLinMotorImpulse = btScalar(0.0);
00065 
00066         m_poweredAngMotor = false;
00067     m_targetAngMotorVelocity = btScalar(0.);
00068     m_maxAngMotorForce = btScalar(0.);
00069         m_accumulatedAngMotorImpulse = btScalar(0.0);
00070 
00071         m_flags = 0;
00072         m_flags = 0;
00073 
00074         m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
00075 
00076         calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00077 }
00078 
00079 
00080 
00081 
00082 
00083 btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
00084         : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
00085                 m_useSolveConstraintObsolete(false),
00086                 m_frameInA(frameInA),
00087         m_frameInB(frameInB),
00088                 m_useLinearReferenceFrameA(useLinearReferenceFrameA)
00089 {
00090         initParams();
00091 }
00092 
00093 
00094 
00095 btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
00096         : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
00097                 m_useSolveConstraintObsolete(false),
00098                 m_frameInB(frameInB),
00099                 m_useLinearReferenceFrameA(useLinearReferenceFrameA)
00100 {
00102         m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
00103 //      m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
00104 
00105         initParams();
00106 }
00107 
00108 
00109 
00110 
00111 
00112 
00113 void btSliderConstraint::getInfo1(btConstraintInfo1* info)
00114 {
00115         if (m_useSolveConstraintObsolete)
00116         {
00117                 info->m_numConstraintRows = 0;
00118                 info->nub = 0;
00119         }
00120         else
00121         {
00122                 info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
00123                 info->nub = 2; 
00124                 //prepare constraint
00125                 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00126                 testAngLimits();
00127                 testLinLimits();
00128                 if(getSolveLinLimit() || getPoweredLinMotor())
00129                 {
00130                         info->m_numConstraintRows++; // limit 3rd linear as well
00131                         info->nub--; 
00132                 }
00133                 if(getSolveAngLimit() || getPoweredAngMotor())
00134                 {
00135                         info->m_numConstraintRows++; // limit 3rd angular as well
00136                         info->nub--; 
00137                 }
00138         }
00139 }
00140 
00141 void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
00142 {
00143 
00144         info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
00145         info->nub = 0; 
00146 }
00147 
00148 void btSliderConstraint::getInfo2(btConstraintInfo2* info)
00149 {
00150         getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
00151 }
00152 
00153 
00154 
00155 
00156 
00157 
00158 
00159 void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
00160 {
00161         if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
00162         {
00163                 m_calculatedTransformA = transA * m_frameInA;
00164                 m_calculatedTransformB = transB * m_frameInB;
00165         }
00166         else
00167         {
00168                 m_calculatedTransformA = transB * m_frameInB;
00169                 m_calculatedTransformB = transA * m_frameInA;
00170         }
00171         m_realPivotAInW = m_calculatedTransformA.getOrigin();
00172         m_realPivotBInW = m_calculatedTransformB.getOrigin();
00173         m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
00174         if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
00175         {
00176                 m_delta = m_realPivotBInW - m_realPivotAInW;
00177         }
00178         else
00179         {
00180                 m_delta = m_realPivotAInW - m_realPivotBInW;
00181         }
00182         m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
00183     btVector3 normalWorld;
00184     int i;
00185     //linear part
00186     for(i = 0; i < 3; i++)
00187     {
00188                 normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
00189                 m_depth[i] = m_delta.dot(normalWorld);
00190     }
00191 }
00192  
00193 
00194 
00195 void btSliderConstraint::testLinLimits(void)
00196 {
00197         m_solveLinLim = false;
00198         m_linPos = m_depth[0];
00199         if(m_lowerLinLimit <= m_upperLinLimit)
00200         {
00201                 if(m_depth[0] > m_upperLinLimit)
00202                 {
00203                         m_depth[0] -= m_upperLinLimit;
00204                         m_solveLinLim = true;
00205                 }
00206                 else if(m_depth[0] < m_lowerLinLimit)
00207                 {
00208                         m_depth[0] -= m_lowerLinLimit;
00209                         m_solveLinLim = true;
00210                 }
00211                 else
00212                 {
00213                         m_depth[0] = btScalar(0.);
00214                 }
00215         }
00216         else
00217         {
00218                 m_depth[0] = btScalar(0.);
00219         }
00220 }
00221 
00222 
00223 
00224 void btSliderConstraint::testAngLimits(void)
00225 {
00226         m_angDepth = btScalar(0.);
00227         m_solveAngLim = false;
00228         if(m_lowerAngLimit <= m_upperAngLimit)
00229         {
00230                 const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
00231                 const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
00232                 const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
00233 //              btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
00234                 btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));  
00235                 rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
00236                 m_angPos = rot;
00237                 if(rot < m_lowerAngLimit)
00238                 {
00239                         m_angDepth = rot - m_lowerAngLimit;
00240                         m_solveAngLim = true;
00241                 } 
00242                 else if(rot > m_upperAngLimit)
00243                 {
00244                         m_angDepth = rot - m_upperAngLimit;
00245                         m_solveAngLim = true;
00246                 }
00247         }
00248 }
00249 
00250 btVector3 btSliderConstraint::getAncorInA(void)
00251 {
00252         btVector3 ancorInA;
00253         ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
00254         ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
00255         return ancorInA;
00256 }
00257 
00258 
00259 
00260 btVector3 btSliderConstraint::getAncorInB(void)
00261 {
00262         btVector3 ancorInB;
00263         ancorInB = m_frameInB.getOrigin();
00264         return ancorInB;
00265 }
00266 
00267 
00268 void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
00269 {
00270         const btTransform& trA = getCalculatedTransformA();
00271         const btTransform& trB = getCalculatedTransformB();
00272         
00273         btAssert(!m_useSolveConstraintObsolete);
00274         int i, s = info->rowskip;
00275         
00276         btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
00277         
00278         // difference between frames in WCS
00279         btVector3 ofs = trB.getOrigin() - trA.getOrigin();
00280         // now get weight factors depending on masses
00281         btScalar miA = rbAinvMass;
00282         btScalar miB = rbBinvMass;
00283         bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
00284         btScalar miS = miA + miB;
00285         btScalar factA, factB;
00286         if(miS > btScalar(0.f))
00287         {
00288                 factA = miB / miS;
00289         }
00290         else 
00291         {
00292                 factA = btScalar(0.5f);
00293         }
00294         factB = btScalar(1.0f) - factA;
00295         btVector3 ax1, p, q;
00296         btVector3 ax1A = trA.getBasis().getColumn(0);
00297         btVector3 ax1B = trB.getBasis().getColumn(0);
00298         if(m_useOffsetForConstraintFrame)
00299         {
00300                 // get the desired direction of slider axis
00301                 // as weighted sum of X-orthos of frameA and frameB in WCS
00302                 ax1 = ax1A * factA + ax1B * factB;
00303                 ax1.normalize();
00304                 // construct two orthos to slider axis
00305                 btPlaneSpace1 (ax1, p, q);
00306         }
00307         else
00308         { // old way - use frameA
00309                 ax1 = trA.getBasis().getColumn(0);
00310                 // get 2 orthos to slider axis (Y, Z)
00311                 p = trA.getBasis().getColumn(1);
00312                 q = trA.getBasis().getColumn(2);
00313         }
00314         // make rotations around these orthos equal
00315         // the slider axis should be the only unconstrained
00316         // rotational axis, the angular velocity of the two bodies perpendicular to
00317         // the slider axis should be equal. thus the constraint equations are
00318         //    p*w1 - p*w2 = 0
00319         //    q*w1 - q*w2 = 0
00320         // where p and q are unit vectors normal to the slider axis, and w1 and w2
00321         // are the angular velocity vectors of the two bodies.
00322         info->m_J1angularAxis[0] = p[0];
00323         info->m_J1angularAxis[1] = p[1];
00324         info->m_J1angularAxis[2] = p[2];
00325         info->m_J1angularAxis[s+0] = q[0];
00326         info->m_J1angularAxis[s+1] = q[1];
00327         info->m_J1angularAxis[s+2] = q[2];
00328 
00329         info->m_J2angularAxis[0] = -p[0];
00330         info->m_J2angularAxis[1] = -p[1];
00331         info->m_J2angularAxis[2] = -p[2];
00332         info->m_J2angularAxis[s+0] = -q[0];
00333         info->m_J2angularAxis[s+1] = -q[1];
00334         info->m_J2angularAxis[s+2] = -q[2];
00335         // compute the right hand side of the constraint equation. set relative
00336         // body velocities along p and q to bring the slider back into alignment.
00337         // if ax1A,ax1B are the unit length slider axes as computed from bodyA and
00338         // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
00339         // if "theta" is the angle between ax1 and ax2, we need an angular velocity
00340         // along u to cover angle erp*theta in one step :
00341         //   |angular_velocity| = angle/time = erp*theta / stepsize
00342         //                      = (erp*fps) * theta
00343         //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
00344         //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
00345         // ...as ax1 and ax2 are unit length. if theta is smallish,
00346         // theta ~= sin(theta), so
00347         //    angular_velocity  = (erp*fps) * (ax1 x ax2)
00348         // ax1 x ax2 is in the plane space of ax1, so we project the angular
00349         // velocity to p and q to find the right hand side.
00350 //      btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
00351         btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
00352         btScalar k = info->fps * currERP;
00353 
00354         btVector3 u = ax1A.cross(ax1B);
00355         info->m_constraintError[0] = k * u.dot(p);
00356         info->m_constraintError[s] = k * u.dot(q);
00357         if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
00358         {
00359                 info->cfm[0] = m_cfmOrthoAng;
00360                 info->cfm[s] = m_cfmOrthoAng;
00361         }
00362 
00363         int nrow = 1; // last filled row
00364         int srow;
00365         btScalar limit_err;
00366         int limit;
00367         int powered;
00368 
00369         // next two rows. 
00370         // we want: velA + wA x relA == velB + wB x relB ... but this would
00371         // result in three equations, so we project along two orthos to the slider axis
00372 
00373         btTransform bodyA_trans = transA;
00374         btTransform bodyB_trans = transB;
00375         nrow++;
00376         int s2 = nrow * s;
00377         nrow++;
00378         int s3 = nrow * s;
00379         btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
00380         if(m_useOffsetForConstraintFrame)
00381         {
00382                 // get vector from bodyB to frameB in WCS
00383                 relB = trB.getOrigin() - bodyB_trans.getOrigin();
00384                 // get its projection to slider axis
00385                 btVector3 projB = ax1 * relB.dot(ax1);
00386                 // get vector directed from bodyB to slider axis (and orthogonal to it)
00387                 btVector3 orthoB = relB - projB;
00388                 // same for bodyA
00389                 relA = trA.getOrigin() - bodyA_trans.getOrigin();
00390                 btVector3 projA = ax1 * relA.dot(ax1);
00391                 btVector3 orthoA = relA - projA;
00392                 // get desired offset between frames A and B along slider axis
00393                 btScalar sliderOffs = m_linPos - m_depth[0];
00394                 // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
00395                 btVector3 totalDist = projA + ax1 * sliderOffs - projB;
00396                 // get offset vectors relA and relB
00397                 relA = orthoA + totalDist * factA;
00398                 relB = orthoB - totalDist * factB;
00399                 // now choose average ortho to slider axis
00400                 p = orthoB * factA + orthoA * factB;
00401                 btScalar len2 = p.length2();
00402                 if(len2 > SIMD_EPSILON)
00403                 {
00404                         p /= btSqrt(len2);
00405                 }
00406                 else
00407                 {
00408                         p = trA.getBasis().getColumn(1);
00409                 }
00410                 // make one more ortho
00411                 q = ax1.cross(p);
00412                 // fill two rows
00413                 tmpA = relA.cross(p);
00414                 tmpB = relB.cross(p);
00415                 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
00416                 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
00417                 tmpA = relA.cross(q);
00418                 tmpB = relB.cross(q);
00419                 if(hasStaticBody && getSolveAngLimit())
00420                 { // to make constraint between static and dynamic objects more rigid
00421                         // remove wA (or wB) from equation if angular limit is hit
00422                         tmpB *= factB;
00423                         tmpA *= factA;
00424                 }
00425                 for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
00426                 for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
00427                 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
00428                 for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
00429         }
00430         else
00431         {       // old way - maybe incorrect if bodies are not on the slider axis
00432                 // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
00433                 c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
00434                 btVector3 tmp = c.cross(p);
00435                 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
00436                 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
00437                 tmp = c.cross(q);
00438                 for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
00439                 for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
00440 
00441                 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
00442                 for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
00443         }
00444         // compute two elements of right hand side
00445 
00446         //      k = info->fps * info->erp * getSoftnessOrthoLin();
00447         currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
00448         k = info->fps * currERP;
00449 
00450         btScalar rhs = k * p.dot(ofs);
00451         info->m_constraintError[s2] = rhs;
00452         rhs = k * q.dot(ofs);
00453         info->m_constraintError[s3] = rhs;
00454         if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
00455         {
00456                 info->cfm[s2] = m_cfmOrthoLin;
00457                 info->cfm[s3] = m_cfmOrthoLin;
00458         }
00459 
00460 
00461         // check linear limits
00462         limit_err = btScalar(0.0);
00463         limit = 0;
00464         if(getSolveLinLimit())
00465         {
00466                 limit_err = getLinDepth() *  signFact;
00467                 limit = (limit_err > btScalar(0.0)) ? 2 : 1;
00468         }
00469         powered = 0;
00470         if(getPoweredLinMotor())
00471         {
00472                 powered = 1;
00473         }
00474         // if the slider has joint limits or motor, add in the extra row
00475         if (limit || powered) 
00476         {
00477                 nrow++;
00478                 srow = nrow * info->rowskip;
00479                 info->m_J1linearAxis[srow+0] = ax1[0];
00480                 info->m_J1linearAxis[srow+1] = ax1[1];
00481                 info->m_J1linearAxis[srow+2] = ax1[2];
00482                 // linear torque decoupling step:
00483                 //
00484                 // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
00485                 // do not create a torque couple. in other words, the points that the
00486                 // constraint force is applied at must lie along the same ax1 axis.
00487                 // a torque couple will result in limited slider-jointed free
00488                 // bodies from gaining angular momentum.
00489                 if(m_useOffsetForConstraintFrame)
00490                 {
00491                         // this is needed only when bodyA and bodyB are both dynamic.
00492                         if(!hasStaticBody)
00493                         {
00494                                 tmpA = relA.cross(ax1);
00495                                 tmpB = relB.cross(ax1);
00496                                 info->m_J1angularAxis[srow+0] = tmpA[0];
00497                                 info->m_J1angularAxis[srow+1] = tmpA[1];
00498                                 info->m_J1angularAxis[srow+2] = tmpA[2];
00499                                 info->m_J2angularAxis[srow+0] = -tmpB[0];
00500                                 info->m_J2angularAxis[srow+1] = -tmpB[1];
00501                                 info->m_J2angularAxis[srow+2] = -tmpB[2];
00502                         }
00503                 }
00504                 else
00505                 { // The old way. May be incorrect if bodies are not on the slider axis
00506                         btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
00507                         ltd = c.cross(ax1);
00508                         info->m_J1angularAxis[srow+0] = factA*ltd[0];
00509                         info->m_J1angularAxis[srow+1] = factA*ltd[1];
00510                         info->m_J1angularAxis[srow+2] = factA*ltd[2];
00511                         info->m_J2angularAxis[srow+0] = factB*ltd[0];
00512                         info->m_J2angularAxis[srow+1] = factB*ltd[1];
00513                         info->m_J2angularAxis[srow+2] = factB*ltd[2];
00514                 }
00515                 // right-hand part
00516                 btScalar lostop = getLowerLinLimit();
00517                 btScalar histop = getUpperLinLimit();
00518                 if(limit && (lostop == histop))
00519                 {  // the joint motor is ineffective
00520                         powered = 0;
00521                 }
00522                 info->m_constraintError[srow] = 0.;
00523                 info->m_lowerLimit[srow] = 0.;
00524                 info->m_upperLimit[srow] = 0.;
00525                 currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
00526                 if(powered)
00527                 {
00528                         if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
00529                         {
00530                                 info->cfm[srow] = m_cfmDirLin;
00531                         }
00532                         btScalar tag_vel = getTargetLinMotorVelocity();
00533                         btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
00534                         info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
00535                         info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
00536                         info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
00537                 }
00538                 if(limit)
00539                 {
00540                         k = info->fps * currERP;
00541                         info->m_constraintError[srow] += k * limit_err;
00542                         if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
00543                         {
00544                                 info->cfm[srow] = m_cfmLimLin;
00545                         }
00546                         if(lostop == histop) 
00547                         {       // limited low and high simultaneously
00548                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00549                                 info->m_upperLimit[srow] = SIMD_INFINITY;
00550                         }
00551                         else if(limit == 1) 
00552                         { // low limit
00553                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00554                                 info->m_upperLimit[srow] = 0;
00555                         }
00556                         else 
00557                         { // high limit
00558                                 info->m_lowerLimit[srow] = 0;
00559                                 info->m_upperLimit[srow] = SIMD_INFINITY;
00560                         }
00561                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
00562                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
00563                         if(bounce > btScalar(0.0))
00564                         {
00565                                 btScalar vel = linVelA.dot(ax1);
00566                                 vel -= linVelB.dot(ax1);
00567                                 vel *= signFact;
00568                                 // only apply bounce if the velocity is incoming, and if the
00569                                 // resulting c[] exceeds what we already have.
00570                                 if(limit == 1)
00571                                 {       // low limit
00572                                         if(vel < 0)
00573                                         {
00574                                                 btScalar newc = -bounce * vel;
00575                                                 if (newc > info->m_constraintError[srow])
00576                                                 {
00577                                                         info->m_constraintError[srow] = newc;
00578                                                 }
00579                                         }
00580                                 }
00581                                 else
00582                                 { // high limit - all those computations are reversed
00583                                         if(vel > 0)
00584                                         {
00585                                                 btScalar newc = -bounce * vel;
00586                                                 if(newc < info->m_constraintError[srow]) 
00587                                                 {
00588                                                         info->m_constraintError[srow] = newc;
00589                                                 }
00590                                         }
00591                                 }
00592                         }
00593                         info->m_constraintError[srow] *= getSoftnessLimLin();
00594                 } // if(limit)
00595         } // if linear limit
00596         // check angular limits
00597         limit_err = btScalar(0.0);
00598         limit = 0;
00599         if(getSolveAngLimit())
00600         {
00601                 limit_err = getAngDepth();
00602                 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
00603         }
00604         // if the slider has joint limits, add in the extra row
00605         powered = 0;
00606         if(getPoweredAngMotor())
00607         {
00608                 powered = 1;
00609         }
00610         if(limit || powered) 
00611         {
00612                 nrow++;
00613                 srow = nrow * info->rowskip;
00614                 info->m_J1angularAxis[srow+0] = ax1[0];
00615                 info->m_J1angularAxis[srow+1] = ax1[1];
00616                 info->m_J1angularAxis[srow+2] = ax1[2];
00617 
00618                 info->m_J2angularAxis[srow+0] = -ax1[0];
00619                 info->m_J2angularAxis[srow+1] = -ax1[1];
00620                 info->m_J2angularAxis[srow+2] = -ax1[2];
00621 
00622                 btScalar lostop = getLowerAngLimit();
00623                 btScalar histop = getUpperAngLimit();
00624                 if(limit && (lostop == histop))
00625                 {  // the joint motor is ineffective
00626                         powered = 0;
00627                 }
00628                 currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
00629                 if(powered)
00630                 {
00631                         if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
00632                         {
00633                                 info->cfm[srow] = m_cfmDirAng;
00634                         }
00635                         btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
00636                         info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
00637                         info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
00638                         info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
00639                 }
00640                 if(limit)
00641                 {
00642                         k = info->fps * currERP;
00643                         info->m_constraintError[srow] += k * limit_err;
00644                         if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
00645                         {
00646                                 info->cfm[srow] = m_cfmLimAng;
00647                         }
00648                         if(lostop == histop) 
00649                         {
00650                                 // limited low and high simultaneously
00651                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00652                                 info->m_upperLimit[srow] = SIMD_INFINITY;
00653                         }
00654                         else if(limit == 1) 
00655                         { // low limit
00656                                 info->m_lowerLimit[srow] = 0;
00657                                 info->m_upperLimit[srow] = SIMD_INFINITY;
00658                         }
00659                         else 
00660                         { // high limit
00661                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00662                                 info->m_upperLimit[srow] = 0;
00663                         }
00664                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
00665                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
00666                         if(bounce > btScalar(0.0))
00667                         {
00668                                 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
00669                                 vel -= m_rbB.getAngularVelocity().dot(ax1);
00670                                 // only apply bounce if the velocity is incoming, and if the
00671                                 // resulting c[] exceeds what we already have.
00672                                 if(limit == 1)
00673                                 {       // low limit
00674                                         if(vel < 0)
00675                                         {
00676                                                 btScalar newc = -bounce * vel;
00677                                                 if(newc > info->m_constraintError[srow])
00678                                                 {
00679                                                         info->m_constraintError[srow] = newc;
00680                                                 }
00681                                         }
00682                                 }
00683                                 else
00684                                 {       // high limit - all those computations are reversed
00685                                         if(vel > 0)
00686                                         {
00687                                                 btScalar newc = -bounce * vel;
00688                                                 if(newc < info->m_constraintError[srow])
00689                                                 {
00690                                                         info->m_constraintError[srow] = newc;
00691                                                 }
00692                                         }
00693                                 }
00694                         }
00695                         info->m_constraintError[srow] *= getSoftnessLimAng();
00696                 } // if(limit)
00697         } // if angular limit or powered
00698 }
00699 
00700 
00703 void btSliderConstraint::setParam(int num, btScalar value, int axis)
00704 {
00705         switch(num)
00706         {
00707         case BT_CONSTRAINT_STOP_ERP :
00708                 if(axis < 1)
00709                 {
00710                         m_softnessLimLin = value;
00711                         m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
00712                 }
00713                 else if(axis < 3)
00714                 {
00715                         m_softnessOrthoLin = value;
00716                         m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
00717                 }
00718                 else if(axis == 3)
00719                 {
00720                         m_softnessLimAng = value;
00721                         m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
00722                 }
00723                 else if(axis < 6)
00724                 {
00725                         m_softnessOrthoAng = value;
00726                         m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
00727                 }
00728                 else
00729                 {
00730                         btAssertConstrParams(0);
00731                 }
00732                 break;
00733         case BT_CONSTRAINT_CFM :
00734                 if(axis < 1)
00735                 {
00736                         m_cfmDirLin = value;
00737                         m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
00738                 }
00739                 else if(axis == 3)
00740                 {
00741                         m_cfmDirAng = value;
00742                         m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
00743                 }
00744                 else
00745                 {
00746                         btAssertConstrParams(0);
00747                 }
00748                 break;
00749         case BT_CONSTRAINT_STOP_CFM :
00750                 if(axis < 1)
00751                 {
00752                         m_cfmLimLin = value;
00753                         m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
00754                 }
00755                 else if(axis < 3)
00756                 {
00757                         m_cfmOrthoLin = value;
00758                         m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
00759                 }
00760                 else if(axis == 3)
00761                 {
00762                         m_cfmLimAng = value;
00763                         m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
00764                 }
00765                 else if(axis < 6)
00766                 {
00767                         m_cfmOrthoAng = value;
00768                         m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
00769                 }
00770                 else
00771                 {
00772                         btAssertConstrParams(0);
00773                 }
00774                 break;
00775         }
00776 }
00777 
00779 btScalar btSliderConstraint::getParam(int num, int axis) const 
00780 {
00781         btScalar retVal(SIMD_INFINITY);
00782         switch(num)
00783         {
00784         case BT_CONSTRAINT_STOP_ERP :
00785                 if(axis < 1)
00786                 {
00787                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
00788                         retVal = m_softnessLimLin;
00789                 }
00790                 else if(axis < 3)
00791                 {
00792                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
00793                         retVal = m_softnessOrthoLin;
00794                 }
00795                 else if(axis == 3)
00796                 {
00797                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
00798                         retVal = m_softnessLimAng;
00799                 }
00800                 else if(axis < 6)
00801                 {
00802                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
00803                         retVal = m_softnessOrthoAng;
00804                 }
00805                 else
00806                 {
00807                         btAssertConstrParams(0);
00808                 }
00809                 break;
00810         case BT_CONSTRAINT_CFM :
00811                 if(axis < 1)
00812                 {
00813                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
00814                         retVal = m_cfmDirLin;
00815                 }
00816                 else if(axis == 3)
00817                 {
00818                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
00819                         retVal = m_cfmDirAng;
00820                 }
00821                 else
00822                 {
00823                         btAssertConstrParams(0);
00824                 }
00825                 break;
00826         case BT_CONSTRAINT_STOP_CFM :
00827                 if(axis < 1)
00828                 {
00829                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
00830                         retVal = m_cfmLimLin;
00831                 }
00832                 else if(axis < 3)
00833                 {
00834                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
00835                         retVal = m_cfmOrthoLin;
00836                 }
00837                 else if(axis == 3)
00838                 {
00839                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
00840                         retVal = m_cfmLimAng;
00841                 }
00842                 else if(axis < 6)
00843                 {
00844                         btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
00845                         retVal = m_cfmOrthoAng;
00846                 }
00847                 else
00848                 {
00849                         btAssertConstrParams(0);
00850                 }
00851                 break;
00852         }
00853         return retVal;
00854 }
00855 
00856 
00857