+ renamed pad3 to m_contactProcessingThreshold (thanks Campbell Barton/ideasman for...
authorErwin Coumans <blender@erwincoumans.com>
Sun, 24 May 2009 06:31:47 +0000 (06:31 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Sun, 24 May 2009 06:31:47 +0000 (06:31 +0000)
+ fixed Python method, PyArg_ParseTuple already checks for errors, no returning of NULL, thanks Campbell too)
+ added linear/angular spring for each of the 6DOFs of a generic 6dof constraint.  This makes the generic 6dof constraint very versatile.

extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
source/blender/blenkernel/intern/object.c
source/blender/blenloader/intern/readfile.c
source/blender/makesdna/DNA_object_types.h
source/blender/src/buttons_logic.c
source/gameengine/Converter/BL_BlenderDataConversion.cpp
source/gameengine/Ketsji/KX_ConstraintWrapper.cpp
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp

index 9644f2b6d5719f058feed1a2f0baf6b0abfaf67c..2edf28c6bac60b5fe57ca4d435d84a0010fbfcbc 100644 (file)
@@ -22,9 +22,11 @@ http://gimpact.sf.net
 #include "btGeneric6DofConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btTransformUtil.h"
 #include <new>
 
 
+
 #define D6_USE_OBSOLETE_METHOD false
 
 
@@ -738,7 +740,9 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
         }
         // if we're limited low and high simultaneously, the joint motor is
         // ineffective
-        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
+        if (limit && (limot->m_loLimit == limot->m_hiLimit)) 
+                       powered = 0;
+
         info->m_constraintError[srow] = btScalar(0.f);
         if (powered)
         {
@@ -827,3 +831,117 @@ 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)
+{
+       for(int i = 0; i < 6; i++)
+       {
+               m_springEnabled[i] = false;
+               m_equilibriumPoint[i] = btScalar(0.f);
+               m_springStiffness[i] = btScalar(0.f);
+       }
+}
+
+
+void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
+{
+       btAssert((index >= 0) && (index < 6));
+       m_springEnabled[index] = onOff;
+       if(index < 3)
+       {
+               m_linearLimits.m_enableMotor[index] = onOff;
+       }
+       else
+       {
+               m_angularLimits[index - 3].m_enableMotor = onOff;
+       }
+}
+
+
+
+void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
+{
+       btAssert((index >= 0) && (index < 6));
+       m_springStiffness[index] = stiffness;
+}
+
+
+void btGeneric6DofSpringConstraint::setEquilibriumPoint()
+{
+       calculateTransforms();
+       for(int i = 0; i < 3; i++)
+       {
+               m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
+       }
+       for(int i = 0; i < 3; i++)
+       {
+               m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
+       }
+}
+
+
+
+void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
+{
+       btAssert((index >= 0) && (index < 6));
+       calculateTransforms();
+       if(index < 3)
+       {
+               m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
+       }
+       else
+       {
+               m_equilibriumPoint[index + 3] = m_calculatedAxisAngleDiff[index];
+       }
+}
+
+
+
+void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
+{
+       // it is assumed that calculateTransforms() have been called before this call
+       int i;
+       btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
+       for(i = 0; i < 3; i++)
+       {
+               if(m_springEnabled[i])
+               {
+                       // get current position of constraint
+                       btScalar currPos = m_calculatedLinearDiff[i];
+                       // calculate difference
+                       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;
+               }
+       }
+       for(i = 0; i < 3; i++)
+       {
+               if(m_springEnabled[i + 3])
+               {
+                       // get current position of constraint
+                       btScalar currPos = m_calculatedAxisAngleDiff[i];
+                       // calculate difference
+                       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;
+                       m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
+               }
+       }
+}
+
+
+void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
+{
+       // this will be called by constraint solver at the constraint setup stage
+       // set current motor parameters
+       internalUpdateSprings(info);
+       // do the rest of job for constraint setup
+       btGeneric6DofConstraint::getInfo2(info);
+}
+
+
+
+
index 5af866390eec6c60e7d99d390d35befad503e63e..33e4749ffd07b1b10aca1f7d3406d31cd90bb402 100644 (file)
@@ -478,4 +478,20 @@ public:
 };
 
 
+class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
+{
+protected:
+       bool            m_springEnabled[6];
+       btScalar        m_equilibriumPoint[6];
+       btScalar        m_springStiffness[6];
+       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 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);
+};
+
 #endif //GENERIC_6DOF_CONSTRAINT_H
index 9739668c7c7d96e2ff2512acdc18bf1ac29bd63d..ec068c35c1182ec1480f879ee94d6c121df7d2aa 100644 (file)
@@ -985,7 +985,7 @@ Object *add_only_object(int type, char *name)
        ob->gameflag= OB_PROP|OB_COLLISION;
        ob->margin = 0.0;
        /* ob->pad3 == Contact Processing Threshold */
-       ob->pad3 = 1.;
+       ob->m_contactProcessingThreshold = 1.;
        
        /* NT fluid sim defaults */
        ob->fluidsimFlag = 0;
index 0b0a97b7ec004bf01a4cced71d38ff18f10181ed..d8566e06ed7f6be78da876507822ab033c831c71 100644 (file)
@@ -8110,7 +8110,7 @@ static void do_versions(FileData *fd, Library *lib, Main *main)
                Object *ob;
                World *wrld;
                for(ob = main->object.first; ob; ob= ob->id.next) {
-                       ob->pad3 = 1.; //pad3 is used for m_contactProcessingThreshold
+                       ob->m_contactProcessingThreshold = 1.; //pad3 is used for m_contactProcessingThreshold
                        if(ob->parent) {
                                /* check if top parent has compound shape set and if yes, set this object
                                   to compound shaper as well (was the behaviour before, now it's optional) */
index 11fa44fe488422ebedbfe8fb08bec3d45a55a26e..741822b5a981587da6ae3d31058d91670a14ff4c 100644 (file)
@@ -159,7 +159,7 @@ typedef struct Object {
        float margin;
        float max_vel; /* clamp the maximum velocity 0.0 is disabled */
        float min_vel; /* clamp the maximum velocity 0.0 is disabled */
-       float pad3; /* pad3 is now used for m_contactProcessingThreshold, can we still rename it? */
+       float m_contactProcessingThreshold;
 
        char dt, dtx;
        char totcol;    /* copy of mesh or curve or meta */
index b0ce3c8a95b79bb26815569803061fb8b70e6df7..c283cd18ca2c6d417e09759f5a6f0f04afedbd48 100644 (file)
@@ -3204,7 +3204,7 @@ static uiBlock *advanced_bullet_menu(void *arg_ob)
                        if (ob->gameflag & OB_RIGID_BODY)
                        {
                                uiDefButF(block, NUM, 0, "CPT",
-                                       xco+180, yco, 180, 19, &ob->pad3, 0.00, 1., 1, 0, 
+                                       xco+180, yco, 180, 19, &ob->m_contactProcessingThreshold, 0.00, 1., 1, 0, 
                                        "Contact Processing Threshold");
                        
                                yco -= 20;
@@ -3287,7 +3287,7 @@ static uiBlock *advanced_bullet_menu(void *arg_ob)
                                        xco, yco, 180, 19, &ob->margin, 0.0, 1.0, 1, 0, 
                                        "Collision margin");
                        uiDefButF(block, NUM, 0, "CPT",
-                               xco+180, yco, 180, 19, &ob->pad3, 0.00, 1., 1, 0, 
+                               xco+180, yco, 180, 19, &ob->m_contactProcessingThreshold, 0.00, 1., 1, 0, 
                                "Contact Processing Threshold");
 
                }
index a9c839595c2c89a674512ba36cb0d8608609d0bd..d9e93d41f342b52a48bc2b3a839ecbc845f42615 100644 (file)
@@ -1422,7 +1422,7 @@ void BL_CreatePhysicsObjectNew(KX_GameObject* gameobj,
        ///contact processing threshold is only for rigid bodies and static geometry, not 'dynamic'
        if (objprop.m_angular_rigidbody || !objprop.m_dyna )
        {
-               objprop.m_contactProcessingThreshold = blenderobject->pad3;
+               objprop.m_contactProcessingThreshold = blenderobject->m_contactProcessingThreshold;
        } else
        {
                objprop.m_contactProcessingThreshold = 0.f;
index b0576424a474cc1d31f8ed3bfc4ac44065011335..b40100db2f7308cc70cce7648f025f9fd86a7b5c 100644 (file)
@@ -59,22 +59,17 @@ PyObject* KX_ConstraintWrapper::PySetParam(PyObject* args, PyObject* kwds)
        int len = PyTuple_Size(args);
        int success = 1;
        
-       if (len == 3)
+       int dof;
+       float minLimit,maxLimit;
+       success = PyArg_ParseTuple(args,"iff:setParam",&dof,&minLimit,&maxLimit);
+       if (success)
        {
-               int dof;
-               float minLimit,maxLimit;
-               success = PyArg_ParseTuple(args,"iff",&dof,&minLimit,&maxLimit);
-               if (success)
-               {
-                       m_physenv->setConstraintParam(m_constraintId,dof,minLimit,maxLimit);
-                       Py_RETURN_NONE;
-               }
+               m_physenv->setConstraintParam(m_constraintId,dof,minLimit,maxLimit);
        }
-       return NULL;
+       Py_RETURN_NONE;
 }
 
 
-
 //python specific stuff
 PyTypeObject KX_ConstraintWrapper::Type = {
 #if (PY_VERSION_HEX >= 0x02060000)
index 561c370854fcf24a14e6b870f49daf8c30eccae7..58720c8cc3082b440bf4baba2ef052e943a4b5de 100644 (file)
@@ -922,7 +922,7 @@ int                 CcdPhysicsEnvironment::createUniversalD6Constraint(
                
 
                bool useReferenceFrameA = true;
-               genericConstraint = new btGeneric6DofConstraint(
+               genericConstraint = new btGeneric6DofSpringConstraint(
                        *rb0,*rb1,
                        frameInA,frameInB,useReferenceFrameA);
                genericConstraint->setLinearLowerLimit(linearMinLimits);
@@ -1831,6 +1831,28 @@ void     CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float
                                        break;
                                }
 
+                       case 12: case 13: case 14: case 15: case 16: case 17:
+                       {
+                               //param 13-17 are for motorized springs on each of the degrees of freedom
+                                       btGeneric6DofSpringConstraint* genCons = (btGeneric6DofSpringConstraint*)typedConstraint;
+                                       int springIndex = param-12;
+                                       if (value0!=0.f)
+                                       {
+                                               bool springEnabled = true;
+                                               genCons->setStiffness(springIndex,value0);
+                                               genCons->enableSpring(springIndex,springEnabled);
+                                               if (value1>0.5f)
+                                               {
+                                                       genCons->setEquilibriumPoint(springIndex);
+                                               }
+                                       } else
+                                       {
+                                               bool springEnabled = false;
+                                               genCons->enableSpring(springIndex,springEnabled);
+                                       }
+                                       break;
+                       }
+
                        default:
                                {
                                }
@@ -2351,7 +2373,7 @@ int                       CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
                                frameInB = inv  * globalFrameA;
                                bool useReferenceFrameA = true;
 
-                               genericConstraint = new btGeneric6DofConstraint(
+                               genericConstraint = new btGeneric6DofSpringConstraint(
                                        *rb0,*rb1,
                                        frameInA,frameInB,useReferenceFrameA);
 
@@ -2375,7 +2397,7 @@ int                       CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
                                frameInB = rb0->getCenterOfMassTransform() * frameInA;
 
                                bool useReferenceFrameA = true;
-                               genericConstraint = new btGeneric6DofConstraint(
+                               genericConstraint = new btGeneric6DofSpringConstraint(
                                        *rb0,s_fixedObject2,
                                        frameInA,frameInB,useReferenceFrameA);
                        }