int simEmbInterpolateTransformations(const float* position1,const float* quaternion1,const float* position2,const float* quaternion2,float interpolFactor,float* positionOut,float* quaternionOut)
{
if (!hasLaunched())
return(-1);
// V-REP quaternion, internally: w x y z
// V-REP quaternion, at interfaces: x y z w (like ROS)
C7Vector tr1;
tr1.Q(0)=quaternion1[3];
tr1.Q(1)=quaternion1[0];
tr1.Q(2)=quaternion1[1];
tr1.Q(3)=quaternion1[2];
tr1.X(0)=position1[0];
tr1.X(1)=position1[1];
tr1.X(2)=position1[2];
C7Vector tr2;
tr2.Q(0)=quaternion2[3];
tr2.Q(1)=quaternion2[0];
tr2.Q(2)=quaternion2[1];
tr2.Q(3)=quaternion2[2];
tr2.X(0)=position2[0];
tr2.X(1)=position2[1];
tr2.X(2)=position2[2];
C7Vector trOut;
trOut.buildInterpolation(tr1,tr2,interpolFactor);
quaternionOut[0]=trOut.Q(1);
quaternionOut[1]=trOut.Q(2);
quaternionOut[2]=trOut.Q(3);
quaternionOut[3]=trOut.Q(0);
positionOut[0]=trOut.X(0);
positionOut[1]=trOut.X(1);
positionOut[2]=trOut.X(2);
return(1);
}
int simEmbInvertTransformation(float* position,float* quaternion)
{
if (!hasLaunched())
return(-1);
// V-REP quaternion, internally: w x y z
// V-REP quaternion, at interfaces: x y z w (like ROS)
C7Vector tr;
tr.Q(0)=quaternion[3];
tr.Q(1)=quaternion[0];
tr.Q(2)=quaternion[1];
tr.Q(3)=quaternion[2];
tr.X(0)=position[0];
tr.X(1)=position[1];
tr.X(2)=position[2];
tr.inverse();
quaternion[0]=tr.Q(1);
quaternion[1]=tr.Q(2);
quaternion[2]=tr.Q(3);
quaternion[3]=tr.Q(0);
position[0]=tr.X(0);
position[1]=tr.X(1);
position[2]=tr.X(2);
return(1);
}
void CQDlgTranslations::_copyTransf(const C7Vector& tr,C7Vector& trIt,bool orientation,int mask)
{
if (orientation)
trIt.Q=tr.Q;
else
{
if (mask&1)
trIt.X(0)=tr.X(0);
if (mask&2)
trIt.X(1)=tr.X(1);
if (mask&4)
trIt.X(2)=tr.X(2);
}
}
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);
}
}
}
}