void CustomCorkScrew::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 two orthonormal axis direction perpendicular to the motion
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
// two rows to restrict rotation around around the parent coordinate system
dFloat sinAngle;
dFloat cosAngle;
CalculateYawAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_up[0]);
CalculateRollAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_right[0]);
// if limit are enable ...
if (m_limitsLinearOn) {
dFloat dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
if (dist < m_minLinearDist) {
// get a point along the up vector and set a constraint
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
} else if (dist > m_maxLinearDist) {
// get a point along the up vector and set a constraint
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
}
}
CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle);
dFloat angle = -m_curJointAngle.Update (cosAngle, sinAngle);
if (m_limitsAngularOn) {
// the joint angle can be determine by getting the angle between any two non parallel vectors
if (angle < m_minAngularDist) {
dFloat relAngle = angle - m_minAngularDist;
// the angle was clipped save the new clip limit
//m_curJointAngle.m_angle = m_minAngularDist;
// 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
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle > m_maxAngularDist) {
dFloat relAngle = angle - m_maxAngularDist;
// the angle was clipped save the new clip limit
//m_curJointAngle.m_angle = m_maxAngularDist;
// 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);
}
}
if (m_angularmotorOn) {
dVector omega0 (0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f);
// get relative angular velocity
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
dFloat relOmega = (omega0 - omega1) % matrix0.m_front;
dFloat relAccel = m_angularAccel - m_angularDamp * relOmega;
// if the motor capability is on, then set angular acceleration with zero angular correction
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
// override the angular acceleration for this Jacobian to the desired acceleration
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
}
void CustomSlider::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 two orthonormal axis direction perpendicular to the motion
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]);
// three rows to restrict rotation around around the parent coordinate system
dFloat sinAngle;
dFloat cosAngle;
CalculatePitchAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_front[0]);
CalculateYawAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_up[0]);
CalculateRollAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_right[0]);
// calculate position and speed
dVector veloc0(0.0f, 0.0f, 0.0f, 0.0f);
dVector veloc1(0.0f, 0.0f, 0.0f, 0.0f);
if (m_body0) {
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
}
if (m_body1) {
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
}
m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
m_speed = (veloc0 - veloc1) % matrix1.m_front;
// if limit are enable ...
m_hitLimitOnLastUpdate = false;
if (m_limitsOn) {
if (m_posit < m_minDist) {
// indicate that this row hit a limit
m_hitLimitOnLastUpdate = true;
// get a point along the up vector and set a constraint
const dVector& p0 = matrix0.m_posit;
dVector p1 (p0 + matrix0.m_front.Scale (m_minDist - m_posit));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
} else if (m_posit > m_maxDist) {
// indicate that this row hit a limit
m_hitLimitOnLastUpdate = true;
// get a point along the up vector and set a constraint
const dVector& p0 = matrix0.m_posit;
dVector p1 (p0 + matrix0.m_front.Scale (m_maxDist - m_posit));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else {
/*
// uncomment this for a slider with friction
// take any point on body0 (origin)
const dVector& p0 = matrix0.m_posit;
dVector veloc0;
dVector veloc1;
dVector omega1;
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
NewtonBodyGetOmega(m_body1, &omega1[0]);
// this assumes the origin of the bodies the matrix pivot are the same
veloc1 += omega1 * (matrix1.m_posit - p0);
dFloat relAccel;
relAccel = ((veloc1 - veloc0) % matrix0.m_front) / timestep;
#define MaxFriction 10.0f
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction (m_joint, -MaxFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, MaxFriction);
*/
}
}
}
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]);
// get the pin fixed to the first body
const dVector& dir0 = matrix0.m_front;
// get the pin fixed to the second body
const dVector& dir1 = matrix1.m_up;
// construct an orthogonal coordinate system with these two vectors
dVector dir2(dir0 * dir1);
dir2 = dir2.Scale(1.0f / dSqrt(dir2 % dir2));
dVector dir3(dir1 * dir2);
dFloat sinAngle = ((dir3 * dir0) % dir2);
dFloat cosAngle = dir3 % dir0;
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &dir2[0]);
dFloat sinAngle_0;
dFloat cosAngle_0;
CalculatePitchAngle(matrix0, matrix1, sinAngle_0, cosAngle_0);
dFloat angle0 = -m_curJointAngle_0.Update(cosAngle_0, sinAngle_0);
dFloat sinAngle_1;
dFloat cosAngle_1;
CalculateYawAngle(matrix0, matrix1, 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);
}
}
