void ik2Bsolver::solveIK(const MPoint &startJointPos, const MPoint &midJointPos, const MPoint &effectorPos, const MPoint &handlePos, const MVector &poleVector, double twistValue, MQuaternion &qStart, MQuaternion &qMid, const double & softDistance, const double & restLength1, const double & restLength2, double & stretching) // // This is method that actually computes the IK solution. // { // vector from startJoint to midJoint MVector vector1 = midJointPos - startJointPos; vector1 = vector1.normal() * restLength1; // vector from midJoint to effector MVector vector2 = effectorPos - midJointPos; vector2 = vector2.normal() * restLength2; // vector from startJoint to handle MVector vectorH = handlePos - startJointPos; // vector from startJoint to effector MVector vectorE = effectorPos - startJointPos; // lengths of those vectors const double length1 = restLength1; const double length2 = restLength2; const double lengthH = vectorH.length(); // component of the vector1 orthogonal to the vectorE const MVector vectorO = vector1 - vectorE*((vector1*vectorE)/(vectorE*vectorE)); ////////////////////////////////////////////////////////////////// // calculate q12 which solves for the midJoint rotation ////////////////////////////////////////////////////////////////// // angle between vector1 and vector2 const double vectorAngle12 = vector1.angle(vector2); // vector orthogonal to vector1 and 2 const MVector vectorCross12 = vector1^vector2; const double lengthHsquared = lengthH * lengthH; double weight = 0.0; double slowLH = lengthH;// / (1.0 + (6.8 - softDistance) / (restLength1 + restLength2)); const double da = restLength1 + restLength2 - softDistance; if(slowLH > da) { float s = (slowLH - da) / softDistance; if(s> 1.f) s= 1.f; Vector3F pt = m_herm.interpolate(s); // MGlobal::displayInfo(MString("herm ")+ pt.y); // approading l1+l2 slower // weight = 1.0 - exp(-(slowLH - da) / softDistance * 6.98); weight = pt.y * weight + (1.f - pt.y) * s; slowLH = da + softDistance * weight; // MGlobal::displayInfo(MString("wei ")+weight); } // // 1 // / \ // l1 / \ l2 // / \ ---l1---1 ---l2--- // 0 ------ l ------ 2 0 ------ l ------- 2 // // angle for arm extension double cos_theta = (slowLH * slowLH - length1*length1 - length2*length2) / (2*length1*length2); if (cos_theta > 1) cos_theta = 1; else if (cos_theta < -1) cos_theta = -1; const double theta = acos(cos_theta); // quaternion for arm extension MQuaternion q12(theta - vectorAngle12, vectorCross12); ////////////////////////////////////////////////////////////////// // calculate qEH which solves for effector rotating onto the handle ////////////////////////////////////////////////////////////////// // vector2 with quaternion q12 applied vector2 = vector2.rotateBy(q12); // vectorE with quaternion q12 applied vectorE = vector1 + vector2; // quaternion for rotating the effector onto the handle MQuaternion qEH(vectorE, vectorH); if(lengthH > vectorE.length()) { // MGlobal::displayInfo(MString("vle ")+(lengthH-vectorE.length())); stretching = (lengthH-vectorE.length()) * weight; } ////////////////////////////////////////////////////////////////// // calculate qNP which solves for the rotate plane ////////////////////////////////////////////////////////////////// // vector1 with quaternion qEH applied vector1 = vector1.rotateBy(qEH); if (vector1.isParallel(vectorH)) // singular case, use orthogonal component instead vector1 = vectorO.rotateBy(qEH); // quaternion for rotate plane MQuaternion qNP; if (!poleVector.isParallel(vectorH) && (lengthHsquared != 0)) { // component of vector1 orthogonal to vectorH MVector vectorN = vector1 - vectorH*((vector1*vectorH)/lengthHsquared); // component of pole vector orthogonal to vectorH MVector vectorP = poleVector - vectorH*((poleVector*vectorH)/lengthHsquared); double dotNP = (vectorN*vectorP)/(vectorN.length()*vectorP.length()); if (absoluteValue(dotNP + 1.0) < kEpsilon) { // singular case, rotate halfway around vectorH MQuaternion qNP1(kPi, vectorH); qNP = qNP1; } else { MQuaternion qNP2(vectorN, vectorP); qNP = qNP2; } } ////////////////////////////////////////////////////////////////// // calculate qTwist which adds the twist ////////////////////////////////////////////////////////////////// MQuaternion qTwist(twistValue, vectorH); // quaternion for the mid joint qMid = q12; // concatenate the quaternions for the start joint qStart = qEH*qNP*qTwist; }
void curveColliderLocator::draw(M3dView &view, const MDagPath &path, M3dView::DisplayStyle style, M3dView::DisplayStatus status) { // // Get the input curve // MObject thisNode = thisMObject(); MPlug radiusPlug(thisNode, colliderRadiusIn); MPlug colorPlugR(thisNode, colliderColorR); MPlug colorPlugG(thisNode, colliderColorG); MPlug colorPlugB(thisNode, colliderColorB); MPlug colorPlugT(thisNode, colliderTransparency); MPlug radiusElement; double radius; double radius2; double param; double param2; double paramNorm; radiusPlug.getValue(radius); MStatus stat; MQuaternion rotateQuat; double degreesToRadians = ( M_PI/ 180.0 ); double radiansToDegrees = ( 180.0/M_PI ); double rotateRadians; MVector crvNormalRotated; MPointArray radiusPts; MPoint radiusPt; // Alternate method of getting the MFnNurbsCurve: MPlug curvePlug(thisNode, colliderCurveIn); MPlugArray inputCrvArray; curvePlug.connectedTo(inputCrvArray, true, false); MObject crvColliderObj = inputCrvArray[0].node(); MFnNurbsCurve cColliderFn(crvColliderObj, &stat); if(!stat){ MGlobal::displayInfo(MString("Error creating MFnNurbsCurve for collider curve")); return; } MFnDagNode crvDagNode(crvColliderObj); MDagPath crvDagPath; crvDagNode.getPath(crvDagPath); MMatrix crvXform = crvDagPath.inclusiveMatrix(); int numSpans = cColliderFn.numSpans(); MPoint crvPoint; MPoint crvPoint2; GLUquadricObj* qobj = gluNewQuadric(); view.beginGL(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////// // DRAW SMOOTH SHADED ///////////////////////////////////////////////////////////////////////////////////////////////////////////// if ( ( style == M3dView::kFlatShaded ) || ( style == M3dView::kGouraudShaded ) ) { glPushAttrib( GL_ALL_ATTRIB_BITS ); glShadeModel(GL_SMOOTH); glEnable(GL_LIGHTING); glEnable(GL_BLEND); glEnable(GL_LIGHT0); glMatrixMode( GL_MODELVIEW ); float3 color; colorPlugR.getValue(color[0]); colorPlugG.getValue(color[1]); colorPlugB.getValue(color[2]); float transparency; colorPlugT.getValue(transparency); float diffuse[] = {color[0], color[1], color[2], transparency}; float specular[] = { 0.7f, 0.7f, 0.7f, transparency}; float shine[] = { 100.0f }; float ambient[] = { 0.2f, 0.2f, 0.2f, transparency }; glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, diffuse); glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, specular); glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, shine); glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, ambient); // Draw the beginning and ending sphere caps glPushMatrix(); cColliderFn.getPointAtParam(0, crvPoint, MSpace::kWorld); crvPoint = crvPoint*crvXform; glTranslatef(crvPoint.x, crvPoint.y, crvPoint.z); radiusElement = radiusPlug.elementByPhysicalIndex(0, &stat); radiusElement.getValue(radius); gluSphere(qobj, radius, 16, 16); glPopMatrix(); glPushMatrix(); cColliderFn.getPointAtParam(numSpans, crvPoint, MSpace::kWorld); crvPoint = crvPoint*crvXform; glTranslatef(crvPoint.x, crvPoint.y, crvPoint.z); radiusElement = radiusPlug.elementByPhysicalIndex( (radiusPlug.numElements() - 1), &stat); radiusElement.getValue(radius); gluSphere(qobj, radius, 16, 16); glPopMatrix(); int numStacks = numSpans*30; int numSlices = 32; // Initialize our point array with the radius values radiusPts.clear(); radiusPts.setLength(radiusPlug.numElements()); for(int radiusItr = 0; radiusItr < radiusPlug.numElements(); radiusItr++){ radiusElement = radiusPlug.elementByPhysicalIndex(radiusItr, &stat); if(!stat){MGlobal::displayError(MString("Could not get radius element.")); return;} radiusElement.getValue(radius); radiusPt.x = (double)radius; radiusPt.y = 0.0; radiusPt.z = 0.0; radiusPts[radiusItr] = radiusPt; } if(numStacks>1){ for(uint crvItr = 0; crvItr < numStacks - 1; crvItr++){ param = (float(crvItr)/float(numStacks-1))*numSpans; param2 = (float(crvItr+1)/float(numStacks-1))*numSpans; cColliderFn.getPointAtParam(param, crvPoint, MSpace::kWorld); crvPoint = crvPoint*crvXform; cColliderFn.getPointAtParam(param2, crvPoint2, MSpace::kWorld); crvPoint2 = crvPoint2 * crvXform; // Determine the radius value int lastRadiusIndex = radiusPlug.numElements() - 1; if(lastRadiusIndex == 0){ radiusElement = radiusPlug.elementByPhysicalIndex(0, &stat); if(!stat){MGlobal::displayError(MString("Could not get radius element.")); return;} radiusElement.getValue(radius); radius2 = radius; }else if(lastRadiusIndex > 0){ paramNorm = param/numSpans; radiusPt = getInterpolatedSplinePoint(paramNorm, radiusPts); radius = radiusPt.x; paramNorm = param2/numSpans; radiusPt = getInterpolatedSplinePoint(paramNorm, radiusPts); radius2 = radiusPt.x; } // First, we need to determine our starting position by travelling along the normal MVector crvNormal = cColliderFn.normal(param, MSpace::kWorld); crvNormal = crvNormal * crvXform; MVector crvTangent = cColliderFn.tangent(param, MSpace::kWorld); crvTangent = crvTangent * crvXform; crvNormal.normalize(); crvTangent.normalize(); MVector crvNormal2 = cColliderFn.normal(param2, MSpace::kWorld); crvNormal2 = crvNormal2 * crvXform; MVector crvTangent2 = cColliderFn.tangent(param2, MSpace::kWorld); crvTangent2 = crvTangent2 * crvXform; crvNormal2.normalize(); crvTangent2.normalize(); // glTranslatef(crvNormal.x, crvNormal.y, crvNormal.z); glBegin(GL_QUADS); for(int sliceItr = 0; sliceItr < numSlices; sliceItr++){ // quad vertex 4 rotateRadians = ((((float)sliceItr+1)/numSlices)*360)*degreesToRadians; rotateQuat.setAxisAngle(crvTangent, rotateRadians); crvNormalRotated = crvNormal.rotateBy(rotateQuat); glNormal3f(crvNormalRotated.x, crvNormalRotated.y, crvNormalRotated.z ); glVertex3f((float)crvPoint.x + (crvNormalRotated.x*radius), (float)crvPoint.y + (crvNormalRotated.y*radius), (float)crvPoint.z + (crvNormalRotated.z*radius)); // quad vertex 3 rotateRadians = ((((float)sliceItr+1)/numSlices)*360)*degreesToRadians; rotateQuat.setAxisAngle(crvTangent2, rotateRadians); crvNormalRotated = crvNormal.rotateBy(rotateQuat); glNormal3f(crvNormalRotated.x, crvNormalRotated.y, crvNormalRotated.z ); glVertex3f((float)crvPoint2.x + (crvNormalRotated.x*radius2), (float)crvPoint2.y + (crvNormalRotated.y*radius2), (float)crvPoint2.z + (crvNormalRotated.z*radius2)); // quad vertex 2 rotateRadians = (((float)sliceItr/numSlices)*360)*degreesToRadians; rotateQuat.setAxisAngle(crvTangent2, rotateRadians); crvNormalRotated = crvNormal.rotateBy(rotateQuat); glNormal3f(crvNormalRotated.x, crvNormalRotated.y, crvNormalRotated.z ); glVertex3f((float)crvPoint2.x + (crvNormalRotated.x*radius2), (float)crvPoint2.y + (crvNormalRotated.y*radius2), (float)crvPoint2.z + (crvNormalRotated.z*radius2)); // quad vertex 1 rotateRadians = (((float)sliceItr/numSlices)*360)*degreesToRadians; rotateQuat.setAxisAngle(crvTangent, rotateRadians); crvNormalRotated = crvNormal.rotateBy(rotateQuat); glNormal3f(crvNormalRotated.x, crvNormalRotated.y, crvNormalRotated.z ); glVertex3f((float)crvPoint.x + (crvNormalRotated.x*radius), (float)crvPoint.y + (crvNormalRotated.y*radius), (float)crvPoint.z + (crvNormalRotated.z*radius)); } glEnd(); } } glPopAttrib(); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////// // END SMOOTH SHADED ///////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////// // DRAW WIREFRAME ///////////////////////////////////////////////////////////////////////////////////////////////////////////// if ( style == M3dView::kWireFrame || status == M3dView::kActive || status == M3dView::kLead) { glPushAttrib( GL_ALL_ATTRIB_BITS ); // Draw the beginning and ending sphere caps // Quadrilateral strips glPolygonMode (GL_FRONT_AND_BACK, GL_LINE); glPushMatrix(); cColliderFn.getPointAtParam(0, crvPoint, MSpace::kWorld); crvPoint = crvPoint*crvXform; glTranslatef(crvPoint.x, crvPoint.y, crvPoint.z); radiusElement = radiusPlug.elementByPhysicalIndex(0, &stat); radiusElement.getValue(radius); gluSphere(qobj, radius, 16, 16); glPopMatrix(); glPushMatrix(); cColliderFn.getPointAtParam(numSpans, crvPoint, MSpace::kWorld); crvPoint = crvPoint*crvXform; glTranslatef(crvPoint.x, crvPoint.y, crvPoint.z); radiusElement = radiusPlug.elementByPhysicalIndex( (radiusPlug.numElements() - 1), &stat); radiusElement.getValue(radius); gluSphere(qobj, radius, 16, 16); glPopMatrix(); int numStacks = numSpans*10; int numSlices = 32; // Initialize our point array with the radius values radiusPts.clear(); radiusPts.setLength(radiusPlug.numElements()); for(int radiusItr = 0; radiusItr < radiusPlug.numElements(); radiusItr++){ radiusElement = radiusPlug.elementByPhysicalIndex(radiusItr, &stat); if(!stat){MGlobal::displayError(MString("Could not get radius element.")); return;} radiusElement.getValue(radius); radiusPt.x = (double)radius; radiusPt.y = 0.0; radiusPt.z = 0.0; radiusPts[radiusItr] = radiusPt; } for(uint crvItr = 0; crvItr < numStacks; crvItr++){ param = (float(crvItr)/float(numStacks))*numSpans; param2 = (float(crvItr+1)/float(numStacks))*numSpans; cColliderFn.getPointAtParam(param, crvPoint, MSpace::kWorld); crvPoint = crvPoint*crvXform; cColliderFn.getPointAtParam(param2, crvPoint2, MSpace::kWorld); crvPoint2 = crvPoint2 * crvXform; // Determine the radius value int lastRadiusIndex = radiusPlug.numElements() - 1; if(lastRadiusIndex == 0){ radiusElement = radiusPlug.elementByPhysicalIndex(0, &stat); if(!stat){MGlobal::displayError(MString("Could not get radius element.")); return;} radiusElement.getValue(radius); radius2 = radius; }else if(lastRadiusIndex > 0){ paramNorm = param/numSpans; radiusPt = getInterpolatedSplinePoint(paramNorm, radiusPts); radius = radiusPt.x; paramNorm = param2/numSpans; radiusPt = getInterpolatedSplinePoint(paramNorm, radiusPts); radius2 = radiusPt.x; } // First, we need to determine our starting position by travelling along the normal MVector crvNormal = cColliderFn.normal(param, MSpace::kWorld); crvNormal = crvNormal * crvXform; MVector crvTangent = cColliderFn.tangent(param, MSpace::kWorld); crvTangent = crvTangent * crvXform; crvNormal.normalize(); crvTangent.normalize(); MVector crvNormal2 = cColliderFn.normal(param2, MSpace::kWorld); crvNormal2 = crvNormal2 * crvXform; MVector crvTangent2 = cColliderFn.tangent(param2, MSpace::kWorld); crvTangent2 = crvTangent2 * crvXform; crvNormal2.normalize(); crvTangent2.normalize(); // glTranslatef(crvNormal.x, crvNormal.y, crvNormal.z); glBegin(GL_LINE_LOOP); for(int sliceItr = 0; sliceItr < numSlices; sliceItr++){ // quad vertex 4 rotateRadians = ((((float)sliceItr+1)/numSlices)*360)*degreesToRadians; rotateQuat.setAxisAngle(crvTangent, rotateRadians); crvNormalRotated = crvNormal.rotateBy(rotateQuat); glNormal3f(crvNormalRotated.x, crvNormalRotated.y, crvNormalRotated.z ); glVertex3f((float)crvPoint.x + (crvNormalRotated.x*radius), (float)crvPoint.y + (crvNormalRotated.y*radius), (float)crvPoint.z + (crvNormalRotated.z*radius)); // quad vertex 3 rotateRadians = ((((float)sliceItr+1)/numSlices)*360)*degreesToRadians; rotateQuat.setAxisAngle(crvTangent2, rotateRadians); crvNormalRotated = crvNormal.rotateBy(rotateQuat); glNormal3f(crvNormalRotated.x, crvNormalRotated.y, crvNormalRotated.z ); glVertex3f((float)crvPoint2.x + (crvNormalRotated.x*radius2), (float)crvPoint2.y + (crvNormalRotated.y*radius2), (float)crvPoint2.z + (crvNormalRotated.z*radius2)); // quad vertex 2 rotateRadians = (((float)sliceItr/numSlices)*360)*degreesToRadians; rotateQuat.setAxisAngle(crvTangent2, rotateRadians); crvNormalRotated = crvNormal.rotateBy(rotateQuat); glNormal3f(crvNormalRotated.x, crvNormalRotated.y, crvNormalRotated.z ); glVertex3f((float)crvPoint2.x + (crvNormalRotated.x*radius2), (float)crvPoint2.y + (crvNormalRotated.y*radius2), (float)crvPoint2.z + (crvNormalRotated.z*radius2)); // quad vertex 1 rotateRadians = (((float)sliceItr/numSlices)*360)*degreesToRadians; rotateQuat.setAxisAngle(crvTangent, rotateRadians); crvNormalRotated = crvNormal.rotateBy(rotateQuat); glNormal3f(crvNormalRotated.x, crvNormalRotated.y, crvNormalRotated.z ); glVertex3f((float)crvPoint.x + (crvNormalRotated.x*radius), (float)crvPoint.y + (crvNormalRotated.y*radius), (float)crvPoint.z + (crvNormalRotated.z*radius)); } glEnd(); } glPopAttrib(); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////// // END WIREFRAME ///////////////////////////////////////////////////////////////////////////////////////////////////////////// // glEnable(GL_LIGHT0); view.endGL(); }