void CustomPulley::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;
	dVector veloc0;
	dVector veloc1;
	dFloat jacobian0[6];
	dFloat jacobian1[6];

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);
	
	// calculate the angular velocity for both bodies
	NewtonBodyGetVelocity(m_body0, &veloc0[0]);
	NewtonBodyGetVelocity(m_body1, &veloc1[0]);

	// get angular velocity relative to the pin vector
	dFloat w0 = veloc0 % matrix0.m_front;
	dFloat w1 = veloc1 % matrix1.m_front;

	// establish the gear equation.
	dFloat relVeloc = w0 + m_gearRatio * w1;
	if (m_gearRatio > dFloat(1.0f)) {
		relVeloc = w0 / m_gearRatio + w1;
	}

	// calculate the relative angular acceleration by dividing by the time step
	// ideally relative acceleration should be zero, but is practice there will always 
	// be a small difference in velocity that need to be compensated. 
	// using the full acceleration will make the to over show a oscillate 
	// so use only fraction of the acceleration
	dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep: 1.0f;
	dFloat relAccel = - 0.3f * relVeloc * invTimestep;

	// set the linear part of Jacobian 0 to translational pin vector	
	jacobian0[0] = 	matrix0.m_front[0];
	jacobian0[1] = 	matrix0.m_front[1];
	jacobian0[2] = 	matrix0.m_front[2];

	// set the rotational part of Jacobian 0 to zero
	jacobian0[3] = 	0.0f;
	jacobian0[4] = 	0.0f;
	jacobian0[5] = 	0.0f;

	// set the linear part of Jacobian 1 to translational pin vector	
	jacobian1[0] = 	matrix1.m_front[0];
	jacobian1[1] = 	matrix1.m_front[1];
	jacobian1[2] = 	matrix1.m_front[2];

	// set the rotational part of Jacobian 1 to zero
	jacobian1[3] = 	0.0f;
	jacobian1[4] = 	0.0f;
	jacobian1[5] = 	0.0f;

	// add a angular constraint
	NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);

	// set the desired angular acceleration between the two bodies
	NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
	void SubmitConstraints(dFloat timestep, int threadIndex)
	{
		dMatrix matrix0;
		dMatrix matrix1;

		// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
		CalculateGlobalMatrix(matrix0, matrix1);

		dVector p0(matrix0.m_posit);
		dVector p1(matrix1.m_posit);

		dVector dir(p1 - p0);
		dFloat mag2 = dir % dir;
		dir = dir.Scale(1.0f / dSqrt(mag2));
		dMatrix matrix(dGrammSchmidt(dir));
		dFloat x = dSqrt(mag2) - m_distance;

		dVector com0;
		dVector com1;
		dVector veloc0;
		dVector veloc1;
		dMatrix body0Matrix;
		dMatrix body1Matrix;

		NewtonBody* const body0 = GetBody0();
		NewtonBody* const body1 = GetBody1();

		NewtonBodyGetCentreOfMass(body0, &com0[0]);
		NewtonBodyGetMatrix(body0, &body0Matrix[0][0]);
		NewtonBodyGetPointVelocity(body0, &p0[0], &veloc0[0]);

		NewtonBodyGetCentreOfMass(body1, &com1[0]);
		NewtonBodyGetMatrix(body1, &body1Matrix[0][0]);
		NewtonBodyGetPointVelocity(body1, &p1[0], &veloc1[0]);

		dFloat v((veloc0 - veloc1) % dir);
		dFloat a = (x - v * timestep) / (timestep * timestep);

		dVector r0((p0 - body0Matrix.TransformVector(com0)) * matrix.m_front);
		dVector r1((p1 - body1Matrix.TransformVector(com1)) * matrix.m_front);
		dFloat jacobian0[6];
		dFloat jacobian1[6];

		jacobian0[0] = matrix[0][0];
		jacobian0[1] = matrix[0][1];
		jacobian0[2] = matrix[0][2];
		jacobian0[3] = r0[0];
		jacobian0[4] = r0[1];
		jacobian0[5] = r0[2];

		jacobian1[0] = -matrix[0][0];
		jacobian1[1] = -matrix[0][1];
		jacobian1[2] = -matrix[0][2];
		jacobian1[3] = -r1[0];
		jacobian1[4] = -r1[1];
		jacobian1[5] = -r1[2];
		NewtonUserJointAddGeneralRow(m_joint, jacobian0, jacobian1);
		NewtonUserJointSetRowAcceleration(m_joint, a);
	}
void dCustomPulley::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;
	dVector veloc0(0.0f);
	dVector veloc1(0.0f);
	dFloat jacobian0[6];
	dFloat jacobian1[6];

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);

	// set the linear part of Jacobian 0 to translational pin vector	
	dVector dir0 (matrix0.m_front.Scale (1.0f/m_gearRatio));
	const dVector& dir1 = matrix1.m_front;
	jacobian0[0] = dir0.m_x;
	jacobian0[1] = dir0.m_y;	
	jacobian0[2] = dir0.m_z;
	jacobian0[3] = 0.0f;
	jacobian0[4] = 0.0f;
	jacobian0[5] = 0.0f;
	
	jacobian1[0] = dir1.m_x;
	jacobian1[1] = dir1.m_y;	
	jacobian1[2] = dir1.m_z;	
	jacobian1[3] = 0.0f;
	jacobian1[4] = 0.0f;
	jacobian1[5] = 0.0f;

	// calculate the angular velocity for both bodies
	NewtonBodyGetVelocity(m_body0, &veloc0[0]);
	NewtonBodyGetVelocity(m_body1, &veloc1[0]);

	// get angular velocity relative to the pin vector
	dFloat w0 = veloc0.DotProduct3(dir0);
	dFloat w1 = veloc1.DotProduct3(dir1);
	dFloat relVeloc = w0 + w1;

	dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
	dFloat relAccel = -0.5f * relVeloc * invTimestep;

	// add a angular constraint
	NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);

	// set the desired angular acceleration between the two bodies
	NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
void dCustomRackAndPinion::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;
	dVector omega0(0.0f);
	dVector veloc1(0.0f);
	dFloat jacobian0[6];
	dFloat jacobian1[6];

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);
	
	// calculate the angular velocity for both bodies
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	NewtonBodyGetVelocity(m_body1, &veloc1[0]);
	dVector dir0 (matrix0.m_front.Scale (m_gearRatio));
	const dVector& dir1 = matrix1.m_front;

	jacobian0[0] = dFloat(0.0f);
	jacobian0[1] = dFloat(0.0f);
	jacobian0[2] = dFloat(0.0f);
	jacobian0[3] = dir0.m_x;
	jacobian0[4] = dir0.m_y;
	jacobian0[5] = dir0.m_z;

	jacobian1[0] = dir1.m_x;
	jacobian1[1] = dir1.m_y;
	jacobian1[2] = dir1.m_z;
	jacobian1[3] = dFloat(0.0f);
	jacobian1[4] = dFloat(0.0f);
	jacobian1[5] = dFloat(0.0f);

	dFloat w0 = omega0.DotProduct3(dir0);
	dFloat w1 = veloc1.DotProduct3(dir1);
	dFloat relOmega = w0 + w1;
	dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
	dFloat relAccel = -0.5f * relOmega * invTimestep;
	NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
	NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
Beispiel #5
0
void CustomGear::SubmitConstrainst (dFloat timestep, int threadIndex)
{
	dFloat w0;
	dFloat w1;
	dFloat relAccel;
	dFloat relOmega;
	dVector omega0;
	dVector omega1;
	dMatrix matrix0;
	dMatrix matrix1;
	dFloat jacobian0[6];
	dFloat jacobian1[6];

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);

	
	// calculate the angular velocity for both bodies
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	NewtonBodyGetOmega(m_body1, &omega1[0]);

	// get angular velocity relative to the pin vector
	w0 = omega0 % matrix0.m_front;
	w1 = omega1 % matrix1.m_front;

	// establish the gear equation.
	relOmega = w0 + m_gearRatio * w1;

	// calculate the relative angular acceleration by dividing by the time step
	// ideally relative acceleration should be zero, but is practice there will always 
	// be a small difference in velocity that need to be compensated. 
	// using the full acceleration will make the to over show a oscillate 
	// so use only fraction of the acceleration
	relAccel = - 0.3f * relOmega / timestep;


	// set the linear part of Jacobian 0 to zero	
	jacobian0[0] = 	0.0f;
	jacobian0[1] = 	0.0f;
	jacobian0[2] = 	0.0f;

	// set the angular part of Jacobian 0 pin vector		
	jacobian0[3] = 	matrix0.m_front[0];
	jacobian0[4] = 	matrix0.m_front[1];
	jacobian0[5] = 	matrix0.m_front[2];

	// set the linear part of Jacobian 1 to zero
	jacobian1[0] = 	0.0f;
	jacobian1[1] = 	0.0f;
	jacobian1[2] = 	0.0f;

	// set the angular part of Jacobian 1 pin vector	
	jacobian1[3] = 	matrix1.m_front[0];
	jacobian1[4] = 	matrix1.m_front[1];
	jacobian1[5] = 	matrix1.m_front[2];

	// add a angular constraint
	NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);

	// set the desired angular acceleration between the two bodies
	NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
    void SubmitConstraints (dFloat timestep, int threadIndex)
    {
        dFloat den;
        dFloat relAccel;
        dFloat jacobian0[6];
        dFloat jacobian1[6];

        dMatrix leftMatrix;
        dMatrix rightMatrix;
        dMatrix chassisMatrix;

        NewtonBodyGetMatrix (m_chassis, &chassisMatrix[0][0]);
        NewtonBodyGetMatrix (m_leftTire->GetBody1(), &leftMatrix[0][0]);
        NewtonBodyGetMatrix (m_rightTire->GetBody1(), &rightMatrix[0][0]);

        // calculate the geometrical turn radius of for this axle

        dVector leftOrigin (chassisMatrix.UntransformVector(leftMatrix.m_posit));
        dVector rightOrigin (chassisMatrix.UntransformVector(rightMatrix.m_posit));
        dVector axleCenter ((rightOrigin + leftOrigin).Scale (0.5f));

        dVector tireAxisDir (chassisMatrix.UnrotateVector((leftMatrix.m_front + rightMatrix.m_front).Scale (0.5f)));
        axleCenter.m_y = 0.0f;
        tireAxisDir.m_y = 0.0f;

        dVector sideDir (0.0f, 0.0f, 1.0f, 0.0f);
        dVector deltaDir (tireAxisDir - sideDir);
        relAccel = 0.0f;
        den = deltaDir % deltaDir;
        if (den > 1.0e-6f) {
            dFloat R;
            dFloat num;
            dFloat ratio;
            dFloat wl;
            dFloat wr;
            dFloat rl;
            dFloat rr;
            dFloat relOmega;

            num = axleCenter % deltaDir;
            R = - num / den;

            rr = (rightOrigin % sideDir);
            rl = (leftOrigin % sideDir);
            ratio = (R + rr) / (R + rl);

            dVector omegaLeft;
            dVector omegaRight;
            // calculate the angular velocity for both bodies
            NewtonBodyGetOmega(m_leftTire->GetBody1(), &omegaLeft[0]);
            NewtonBodyGetOmega(m_rightTire->GetBody1(), &omegaRight[0]);

            // get angular velocity relative to the pin vector
            wl = -(omegaLeft % leftMatrix.m_front);
            wr = omegaRight % rightMatrix.m_front;

            // establish the gear equation.
            relOmega = wl + ratio * wr;
            relAccel = - 0.5f * relOmega / timestep;
        }

        jacobian0[0] = 0.0f;
        jacobian0[1] = 0.0f;
        jacobian0[2] = 0.0f;
        jacobian0[3] = leftMatrix.m_front.m_x * -1.0f;
        jacobian0[4] = leftMatrix.m_front.m_y * -1.0f;
        jacobian0[5] = leftMatrix.m_front.m_z * -1.0f;

        jacobian1[0] = 0.0f;
        jacobian1[1] = 0.0f;
        jacobian1[2] = 0.0f;
        jacobian1[3] = rightMatrix.m_front.m_x;
        jacobian1[4] = rightMatrix.m_front.m_y;
        jacobian1[5] = rightMatrix.m_front.m_z;

        NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
        NewtonUserJointSetRowAcceleration (m_joint, relAccel);
        NewtonUserJointSetRowMaximumFriction(m_joint, m_maxfriction);
        NewtonUserJointSetRowMinimumFriction(m_joint, -m_maxfriction);

    }