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