forked from bartvdbraak/blender
+ 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:
parent
45f2463c83
commit
83bb096f24
@ -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);
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user