bool AwQuaternion::getAxisAngle(AwVector &axis, double &theta) const
//
// Description:
// This function converts this quaternion into a user understandable
// representation. That is, the quaternion is represented as a pivot
// vector 'axis' and a rotation 'theta' (in radians) about that pivot
// vector.
//
// Returns:
// true - angle != 0
// false - angle == 0 (uses arbitrary axis, if given axis not valid)
//
// Notes:
// If the identity unit quaternion is attempted to be converted to the
// pivot axis and angle representation it will be set to a zero degree
// rotation about the axis that was passed in. (Note that any axis will
// do, since an infinity of axis with rotation of zero satisfy the identity
// rotation.) If the axis is zero length, then an arbitrary axis will be
// chosen (z-axis).
//
{
bool result;
double inverseOfSinThetaByTwo, thetaExtended;
if (::equivalent(w, (double) 1.0)) {
theta = 0.0;
if (axis.length() < kDoubleEpsilon) {
// Passed in axis invalid, choose an arbitrary axis.
axis.set(0.0,0.0,1.0);
}
result = false;
}
else {
thetaExtended = acos(clamp(w,-1.0,1.0));
theta = thetaExtended * 2.0;
inverseOfSinThetaByTwo = 1.0 / sin(thetaExtended);
axis.x = x * inverseOfSinThetaByTwo;
axis.y = y * inverseOfSinThetaByTwo;
axis.z = z * inverseOfSinThetaByTwo;
result = true;
}
return result;
}
AwQuaternion::AwQuaternion(const AwVector &a, const AwVector &b)
//
// Description:
// This constructor creates a new quaternion that will rotate vector
// a into vector b about their mutually perpendicular axis. (if one
// exists)
//
: w(1.0), x(0.0), y(0.0), z(0.0)
{
double factor = a.length() * b.length();
if (fabs(factor) > kFloatEpsilon) {
// Vectors have length > 0
AwVector pivotVector;
double dot = a.dotProduct(b) / factor;
double theta = acos(clamp(dot, -1.0, 1.0));
pivotVector = a^b;
if (dot < 0.0 && pivotVector.length() < kFloatEpsilon) {
// Vectors parallel and opposite direction, therefore a rotation
// of 180 degrees about any vector perpendicular to this vector
// will rotate vector a onto vector b.
//
// The following guarantees the dot-product will be 0.0.
//
size_t dominantIndex = a.dominantAxis();
pivotVector[dominantIndex] = -a[(dominantIndex+1)%3];
pivotVector[(dominantIndex+1)%3] = a[dominantIndex];
pivotVector[(dominantIndex+2)%3] = 0.0;
}
setAxisAngle(pivotVector, theta);
}
}
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;
}
