void QuatRot(Vector3d<T> *v_rot, const Quaternion<T> *q, const Vector3d<T> *v_in){
Quaternion<T> qV, qVr, qc, tmp;
qV(0) = static_cast<T>(0);
qV(1) = (*v_in)(0);
qV(2) = (*v_in)(1);
qV(3) = (*v_in)(2);
qc = Qcon(q);
tmp = Qmul(q, &qV);
qVr = Qmul(&tmp, &qc);
(*v_rot)(0) = qVr(1);
(*v_rot)(1) = qVr(2);
(*v_rot)(2) = qVr(3);
}
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;
}
