+ renamed pad3 to m_contactProcessingThreshold (thanks Campbell Barton/ideasman for confirming it is ok to rename it)

+ 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.
This commit is contained in:
Erwin Coumans 2009-05-24 06:31:47 +00:00
parent 45f2463c83
commit 83bb096f24
9 changed files with 172 additions and 21 deletions

@ -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);
}

@ -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

@ -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;

@ -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) */

@ -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 */

@ -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");
}

@ -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;

@ -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)

@ -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);
}