Subversion Repositories shark

Rev

Rev 1655 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1655 giacomo 1
#include <drivers/glib.h>
2
#include <stdlib.h>
3
#include <math.h>
4
#include "include/simcity.h"
5
#include "include/misc.h"
6
#include "include/car.h"
7
#include "include/draw.h"
8
 
9
char collision_sensor(car_data *cd) {
10
  float i,dx,dx1,dy,dy1,deltax,deltay;
11
  int colore,colore1;
12
  int speed_done=0;
13
  char col=FREE;
14
 
15
  dx=cd->xpos+2.0*cosine[normalize(cd->angle+90)];
16
  dy=cd->ypos-2.0*sine[normalize(cd->angle+90)];
17
  dx1=cd->xpos-8.0*cosine[normalize(cd->angle+90)];
18
  dy1=cd->ypos+8.0*sine[normalize(cd->angle+90)];
19
 
20
  for (i=0.0;i<MAX_LOOK;i+=0.1) {
21
    deltax=i*cosine[normalize(cd->angle)];
22
    deltay=i*sine[normalize(cd->angle)];
23
    sem_wait(&mutex);
24
    colore=grx_getpixel(my_rint(dx+deltax),my_rint(dy-deltay));
25
    colore1=grx_getpixel(my_rint(dx1+deltax),my_rint(dy1-deltay));
26
    sem_post(&mutex);
27
    if (((colore==white)||(colore1==white) || ((colore==red)||(colore1==red)))) {
28
      if((colore==white)||(colore1==white))
29
        col=STOP;
30
      else
31
        col=CAR;
32
      cd->dist_obs=i;
33
      cd->collision=1;
34
      speed_done=1;
35
    }
36
    if (!speed_done) {
37
      if (sens) {
38
        sem_wait(&mutex);
39
        grx_plot(my_rint(dx+deltax),my_rint(dy-deltay),gray);
40
        grx_plot(my_rint(dx1+deltax),my_rint(dy1-deltay),gray);
41
        sem_post(&mutex);
42
      }
43
    }
44
        }
45
        if (!speed_done) {
46
    cd->collision=0;
47
    cd->dist_obs=MAX_LOOK;
48
        }
49
  return col;
50
}
51
 
52
int sensore(car_data *cd) {
53
        int col_front,col_rear,col_middle;
54
        int done_front=0,done_rear=0,done_middle=0;
55
  float front,rear,middle,store_middle;
56
        int angolo;
57
        float i;
58
        float x1,y1,x05,y05;
59
        float xs,ys,xs1,ys1;
60
        float coordx,coordy,distx,disty;
61
 
62
  cd->front=cd->rear=cd->middle=front=rear=middle=store_middle=NORM_DIST;
63
  x1=cd->xpos-10.0*cosine[normalize(cd->angle)];  // coordinata sensore dietro
64
  y1=cd->ypos+10.0*sine[normalize(cd->angle)];
65
 
66
  x05=cd->xpos-5.0*cosine[normalize(cd->angle)];  // coordinata sensore mezzo
67
  y05=cd->ypos+5.0*sine[normalize(cd->angle)];
68
 
69
  for (i=0.0;i<MAX_DIST;i+=0.1) {
70
    coordx=i*cosine[normalize(cd->angle-LOOK_ANGLE)];
71
    coordy=i*sine[normalize(cd->angle-LOOK_ANGLE)];
72
    distx=i*cosine[normalize(cd->angle-DIST_ANGLE)];
73
    disty=i*sine[normalize(cd->angle-DIST_ANGLE)];
74
    col_front=grx_getpixel(my_rint(cd->xpos+coordx),my_rint(cd->ypos-coordy));
75
    col_rear=grx_getpixel(my_rint(x1+coordx),my_rint(y1-coordy));
76
    col_middle=grx_getpixel(my_rint(x05+distx),my_rint(y05-disty));
77
    if ((col_front==border) && !done_front) {
78
      done_front=1;
79
      cd->front=front=i;
80
                }
81
    if ((col_middle==border) && !done_middle) {
82
                  done_middle=1;
83
      middle=cd->middle=i;
84
                }
85
    if ((col_rear==border) && !done_rear) {
86
      done_rear=1;
87
      cd->rear=rear=i;
88
                }
89
    if (sens) {
90
      sem_wait(&mutex);
91
      if (!done_front) {
92
        grx_plot(my_rint(cd->xpos+coordx),my_rint(cd->ypos-coordy),gray);
93
      }
94
      if (!done_rear) {
95
        grx_plot(my_rint(x1+coordx),my_rint(y1-coordy),blue);
96
      }
97
      if (!done_middle) {
98
        grx_plot(my_rint(x05+distx),my_rint(y05-disty),green);
99
      }
100
      sem_post(&mutex);
101
    }
102
        }
103
 
104
        if (!done_front) {
105
    front=cd->front=MAX_DIST;
106
        }
107
        if (!done_rear) {
108
    rear=cd->rear=MAX_DIST;
109
        }
110
        if (!done_middle) {
111
    cd->middle=middle=MAX_DIST;
112
        }
113
  xs=cd->xpos+front*cosine[normalize(cd->angle-LOOK_ANGLE)];
114
  xs1=x1+rear*cosine[normalize(cd->angle-LOOK_ANGLE)];
115
  ys=cd->ypos-front*sine[normalize(cd->angle-LOOK_ANGLE)];
116
  ys1=y1-rear*sine[normalize(cd->angle-LOOK_ANGLE)];
117
        if (xs==xs1) {
118
          angolo=90;
119
        } else {
120
                angolo=my_rint(RAD_TO_DEG(atan((-ys1+ys)/(xs1-xs))));
121
        }
122
 
123
        if (angolo<0) angolo+=360;
124
        if (angolo>=360) angolo-=360;
125
 
126
        if ((ys-ys1<0) && (xs1-xs>0)) {
127
                angolo-=180;
128
        }
129
        if ((ys-ys1>=0) && (xs1-xs>0)) {
130
                angolo+=180;
131
        }
132
 
133
        if ((xs1-xs==0) && (ys-ys1>0)) {
134
                angolo=270;
135
        }
136
 
137
  if (abs(middle-NORM_DIST)>FRANTIC) {
138
    cd->correggi=1;
139
        } else {
140
    cd->somma=0;
141
    cd->correggi=0;
142
        }
143
 
144
  if (cd->correggi) {
145
          if (middle>NORM_DIST) {
146
      cd->somma-=CORR_FACT;
147
                }
148
                if (middle<NORM_DIST) {
149
      cd->somma+=CORR_FACT;
150
                }
151
        }
152
  angolo+=cd->somma;
153
        return angolo;
154
}
155
 
156
void ch_spd(car_data *cd,char ob_type) {
157
  float set_spd=0.0;
158
  float bf;
159
 
160
  if(cd->collision) {
161
    if(ob_type==STOP)
162
      bf=0.001;
163
    else
164
      bf=0.7;
165
    set_spd=cd->speed-(MAX_LOOK-cd->dist_obs)*bf;
166
    if(set_spd<0)
167
      set_spd=0;
168
  }
169
  else {
170
    set_spd=cd->speed+0.4;
171
    if(set_spd>MAX_SPEED)
172
      set_spd=MAX_SPEED;
173
  }
174
  cd->speed=set_spd;
175
}