void solveIK(AwPoint startJointPos,
AwPoint midJointPos,
AwPoint effectorPos,
AwPoint handlePos,
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.sub(startJointPos);
// vector from midJoint to effector
AwVector vector2 = effectorPos.sub(midJointPos);
// vector from startJoint to handle
AwVector vectorH = handlePos.sub(startJointPos);
// vector from startJoint to effector
AwVector vectorE = effectorPos.sub(startJointPos);
// lengths of those vectors
double length1 = vector1.length();
double length2 = vector2.length();
double lengthH = vectorH.length();
double d = vector1.mul(vectorE) / vectorE.mul(vectorE);
AwVector vectorO = vector1.sub(vectorE.mul(d));
//////////////////////////////////////////////////////////////////
// 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.crossProduct(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 = Math.Acos(cos_theta);
AwQuaternion q12 = new AwQuaternion(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.add(vector2);
// quaternion for rotating the effector onto the handle
AwQuaternion qEH = new AwQuaternion(vectorE, vectorH);
// calculate qNP which solves for the rotate plane
//////////////////////////////////////////////////////////////////
// vector1 with quaternion qEH applied
vector1 = vector1.rotateBy(qEH);
if (vector1.isParallel(vectorH, AwMath.kDoubleEpsilon))
{
// singular case, use orthogonal component instead
vector1 = vectorO.rotateBy(qEH);
}
AwQuaternion qNP = new AwQuaternion();
if (!poleVector.isParallel(vectorH, AwMath.kDoubleEpsilon) && (lengthHsquared != 0))
{
double temp = poleVector.mul(vectorH) / lengthHsquared;
AwVector vectorN = vector1.sub(vectorH.mul(temp));
AwVector vectorP = poleVector.sub(vectorH.mul(vector1.mul(vectorH) / lengthHsquared));
double dotNP = (vectorN.mul(vectorP)) / (vectorN.length() * vectorP.length());
if (Math.Abs(dotNP + 1.0) < kEpsilon)
{
// singular case, rotate halfway around vectorH
AwQuaternion qNP1 = new AwQuaternion(AwMath.kPi, vectorH);
qNP = qNP1;
}
else
{
AwQuaternion qNP2 = new AwQuaternion(vectorN, vectorP);
qNP = qNP2;
}
}
//////////////////////////////////////////////////////////////////
// calculate qTwist which adds the twist
//////////////////////////////////////////////////////////////////
AwQuaternion qTwist = new AwQuaternion(twistValue, vectorH);
// quaternion for the mid joint
qMid = q12;
// concatenate the quaternions for the start joint
AwQuaternion qTemp = qEH.mul(qNP);
qStart = qTemp.mul(qTwist);
}
// This is method that actually computes the IK solution.
//
void solveIK( AwPoint startJointPos,
AwPoint midJointPos,
AwPoint effectorPos,
AwPoint handlePos,
AwVector poleVector,
double twistValue,
AwQuaternion qStart,
AwQuaternion qMid)
{
// vector from startJoint to midJoint
AwVector vector1 = midJointPos.sub(startJointPos);
// vector from midJoint to effector
AwVector vector2 = effectorPos.sub(midJointPos);
// vector from startJoint to handle
AwVector vectorH = handlePos.sub(startJointPos);
// vector from startJoint to effector
AwVector vectorE = effectorPos.sub(startJointPos);
// lengths of those vectors
double length1 = vector1.length();
double length2 = vector2.length();
double lengthH = vectorH.length();
double d = vector1.mul(vectorE) / vectorE.mul(vectorE);
AwVector vectorO = vector1.sub(vectorE.mul(d));
//////////////////////////////////////////////////////////////////
// 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.crossProduct(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 = Math.Acos(cos_theta);
AwQuaternion q12 = new AwQuaternion(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.add(vector2);
// quaternion for rotating the effector onto the handle
AwQuaternion qEH = new AwQuaternion(vectorE, vectorH);
// calculate qNP which solves for the rotate plane
//////////////////////////////////////////////////////////////////
// vector1 with quaternion qEH applied
vector1 = vector1.rotateBy(qEH);
if (vector1.isParallel(vectorH,AwMath.kDoubleEpsilon))
// singular case, use orthogonal component instead
vector1 = vectorO.rotateBy(qEH);
AwQuaternion qNP = new AwQuaternion();
if (!poleVector.isParallel(vectorH, AwMath.kDoubleEpsilon) && (lengthHsquared != 0))
{
double temp = poleVector.mul(vectorH) / lengthHsquared;
AwVector vectorN = vector1.sub(vectorH.mul(temp));
AwVector vectorP = poleVector.sub(vectorH.mul(vector1.mul(vectorH) / lengthHsquared));
double dotNP = (vectorN.mul(vectorP)) / (vectorN.length() * vectorP.length());
if (Math.Abs(dotNP + 1.0) < kEpsilon)
{
// singular case, rotate halfway around vectorH
AwQuaternion qNP1 = new AwQuaternion(AwMath.kPi, vectorH);
qNP = qNP1;
}
else
{
AwQuaternion qNP2 = new AwQuaternion(vectorN, vectorP);
qNP = qNP2;
}
}
//////////////////////////////////////////////////////////////////
// calculate qTwist which adds the twist
//////////////////////////////////////////////////////////////////
AwQuaternion qTwist = new AwQuaternion(twistValue, vectorH);
// quaternion for the mid joint
qMid = q12;
// concatenate the quaternions for the start joint
AwQuaternion qTemp = qEH.mul(qNP);
qStart = qTemp.mul(qTwist);
}