TString MillePedeTrees::Alpha(const TString &tree, bool betaMpiPpi) const { // a la AlignmentTransformations::eulerAngles // double rot[3][3]; // rot[0][0] = Rot[0]; // Rot from tree // rot[0][1] = Rot[1]; // rot[0][2] = Rot[2]; // rot[1][0] = Rot[3]; // rot[1][1] = Rot[4]; // rot[1][2] = Rot[5]; // rot[2][0] = Rot[6]; // rot[2][1] = Rot[7]; // rot[2][2] = Rot[8]; TString euler1("TMath::ASin(Rot[6])"); if (!betaMpiPpi) euler1.Prepend("TMath::Pi() - "); TString euler0("TMath::ATan(-Rot[7]/Rot[8]) + (TMath::Pi() * (TMath::Cos("); euler0 += euler1; euler0 += ") * Rot[8] <= 0))"; TString result(Form("(TMath::Abs(Rot[6] - 1.0) >= 1.e-6) * (%s)", euler0.Data())); result.ReplaceAll("Rot[", tree + "Rot["); return result; // if (TMath::Abs(Rot[6] - 1.0) > 1.e-6) { // If angle1 is not +-PI/2 // if (flag == 0) // assuming -PI/2 < angle1 < PI/2 // euler[1] = TMath::ASin(Rot[6]); // New beta sign convention // else // assuming angle1 < -PI/2 or angle1 >PI/2 // euler[1] = TMath::Pi() - TMath::ASin(Rot[6]); // New beta sign convention // if (TMath::Cos(euler[1]) * Rot[8] > 0) // euler[0] = TMath::ATan(-Rot[7]/Rot[8]); // else // euler[0] = TMath::ATan(-Rot[7]/Rot[8]) + TMath::Pi(); // if (TMath::Cos(euler[1]) * Rot[0] > 0) // euler[2] = TMath::ATan(-Rot[3]/Rot[0]); // else // euler[2] = TMath::ATan(-Rot[3]/Rot[0]) + TMath::Pi(); // } else { // if angle1 == +-PI/2 // euler[1] = TMath::PiOver2(); // chose positve Solution // if(Rot[8] > 0) { // euler[2] = TMath::ATan(Rot[5]/Rot[4]); // euler[0] = 0; // } // } }
void Custom6DOF::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); // add the linear limits const dVector& p0 = matrix0.m_posit; const dVector& p1 = matrix1.m_posit; dVector dp (p0 - p1); for (int i = 0; i < 3; i ++) { if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) { NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); } else { // it is a limited linear dof, check if it pass the limits dFloat dist = dp.DotProduct3(matrix1[i]); if (dist > m_maxLinearLimits[i]) { dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i])); // clamp the error, so the not too much energy is added when constraint violation occurs dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]); if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) { q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION); } NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the object to return but not to kick going forward NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f); } else if (dist < m_minLinearLimits[i]) { dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i])); // clamp the error, so the not too much energy is added when constraint violation occurs dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]); if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) { q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION); } NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the object to return but not to kick going forward NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } } } dVector euler0(0.0f); dVector euler1(0.0f); dMatrix localMatrix (matrix0 * matrix1.Inverse()); localMatrix.GetEulerAngles(euler0, euler1); AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch); AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch); if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) { euler0 = euler1; } dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f); //dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f, euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f)); bool limitViolation = false; for (int i = 0; i < 3; i ++) { if (euler[i] < m_minAngularLimits[i]) { limitViolation = true; euler[i] = m_minAngularLimits[i]; } else if (euler[i] > m_maxAngularLimits[i]) { limitViolation = true; euler[i] = m_maxAngularLimits[i]; } } if (limitViolation) { //dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle)); dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2])); dMatrix baseMatrix (p0y0r0 * matrix1); dMatrix rotation (matrix0.Inverse() * baseMatrix); dQuaternion quat (rotation); if (quat.m_q0 > dFloat (0.99995f)) { //dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH)); //dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH)); //NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]); //NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f); //dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH)); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]); } else { dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f))); dVector q0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH)); dVector q1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH))); NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &basis[2][0]); NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f); //dVector q0 (matrix0[3] + basis[0].Scale (MIN_JOINT_PIN_LENGTH)); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[1][0]); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[2][0]); } } }