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