Subversion Repositories shark

Rev

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

Rev Author Line No. Line
1655 giacomo 1
/*
2
 * Project: S.Ha.R.K.
3
 *
4
 * Coordinators:
5
 *   Giorgio Buttazzo    <giorgio@sssup.it>
6
 *   Paolo Gai           <pj@gandalf.sssup.it>
7
 *
8
 * Authors     :
9
 *   Paolo Gai           <pj@gandalf.sssup.it>
10
 *   Massimiliano Giorgi <massy@gandalf.sssup.it>
11
 *   Luca Abeni          <luca@gandalf.sssup.it>
12
 *   (see the web pages for full authors list)
13
 *
14
 * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
15
 *
16
 * http://www.sssup.it
17
 * http://retis.sssup.it
18
 * http://shark.sssup.it
19
 */
20
 
21
 
22
/**
23
 ------------
24
 CVS :        $Id: sensor.c,v 1.1.1.1 2004-05-24 18:03:39 giacomo Exp $
25
 
26
 File:        $File$
27
 Revision:    $Revision: 1.1.1.1 $
28
 Last update: $Date: 2004-05-24 18:03:39 $
29
 ------------
30
**/
31
 
32
/*
33
 * Copyright (C) 2000 Marco Dallera and Marco Fiocca
34
 *
35
 * This program is free software; you can redistribute it and/or modify
36
 * it under the terms of the GNU General Public License as published by
37
 * the Free Software Foundation; either version 2 of the License, or
38
 * (at your option) any later version.
39
 *
40
 * This program is distributed in the hope that it will be useful,
41
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
42
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
43
 * GNU General Public License for more details.
44
 *
45
 * You should have received a copy of the GNU General Public License
46
 * along with this program; if not, write to the Free Software
47
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
48
 *
49
 */
50
 
51
/*
52
 *              AUTO
53
 *
54
 * Another Unuseful Track simulatOr
55
 *
56
 * Authors: Marco Dallera
57
 *          Marco Fiocca
58
 *
59
 */
60
 
61
/* ------------ */
62
/*  Camera car  */
63
/* ------------ */
64
 
65
#include "include/auto.h"
66
#include "include/const.h"
67
#include "include/utils.h"
68
#include <drivers/glib.h>
69
 
70
#define RADIUS                  1
71
#define TOLL_FACTOR         4.0
72
#define MIN_DIFF        3
73
#define ROAD_COLOR              27
74
#define OFFROAD_COLOR   0
75
#define DEBUG_COLOR             GREEN
76
#define OUT_OF_TRACK    230
77
#define STRIP_DEFAULT   TEL_WIDTH+1
78
 
79
extern sem_t grx_mutex;
80
extern car_params cars[MAX_CAR_NUMBER];
81
 
82
void scanline(point_f *buffer, float x1, float y1, float x2, float y2, int factor);
83
void get_camera_car(BYTE *buffer, car_status status);
84
int find_track(BYTE *image, road_info *info);
85
int find_opps(BYTE *image, road_info *info);
86
void find_collision(BYTE *image, road_info *info, car_status status);
87
 
88
/* ----------------------------------------------------------------- */
89
 
90
TASK sensor(int index) {
91
  car_status actual_car_status;         // the actual car state
92
  road_info actual_road_info;           // the actual road informations
93
  BYTE image[TEL_WIDTH*TEL_HEIGHT];     // buffer contenente la visione della telecamera ruotata
94
 
95
  point sq1, sq2;
96
  point im1, im2;
97
 
98
  CAB road_status_cab = cars[index].road_status_cab;
99
  CAB car_status_cab  = cars[index].car_status_cab;
100
 
101
  // Draws display squares
102
  sq1.x = TRACK_X1 + TRACK_WIDTH + TEL_HEIGHT;
103
  sq1.y = TEL_HEIGHT*index + 1 + index;
104
  sq2.x = sq1.x + TEL_WIDTH + 1;
105
  sq2.y = sq1.y + TEL_HEIGHT + 1;
106
 
107
  // Sets viewer coords
108
  im1.x = sq1.x + 1;
109
  im1.y = sq1.y + 1;
110
  im2.x = im1.x + TEL_WIDTH - 1;
111
  im2.y = im1.y + TEL_HEIGHT - 1;
112
 
113
  // Inizializza il cab della strada
114
  actual_road_info.left_border = TEL_WIDTH/2;
115
  actual_road_info.right_border = TEL_WIDTH/2;
116
  actual_road_info.distance = TEL_HEIGHT;
117
  actual_road_info.curve = 0.0;
118
  actual_road_info.opps_number = 0;
119
  actual_road_info.flag = ROAD_OK;
120
  actual_road_info.collision = NO_COLL;
121
  set_road_info(actual_road_info, road_status_cab);
122
 
123
  task_endcycle();
124
 
125
  while(1) {
126
    /* reads car informations */
127
    actual_car_status = get_car_status(car_status_cab);
128
 
129
    /* implementazione riconoscimento strada (sti cazzi!!!) */
130
    get_camera_car(image, actual_car_status);
131
    find_track(image, &actual_road_info);
132
    find_opps(image, &actual_road_info);
133
    find_collision(image, &actual_road_info, actual_car_status);
134
 
135
    sem_wait(&grx_mutex);
136
    grx_rect(sq1.x, sq1.y, sq2.x, sq2.y, cars[index].color);
137
    grx_putimage(im1.x, im1.y, im2.x, im2.y, image);
138
    sem_post(&grx_mutex);
139
 
140
    /* sends informations to control task */
141
    set_road_info(actual_road_info, road_status_cab);
142
    task_endcycle();
143
  }
144
 
145
}
146
 
147
/* ----------------------------------------------------------------- */
148
 
149
void find_collision(BYTE *image, road_info *info, car_status status)
150
{
151
  point_f lt,rt,lb,rb,ct;
152
  /* the lines used to scan the quadrilate */
153
  point_f height_line[CAR_HEIGHT];
154
  int i;
155
  BYTE pix;
156
 
157
  info->collision = NO_COLL;
158
 
159
  ct.x = status.pos.x + (float)(CAR_HEIGHT/2 + 1) * cos(degree_to_rad(status.orient));
160
  ct.y = status.pos.y - (float)(CAR_WIDTH/2 + 1) * sin(degree_to_rad(status.orient));
161
 
162
  lb.x = ct.x - (float)(CAR_WIDTH/2.0) * cos(degree_to_rad(90-status.orient));
163
  rb.x = ct.x + (float)(CAR_WIDTH/2.0) * cos(degree_to_rad(90-status.orient));
164
  lt.x = lb.x - (float)(CAR_HEIGHT) * cos(degree_to_rad(status.orient));
165
  rt.x = rb.x - (float)(CAR_HEIGHT) * cos(degree_to_rad(status.orient));
166
 
167
  lb.y = ct.y - (float)(CAR_WIDTH/2.0) * sin(degree_to_rad(90-status.orient));
168
  rb.y = ct.y + (float)(CAR_WIDTH/2.0) * sin(degree_to_rad(90-status.orient));
169
  lt.y = lb.y + (float)(CAR_HEIGHT) * sin(degree_to_rad(status.orient));
170
  rt.y = rb.y + (float)(CAR_HEIGHT) * sin(degree_to_rad(status.orient));
171
 
172
  sem_wait(&grx_mutex);
173
  scanline(height_line,rb.x,rb.y,rt.x,rt.y,CAR_HEIGHT);
174
  for (i=0; i < CAR_HEIGHT; i++) {
175
    pix = grx_getpixel(round(height_line[i].x),round(height_line[i].y));
176
    if (pix != OFFROAD_COLOR && pix != ROAD_COLOR)
177
      info->collision = COLLISION_RIGHT;
178
  }
179
  scanline(height_line,lb.x,lb.y,lt.x,lt.y,CAR_HEIGHT);
180
  for (i=0; i < CAR_HEIGHT; i++) {
181
    pix = grx_getpixel(round(height_line[i].x),round(height_line[i].y));
182
    if (pix != OFFROAD_COLOR && pix != ROAD_COLOR)
183
      info->collision = COLLISION_LEFT;
184
  }
185
  sem_post(&grx_mutex);
186
 
187
}
188
 
189
/* ----------------------------------------------------------------- */
190
 
191
/* Puts in a buffer the image viewed from the camera according to the car status */
192
void get_camera_car(BYTE *buffer, car_status status) {
193
  /*
194
   *  lt ------ rt
195
   *   |            |
196
   *   |            |
197
   *  lb --ct-- rb
198
   */
199
  point_f lt,rt,lb,rb,ct;
200
  /* the lines used to scan the quadrilate */
201
  point_f width_line[TEL_WIDTH];
202
  point_f height_line[TEL_HEIGHT];
203
  int i,j;
204
  int tmpx,tmpy,tx,ty;
205
  float x,y;
206
  BYTE *row;
207
 
208
  ct.x = status.pos.x + (float)(CAR_WIDTH/2 + 1) * cos(degree_to_rad(status.orient));
209
  ct.y = status.pos.y - (float)(CAR_WIDTH/2 + 1) * sin(degree_to_rad(status.orient));
210
 
211
  /* for now the visual starts from the center of the car */
212
  lb.x = ct.x - (float)(TEL_WIDTH/2.0) * cos(degree_to_rad(90-status.orient));
213
  rb.x = ct.x + (float)(TEL_WIDTH/2.0) * cos(degree_to_rad(90-status.orient));
214
  lt.x = lb.x + (float)(TEL_HEIGHT) * cos(degree_to_rad(status.orient));
215
  rt.x = rb.x + (float)(TEL_HEIGHT) * cos(degree_to_rad(status.orient));
216
 
217
  lb.y = ct.y - (float)(TEL_WIDTH/2.0) * sin(degree_to_rad(90-status.orient));
218
  rb.y = ct.y + (float)(TEL_WIDTH/2.0) * sin(degree_to_rad(90-status.orient));
219
  lt.y = lb.y - (float)(TEL_HEIGHT) * sin(degree_to_rad(status.orient));
220
  rt.y = rb.y - (float)(TEL_HEIGHT) * sin(degree_to_rad(status.orient));
221
 
222
  /* scan only 2 sense thanks to parallelism */
223
  /*  ^ <-(height)
224
   *  ! lt
225
   *  !  |
226
   *  !  |
227
   *  ! lb ------ rb )
228
   *  0 -------------> <-(width
229
   */
230
  /* the width_line coordinates are relative, so in the for cycle
231
     they are summarized to absolute height_line values */
232
  scanline(width_line,0,0,rb.x - lb.x,rb.y - lb.y,TEL_WIDTH);
233
  scanline(height_line,lb.x,lb.y,lt.x,lt.y,TEL_HEIGHT);
234
 
235
  /* First fill row TEL_HEIGHT-1 of image and so on until row 0  */
236
  sem_wait(&grx_mutex);
237
  for (i=0; i <  TEL_HEIGHT; i++) {
238
    x = height_line[i].x;
239
    y = height_line[i].y;
240
    row = &buffer[TEL_WIDTH * (TEL_HEIGHT-1-i)];
241
    for (j=0; j <  TEL_WIDTH; j++) {
242
      tmpx = round(x + width_line[j].x);
243
      tmpy = round(y + width_line[j].y);
244
 
245
      // Test rispetto ai bordi
246
      if (tmpx >= 0 && tmpx < SCREEN_WIDTH && tmpy >= 0 && tmpy < SCREEN_HEIGHT) {
247
        tx = tmpx - TRACK_X1-1;
248
        ty = tmpy - TRACK_Y1-1;
249
        if (tx >= 0 && tx < TRACK_WIDTH && ty >= 0 && ty < TRACK_HEIGHT)
250
          row[j] = grx_getpixel(tmpx,tmpy);
251
        else
252
          row[j] = OUT_OF_TRACK;
253
      } else {
254
        row[j] = OUT_OF_TRACK;
255
      }
256
    }
257
  }
258
  sem_post(&grx_mutex);
259
}
260
 
261
/* ----------------------------------------------------------------- */
262
 
263
void scanline(point_f *buffer, float x1, float y1, float x2, float y2, int factor) {
264
  int i;
265
  float x,y;
266
  float xstep,ystep;
267
 
268
  xstep = (x2 - x1) / factor;
269
  ystep = (y2 - y1) / factor;
270
 
271
  // Start scan at <x1, y1>
272
  x = x1;
273
  y = y1;
274
 
275
  for (i = 0; i < factor; i++) {
276
    buffer[i].x = x;
277
    buffer[i].y = y;
278
    x += xstep;
279
    y += ystep;
280
  }
281
}
282
 
283
/* ----------------------------------------------------------------- */
284
 
285
/*
286
 * Approssima la curva quando non è più possibile farlo con la scansione
287
 * orizzontale
288
 * - image
289
 * - index              l'indice da cui cominciare la ricerca della curva
290
 * - distance   la distanza dalla curva
291
 */
292
float approx_curve(BYTE *image, int index, int distance, int d_center, road_strip *strips) {
293
  BYTE *p,tmp;
294
  int i,j,k,ctrl=0;
295
  point ms_center,ct;
296
 
297
  // Decide se la curva e' verso sinistra o verso destra
298
  p = &image[(TEL_HEIGHT - index - 1) * TEL_WIDTH];
299
 
300
  ms_center.x = strips[index-1].left;
301
  for (i = 0; i < min(3,strips[index-1].left); i++) {
302
    // La curva è a destra
303
    tmp = *(p + round(ms_center.x) - i);
304
    if (tmp != ROAD_COLOR) {
305
      ms_center.x = strips[index-1].right;
306
      break;
307
    }
308
  }
309
 
310
  p += round(ms_center.x);
311
  // Scandisce la colonna fino alla fine della strada
312
  for (i = index; i < TEL_HEIGHT; i++, p -= TEL_WIDTH) {
313
    if (*p != ROAD_COLOR)       break;
314
  }
315
 
316
  // Imposta il nuovo max_scost empirico
317
  ms_center.y = (i-1 + index) / 2;
318
 
319
  // i-1 è l'indice dell'ultima riga bianca
320
  p = &image[(TEL_HEIGHT - (i-1) - 1) * TEL_WIDTH + round(ms_center.x)];
321
  // scandisce da destra a sinistra
322
  if (round(ms_center.x) > 0) {
323
    for (j = 0; j < (round(ms_center.x) - strips[index-1].left); j++, p--) {
324
      if (*p != ROAD_COLOR)     break;
325
      // scandisce dall'alto in basso finchè non trova nero
326
      for (k = i-1; k > index; k--) {
327
        tmp = *(p + (i-1-k)*TEL_WIDTH);
328
        if (tmp != ROAD_COLOR) {
329
          ms_center.y = (k + index) / 2;
330
          ctrl = 1;
331
          break;
332
        }
333
      }
334
      if (ctrl) break;
335
    }
336
    ct.x = ms_center.x - j/2;
337
    ct.y = ms_center.y;
338
  } else {
339
    // scandisce da sinistra a destra
340
    for (j = 0; j < (strips[index-1].right - round(ms_center.x)); j++, p++) {
341
      if (*p != ROAD_COLOR)     break;
342
      // scandisce dall'alto in basso finchè non trova nero
343
      for (k = i-1; k > index; k--) {
344
        tmp = *(p + (i-1-k)*TEL_WIDTH);
345
        if (tmp != ROAD_COLOR) {
346
          ms_center.y = (k + index) / 2;
347
          ctrl = 1;
348
          break;
349
        }
350
      }
351
      if (ctrl) break;
352
    }
353
    ct.x = ms_center.x + j/2;
354
    ct.y = ms_center.y;
355
  }
356
 
357
  return rad_to_degree(atan2((double)(ct.y - distance),(double)(ct.x - d_center)));
358
}
359
 
360
/* ----------------------------------------------------------------- */
361
 
362
int find_distance(road_strip *strips, int max_scost, int rett_tol) {
363
  int i, distance;
364
  int center_car = TEL_WIDTH/2;
365
 
366
  // La distanza non puo' essere maggiore del max_scost
367
  distance = max_scost;
368
 
369
  for (i=1; i < max_scost; i++) {
370
    // Calcola la distanza dalla curva
371
    if (abs(strips[i].center - center_car) > rett_tol) {
372
      distance = i;
373
      break;
374
    }
375
  }
376
  return distance;
377
}
378
 
379
/* ----------------------------------------------------------------- */
380
 
381
/* Scandisce la linea row e inserisce nella strip index le informazioni.
382
 * Ritorna 1 se trova entrambi i bordi, altrimenti 0
383
 */
384
int scan_strip(BYTE *row, int index, road_strip *strips) {
385
  // Tiene conto della posizione dei bordi precedenti per trattare
386
  // meglio il caso di curve molto strette, dove possono esistere
387
  // 2 bordi destri o sinistri
388
 
389
  // Per evitare problemi nei casi in cui siano possibili 2
390
  // bordi destri o sinistri
391
  int lfound = 0;
392
  int rfound = 0;
393
  int center_car = TEL_WIDTH/2;
394
  int i;
395
 
396
  // Parte dal secondo pixel e finisce al penultimo
397
  for(i=1; i < (TEL_WIDTH-1); i++) {
398
    // Se trova la strada controlla se si tratta di bordo
399
    // destro o sinistro
400
    if (row[i] == ROAD_COLOR) {
401
      // bordo sinistro
402
      if (row[i - 1] != ROAD_COLOR) {
403
                                // Se ho già trovato questo tipo di bordo cerco
404
                                // quello + plausibile ...
405
        if (lfound) {
406
          // Test sulla striscia precedente
407
          if (index > 0) {
408
            if (abs(i - strips[index-1].left) <
409
                abs(strips[index].left - strips[index-1].left))
410
              strips[index].left = i;
411
          } else {
412
            // Test sul centro della macchina
413
            if (abs(i - center_car) < abs(strips[0].left - center_car))
414
              strips[index].left = i;
415
          }
416
          lfound++;
417
        } else {
418
          // ... altrimenti lo inizializzo
419
          strips[index].left = i;
420
          lfound = 1;
421
        }
422
                                // bordo destro
423
      } else if ((row[i + 1] != ROAD_COLOR) && lfound) {
424
                                // Se ho già trovato questo tipo di bordo cerco
425
                                // quello + plausibile ...
426
        if (rfound) {
427
          // Test sulla striscia precedente
428
          if (index > 0) {
429
            if (abs(i - strips[index-1].right) <
430
                abs(strips[index].right - strips[index-1].right))
431
              strips[index].right = i;
432
          } else {
433
            // Test sul centro della macchina
434
            if (abs(i - center_car) < abs(strips[0].right - center_car))
435
              strips[index].right = i;
436
          }
437
          rfound++;
438
        } else {
439
          // ... altrimenti lo inizializzo
440
          strips[index].right = i;
441
          rfound = 1;
442
        }
443
      }
444
    }
445
  }
446
 
447
  // Calcola il centro della strada se trova i bordi
448
  if (lfound && rfound)
449
    strips[index].center = (strips[index].left + strips[index].right) / 2;
450
 
451
  return (lfound && rfound);
452
}
453
 
454
/* ----------------------------------------------------------------- */
455
 
456
void where_am_i(BYTE *row, road_strip strip, road_info *info) {
457
  int i;
458
  int lfound = 0;
459
  int rfound = 0;
460
  int car_center = TEL_WIDTH/2;
461
 
462
  for(i=1; i < (TEL_WIDTH-1); i++) {
463
    // Se trova la strada controlla se si tratta di bordo
464
    // destro o sinistro
465
    if (row[i] == ROAD_COLOR) {
466
      // bordo sinistro
467
      if (row[i - 1] != ROAD_COLOR) {
468
        strip.left = i;
469
        lfound = 1;
470
        break;
471
      } else if (row[i + 1] != ROAD_COLOR) {
472
        strip.right = i;
473
        rfound = 1;
474
        break;
475
      }
476
    }
477
  }
478
 
479
  if (lfound && rfound) {
480
    info->flag = ROAD_OK;
481
  }
482
  else if (lfound && !rfound)   info->flag = LEFT_ONLY;
483
  else if (!lfound && rfound)    info->flag = RIGHT_ONLY;
484
  else {
485
    if (row[car_center] == OFFROAD_COLOR) info->flag = NO_ROAD;
486
    else    info->flag = ROAD_OK;
487
  }
488
  info->left_border = car_center - strip.left;
489
  info->right_border = strip.right - car_center;
490
}
491
 
492
/* ----------------------------------------------------------------- */
493
 
494
/* Analizza l'immagine vista dalla telecamera inserendo le informazioni
495
 * necessarie in info, ovvero la distanza dai bordi destro e sinistro,
496
 * la distanza dalla prossima curva e il suo angolo di curvatura.
497
 * Per il momento ritorna -1 se non riesce a trovare entrambi i bordi della
498
 * strada; in futuro gestiremo anche questa situazione.
499
 */
500
int find_track(BYTE *image, road_info *info) {
501
  // vettore contenente le informazioni su ogni striscia
502
  road_strip strips[TEL_HEIGHT];
503
  BYTE *row;
504
  int car_center = TEL_WIDTH/2;
505
  int max_scost=0;
506
  int once_scost=0;
507
  int j, rett_tol=0;
508
 
509
  info->flag = ROAD_OK;
510
 
511
  strips[0].left = STRIP_DEFAULT;
512
  strips[0].right = STRIP_DEFAULT;
513
  /* Tratta in modo personalizzato il primo tratto di strada per
514
     inizializzare alcune variabili e per capire situazioni particolari */
515
  row = &image[(TEL_HEIGHT - 1)*TEL_WIDTH];
516
  // Se trova i bordi ...
517
  if (scan_strip(row,0,strips)) {
518
    // ... calcola i bordi della strada e ...
519
    info->left_border = car_center - strips[0].left;
520
    info->right_border = strips[0].right - car_center;
521
    // ... inizializza la tolleranza per il rettilineo.
522
    rett_tol = (strips[0].right - strips[0].left) / TOLL_FACTOR;
523
  } else {
524
    // Se non trova il bordo ...
525
    // Mi sono perso, cercami la strada
526
    where_am_i(row,strips[0],info);
527
    return 1;
528
  }
529
 
530
  /* Ora controlla le altre strisce della strada */
531
 
532
  for(j=1; j < TEL_HEIGHT ; j++) {
533
    row = &image[(TEL_HEIGHT - j - 1)*TEL_WIDTH];
534
    // Se trova i bordi ...
535
    if (scan_strip(row, j, strips)) {
536
      // ... aggiorna l'indice di max scostamento.
537
      if (abs(strips[j].center - car_center) >
538
          abs(strips[max_scost].center - car_center)
539
          && abs(strips[j].center - car_center) > rett_tol) {
540
 
541
        max_scost = j;
542
        once_scost=1; // ha trovato almeno un max scost
543
      }
544
    } else {
545
      // Se max_scost non è mai stato settato, prendo il max
546
      if (!once_scost) {
547
        max_scost = j-1;
548
        once_scost = 1;
549
      }
550
 
551
      // se la distanza e il max_scost sono troppo vicini approssima la curva
552
      info->distance = find_distance(strips, max_scost, rett_tol);
553
      if ((max_scost - info->distance) < MIN_DIFF) {
554
        info->curve = approx_curve(image, j, info->distance,
555
                                   strips[info->distance].center,strips) - 90.0;
556
        return 1;
557
      }
558
      break;
559
    }
560
  }
561
 
562
  // Se max_scost non è mai stato settato, prendo il max
563
  if (!once_scost)      max_scost = TEL_HEIGHT-1;
564
  max_scost = min(max_scost,TEL_HEIGHT-1);
565
 
566
  info->distance = find_distance(strips,max_scost,rett_tol);
567
 
568
  // calcola l'angolo di curvatura solo se il conto e' attendibile
569
  if ((info->distance < (TEL_HEIGHT*0.9)) &&
570
      ((max_scost - info->distance) >= MIN_DIFF))
571
    info->curve = rad_to_degree(atan2((double)(max_scost - info->distance),
572
                                      (double)(strips[max_scost].center - strips[info->distance].center)));
573
  else {
574
    // se i conti sono empirici, si guarda la posizione dei due centri
575
    if (strips[max_scost].center > strips[info->distance].center)
576
      info->curve = 88.0;
577
    else if (strips[max_scost].center < strips[info->distance].center)
578
      info->curve = 92.0;
579
    else
580
      info->curve = 90.0;
581
  }
582
  info->curve -= 90.0;
583
  return 0;
584
}
585
 
586
/* ----------------------------------------------------------------- */
587
 
588
int find_opps(BYTE *image, road_info *info)
589
{
590
  int i, j;
591
  int l_limit, r_limit;         // the opposite empiric limits
592
  BYTE *row;
593
  int car_center = TEL_WIDTH/2;
594
  int opps_number = 0;
595
 
596
  // by default it needs to scan the entire row
597
  l_limit = TEL_WIDTH - 1;
598
  r_limit = 0;
599
 
600
  // Scans one row at a time
601
  for(j=1; j<TEL_HEIGHT; j++) {
602
    row = &image[(TEL_HEIGHT - j - 1) * TEL_WIDTH];
603
 
604
    // if some of the previous row has been identified as a car...
605
    if (opps_number > 0 &&
606
        j < (info->opps_list[opps_number - 1].y + CAR_HEIGHT + 1)) {
607
      // excludes CAR_HEIGHT lines from the bottom if the car
608
      l_limit = (car_center + info->opps_list[opps_number - 1].x) - CAR_WIDTH;
609
      r_limit = (car_center + info->opps_list[opps_number - 1].x) + CAR_WIDTH;
610
    }
611
    else {
612
      // it needs to scan the entire row
613
      l_limit = TEL_WIDTH - 1;
614
      r_limit = 0;
615
    }
616
 
617
    // Scans the current row
618
    for (i=0; i<TEL_WIDTH; i++) {
619
      if( (i < l_limit || i > r_limit) &&
620
          row[i] != ROAD_COLOR && row[i] != OFFROAD_COLOR &&
621
          row[i] != DEBUG_COLOR && row[i] != OUT_OF_TRACK) {
622
                                // there is an opponent car!!
623
                                // stores its position
624
        info->opps_list[opps_number].x = i - car_center;                // left -, right +
625
        info->opps_list[opps_number].y = j;
626
 
627
                                // increases number of opposites car in the sensor view range
628
        opps_number++;
629
      }
630
    }
631
  }
632
 
633
  info->opps_number = opps_number;
634
 
635
  return 0;
636
}
637
 
638
 
639
 
640
 
641
 
642
 
643
 
644
 
645
 
646
 
647
 
648
 
649
 
650