public AwVector normal()
{
AwVector tmp = new AwVector(this);
tmp.normalize();
return(tmp);
}
public AwQuaternion setAxisAngle(AwVector axis, double theta)
{
double sumOfSquares =
(double)axis.x * axis.x +
(double)axis.y * axis.y +
(double)axis.z * axis.z;
if (sumOfSquares <= AwMath.kDoubleEpsilon)
{
w = 1.0;
x = 0.0;
y = 0.0;
z = 0.0;
}
else
{
theta *= 0.5;
w = Math.Cos(theta);
double commonFactor = Math.Sin(theta);
if (!AwMath.equivalent(sumOfSquares, 1.0))
{
commonFactor /= Math.Sqrt(sumOfSquares);
}
x = commonFactor * (double)axis.x;
y = commonFactor * (double)axis.y;
z = commonFactor * (double)axis.z;
}
return(this);
}
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(double angle, AwVector axis)
{
w = 1.0;
x = 0.0;
y = 0.0;
z = 0.0;
setAxisAngle(axis, angle);
}
public AwQuaternion(double angle, AwVector axis)
{
w = 1.0;
x = 0.0;
y = 0.0;
z = 0.0;
setAxisAngle(axis, angle);
}
public AwVector add(AwVector v)
{
AwVector tmp = new AwVector(this);
tmp.x = tmp.x + v.x;
tmp.y = tmp.y + v.y;
tmp.z = tmp.z + v.z;
return tmp;
}
public AwVector mulMatrix(AwMatrix matrix)
{
AwVector tmp = new AwVector();
tmp.x = x * matrix.getIndex(0, 0) + y * matrix.getIndex(1, 0) + z * matrix.getIndex(2, 0);
tmp.y = x * matrix.getIndex(0, 1) + y * matrix.getIndex(1, 1) + z * matrix.getIndex(2, 1);
tmp.z = x * matrix.getIndex(0, 2) + y * matrix.getIndex(1, 2) + z * matrix.getIndex(2, 2);
return(tmp);
}
public AwVector sub(AwVector v)
{
AwVector tmp = new AwVector(this);
tmp.x = tmp.x - v.x;
tmp.y = tmp.y - v.y;
tmp.z = tmp.z - v.z;
return(tmp);
}
public AwVector add(AwVector v)
{
AwVector tmp = new AwVector(this);
tmp.x = tmp.x + v.x;
tmp.y = tmp.y + v.y;
tmp.z = tmp.z + v.z;
return(tmp);
}
public AwVector mul(double s)
{
AwVector tmp = new AwVector(this);
tmp.x = tmp.x * s;
tmp.y = tmp.y * s;
tmp.z = tmp.z * s;
return(tmp);
}
public bool isParallel(AwVector otherVector, double tolerance)
{
AwVector v1, v2;
v1 = normal();
v2 = otherVector.normal();
double dotPrd = v1.dotProduct(v2);
return(AwMath.equivalent(Math.Abs(dotPrd), (double)1.0, tolerance));
}
public AwVector rotateBy(AwQuaternion q)
{
double rw = -q.x * x - q.y * y - q.z * z;
double rx = q.w * x + q.y * z - q.z * y;
double ry = q.w * y + q.z * x - q.x * z;
double rz = q.w * z + q.x * y - q.y * x;
AwVector v = new AwVector(-rw * q.x + rx * q.w - ry * q.z + rz * q.y,
-rw * q.y + ry * q.w - rz * q.x + rx * q.z,
-rw * q.z + rz * q.w - rx * q.y + ry * q.x);
return(v);
}
public double angle(AwVector vec)
{
double cosine = normal().dotProduct(vec.normal());
double angle;
if (cosine >= 1.0)
angle = 0.0;
else if (cosine <= -1.0)
angle = AwMath.kPi;
else
angle = Math.Acos(cosine);
return angle;
}
AwVector poleVectorFromHandle(MDagPath handlePath)
{
MFnIkHandle handleFn = new MFnIkHandle(handlePath);
MPlug pvxPlug = handleFn.findPlug("pvx");
MPlug pvyPlug = handleFn.findPlug("pvy");
MPlug pvzPlug = handleFn.findPlug("pvz");
double pvxValue, pvyValue, pvzValue;
pvxValue = pvyValue = pvzValue = 0;
pvxPlug.getValue(pvxValue);
pvyPlug.getValue(pvyValue);
pvzPlug.getValue(pvzValue);
AwVector poleVector = new AwVector(pvxValue, pvyValue, pvzValue);
return(poleVector);
}
public double angle(AwVector vec)
{
double cosine = normal().dotProduct(vec.normal());
double angle;
if (cosine >= 1.0)
{
angle = 0.0;
}
else if (cosine <= -1.0)
{
angle = AwMath.kPi;
}
else
{
angle = Math.Acos(cosine);
}
return(angle);
}
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 double dotProduct(AwVector v)
{
return(x * v.x + y * v.y + z * v.z);
}
public AwVector(AwVector v)
{
x = v.x;
y = v.y;
z = v.z;
}
public AwVector(AwVector v)
{
x = v.x;
y = v.y;
z = v.z;
}
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 AwVector normal()
{
AwVector tmp = new AwVector(this);
tmp.normalize();
return tmp;
}
// 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);
}
AwVector poleVectorFromHandle( MDagPath handlePath)
{
MFnIkHandle handleFn = new MFnIkHandle(handlePath);
MPlug pvxPlug = handleFn.findPlug("pvx");
MPlug pvyPlug = handleFn.findPlug("pvy");
MPlug pvzPlug = handleFn.findPlug("pvz");
double pvxValue, pvyValue, pvzValue;
pvxValue=pvyValue=pvzValue=0;
pvxPlug.getValue(pvxValue);
pvyPlug.getValue(pvyValue);
pvzPlug.getValue(pvzValue);
AwVector poleVector = new AwVector(pvxValue, pvyValue, pvzValue);
return poleVector;
}
public AwVector crossProduct( AwVector r)
{
return new AwVector(y*r.z - z*r.y, z*r.x - x*r.z, x*r.y - y*r.x);
}
public AwVector rotateBy( AwQuaternion q)
{
double rw = -q.x * x - q.y * y - q.z * z;
double rx = q.w * x + q.y * z - q.z * y;
double ry = q.w * y + q.z * x - q.x * z;
double rz = q.w * z + q.x * y - q.y * x;
AwVector v = new AwVector(-rw * q.x + rx * q.w - ry * q.z + rz * q.y,
-rw * q.y + ry * q.w - rz * q.x + rx * q.z,
-rw * q.z + rz * q.w - rx * q.y + ry * q.x);
return v;
}
public AwVector crossProduct(AwVector r)
{
return(new AwVector(y * r.z - z * r.y, z * r.x - x * r.z, x * r.y - y * r.x));
}
public double dotProduct(AwVector v)
{
return x * v.x + y * v.y + z * v.z;
}
public AwVector mulMatrix(AwMatrix matrix)
{
AwVector tmp = new AwVector();
tmp.x = x * matrix.getIndex(0, 0) + y * matrix.getIndex(1, 0) + z * matrix.getIndex(2, 0);
tmp.y = x * matrix.getIndex(0, 1) + y * matrix.getIndex(1, 1) + z * matrix.getIndex(2, 1);
tmp.z = x * matrix.getIndex(0, 2) + y * matrix.getIndex(1, 2) + z * matrix.getIndex(2, 2);
return tmp;
}
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);
}
public AwVector mul(double s)
{
AwVector tmp = new AwVector(this);
tmp.x = tmp.x * s;
tmp.y = tmp.y * s;
tmp.z = tmp.z * s;
return tmp;
}
public override void doSolve()
{
MIkHandleGroup handle_group = handleGroup;
if (handle_group == null)
{
throw new InvalidOperationException("Invalid handle group");
}
MObject handle = handle_group.handle(0);
MDagPath handlePath = MDagPath.getAPathTo(handle);
MFnIkHandle handleFn = new MFnIkHandle(handlePath);
//Effector
//
MDagPath effectorPath = new MDagPath();
handleFn.getEffector(effectorPath);
MFnIkEffector effectorFn = new MFnIkEffector(effectorPath);
effectorPath.pop();
MFnIkJoint midJoinFn = new MFnIkJoint(effectorPath);
// Start Joint
//
MDagPath startJointPath = new MDagPath();
handleFn.getStartJoint(startJointPath);
MFnIkJoint startJointFn = new MFnIkJoint(startJointPath);
// Preferred angles
//
double [] startJointPrefAngle = new double[3];
double[] midJointPrefAngle = new double[3];
startJointFn.getPreferedAngle(startJointPrefAngle);
midJoinFn.getPreferedAngle(midJointPrefAngle);
// Set to preferred angles
//
startJointFn.setRotation(startJointPrefAngle, startJointFn.rotationOrder);
midJoinFn.setRotation(midJointPrefAngle, midJoinFn.rotationOrder);
MPoint handlePos = handleFn.rotatePivot(MSpace.Space.kWorld);
AwPoint awHandlePos = new AwPoint(handlePos.x, handlePos.y, handlePos.z, handlePos.w);
MPoint effectorPos = effectorFn.rotatePivot(MSpace.Space.kWorld);
AwPoint awEffectorPos = new AwPoint(effectorPos.x, effectorPos.y, effectorPos.z, effectorPos.w);
MPoint midJoinPos = midJoinFn.rotatePivot(MSpace.Space.kWorld);
AwPoint awMidJoinPos = new AwPoint(midJoinPos.x, midJoinPos.y, midJoinPos.z, midJoinPos.w);
MPoint startJointPos = startJointFn.rotatePivot(MSpace.Space.kWorld);
AwPoint awStartJointPos = new AwPoint(startJointPos.x, startJointPos.y, startJointPos.z, startJointPos.w);
AwVector poleVector = poleVectorFromHandle(handlePath);
MMatrix m = handlePath.exclusiveMatrix;
AwMatrix awM = new AwMatrix();
awM.setMatrix(m);
poleVector = poleVector.mulMatrix(awM);
double twistValue = twistFromHandle(handlePath);
AwQuaternion qStart = new AwQuaternion();
AwQuaternion qMid = new AwQuaternion();
solveIK(awStartJointPos, awMidJoinPos, awEffectorPos, awHandlePos,
poleVector, twistValue, qStart, qMid);
MQuaternion mid = new MQuaternion(qMid.x, qMid.y, qMid.z, qMid.w);
MQuaternion start = new MQuaternion(qStart.x, qStart.y, qStart.z, qStart.w);
midJoinFn.rotateBy(mid, MSpace.Space.kWorld);
startJointFn.rotateBy(start, MSpace.Space.kWorld);
return;
}
public double mul(AwVector v)
{
return dotProduct(v);
}
public double mul(AwVector v)
{
return(dotProduct(v));
}
public bool isParallel( AwVector otherVector, double tolerance)
{
AwVector v1, v2;
v1 = normal();
v2 = otherVector.normal();
double dotPrd = v1.dotProduct(v2);
return (AwMath.equivalent( Math.Abs(dotPrd), (double) 1.0, tolerance));
}
public AwQuaternion setAxisAngle(AwVector axis, double theta)
{
double sumOfSquares =
(double) axis.x * axis.x +
(double) axis.y * axis.y +
(double) axis.z * axis.z;
if (sumOfSquares <= AwMath.kDoubleEpsilon)
{
w = 1.0;
x = 0.0;
y = 0.0;
z = 0.0;
}
else
{
theta *= 0.5;
w = Math.Cos(theta);
double commonFactor = Math.Sin(theta);
if (!AwMath.equivalent(sumOfSquares, 1.0))
commonFactor /= Math.Sqrt(sumOfSquares);
x = commonFactor * (double) axis.x;
y = commonFactor * (double) axis.y;
z = commonFactor * (double) axis.z;
}
return this;
}
public AwVector sub(AwVector v)
{
AwVector tmp = new AwVector(this);
tmp.x = tmp.x - v.x;
tmp.y = tmp.y - v.y;
tmp.z = tmp.z - v.z;
return tmp;
}
