Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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!
}
Exemplo n.º 3
0
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);
}