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