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