diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 9644f2b6d57..2edf28c6bac 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -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 + #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); +} + + + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 5af866390ee..33e4749ffd0 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -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 diff --git a/source/blender/blenkernel/intern/object.c b/source/blender/blenkernel/intern/object.c index 9739668c7c7..ec068c35c11 100644 --- a/source/blender/blenkernel/intern/object.c +++ b/source/blender/blenkernel/intern/object.c @@ -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; diff --git a/source/blender/blenloader/intern/readfile.c b/source/blender/blenloader/intern/readfile.c index 0b0a97b7ec0..d8566e06ed7 100644 --- a/source/blender/blenloader/intern/readfile.c +++ b/source/blender/blenloader/intern/readfile.c @@ -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) */ diff --git a/source/blender/makesdna/DNA_object_types.h b/source/blender/makesdna/DNA_object_types.h index 11fa44fe488..741822b5a98 100644 --- a/source/blender/makesdna/DNA_object_types.h +++ b/source/blender/makesdna/DNA_object_types.h @@ -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 */ diff --git a/source/blender/src/buttons_logic.c b/source/blender/src/buttons_logic.c index b0ce3c8a95b..c283cd18ca2 100644 --- a/source/blender/src/buttons_logic.c +++ b/source/blender/src/buttons_logic.c @@ -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"); } diff --git a/source/gameengine/Converter/BL_BlenderDataConversion.cpp b/source/gameengine/Converter/BL_BlenderDataConversion.cpp index a9c839595c2..d9e93d41f34 100644 --- a/source/gameengine/Converter/BL_BlenderDataConversion.cpp +++ b/source/gameengine/Converter/BL_BlenderDataConversion.cpp @@ -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; diff --git a/source/gameengine/Ketsji/KX_ConstraintWrapper.cpp b/source/gameengine/Ketsji/KX_ConstraintWrapper.cpp index b0576424a47..b40100db2f7 100644 --- a/source/gameengine/Ketsji/KX_ConstraintWrapper.cpp +++ b/source/gameengine/Ketsji/KX_ConstraintWrapper.cpp @@ -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) diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp index 561c370854f..58720c8cc30 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp @@ -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); }