void C4X4Matrix::buildInterpolation(const C4X4Matrix& fromThis,const C4X4Matrix& toThat,float t) { // Builds the interpolation (based on t) from 'fromThis' to 'toThat' C7Vector out; out.buildInterpolation(fromThis.getTransformation(),toThat.getTransformation(),t); (*this)=out; }
void CikEl::prepareIkEquations(extIkReal interpolFact) { // Before calling this function, make sure that joint's temp. param. are initialized! // Make also sure the tooltip is built on a joint before 'base' and that base // is parent of 'tooltip'. // interpolFact is the interpolation factor we use to compute the target pose: // interpolPose=tooltipPose*(1-interpolFact)+targetPose*interpolFact // We first take care of dummies linked to path objects in a "sliding" manner (not fixed but assigned to the path): // Case 1. Target is the free sliding dummy: CDummy* dummyObj=Ct::ct->objCont->getDummy(getTarget()); CDummy* tipObj=Ct::ct->objCont->getDummy(tooltip); // We get the jacobian and the rowJointIDs: rowJointIDs=new std::vector<int>; rowJointStages=new std::vector<int>; C4X4Matrix oldMatr; CMatrix* Ja=CIkRoutine::getJacobian(this,oldMatr,rowJointIDs,rowJointStages); // oldMatr now contains the cumulative transf. matr. of tooltip relative to base C4X4Matrix oldMatrInv(oldMatr.getInverse()); int doF=Ja->cols; int equationNumber=0; C4X4Matrix dummyCumul; C4X4Matrix m; if (dummyObj!=NULL) { C3DObject* baseObj=Ct::ct->objCont->getObject(base); C4X4Matrix baseCumul; baseCumul.setIdentity(); if (baseObj!=NULL) baseCumul=baseObj->getCumulativeTransformation(true).getMatrix(); baseObj=Ct::ct->objCont->getObject(alternativeBaseForConstraints); if (baseObj!=NULL) baseCumul=baseObj->getCumulativeTransformation(true).getMatrix(); baseCumul.inverse(); dummyCumul=dummyObj->getCumulativeTransformationPart1(true).getMatrix(); dummyCumul=baseCumul*dummyCumul; // target is relative to the base (or the alternative base)! C7Vector tr; tr.buildInterpolation(oldMatr.getTransformation(),dummyCumul.getTransformation(),interpolFact); m=tr; // We prepare matrix and errorVector and their respective sizes: if (constraints&sim_ik_x_constraint) equationNumber++; if (constraints&sim_ik_y_constraint) equationNumber++; if (constraints&sim_ik_z_constraint) equationNumber++; if (constraints&sim_ik_alpha_beta_constraint) equationNumber+=2; if (constraints&sim_ik_gamma_constraint) equationNumber++; } matrix=new CMatrix(equationNumber,doF); matrix_correctJacobian=new CMatrix(equationNumber,doF); errorVector=new CMatrix(equationNumber,1); if (dummyObj!=NULL) { // We set up the position/orientation errorVector and the matrix: int pos=0; if (constraints&sim_ik_x_constraint) { for (int i=0;i<doF;i++) { (*matrix)(pos,i)=(*Ja)(0,i); (*matrix_correctJacobian)(pos,i)=(*Ja)(0,i); } (*errorVector)(pos,0)=(m.X(0)-oldMatr.X(0))*positionWeight; pos++; } if (constraints&sim_ik_y_constraint) { for (int i=0;i<doF;i++) { (*matrix)(pos,i)=(*Ja)(1,i); (*matrix_correctJacobian)(pos,i)=(*Ja)(1,i); } (*errorVector)(pos,0)=(m.X(1)-oldMatr.X(1))*positionWeight; pos++; } if (constraints&sim_ik_z_constraint) { for (int i=0;i<doF;i++) { (*matrix)(pos,i)=(*Ja)(2,i); (*matrix_correctJacobian)(pos,i)=(*Ja)(2,i); } (*errorVector)(pos,0)=(m.X(2)-oldMatr.X(2))*positionWeight; pos++; } if ( (constraints&sim_ik_alpha_beta_constraint)&&(constraints&sim_ik_gamma_constraint) ) { for (int i=0;i<doF;i++) { (*matrix)(pos,i)=(*Ja)(3,i); (*matrix)(pos+1,i)=(*Ja)(4,i); (*matrix)(pos+2,i)=(*Ja)(5,i); (*matrix_correctJacobian)(pos,i)=(*Ja)(3,i)*IK_DIVISION_FACTOR; (*matrix_correctJacobian)(pos+1,i)=(*Ja)(4,i)*IK_DIVISION_FACTOR; (*matrix_correctJacobian)(pos+2,i)=(*Ja)(5,i)*IK_DIVISION_FACTOR; } C4X4Matrix diff(oldMatrInv*m); C3Vector euler(diff.M.getEulerAngles()); (*errorVector)(pos,0)=euler(0)*orientationWeight/IK_DIVISION_FACTOR; (*errorVector)(pos+1,0)=euler(1)*orientationWeight/IK_DIVISION_FACTOR; (*errorVector)(pos+2,0)=euler(2)*orientationWeight/IK_DIVISION_FACTOR; pos=pos+3; } else { if (constraints&sim_ik_alpha_beta_constraint) { for (int i=0;i<doF;i++) { (*matrix)(pos,i)=(*Ja)(3,i); (*matrix)(pos+1,i)=(*Ja)(4,i); (*matrix_correctJacobian)(pos,i)=(*Ja)(3,i)*IK_DIVISION_FACTOR; (*matrix_correctJacobian)(pos+1,i)=(*Ja)(4,i)*IK_DIVISION_FACTOR; } C4X4Matrix diff(oldMatrInv*m); C3Vector euler(diff.M.getEulerAngles()); (*errorVector)(pos,0)=euler(0)*orientationWeight/IK_DIVISION_FACTOR; (*errorVector)(pos+1,0)=euler(1)*orientationWeight/IK_DIVISION_FACTOR; pos=pos+2; } if (constraints&sim_ik_gamma_constraint) { // sim_gamma_constraint can't exist without sim_alpha_beta_constraint! for (int i=0;i<doF;i++) { (*matrix)(pos,i)=(*Ja)(5,i); (*matrix_correctJacobian)(pos,i)=(*Ja)(5,i)*IK_DIVISION_FACTOR; } C4X4Matrix diff(oldMatrInv*m); C3Vector euler(diff.M.getEulerAngles()); (*errorVector)(pos,0)=euler(2)*orientationWeight/IK_DIVISION_FACTOR; pos++; } } } delete Ja; // We delete the jacobian! }
void C7Vector::set(const C4X4Matrix& m) { (*this)=m.getTransformation(); }
bool CGeometricConstraintSolver::solve(CIKGraphObjCont& graphContainer,SGeomConstrSolverParam& parameters) { if (graphContainer.identifyElements()==0) return(false); // Nothing to solve (no active joint in the mechanism) graphContainer.putElementsInPlace(); // We create a branched tree, where each extremity has a tip dummy // (which will later be constrained to its respective target dummy) CIKGraphObject* baseIKGraphObject=graphContainer.getBaseObject(); CIKGraphNode* graphIterator=baseIKGraphObject; CIKGraphNode* previousPosition=NULL; CIKGraphNode* nextPosition=NULL; C7Vector localTransformation; localTransformation.setIdentity(); CIKObjCont ikObjs; CIKJoint* lastJoint=NULL; CIKJoint* treeHandle=NULL; // Some precalculations of some fixed rotations: C4X4Matrix tmpRot; tmpRot.setIdentity(); tmpRot.M(0,0)=-1.0f; tmpRot.M(2,2)=-1.0f; C7Vector rotY180(tmpRot.getTransformation()); tmpRot.M.clear(); tmpRot.M(0,0)=1.0f; tmpRot.M(2,1)=1.0f; tmpRot.M(1,2)=-1.0f; C7Vector rotX90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f)); tmpRot.M.clear(); tmpRot.M(2,0)=-1.0f; tmpRot.M(1,1)=1.0f; tmpRot.M(0,2)=1.0f; C7Vector rotY90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f)); tmpRot.M.clear(); tmpRot.M(1,0)=1.0f; tmpRot.M(0,1)=-1.0f; tmpRot.M(2,2)=1.0f; C7Vector rotZ90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f)); std::vector<CIKGraphNode*> graphObjectsToBeExplored; graphObjectsToBeExplored.push_back(baseIKGraphObject); std::vector<CIKJoint*> lastJoints; lastJoints.push_back(NULL); std::vector<CIKGraphNode*> previousPositions; previousPositions.push_back(NULL); std::vector<C7Vector> localTransformations; localTransformations.push_back(localTransformation); int explorationID=0; while (graphObjectsToBeExplored.size()!=0) { graphIterator=graphObjectsToBeExplored.back(); graphObjectsToBeExplored.pop_back(); lastJoint=lastJoints.back(); lastJoints.pop_back(); previousPosition=previousPositions.back(); previousPositions.pop_back(); localTransformation=localTransformations.back(); localTransformations.pop_back(); bool doIt=(graphIterator->explorationID==-1); bool goingDown=false; bool closeComplexLoop=false; while (doIt) { if (graphIterator->explorationID==-1) graphIterator->explorationID=explorationID; explorationID++; C7Vector previousCT; if (previousPosition!=NULL) { if (previousPosition->type==IK_GRAPH_JOINT_TYPE) previousCT=((CIKGraphObject*)graphIterator)->cumulativeTransformation; else previousCT=((CIKGraphObject*)previousPosition)->cumulativeTransformation; } else { previousCT=baseIKGraphObject->cumulativeTransformation; localTransformation=previousCT; } if (graphIterator->type==IK_GRAPH_JOINT_TYPE) { // Joint: we have to introduce a joint CIKGraphJoint* graphJoint=(CIKGraphJoint*)graphIterator; if (!graphJoint->disabled) { C7Vector sphTr; sphTr.setIdentity(); sphTr.Q=graphJoint->sphericalTransformation; CIKJoint* newIKJoint; if (graphJoint->jointType==IK_GRAPH_SPHERICAL_JOINT_TYPE) { int dataValueBase=10*graphJoint->nodeID; CIKJoint* avatarParent; if (graphJoint->topObject==(CIKGraphObject*)previousPosition) { // From tip to base C7Vector rel(localTransformation*rotY180); newIKJoint=new CIKJoint(graphJoint,rel,false,false); if (lastJoint==NULL) { treeHandle=newIKJoint; lastJoint=treeHandle; ikObjs.addRoot(lastJoint); } else { ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; } avatarParent=ikObjs.getJointWithData(dataValueBase+3); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+3; rel=rotX90; newIKJoint=new CIKJoint(graphJoint,rel,false,false); ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; avatarParent=ikObjs.getJointWithData(dataValueBase+2); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+2; rel=rotY90; newIKJoint=new CIKJoint(graphJoint,rel,false,false); ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; avatarParent=ikObjs.getJointWithData(dataValueBase+1); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+1; rel=rotX90*rotZ90.getInverse()*sphTr.getInverse()*rotY180; newIKJoint=new CIKJoint(graphJoint,rel,true,false); lastJoint->topJoint=newIKJoint; // This is mainly needed by the joint-limitation part! ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; lastJoint->active=false; // Inactive for now (we can activate it later) avatarParent=ikObjs.getJointWithData(dataValueBase+0); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+0; localTransformation=rotY180; } else { // From base to tip C7Vector rel(localTransformation); newIKJoint=new CIKJoint(graphJoint,rel,false,true); if (lastJoint==NULL) { treeHandle=newIKJoint; lastJoint=treeHandle; ikObjs.addRoot(lastJoint); } else { ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; } lastJoint->active=false; // Inactive for now (we can activate it later) avatarParent=ikObjs.getJointWithData(dataValueBase+0); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+0; rel=sphTr*rotY90; newIKJoint=new CIKJoint(graphJoint,rel,false,true); ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; avatarParent=ikObjs.getJointWithData(dataValueBase+1); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+1; rel=rotX90.getInverse(); newIKJoint=new CIKJoint(graphJoint,rel,false,true); ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; avatarParent=ikObjs.getJointWithData(dataValueBase+2); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+2; rel=rotY90.getInverse()*rotZ90.getInverse(); newIKJoint=new CIKJoint(graphJoint,rel,true,true); newIKJoint->topJoint=newIKJoint; // Top-joint is itself! ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; avatarParent=ikObjs.getJointWithData(dataValueBase+3); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+3; localTransformation.setIdentity(); } } else { if (graphJoint->topObject==(CIKGraphObject*)previousPosition) { // From tip to base C7Vector rel(localTransformation*rotY180); newIKJoint=new CIKJoint(graphJoint,rel,false,false); localTransformation=rotY180; } else { // From base to tip C7Vector rel(localTransformation); newIKJoint=new CIKJoint(graphJoint,rel,false,false); localTransformation.setIdentity(); } if (lastJoint==NULL) { treeHandle=newIKJoint; lastJoint=treeHandle; ikObjs.addRoot(lastJoint); } else { ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; } int dataValue=10*graphJoint->nodeID+0; CIKJoint* avatarParent=ikObjs.getJointWithData(dataValue); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValue; } } else { // In case a graph-joint is disabled: if (graphJoint->topObject==(CIKGraphObject*)previousPosition) { // From tip to base localTransformation=localTransformation*graphJoint->getDownToTopTransformation().getInverse(); } else { // From base to tip localTransformation=localTransformation*graphJoint->getDownToTopTransformation(); } } } else { CIKGraphObject* theObject=(CIKGraphObject*)graphIterator; if (theObject->objectType==IK_GRAPH_LINK_OBJECT_TYPE) { // Link if (previousPosition!=NULL) { if (theObject->linkPartner!=previousPosition) localTransformation=localTransformation*previousCT.getInverse()*theObject->cumulativeTransformation; // If (theObject->linkPartner==previousPosition) then we don't do anything! } } else { // Here we have a dummy we have to assign to a configuration or a passive object // We treat all cases first as passive objects: if (previousPosition!=NULL) { localTransformation=localTransformation*previousCT.getInverse()*theObject->cumulativeTransformation; if ( (theObject->objectType==IK_GRAPH_TIP_OBJECT_TYPE)&&(lastJoint!=NULL) ) { // This is a valid dummy-tip! CIKDummy* newIKDummy=new CIKDummy(localTransformation,theObject->targetCumulativeTransformation); ikObjs.addChild(lastJoint,newIKDummy); newIKDummy->constraints=(IK_X_CONSTRAINT|IK_Y_CONSTRAINT|IK_Z_CONSTRAINT); newIKDummy->dampingFactor=1.0f; newIKDummy->loopClosureDummy=false; if (graphIterator->getConnectionNumber()==1) break; } } } } int unexploredSize=graphIterator->getNumberOfUnexplored(); if ( (unexploredSize==0)||goingDown||closeComplexLoop ) { if ( (graphIterator->getConnectionNumber()==1)&&(!closeComplexLoop) ) break; // This is a rare case where we have an endpoint without a tip-dummy mobile-part if (closeComplexLoop) { CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation); ikObjs.addChild(lastJoint,tipDummy); break; } nextPosition=graphIterator->getExploredWithSmallestExplorationID(); if ( (nextPosition->explorationID==0)&&(!goingDown) ) { // The loop can now be closed (simple loop with each joint present at most once) previousCT=((CIKGraphObject*)graphIterator)->cumulativeTransformation; localTransformation=localTransformation*previousCT.getInverse()*((CIKGraphObject*)nextPosition)->cumulativeTransformation; CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation); ikObjs.addChild(lastJoint,tipDummy); break; } if ( (nextPosition->explorationID==0)&&goingDown ) closeComplexLoop=true; goingDown=true; } else if ((graphIterator->getNeighbourWithExplorationID(0)!=NULL)&&(!goingDown)&&(previousPosition->explorationID!=0)) { // Here we have to close the loop too! // We first put unexplored paths onto the stack: for (int i=0;i<unexploredSize;i++) { // We throw unexplored nodes onto the exploration stack: graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i)); lastJoints.push_back(lastJoint); previousPositions.push_back(graphIterator); localTransformations.push_back(localTransformation); } nextPosition=graphIterator->getExploredWithSmallestExplorationID(); previousCT=((CIKGraphObject*)previousPosition)->cumulativeTransformation; localTransformation=localTransformation*previousCT.getInverse()*((CIKGraphObject*)nextPosition)->cumulativeTransformation; CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation); ikObjs.addChild(lastJoint,tipDummy); break; } else { if (previousPosition==NULL) { // This is the start. We should always explore first two links which belong together // or the 3 objects making up a joint! nextPosition=NULL; for (int i=0;i<unexploredSize;i++) { CIKGraphNode* nextPositionTmp=graphIterator->getUnexplored(i); if ( (((CIKGraphObject*)graphIterator)->linkPartner==nextPositionTmp)|| (nextPositionTmp->type==IK_GRAPH_JOINT_TYPE) ) nextPosition=nextPositionTmp; else { graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i)); lastJoints.push_back(lastJoint); previousPositions.push_back(graphIterator); localTransformations.push_back(localTransformation); if (nextPosition==NULL) nextPosition=graphIterator->getUnexplored(i); } } } else { nextPosition=graphIterator->getUnexplored(0); for (int i=1;i<unexploredSize;i++) { // We throw unexplored nodes onto the exploration stack: graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i)); lastJoints.push_back(lastJoint); previousPositions.push_back(graphIterator); localTransformations.push_back(localTransformation); } } } previousPosition=graphIterator; graphIterator=nextPosition; } } solveHierarchy(&ikObjs,parameters); for (int i=0;i<int(ikObjs.allObjects.size());i++) { CIKObject* it=ikObjs.allObjects[i]; if (it->objectType==IK_JOINT_TYPE) { CIKJoint* theJoint=(CIKJoint*)it; if (theJoint->avatarParent==NULL) { if (theJoint->spherical) { if (theJoint->topSpherical) { float a0=theJoint->parameter; float a1=((CIKJoint*)theJoint->parent)->parameter; float a2=((CIKJoint*)theJoint->parent->parent)->parameter; float a3=((CIKJoint*)theJoint->parent->parent->parent)->parameter; if (theJoint->sphericalUp) { theJoint->graphJoint->sphericalTransformation=C4Vector(a3,C3Vector(0.0f,0.0f,1.0f))*theJoint->graphJoint->sphericalTransformation*C4Vector(C3Vector(a2,a1,a0)); } else { theJoint->graphJoint->sphericalTransformation=C4Vector(a0,C3Vector(0.0f,0.0f,1.0f))*theJoint->graphJoint->sphericalTransformation*C4Vector(C3Vector(a1,a2,a3)); } } } else theJoint->graphJoint->parameter=theJoint->parameter; } } } graphContainer.actualizeAllTransformations(); graphContainer.putElementsInPlace(); return(true); }