struct xpr
cxarg (struct cxpr z)
{
int rs, is;
rs = xsgn (&z.re);
is = xsgn (&z.im);
if (rs > 0)
return xatan (xdiv (z.im, z.re));
else if (rs < 0)
{
z.re.nmm[0] ^= xM_sgn;
z.im.nmm[0] ^= xM_sgn;
if (is >= 0)
return xadd (xPi, xatan (xdiv (z.im, z.re)), 0);
else
return xadd (xatan (xdiv (z.im, z.re)), xPi, 1);
}
else /* z.re is zero ! */
{
if (!xsigerr (is == 0, XEDOM, "cxarg()"))
return (is > 0 ? xPi2 : xneg (xPi2));
else
return xneg (xPi2); /* Dummy value :) */
}
}
struct xpr
xtan (struct xpr z)
{
int k, m;
z = rred (z, 't', &k);
if ((xsigerr (xprcmp (&z, &xPi2) >= 0, XEDOM, "xtan()")))
return (!k ? xPinf : xMinf);
else
{
if (xprcmp (&z, &xPi4) == 1)
{
m = 1;
z = xadd (xPi2, z, 1);
}
else
m = 0;
if ((k))
z = xneg (c_tan (z));
else
z = c_tan (z);
if (m)
return xdiv (xOne, z);
else
return z;
}
}
static struct xpr
rred (struct xpr z, int kf, int *ps)
{
struct xpr is, q;
if (x_neg (&z))
{
z = xneg (z);
is = xOne;
}
else
is = xZero;
z = xfmod (z, xPi, &q);
if (kf == 't')
q = is;
else if (kf == 's')
q = xadd (q, is, 0);
if (xprcmp (&z, &xPi2) == 1)
{
z = xadd (xPi, z, 1);
if (kf == 'c' || kf == 't')
q = xadd (q, xOne, 0);
}
*ps = (xodd (q)) ? 1 : 0;
return z;
}
int
main (void)
{
struct xpr z, h, f, u;
printf (" Test of Exp Functions\n");
z = xneg (xOne);
h = atox ("0.5");
u = atox ("3.01");
for (; xprcmp (&z, &u) < 0; z = xadd (z, h, 0))
{
/* compute extended precision exponential */
f = xexp (z);
printf (" %8.4f ", xtodbl (z));
xprxpr (f, decd);
f = xexp2 (z);
printf ("\n ");
xprxpr (f, decd);
f = xexp10 (z);
printf ("\n ");
xprxpr (f, decd);
putchar ('\n');
}
return 0;
}
void Cube::initCubeTexture()
{
QImage xpos("./bin/textures/room/xpos.png");
QImage xneg("./bin/textures/room/xneg.png");
QImage ypos("./bin/textures/room/ypos.png");
QImage yneg("./bin/textures/room/yneg.png");
QImage zpos("./bin/textures/room/zpos.png");
QImage zneg("./bin/textures/room/zneg.png");
int width = xpos.width();
int height = xpos.height();
glGenTextures(1, &m_texture);
glBindTexture(GL_TEXTURE_CUBE_MAP, m_texture);
glTexParameteri(GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE);
glTexImage2D(GL_TEXTURE_CUBE_MAP_POSITIVE_X, 0, GL_RGBA, width, height, 0, GL_BGRA, GL_UNSIGNED_BYTE, xpos.bits());
glTexImage2D(GL_TEXTURE_CUBE_MAP_NEGATIVE_X, 0, GL_RGBA, width, height, 0, GL_BGRA, GL_UNSIGNED_BYTE, xneg.bits());
glTexImage2D(GL_TEXTURE_CUBE_MAP_POSITIVE_Y, 0, GL_RGBA, width, height, 0, GL_BGRA, GL_UNSIGNED_BYTE, ypos.bits());
glTexImage2D(GL_TEXTURE_CUBE_MAP_NEGATIVE_Y, 0, GL_RGBA, width, height, 0, GL_BGRA, GL_UNSIGNED_BYTE, yneg.bits());
glTexImage2D(GL_TEXTURE_CUBE_MAP_POSITIVE_Z, 0, GL_RGBA, width, height, 0, GL_BGRA, GL_UNSIGNED_BYTE, zpos.bits());
glTexImage2D(GL_TEXTURE_CUBE_MAP_NEGATIVE_Z, 0, GL_RGBA, width, height, 0, GL_BGRA, GL_UNSIGNED_BYTE, zneg.bits());
glBindTexture(GL_TEXTURE_CUBE_MAP, 0);
}
struct xpr
xcos (struct xpr z)
{
int k;
z = rred (z, 'c', &k);
if (x_exp (&z) < xK_lin)
{
if ((k))
return xneg (xOne);
else
return xOne;
}
z = c_tan (xpr2 (z, -1));
z = xmul (z, z);
z = xdiv (xadd (xOne, z, 1), xadd (xOne, z, 0));
if ((k))
return xneg (z);
else
return z;
}
struct xpr
xsin (struct xpr z)
{
int k;
z = rred (z, 's', &k);
if (x_exp (&z) >= xK_lin)
{
z = c_tan (xpr2 (z, -1));
z = xdiv (xpr2 (z, 1), xadd (xOne, xmul (z, z), 0));
}
if ((k))
return xneg (z);
else
return z;
}
static struct xpr
c_tan (struct xpr z)
{
struct xpr s, f, d;
int m;
unsigned short k;
if (x_exp (&z) < xK_lin)
return z;
s = xneg (xmul (z, z));
for (k = 1; k <= XDIM && s.nmm[k] == 0; k++);
if ((xsigerr (s.nmm[0] == 0xffff && k > XDIM, XFPOFLOW, NULL)))
return xZero;
else
{
f = xZero;
for (d = inttox (m = xMS_trg); m > 1;)
{
f = xdiv (s, xadd (d, f, 0));
d = inttox (m -= 2);
}
return xdiv (z, xadd (d, f, 0));
}
}
/**
* \brief Default constructor
*/
SpecificWorker::SpecificWorker(MapPrx& mprx) : GenericWorker(mprx)
{
connect(ci, SIGNAL(clicked()), this, SLOT(moverpataci()));
connect(cd, SIGNAL(clicked()), this, SLOT(moverpatacd()));
connect(Xpos, SIGNAL(clicked()), this, SLOT(xpos()));
connect(Xneg, SIGNAL(clicked()), this, SLOT(xneg()));
connect(Ypos, SIGNAL(clicked()), this, SLOT(ypos()));
connect(Yneg, SIGNAL(clicked()), this, SLOT(yneg()));
connect(Zpos, SIGNAL(clicked()), this, SLOT(zpos()));
connect(Zneg, SIGNAL(clicked()), this, SLOT(zneg()));
connect(Actualizar, SIGNAL(clicked()), this, SLOT(actualizar()));
inner = new InnerModel("/home/ivan/robocomp/files/innermodel/hexapod1pata.xml");
Posini=inner->transform("base","axisA1T");
Posfin=Posini;
QVec aux=inner->transform("arm1motor2","arm1motor3");
qDebug()<<aux;
angle1=atan(aux.y()/aux.z());
Femur=aux.norm2();
aux=inner->transform("arm1motor3","axisA1T");
angle2=atan(aux.y()/aux.z());
Tibia=aux.norm2();
qDebug()<<aux;
Coxa=52;
qDebug()<<"-----------------------------";
qDebug()<<" Coxa = "<<Coxa;
qDebug()<<" Femur = "<<Femur;
qDebug()<<" Tibia = "<<Tibia;
qDebug()<<" angle1 = "<<angle1;
qDebug()<<" angle2 = "<<angle2;
qDebug()<<"-----------------------------";
// Femur=sqrt(14.5*14.5+64.5*64.5);
// Tibia=sqrt(123.714*123.714+ 23.6179 *23.6179);
H=18.5;
// Posini=QVec::zeros(3);
// RoboCompJointMotor::MotorParamsList mp = jointmotor_proxy->getAllMotorParams();
// RoboCompJointMotor::MotorGoalPositionList mg;
// for(auto m:mp){
// cout<<m.name<<endl;
// RoboCompJointMotor::MotorGoalPosition p;
// p.name=m.name;
// p.maxSpeed=0.1;
// p.position=0;
// mg.push_back(p);
// // mg.insert(p);
// }
// jointmotor_proxy->setSyncPosition(mg);
m1="arm1motor1";
m2="arm1motor2";
m3="arm1motor3";
}