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

added quickstep improvements, to allow for constraints (point to point etc).

Browse Source 
Thanks Francisco Leon/projectileman
This commit is contained in:
ejcoumans
2007-09-13 07:44:05 +00:00
parent 7f5823ee16
commit 0bf8124668
6 changed files with 1043 additions and 352 deletions
Download Patch File Download Diff File Expand all files Collapse all files

393
Extras/quickstep/OdeConstraintSolver.cpp
View File

@@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -24,6 +24,7 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "OdeJoint.h"
#include "OdeContactJoint.h"
#include "OdeTypedJoint.h"
#include "OdeSolverBody.h"
#include <new.h>
#include "LinearMath/btQuickprof.h"
@@ -61,11 +62,12 @@ class BU_Joint;
#define ODE_MAX_SOLVER_JOINTS 65535
static OdeSolverBody gSolverBodyArray[ODE_MAX_SOLVER_BODIES];
static ContactJoint gJointArray[ODE_MAX_SOLVER_JOINTS];
static OdeTypedJoint gTypedJointArray[ODE_MAX_SOLVER_JOINTS];
OdeConstraintSolver::OdeConstraintSolver():
m_cfm(0.f),//1e-5f),
m_erp(0.4f)
m_cfm(0.f),//1e-5f),
m_erp(0.4f)
{
}
@@ -74,45 +76,56 @@ m_erp(0.4f)
//iterative lcp and penalty method
btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher)
{
BEGIN_PROFILE("prepareConstraints");
BEGIN_PROFILE("prepareConstraints");
m_CurBody = 0;
m_CurJoint = 0;
m_CurBody = 0;
m_CurJoint = 0;
m_CurTypedJoint = 0;
int numBodies = 0;
OdeSolverBody* odeBodies [ODE_MAX_SOLVER_BODIES];
int numJoints = 0;
BU_Joint* joints [ODE_MAX_SOLVER_JOINTS];
for (int j=0;j<numManifolds;j++)
{
int numBodies = 0;
OdeSolverBody* odeBodies [ODE_MAX_SOLVER_BODIES];
int numJoints = 0;
BU_Joint* joints [ODE_MAX_SOLVER_JOINTS*2];
int body0=-1,body1=-1;
//capture contacts
int j;
int body0=-1,body1=-1;
for (j=0;j<numManifolds;j++)
{
btPersistentManifold* manifold = manifoldPtr[j];
if (manifold->getNumContacts() > 0)
{
body0 = ConvertBody((btRigidBody*)manifold->getBody0(),odeBodies,numBodies);
body1 = ConvertBody((btRigidBody*)manifold->getBody1(),odeBodies,numBodies);
ConvertConstraint(manifold,joints,numJoints,odeBodies,body0,body1,debugDrawer);
}
}
btPersistentManifold* manifold = manifoldPtr[j];
if (manifold->getNumContacts() > 0)
{
body0 = ConvertBody((btRigidBody*)manifold->getBody0(),odeBodies,numBodies);
body1 = ConvertBody((btRigidBody*)manifold->getBody1(),odeBodies,numBodies);
ConvertConstraint(manifold,joints,numJoints,odeBodies,body0,body1,debugDrawer);
}
}
//capture constraints
for (j=0;j<numConstraints;j++)
{
btTypedConstraint * typedconstraint = constraints[j];
body0 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyA(),odeBodies,numBodies);
body1 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyB(),odeBodies,numBodies);
ConvertTypedConstraint(typedconstraint,joints,numJoints,odeBodies,body0,body1,debugDrawer);
}
END_PROFILE("prepareConstraints");
BEGIN_PROFILE("solveConstraints");
SolveInternal1(m_cfm,m_erp,odeBodies,numBodies,joints,numJoints,infoGlobal);
//write back resulting velocities
for (int i=0;i<numBodies;i++)
{
if (odeBodies[i]->m_invMass)
{
odeBodies[i]->m_originalBody->setLinearVelocity(odeBodies[i]->m_linearVelocity);
odeBodies[i]->m_originalBody->setAngularVelocity(odeBodies[i]->m_angularVelocity);
}
}
END_PROFILE("solveConstraints");
return 0.f;
END_PROFILE("prepareConstraints");
BEGIN_PROFILE("solveConstraints");
SolveInternal1(m_cfm,m_erp,odeBodies,numBodies,joints,numJoints,infoGlobal);
//write back resulting velocities
for (int i=0;i<numBodies;i++)
{
if (odeBodies[i]->m_invMass)
{
odeBodies[i]->m_originalBody->setLinearVelocity(odeBodies[i]->m_linearVelocity);
odeBodies[i]->m_originalBody->setAngularVelocity(odeBodies[i]->m_angularVelocity);
}
}
END_PROFILE("solveConstraints");
return 0.f;
}
@@ -124,24 +137,24 @@ typedef btScalar dQuaternion[4];
void dRfromQ1 (dMatrix3 R, const dQuaternion q)
{
// q = (s,vx,vy,vz)
btScalar qq1 = 2.f*q[1]*q[1];
btScalar qq2 = 2.f*q[2]*q[2];
btScalar qq3 = 2.f*q[3]*q[3];
_R(0,0) = 1.f - qq2 - qq3;
_R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
_R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
_R(0,3) = 0.f;
_R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
_R(1,1) = 1.f - qq1 - qq3;
_R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
_R(1,3) = 0.f;
// q = (s,vx,vy,vz)
btScalar qq1 = 2.f*q[1]*q[1];
btScalar qq2 = 2.f*q[2]*q[2];
btScalar qq3 = 2.f*q[3]*q[3];
_R(0,0) = 1.f - qq2 - qq3;
_R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
_R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
_R(0,3) = 0.f;
_R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
_R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
_R(2,2) = 1.f - qq1 - qq2;
_R(2,3) = 0.f;
_R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
_R(1,1) = 1.f - qq1 - qq3;
_R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
_R(1,3) = 0.f;
_R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
_R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
_R(2,2) = 1.f - qq1 - qq2;
_R(2,3) = 0.f;
}
@@ -149,149 +162,207 @@ void dRfromQ1 (dMatrix3 R, const dQuaternion q)
int OdeConstraintSolver::ConvertBody(btRigidBody* orgBody,OdeSolverBody** bodies,int& numBodies)
{
assert(orgBody);
if (!orgBody || (orgBody->getInvMass() == 0.f) )
{
return -1;
}
assert(orgBody);
if (!orgBody || (orgBody->getInvMass() == 0.f) )
{
return -1;
}
if (orgBody->getCompanionId()>=0)
{
return orgBody->getCompanionId();
}
//first try to find
int i,j;
//if not found, create a new body
OdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies];
orgBody->setCompanionId(numBodies);
if (orgBody->getCompanionId()>=0)
{
return orgBody->getCompanionId();
}
//first try to find
int i,j;
numBodies++;
//if not found, create a new body
OdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies];
orgBody->setCompanionId(numBodies);
body->m_originalBody = orgBody;
numBodies++;
body->m_facc.setValue(0,0,0,0);
body->m_tacc.setValue(0,0,0,0);
body->m_originalBody = orgBody;
body->m_linearVelocity = orgBody->getLinearVelocity();
body->m_angularVelocity = orgBody->getAngularVelocity();
body->m_invMass = orgBody->getInvMass();
body->m_centerOfMassPosition = orgBody->getCenterOfMassPosition();
body->m_friction = orgBody->getFriction();
//are the indices the same ?
for (i=0;i<4;i++)
{
for ( j=0;j<3;j++)
{
body->m_invI[i+4*j] = 0.f;
body->m_I[i+4*j] = 0.f;
}
}
body->m_invI[0+4*0] = orgBody->getInvInertiaDiagLocal().x();
body->m_invI[1+4*1] = orgBody->getInvInertiaDiagLocal().y();
body->m_invI[2+4*2] = orgBody->getInvInertiaDiagLocal().z();
body->m_facc.setValue(0,0,0,0);
body->m_tacc.setValue(0,0,0,0);
body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal().x();
body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal().y();
body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal().z();
body->m_linearVelocity = orgBody->getLinearVelocity();
body->m_angularVelocity = orgBody->getAngularVelocity();
body->m_invMass = orgBody->getInvMass();
body->m_centerOfMassPosition = orgBody->getCenterOfMassPosition();
body->m_friction = orgBody->getFriction();
dQuaternion q;
//are the indices the same ?
for (i=0;i<4;i++)
{
for ( j=0;j<3;j++)
{
body->m_invI[i+4*j] = 0.f;
body->m_I[i+4*j] = 0.f;
}
}
body->m_invI[0+4*0] = orgBody->getInvInertiaDiagLocal().x();
body->m_invI[1+4*1] = orgBody->getInvInertiaDiagLocal().y();
body->m_invI[2+4*2] = orgBody->getInvInertiaDiagLocal().z();
q[1] = orgBody->getOrientation().x();
q[2] = orgBody->getOrientation().y();
q[3] = orgBody->getOrientation().z();
q[0] = orgBody->getOrientation().w();
dRfromQ1(body->m_R,q);
return numBodies-1;
body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal().x();
body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal().y();
body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal().z();
dQuaternion q;
q[1] = orgBody->getOrientation().x();
q[2] = orgBody->getOrientation().y();
q[3] = orgBody->getOrientation().z();
q[0] = orgBody->getOrientation().w();
dRfromQ1(body->m_R,q);
return numBodies-1;
}
void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints,
OdeSolverBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
OdeSolverBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
{
manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(),
((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform());
manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(),
((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform());
int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
int i,numContacts = manifold->getNumContacts();
bool swapBodies = (bodyId0 < 0);
int i,numContacts = manifold->getNumContacts();
OdeSolverBody* body0,*body1;
bool swapBodies = (bodyId0 < 0);
if (swapBodies)
{
bodyId0 = _bodyId1;
bodyId1 = _bodyId0;
body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1();
body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0();
OdeSolverBody* body0,*body1;
} else
{
body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0();
body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1();
}
if (swapBodies)
{
bodyId0 = _bodyId1;
bodyId1 = _bodyId0;
assert(bodyId0 >= 0);
body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1();
body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0();
btVector3 color(0,1,0);
for (i=0;i<numContacts;i++)
{
}
else
{
body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0();
body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1();
}
if (debugDrawer)
{
const btManifoldPoint& cp = manifold->getContactPoint(i);
assert(bodyId0 >= 0);
debugDrawer->drawContactPoint(
cp.m_positionWorldOnB,
cp.m_normalWorldOnB,
cp.getDistance(),
cp.getLifeTime(),
color);
btVector3 color(0,1,0);
for (i=0;i<numContacts;i++)
{
}
assert (m_CurJoint < ODE_MAX_SOLVER_JOINTS);
if (debugDrawer)
{
const btManifoldPoint& cp = manifold->getContactPoint(i);
debugDrawer->drawContactPoint(
cp.m_positionWorldOnB,
cp.m_normalWorldOnB,
cp.getDistance(),
cp.getLifeTime(),
color);
}
assert (m_CurJoint < ODE_MAX_SOLVER_JOINTS);
// if (manifold->getContactPoint(i).getDistance() < 0.0f)
{
ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1);
{
ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1);
cont->node[0].joint = cont;
cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0;
cont->node[1].joint = cont;
cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0;
joints[numJoints++] = cont;
for (int i=0;i<6;i++)
cont->lambda[i] = 0.f;
cont->node[0].joint = cont;
cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0;
cont->flags = 0;
}
cont->node[1].joint = cont;
cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0;
joints[numJoints++] = cont;
for (int i=0;i<6;i++)
cont->lambda[i] = 0.f;
cont->flags = 0;
}
}
//create a new contact constraint
}
void OdeConstraintSolver::ConvertTypedConstraint(
btTypedConstraint * constraint,BU_Joint** joints,int& numJoints,
OdeSolverBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
{
int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
bool swapBodies = (bodyId0 < 0);
OdeSolverBody* body0,*body1;
if (swapBodies)
{
bodyId0 = _bodyId1;
bodyId1 = _bodyId0;
body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1();
body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0();
}
else
{
body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0();
body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1();
}
assert(bodyId0 >= 0);
assert (m_CurTypedJoint < ODE_MAX_SOLVER_JOINTS);
OdeTypedJoint * cont = NULL;
// Determine constraint type
int joint_type = constraint->getConstraintType();
switch(joint_type)
{
case POINT2POINT_CONSTRAINT_TYPE:
case D6_CONSTRAINT_TYPE:
cont = new (&gTypedJointArray[m_CurTypedJoint ++]) OdeTypedJoint(constraint,0, swapBodies,body0,body1);
break;
};
if(cont)
{
cont->node[0].joint = cont;
cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0;
cont->node[1].joint = cont;
cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0;
joints[numJoints++] = cont;
for (int i=0;i<6;i++)
cont->lambda[i] = 0.f;
cont->flags = 0;
}
//create a new contact constraint
};
void OdeConstraintSolver::reset()
{
}