예제 #1
0
파일: Path.cpp 프로젝트: dtbinh/vrep_altair
bool CPath::transformSelectedPathPoints(const C4X4Matrix& cameraAbsConf,const C3Vector& clicked3DPoint,float prevPos[2],float pos[2],float screenHalfSizes[2],float halfSizes[2],bool perspective,int eventID)
{
	C3Vector pointCenter;
	pointCenter.clear();
	CPathCont* pc;
	std::vector<int> selectedPathPoints;
	if (App::ct->objCont->getEditModeType()&PATH_EDIT_MODE)
	{
		pc=App::ct->objCont->_editionPath;
		selectedPathPoints.assign(App::ct->objCont->editModeBuffer.begin(),App::ct->objCont->editModeBuffer.end());
	}
	else
	{
		pc=pathContainer;
		selectedPathPoints.assign(App::ct->objCont->getPointerToSelectedPathPointIndices_nonEditMode()->begin(),App::ct->objCont->getPointerToSelectedPathPointIndices_nonEditMode()->end());
	}
	for (int i=0;i<int(selectedPathPoints.size());i++)
	{
		CSimplePathPoint* aPt=pc->getSimplePathPoint(selectedPathPoints[i]);
		if (aPt!=NULL)
		{
			pointCenter+=(getCumulativeTransformationPart1()*aPt->getTransformation()).X;
		}
	}
	if (selectedPathPoints.size()!=0)
		pointCenter/=float(selectedPathPoints.size());

	C4X4Matrix objAbs;
	objAbs.X=pointCenter;
		objAbs.M=getCumulativeTransformationPart1().getMatrix().M;
	bool ctrlKeyDown=((App::mainWindow!=NULL)&&App::mainWindow->ctrlKeyDown);
	if (eventID!=_objectManipulationModeEventId)
		_objectManipulationModeRelativePositionOfClickedPoint=clicked3DPoint-objAbs.X;
	if ( (eventID!=_objectManipulationModeEventId)||(ctrlKeyDown!=_objectManipulationModePermissionsPreviousCtrlKeyDown) )
	{
		_objectManipulationModeSubTranslation.clear();
		_objectManipulationModeSubRotation=0.0f;
		_objectManipulationModeEventId=eventID;
		_objectManipulationModeTotalTranslation.clear();
		_objectManipulationModeTotalRotation=0.0f;
		// Let's first see on which plane we wanna translate:
		bool specialMode=false;
		if (ctrlKeyDown)
			specialMode=true;
		_objectManipulationModeAxisIndex=2; // x/y plane
		if (specialMode&&((pc->getAttributes()&sim_pathproperty_flat_path)==0))
			_objectManipulationModeAxisIndex+=3;
	}

	_objectManipulationModePermissionsPreviousCtrlKeyDown=ctrlKeyDown;

	C4X4Matrix originalPlane(objAbs); // x-y plane
	originalPlane.X+=_objectManipulationModeRelativePositionOfClickedPoint;
	bool projectOntoXAxis=false;
	if (_objectManipulationModeAxisIndex==5)
	{ // z axis
		projectOntoXAxis=true;
		C3X3Matrix rot;
		rot.buildYRotation(piValD2);
		originalPlane.M*=rot; 
	}

	C4X4Matrix plane(originalPlane);
	C3Vector p[2]; // previous and current point on the plane
	float d=-(plane.X*plane.M.axis[2]);
	float screenP[2]={prevPos[0],prevPos[1]};
	C4X4Matrix cam(cameraAbsConf);
	bool singularityProblem=false;

	for (int pass=0;pass<2;pass++)
	{
		float tt[2];
		for (int i=0;i<2;i++)
		{
			if (i==1)
			{
				screenP[0]=pos[0];
				screenP[1]=pos[1];
			}
			C3Vector pp(cam.X);
			if (!perspective)
			{
				if (fabs(cam.M.axis[2]*plane.M.axis[2])<0.05f)
				{
					singularityProblem=true;
					break;
				}
				pp-=cam.M.axis[0]*halfSizes[0]*(screenP[0]/screenHalfSizes[0]);
				pp+=cam.M.axis[1]*halfSizes[1]*(screenP[1]/screenHalfSizes[1]);
				float t=(-d-(plane.M.axis[2]*pp))/(cam.M.axis[2]*plane.M.axis[2]);
				p[i]=pp+cam.M.axis[2]*t;
			}
			else
			{
				C3Vector v(cam.M.axis[2]+cam.M.axis[0]*tan(-screenP[0])+cam.M.axis[1]*tan(screenP[1]));
				v.normalize();
				pp+=v;
				if (fabs(v*plane.M.axis[2])<0.05f)
				{
					singularityProblem=true;
					break;
				}
				float t=(-d-(plane.M.axis[2]*pp))/(v*plane.M.axis[2]);
				tt[i]=t;
				p[i]=pp+v*t;
			}
		}
		if (!singularityProblem)
		{
			if ((!perspective)||(tt[0]*tt[1]>0.0f))
				break;
			singularityProblem=true;
		}
		plane.M=cam.M;
	}
	if (projectOntoXAxis)
	{
		C4X4Matrix inv(originalPlane.getInverse());
		p[0]*=inv;
		p[1]*=inv;
		p[0](1)=0.0f;
		p[0](2)=0.0f;
		p[1](1)=0.0f;
		p[1](2)=0.0f;
		p[0]*=originalPlane;
		p[1]*=originalPlane;
	}
	else
	{
		if (singularityProblem)
		{ // we have to project the coordinates onto the original plane:
			C4X4Matrix inv(originalPlane.getInverse());
			p[0]*=inv;
			p[1]*=inv;
			p[0](2)=0.0f;
			p[1](2)=0.0f;
			p[0]*=originalPlane;
			p[1]*=originalPlane;
		}
	}

	// We snap the translation!
	C3Vector v(p[1]-p[0]);
	v=objAbs.getInverse().M*v;
	_objectManipulationModeSubTranslation+=v;
	for (int i=0;i<3;i++)
	{
		float ss=getNonDefaultTranslationStepSize_currentSettings();
		if (ss==0.0f)
			ss=App::userSettings->getTranslationStepSize();
		if ((App::mainWindow!=NULL)&&App::mainWindow->shiftKeyDown)
			ss=0.001f;
		float w=fmod(_objectManipulationModeSubTranslation(i),ss);
		v(i)=_objectManipulationModeSubTranslation(i)-w;
		_objectManipulationModeTotalTranslation(i)+=_objectManipulationModeSubTranslation(i)-w;
		_objectManipulationModeSubTranslation(i)=w;
	}
	v=objAbs.M*v;
	for (int i=0;i<int(selectedPathPoints.size());i++)
	{
		CSimplePathPoint* aPt=pc->getSimplePathPoint(selectedPathPoints[i]);
		if (aPt!=NULL)
		{
			C4X4Matrix m(getCumulativeTransformationPart1()*aPt->getTransformation());
			m.X+=v;
			aPt->setTransformation(getCumulativeTransformationPart1().getInverse().getMatrix()*m,pc->getAttributes());
		}
	}
	_objectManipulationMode_flaggedForGridOverlay=_objectManipulationModeAxisIndex+16;
	pc->actualizePath();
	return(true);
}
예제 #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!
}
예제 #3
0
CIKChain::CIKChain(CIKDummy* tip,float interpolFact,int jointNbToExclude)
{
	tooltip=tip;
	chainIsValid=true;
	// interpolFact is the interpolation factor we use to compute the target pose:
	// interpolPose=tooltipPose*(1-interpolFact)+targetPose*interpolFact
	// We get the jacobian and the rowJointIDs:
	C4X4Matrix oldMatr;
	int theConstraints=tip->constraints;
	tip->baseReorient.setIdentity();
	CMatrix* Ja=NULL;

	if (tip->loopClosureDummy)
	{
		// The purpose of the following code is to have tip and target dummies reoriented
		// and the appropriate constraints calculated automatically (only for loop closure dummies!)
		tip->constraints=0;
		int posDimensions=0;
		int orDimensions=0;
		C3Vector firstJointZAxis(0.0f,0.0f,1.0f);
		C4X4Matrix firstJointCumul;
		// here we reorient tip and targetCumulativeMatrix
		// 1. We find the first revolute joint:
		CIKObject* iterat=tip->parent;
		while (iterat!=NULL)
		{
			if ( (iterat->objectType==IK_JOINT_TYPE)&&((CIKJoint*)iterat)->revolute&&((CIKJoint*)iterat)->active )
				break;
			iterat=iterat->parent;
		}
		if (iterat!=NULL)
		{ // We have the first revolute joint from tip
			orDimensions=1;
			CIKObject* firstJoint=iterat;
			firstJointCumul=C4X4Matrix(firstJoint->getCumulativeTransformationPart1(true).getMatrix());
			firstJointZAxis=firstJointCumul.M.axis[2];
			C3Vector normalVect;
			// We search for a second revolute joint which is not aligned with the first one:
			iterat=iterat->parent;
			while (iterat!=NULL)
			{
				if ( (iterat->objectType==IK_JOINT_TYPE)&&((CIKJoint*)iterat)->revolute&&((CIKJoint*)iterat)->active )
				{
					C4X4Matrix secondJointCumul(iterat->getCumulativeTransformationPart1(true).getMatrix());
					C3Vector secondJointZAxis(secondJointCumul.M.axis[2]);
					if (fabs(firstJointZAxis*secondJointZAxis)<0.999999f) // Approx. 0.08 degrees
					{
						normalVect=(firstJointZAxis^secondJointZAxis).getNormalized();
						if (firstJointZAxis*secondJointZAxis<0.0f)
							secondJointZAxis=secondJointZAxis*-1.0f;
						firstJointZAxis=((firstJointZAxis+secondJointZAxis)/2.0f).getNormalized();
						break;
					}
				}
				iterat=iterat->parent;
			}
			if (iterat!=NULL)
			{
				orDimensions=2;
				// We search for a third revolute joint which is not orthogonal with normalVect:
				iterat=iterat->parent;
				while (iterat!=NULL)
				{
					if ( (iterat->objectType==IK_JOINT_TYPE)&&((CIKJoint*)iterat)->revolute&&((CIKJoint*)iterat)->active )
					{
						C4X4Matrix thirdJointCumul(iterat->getCumulativeTransformationPart1(true).getMatrix());
						C3Vector thirdJointZAxis(thirdJointCumul.M.axis[2]);
						if (fabs(normalVect*thirdJointZAxis)>0.0001f) // Approx. 0.005 degrees
						{
							orDimensions=3;
							break;
						}
					}
					iterat=iterat->parent;
				}
			}
		}			
		if ( (orDimensions==1)||(orDimensions==2) )
		{
			// We align the tip dummy's z axis with the joint axis 
			// (and do the same transformation to targetCumulativeMatrix)

			C4Vector rot(C4X4Matrix(firstJointZAxis).M.getQuaternion());
			C4Vector tipParentInverse(tip->getParentCumulativeTransformation(true).Q.getInverse());
			C4Vector tipNewLocal(tipParentInverse*rot);
			C4Vector postTr(tip->getLocalTransformation(true).Q.getInverse()*tipNewLocal);
			tip->transformation.Q=tipNewLocal;
			C7Vector postTr2;
			postTr2.setIdentity();
			postTr2.Q=postTr;
			tip->targetCumulativeTransformation=tip->targetCumulativeTransformation*postTr2;
		}
		Ja=getJacobian(tip,oldMatr,rowJoints,jointNbToExclude);
		C3Vector posV;
		for (int i=0;i<Ja->cols;i++)
		{
			// 1. Position space:
			C3Vector vc((*Ja)(0,i),(*Ja)(1,i),(*Ja)(2,i));
			float l=vc.getLength();
			if (l>0.0001f) // 0.1 mm
			{
				vc=vc/l;
				if (posDimensions==0)
				{
					posV=vc;
					posDimensions++;
				}
				else if (posDimensions==1)
				{
					if (fabs(posV*vc)<0.999999f) // Approx. 0.08 degrees
					{
						posV=(posV^vc).getNormalized();
						posDimensions++;
					}
				}
				else if (posDimensions==2)
				{
					if (fabs(posV*vc)>0.0001f) // Approx. 0.005 degrees
						posDimensions++;
				}
			}
		}
		if (posDimensions!=3)
		{
			if (posDimensions!=0)
			{
				C4X4Matrix aligned(posV);
				tip->baseReorient=aligned.getInverse().getTransformation();
				for (int i=0;i<Ja->cols;i++)
				{
					posV(0)=(*Ja)(0,i);
					posV(1)=(*Ja)(1,i);
					posV(2)=(*Ja)(2,i);
					posV=tip->baseReorient.Q*posV; // baseReorient.X is 0 anyway...
					(*Ja)(0,i)=posV(0);
					(*Ja)(1,i)=posV(1);
					(*Ja)(2,i)=posV(2);
				}
				oldMatr=tip->baseReorient.getMatrix()*oldMatr;
			
				if (posDimensions==1)
					tip->constraints+=IK_Z_CONSTRAINT;
				else
					tip->constraints+=(IK_X_CONSTRAINT+IK_Y_CONSTRAINT);
			}
		}
		else
			tip->constraints+=(IK_X_CONSTRAINT+IK_Y_CONSTRAINT+IK_Z_CONSTRAINT);

		int doF=Ja->cols;
		if (orDimensions==1)
		{
			if (doF-posDimensions>=1)
				tip->constraints+=IK_GAMMA_CONSTRAINT;
		}
		else if (orDimensions==2)
		{
			if (doF-posDimensions>=1) // Is this correct?
				tip->constraints+=IK_GAMMA_CONSTRAINT;
		}
		else if (orDimensions==3)
		{
			if (doF-posDimensions>=3)
				tip->constraints+=(IK_ALPHA_BETA_CONSTRAINT|IK_GAMMA_CONSTRAINT);
		}
		
		theConstraints=tip->constraints;
	}
	else
		Ja=getJacobian(tip,oldMatr,rowJoints,jointNbToExclude);

	if (Ja==NULL)
	{ // Error or not enough joints (effJoint=joints-jointNbToExclude) to get a Jacobian
		delete Ja;
		// We create dummy objects (so that we don't get an error when destroyed)
		matrix=new CMatrix(0,0);
		errorVector=new CMatrix(0,1);
		chainIsValid=false;
		return;
	}
	// oldMatr now contains the cumulative transf. matr. of tooltip relative to base
	C4X4Matrix oldMatrInv(oldMatr.getInverse());
	int doF=Ja->cols;
	int equationNumber=0;

	C4X4Matrix m;
	C4X4Matrix targetCumulativeMatrix((tip->baseReorient*tip->targetCumulativeTransformation).getMatrix());
	m.buildInterpolation(oldMatr,targetCumulativeMatrix,interpolFact);
	// We prepare matrix and errorVector and their respective sizes:
	if (theConstraints&IK_X_CONSTRAINT)
		equationNumber++;
	if (theConstraints&IK_Y_CONSTRAINT)
		equationNumber++;
	if (theConstraints&IK_Z_CONSTRAINT)
		equationNumber++;
	if (theConstraints&IK_ALPHA_BETA_CONSTRAINT)
		equationNumber=equationNumber+2;
	if (theConstraints&IK_GAMMA_CONSTRAINT)
		equationNumber++;
	matrix=new CMatrix(equationNumber,doF);
	errorVector=new CMatrix(equationNumber,1);
	// We set up the position/orientation errorVector and the matrix:
	
	float positionWeight=1.0f;
	float orientationWeight=1.0f;
	int pos=0;
	if (theConstraints&IK_X_CONSTRAINT)
	{
		for (int i=0;i<doF;i++)
			(*matrix)(pos,i)=(*Ja)(0,i);
		(*errorVector)(pos,0)=(m.X(0)-oldMatr.X(0))*positionWeight;
		pos++;
	}
	if (theConstraints&IK_Y_CONSTRAINT)
	{
		for (int i=0;i<doF;i++)
			(*matrix)(pos,i)=(*Ja)(1,i);
		(*errorVector)(pos,0)=(m.X(1)-oldMatr.X(1))*positionWeight;
		pos++;
	}
	if (theConstraints&IK_Z_CONSTRAINT)
	{
		for (int i=0;i<doF;i++)
			(*matrix)(pos,i)=(*Ja)(2,i);
		(*errorVector)(pos,0)=(m.X(2)-oldMatr.X(2))*positionWeight;
		pos++;
	}

	if (theConstraints&IK_ALPHA_BETA_CONSTRAINT)
	{
		for (int i=0;i<doF;i++)
		{
			(*matrix)(pos,i)=(*Ja)(3,i);
			(*matrix)(pos+1,i)=(*Ja)(4,i);
		}
		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 (theConstraints&IK_GAMMA_CONSTRAINT)
	{
		for (int i=0;i<doF;i++)
			(*matrix)(pos,i)=(*Ja)(5,i);
		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!
}