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__

}
Пример #2
0
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;
}