diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index c18974130..6e2cd66b8 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -285,8 +285,59 @@ void btHingeConstraint::buildJacobian() #endif //__SPU__ +static inline btScalar btNormalizeAnglePositive(btScalar angle) +{ + return btFmod(btFmod(angle, 2.0*SIMD_PI) + 2.0*SIMD_PI, 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; @@ -606,6 +657,8 @@ void btHingeConstraint::updateRHS(btScalar timeStep) } + + btScalar btHingeConstraint::getHingeAngle() { return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 7c33ac24e..30d506c6e 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -236,6 +236,7 @@ public: } + ///The getHingeAngle gives the hinge angle in range [-PI,PI] btScalar getHingeAngle(); btScalar getHingeAngle(const btTransform& transA,const btTransform& transB); @@ -326,6 +327,43 @@ struct btHingeConstraintDoubleData }; #endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION +///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account +ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint +{ +protected: + btScalar m_accumulatedAngle; +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false) + :btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA ) + { + m_accumulatedAngle=getHingeAngle(); + } + + btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false) + :btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA) + { + m_accumulatedAngle=getHingeAngle(); + } + + btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false) + :btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA ) + { + m_accumulatedAngle=getHingeAngle(); + } + + btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false) + :btHingeConstraint(rbA,rbAFrame, useReferenceFrameA ) + { + m_accumulatedAngle=getHingeAngle(); + } + btScalar getAccumulatedHingeAngle(); + void setAccumulatedHingeAngle(btScalar accAngle); + virtual void getInfo1 (btConstraintInfo1* info); + +}; struct btHingeConstraintFloatData {