Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btHingeConstraint.cpp
index c1897413078be52712485fd19a0bc84aaabd2b1f..76a1509471e89abd24fd49cc581c750e2a202e79 100644 (file)
@@ -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);
                }