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 splineSolverNode::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.
//
{
//Do Real Solve
//
MStatus stat;
float curCurveLength = curveFn.length();
MPlug crvLengPlug = fnHandle.findPlug("cvLen");
double initCurveLength = crvLengPlug.asDouble();
//Twist
MPlug twistRamp = fnHandle.findPlug("twistRamp");
MRampAttribute curveAttribute( twistRamp, &stat );
MPlug startTwistPlug = fnHandle.findPlug("strtw");
double startTwist = startTwistPlug.asDouble();
MPlug endTwistPlug = fnHandle.findPlug("endtw");
double endTwist = endTwistPlug.asDouble();
//Scale Ramp
MPlug scaleRamp = fnHandle.findPlug("scaleRamp");
MRampAttribute curveScaleAttribute( scaleRamp, &stat );
//Roll
MPlug rollPlug = fnHandle.findPlug("roll");
double roll = rollPlug.asDouble();
MPlug strPlug = fnHandle.findPlug("str");
float stretchRatio = strPlug.asDouble();
float normCrvLength = curCurveLength / initCurveLength;
double scale[3] = {1 +stretchRatio*(normCrvLength -1), 1, 1};
//Get Anchor Position
MPlug ancPosPlug = fnHandle.findPlug("ancp");
double anchorPos = ancPosPlug.asDouble();
MPlug jointsTotLengthPlug = fnHandle.findPlug("jsLen");
double jointsTotalLength = jointsTotLengthPlug.asDouble();
double difLengthCurveJoints = curCurveLength - (jointsTotalLength * scale[0]);
float startLength = 0.0 + anchorPos*( difLengthCurveJoints );
float parm = curveFn.findParamFromLength( startLength );
MPoint pBaseJoint, pEndJoint;
curveFn.getPointAtParam( parm, pBaseJoint );
//get Init normal
MPlug initNormalPlug = fnHandle.findPlug("norm");
double nx = initNormalPlug.child(0).asDouble();
double ny = initNormalPlug.child(1).asDouble();
double nz = initNormalPlug.child(2).asDouble();
MVector eyeUp( nx, ny, nz );
//get Init Tangent
MPlug initTangentPlug = fnHandle.findPlug("tang");
double tx = initTangentPlug.child(0).asDouble();
double ty = initTangentPlug.child(1).asDouble();
double tz = initTangentPlug.child(2).asDouble();
MVector eyeV( tx, ty, tz );
MFnIkJoint j( joints[0] );
j.setTranslation( MVector( pBaseJoint ), MSpace::kWorld );
float jointRotPercent = 1.0/joints.size();
float currJointRot = 0;
float prevTwist = 0;
double angle;
//j.setScale(scale);
for (int i = 0; i < joints.size(); i++)
{
MFnIkJoint j( joints[i]);
pBaseJoint = j.rotatePivot(MSpace::kWorld);
//Calculate Scale
float scaleValue;
curveScaleAttribute.getValueAtPosition(currJointRot, scaleValue, &stat);
//if ( scale[0] >= 1 ) // Stretch
scale[1] = 1 + scaleValue * ( 1 - scale[0] );
/*
else //Squash
scale[1] = 1 + scaleValue * ( 1.0 - scale[0] );
*/
if (scale[1] < 0)
scale[1] = 0;
scale[2] = scale[1];
j.setScale(scale);
//j.setRotation( rot, j.rotationOrder() );
if( i == joints.size() - 1)
//Effector Position
pEndJoint = tran.rotatePivot(MSpace::kWorld);
else
{
//get position of next joint
MFnIkJoint j2( joints[i + 1]);
pEndJoint = j2.rotatePivot(MSpace::kWorld);
}
MVector vBaseJoint(pBaseJoint[0]-pEndJoint[0], pBaseJoint[1]-pEndJoint[1], pBaseJoint[2]-pEndJoint[2]);
startLength += vBaseJoint.length();
MVector eyeAim(1.0,0.0,0.0);
MPoint pFinalPos;
float parm = curveFn.findParamFromLength( startLength );
//Aim to final Pos
curveFn.getPointAtParam( parm, pFinalPos, MSpace::kWorld );
MVector eyeU(pBaseJoint[0]-pFinalPos[0], pBaseJoint[1]-pFinalPos[1], pBaseJoint[2]-pFinalPos[2]);
eyeU.normalize();
MVector eyeW( eyeU ^ eyeV );
eyeW.normalize();
eyeV = eyeW ^ eyeU;
MQuaternion qU( -eyeAim, eyeU );
MVector upRotated( eyeUp.rotateBy( qU ));
angle = acos( upRotated*eyeV );
//Calculate Twist
{
float twistValue;
curveAttribute.getValueAtPosition(currJointRot, twistValue, &stat);
double rotVal = (1-twistValue)*startTwist + twistValue*endTwist;
angle += MAngle(rotVal, MAngle::kDegrees).asRadians();
currJointRot += jointRotPercent;
}
//Calculate Roll
angle += roll;
MQuaternion qV(angle, eyeU);
MQuaternion q(qU*qV);
j.setRotation( q, MSpace::kWorld );
}
return MS::kSuccess;
}
