int simEmbSetObjectTransformation(int objectHandle,int relativeToObjectHandle,const float* position,const 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)
C3DObject* it=ct::objCont->getObject(objectHandle);
if (it==NULL)
return(-1);
if (relativeToObjectHandle==sim_handle_parent)
{
relativeToObjectHandle=-1;
C3DObject* parent=it->getParent();
if (parent!=NULL)
relativeToObjectHandle=parent->getID();
}
C3DObject* relObj=ct::objCont->getObject(relativeToObjectHandle);
if (relativeToObjectHandle!=-1)
{
if (relObj==NULL)
return(-1);
}
if (relativeToObjectHandle==-1)
{
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];
ct::objCont->setAbsoluteConfiguration(it->getID(),tr,false);
}
else
{
C7Vector absTr(it->getCumulativeTransformationPart1());
C7Vector relTr(relObj->getCumulativeTransformationPart1()); // added ..Part1 on 2010/06/14
C7Vector x(relTr.getInverse()*absTr);
x.Q(0)=quaternion[3];
x.Q(1)=quaternion[0];
x.Q(2)=quaternion[1];
x.Q(3)=quaternion[2];
x.X(0)=position[0];
x.X(1)=position[1];
x.X(2)=position[2];
absTr=relTr*x;
ct::objCont->setAbsoluteConfiguration(it->getID(),absTr,false);
}
return(1);
}
int simEmbRotateAroundAxis(const float* positionIn,const float* quaternionIn,const float* axisVector,const float* axisPosition,float angle,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 m;
m.Q(0)=quaternionIn[3];
m.Q(1)=quaternionIn[0];
m.Q(2)=quaternionIn[1];
m.Q(3)=quaternionIn[2];
m.X(0)=positionIn[0];
m.X(1)=positionIn[1];
m.X(2)=positionIn[2];
C3Vector ax(axisVector);
C3Vector pos(axisPosition);
float alpha=-atan2(ax(1),ax(0));
float beta=atan2(-sqrt(ax(0)*ax(0)+ax(1)*ax(1)),ax(2));
m.X-=pos;
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,angle);
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;
m.X+=pos;
quaternionOut[0]=m.Q(1);
quaternionOut[1]=m.Q(2);
quaternionOut[2]=m.Q(3);
quaternionOut[3]=m.Q(0);
positionOut[0]=m.X(0);
positionOut[1]=m.X(1);
positionOut[2]=m.X(2);
return(1);
}
int simEmbGetObjectTransformation(int objectHandle,int relativeToObjectHandle,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)
C3DObject* it=ct::objCont->getObject(objectHandle);
if (it==NULL)
return(-1);
if (relativeToObjectHandle==sim_handle_parent)
{
relativeToObjectHandle=-1;
C3DObject* parent=it->getParent();
if (parent!=NULL)
relativeToObjectHandle=parent->getID();
}
C3DObject* relObj=ct::objCont->getObject(relativeToObjectHandle);
if (relativeToObjectHandle!=-1)
{
if (relObj==NULL)
return(-1);
}
C7Vector tr;
if (relativeToObjectHandle==-1)
tr=it->getCumulativeTransformationPart1();
else
{
C7Vector relTr(relObj->getCumulativeTransformationPart1()); // added ..Part1 on 2010/06/14
tr=relTr.getInverse()*it->getCumulativeTransformationPart1(); // Corrected bug on 2011/01/22: was getLocalTransformationPart1 before!!!
}
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);
}
int simEmbMultTransformationWithVector(const float* position,const float* quaternion,float* vect)
{
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];
C3Vector v1(vect);
C3Vector v2(tr*v1);
vect[0]=v2(0);
vect[1]=v2(1);
vect[2]=v2(2);
return(1);
}
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);
}
