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