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