void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
{
//prepare constraint
calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies);
info->m_numConstraintRows = 0;
info->nub = 6;
int i;
//test linear limits
for(i = 0; i < 3; i++)
{
if(m_linearLimits.needApplyForce(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
//test angular limits
for (i=0;i<3 ;i++ )
{
if(testAngularLimitMotor(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
// printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
}
void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies)
{
b3Transform transA;
b3Transform transB;
transA = getCenterOfMassTransform(bodies[m_rbA]);
transB = getCenterOfMassTransform(bodies[m_rbB]);
calculateTransforms(transA,transB,bodies);
}
void btRigidBody::debugLocalProperties(btIDebugDraw* glDebugDrawer){
btTransform chassisTrans = getCenterOfMassTransform();
btVector3 sideVector (
chassisTrans.getBasis()[0][0],
chassisTrans.getBasis()[1][0],
chassisTrans.getBasis()[2][0]);
btVector3 fwdVector (
chassisTrans.getBasis()[0][2],
chassisTrans.getBasis()[1][2],
chassisTrans.getBasis()[2][2]);
if(idDebug == 1){
for(int i = 0; i < 4;i++){
btVector3 angLinearVelocity = debugAngularVelocity[i].cross(debugRelVector[i]);
btVector3 relativePosition = debugRelVector[i] + getCenterOfMassPosition();
btVector3 toRelativePosition = relativePosition+angLinearVelocity*2;
//glDebugDrawer->drawLine(relativePosition,toRelativePosition,btVector3(0.2,0.7,0.5));
//angLinearVelocity = debugAngularVelocity.cross(4.9*-sideVector);
relativePosition = getCenterOfMassPosition()-sideVector*6;
toRelativePosition = relativePosition+angLinearVelocity*1000;
//glDebugDrawer->drawLine(relativePosition,toRelativePosition,btVector3(0.2,0.7,0.5));
relativePosition = getCenterOfMassPosition()+fwdVector*4+btVector3(0,3,0);
toRelativePosition = relativePosition+debugLinearVelocity*1000;
//glDebugDrawer->drawLine(relativePosition,toRelativePosition,btVector3(0.2,0.7,0.5));
}
}
}
void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies)
{
b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
const b3Vector3& linVelB = bodies[m_rbB].m_linVel;
const b3Vector3& angVelA = bodies[m_rbA].m_angVel;
const b3Vector3& angVelB = bodies[m_rbB].m_angVel;
if(m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
}
else
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
}
}