Changeset 8351 for code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
- Timestamp:
- Apr 28, 2011, 7:15:14 AM (13 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
r5781 r8351 13 13 3. This notice may not be removed or altered from any source distribution. 14 14 */ 15 16 /// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev 17 /// Added support for generic constraint solver through getInfo1/getInfo2 methods 18 15 19 /* 16 20 2007-09-09 … … 46 50 btScalar m_damping;//!< Damping. 47 51 btScalar m_limitSoftness;//! Relaxation factor 48 btScalar m_ERP;//!< Error tolerance factor when joint is at limit 52 btScalar m_normalCFM;//!< Constraint force mixing factor 53 btScalar m_stopERP;//!< Error tolerance factor when joint is at limit 54 btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit 49 55 btScalar m_bounce;//!< restitution factor 50 56 bool m_enableMotor; … … 55 61 //!@{ 56 62 btScalar m_currentLimitError;//! How much is violated this limit 63 btScalar m_currentPosition; //! current value of angle 57 64 int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit 58 65 btScalar m_accumulatedImpulse; … … 65 72 m_maxMotorForce = 0.1f; 66 73 m_maxLimitForce = 300.0f; 67 m_loLimit = -SIMD_INFINITY; 68 m_hiLimit = SIMD_INFINITY; 69 m_ERP = 0.5f; 74 m_loLimit = 1.0f; 75 m_hiLimit = -1.0f; 76 m_normalCFM = 0.f; 77 m_stopERP = 0.2f; 78 m_stopCFM = 0.f; 70 79 m_bounce = 0.0f; 71 80 m_damping = 1.0f; … … 83 92 m_loLimit = limot.m_loLimit; 84 93 m_hiLimit = limot.m_hiLimit; 85 m_ERP = limot.m_ERP; 94 m_normalCFM = limot.m_normalCFM; 95 m_stopERP = limot.m_stopERP; 96 m_stopCFM = limot.m_stopCFM; 86 97 m_bounce = limot.m_bounce; 87 98 m_currentLimit = limot.m_currentLimit; … … 113 124 114 125 //! apply the correction impulses for two bodies 115 btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, bt SolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);126 btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); 116 127 117 128 }; … … 130 141 btScalar m_damping;//!< Damping for linear limit 131 142 btScalar m_restitution;//! Bounce parameter for linear limit 143 btVector3 m_normalCFM;//!< Constraint force mixing factor 144 btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit 145 btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit 132 146 //!@} 133 147 bool m_enableMotor[3]; … … 135 149 btVector3 m_maxMotorForce;//!< max force on motor 136 150 btVector3 m_currentLimitError;//! How much is violated this limit 151 btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames 137 152 int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit 138 153 … … 142 157 m_upperLimit.setValue(0.f,0.f,0.f); 143 158 m_accumulatedImpulse.setValue(0.f,0.f,0.f); 159 m_normalCFM.setValue(0.f, 0.f, 0.f); 160 m_stopERP.setValue(0.2f, 0.2f, 0.2f); 161 m_stopCFM.setValue(0.f, 0.f, 0.f); 144 162 145 163 m_limitSoftness = 0.7f; … … 163 181 m_damping = other.m_damping; 164 182 m_restitution = other.m_restitution; 183 m_normalCFM = other.m_normalCFM; 184 m_stopERP = other.m_stopERP; 185 m_stopCFM = other.m_stopCFM; 186 165 187 for(int i=0; i < 3; i++) 166 188 { … … 193 215 btScalar timeStep, 194 216 btScalar jacDiagABInv, 195 btRigidBody& body1, btSolverBody& bodyA,const btVector3 &pointInA,196 btRigidBody& body2, btSolverBody& bodyB,const btVector3 &pointInB,217 btRigidBody& body1,const btVector3 &pointInA, 218 btRigidBody& body2,const btVector3 &pointInB, 197 219 int limit_index, 198 220 const btVector3 & axis_normal_on_a, … … 201 223 202 224 }; 225 226 enum bt6DofFlags 227 { 228 BT_6DOF_FLAGS_CFM_NORM = 1, 229 BT_6DOF_FLAGS_CFM_STOP = 2, 230 BT_6DOF_FLAGS_ERP_STOP = 4 231 }; 232 #define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis 233 203 234 204 235 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space … … 216 247 <li> Angulars limits have these possible ranges: 217 248 <table border=1 > 218 <tr 219 249 <tr> 220 250 <td><b>AXIS</b></td> 221 251 <td><b>MIN ANGLE</b></td> 222 252 <td><b>MAX ANGLE</b></td> 253 </tr><tr> 223 254 <td>X</td> 224 <td>-PI</td> 225 <td>PI</td> 255 <td>-PI</td> 256 <td>PI</td> 257 </tr><tr> 226 258 <td>Y</td> 227 <td>-PI/2</td> 228 <td>PI/2</td> 259 <td>-PI/2</td> 260 <td>PI/2</td> 261 </tr><tr> 229 262 <td>Z</td> 230 <td>-PI/2</td>231 <td>PI/2</td>263 <td>-PI</td> 264 <td>PI</td> 232 265 </tr> 233 266 </table> … … 273 306 btVector3 m_calculatedAxis[3]; 274 307 btVector3 m_calculatedLinearDiff; 308 btScalar m_factA; 309 btScalar m_factB; 310 bool m_hasStaticBody; 275 311 276 312 btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes 277 313 278 314 bool m_useLinearReferenceFrameA; 315 bool m_useOffsetForConstraintFrame; 279 316 317 int m_flags; 318 280 319 //!@} 281 320 … … 288 327 289 328 290 int setAngularLimits(btConstraintInfo2 *info, int row_offset );291 292 int setLinearLimits(btConstraintInfo2 *info );329 int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); 330 331 int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); 293 332 294 333 void buildLinearJacobian( … … 312 351 313 352 btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); 314 315 btGeneric6DofConstraint(); 316 353 btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); 354 317 355 //! Calcs global transform of the offsets 318 356 /*! … … 320 358 \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo 321 359 */ 322 void calculateTransforms(); 360 void calculateTransforms(const btTransform& transA,const btTransform& transB); 361 362 void calculateTransforms(); 323 363 324 364 //! Gets the global transform of the offset for body A … … 367 407 virtual void getInfo1 (btConstraintInfo1* info); 368 408 409 void getInfo1NonVirtual (btConstraintInfo1* info); 410 369 411 virtual void getInfo2 (btConstraintInfo2* info); 370 412 371 virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); 413 void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); 414 372 415 373 416 void updateRHS(btScalar timeStep); … … 381 424 //! Get the relative Euler angle 382 425 /*! 383 \pre btGeneric6DofConstraint .buildJacobianmust be called previously.426 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. 384 427 */ 385 428 btScalar getAngle(int axis_index) const; 429 430 //! Get the relative position of the constraint pivot 431 /*! 432 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. 433 */ 434 btScalar getRelativePivotPosition(int axis_index) const; 435 386 436 387 437 //! Test angular limit. 388 438 /*! 389 439 Calculates angular correction and returns true if limit needs to be corrected. 390 \pre btGeneric6DofConstraint .buildJacobianmust be called previously.440 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. 391 441 */ 392 442 bool testAngularLimitMotor(int axis_index); … … 404 454 void setAngularLowerLimit(const btVector3& angularLower) 405 455 { 406 m_angularLimits[0].m_loLimit = angularLower.getX(); 407 m_angularLimits[1].m_loLimit = angularLower.getY(); 408 m_angularLimits[2].m_loLimit = angularLower.getZ(); 456 for(int i = 0; i < 3; i++) 457 m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); 409 458 } 410 459 411 460 void setAngularUpperLimit(const btVector3& angularUpper) 412 461 { 413 m_angularLimits[0].m_hiLimit = angularUpper.getX(); 414 m_angularLimits[1].m_hiLimit = angularUpper.getY(); 415 m_angularLimits[2].m_hiLimit = angularUpper.getZ(); 462 for(int i = 0; i < 3; i++) 463 m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); 416 464 } 417 465 … … 438 486 else 439 487 { 488 lo = btNormalizeAngle(lo); 489 hi = btNormalizeAngle(hi); 440 490 m_angularLimits[axis-3].m_loLimit = lo; 441 491 m_angularLimits[axis-3].m_hiLimit = hi; … … 460 510 } 461 511 462 const btRigidBody& getRigidBodyA() const463 {464 return m_rbA;465 }466 const btRigidBody& getRigidBodyB() const467 {468 return m_rbB;469 }470 471 512 virtual void calcAnchorPos(void); // overridable 472 513 473 514 int get_limit_motor_info2( btRotationalLimitMotor * limot, 474 btRigidBody * body0, btRigidBody * body1, 475 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational); 476 477 515 const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, 516 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false); 517 518 // access for UseFrameOffset 519 bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } 520 void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } 521 522 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 523 ///If no axis is provided, it uses the default axis for this constraint. 524 virtual void setParam(int num, btScalar value, int axis = -1); 525 ///return the local value of parameter 526 virtual btScalar getParam(int num, int axis = -1) const; 527 528 virtual int calculateSerializeBufferSize() const; 529 530 ///fills the dataBuffer and returns the struct name (and 0 on failure) 531 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; 532 533 478 534 }; 479 535 536 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 537 struct btGeneric6DofConstraintData 538 { 539 btTypedConstraintData m_typeConstraintData; 540 btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. 541 btTransformFloatData m_rbBFrame; 542 543 btVector3FloatData m_linearUpperLimit; 544 btVector3FloatData m_linearLowerLimit; 545 546 btVector3FloatData m_angularUpperLimit; 547 btVector3FloatData m_angularLowerLimit; 548 549 int m_useLinearReferenceFrameA; 550 int m_useOffsetForConstraintFrame; 551 }; 552 553 SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const 554 { 555 return sizeof(btGeneric6DofConstraintData); 556 } 557 558 ///fills the dataBuffer and returns the struct name (and 0 on failure) 559 SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const 560 { 561 562 btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer; 563 btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); 564 565 m_frameInA.serializeFloat(dof->m_rbAFrame); 566 m_frameInB.serializeFloat(dof->m_rbBFrame); 567 568 569 int i; 570 for (i=0;i<3;i++) 571 { 572 dof->m_angularLowerLimit.m_floats[i] = float(m_angularLimits[i].m_loLimit); 573 dof->m_angularUpperLimit.m_floats[i] = float(m_angularLimits[i].m_hiLimit); 574 dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]); 575 dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]); 576 } 577 578 dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0; 579 dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0; 580 581 return "btGeneric6DofConstraintData"; 582 } 583 584 585 586 587 480 588 #endif //GENERIC_6DOF_CONSTRAINT_H
Note: See TracChangeset
for help on using the changeset viewer.