Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Basic support for COLLADA physics constraints (each DOF can be completely locked or free, no limits yet)

Browse Source
This commit is contained in:
ejcoumans
2006-07-28 21:49:26 +00:00
parent 63594284c3
commit b8cbfe5f72
5 changed files with 379 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp
View File

@@ -32,6 +32,10 @@ Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, con
, m_frameInA(frameInA)
, m_frameInB(frameInB)
{
//free means upper < lower,
//locked means upper == lower
//limited means upper > lower
//so start all locked
for (int i=0; i<6;++i)
{
m_lowerLimit[i] = 0.0f;
@@ -58,11 +62,13 @@ void Generic6DofConstraint::BuildJacobian()
int i;
//linear part
for (i=0;i<3;i++)
{
if (isLimited(i))
{
normal[i] = 1;
// Create linear atom
new (&m_jac[i]) JacobianEntry(
new (&m_jacLinear[i]) JacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
@@ -81,9 +87,12 @@ void Generic6DofConstraint::BuildJacobian()
normal[i] = 0;
}
}
// angular part
for (i=0;i<3;i++)
{
if (isLimited(i+3))
{
SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
@@ -104,6 +113,7 @@ void Generic6DofConstraint::BuildJacobian()
m_rbA.applyTorqueImpulse( impulse_vector);
m_rbB.applyTorqueImpulse(-impulse_vector);
}
}
}
void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
@@ -122,16 +132,18 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
// linear
for (i=0;i<3;i++)
{
if (isLimited(i))
{
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
normal[i] = 1;
SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
SimdScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal();
//velocity error (first order error)
SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
SimdScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
//positional error (zeroth order error)
@@ -146,9 +158,12 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
normal[i] = 0;
}
}
// angular
for (i=0;i<3;i++)
{
if (isLimited(i+3))
{
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
@@ -176,6 +191,7 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
m_rbA.applyTorqueImpulse( impulse_vector);
m_rbB.applyTorqueImpulse(-impulse_vector);
}
}
}
void Generic6DofConstraint::UpdateRHS(SimdScalar timeStep)

46
BulletDynamics/ConstraintSolver/Generic6DofConstraint.h
View File

@@ -30,7 +30,7 @@ class RigidBody;
/// Work in progress (is still a Hinge actually)
class Generic6DofConstraint : public TypedConstraint
{
JacobianEntry m_jac[3]; // 3 orthogonal linear constraints
JacobianEntry m_jacLinear[3]; // 3 orthogonal linear constraints
JacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints
SimdTransform m_frameInA; // the constraint space w.r.t body A
@@ -55,10 +55,48 @@ public:
SimdScalar ComputeAngle(int axis) const;
void SetAngularLimit(int axis, SimdScalar lo, SimdScalar hi)
void setLinearLowerLimit(const SimdVector3& linearLower)
{
m_lowerLimit[axis + 3] = lo;
m_upperLimit[axis + 3] = hi;
m_lowerLimit[0] = linearLower.getX();
m_lowerLimit[1] = linearLower.getY();
m_lowerLimit[2] = linearLower.getZ();
}
void setLinearUpperLimit(const SimdVector3& linearUpper)
{
m_upperLimit[0] = linearUpper.getX();
m_upperLimit[1] = linearUpper.getY();
m_upperLimit[2] = linearUpper.getZ();
}
void setAngularLowerLimit(const SimdVector3& angularLower)
{
m_lowerLimit[3] = angularLower.getX();
m_lowerLimit[4] = angularLower.getY();
m_lowerLimit[5] = angularLower.getZ();
}
void setAngularUpperLimit(const SimdVector3& angularUpper)
{
m_upperLimit[3] = angularUpper.getX();
m_upperLimit[4] = angularUpper.getY();
m_upperLimit[5] = angularUpper.getZ();
}
//first 3 are linear, next 3 are angular
void SetLimit(int axis, SimdScalar lo, SimdScalar hi)
{
m_lowerLimit[axis] = lo;
m_upperLimit[axis] = hi;
}
//free means upper < lower,
//locked means upper == lower
//limited means upper > lower
//limitIndex: first 3 are linear, next 3 are angular
bool isLimited(int limitIndex)
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
const RigidBody& GetRigidBodyA() const

217
Demos/ColladaDemo/ColladaDemo.cpp
View File

@@ -87,9 +87,57 @@ extern int gForwardAxis;
#include "dae.h"
#include "dom/domCOLLADA.h"
DAE* collada = 0;
domCOLLADA* dom = 0;
domMatrix_Array emptyMatrixArray;
SimdTransform GetSimdTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
domRotate_Array& rotateArray,
domTranslate_Array& translateArray
)
{
SimdTransform startTransform;
startTransform.setIdentity();
int i;
//either load the matrix (worldspace) or incrementally build the transform from 'translate'/'rotate'
for (i=0;i<matrixArray.getCount();i++)
{
domMatrixRef matrixRef = matrixArray[i];
domFloat4x4 fl16 = matrixRef->getValue();
SimdVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11));
startTransform.setOrigin(origin);
SimdMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2),
fl16.get(4),fl16.get(5),fl16.get(6),
fl16.get(8),fl16.get(9),fl16.get(10));
startTransform.setBasis(basis);
}
for (i=0;i<rotateArray.getCount();i++)
{
domRotateRef rotateRef = rotateArray[i];
domFloat4 fl4 = rotateRef->getValue();
float angleRad = SIMD_RADS_PER_DEG*fl4.get(3);
SimdQuaternion rotQuat(SimdVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad);
startTransform.getBasis() = startTransform.getBasis() * SimdMatrix3x3(rotQuat);
}
for (i=0;i<translateArray.getCount();i++)
{
domTranslateRef translateRef = translateArray[i];
domFloat3 fl3 = translateRef->getValue();
startTransform.getOrigin() += SimdVector3(fl3.get(0),fl3.get(1),fl3.get(2));
}
return startTransform;
}
#endif
@@ -160,7 +208,7 @@ CollisionShape* gShapePtr[maxNumObjects];//1 rigidbody has 1 shape (no re-use of
////////////////////////////////////
///Very basic import
void CreatePhysicsObject(bool isDynamic, float mass, const SimdTransform& startTransform,CollisionShape* shape)
CcdPhysicsController* CreatePhysicsObject(bool isDynamic, float mass, const SimdTransform& startTransform,CollisionShape* shape)
{
startTransforms[numObjects] = startTransform;
@@ -245,8 +293,8 @@ void CreatePhysicsObject(bool isDynamic, float mass, const SimdTransform& startT
}
numObjects++;
//return newly created PhysicsController
return physObjects[numObjects++];
}
@@ -1449,38 +1497,12 @@ int main(int argc,char** argv)
//find transform of the node that this rigidbody maps to
int i;
//either load the matrix (worldspace) or incrementally build the transform from 'translate'/'rotate'
for (i=0;i<node->getMatrix_array().getCount();i++)
{
domMatrixRef matrixRef = node->getMatrix_array()[i];
domFloat4x4 fl16 = matrixRef->getValue();
SimdVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11));
startTransform.setOrigin(origin);
SimdMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2),
fl16.get(4),fl16.get(5),fl16.get(6),
fl16.get(8),fl16.get(9),fl16.get(10));
startTransform.setBasis(basis);
}
for (i=0;i<node->getRotate_array().getCount();i++)
{
domRotateRef rotateRef = node->getRotate_array()[i];
domFloat4 fl4 = rotateRef->getValue();
float angleRad = SIMD_RADS_PER_DEG*fl4.get(3);
SimdQuaternion rotQuat(SimdVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad);
startTransform.getBasis() = startTransform.getBasis() * SimdMatrix3x3(rotQuat);
}
for (i=0;i<node->getTranslate_array().getCount();i++)
{
domTranslateRef translateRef = node->getTranslate_array()[i];
domFloat3 fl3 = translateRef->getValue();
startTransform.getOrigin() += SimdVector3(fl3.get(0),fl3.get(1),fl3.get(2));
}
startTransform = GetSimdTransformFromCOLLADA_DOM(
node->getMatrix_array(),
node->getRotate_array(),
node->getTranslate_array()
);
for (i=0;i<node->getScale_array().getCount();i++)
{
@@ -1493,7 +1515,9 @@ int main(int argc,char** argv)
CreatePhysicsObject(isDynamics,mass,startTransform,colShape);
CcdPhysicsController* ctrl = CreatePhysicsObject(isDynamics,mass,startTransform,colShape);
//for bodyName lookup in constraints
ctrl->setNewClientInfo((void*)bodyName);
}
@@ -1527,19 +1551,128 @@ int main(int argc,char** argv)
if (rigidConstraintRef->getSid() && !strcmp(rigidConstraintRef->getSid(),constraintName))
{
/*
//two bodies
rigidConstraintRef->getRef_attachment();
rigidConstraintRef->getAttachment();
const domRigid_constraint::domRef_attachmentRef attachRefBody = rigidConstraintRef->getRef_attachment();
const domRigid_constraint::domAttachmentRef attachBody1 = rigidConstraintRef->getAttachment();
daeString uri = attachRefBody->getRigid_body().getURI();
daeString orgUri0 = attachRefBody->getRigid_body().getOriginalURI();
daeString orgUri1 = attachBody1->getRigid_body().getOriginalURI();
CcdPhysicsController* ctrl0=0,*ctrl1=0;
for (int i=0;i<numObjects;i++)
{
char* bodyName = (char*)physObjects[i]->getNewClientInfo();
if (!strcmp(bodyName,orgUri0))
{
printf("found\n");
ctrl0=physObjects[i];
}
if (!strcmp(bodyName,orgUri1))
{
ctrl1=physObjects[i];
}
}
const domRigid_constraint::domAttachmentRef attachOtherBody = rigidConstraintRef->getAttachment();
const domRigid_constraint::domTechnique_commonRef commonRef = rigidConstraintRef->getTechnique_common();
domFloat3 flMax = commonRef->getLimits()->getLinear()->getMax()->getValue();
SimdVector3 maxLinearLimit(flMax.get(0),flMax.get(1),flMax.get(2));
domFloat3 flMin = commonRef->getLimits()->getLinear()->getMin()->getValue();
SimdVector3 minLinearLimit(flMin.get(0),flMin.get(1),flMin.get(2));
commonRef->getLimits()->getSwing_cone_and_twist();
*/
domFloat3 flMax = commonRef->getLimits()->getLinear()->getMax()->getValue();
SimdVector3 maxLinearLimit(flMax.get(0),flMax.get(1),flMax.get(2));
domFloat3 coneMinLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMin()->getValue();
SimdVector3 angularMin(coneMinLimit.get(0),coneMinLimit.get(1),coneMinLimit.get(2));
domFloat3 coneMaxLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMax()->getValue();
SimdVector3 angularMax(coneMaxLimit.get(0),coneMaxLimit.get(1),coneMaxLimit.get(2));
{
int constraintId;
SimdTransform attachFrameRef0;
attachFrameRef0 =
GetSimdTransformFromCOLLADA_DOM
(
emptyMatrixArray,
attachRefBody->getRotate_array(),
attachRefBody->getTranslate_array());
SimdTransform attachFrameOther;
attachFrameOther =
GetSimdTransformFromCOLLADA_DOM
(
emptyMatrixArray,
attachBody1->getRotate_array(),
attachBody1->getTranslate_array()
);
//convert INF / -INF into lower > upper
//currently there is a hack in the DOM to detect INF / -INF
//see daeMetaAttribute.cpp
//INF -> 999999.9
//-INF -> -999999.9
float linearCheckTreshold = 999999.0;
float angularCheckTreshold = 180.0;//check this
//free means upper < lower,
//locked means upper == lower
//limited means upper > lower
//limitIndex: first 3 are linear, next 3 are angular
SimdVector3 linearLowerLimits = minLinearLimit;
SimdVector3 linearUpperLimits = maxLinearLimit;
SimdVector3 angularLowerLimits = angularMin;
SimdVector3 angularUpperLimits = angularMax;
{
for (int i=0;i<3;i++)
{
if ((linearLowerLimits[i] < -linearCheckTreshold) ||
(linearUpperLimits[i] > linearCheckTreshold))
{
//disable limits
linearLowerLimits[i] = 1;
linearUpperLimits[i] = 0;
}
if ((angularLowerLimits[i] < -angularCheckTreshold) ||
(angularUpperLimits[i] > angularCheckTreshold))
{
//disable limits
angularLowerLimits[i] = 1;
angularUpperLimits[i] = 0;
}
}
}
constraintId =physicsEnvironmentPtr->createUniversalD6Constraint(
ctrl0,
ctrl1,
attachFrameRef0,
attachFrameOther,
linearLowerLimits,
linearUpperLimits,
angularLowerLimits,
angularUpperLimits
);
}
}
}

56
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
View File

@@ -1327,6 +1327,62 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
}
//Following the COLLADA physics specification for constraints
int CcdPhysicsEnvironment::createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& frameInA,
SimdTransform& frameInB,
const SimdVector3& linearMinLimits,
const SimdVector3& linearMaxLimits,
const SimdVector3& angularMinLimits,
const SimdVector3& angularMaxLimits
)
{
//we could either add some logic to recognize ball-socket and hinge, or let that up to the user
//perhaps some warning or hint that hinge/ball-socket is more efficient?
Generic6DofConstraint* genericConstraint = 0;
CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
RigidBody* rb0 = ctrl0->GetRigidBody();
RigidBody* rb1 = ctrl1->GetRigidBody();
if (rb1)
{
genericConstraint = new Generic6DofConstraint(
*rb0,*rb1,
frameInA,frameInB);
genericConstraint->setLinearLowerLimit(linearMinLimits);
genericConstraint->setLinearUpperLimit(linearMaxLimits);
genericConstraint->setAngularLowerLimit(angularMinLimits);
genericConstraint->setAngularUpperLimit(angularMaxLimits);
} else
{
// TODO: Implement single body case...
//No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
}
if (genericConstraint)
{
m_constraints.push_back(genericConstraint);
genericConstraint->SetUserConstraintId(gConstraintUid++);
genericConstraint->SetUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
//64 bit systems can't cast pointer to int. could use size_t instead.
return genericConstraint->GetUserConstraintId();
}
return 0;
}
void CcdPhysicsEnvironment::removeConstraint(int constraintId)
{
std::vector<TypedConstraint*>::iterator i;

16
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
View File

@@ -20,6 +20,8 @@ subject to the following restrictions:
#include <vector>
class CcdPhysicsController;
#include "SimdVector3.h"
#include "SimdTransform.h"
@@ -119,6 +121,20 @@ protected:
virtual int createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
float pivotX,float pivotY,float pivotZ,
float axisX,float axisY,float axisZ);
//Following the COLLADA physics specification for constraints
virtual int createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& localAttachmentFrameRef,
SimdTransform& localAttachmentOther,
const SimdVector3& linearMinLimits,
const SimdVector3& linearMaxLimits,
const SimdVector3& angularMinLimits,
const SimdVector3& angularMaxLimits
);
virtual void removeConstraint(int constraintid);