public bool getAxisAngle(AwVector axis, ref double theta) { bool result; double inverseOfSinThetaByTwo, thetaExtended; if (AwMath.equivalent(w, (double)1.0)) { theta = 0.0; if (axis.length() < AwMath.kDoubleEpsilon) { axis.set(0.0, 0.0, 1.0); } result = false; } else { thetaExtended = Math.Acos(AwMath.clamp(w, -1.0, 1.0)); theta = thetaExtended * 2.0; inverseOfSinThetaByTwo = 1.0 / Math.Sin(thetaExtended); axis.x = x * inverseOfSinThetaByTwo; axis.y = y * inverseOfSinThetaByTwo; axis.z = z * inverseOfSinThetaByTwo; result = true; } return(result); }
public AwQuaternion(AwVector a, AwVector b) { w = 1.0; x = 0.0; y = 0.0; z = 0.0; double factor = a.length() * b.length(); if (Math.Abs(factor) > AwMath.kFloatEpsilon) { // Vectors have length > 0 AwVector pivotVector = new AwVector(); double dot = a.dotProduct(b) / factor; double theta = Math.Acos(AwMath.clamp(dot, -1.0, 1.0)); pivotVector = a.crossProduct(b); if (dot < 0.0 && pivotVector.length() < AwMath.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. // uint dominantIndex = (uint)a.dominantAxis(); uint index = ( dominantIndex + 1) % 3; double value = -a.getIndex( index) ; pivotVector.setIndex(dominantIndex, value); pivotVector.setIndex((dominantIndex + 1) % 3, a.getIndex(dominantIndex)); pivotVector.setIndex((dominantIndex + 2) % 3, 0); } setAxisAngle(pivotVector, theta); } }
public AwQuaternion(AwVector a, AwVector b) { w = 1.0; x = 0.0; y = 0.0; z = 0.0; double factor = a.length() * b.length(); if (Math.Abs(factor) > AwMath.kFloatEpsilon) { // Vectors have length > 0 AwVector pivotVector = new AwVector(); double dot = a.dotProduct(b) / factor; double theta = Math.Acos(AwMath.clamp(dot, -1.0, 1.0)); pivotVector = a.crossProduct(b); if (dot < 0.0 && pivotVector.length() < AwMath.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. // uint dominantIndex = (uint)a.dominantAxis(); uint index = (dominantIndex + 1) % 3; double value = -a.getIndex(index); pivotVector.setIndex(dominantIndex, value); pivotVector.setIndex((dominantIndex + 1) % 3, a.getIndex(dominantIndex)); pivotVector.setIndex((dominantIndex + 2) % 3, 0); } setAxisAngle(pivotVector, theta); } }
public bool getAxisAngle(AwVector axis, ref double theta) { bool result; double inverseOfSinThetaByTwo, thetaExtended; if (AwMath.equivalent(w, (double) 1.0)) { theta = 0.0; if (axis.length() < AwMath.kDoubleEpsilon) { axis.set(0.0,0.0,1.0); } result = false; } else { thetaExtended = Math.Acos(AwMath.clamp(w,-1.0,1.0)); theta = thetaExtended * 2.0; inverseOfSinThetaByTwo = 1.0 / Math.Sin(thetaExtended); axis.x = x * inverseOfSinThetaByTwo; axis.y = y * inverseOfSinThetaByTwo; axis.z = z * inverseOfSinThetaByTwo; result = true; } return result; }
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); }