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