void solveIK(const AwPoint &startJointPos, const AwPoint &midJointPos, const AwPoint &effectorPos, const AwPoint &handlePos, const AwVector &poleVector, double twistValue, AwQuaternion &qStart, AwQuaternion &qMid) // // This is method that actually computes the IK solution. // { // vector from startJoint to midJoint AwVector vector1 = midJointPos - startJointPos; // vector from midJoint to effector AwVector vector2 = effectorPos - midJointPos; // vector from startJoint to handle AwVector vectorH = handlePos - startJointPos; // vector from startJoint to effector AwVector vectorE = effectorPos - startJointPos; // lengths of those vectors double length1 = vector1.length(); double length2 = vector2.length(); double lengthH = vectorH.length(); // component of the vector1 orthogonal to the vectorE AwVector vectorO = vector1 - vectorE*((vector1*vectorE)/(vectorE*vectorE)); ////////////////////////////////////////////////////////////////// // calculate q12 which solves for the midJoint rotation ////////////////////////////////////////////////////////////////// // angle between vector1 and vector2 double vectorAngle12 = vector1.angle(vector2); // vector orthogonal to vector1 and 2 AwVector vectorCross12 = vector1^vector2; double lengthHsquared = lengthH*lengthH; // angle for arm extension double cos_theta = (lengthHsquared - length1*length1 - length2*length2) /(2*length1*length2); if (cos_theta > 1) cos_theta = 1; else if (cos_theta < -1) cos_theta = -1; double theta = acos(cos_theta); // quaternion for arm extension AwQuaternion q12(theta - vectorAngle12, vectorCross12); ////////////////////////////////////////////////////////////////// // calculate qEH which solves for effector rotating onto the handle ////////////////////////////////////////////////////////////////// // vector2 with quaternion q12 applied vector2 = vector2.rotateBy(q12); // vectorE with quaternion q12 applied vectorE = vector1 + vector2; // quaternion for rotating the effector onto the handle AwQuaternion qEH(vectorE, vectorH); ////////////////////////////////////////////////////////////////// // calculate qNP which solves for the rotate plane ////////////////////////////////////////////////////////////////// // vector1 with quaternion qEH applied vector1 = vector1.rotateBy(qEH); if (vector1.isParallel(vectorH)) // singular case, use orthogonal component instead vector1 = vectorO.rotateBy(qEH); // quaternion for rotate plane AwQuaternion qNP; if (!poleVector.isParallel(vectorH) && (lengthHsquared != 0)) { // component of vector1 orthogonal to vectorH AwVector vectorN = vector1 - vectorH*((vector1*vectorH)/lengthHsquared); // component of pole vector orthogonal to vectorH AwVector vectorP = poleVector - vectorH*((poleVector*vectorH)/lengthHsquared); double dotNP = (vectorN*vectorP)/(vectorN.length()*vectorP.length()); if (absoluteValue(dotNP + 1.0) < kEpsilon) { // singular case, rotate halfway around vectorH AwQuaternion qNP1(kPi, vectorH); qNP = qNP1; } else { AwQuaternion qNP2(vectorN, vectorP); qNP = qNP2; } } ////////////////////////////////////////////////////////////////// // calculate qTwist which adds the twist ////////////////////////////////////////////////////////////////// AwQuaternion qTwist(twistValue, vectorH); // quaternion for the mid joint qMid = q12; // concatenate the quaternions for the start joint qStart = qEH*qNP*qTwist; }
void ik2Bsolver::solveIK(const MPoint &startJointPos, const MPoint &midJointPos, const MPoint &effectorPos, const MPoint &handlePos, const MVector &poleVector, double twistValue, MQuaternion &qStart, MQuaternion &qMid, const double & softDistance, const double & restLength1, const double & restLength2, double & stretching) // // This is method that actually computes the IK solution. // { // vector from startJoint to midJoint MVector vector1 = midJointPos - startJointPos; vector1 = vector1.normal() * restLength1; // vector from midJoint to effector MVector vector2 = effectorPos - midJointPos; vector2 = vector2.normal() * restLength2; // vector from startJoint to handle MVector vectorH = handlePos - startJointPos; // vector from startJoint to effector MVector vectorE = effectorPos - startJointPos; // lengths of those vectors const double length1 = restLength1; const double length2 = restLength2; const double lengthH = vectorH.length(); // component of the vector1 orthogonal to the vectorE const MVector vectorO = vector1 - vectorE*((vector1*vectorE)/(vectorE*vectorE)); ////////////////////////////////////////////////////////////////// // calculate q12 which solves for the midJoint rotation ////////////////////////////////////////////////////////////////// // angle between vector1 and vector2 const double vectorAngle12 = vector1.angle(vector2); // vector orthogonal to vector1 and 2 const MVector vectorCross12 = vector1^vector2; const double lengthHsquared = lengthH * lengthH; double weight = 0.0; double slowLH = lengthH;// / (1.0 + (6.8 - softDistance) / (restLength1 + restLength2)); const double da = restLength1 + restLength2 - softDistance; if(slowLH > da) { float s = (slowLH - da) / softDistance; if(s> 1.f) s= 1.f; Vector3F pt = m_herm.interpolate(s); // MGlobal::displayInfo(MString("herm ")+ pt.y); // approading l1+l2 slower // weight = 1.0 - exp(-(slowLH - da) / softDistance * 6.98); weight = pt.y * weight + (1.f - pt.y) * s; slowLH = da + softDistance * weight; // MGlobal::displayInfo(MString("wei ")+weight); } // // 1 // / \ // l1 / \ l2 // / \ ---l1---1 ---l2--- // 0 ------ l ------ 2 0 ------ l ------- 2 // // angle for arm extension double cos_theta = (slowLH * slowLH - length1*length1 - length2*length2) / (2*length1*length2); if (cos_theta > 1) cos_theta = 1; else if (cos_theta < -1) cos_theta = -1; const double theta = acos(cos_theta); // quaternion for arm extension MQuaternion q12(theta - vectorAngle12, vectorCross12); ////////////////////////////////////////////////////////////////// // calculate qEH which solves for effector rotating onto the handle ////////////////////////////////////////////////////////////////// // vector2 with quaternion q12 applied vector2 = vector2.rotateBy(q12); // vectorE with quaternion q12 applied vectorE = vector1 + vector2; // quaternion for rotating the effector onto the handle MQuaternion qEH(vectorE, vectorH); if(lengthH > vectorE.length()) { // MGlobal::displayInfo(MString("vle ")+(lengthH-vectorE.length())); stretching = (lengthH-vectorE.length()) * weight; } ////////////////////////////////////////////////////////////////// // calculate qNP which solves for the rotate plane ////////////////////////////////////////////////////////////////// // vector1 with quaternion qEH applied vector1 = vector1.rotateBy(qEH); if (vector1.isParallel(vectorH)) // singular case, use orthogonal component instead vector1 = vectorO.rotateBy(qEH); // quaternion for rotate plane MQuaternion qNP; if (!poleVector.isParallel(vectorH) && (lengthHsquared != 0)) { // component of vector1 orthogonal to vectorH MVector vectorN = vector1 - vectorH*((vector1*vectorH)/lengthHsquared); // component of pole vector orthogonal to vectorH MVector vectorP = poleVector - vectorH*((poleVector*vectorH)/lengthHsquared); double dotNP = (vectorN*vectorP)/(vectorN.length()*vectorP.length()); if (absoluteValue(dotNP + 1.0) < kEpsilon) { // singular case, rotate halfway around vectorH MQuaternion qNP1(kPi, vectorH); qNP = qNP1; } else { MQuaternion qNP2(vectorN, vectorP); qNP = qNP2; } } ////////////////////////////////////////////////////////////////// // calculate qTwist which adds the twist ////////////////////////////////////////////////////////////////// MQuaternion qTwist(twistValue, vectorH); // quaternion for the mid joint qMid = q12; // concatenate the quaternions for the start joint qStart = qEH*qNP*qTwist; }