C++ C7Vector::setIdentity方法代码示例

void CQDlgTranslations::_setCoord_userUnit(float newValueInUserUnit,bool orientation,int index)
{
	int editMode=App::ct->objCont->getEditModeType();
	C3DObject* object=App::ct->objCont->getLastSelection();
	if ( (editMode==NO_EDIT_MODE)&&(object!=NULL) )
	{
		C7Vector tr;
		if (coordMode==0)
			tr=object->getCumulativeTransformationPart1();
		else
			tr=object->getLocalTransformationPart1();
		tr=_getNewTransf(tr,newValueInUserUnit,orientation,index);
		if (coordMode==0)
			object->setLocalTransformation(object->getParentCumulativeTransformation().getInverse()*tr);
		else
			object->setLocalTransformation(tr);
	}
	if ( (editMode&PATH_EDIT_MODE)&&(App::ct->objCont->editModeBuffer.size()!=0)&&(App::ct->objCont->_editionPath!=NULL) )
	{
		CPathCont* pathCont=App::ct->objCont->_editionPath;
		int ind=App::ct->objCont->editModeBuffer[App::ct->objCont->editModeBuffer.size()-1];
		CSimplePathPoint* pp=pathCont->getSimplePathPoint(ind);
		CPath* path=App::ct->objCont->getPath(App::ct->objCont->getEditModeObjectID());
		if ( (pp!=NULL)&&(path!=NULL) )
		{
			C7Vector tr(pp->getTransformation());
			if (coordMode==0)
				tr=path->getCumulativeTransformationPart1()*tr;
			tr=_getNewTransf(tr,newValueInUserUnit,orientation,index);
			if (coordMode==0)
				pp->setTransformation(path->getCumulativeTransformation().getInverse()*tr,pathCont->getAttributes());
			else
				pp->setTransformation(tr,pathCont->getAttributes());
			pathCont->actualizePath();
		}
	}
	if ( (editMode&VERTEX_EDIT_MODE)&&(App::ct->objCont->editModeBuffer.size()!=0) )
	{
		int ind=App::ct->objCont->editModeBuffer[App::ct->objCont->editModeBuffer.size()-1];
		C3Vector v(App::ct->objCont->_editionVertices[3*ind+0],App::ct->objCont->_editionVertices[3*ind+1],App::ct->objCont->_editionVertices[3*ind+2]);
		CShape* shape=App::ct->objCont->getShape(App::ct->objCont->getEditModeObjectID());
		if (shape!=NULL)
		{
			C7Vector tr;
			tr.setIdentity();
			tr.X=v;
			if (coordMode==0)
				tr=shape->getCumulativeTransformationPart1()*tr;
			tr=_getNewTransf(tr,newValueInUserUnit,orientation,index);
			if (coordMode==0)
				tr=shape->getCumulativeTransformation().getInverse()*tr;
			App::ct->objCont->_editionVertices[3*ind+0]=tr.X(0);
			App::ct->objCont->_editionVertices[3*ind+1]=tr.X(1);
			App::ct->objCont->_editionVertices[3*ind+2]=tr.X(2);
		}
	}
}

C7Vector CIKGraphJoint::getDownToTopTransformation()
{
	C7Vector retVal;
	retVal.setIdentity();
	if (jointType==IK_GRAPH_SPHERICAL_JOINT_TYPE)
		retVal.Q=sphericalTransformation;
	else if (jointType==IK_GRAPH_REVOLUTE_JOINT_TYPE)
		retVal.Q.setAngleAndAxis(parameter,C3Vector(0.0f,0.0f,1.0f));
	else if (jointType==IK_GRAPH_PRISMATIC_JOINT_TYPE)
		retVal.X(2)=parameter;
	return(retVal);
}

C7Vector::C7Vector(float angle,const C3Vector& pos,const C3Vector& dir)
{ // Builds a rotation around dir at position pos of angle angle (in radians)
	C7Vector shift1;
	shift1.setIdentity();
	shift1.X(0)=-pos(0);
	shift1.X(1)=-pos(1);
	shift1.X(2)=-pos(2);
	C7Vector shift2;
	shift2.setIdentity();
	shift2.X=pos;
	C7Vector rot;
	rot.setIdentity();
	rot.Q.setAngleAndAxis(angle,dir);
	(*this)=shift2*rot*shift1;
}

void CikEl::checkIfWithinTolerance(bool& position,bool& orientation,bool useTempValues)
{
	position=true;
	orientation=true;
	CDummy* targetObj=Ct::ct->objCont->getDummy(getTarget());
	if (targetObj==NULL)
		return; // The tooltip is not constrained!
	CDummy* tooltipObj=Ct::ct->objCont->getDummy(tooltip);
	C7Vector targetM(targetObj->getCumulativeTransformationPart1(useTempValues));
	C7Vector tooltipM(tooltipObj->getCumulativeTransformationPart1(useTempValues));
 
	// Since everything is relative to the base
	C7Vector baseM;
	baseM.setIdentity();
	CDummy* baseObj=Ct::ct->objCont->getDummy(base);
	if (baseObj!=NULL)
		baseM=baseObj->getCumulativeTransformationPart1(useTempValues).getInverse();

	baseObj=Ct::ct->objCont->getDummy(alternativeBaseForConstraints);
	if (baseObj!=NULL)
		baseM=baseObj->getCumulativeTransformationPart1(useTempValues).getInverse();

	targetM=baseM*targetM;
	tooltipM=baseM*tooltipM;

	extIkReal err[2];
	getError(targetM.getMatrix(),tooltipM.getMatrix(),err,(constraints&sim_ik_x_constraint)!=0,
		(constraints&sim_ik_y_constraint)!=0,(constraints&sim_ik_z_constraint)!=0,
		(constraints&sim_ik_alpha_beta_constraint)!=0,(constraints&sim_ik_gamma_constraint)!=0);
	if (constraints&(sim_ik_x_constraint|sim_ik_y_constraint|sim_ik_z_constraint))
	{
		if (minLinearPrecision<err[0])
			position=false;
	}
	if (constraints&(sim_ik_alpha_beta_constraint|sim_ik_gamma_constraint))
	{
		if (minAngularPrecision<err[1])
			orientation=false;
	}
}

void CQDlgTranslations::_transform(C7Vector& tr,int t,bool self)
{ // t==0: rotation, t==1: translation, t==2: scaling
	if (t==2)
	{
		tr.X(0)=tr.X(0)*scalingValues[0];
		tr.X(1)=tr.X(1)*scalingValues[1];
		tr.X(2)=tr.X(2)*scalingValues[2];
	}
	else
	{
		C7Vector m;
		m.setIdentity();
		if (t==0)
			m.Q.setEulerAngles(rotAngles[0],rotAngles[1],rotAngles[2]);
		if (t==1)
			m.X.set(translationValues);
		if (self)
			tr=tr*m;
		else
			tr=m*tr;
	}
}

void CIKGraphObjCont::actualizeTransformationsWithElementID(int elementID)
{
	CIKGraphNode* it=NULL;
	if (elementID==0)
		it=getBaseObject();
	else
	{
		for (int i=0;i<int(container.size());i++)
		{
			if ( (container[i]->elementID==elementID)&&(container[i]->type==IK_GRAPH_OBJECT_TYPE) )
			{
				it=container[i];
				break;
			}
		}
	}
	C7Vector prevToCurrTr;
	prevToCurrTr.setIdentity();
	std::vector<C7Vector> prevToCurrTrs;
	prevToCurrTrs.push_back(prevToCurrTr);
	CIKGraphNode* prevNode=NULL;
	std::vector<CIKGraphNode*> prevNodes;
	prevNodes.push_back(prevNode);
	CIKGraphNode* currentNode=it;
	std::vector<CIKGraphNode*> currentNodes;
	currentNodes.push_back(currentNode);
	while (currentNodes.size()!=0)
	{
		prevToCurrTr=prevToCurrTrs.back();
		prevToCurrTrs.pop_back();
		prevNode=prevNodes.back();
		prevNodes.pop_back();
		currentNode=currentNodes.back();
		currentNodes.pop_back();
		C7Vector futurePrevOldTr;
		if (currentNode->explorationID==-1)
		{
			if ( (prevNode!=NULL)&&(prevNode->type==IK_GRAPH_JOINT_TYPE) )
			{ // We have : joint--->object
				//1. We prepare future explorations first:
				int i=0;
				while (currentNode->getUnexplored(i)!=NULL)
				{
					CIKGraphNode* aNode=currentNode->getUnexplored(i);
					C7Vector currentCTMI(((CIKGraphObject*)currentNode)->cumulativeTransformation.getInverse());
					C7Vector nextCTM(((CIKGraphObject*)aNode)->cumulativeTransformation);
					prevToCurrTrs.push_back(currentCTMI*nextCTM);
					prevNodes.push_back(currentNode);
					currentNodes.push_back(aNode);
					i++;
				}
				//2. We handle this object:
				currentNode->explorationID=0; // We flag this
				CIKGraphJoint* prevJoint=(CIKGraphJoint*)prevNode;
				if (prevJoint->topObject==currentNode)
					prevJoint->topObject->cumulativeTransformation=prevJoint->downObject->cumulativeTransformation*prevJoint->getDownToTopTransformation();
				else
					prevJoint->downObject->cumulativeTransformation=prevJoint->topObject->cumulativeTransformation*prevJoint->getDownToTopTransformation().getInverse();
			}
			else if (currentNode->type==IK_GRAPH_JOINT_TYPE)
			{ // We have : object--->joint
				//1. We prepare future explorations first:
				int i=0;
				while (currentNode->getUnexplored(i)!=NULL)
				{
					CIKGraphNode* aNode=currentNode->getUnexplored(i);
					prevToCurrTrs.push_back(prevToCurrTr); // The content doesn't matter here
					prevNodes.push_back(currentNode);
					currentNodes.push_back(aNode);
					i++;
				}
				//2. We handle this object:
				currentNode->explorationID=0; // We flag this
			}
			else
			{ // We have : object--->object or NULL--->object
				//1. We prepare future explorations first:
				int i=0;
				while (currentNode->getUnexplored(i)!=NULL)
				{
					CIKGraphNode* aNode=currentNode->getUnexplored(i);
					C7Vector currentCTMI(((CIKGraphObject*)currentNode)->cumulativeTransformation.getInverse());
					C7Vector nextCTM;
					if (aNode->type==IK_GRAPH_OBJECT_TYPE)
					{
						nextCTM=(((CIKGraphObject*)aNode)->cumulativeTransformation);
						// In case aNode is a joint type, nextCTM can be anything (above)
					}
					if (aNode->elementID==elementID)
					{ // We follow only same elementIDs!
						prevToCurrTrs.push_back(currentCTMI*nextCTM);
						prevNodes.push_back(currentNode);
						currentNodes.push_back(aNode);
					}
					i++;
				}
				//2. We handle this object:
				currentNode->explorationID=0; // We flag this
				if (prevNode!=NULL)
					((CIKGraphObject*)currentNode)->cumulativeTransformation=((CIKGraphObject*)prevNode)->cumulativeTransformation*prevToCurrTr;
//.........这里部分代码省略.........

void robot::createSensors()
{
		std::string txt("There are "+boost::lexical_cast<std::string>(vSensors.size())+" sensors.");
		printToConsole(txt.c_str());
		for(size_t i = 0; i < vSensors.size() ; i++)
		{
			sensor *Sensor = vSensors.at(i);
			if (Sensor->gazeboSpec)
				printToConsole("ERROR: sensor will not be created: the URDF specification is supported, but this is a Gazebo tag which is not documented as it seems.");
			else
			{
				if (Sensor->cameraSensorPresent)
				{
					int intParams[4]={Sensor->resolution[0],Sensor->resolution[1],0,0};
					float floatParams[11]={Sensor->clippingPlanes[0],Sensor->clippingPlanes[1],60.0f*piValue/180.0f,0.2f,0.2f,0.4f,0.0f,0.0f,0.0f,0.0f,0.0f};
					Sensor->nSensor=simCreateVisionSensor(1,intParams,floatParams,NULL);
					//Set the name:
					setVrepObjectName(Sensor->nSensor,std::string(name+"_camera").c_str());
				}
				int proxSensHandle=-1;
				if (Sensor->proximitySensorPresent)
				{ // Proximity sensors seem to be very very specific and not general / generic at all. How come?! I.e. a succession of ray description (with min/max distances) would do
					int intParams[8]={16,16,1,4,16,1,0,0};
					float floatParams[15]={0.0f,0.48f,0.1f,0.1f,0.1f,0.1f,0.0f,0.02f,0.02f,30.0f*piValue/180.0f,piValue/2.0f,0.0f,0.02f,0.0f,0.0f};
					proxSensHandle=simCreateProximitySensor(sim_proximitysensor_cone_subtype,sim_objectspecialproperty_detectable_all,0,intParams,floatParams,NULL);
					//Set the name:
					setVrepObjectName(proxSensHandle,std::string(Sensor->name+"_proximity").c_str());
				}
				// the doc doesn't state if a vision and proximity sensor can be declared at the same time...
				if (proxSensHandle!=-1)
				{
					if (Sensor->nSensor!=-1)
					{
						Sensor->nSensorAux=proxSensHandle;
						simSetObjectParent(Sensor->nSensorAux,Sensor->nSensor,true);
					}
					else
						Sensor->nSensor=proxSensHandle;
				}

				// Find the local configuration:
				C7Vector sensorLocal;
				sensorLocal.X.set(Sensor->origin_xyz);
				sensorLocal.Q=getQuaternionFromRpy(Sensor->origin_rpy);
				C4Vector rot(0.0f,0.0f,piValue); // the V-REP sensors are rotated by 180deg around the Z-axis
				sensorLocal.Q=sensorLocal.Q*rot;


				// We attach the sensor to a link:
				C7Vector x;
				x.setIdentity();
				int parentLinkIndex=getLinkPosition(Sensor->parentLink);
				if (parentLinkIndex!=-1)
				{
					int parentJointLinkIndex=getJointPosition(vLinks.at(parentLinkIndex)->parent);
					if (parentJointLinkIndex!=-1)
						x=vJoints.at(parentJointLinkIndex)->jointBaseFrame;
				}
				C7Vector sensorGlobal(x*sensorLocal);
				if (Sensor->nSensor!=-1)
				{
					simSetObjectPosition(Sensor->nSensor,-1,sensorGlobal.X.data);
					simSetObjectOrientation(Sensor->nSensor,-1,sensorGlobal.Q.getEulerAngles().data);
				}
				if ((parentLinkIndex!=-1)&&(Sensor->nSensor!=-1))
				{
					if (vLinks.at(parentLinkIndex)->visuals.size()!=0)
						simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkVisual,true);
					if (vLinks.at(parentLinkIndex)->nLinkCollision!=-1)
						simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkCollision,true);
				}
			}
		}
}

void robot::createJoints(bool hideJoints,bool positionCtrl)
{
	std::string txt("There are "+boost::lexical_cast<std::string>(vJoints.size())+" joints.");
	printToConsole(txt.c_str());

	//Set parents and childs for all the links
	for(size_t i = 0; i < vJoints.size() ; i++)
	{
		vLinks.at(getLinkPosition(vJoints.at(i)->parentLink))->child = vJoints.at(i)->name;
		vLinks.at(getLinkPosition(vJoints.at(i)->childLink))->parent = vJoints.at(i)->name;
	}

	//Create the joints
	for(size_t i = 0; i < vJoints.size() ; i++)
	{
		//Move the joints to the positions specifieds by the urdf file
		C7Vector tmp;
		tmp.setIdentity();
		tmp.X.set(vJoints.at(i)->origin_xyz);
		tmp.Q=getQuaternionFromRpy(vJoints.at(i)->origin_rpy);
		vJoints.at(i)->jointBaseFrame=vJoints.at(i)->jointBaseFrame*tmp;

		//Set name jointParent to each joint
		int nParentLink = getLinkPosition(vJoints.at(i)->parentLink);
		vJoints.at(i)->parentJoint = vLinks.at(nParentLink)->parent;

		//Create the joint/forceSensor/dummy:
		if (vJoints.at(i)->jointType==-1)
			vJoints.at(i)->nJoint = simCreateDummy(0.02f,NULL); // when joint type was not recognized
		if (vJoints.at(i)->jointType==0)
			vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
		if (vJoints.at(i)->jointType==1)
			vJoints.at(i)->nJoint = simCreateJoint(sim_joint_prismatic_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
		if (vJoints.at(i)->jointType==2)
			vJoints.at(i)->nJoint = simCreateJoint(sim_joint_spherical_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
		if (vJoints.at(i)->jointType==3)
			vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
		if (vJoints.at(i)->jointType==4)
		{ // when joint type is "fixed"
			int intParams[5]={1,4,4,0,0};
			float floatParams[5]={0.02f,1.0f,1.0f,0.0f,0.0f};
			vJoints.at(i)->nJoint = simCreateForceSensor(0,intParams,floatParams,NULL);
		}

		if ( (vJoints.at(i)->jointType==0)||(vJoints.at(i)->jointType==1) )
		{
			float interval[2]={vJoints.at(i)->lowerLimit,vJoints.at(i)->upperLimit-vJoints.at(i)->lowerLimit};
			simSetJointInterval(vJoints.at(i)->nJoint,0,interval);
			if (vJoints.at(i)->jointType==0)
			{ // revolute
				simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitAngular);
				simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitAngular);
			}
			else
			{ // prismatic
				simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitLinear);
				simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitLinear);
			}
			// We turn the position control on:
			if (positionCtrl)
			{
				simSetObjectIntParameter(vJoints.at(i)->nJoint,2000,1);
				simSetObjectIntParameter(vJoints.at(i)->nJoint,2001,1);
			}
		}

		//Set the name:
		setVrepObjectName(vJoints.at(i)->nJoint,vJoints.at(i)->name.c_str());
		if (hideJoints)
			simSetObjectIntParameter(vJoints.at(i)->nJoint,10,512); // layer 10
	}

	//Set positions to joints from the 4x4matrix
	for(size_t i = 0; i < vJoints.size() ; i++)
	{
		simSetObjectPosition(vJoints.at(i)->nJoint,-1,vJoints.at(i)->jointBaseFrame.X.data);
		simSetObjectOrientation(vJoints.at(i)->nJoint,-1 ,vJoints.at(i)->jointBaseFrame.Q.getEulerAngles().data);
	}

	//Set joint parentship between them (thes parentship will be remove before adding the joints)
	for(size_t i = 0; i < vJoints.size() ; i++)
	{   
		int parentJointIndex=getJointPosition(vJoints.at(i)->parentJoint);
		if ( parentJointIndex!= -1)
		{
			simInt nParentJoint = vJoints.at(parentJointIndex)->nJoint;
			simInt nJoint = vJoints.at(i)->nJoint;
			simSetObjectParent(nJoint,nParentJoint,false);
		}	
	}

	//Delete all the partnership without moving the joints but after doing that update the transform matrix
	for(size_t i = 0; i < vJoints.size() ; i++)
	{ 
		C4X4Matrix tmp;  
		simGetObjectPosition(vJoints.at(i)->nJoint,-1,tmp.X.data);
		C3Vector euler;
		simGetObjectOrientation(vJoints.at(i)->nJoint,-1,euler.data);
		tmp.M.setEulerAngles(euler);
		vJoints.at(i)->jointBaseFrame = tmp;
//.........这里部分代码省略.........

void CQDlgTranslations::_applyTransformation(int t)
{ // t==0: rotation, t==1: translation, t==2: scaling
	int editMode=App::ct->objCont->getEditModeType();
	int objSelSize=App::ct->objCont->getSelSize();
	int editObjSelSize=App::ct->objCont->editModeBuffer.size();
	if ( (editMode==NO_EDIT_MODE)&&(objSelSize>0) )
	{
		for (int i=0;i<objSelSize;i++)
		{
			C3DObject* object=App::ct->objCont->getObject(App::ct->objCont->getSelID(i));
			bool hasParentPresent=false;
			if ((transfMode==0)&&(t!=2)) // scaling is different!
			{ // We do a transformation relative to the world. If this object has a parent that also is selected, we don't process this object!
				C3DObject* p=object->getParent();
				while (p!=NULL)
				{
					for (int j=0;j<objSelSize;j++)
					{
						if (App::ct->objCont->getSelID(j)==p->getID())
						{
							hasParentPresent=true;
							break;
						}
					}
					if (hasParentPresent)
						break;
					p=p->getParent();
				}
			}
			if (!hasParentPresent)
			{
				C7Vector tr;
				if (transfMode==0)
					tr=object->getCumulativeTransformationPart1();
				else
					tr=object->getLocalTransformationPart1();
				_transform(tr,t,transfMode==2);
				if (transfMode==0)
					tr=object->getParentCumulativeTransformation().getInverse()*tr;
				object->setLocalTransformation(tr);
			}
		}
	}
	if ( (editMode&PATH_EDIT_MODE)&&(editObjSelSize>0)&&(App::ct->objCont->_editionPath!=NULL) )
	{
		CPathCont* pathCont=App::ct->objCont->_editionPath;
		CPath* path=App::ct->objCont->getPath(App::ct->objCont->getEditModeObjectID());
		for (int i=0;i<editObjSelSize;i++)
		{
			CSimplePathPoint* pp=pathCont->getSimplePathPoint(App::ct->objCont->editModeBuffer[i]);
			if ( (pp!=NULL)&&(path!=NULL) )
			{
				C7Vector tr(pp->getTransformation());
				if (transfMode==0)
					tr=path->getCumulativeTransformationPart1()*tr;
				_transform(tr,t,transfMode==2);
				if (transfMode==0)
					tr=path->getCumulativeTransformationPart1().getInverse()*tr;
				pp->setTransformation(tr,pathCont->getAttributes());
			}
		}
		pathCont->actualizePath();
	}
	if ( (editMode&VERTEX_EDIT_MODE)&&(editObjSelSize>0) )
	{
		CShape* shape=App::ct->objCont->getShape(App::ct->objCont->getEditModeObjectID());
		if (shape!=NULL)
		{
			for (int i=0;i<editObjSelSize;i++)
			{
				C7Vector tr;
				tr.setIdentity();
				int ind=App::ct->objCont->editModeBuffer[i];
				tr.X.set(&App::ct->objCont->_editionVertices[3*ind+0]);
				if (transfMode==0)
					tr=shape->getCumulativeTransformationPart1()*tr;
				_transform(tr,t,transfMode==2);
				if (transfMode==0)
					tr=shape->getCumulativeTransformationPart1().getInverse()*tr;
				App::ct->objCont->_editionVertices[3*ind+0]=tr.X(0);
				App::ct->objCont->_editionVertices[3*ind+1]=tr.X(1);
				App::ct->objCont->_editionVertices[3*ind+2]=tr.X(2);
			}
		}
	}
}

void CQDlgTranslations::_applyCoord(bool orientation,int mask)
{
	int editMode=App::ct->objCont->getEditModeType();
	C3DObject* object=App::ct->objCont->getLastSelection();
	int objSelSize=App::ct->objCont->getSelSize();
	int editObjSelSize=App::ct->objCont->editModeBuffer.size();
	if ( (editMode==NO_EDIT_MODE)&&(object!=NULL)&&(objSelSize>1) )
	{
		C7Vector tr;
		if (coordMode==0)
			tr=object->getCumulativeTransformationPart1();
		else
			tr=object->getLocalTransformationPart1();
		for (int i=0;i<objSelSize-1;i++)
		{
			C3DObject* it=App::ct->objCont->getObject(App::ct->objCont->getSelID(i));
			C7Vector trIt;
			if (coordMode==0)
				trIt=it->getCumulativeTransformationPart1();
			else
				trIt=it->getLocalTransformationPart1();
			_copyTransf(tr,trIt,orientation,mask);
			if (coordMode==0)
				it->setLocalTransformation(it->getParentCumulativeTransformation().getInverse()*trIt);
			else
				it->setLocalTransformation(trIt);
		}
	}
	if ( (editMode&PATH_EDIT_MODE)&&(editObjSelSize>1)&&(App::ct->objCont->_editionPath!=NULL) )
	{
		CPathCont* pathCont=App::ct->objCont->_editionPath;
		int ind=App::ct->objCont->editModeBuffer[App::ct->objCont->editModeBuffer.size()-1];
		CSimplePathPoint* pp=pathCont->getSimplePathPoint(ind);
		CPath* path=App::ct->objCont->getPath(App::ct->objCont->getEditModeObjectID());
		if ( (pp!=NULL)&&(path!=NULL) )
		{
			C7Vector tr(pp->getTransformation());
			if (coordMode==0)
				tr=path->getCumulativeTransformationPart1()*tr;
			for (int i=0;i<editObjSelSize-1;i++)
			{
				CSimplePathPoint* ppIt=pathCont->getSimplePathPoint(App::ct->objCont->editModeBuffer[i]);
				if (ppIt!=NULL)
				{
					C7Vector trIt(ppIt->getTransformation());
					if (coordMode==0)
						trIt=path->getCumulativeTransformationPart1()*trIt;
					_copyTransf(tr,trIt,orientation,mask);
					if (coordMode==0)
						trIt=path->getCumulativeTransformationPart1().getInverse()*trIt;
					ppIt->setTransformation(trIt,pathCont->getAttributes());
				}
			}
			pathCont->actualizePath();
		}
	}
	if ( (editMode&VERTEX_EDIT_MODE)&&(editObjSelSize>1) )
	{
		int ind=App::ct->objCont->editModeBuffer[App::ct->objCont->editModeBuffer.size()-1];
		C3Vector v(App::ct->objCont->_editionVertices[3*ind+0],App::ct->objCont->_editionVertices[3*ind+1],App::ct->objCont->_editionVertices[3*ind+2]);
		CShape* shape=App::ct->objCont->getShape(App::ct->objCont->getEditModeObjectID());
		if (shape!=NULL)
		{
			C7Vector tr;
			tr.setIdentity();
			tr.X=v;
			if (coordMode==0)
				tr=shape->getCumulativeTransformationPart1()*tr;
			for (int i=0;i<editObjSelSize-1;i++)
			{
				ind=App::ct->objCont->editModeBuffer[i];
				C7Vector trIt;
				trIt.setIdentity();
				trIt.X.set(&App::ct->objCont->_editionVertices[3*ind+0]);
				if (coordMode==0)
					trIt=shape->getCumulativeTransformationPart1()*trIt;
				_copyTransf(tr,trIt,orientation,mask);
				if (coordMode==0)
					trIt=shape->getCumulativeTransformationPart1().getInverse()*trIt;
				App::ct->objCont->_editionVertices[3*ind+0]=trIt.X(0);
				App::ct->objCont->_editionVertices[3*ind+1]=trIt.X(1);
				App::ct->objCont->_editionVertices[3*ind+2]=trIt.X(2);
			}
		}
	}
}

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
//.........这里部分代码省略.........

CMatrix* CIkRoutine::getJacobian(CikEl* ikElement,C4X4Matrix& tooltipTransf,std::vector<int>* rowJointIDs,std::vector<int>* rowJointStages)
{	// rowJointIDs is NULL by default. If not null, it will contain the ids of the joints
	// corresponding to the rows of the jacobian.
	// Return value NULL means that is ikElement is either inactive, either invalid
	// tooltipTransf is the cumulative transformation matrix of the tooltip,
	// computed relative to the base!
	// The temporary joint parameters need to be initialized before calling this function!
	// We check if the ikElement's base is in the chain and that tooltip is valid!
	CDummy* tooltip=ct::objCont->getDummy(ikElement->getTooltip());
	if (tooltip==NULL)
	{ // Should normally never happen!
		ikElement->setActive(false);
		return(NULL);
	}
	C3DObject* base=ct::objCont->getObject(ikElement->getBase());
	if ( (base!=NULL)&&(!tooltip->isObjectParentedWith(base)) )
	{ // This case can happen (when the base's parenting was changed for instance)
		ikElement->setBase(-1);
		ikElement->setActive(false);
		return(NULL);
	}

	// We check the number of degrees of freedom and prepare the rowJointIDs vector:
	C3DObject* iterat=tooltip;
	int doF=0;
	while (iterat!=base)
	{
		iterat=iterat->getParent();
		if ( (iterat!=NULL)&&(iterat!=base) )
		{
			if (iterat->getObjectType()==sim_object_joint_type)
			{
				if ( (((CJoint*)iterat)->getJointMode()==sim_jointmode_ik)||(((CJoint*)iterat)->getJointMode()==sim_jointmode_ikdependent) )
				{
					int d=((CJoint*)iterat)->getDoFs();
					for (int i=d-1;i>=0;i--)
					{
						if (rowJointIDs!=NULL)
						{
							rowJointIDs->push_back(iterat->getID());
							rowJointStages->push_back(i);
						}
					}
					doF+=d;
				}
			}
		}
	}
	CMatrix* J=new CMatrix(6,(unsigned char)doF);
	std::vector<C4X4FullMatrix*> jMatrices;
	for (int i=0;i<(doF+1);i++)
	{
		C4X4FullMatrix* matr=new C4X4FullMatrix();
		if (i==0)
			(*matr).setIdentity();
		else
			(*matr).clear();
		jMatrices.push_back(matr);
	}

	// Now we go from tip to base:
	iterat=tooltip;
	C4X4FullMatrix buff;
	buff.setIdentity();
	int positionCounter=0;
	C4X4FullMatrix d0;
	C4X4FullMatrix dp;
	C4X4FullMatrix paramPart;
	CJoint* lastJoint=NULL;
	int indexCnt=-1;
	int indexCntLast=-1;
	while (iterat!=base)
	{
		C3DObject* nextIterat=iterat->getParent();
		C7Vector local;
		if (iterat->getObjectType()==sim_object_joint_type)
		{
			if ( (((CJoint*)iterat)->getJointMode()!=sim_jointmode_ik)&&(((CJoint*)iterat)->getJointMode()!=sim_jointmode_ikdependent) )
				local=iterat->getLocalTransformation(true);
			else
			{
				CJoint* it=(CJoint*)iterat;
				if (it->getJointType()==sim_joint_spherical_subtype)
				{
					if (indexCnt==-1)
						indexCnt=it->getDoFs()-1;
					it->getLocalTransformationExPart1(local,indexCnt--,true);
					if (indexCnt!=-1)
						nextIterat=iterat; // We keep the same object! (but indexCnt has decreased)
				}
				else
					local=iterat->getLocalTransformationPart1(true);
			}
		}
		else
			local=iterat->getLocalTransformation(true); 

		buff=C4X4FullMatrix(local.getMatrix())*buff;
		iterat=nextIterat;
		bool activeJoint=false;
//.........这里部分代码省略.........

CIKGraphNode* CGeometricConstraintSolverInt::createTree(CIKGraphObjCont& graphContainer,C3DObject* objectOnTree,std::vector<C3DObject*>& exploredObjs,std::vector<C3DObject*>& links,bool keepShapes,int& baseObjectID)
{  // Creates a tree of linked objects (linked dummies are not followed!)
	// Return value is the base IKGraphObject of that tree
	CIKGraphNode* toBeReturned=NULL;
	while (objectOnTree->getParent()!=NULL)
		objectOnTree=objectOnTree->getParent();
	baseObjectID=objectOnTree->getID();
	std::vector<C3DObject*> objectsToExplore;
	std::vector<CIKGraphObject*> lastAdded;
	objectsToExplore.push_back(objectOnTree);
	lastAdded.push_back(NULL);
	std::vector<CIKGraphObject*> noParent;
	while (objectsToExplore.size()!=0)
	{
		C3DObject* object=objectsToExplore.back();
		objectsToExplore.pop_back();
		CIKGraphObject* lastAddedNode=lastAdded.back();
		lastAdded.pop_back();
		// 1. We have to insert this object (maybe)
		int insert=-1;
		CJoint* act=NULL;
		CDummy* dum=NULL;
		if ( (object->getObjectType()==sim_object_shape_type) )
			insert=3;
		if (object->getObjectType()==sim_object_joint_type)
		{
			act=(CJoint*)object;
			if ( (act->getJointMode()==sim_jointmode_ik)||(act->getJointMode()==sim_jointmode_ikdependent) )
				insert=0;
		}
		if (object->getObjectType()==sim_object_dummy_type)
		{
			dum=(CDummy*)object;

			if (dum->getLinkedDummyID()!=-1)
			{
				if (dum->getLinkType()==sim_dummy_linktype_gcs_loop_closure)
					insert=1;
				if (dum->getLinkType()==sim_dummy_linktype_gcs_tip)
					insert=2;
			}
		}
		if (insert!=-1)
		{
			CIKGraphObject* justInsertedObject=NULL;
			CIKGraphJoint* justInsertedJoint=NULL;
			C7Vector transf(object->getCumulativeTransformationPart1());
			if (insert==0)
			{
				if (act->getJointType()==sim_joint_revolute_subtype)
					justInsertedJoint=graphContainer.insertRevoluteJointNode(transf,act->getPosition(),act->getPositionIntervalMin(),act->getPositionIntervalRange(),act->getScrewPitch(),act->getPositionIsCyclic(),act->getIKWeight()); // added getPositionIsCyclic on 2009/07/11
				if (act->getJointType()==sim_joint_prismatic_subtype)
					justInsertedJoint=graphContainer.insertPrismaticJointNode(transf,act->getPosition(),act->getPositionIntervalMin(),act->getPositionIntervalRange(),act->getIKWeight());
				if (act->getJointType()==sim_joint_spherical_subtype)
					justInsertedJoint=graphContainer.insertBallJointNode(transf,act->getSphericalTransformation(),act->getPositionIntervalRange(),act->getIKWeight());
			}
			else if (insert==1)
			{ // if we enter in this section, it is sure the dummies are linked and the link type is GCS_LOOP_CLOSURE
				int data=dum->getID();
				if (data>dum->getLinkedDummyID())
					data=dum->getLinkedDummyID();
				justInsertedObject=graphContainer.insertPassiveObjectNode(transf);	
				justInsertedObject->userData0=data;
				links.push_back(App::ct->objCont->getDummy(dum->getLinkedDummyID()));
			}
			else if (insert==2)
			{  // if we enter in this section, it is sure the dummies are linked and the link type is GCS_TIP
				CDummy* targetD=App::ct->objCont->getDummy(dum->getLinkedDummyID());
				justInsertedObject=graphContainer.insertTipObjectNode(transf,targetD->getCumulativeTransformation());	
			}
			else if (insert==3)
			{
				justInsertedObject=graphContainer.insertPassiveObjectNode(transf);
//*********************************** IK Manipulation *********************************
				if (object->getID()==App::ct->objCont->_ikManipulationObjectID)
				{
					C7Vector tipTransf;
					C7Vector targetTransf;
					tipTransf.setIdentity();
					targetTransf.setIdentity();
					tipTransf.X=App::ct->objCont->_ikManipulationStartPosRel;
					tipTransf=object->getCumulativeTransformation()*tipTransf;
					targetTransf.X=App::ct->objCont->_ikManipulationCurrentPosAbs;
					CIKGraphObject* constr=graphContainer.insertTipObjectNode(tipTransf,targetTransf);
					justInsertedObject->linkWithObject(constr);				
				}
//*************************************************************************************
			}
			if (insert!=0)
			{
				justInsertedObject->userData1=object->getID();
				if (lastAddedNode!=NULL)
				{
					lastAddedNode->linkWithObject(justInsertedObject);
					lastAddedNode=justInsertedObject;
				}
				else
				{
					lastAddedNode=justInsertedObject;
					if (toBeReturned==NULL)
//.........这里部分代码省略.........