MStatus CIKSolverNode::doSimpleSolver() { MStatus stat; // Get the handle and create a function set for it // MIkHandleGroup* handle_group = handleGroup(); if (NULL == handle_group) { return MS::kFailure; } MObject handle = handle_group->handle(0); MDagPath handlePath = MDagPath::getAPathTo(handle); MFnIkHandle fnHandle(handlePath, &stat); // End-Effector MDagPath endEffectorPath; fnHandle.getEffector(endEffectorPath); MFnIkEffector fnEffector(endEffectorPath); MPoint effectorPos = fnEffector.rotatePivot(MSpace::kWorld); unsigned int numJoints = endEffectorPath.length(); std::vector<MDagPath> jointsDagPaths; jointsDagPaths.reserve(numJoints); while (endEffectorPath.length() > 1) { endEffectorPath.pop(); jointsDagPaths.push_back( endEffectorPath ); } std::reverse(jointsDagPaths.begin(), jointsDagPaths.end()); static bool builtLocalSkeleton = false; if (builtLocalSkeleton == false) { for (int jointIdx = 0; jointIdx < jointsDagPaths.size(); ++jointIdx) { MFnIkJoint curJoint(jointsDagPaths[jointIdx]); m_localJointsPos.push_back( curJoint.getTranslation(MSpace::kWorld) ); } m_localJointsPos.push_back(effectorPos ); builtLocalSkeleton = true; } MPoint startJointPos = MFnIkJoint(jointsDagPaths.front()).getTranslation(MSpace::kWorld); MVector startToEndEff = m_localJointsPos.back() - m_localJointsPos.front(); double curveLength = (getPosition(1.0) - getPosition(0.0)).length(); double chainLength = startToEndEff.length(); // in local space. double stretchFactor = curveLength / chainLength; double uVal = 0.0f; MVector jointPosL = m_localJointsPos[0]; for (int jointIdx = 0; jointIdx < jointsDagPaths.size(); ++jointIdx) { MFnIkJoint curJoint(jointsDagPaths[jointIdx]); MVector curJointPosL = m_localJointsPos[jointIdx]; double dist = stretchFactor * (curJointPosL - jointPosL).length(); uVal = uVal + dist / curveLength; MVector curCurveJointPos = getPosition(uVal); curJoint.setTranslation(curCurveJointPos, MSpace::kWorld); jointPosL = curJointPosL; } MVector effectorCurvePos = getPosition(1.0); MVector curCurveJointPos = getPosition(uVal); MVector effectorVec = (effectorCurvePos - curCurveJointPos).normal(); double endJointAngle[3]; MVector effectorVecXY = MVector(effectorVec(0), effectorVec(1), 0.0); endJointAngle[2] = effectorVecXY.angle(MVector(1, 0, 0)); if ((MVector(1, 0, 0) ^ effectorVecXY) * MVector(0, 0, 1) < 0.0) { endJointAngle[2] = -endJointAngle[2]; } MVector effectorVecXZ = MVector(effectorVec(0), 0.0, effectorVec(2)); endJointAngle[1] = effectorVecXZ.angle(MVector(1, 0, 0)); if ((MVector(1, 0, 0) ^ effectorVecXZ) * MVector(0, 1, 0) < 0.0) { endJointAngle[1] = -endJointAngle[1]; } endJointAngle[0] = 0.0; MFnIkJoint curJoint(jointsDagPaths.back()); curJoint.setRotation(endJointAngle, curJoint.rotationOrder()); return MS::kSuccess; }
void n_tentacle::computeSlerp(const MMatrix &matrix1, const MMatrix &matrix2, const MFnNurbsCurve &curve, double parameter, double blendRot, double iniLength, double curveLength, double stretch, double globalScale, int tangentAxis, MVector &outPos, MVector &outRot) { //curveLength = curve.length() double lenRatio = iniLength / curveLength; MQuaternion quat1; quat1 = matrix1; MQuaternion quat2; quat2 = matrix2; this->bipolarityCheck(quat1, quat2); //need to adjust the parameter in order to maintain the length between elements, also for the stretch MVector tangent; MPoint pointAtParam; MPoint finaPos; double p = lenRatio * parameter * globalScale; double finalParam = p + (parameter - p) * stretch; if(curveLength * finalParam > curveLength) { double lengthDiff = curveLength - (iniLength * parameter); double param = curve.knot(curve.numKnots() - 1); tangent = curve.tangent(param, MSpace::kWorld); tangent.normalize(); curve.getPointAtParam(param, pointAtParam, MSpace::kWorld); finaPos = pointAtParam; pointAtParam += (- tangent) * lengthDiff; //MGlobal::displayInfo("sdf"); } else { double param = curve.findParamFromLength(curveLength * finalParam); tangent = curve.tangent(param, MSpace::kWorld); tangent.normalize(); curve.getPointAtParam(param, pointAtParam, MSpace::kWorld); } MQuaternion slerpQuat = slerp(quat1, quat2, blendRot); MMatrix slerpMatrix = slerpQuat.asMatrix(); int axisId = abs(tangentAxis) - 1; MVector slerpMatrixYAxis = MVector(slerpMatrix(axisId, 0), slerpMatrix(axisId, 1), slerpMatrix(axisId, 2)); slerpMatrixYAxis.normalize(); if(tangentAxis < 0) slerpMatrixYAxis = - slerpMatrixYAxis; double angle = tangent.angle(slerpMatrixYAxis); MVector axis = slerpMatrixYAxis ^ tangent; axis.normalize(); MQuaternion rotationToSnapOnCurve(angle, axis); MQuaternion finalQuat = slerpQuat * rotationToSnapOnCurve; MEulerRotation finalEuler = finalQuat.asEulerRotation(); outRot.x = finalEuler.x; outRot.y = finalEuler.y; outRot.z = finalEuler.z; outPos = pointAtParam; }
MStatus buildRotation::compute( const MPlug& plug, MDataBlock& data ) { MStatus returnStatus; if ((plug == rotate) || (plug.parent() == rotate) || (plug == rotateMatrix)) { MDataHandle upData = data.inputValue( up, &returnStatus ); McheckErr(returnStatus,"ERROR getting up vector data"); MDataHandle forwardData = data.inputValue( forward, &returnStatus ); McheckErr(returnStatus,"ERROR getting forward vector data"); MVector up = upData.asVector(); MVector forward = forwardData.asVector(); // Make sure that the up and forward vectors are orthogonal // if ( fabs( up * forward ) > EPSILON ) { // Non-zero dot product // MVector orthoVec = up ^ forward; MVector newForward = orthoVec ^ up; if ( forward * newForward < 0.0 ) { // Reverse the vector // newForward *= -1.0; } forward = newForward; } // Calculate the rotation required to align the y-axis with the up // vector // MTransformationMatrix firstRot; MVector rotAxis = MVector::yAxis ^ up; rotAxis.normalize(); firstRot.setToRotationAxis( rotAxis, MVector::yAxis.angle( up ) ); // Calculate the second rotation required to align the forward vector // MTransformationMatrix secondRot; MVector transformedForward = firstRot.asMatrix() * forward; transformedForward.normalize(); double angle = transformedForward.angle( MVector::zAxis ); if ( transformedForward.x < 0.0 ) { // Compensate for the fact that the angle method returns // the absolute value // angle *= -1.0; } secondRot.setToRotationAxis( up, angle ); // Get the requested rotation order // MDataHandle orderHandle = data.inputValue( rotateOrder ); short order = orderHandle.asShort(); MTransformationMatrix::RotationOrder rotOrder; switch ( order ) { case ROTATE_ORDER_XYZ: rotOrder = MTransformationMatrix::kXYZ; break; case ROTATE_ORDER_YZX: rotOrder = MTransformationMatrix::kYZX; break; case ROTATE_ORDER_ZXY: rotOrder = MTransformationMatrix::kZXY; break; case ROTATE_ORDER_XZY: rotOrder = MTransformationMatrix::kXZY; break; case ROTATE_ORDER_YXZ: rotOrder = MTransformationMatrix::kYXZ; break; case ROTATE_ORDER_ZYX: rotOrder = MTransformationMatrix::kZYX; break; default: rotOrder = MTransformationMatrix::kInvalid; break; } MTransformationMatrix result = firstRot.asMatrix() * secondRot.asMatrix(); result.reorderRotation( rotOrder ); double rotation[3]; result.getRotation( rotation, rotOrder, MSpace::kTransform ); MDataHandle outputRot = data.outputValue( rotate ); outputRot.set( rotation[0], rotation[1], rotation[2] ); outputRot.setClean(); MDataHandle outputMatrix = data.outputValue( rotateMatrix ); outputMatrix.set( result.asMatrix() ); outputMatrix.setClean(); } else return MS::kUnknownParameter; return MS::kSuccess; }