double ik2Bsolver::twistFromHandle(const MDagPath &handlePath)
//
// This method returns the twist of the IK handle.
//
{
MStatus stat;
MFnIkHandle handleFn(handlePath, &stat);
MPlug twistPlug = handleFn.findPlug("twist");
double twistValue;
twistPlug.getValue(twistValue);
return twistValue;
}
AwVector ik2Bsolver::poleVectorFromHandle(const MDagPath &handlePath)
//
// This method returns the pole vector of the IK handle.
//
{
MStatus stat;
MFnIkHandle handleFn(handlePath, &stat);
MPlug pvxPlug = handleFn.findPlug("pvx");
MPlug pvyPlug = handleFn.findPlug("pvy");
MPlug pvzPlug = handleFn.findPlug("pvz");
double pvxValue, pvyValue, pvzValue;
pvxPlug.getValue(pvxValue);
pvyPlug.getValue(pvyValue);
pvzPlug.getValue(pvzValue);
AwVector poleVector(pvxValue, pvyValue, pvzValue);
return poleVector;
}
MStatus ik2Bsolver::doSolve()
//
// This is the doSolve method which calls solveIK.
//
{
MStatus stat;
// Handle Group
//
MIkHandleGroup * handle_group = handleGroup();
if (NULL == handle_group) {
return MS::kFailure;
}
// Handle
//
// For single chain types of solvers, get the 0th handle.
// Single chain solvers are solvers which act on one handle only,
// i.e. the handle group for a single chain solver
// has only one handle
//
MObject handle = handle_group->handle(0);
MDagPath handlePath = MDagPath::getAPathTo(handle);
MFnIkHandle handleFn(handlePath, &stat);
// Effector
//
MDagPath effectorPath;
handleFn.getEffector(effectorPath);
MFnIkEffector effectorFn(effectorPath);
// Mid Joint
//
effectorPath.pop();
MFnIkJoint midJointFn(effectorPath);
// Start Joint
//
MDagPath startJointPath;
handleFn.getStartJoint(startJointPath);
MFnIkJoint startJointFn(startJointPath);
// Preferred angles
//
double startJointPrefAngle[3];
double midJointPrefAngle[3];
startJointFn.getPreferedAngle(startJointPrefAngle);
midJointFn.getPreferedAngle(midJointPrefAngle);
// Set to preferred angles
//
startJointFn.setRotation(startJointPrefAngle,
startJointFn.rotationOrder());
midJointFn.setRotation(midJointPrefAngle,
midJointFn.rotationOrder());
AwPoint handlePos = handleFn.rotatePivot(MSpace::kWorld);
AwPoint effectorPos = effectorFn.rotatePivot(MSpace::kWorld);
AwPoint midJointPos = midJointFn.rotatePivot(MSpace::kWorld);
AwPoint startJointPos = startJointFn.rotatePivot(MSpace::kWorld);
AwVector poleVector = poleVectorFromHandle(handlePath);
poleVector *= handlePath.exclusiveMatrix();
double twistValue = twistFromHandle(handlePath);
AwQuaternion qStart, qMid;
solveIK(startJointPos,
midJointPos,
effectorPos,
handlePos,
poleVector,
twistValue,
qStart,
qMid);
midJointFn.rotateBy(qMid, MSpace::kWorld);
startJointFn.rotateBy(qStart, MSpace::kWorld);
return MS::kSuccess;
}
MStatus ik2Bsolver::doSolve()
//
// This is the doSolve method which calls solveIK.
//
{
MStatus stat;
// Handle Group
//
MIkHandleGroup * handle_group = handleGroup();
if (NULL == handle_group) {
return MS::kFailure;
}
// Handle
//
// For single chain types of solvers, get the 0th handle.
// Single chain solvers are solvers which act on one handle only,
// i.e. the handle group for a single chain solver
// has only one handle
//
MObject handle = handle_group->handle(0);
MDagPath handlePath = MDagPath::getAPathTo(handle);
MFnIkHandle handleFn(handlePath, &stat);
// Effector
//
MDagPath effectorPath;
handleFn.getEffector(effectorPath);
// MGlobal::displayInfo(effectorPath.fullPathName());
MFnIkEffector effectorFn(effectorPath);
// Mid Joint
//
effectorPath.pop();
MFnIkJoint midJointFn(effectorPath);
// End Joint
//
MDagPath endJointPath;
bool hasEndJ = findFirstJointChild(effectorPath, endJointPath);
// if(hasEndJ) MGlobal::displayInfo(endJointPath.fullPathName());
MFnIkJoint endJointFn(endJointPath);
// Start Joint
//
MDagPath startJointPath;
handleFn.getStartJoint(startJointPath);
MFnIkJoint startJointFn(startJointPath);
// Preferred angles
//
double startJointPrefAngle[3];
double midJointPrefAngle[3];
startJointFn.getPreferedAngle(startJointPrefAngle);
midJointFn.getPreferedAngle(midJointPrefAngle);
// Set to preferred angles
//
startJointFn.setRotation(startJointPrefAngle,
startJointFn.rotationOrder());
midJointFn.setRotation(midJointPrefAngle,
midJointFn.rotationOrder());
MPoint handlePos = handleFn.rotatePivot(MSpace::kWorld);
MPoint effectorPos = effectorFn.rotatePivot(MSpace::kWorld);
MPoint midJointPos = midJointFn.rotatePivot(MSpace::kWorld);
MPoint startJointPos = startJointFn.rotatePivot(MSpace::kWorld);
MVector poleVector = poleVectorFromHandle(handlePath);
poleVector *= handlePath.exclusiveMatrix();
double twistValue = twistFromHandle(handlePath);
MObject thisNode = thisMObject();
MVector localEndJointT = endJointFn.getTranslation(MSpace::kTransform);
MVector worldEndJointT = endJointFn.rotatePivot(MSpace::kWorld) - midJointPos;
double scaling = worldEndJointT.length() / ( localEndJointT.length() + 1e-6);
// MGlobal::displayInfo(MString(" scaling ")+scaling);
// get rest length
//
double restL1, restL2;
MPlug(thisNode, arestLength1).getValue(restL1);
MPlug(thisNode, arestLength2).getValue(restL2);
// get soft distance
//
MPlug plug( thisNode, asoftDistance );
double softD = 0.0;
plug.getValue(softD);
restL1 *= scaling;
restL2 *= scaling;
softD *= scaling;
// get max stretching
double maxStretching = 0.0;
MPlug(thisNode, amaxStretching).getValue(maxStretching);
MQuaternion qStart, qMid;
double stretching = 0.0;
solveIK(startJointPos,
midJointPos,
effectorPos,
handlePos,
poleVector,
twistValue,
qStart,
qMid,
softD,
restL1, restL2,
stretching);
midJointFn.rotateBy(qMid, MSpace::kWorld);
startJointFn.rotateBy(qStart, MSpace::kWorld);
midJointFn.setTranslation(MVector(restL1 / scaling, 0.0, 0.0), MSpace::kTransform);
endJointFn.setTranslation(MVector(restL2 / scaling, 0.0, 0.0), MSpace::kTransform);
// if(stretching > maxStretching) stretching = maxStretching;
if(maxStretching > 0.0) {
MVector vstretch(stretching* 0.5 / scaling, 0.0, 0.0);
midJointFn.translateBy(vstretch, MSpace::kTransform);
endJointFn.translateBy(vstretch, MSpace::kTransform);
}
return MS::kSuccess;
}
