static void segment (float w, float *x, float *y) {
int i, j;
float t, p[3], q[3], up[3] = {0, 0, 1}, m[3], epsilon = 0.01;
p[0] = x[0]; p[1] = y[1];
glNormal3f (0, 0, 1);
glBegin (GL_QUAD_STRIP);
for (i = 0; i <= CONFIG_RENDER_DETAIL; ++i) {
t = (float) i / CONFIG_RENDER_DETAIL;
bezier3 (t, x, y, p);
bezier3 (t + epsilon, x, y, q);
p[2] = terrain_h (p[0], p[1]);
q[2] = terrain_h (q[0], q[1]);
for (j = 0; j < len (q); ++j) q[j] -= p[j];
crossproduct (q, up, m);
normalize (m, len (m));
for (j = 0; j < len (m); ++j) p[j] -= w * m[j];
glTexCoord2f (0, t);
glVertex3fv (p);
for (j = 0; j < len (m); ++j) p[j] += 2*w * m[j];
glTexCoord2f (1, t);
glVertex3fv (p);
}
glEnd ();
}
bool SpecificWorker::Quadruped()
{
static float i=0;
bool ismoving=false;
if(i==0)
{
for(int k=0;k<6;k++)
if(proxies[k]->getStateLeg().ismoving)
{
ismoving=true;
break;
}
if(!ismoving)
{
stateQuadruped.enqueue(stateQuadruped.dequeue());
stateQuadruped.enqueue(stateQuadruped.dequeue());
for(int i=0;i<6;i++)
pre_statelegs[i]=statelegs[i];
}
}
if(!ismoving)
{
if(lini!=QVec::vec3(0, 0, 0)&&lfin!=QVec::vec3(0,0,0))
{
RoboCompLegController::PoseLeg p;
RoboCompLegController::StateLeg st;
QVec ini,fin,tmp,med;
for(int j=0;j<6;j++)
{
int aux=stateQuadruped.dequeue();
st=pre_statelegs[aux];
if(j==0||j==1)
{ //patas por arco
ini =QVec::vec3(st.x,legsp[aux].y(),st.z),
fin = legsp[aux]+lfin,med=legsp[aux],
tmp=bezier3(ini,QVec::vec3(med.x(),0,med.z()),fin,i);
}
else
{ //patas por tierra
int a=2;
if(j==2||j==3)
a=1;
ini = QVec::vec3(st.x,legsp[aux].y(),st.z),
fin = legsp[aux]-lfin-(((lini-lfin)/3)*a),
tmp = bezier2(ini,fin,i);
}
stateQuadruped.enqueue(aux);
p.x=tmp.x();
p.y=tmp.y();
p.z=tmp.z();
p.ref=base.toStdString();
p.vel = vel;
proxies[aux]->setIKLeg(p,false);
}
i+=tbezier;
if(i>1)
{
i=0;
return true;
}
return false;
}
}
return true;
}
bool SpecificWorker::caminar3x3()
{
static float i=0;
bool ismoving=false;
if(i==0)
{
for(int k=0;k<6;k++)
if(proxies[k]->getStateLeg().ismoving)
{
ismoving=true;
break;
}
if(!ismoving)
for(int i=0;i<6;i++)
pre_statelegs[i]=statelegs[i];
}
if(!ismoving)
{
if(lini!=QVec::vec3(0, 0, 0)&&lfin!=QVec::vec3(0,0,0))
{
//patas por arco
for(int s=0;s<3;s++)
{
RoboCompLegController::StateLeg st=pre_statelegs[l1[s]];
QVec posini =QVec::vec3(st.x,legsp[l1[s]].y(),st.z);
QVec ini = posini,fin = legsp[l1[s]]+lfin,med=legsp[l1[s]];
QVec tmp=bezier3(ini,QVec::vec3(med.x(),0,med.z()),fin,i);
RoboCompLegController::PoseLeg p;
p.x=tmp.x();
p.y=tmp.y();
p.z=tmp.z();
p.ref=base.toStdString();
p.vel = vel;
proxies[l1[s]]->setIKLeg(p,false);
}
// patas por tierra
for(int s=0;s<3;s++)
{
RoboCompLegController::StateLeg st=pre_statelegs[l2[s]];
QVec posini =QVec::vec3(st.x,legsp[l2[s]].y(),st.z);
QVec ini = posini,fin = legsp[l2[s]]+lini;
QVec tmp=bezier2(ini,fin,i);
RoboCompLegController::PoseLeg p;
p.x=tmp.x();
p.y=tmp.y();
p.z=tmp.z();
p.ref=base.toStdString();
p.vel = vel;
proxies[l2[s]]->setIKLeg(p,false);
}
i+=tbezier;
if (i>1)
{
int aux[]={l1[0],l1[1],l1[2]};
l1[0]=l2[0];
l1[1]=l2[1];
l1[2]=l2[2];
l2[0]=aux[0];
l2[1]=aux[1];
l2[2]=aux[2];
i=0;
return true;
}
return false;
}
}
return true;
}
bool SpecificWorker::rotar()
{
static bool estado=true;
if(lrot!=QVec::vec3(0, 0, 0))
{
static float i=0;
bool ismoving=false;
for(int k=0;k<6;k++)
if(proxies[k]->getStateLeg().ismoving)
{
ismoving=true;
break;
}
if(!ismoving)
{
if(i==0)
for(int i=0;i<6;i++)
pre_statelegs[i]=statelegs[i];
QVec ini,fin;
for(int j=0;j<6;j++)
{
RoboCompLegController::StateLeg st=pre_statelegs[j];
QVec posini =QVec::vec3(st.x,legsp[j].y(),st.z);
if((j==2 || j==3))
{
ini = posini;
if(estado)
fin = legsp[j]+lrot;
else
fin = legsp[j]-lrot;
}
else
{
ini = posini;
if(estado)
fin = legsp[j]-lrot;
else
fin = legsp[j]+lrot;
}
QVec tmp;
if(estado)
if(j==0||j==3||j==4)
tmp=bezier3(ini,bezier2(ini,fin,0.5)+lmed,fin,i);
else
tmp = bezier2(ini,fin,i);
else
if(j==0||j==3||j==4)
tmp = bezier2(ini,fin,i);
else
tmp=bezier3(ini,bezier2(ini,fin,0.5)+lmed,fin,i);
RoboCompLegController::PoseLeg p;
p.x = tmp.x();
p.y = tmp.y();
p.z = tmp.z();
p.ref = base.toStdString();
p.vel = 6;
proxies[j]->setIKLeg(p,false);
}
i+=tbezier;
if (i>1)
{
i=0;
estado=!estado;
return true;
}
}
return false;
}
else
return true;
}
bool SpecificWorker::Quadruped()
{
static float i=0;
bool ismoving=false;
if(lini!=QVec::vec3(0, 0, 0)&&lfin!=QVec::vec3(0,0,0))
{
//Comprueba que alguna pata se mueva
for(int k=0;k<6;k++)
if(proxies[k]->getStateLeg().ismoving){
ismoving=true;
break;
}
if(!ismoving)
{
//patas por arco
for(int s=0;s<2;s++)
{
RoboCompLegController::StateLeg st=posiciones[legsQuadrupedOn[s]];
QVec posini = QVec::vec3(st.x,legCoord[legsQuadrupedOn[s]].y(),st.z);
QVec ini = posini,fin = legCoord[legsQuadrupedOn[s]]+lfin,med=legCoord[legsQuadrupedOn[s]];
QVec tmp = bezier3(ini,QVec::vec3(med.x(),0,med.z()),fin,i);
RoboCompLegController::PoseLeg p;
p.x=tmp.x();
p.y=tmp.y();
p.z=tmp.z();
p.ref=base.toStdString();
p.vel=6;
proxies[legsQuadrupedOn[s]]->setIKLeg(p,false);
}
// patas por tierra
for(int s=0;s<4;s++)
{
RoboCompLegController::StateLeg st=posiciones[legsQuadrupedOff[s]];
QVec posini =QVec::vec3(st.x,legCoord[legsQuadrupedOff[s]].y(),st.z);
lini = QVec::vec3(-X,0,-Z/3);
QVec ini = posini,fin = legCoord[legsQuadrupedOff[s]]+lini;
QVec tmp=bezier2(ini,fin,i);
RoboCompLegController::PoseLeg p;
p.x=tmp.x();
p.y=tmp.y();
p.z=tmp.z();
p.ref=base.toStdString();
p.vel=6;
proxies[legsQuadrupedOff[s]]->setIKLeg(p,false);
}
i+=.1;
if (i>1)
{
int aux[]={legsQuadrupedOn[0],legsQuadrupedOn[1]};
legsQuadrupedOn[0]=legsQuadrupedOff[0];
legsQuadrupedOn[1]=legsQuadrupedOff[1];
legsQuadrupedOff[0]=legsQuadrupedOff[2];
legsQuadrupedOff[1]=legsQuadrupedOff[3];
legsQuadrupedOff[2]=aux[0];
legsQuadrupedOff[3]=aux[1];
i=0;
return true;
}
}
return false;
}
return true;
}
