00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
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;
00123 info->nub = 2;
00124
00125 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00126 testAngLimits();
00127 testLinLimits();
00128 if(getSolveLinLimit() || getPoweredLinMotor())
00129 {
00130 info->m_numConstraintRows++;
00131 info->nub--;
00132 }
00133 if(getSolveAngLimit() || getPoweredAngMotor())
00134 {
00135 info->m_numConstraintRows++;
00136 info->nub--;
00137 }
00138 }
00139 }
00140
00141 void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
00142 {
00143
00144 info->m_numConstraintRows = 6;
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);
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
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
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
00279 btVector3 ofs = trB.getOrigin() - trA.getOrigin();
00280
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
00301
00302 ax1 = ax1A * factA + ax1B * factB;
00303 ax1.normalize();
00304
00305 btPlaneSpace1 (ax1, p, q);
00306 }
00307 else
00308 {
00309 ax1 = trA.getBasis().getColumn(0);
00310
00311 p = trA.getBasis().getColumn(1);
00312 q = trA.getBasis().getColumn(2);
00313 }
00314
00315
00316
00317
00318
00319
00320
00321
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
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
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;
00364 int srow;
00365 btScalar limit_err;
00366 int limit;
00367 int powered;
00368
00369
00370
00371
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
00383 relB = trB.getOrigin() - bodyB_trans.getOrigin();
00384
00385 btVector3 projB = ax1 * relB.dot(ax1);
00386
00387 btVector3 orthoB = relB - projB;
00388
00389 relA = trA.getOrigin() - bodyA_trans.getOrigin();
00390 btVector3 projA = ax1 * relA.dot(ax1);
00391 btVector3 orthoA = relA - projA;
00392
00393 btScalar sliderOffs = m_linPos - m_depth[0];
00394
00395 btVector3 totalDist = projA + ax1 * sliderOffs - projB;
00396
00397 relA = orthoA + totalDist * factA;
00398 relB = orthoB - totalDist * factB;
00399
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
00411 q = ax1.cross(p);
00412
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 {
00421
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 {
00432
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
00445
00446
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
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
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
00483
00484
00485
00486
00487
00488
00489 if(m_useOffsetForConstraintFrame)
00490 {
00491
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 {
00506 btVector3 ltd;
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
00516 btScalar lostop = getLowerLinLimit();
00517 btScalar histop = getUpperLinLimit();
00518 if(limit && (lostop == histop))
00519 {
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 {
00548 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00549 info->m_upperLimit[srow] = SIMD_INFINITY;
00550 }
00551 else if(limit == 1)
00552 {
00553 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00554 info->m_upperLimit[srow] = 0;
00555 }
00556 else
00557 {
00558 info->m_lowerLimit[srow] = 0;
00559 info->m_upperLimit[srow] = SIMD_INFINITY;
00560 }
00561
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
00569
00570 if(limit == 1)
00571 {
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 {
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 }
00595 }
00596
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
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 {
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
00651 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00652 info->m_upperLimit[srow] = SIMD_INFINITY;
00653 }
00654 else if(limit == 1)
00655 {
00656 info->m_lowerLimit[srow] = 0;
00657 info->m_upperLimit[srow] = SIMD_INFINITY;
00658 }
00659 else
00660 {
00661 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00662 info->m_upperLimit[srow] = 0;
00663 }
00664
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
00671
00672 if(limit == 1)
00673 {
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 {
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 }
00697 }
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