void CustomBallAndSocketWithFriction::SubmitConstraints(dFloat timestep, int threadIndex)
{
CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
dVector omega0(0.0f, 0.0f, 0.0f, 0.0f);
dVector omega1(0.0f, 0.0f, 0.0f, 0.0f);
// get the omega vector
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
dVector relOmega(omega0 - omega1);
dFloat omegaMag = dSqrt(relOmega % relOmega);
if (omegaMag > 0.1f) {
// tell newton to used this the friction of the omega vector to apply the rolling friction
dMatrix basis(dGrammSchmidt(relOmega));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
// calculate the acceleration to stop the ball in one time step
dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[0][0]);
NewtonUserJointSetRowAcceleration(m_joint, -omegaMag * invTimestep);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
} else {
// when omega is too low this is correct but the small angle approximation theorem.
dMatrix basis(dGetIdentityMatrix());
for (int i = 0; i < 3; i++) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[i][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
}
}
}
dgVector dgBallConstraint::GetJointOmega () const
{
dgAssert (m_body0);
dgAssert (m_body1);
const dgMatrix& matrix = m_body0->GetMatrix();
dgVector dir0 (matrix.RotateVector (m_localMatrix0[0]));
dgVector dir1 (matrix.RotateVector (m_localMatrix0[1]));
dgVector dir2 (matrix.RotateVector (m_localMatrix0[2]));
const dgVector& omega0 = m_body0->GetOmega();
const dgVector& omega1 = m_body1->GetOmega();
// dgVector omega1 (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
// if (m_body1) {
// omega1 = m_body1->GetOmega();
// }
dgVector relOmega (omega0 - omega1);
return dgVector (relOmega % dir0, relOmega % dir1, relOmega % dir2, dgFloat32 (0.0f));
}
void CustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex)
{
// check if this is an impulsive time step
if (timestep > 0.0f) {
dMatrix matrix0;
dVector v(0.0f);
dVector w(0.0f);
dVector cg(0.0f);
dFloat invTimestep = 1.0f / timestep;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
NewtonBodyGetOmega (m_body0, &w[0]);
NewtonBodyGetVelocity (m_body0, &v[0]);
NewtonBodyGetCentreOfMass (m_body0, &cg[0]);
NewtonBodyGetMatrix (m_body0, &matrix0[0][0]);
dVector p0 (matrix0.TransformVector (m_localHandle));
dVector pointVeloc (v + w * matrix0.RotateVector (m_localHandle - cg));
dVector relPosit (m_targetPosit - p0);
dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc);
dVector relAccel (relVeloc.Scale (invTimestep * 0.3f));
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
if (m_pickMode) {
dQuaternion rotation;
NewtonBodyGetRotation (m_body0, &rotation.m_q0);
if (m_targetRot.DotProduct (rotation) < 0.0f) {
rotation.m_q0 *= -1.0f;
rotation.m_q1 *= -1.0f;
rotation.m_q2 *= -1.0f;
rotation.m_q3 *= -1.0f;
}
dVector relOmega (rotation.CalcAverageOmega (m_targetRot, invTimestep) - w);
dFloat mag = relOmega % relOmega;
if (mag > 1.0e-6f) {
dVector pin (relOmega.Scale (1.0f / mag));
dMatrix basis (dGrammSchmidt (pin));
dFloat relSpeed = dSqrt (relOmega % relOmega);
dFloat relAlpha = relSpeed * invTimestep;
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
} else {
dVector relAlpha (w.Scale (-invTimestep));
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
}
} else {
// this is the single handle pick mode, add some angular friction
dVector relAlpha = w.Scale (-invTimestep);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
}
}
}
void CustomUniversal::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);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
// construct an orthogonal coordinate system with these two vectors
dMatrix matrix1_1;
matrix1_1.m_up = matrix1.m_up;
matrix1_1.m_right = matrix0.m_front * matrix1.m_up;
matrix1_1.m_right = matrix1_1.m_right.Scale (1.0f / dSqrt (matrix1_1.m_right % matrix1_1.m_right));
matrix1_1.m_front = matrix1_1.m_up * matrix1_1.m_right;
NewtonUserJointAddAngularRow (m_joint, CalculateAngle (matrix0.m_front, matrix1_1.m_front, matrix1_1.m_right), &matrix1_1.m_right[0]);
dFloat sinAngle_0;
dFloat cosAngle_0;
CalculateAngle (matrix1_1.m_up, matrix0.m_up, matrix1_1.m_front, sinAngle_0, cosAngle_0);
dFloat angle0 = -m_curJointAngle_0.Update (cosAngle_0, sinAngle_0);
dFloat sinAngle_1;
dFloat cosAngle_1;
CalculateAngle(matrix1.m_front, matrix1_1.m_front, matrix1_1.m_up, sinAngle_1, cosAngle_1);
dFloat angle1 = -m_curJointAngle_1.Update (cosAngle_1, sinAngle_1);
dVector omega0 (0.0f, 0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
dVector relOmega (omega0 - omega1);
m_jointOmega_0 = relOmega % matrix0.m_front;
m_jointOmega_1 = relOmega % matrix1.m_up;
// check is the joint limit are enable
if (m_limit_0_On) {
if (angle0 < m_minAngle_0) {
dFloat relAngle = angle0 - m_minAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle0 > m_maxAngle_0) {
dFloat relAngle = angle0 - m_maxAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
// check is the joint limit motor is enable
} else if (m_angularMotor_0_On) {
// calculate the desired acceleration
// dFloat relOmega = (omega0 - omega1) % matrix0.m_front;
dFloat relAccel = m_angularAccel_0 - m_angularDamp_0 * m_jointOmega_0;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
// if limit are enable ...
if (m_limit_1_On) {
if (angle1 < m_minAngle_1) {
dFloat relAngle = angle1 - m_minAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle1 > m_maxAngle_1) {
dFloat relAngle = angle1 - m_maxAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
} else if (m_angularMotor_1_On) {
// calculate the desired acceleration
dFloat relAccel = m_angularAccel_1 - m_angularDamp_1 * m_jointOmega_1;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
}
void dCustomBallAndSocket::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);
SubmitLinearRows(0x07, matrix0, matrix1);
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngleCos = coneDir1.DotProduct3(coneDir0);
dMatrix coneRotation(dGetIdentityMatrix());
dVector lateralDir(matrix0.m_up);
if (cosAngleCos < 0.9999f) {
lateralDir = coneDir1.CrossProduct(coneDir0);
dFloat mag2 = lateralDir.DotProduct3(lateralDir);
if (mag2 > 1.0e-4f) {
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit);
} else {
lateralDir = matrix0.m_up.Scale (-1.0f);
coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit);
}
}
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
dVector relOmega(omega0 - omega1);
// do twist angle calculations
dMatrix twistMatrix(matrix0 * (matrix1 * coneRotation).Inverse());
dFloat twistAngle = m_twistAngle.Update(dAtan2(twistMatrix[1][2], twistMatrix[1][1]));
if (m_options.m_option0) {
if ((m_minTwistAngle == 0.0f) && (m_minTwistAngle == 0.0f)) {
NewtonUserJointAddAngularRow(m_joint, -twistAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
} else {
if (m_options.m_option1) {
// TODO spring option
dAssert (0);
} else {
SubmitConstraintTwistLimits(matrix0, matrix1, relOmega, timestep);
}
}
} else if (m_options.m_option1) {
// TODO spring option
dAssert (0);
} else if (m_twistFriction > 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
}
// do twist cone angle calculations
if (m_options.m_option2) {
if ((m_maxConeAngle == 0.0f)) {
dMatrix localMatrix(matrix0 * matrix1.Inverse());
dVector euler0;
dVector euler1;
localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, -euler0[2], &matrix1[2][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
} else {
if (m_options.m_option3) {
// TODO spring option
dAssert(0);
} else {
dFloat jointOmega = relOmega.DotProduct3(lateralDir);
dFloat currentAngle = dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)));
dFloat coneAngle = currentAngle + jointOmega * timestep;
if (coneAngle >= m_maxConeAngle) {
//dQuaternion rot(lateralDir, coneAngle);
//dVector frontDir(rot.RotateVector(coneDir1));
//dVector upDir(lateralDir.CrossProduct(frontDir));
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
const dFloat invtimestep = 1.0f / timestep;
const dFloat speed = 0.5f * (m_maxConeAngle - currentAngle) * invtimestep;
const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
} else if (m_coneFriction != 0) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
}
}
}
} else if (m_options.m_option3) {
// TODO spring option
dAssert(0);
} else if (m_coneFriction > 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
}
}