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 | } |