double twistFromHandle(MDagPath handlePath)
// This method returns the twist of the IK handle.
//
{
MFnIkHandle handleFn = new MFnIkHandle(handlePath);
MPlug twistPlug = handleFn.findPlug("twist");
double twistValue = 0.0;
twistPlug.getValue(twistValue);
return(twistValue);
}
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);
}
private void doSimpleSolver()
//
// Solve single joint in the x-y plane
//
// - first it calculates the angle between the handle and the end-effector.
// - then it determines which way to rotate the joint.
//
{
// Get the handle and create a function set for it
//
MIkHandleGroup handle_group = handleGroup;
if (null == handle_group) {
throw new System.InvalidOperationException("invalid handle group.");
}
MObject handle = handle_group.handle( 0 );
MDagPath handlePath = MDagPath.getAPathTo( handle );
MFnIkHandle fnHandle = new MFnIkHandle( handlePath );
// Get the position of the end_effector
//
MDagPath end_effector = new MDagPath();
fnHandle.getEffector(end_effector);
MFnTransform tran = new MFnTransform( end_effector );
MPoint effector_position = tran.rotatePivot( MSpace.Space.kWorld );
// Get the position of the handle
//
MPoint handle_position = fnHandle.rotatePivot( MSpace.Space.kWorld );
// Get the start joint position
//
MDagPath start_joint = new MDagPath();
fnHandle.getStartJoint( start_joint );
MFnTransform start_transform = new MFnTransform( start_joint );
MPoint start_position = start_transform.rotatePivot( MSpace.Space.kWorld );
// Calculate the rotation angle
//
MVector v1 = start_position.minus( effector_position );
MVector v2 = start_position.minus( handle_position );
double angle = v1.angle( v2 );
// -------- Figure out which way to rotate --------
//
// define two vectors U and V as follows
// U = EndEffector(E) - StartJoint(S)
// N = Normal to U passing through EndEffector
//
// Clip handle_position to half-plane U to determine the region it
// lies in. Use the region to determine the rotation direction.
//
// U
// ^ Region Rotation
// | B
// (E)---N A C-C-W
// A | B C-W
// | B
// |
// (S)
//
double rot = 0.0; // Rotation about Z-axis
// U and N define a half-plane to clip the handle against
//
MVector U = effector_position.minus( start_position );
U.normalize();
// Get a normal to U
//
MVector zAxis = new MVector( 0.0, 0.0, 1.0 );
MVector N = U.crossProduct( zAxis ); // Cross product
N.normalize();
// P is the handle position vector
//
MVector P = handle_position.minus( effector_position );
// Determine the rotation direction
//
double PdotN = P[0]*N[0] + P[1]*N[1];
if ( PdotN < 0 ) {
// counter-clockwise
rot = angle;
} else {
// clockwise
rot = -1.0 * angle;
}
// get and set the Joint Angles
//
MDoubleArray jointAngles = new MDoubleArray();
getJointAngles( jointAngles );
jointAngles.set( jointAngles[0] + rot, 0 );
setJointAngles( jointAngles );
}
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;
}
// This method returns the twist of the IK handle.
//
double twistFromHandle(MDagPath handlePath)
{
MFnIkHandle handleFn = new MFnIkHandle(handlePath);
MPlug twistPlug = handleFn.findPlug("twist");
double twistValue = 0.0;
twistPlug.getValue(twistValue);
return twistValue;
}
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 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;
}
double twistFromHandle(MDagPath handlePath)
// This method returns the twist of the IK handle.
//
{
MFnIkHandle handleFn = new MFnIkHandle(handlePath);
MPlug twistPlug = handleFn.findPlug("twist");
double twistValue = 0.0;
twistPlug.getValue(twistValue);
return twistValue;
}
private void doSimpleSolver()
//
// Solve single joint in the x-y plane
//
// - first it calculates the angle between the handle and the end-effector.
// - then it determines which way to rotate the joint.
//
{
// Get the handle and create a function set for it
//
MIkHandleGroup handle_group = handleGroup;
if (null == handle_group)
{
throw new System.InvalidOperationException("invalid handle group.");
}
MObject handle = handle_group.handle(0);
MDagPath handlePath = MDagPath.getAPathTo(handle);
MFnIkHandle fnHandle = new MFnIkHandle(handlePath);
// Get the position of the end_effector
//
MDagPath end_effector = new MDagPath();
fnHandle.getEffector(end_effector);
MFnTransform tran = new MFnTransform(end_effector);
MPoint effector_position = tran.rotatePivot(MSpace.Space.kWorld);
// Get the position of the handle
//
MPoint handle_position = fnHandle.rotatePivot(MSpace.Space.kWorld);
// Get the start joint position
//
MDagPath start_joint = new MDagPath();
fnHandle.getStartJoint(start_joint);
MFnTransform start_transform = new MFnTransform(start_joint);
MPoint start_position = start_transform.rotatePivot(MSpace.Space.kWorld);
// Calculate the rotation angle
//
MVector v1 = start_position.minus(effector_position);
MVector v2 = start_position.minus(handle_position);
double angle = v1.angle(v2);
// -------- Figure out which way to rotate --------
//
// define two vectors U and V as follows
// U = EndEffector(E) - StartJoint(S)
// N = Normal to U passing through EndEffector
//
// Clip handle_position to half-plane U to determine the region it
// lies in. Use the region to determine the rotation direction.
//
// U
// ^ Region Rotation
// | B
// (E)---N A C-C-W
// A | B C-W
// | B
// |
// (S)
//
double rot = 0.0; // Rotation about Z-axis
// U and N define a half-plane to clip the handle against
//
MVector U = effector_position.minus(start_position);
U.normalize();
// Get a normal to U
//
MVector zAxis = new MVector(0.0, 0.0, 1.0);
MVector N = U.crossProduct(zAxis); // Cross product
N.normalize();
// P is the handle position vector
//
MVector P = handle_position.minus(effector_position);
// Determine the rotation direction
//
double PdotN = P[0] * N[0] + P[1] * N[1];
if (PdotN < 0)
{
// counter-clockwise
rot = angle;
}
else
{
// clockwise
rot = -1.0 * angle;
}
// get and set the Joint Angles
//
MDoubleArray jointAngles = new MDoubleArray();
getJointAngles(jointAngles);
jointAngles.set(jointAngles[0] + rot, 0);
setJointAngles(jointAngles);
}
