/*----------------------------------------------------------------------------*/
void draw_fish(int x,int y,int type)
{
if(type==SHARK)
FillRectSDL(SCALE*x,SCALE*y,SCALE,SCALE,getColorSDL(clShark));
else if(type==SARDINE)
FillRectSDL(SCALE*x,SCALE*y,SCALE,SCALE,getColorSDL(clSardine));
}
/*----------------------------------------------------------------------------*/
void draw_zone(int x,int y,int w,int h,int color)
{;
if(x+w<=ocean_size)
{
FillRectSDL(SCALE*x,SCALE*y,SCALE*w,SCALE*h,color==0?getColorSDL(clZone1):(color==1?getColorSDL(clZone2):getColorSDL(clZone3)));
}
else
{
FillRectSDL(SCALE*x,SCALE*y,SCALE*(ocean_size-x),SCALE*h,color==0?getColorSDL(clZone1):(color==1?getColorSDL(clZone2):getColorSDL(clZone3)));
FillRectSDL(0,SCALE*y,SCALE*(w-ocean_size+x),SCALE*h,color==0?getColorSDL(clZone1):(color==1?getColorSDL(clZone2):getColorSDL(clZone3)));
}
}
//----------------------------------------------------------------------------
robot_t::robot_t()
{
objects.clear();
rect_obj_t *robot;
robot=new rect_obj_t;
robot->type=OBJ_ROBOT;
robot->couleur=getColorSDL(clDarkGray);
add_object(robot);
robot=new rect_obj_t;
robot->type=OBJ_ROBOT;
robot->couleur=getColorSDL(clDarkGray);
add_object(robot);
robot=new rect_obj_t;
robot->type=OBJ_ROBOT;
robot->couleur=getColorSDL(clDarkGray);
add_object(robot);
union_obj_t *pince=new union_obj_t;
pince->type=OBJ_ROBOT;
rect_obj_t *pince1=new rect_obj_t;
pince1->dimX=0.01;
pince1->dimY=0.12;
pince1->hauteur=0.3;
pince1->z=0.3/2;
pince1->couleur=getColorSDL(clLightGray);
pince1->type=OBJ_ROBOT;
pince->add_object(pince1);
circle_obj_t *pince2=new circle_obj_t;
pince2->rayon=0.04;
pince2->hauteur=0.115;
pince2->z=0.115/2;
pince2->type=OBJ_PINCE_OUVERTE;
pince2->couleur=getColorSDL(clGray);
pince->add_object(pince2);
pince->objects_pos[0].x=0.;
pince->objects_pos[0].y=0.;
pince->objects_pos[1].x=0.08;
pince->objects_pos[1].y=0.;
add_object(pince);
for(int i=0;i<2;i++)
tension_moteur[i]=0;
tension_courroie=0;
webcams.clear();
color_captors.clear();
dist_captors.clear();
element_in_clamp.clear();
}
//----------------------------------------------------------------------------
void robot_t::close_clamp()
{
object_t *clamp = ((union_obj_t*)objects[3])->objects[1];
vector_t clamp_pos = clamp->position -
clamp->G_rot;
double r2 = ((circle_obj_t*)clamp)->rayon;
r2 *= r2;
vector_t pos = ((union_obj_t*)objects[3])->objects_pos[1];
for(unsigned int i=0; i<simul_info->palets.size(); i++)
if((simul_info->palets[i]->position - clamp_pos).norme2() < r2 &&
clamp->z-clamp->hauteur/2. < simul_info->palets[i]->z+simul_info->palets[i]->hauteur/2. &&
clamp->z+clamp->hauteur/2. > simul_info->palets[i]->z-simul_info->palets[i]->hauteur/2.)
{
((palet_t*)(simul_info->palets[i]))->set_taken();
element_in_clamp.resize(element_in_clamp.size()+1);
element_in_clamp[element_in_clamp.size()-1] = (palet_t*)simul_info->palets[i];
((union_obj_t*)objects[3])->attach_object(simul_info->palets[i],pos);
simul_info->palets[i]->G_init=G_init-pos;
simul_info->detach_palet(simul_info->palets[i]);
}
clamp->type=OBJ_PINCE_FERMEE;
clamp->couleur=getColorSDL(clDarkGray);
}
//----------------------------------------------------------------------------
void robot_t::open_clamp()
{
((union_obj_t*)objects[3])->objects[1]->type=OBJ_PINCE_OUVERTE;
((union_obj_t*)objects[3])->objects[1]->couleur=getColorSDL(clGray);
for(unsigned int i=0; i<element_in_clamp.size(); i++)
{
simul_info->rattach_palet(element_in_clamp[i]);
((union_obj_t*)objects[3])->detach_object(element_in_clamp[i]);
element_in_clamp[i]->position -= element_in_clamp[i]->G_rot;
element_in_clamp[i]->G_init.y = 0.;
element_in_clamp[i]->G_init.x = 0.;
element_in_clamp[i]->G_init.y = 0.;
element_in_clamp[i]->G_rot.x = 0.;
element_in_clamp[i]->G_rot.y = 0.;
element_in_clamp[i]->set_free();
}
element_in_clamp.clear();
}
//------------------------------------------------------------------------------
void SDL_Draw_Robot()
{
vector_t N,T,u,v;
point_t point[4];
position_t pos,dest;
while(true)
{
Draw_SDL_Background();
// Draw
pos = cine_get_position();
dest = pp_get_dest();
// Robot supposé
N.x=cos(pos.a);
N.y=sin(pos.a);
T.x=-sin(pos.a);
T.y=cos(pos.a);
v=vector_t(_ROUE_X,0.).rotate(pos.a);
point[0].x=((int)((pos.x+N.x*_LONGUEUR_ROBOT/2.+T.x*_LARGEUR_ROBOT/2.-v.x)*_SCALE_SDL));
point[0].y=((int)((pos.y+N.y*_LONGUEUR_ROBOT/2.+T.y*_LARGEUR_ROBOT/2.-v.y)*_SCALE_SDL));
point[1].x=((int)((pos.x+N.x*_LONGUEUR_ROBOT/2.-T.x*_LARGEUR_ROBOT/2.-v.x)*_SCALE_SDL));
point[1].y=((int)((pos.y+N.y*_LONGUEUR_ROBOT/2.-T.y*_LARGEUR_ROBOT/2.-v.y)*_SCALE_SDL));
point[2].x=((int)((pos.x-N.x*_LONGUEUR_ROBOT/2.-T.x*_LARGEUR_ROBOT/2.-v.x)*_SCALE_SDL));
point[2].y=((int)((pos.y-N.y*_LONGUEUR_ROBOT/2.-T.y*_LARGEUR_ROBOT/2.-v.y)*_SCALE_SDL));
point[3].x=((int)((pos.x-N.x*_LONGUEUR_ROBOT/2.+T.x*_LARGEUR_ROBOT/2.-v.x)*_SCALE_SDL));
point[3].y=((int)((pos.y-N.y*_LONGUEUR_ROBOT/2.+T.y*_LARGEUR_ROBOT/2.-v.y)*_SCALE_SDL));
PolylineSDL(point, 4, getColorSDL(clWhite));
LigneSDL(point[2].x, point[2].y, ((int)(pos.x*_SCALE_SDL)), ((int)(pos.y*_SCALE_SDL)), getColorSDL(clWhite));
LigneSDL(point[3].x, point[3].y, ((int)(pos.x*_SCALE_SDL)), ((int)(pos.y*_SCALE_SDL)), getColorSDL(clWhite));
// Destination
N.x=cos(dest.a);
N.y=sin(dest.a);
T.x=-sin(dest.a)*0.03;
T.y=cos(dest.a)*0.03;
LigneSDL(((int)(_SCALE_SDL*dest.x)), ((int)(_SCALE_SDL*dest.y)),
((int)(_SCALE_SDL*(dest.x+N.x*0.1))), ((int)(_SCALE_SDL*(dest.y+N.y*0.1))), getColorSDL(clBlack));
LigneSDL(((int)(_SCALE_SDL*(dest.x+N.x*0.1))), ((int)(_SCALE_SDL*(dest.y+N.y*0.1))),
((int)(_SCALE_SDL*(dest.x+N.x*0.07+T.x))), ((int)(_SCALE_SDL*(dest.y+N.y*0.07+T.y))), getColorSDL(clBlack));
LigneSDL(((int)(_SCALE_SDL*(dest.x+N.x*0.1))), ((int)(_SCALE_SDL*(dest.y+N.y*0.1))),
((int)(_SCALE_SDL*(dest.x+N.x*0.07-T.x))), ((int)(_SCALE_SDL*(dest.y+N.y*0.07-T.y))), getColorSDL(clBlack));
// Captors
for(int i=0;i<4;i++)
{
vector_t pos = captor_get_position(i);
Uint32 color;
switch(captor_get_status(i))
{
case 0: color = getColorSDL(clBlack); break;
case 1: color = SDL_MapRGB(affichage->format, 255, 0, 180); break;
case 2: color = SDL_MapRGB(affichage->format, 255, 0, 0); break;
case 3: color = SDL_MapRGB(affichage->format, 60, 60, 60); break;
}
DisqueSDL(((int)(_SCALE_SDL*pos.x)),
((int)(_SCALE_SDL*pos.y)),
((int)(_SCALE_SDL*0.01)),color);
}
RefreshSDL();
usleep(40000);
}
}
//------------------------------------------------------------------------------
void SDL_First_Background()
{
printf("Creating background... ");
fflush(stdout);
FillRectSDL(0,0,((int)(_SCALE_SDL*_LONGUEUR_TER)),
((int)(_SCALE_SDL*_LARGEUR_TER)),getColorSDL(clGround));
FillRectSDL(0,0,((int)(_SCALE_SDL*0.5)),
((int)(_SCALE_SDL*0.5)),getColorSDL(clDarkGreen));
FillRectSDL(((int)(_SCALE_SDL*(_LONGUEUR_TER-0.5))),
0,
((int)(_SCALE_SDL*0.5)),
((int)(_SCALE_SDL*0.5)),getColorSDL(clDarkRed));
FillRectSDL(((int)(_SCALE_SDL*(_LONGUEUR_TER/2-0.9))),
((int)(_SCALE_SDL*(_LARGEUR_TER-0.1))),
((int)(_SCALE_SDL*1.8)),
((int)(_SCALE_SDL*0.1)),getColorSDL(clBrown));
FillRectSDL(((int)(_SCALE_SDL*(_LONGUEUR_TER/2-0.3))),
((int)(_SCALE_SDL*(_LARGEUR_TER-0.1))),
((int)(_SCALE_SDL*0.6)),
((int)(_SCALE_SDL*0.1)),getColorSDL(clDarkBrown));
DisqueSDL(((int)(_SCALE_SDL*_LONGUEUR_TER/2)),
((int)(_SCALE_SDL*_LARGEUR_TER/2)),
((int)(_SCALE_SDL*0.15)),getColorSDL(clDarkBrown));
FillRectSDL(((int)(_SCALE_SDL*(0.578))),
((int)(_SCALE_SDL*(_LARGEUR_TER-0.1))),
((int)(_SCALE_SDL*0.022)),
((int)(_SCALE_SDL*0.1)),getColorSDL(clWhite));
FillRectSDL(((int)(_SCALE_SDL*(_LONGUEUR_TER-0.6))),
((int)(_SCALE_SDL*(_LARGEUR_TER-0.1))),
((int)(_SCALE_SDL*0.022)),
((int)(_SCALE_SDL*0.1)),getColorSDL(clWhite));
DisqueSDL(((int)(_SCALE_SDL*0.289)),
((int)(_SCALE_SDL*(_LARGEUR_TER-0.04))),
((int)(_SCALE_SDL*0.04)),getColorSDL(clGray));
DisqueSDL(((int)(_SCALE_SDL*(_LONGUEUR_TER-0.289))),
((int)(_SCALE_SDL*(_LARGEUR_TER-0.04))),
((int)(_SCALE_SDL*0.04)),getColorSDL(clGray));
DisqueSDL(((int)(_SCALE_SDL*0.04)),
((int)(_SCALE_SDL*(_LARGEUR_TER/2-0.25))),
((int)(_SCALE_SDL*0.04)),getColorSDL(clGray));
DisqueSDL(((int)(_SCALE_SDL*(_LONGUEUR_TER-0.04))),
((int)(_SCALE_SDL*(_LARGEUR_TER/2-0.25))),
((int)(_SCALE_SDL*0.04)),getColorSDL(clGray));
DisqueSDL(((int)(_SCALE_SDL*0.04)),
((int)(_SCALE_SDL*(_LARGEUR_TER/2+0.25))),
((int)(_SCALE_SDL*0.04)),getColorSDL(clGray));
DisqueSDL(((int)(_SCALE_SDL*(_LONGUEUR_TER-0.04))),
((int)(_SCALE_SDL*(_LARGEUR_TER/2+0.25))),
((int)(_SCALE_SDL*0.04)),getColorSDL(clGray));
for(int i=0;i<3;i++)
for(int j=0;j<4;j++)
FillRectSDL(((int)(_SCALE_SDL*(_LONGUEUR_TER/2-0.9+0.105-0.0075+i*0.6+j*0.13))),
((int)(_SCALE_SDL*(_LARGEUR_TER-0.35))),
((int)(_SCALE_SDL*0.015)),
((int)(_SCALE_SDL*0.25)),getColorSDL(clBlack));
for(int i=0;i<4;i++)
FillRectSDL(((int)(_SCALE_SDL*(_LONGUEUR_TER/2-0.6-0.0075+i*0.4))),
0,
((int)(_SCALE_SDL*0.015)),
((int)(_SCALE_SDL*0.25)),getColorSDL(clBlack));
for(int i=0;i<3;i++)
for(int j=0;j<4;j++)
{
DisqueSDL(((int)(_SCALE_SDL*(_LONGUEUR_TER/2-0.4-i*0.25))),
((int)(_SCALE_SDL*(_LARGEUR_TER/2+0.125-j*0.2))),
((int)(_SCALE_SDL*0.005)),getColorSDL(clBlack));
DisqueSDL(((int)(_SCALE_SDL*(_LONGUEUR_TER/2+0.4+i*0.25))),
((int)(_SCALE_SDL*(_LARGEUR_TER/2+0.125-j*0.2))),
((int)(_SCALE_SDL*0.005)),getColorSDL(clBlack));
}
printf("ok\n");
fflush(stdout);
}
//----------------------------------------------------------------------------
void robot_t::draw()
{
vector_t _N,_T,u,v;
point_t point[4];
Uint32 C;
double demi_largeur_roue=0.005;
int i;
if(nbr_pics == 2) i = 1; else i = 0;
// Robot supposé
_N.x=cos(data[i].angle);
_N.y=sin(data[i].angle);
_T.x=-sin(data[i].angle);
_T.y=cos(data[i].angle);
v=G_init.rotate(data[i].angle);
point[0].x=((int)((data[i].posX+_N.x*dimX/2.+_T.x*dimY/2.-v.x)*simul_info->scale));
point[0].y=((int)((data[i].posY+_N.y*dimX/2.+_T.y*dimY/2.-v.y)*simul_info->scale));
point[1].x=((int)((data[i].posX+_N.x*dimX/2.-_T.x*dimY/2.-v.x)*simul_info->scale));
point[1].y=((int)((data[i].posY+_N.y*dimX/2.-_T.y*dimY/2.-v.y)*simul_info->scale));
point[2].x=((int)((data[i].posX-_N.x*dimX/2.-_T.x*dimY/2.-v.x)*simul_info->scale));
point[2].y=((int)((data[i].posY-_N.y*dimX/2.-_T.y*dimY/2.-v.y)*simul_info->scale));
point[3].x=((int)((data[i].posX-_N.x*dimX/2.+_T.x*dimY/2.-v.x)*simul_info->scale));
point[3].y=((int)((data[i].posY-_N.y*dimX/2.+_T.y*dimY/2.-v.y)*simul_info->scale));
PolylineSDL(point, 4, getColorSDL(clWhite));
LigneSDL(point[2].x, point[2].y, ((int)(data[i].posX*simul_info->scale)), ((int)(data[i].posY*simul_info->scale)), getColorSDL(clWhite));
LigneSDL(point[3].x, point[3].y, ((int)(data[i].posX*simul_info->scale)), ((int)(data[i].posY*simul_info->scale)), getColorSDL(clWhite));
// Robot
for(unsigned int i=0; i<objects.size(); i++)
objects[i]->draw();
// Destination
_N.x=cos(data[i].destA);
_N.y=sin(data[i].destA);
_T.x=-sin(data[i].destA)*0.03;
_T.y=cos(data[i].destA)*0.03;
LigneSDL(((int)(simul_info->scale*data[i].destX)), ((int)(simul_info->scale*data[i].destY)),
((int)(simul_info->scale*(data[i].destX+_N.x*0.1))), ((int)(simul_info->scale*(data[i].destY+_N.y*0.1))), getColorSDL(clBlack));
LigneSDL(((int)(simul_info->scale*(data[i].destX+_N.x*0.1))), ((int)(simul_info->scale*(data[i].destY+_N.y*0.1))),
((int)(simul_info->scale*(data[i].destX+_N.x*0.07+_T.x))), ((int)(simul_info->scale*(data[i].destY+_N.y*0.07+_T.y))), getColorSDL(clBlack));
LigneSDL(((int)(simul_info->scale*(data[i].destX+_N.x*0.1))), ((int)(simul_info->scale*(data[i].destY+_N.y*0.1))),
((int)(simul_info->scale*(data[i].destX+_N.x*0.07-_T.x))), ((int)(simul_info->scale*(data[i].destY+_N.y*0.07-_T.y))), getColorSDL(clBlack));
// roues
for(int i=0;i<2;i++)
{
u=position+roue[i].rotate_spec(cosinus,sinus);
v=u+N*rayon_roue+T*demi_largeur_roue;
v*=simul_info->scale;
point[0].x=((int)v.x);
point[0].y=((int)v.y);
v=u+N*rayon_roue-T*demi_largeur_roue;
v*=simul_info->scale;
point[1].x=((int)v.x);
point[1].y=((int)v.y);
v=u-N*rayon_roue-T*demi_largeur_roue;
v*=simul_info->scale;
point[2].x=((int)v.x);
point[2].y=((int)v.y);
v=u-N*rayon_roue+T*demi_largeur_roue;
v*=simul_info->scale;
point[3].x=((int)v.x);
point[3].y=((int)v.y);
C=makeColorSDL(moteur[i].w<0?127:0, moteur[i].w>0?127:0, moteur[i].derape?127:0);
PolylineSDL(point, 4, C);
LigneSDL(point[0].x, point[0].y, point[2].x, point[2].y, C);
LigneSDL(point[1].x, point[1].y, point[3].x, point[3].y, C);
LigneSDL((point[0].x+point[1].x)/2, (point[0].y+point[1].y)/2, (point[2].x+point[3].x)/2, (point[2].y+point[3].y)/2, C);
}
for(int i=0;i<nbr_color_captors;i++)
color_captors[i].draw();
}
