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__
}
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;
}