void btGeneric6DofConstraint::buildJacobian()
{
#ifndef __SPU__
if (m_useSolveConstraintObsolete)
{
// Clear accumulated impulses for the next simulation step
m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
int i;
for(i = 0; i < 3; i++)
{
m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
}
//calculates transform
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
calcAnchorPos();
btVector3 pivotAInW = m_AnchorPos;
btVector3 pivotBInW = m_AnchorPos;
// not used here
// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 normalWorld;
//linear part
for (i=0;i<3;i++)
{
if (m_linearLimits.isLimited(i))
{
if (m_useLinearReferenceFrameA)
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
else
normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
buildLinearJacobian(
m_jacLinear[i],normalWorld ,
pivotAInW,pivotBInW);
}
}
// angular part
for (i=0;i<3;i++)
{
//calculates error angle
if (testAngularLimitMotor(i))
{
normalWorld = this->getAxis(i);
// Create angular atom
buildAngularJacobian(m_jacAng[i],normalWorld);
}
}
}
#endif //__SPU__
}
void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
} else
{
//prepare constraint
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
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--;
}
}
}
}
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 btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info)
{
//prepare constraint
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
info->m_numConstraintRows = 0;
info->nub = 0;
int i;
//test linear limits
for(i = 0; i < 3; i++)
{
if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2;
else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1;
if (m_linearLimits.m_enableMotor[i] ) info->m_numConstraintRows += 1;
if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1;
}
//test angular limits
for (i=0;i<3 ;i++ )
{
testAngularLimitMotor(i);
if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2;
else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1;
if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1;
if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
}
}
bool Generic6DOFJointSW::setup(float p_step) {
// Clear accumulated impulses for the next simulation step
m_linearLimits.m_accumulatedImpulse=Vector3(real_t(0.), real_t(0.), real_t(0.));
int i;
for(i = 0; i < 3; i++)
{
m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
}
//calculates transform
calculateTransforms();
// const Vector3& pivotAInW = m_calculatedTransformA.origin;
// const Vector3& pivotBInW = m_calculatedTransformB.origin;
calcAnchorPos();
Vector3 pivotAInW = m_AnchorPos;
Vector3 pivotBInW = m_AnchorPos;
// not used here
// Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
// Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
Vector3 normalWorld;
//linear part
for (i=0;i<3;i++)
{
if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
{
if (m_useLinearReferenceFrameA)
normalWorld = m_calculatedTransformA.basis.get_axis(i);
else
normalWorld = m_calculatedTransformB.basis.get_axis(i);
buildLinearJacobian(
m_jacLinear[i],normalWorld ,
pivotAInW,pivotBInW);
}
}
// angular part
for (i=0;i<3;i++)
{
//calculates error angle
if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i))
{
normalWorld = this->getAxis(i);
// Create angular atom
buildAngularJacobian(m_jacAng[i],normalWorld);
}
}
return true;
}
void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies)
{
//prepare constraint
calculateTransforms(transA,transB,bodies);
int i;
for (i=0;i<3 ;i++ )
{
testAngularLimitMotor(i);
}
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);
}
}
void btGeneric6DofConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
{
btAssert(!m_useSolveConstraintObsolete);
//prepare constraint
calculateTransforms(transA, transB);
int i;
for (i = 0; i < 3; i++)
{
testAngularLimitMotor(i);
}
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);
}
}