void C4X4Matrix::rotateAroundY(extIkReal angle)
{
C4X4Matrix rot;
rot.setIdentity();
rot.M.buildYRotation(angle);
(*this)=rot*(*this);
}
void C4X4Matrix::rotateAroundZ(float angle)
{
C4X4Matrix rot;
rot.setIdentity();
rot.M.buildZRotation(angle);
(*this)=rot*(*this);
}
int CCuttingRoutine::cutEntity(int millID,int entityID,int& cutObject,float& cutSurface,float& cutVolume,bool justForInitialization,bool overrideCuttableFlagIfNonCollection)
{ // entityID==-1 --> checks all objects in the scene. Return value is the number of cut objects
int cutCount=0;
cutObject=-1;
cutSurface=0.0f;
cutVolume=0.0f;
C3DObject* object=App::ct->objCont->getObject(entityID);
CMill* sens=App::ct->objCont->getMill(millID);
if (sens==NULL)
return(0); // should never happen!
App::ct->infoDisplay->millSimulationStart();
C3Vector minV,maxV;
sens->getMillingVolumeBoundingBox(minV,maxV);
C4X4Matrix millingVolumeBox;
millingVolumeBox.setIdentity();
millingVolumeBox.X=(maxV+minV)*0.5f;
C3Vector millingVolumeSize(maxV-minV);
if (object==NULL)
cutCount=_cutGroup(millID,entityID,cutObject,cutSurface,cutVolume,justForInitialization);
else
{ // Cutting objects:
if (object->getObjectType()==sim_object_shape_type)
{
if (_cutShape(millID,entityID,cutSurface,justForInitialization,overrideCuttableFlagIfNonCollection))
{
cutObject=entityID;
cutCount++;
}
}
if (object->getObjectType()==sim_object_volume_type)
{
if (_cutVolumeObject(millID,entityID,cutVolume,justForInitialization,overrideCuttableFlagIfNonCollection))
{
cutObject=entityID;
cutCount++;
}
}
}
App::ct->infoDisplay->millSimulationEnd(cutSurface,cutVolume);
return(cutCount);
}
CMatrix CMatrix::operator* (const C4X4Matrix& m) const
{
CMatrix retM(rows,4);
for (int i=0;i<rows;i++)
{
for (int j=0;j<3;j++)
{
retM(i,j)=0.0f;
for (int k=0;k<3;k++)
retM(i,j)+=( (*this)(i,k)*m.M.axis[j](k) );
}
retM(i,3)=(*this)(i,3);
for (int k=0;k<3;k++)
retM(i,3)+=( (*this)(i,k)*m.X(k) );
}
return(retM);
}
void C4X4Matrix::buildInterpolation(const C4X4Matrix& fromThis,const C4X4Matrix& toThat,float t)
{ // Builds the interpolation (based on t) from 'fromThis' to 'toThat'
C7Vector out;
out.buildInterpolation(fromThis.getTransformation(),toThat.getTransformation(),t);
(*this)=out;
}
void 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 C7Vector::set(const C4X4Matrix& m)
{
(*this)=m.getTransformation();
}
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!
}
bool CDistanceRoutine::_getGroupDummyDistanceIfSmaller(int groupID,int dummyID,
float& dist,float ray[7],int buffer[2],bool overrideMeasurableFlagDummy,bool pathPlanningRoutineCalling)
{ // Distance is measured from collection to dummy
// If the distance is smaller than 'dist', 'dist' is replaced and the return value is true
// If the distance is bigger, 'dist' doesn't change and the return value is false
// ray contains the point on object1 (0-2), the point on object2 (3-5) and the distance (6)
// groupID can be -1, in which case all other objects are tested
// buffer[0]=-1; // Artificially disabling caching for tests
int propMask=sim_objectspecialproperty_measurable;
// We get the dummy and all shapes/dummies in the group:
CDummy* dummy=App::ct->objCont->getDummy(dummyID);
if (dummy==NULL)
return(false);
if ( ((dummy->getMainProperty()&sim_objectspecialproperty_measurable)==0)&&(!overrideMeasurableFlagDummy) )
return(false);
if (pathPlanningRoutineCalling&&(dummy->getMainProperty()&sim_objectspecialproperty_pathplanning_ignored)&&(!overrideMeasurableFlagDummy))
return(false);
std::vector<C3DObject*> group;
bool overrideMeasurableFlagGroupObjects=false;
if (groupID==-1)
{ // Special group:
std::vector<C3DObject*> exceptionObject;
exceptionObject.push_back(dummy);
App::ct->objCont->getAllShapesAndDummiesFromSceneExcept(exceptionObject,group,propMask,pathPlanningRoutineCalling);
}
else
{ // regular group
if (!App::ct->collections->getShapesAndDummiesFromGroup(groupID,&group,propMask,pathPlanningRoutineCalling))
return(false);
overrideMeasurableFlagGroupObjects=App::ct->collections->getGroup(groupID)->getOverridesObjectMainProperties();
}
// Build the collision nodes only when needed. So do it right here!
for (int i=0; i<int(group.size()); i++)
{
if (group[i]->getObjectType()==sim_object_shape_type)
((CShape*)group[i])->initializeCalculationStructureIfNeeded();
}
bool returnValue=false;
// We first process all dummy-dummy distances because it's so fast (doesn't use caching):
int i=0;
while (i<int(group.size()))
{
if (group[i]->getObjectType()==sim_object_dummy_type)
{
if (group[i]!=dummy) // We never measure a dummy against itself!
{
if (_getDummyDummyDistanceIfSmaller(group[i]->getID(),dummy->getID(),dist,ray,overrideMeasurableFlagGroupObjects,overrideMeasurableFlagDummy,pathPlanningRoutineCalling))
returnValue=true;
}
group.erase(group.begin()+i);
}
else
i++;
}
// This part is for handling the cached distance:
C3DObject* buffTree=App::ct->objCont->getObject(buffer[0]);
if (buffTree!=NULL)
{ //We check if buffTree is in the group:
for (int i=0; i<int(group.size()); i++)
{
if (group[i]==buffTree)
{
group.erase(group.begin()+i); // We remove that element because we measure it here
int auxBuff=buffer[1];
if (buffTree->getObjectType()==sim_object_shape_type)
{ // We have a buffered shape here:
bool retVal=_getBufferedDistance_IfSmaller((CShape*)buffTree,dummy,auxBuff,dist,ray);
// Now we check this shape & dummy:
if (((CShape*)buffTree)->getDistanceToDummy_IfSmaller(dummy,dist,ray,auxBuff))
retVal=true;
if (retVal)
buffer[1]=auxBuff;
returnValue=returnValue||retVal; // ReturnValue may already be true (dummy-dummy were already processed!)
}
break;
}
}
}
// Group now only contains shapes
// Now go through all shapes in the group and order them according to
// their approximate distance to dummy (from smallest to biggest):
std::vector<CShape*> exploringOrder;
std::vector<float> approxDistances;
exploringOrder.reserve(group.size());
approxDistances.reserve(group.size());
C4X4Matrix groupCM;
C3Vector groupSize;
float groupCMAux[4][4];
dummy->getCumulativeTransformationMatrix(groupCMAux); // We borrow groupCM just for here
groupCM.set(groupCMAux);
C3Vector dummyPos(groupCM.X);
for (int i=0; i<int(group.size()); i++)
{
if (group[i]->getObjectType()==sim_object_shape_type)
{ // Just in case... (should anyway contain only shapes!)
float testDist;
group[i]->getCumulativeTransformationMatrix(groupCMAux);
groupSize=((CShape*)group[i])->geomData->getBoundingBoxHalfSizes();
testDist=CCollDistInterface::getBoxPointDistance(groupCM,groupSize,dummyPos);
// We have the approx distance and put it in the ordered list:
int k;
for (k=0; k<int(approxDistances.size()); k++)
{
if (testDist<approxDistances[k])
break;
}
approxDistances.insert(approxDistances.begin()+k,testDist);
exploringOrder.insert(exploringOrder.begin()+k,(CShape*)group[i]);
}
}
// Now find the smallest distance:
int auxBuffer;
for (int i=0; i<int(approxDistances.size()); i++)
{
if (approxDistances[i]>dist)
break;
if (dist==0.0f)
break;
if (((CShape*)exploringOrder[i])->getDistanceToDummy_IfSmaller(dummy,dist,ray,auxBuffer))
{
buffer[0]=exploringOrder[i]->getID();
buffer[1]=auxBuffer;
returnValue=true;
}
}
return(returnValue);
}
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);
}
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);
}
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!
}