Subversion Repositories shark

#include <drivers/glib.h>
#include <stdlib.h>
#include <math.h>
#include "include/simcity.h"
#include "include/misc.h"
#include "include/car.h"
#include "include/draw.h"

char collision_sensor(car_data *cd) {
  float i,dx,dx1,dy,dy1,deltax,deltay;
  int colore,colore1;
  int speed_done=0;
  char col=FREE;

  dx=cd->xpos+2.0*cosine[normalize(cd->angle+90)];
  dy=cd->ypos-2.0*sine[normalize(cd->angle+90)];
  dx1=cd->xpos-8.0*cosine[normalize(cd->angle+90)];
  dy1=cd->ypos+8.0*sine[normalize(cd->angle+90)];

  for (i=0.0;i<MAX_LOOK;i+=0.1) {
    deltax=i*cosine[normalize(cd->angle)];
    deltay=i*sine[normalize(cd->angle)];
    sem_wait(&mutex);
    colore=grx_getpixel(my_rint(dx+deltax),my_rint(dy-deltay));
    colore1=grx_getpixel(my_rint(dx1+deltax),my_rint(dy1-deltay));
    sem_post(&mutex);
    if (((colore==white)||(colore1==white) || ((colore==red)||(colore1==red)))) {
      if((colore==white)||(colore1==white))
        col=STOP;
      else
        col=CAR;
      cd->dist_obs=i;
      cd->collision=1;
      speed_done=1;
    }
    if (!speed_done) {
      if (sens) {
        sem_wait(&mutex);
        grx_plot(my_rint(dx+deltax),my_rint(dy-deltay),gray);
        grx_plot(my_rint(dx1+deltax),my_rint(dy1-deltay),gray);
        sem_post(&mutex);
      }
    }
        }
        if (!speed_done) {
    cd->collision=0;
    cd->dist_obs=MAX_LOOK;
        }
  return col;
}

int sensore(car_data *cd) {
        int col_front,col_rear,col_middle;
        int done_front=0,done_rear=0,done_middle=0;
  float front,rear,middle,store_middle;
        int angolo;
        float i;
        float x1,y1,x05,y05;
        float xs,ys,xs1,ys1;
        float coordx,coordy,distx,disty;

  cd->front=cd->rear=cd->middle=front=rear=middle=store_middle=NORM_DIST;
  x1=cd->xpos-10.0*cosine[normalize(cd->angle)];  // coordinata sensore dietro
  y1=cd->ypos+10.0*sine[normalize(cd->angle)];

  x05=cd->xpos-5.0*cosine[normalize(cd->angle)];  // coordinata sensore mezzo
  y05=cd->ypos+5.0*sine[normalize(cd->angle)];

  for (i=0.0;i<MAX_DIST;i+=0.1) {
    coordx=i*cosine[normalize(cd->angle-LOOK_ANGLE)];
    coordy=i*sine[normalize(cd->angle-LOOK_ANGLE)];
    distx=i*cosine[normalize(cd->angle-DIST_ANGLE)];
    disty=i*sine[normalize(cd->angle-DIST_ANGLE)];
    col_front=grx_getpixel(my_rint(cd->xpos+coordx),my_rint(cd->ypos-coordy));
    col_rear=grx_getpixel(my_rint(x1+coordx),my_rint(y1-coordy));
    col_middle=grx_getpixel(my_rint(x05+distx),my_rint(y05-disty));
    if ((col_front==border) && !done_front) {
      done_front=1;
      cd->front=front=i;
                }
    if ((col_middle==border) && !done_middle) {
                  done_middle=1;
      middle=cd->middle=i;
                }
    if ((col_rear==border) && !done_rear) {
      done_rear=1;
      cd->rear=rear=i;
                }
    if (sens) {
      sem_wait(&mutex);
      if (!done_front) {
        grx_plot(my_rint(cd->xpos+coordx),my_rint(cd->ypos-coordy),gray);
      }
      if (!done_rear) {
        grx_plot(my_rint(x1+coordx),my_rint(y1-coordy),blue);
      }
      if (!done_middle) {
        grx_plot(my_rint(x05+distx),my_rint(y05-disty),green);
      }
      sem_post(&mutex);
    }
        }

        if (!done_front) {
    front=cd->front=MAX_DIST;
        }
        if (!done_rear) {
    rear=cd->rear=MAX_DIST;
        }
        if (!done_middle) {
    cd->middle=middle=MAX_DIST;
        }
  xs=cd->xpos+front*cosine[normalize(cd->angle-LOOK_ANGLE)];
  xs1=x1+rear*cosine[normalize(cd->angle-LOOK_ANGLE)];
  ys=cd->ypos-front*sine[normalize(cd->angle-LOOK_ANGLE)];
  ys1=y1-rear*sine[normalize(cd->angle-LOOK_ANGLE)];
        if (xs==xs1) {
          angolo=90;
        } else {
                angolo=my_rint(RAD_TO_DEG(atan((-ys1+ys)/(xs1-xs))));
        }

        if (angolo<0) angolo+=360;
        if (angolo>=360) angolo-=360;

        if ((ys-ys1<0) && (xs1-xs>0)) {
                angolo-=180;
        }
        if ((ys-ys1>=0) && (xs1-xs>0)) {
                angolo+=180;
        }

        if ((xs1-xs==0) && (ys-ys1>0)) {
                angolo=270;
        }

  if (abs(middle-NORM_DIST)>FRANTIC) {
    cd->correggi=1;
        } else {
    cd->somma=0;
    cd->correggi=0;
        }

  if (cd->correggi) {
          if (middle>NORM_DIST) {
      cd->somma-=CORR_FACT;
                }
                if (middle<NORM_DIST) {
      cd->somma+=CORR_FACT;
                }
        }
  angolo+=cd->somma;
        return angolo;
}

void ch_spd(car_data *cd,char ob_type) {
  float set_spd=0.0;
  float bf;

  if(cd->collision) {
    if(ob_type==STOP)
      bf=0.001;
    else
      bf=0.7;
    set_spd=cd->speed-(MAX_LOOK-cd->dist_obs)*bf;
    if(set_spd<0)
      set_spd=0;
  }
  else {
    set_spd=cd->speed+0.4;
    if(set_spd>MAX_SPEED)
      set_spd=MAX_SPEED;
  }
  cd->speed=set_spd;
}