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);
}
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!
}
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!
}
