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