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