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;
simInt nJoint = vJoints.at(i)->nJoint;
simSetObjectParent(nJoint,-1,true);
}
for(size_t i = 0; i < vJoints.size() ; i++)
{
C4X4Matrix jointAxisMatrix;
jointAxisMatrix.setIdentity();
C3Vector axis(vJoints.at(i)->axis);
C3Vector rotAxis;
float rotAngle=0.0f;
if (axis(2)<1.0f)
{
if (axis(2)<=-1.0f)
rotAngle=3.14159265359f;
else
rotAngle=acosf(axis(2));
rotAxis(0)=-axis(1);
rotAxis(1)=axis(0);
rotAxis(2)=0.0f;
rotAxis.normalize();
C7Vector m(jointAxisMatrix);
float alpha=-atan2(rotAxis(1),rotAxis(0));
float beta=atan2(-sqrt(rotAxis(0)*rotAxis(0)+rotAxis(1)*rotAxis(1)),rotAxis(2));
C7Vector r;
r.X.clear();
r.Q.setEulerAngles(0.0f,0.0f,alpha);
m=r*m;
r.Q.setEulerAngles(0.0f,beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,rotAngle);
m=r*m;
r.Q.setEulerAngles(0.0f,-beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,-alpha);
m=r*m;
jointAxisMatrix=m.getMatrix();
}
C4Vector q((vJoints.at(i)->jointBaseFrame*jointAxisMatrix).Q);
simSetObjectOrientation(vJoints.at(i)->nJoint,-1,q.getEulerAngles().data);
}
}
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
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);
}
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;
if (iterat!=NULL) // Following lines recently changed!
{
if (iterat->getObjectType()==sim_object_joint_type)
activeJoint=( (((CJoint*)iterat)->getJointMode()==sim_jointmode_ik)||(((CJoint*)iterat)->getJointMode()==sim_jointmode_ikdependent) );
}
if ( (iterat==base)||activeJoint )
{ // If base is NULL then the second part is not evaluated (iterat->getObjectType())
if (positionCounter==0)
{ // Here we have the first part (from tooltip to first joint)
d0=buff;
dp.clear();
multiply(d0,dp,0,jMatrices);
}
else
{ // Here we have a joint:
if (lastJoint->getJointType()==sim_joint_revolute_subtype)
{
buildDeltaZRotation(d0,dp,lastJoint->getScrewPitch());
multiply(d0,dp,positionCounter,jMatrices);
paramPart.buildZRotation(lastJoint->getPosition(true));
}
else if (lastJoint->getJointType()==sim_joint_prismatic_subtype)
{
buildDeltaZTranslation(d0,dp);
multiply(d0,dp,positionCounter,jMatrices);
paramPart.buildTranslation(0.0f,0.0f,lastJoint->getPosition(true));
}
else
{ // Spherical joint part!
buildDeltaZRotation(d0,dp,0.0f);
multiply(d0,dp,positionCounter,jMatrices);
if (indexCntLast==-1)
indexCntLast=lastJoint->getDoFs()-1;
paramPart.buildZRotation(lastJoint->getTempParameterEx(indexCntLast--));
}
d0=buff*paramPart;
dp.clear();
multiply(d0,dp,0,jMatrices);
}
buff.setIdentity();
lastJoint=(CJoint*)iterat;
positionCounter++;
}
}
int alternativeBaseForConstraints=ikElement->getAlternativeBaseForConstraints();
if (alternativeBaseForConstraints!=-1)
{
CDummy* alb=ct::objCont->getDummy(alternativeBaseForConstraints);
if (alb!=NULL)
{ // We want everything relative to the alternativeBaseForConstraints dummy orientation!
C7Vector alternativeBase(alb->getCumulativeTransformationPart1(true));
C7Vector currentBase;
currentBase.setIdentity();
if (base!=NULL)
currentBase=base->getCumulativeTransformation(true); // could be a joint, we want also the joint intrinsic transformation part!
C4X4FullMatrix correction((alternativeBase.getInverse()*currentBase).getMatrix());
dp.clear();
multiply(correction,dp,0,jMatrices);
}
}
// The x-, y- and z-component:
for (int i=0;i<doF;i++)
{
(*J)(0,i)=(*jMatrices[1+i])(0,3);
(*J)(1,i)=(*jMatrices[1+i])(1,3);
(*J)(2,i)=(*jMatrices[1+i])(2,3);
}
// We divide all delta components (to avoid distorsions)...
for (int i=0;i<doF;i++)
(*jMatrices[1+i])/=IK_DIVISION_FACTOR;
// ...and add the cumulative transform to the delta-components:
for (int i=0;i<doF;i++)
(*jMatrices[1+i])+=(*jMatrices[0]);
// We also copy the cumulative transform to 'tooltipTransf':
tooltipTransf=(*jMatrices[0]);
// Now we extract the delta Euler components:
C4X4FullMatrix mainInverse(*jMatrices[0]);
mainInverse.invert();
C4X4FullMatrix tmp;
// Alpha-, Beta- and Gamma-components:
for (int i=0;i<doF;i++)
{
tmp=mainInverse*(*jMatrices[1+i]);
C3Vector euler(tmp.getEulerAngles());
(*J)(3,i)=euler(0);
(*J)(4,i)=euler(1);
(*J)(5,i)=euler(2);
}
// We free the memory allocated for each joint variable:
for (int i=0;i<int(jMatrices.size());i++)
delete jMatrices[i];
return(J);
}
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)
toBeReturned=justInsertedObject;
noParent.push_back(justInsertedObject);
}
}
else
{
justInsertedJoint->userData1=object->getID();
if (lastAddedNode!=NULL)
{
lastAddedNode->linkWithObject(justInsertedJoint->getDownIKGraphObject());
lastAddedNode=justInsertedJoint->getTopIKGraphObject();
}
else
{
lastAddedNode=justInsertedJoint->getTopIKGraphObject();
if (toBeReturned==NULL)
toBeReturned=justInsertedJoint;
noParent.push_back(justInsertedJoint->getDownIKGraphObject());
}
}
exploredObjs.push_back(object);
}
// 2. We prepare further exploration:
for (int i=0;i<int(object->childList.size());i++)
{
objectsToExplore.push_back(object->childList[i]);
lastAdded.push_back(lastAddedNode);
}
}
if (noParent.size()>1)
{ // We have to link those objects aginst each other (happens when the base object is not inserted)
for (int i=1;i<int(noParent.size());i++)
noParent[0]->linkWithObject(noParent[i]);
}
return(toBeReturned);
}