diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp')
-rw-r--r-- | extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp | 100 |
1 files changed, 91 insertions, 9 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index c1897413078..76a1509471e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -45,7 +45,11 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), - m_flags(0) + m_flags(0), + m_normalCFM(0), + m_normalERP(0), + m_stopCFM(0), + m_stopERP(0) { m_rbAFrame.getOrigin() = pivotInA; @@ -101,7 +105,11 @@ m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), -m_flags(0) +m_flags(0), +m_normalCFM(0), +m_normalERP(0), +m_stopCFM(0), +m_stopERP(0) { // since no frame is given, assume this to be zero angle and just pick rb transform axis @@ -151,7 +159,11 @@ m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), -m_flags(0) +m_flags(0), +m_normalCFM(0), +m_normalERP(0), +m_stopCFM(0), +m_stopERP(0) { #ifndef _BT_USE_CENTER_LIMIT_ //start with free @@ -177,7 +189,11 @@ m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), -m_flags(0) +m_flags(0), +m_normalCFM(0), +m_normalERP(0), +m_stopCFM(0), +m_stopERP(0) { ///not providing rigidbody B means implicitly using worldspace for body B @@ -285,8 +301,60 @@ void btHingeConstraint::buildJacobian() #endif //__SPU__ +static inline btScalar btNormalizeAnglePositive(btScalar angle) +{ + return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI)); +} + + + +static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle) +{ + btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) - + btNormalizeAnglePositive(accAngle))); + return result; +} + +static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle) +{ + btScalar tol(0.3); + btScalar result = btShortestAngularDistance(accAngle, curAngle); + + if (btFabs(result) > tol) + return curAngle; + else + return accAngle + result; + + return curAngle; +} + + +btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle() +{ + btScalar hingeAngle = getHingeAngle(); + m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle); + return m_accumulatedAngle; +} +void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle) +{ + m_accumulatedAngle = accAngle; +} + +void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info) +{ + //update m_accumulatedAngle + btScalar curHingeAngle = getHingeAngle(); + m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle); + + btHingeConstraint::getInfo1(info); + +} + + void btHingeConstraint::getInfo1(btConstraintInfo1* info) { + + if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; @@ -413,7 +481,9 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf a2.getSkewSymmetricMatrix(angular0,angular1,angular2); } // linear RHS - btScalar k = info->fps * info->erp; + btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp; + + btScalar k = info->fps * normalErp; if (!m_angularOnly) { for(i = 0; i < 3; i++) @@ -510,7 +580,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf powered = 0; } info->m_constraintError[srow] = btScalar(0.0f); - btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; + btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; if(powered) { if(m_flags & BT_HINGE_FLAGS_CFM_NORM) @@ -606,6 +676,8 @@ void btHingeConstraint::updateRHS(btScalar timeStep) } + + btScalar btHingeConstraint::getHingeAngle() { return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); @@ -798,7 +870,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; - btScalar k = info->fps * info->erp; + btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp; + btScalar k = info->fps * normalErp; if (!m_angularOnly) { @@ -856,7 +929,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. - k = info->fps * info->erp; + k = info->fps * normalErp;//?? + btVector3 u = ax1A.cross(ax1B); info->m_constraintError[s3] = k * u.dot(p); info->m_constraintError[s4] = k * u.dot(q); @@ -901,7 +975,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info powered = 0; } info->m_constraintError[srow] = btScalar(0.0f); - btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; + btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; if(powered) { if(m_flags & BT_HINGE_FLAGS_CFM_NORM) @@ -1002,6 +1076,10 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis) m_normalCFM = value; m_flags |= BT_HINGE_FLAGS_CFM_NORM; break; + case BT_CONSTRAINT_ERP: + m_normalERP = value; + m_flags |= BT_HINGE_FLAGS_ERP_NORM; + break; default : btAssertConstrParams(0); } @@ -1032,6 +1110,10 @@ btScalar btHingeConstraint::getParam(int num, int axis) const btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); retVal = m_normalCFM; break; + case BT_CONSTRAINT_ERP: + btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM); + retVal = m_normalERP; + break; default : btAssertConstrParams(0); } |