#18872 bugfix for torque on dynamic objects
authorErwin Coumans <blender@erwincoumans.com>
Sat, 6 Jun 2009 00:12:49 +0000 (00:12 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Sat, 6 Jun 2009 00:12:49 +0000 (00:12 +0000)
#18893, fix to getParam for generic 6dof constraints

extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
source/gameengine/Ketsji/KX_ConstraintWrapper.cpp
source/gameengine/Ketsji/KX_ConstraintWrapper.h
source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
source/gameengine/Physics/Dummy/DummyPhysicsEnvironment.h
source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.h
source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h

index f60eabe2d2b692998d5e98bdc896e23b93e15aeb..4d69fe75f8388ebe02751c2d2a782d052809d879 100644 (file)
@@ -663,10 +663,15 @@ btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
 }
 
 
+btScalar       btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
+{
+       return m_calculatedLinearDiff[axisIndex];
+}
+
 
-btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
+btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
 {
-       return m_calculatedAxisAngleDiff[axis_index];
+       return m_calculatedAxisAngleDiff[axisIndex];
 }
 
 
@@ -839,7 +844,6 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
 
 
 
-
 btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
        : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
 {
@@ -848,6 +852,7 @@ btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, b
                m_springEnabled[i] = false;
                m_equilibriumPoint[i] = btScalar(0.f);
                m_springStiffness[i] = btScalar(0.f);
+               m_springDamping[i] = btScalar(1.f);
        }
 }
 
@@ -875,6 +880,13 @@ void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
 }
 
 
+void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
+{
+       btAssert((index >= 0) && (index < 6));
+       m_springDamping[index] = damping;
+}
+
+
 void btGeneric6DofSpringConstraint::setEquilibriumPoint()
 {
        calculateTransforms();
@@ -908,6 +920,7 @@ void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
 
 void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
 {
+       calculateTransforms();
        // it is assumed that calculateTransforms() have been called before this call
        int i;
        btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
@@ -921,8 +934,9 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
                        btScalar delta = currPos - m_equilibriumPoint[i];
                        // spring force is (delta * m_stiffness) according to Hooke's Law
                        btScalar force = delta * m_springStiffness[i];
-                       m_linearLimits.m_targetVelocity[i] = force  * info->fps;
-                       m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps;
+                       btScalar velFactor = info->fps * m_springDamping[i];
+                       m_linearLimits.m_targetVelocity[i] =  velFactor * force;
+                       m_linearLimits.m_maxMotorForce[i] =  btFabs(force) / info->fps;
                }
        }
        for(i = 0; i < 3; i++)
@@ -935,7 +949,8 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
                        btScalar delta = currPos - m_equilibriumPoint[i+3];
                        // spring force is (-delta * m_stiffness) according to Hooke's Law
                        btScalar force = -delta * m_springStiffness[i+3];
-                       m_angularLimits[i].m_targetVelocity = force  * info->fps;
+                       btScalar velFactor = info->fps * m_springDamping[i+3];
+                       m_angularLimits[i].m_targetVelocity = velFactor * force;
                        m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
                }
        }
index d01c9cf63abca6267b128d4fbb6abfca461d77d3..8082eb1f1329623caedc089c40918d983af3808d 100644 (file)
@@ -382,14 +382,21 @@ public:
 
     //! Get the relative Euler angle
     /*!
-       \pre btGeneric6DofConstraint.buildJacobian must be called previously.
+       \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
        */
     btScalar getAngle(int axis_index) const;
 
+       //! Get the relative position of the constraint pivot
+    /*!
+       \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
+       */
+       btScalar getRelativePivotPosition(int axis_index) const;
+
+
        //! Test angular limit.
        /*!
        Calculates angular correction and returns true if limit needs to be corrected.
-       \pre btGeneric6DofConstraint.buildJacobian must be called previously.
+       \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
        */
     bool testAngularLimitMotor(int axis_index);
 
@@ -496,11 +503,13 @@ protected:
        bool            m_springEnabled[6];
        btScalar        m_equilibriumPoint[6];
        btScalar        m_springStiffness[6];
+       btScalar        m_springDamping[6]; // between 0 and 1 (1 == no damping)
        void internalUpdateSprings(btConstraintInfo2* info);
 public: 
     btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
        void enableSpring(int index, bool onOff);
        void setStiffness(int index, btScalar stiffness);
+       void setDamping(int index, btScalar damping);
        void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
        void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
        virtual void getInfo2 (btConstraintInfo2* info);
index 3e5594e0d1c45b42524f125db207408a4cbf4b02..c5cf67af67dd74f474e1ea183fb26a3a6df5daf8 100644 (file)
@@ -54,6 +54,20 @@ PyObject* KX_ConstraintWrapper::PyGetConstraintId()
        return PyInt_FromLong(m_constraintId);
 }
 
+
+PyObject* KX_ConstraintWrapper::PyGetParam(PyObject* args, PyObject* kwds)
+{
+       int dof;
+       float value;
+       
+       if (!PyArg_ParseTuple(args,"i:getParam",&dof))
+               return NULL;
+       
+       value = m_physenv->getConstraintParam(m_constraintId,dof);
+       return PyFloat_FromDouble(value);
+       
+}
+
 PyObject* KX_ConstraintWrapper::PySetParam(PyObject* args, PyObject* kwds)
 {
        int dof;
@@ -119,6 +133,7 @@ int KX_ConstraintWrapper::py_setattro(PyObject *attr,PyObject* value)
 PyMethodDef KX_ConstraintWrapper::Methods[] = {
        {"getConstraintId",(PyCFunction) KX_ConstraintWrapper::sPyGetConstraintId, METH_NOARGS},
        {"setParam",(PyCFunction) KX_ConstraintWrapper::sPySetParam, METH_VARARGS},
+       {"getParam",(PyCFunction) KX_ConstraintWrapper::sPyGetParam, METH_VARARGS},
        {NULL,NULL} //Sentinel
 };
 
index d4f038e2898c6070fc8efe518e85187faf583c04..03813e0f16781aec73c6da35c999a7af86dd467e 100644 (file)
@@ -45,6 +45,7 @@ public:
        
        KX_PYMETHOD_NOARGS(KX_ConstraintWrapper,GetConstraintId);
        KX_PYMETHOD(KX_ConstraintWrapper,SetParam);
+       KX_PYMETHOD(KX_ConstraintWrapper,GetParam);
 
 private:
        int                                     m_constraintId;
index 3a3c817698be652281ffc2290a354f87f2119559..d22c09b4d3e5d213f7ffe25aa9597f8fd439239f 100644 (file)
@@ -1019,7 +1019,8 @@ void              CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torque
                        //workaround for incompatibility between 'DYNAMIC' game object, and angular factor
                        //a DYNAMIC object has some inconsistency: it has no angular effect due to collisions, but still has torque
                        const btVector3& angFac = body->getAngularFactor();
-                       body->setAngularFactor(1.f);
+                       btVector3 tmpFac(0,0,1);
+                       body->setAngularFactor(tmpFac);
                        body->applyTorque(torque);
                        body->setAngularFactor(angFac);
                }
index 2dc7bffe618593ad5ddc7d2ebd7ec7c815381d8c..bc7ccacc39bba7b37645145c71bee49f2f5d5e60 100644 (file)
@@ -1786,6 +1786,45 @@ CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
 }
 
 
+float  CcdPhysicsEnvironment::getConstraintParam(int constraintId,int param)
+{
+       btTypedConstraint* typedConstraint = getConstraintById(constraintId);
+       switch (typedConstraint->getUserConstraintType())
+       {
+       case PHY_GENERIC_6DOF_CONSTRAINT:
+               {
+                       
+                       switch (param)
+                       {
+                       case 0: case 1: case 2: 
+                               {
+                                       //param = 0..2 are linear constraint values
+                                       btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
+                                       genCons->calculateTransforms();
+                                       return genCons->getRelativePivotPosition(param);
+                                       break;
+                               }
+                               case 3: case 4: case 5:
+                               {
+                                       //param = 3..5 are relative constraint (Euler) angles
+                                       btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
+                                       genCons->calculateTransforms();
+                                       return genCons->getAngle(param-3);
+                                       break;
+                               }
+                       default:
+                               {
+                               }
+                       }
+                       break;
+               };
+       default:
+               {
+               };
+       };
+       return 0.f;
+}
+
 void   CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
 {
        btTypedConstraint* typedConstraint = getConstraintById(constraintId);
@@ -1835,11 +1874,9 @@ void     CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float
                                        {
                                                bool springEnabled = true;
                                                genCons->setStiffness(springIndex,value0);
+                                               genCons->setDamping(springIndex,value1);
                                                genCons->enableSpring(springIndex,springEnabled);
-                                               if (value1>0.5f)
-                                               {
-                                                       genCons->setEquilibriumPoint(springIndex);
-                                               }
+                                               genCons->setEquilibriumPoint(springIndex);
                                        } else
                                        {
                                                bool springEnabled = false;
index 4e39d531cd635ae4036e85747112203b1ecfbcb3..bc5491e00ccd06260392fb413e81e4727a54ac2c 100644 (file)
@@ -149,7 +149,10 @@ protected:
                        const btVector3& angularMaxLimits,int flags
                        );
 
+               
                virtual void    setConstraintParam(int constraintId,int param,float value,float value1);
+               
+               virtual float   getConstraintParam(int constraintId,int param);
 
            virtual void                removeConstraint(int    constraintid);
 
index 73e7e94735577ebed28446f3a2d4552e38fb3584..8dbd137f9de3c184f329dfce47c3af44c69582fc 100644 (file)
@@ -88,6 +88,11 @@ public:
                {
                }
 
+               virtual float   getConstraintParam(int constraintId,int param)
+               {
+                       return 0.f;
+               }
+
 };
 
 #endif //_DUMMYPHYSICSENVIRONMENT
index 4c9d59e3673163ddf87fe24b63657b5740bc5eeb..5ae33eb4b0e49bc2cb8c7a6205dad516f9a65b30 100644 (file)
@@ -88,6 +88,10 @@ public:
        virtual PHY_IPhysicsController* CreateSphereController(float radius,const PHY__Vector3& position);
        virtual PHY_IPhysicsController* CreateConeController(float coneradius,float coneheight);
 
+       virtual float   getConstraintParam(int constraintId,int param)
+       {
+               return 0.f;
+       }
        virtual void    setConstraintParam(int constraintId,int param,float value,float value1)
        {
        }
index 1939083ef5fb6cb3f120c0edb65c0863c80d6180..c76e9d175ce91d6f27f3eaec7a144d7b69865fce 100644 (file)
@@ -159,6 +159,7 @@ class PHY_IPhysicsEnvironment
                virtual PHY_IPhysicsController* CreateConeController(float coneradius,float coneheight)=0;
                
                virtual void    setConstraintParam(int constraintId,int param,float value,float value1) = 0;
+               virtual float   getConstraintParam(int constraintId,int param) = 0;
 };
 
 #endif //_IPHYSICSENVIRONMENT