960 lines
30 KiB
C
960 lines
30 KiB
C
#include "map.h"
|
|
|
|
#include <string.h>
|
|
|
|
#include "mymath.h"
|
|
#include "cutils.h"
|
|
|
|
map_t *map;
|
|
|
|
/*
|
|
* =========================================================================
|
|
* Geometry
|
|
* =========================================================================
|
|
*/
|
|
|
|
/** Euclidian distance betwen two points (xa,ya) and (xb,yb) */
|
|
float distance(float xa, float ya, float xb, float yb)
|
|
{
|
|
/* use of math.h function */
|
|
return hypotf(xa - xb, ya - yb);
|
|
}
|
|
|
|
/** Convert angle from radian to degree */
|
|
float todegree(float a)
|
|
{
|
|
return (a * 180.0) / M_PI;
|
|
}
|
|
|
|
/** Convert angle from degree to radian */
|
|
float toradian(float a)
|
|
{
|
|
return (a * M_PI) / 180.0;
|
|
}
|
|
|
|
/** Get the cadran of the angle (in degree) */
|
|
int tocadran(float a) {
|
|
if ((0 <= a && a < 90) ||
|
|
(-360 <= a && a < -270))
|
|
return 1;
|
|
if ((90 <= a && a < 180) ||
|
|
(-270 <= a && a < -180))
|
|
return 2;
|
|
if ((180 <= a && a < 270) ||
|
|
(-180 <= a && a < -90))
|
|
return 3;
|
|
return 4;
|
|
}
|
|
|
|
/** Angle (in degree) of a segment */
|
|
float lineAngle(float x1, float y1, float x2, float y2)
|
|
{
|
|
/* use of math.h function for arctan */
|
|
return todegree(atan2(y2-y1,x2-x1));
|
|
}
|
|
|
|
/** Angle inside an arc delimited by (from,to) */
|
|
bool isInArc(float a, float from, float to)
|
|
{
|
|
if (from < to) {
|
|
// trigo direction
|
|
return (from <= a && a <= to) || (from <= (a+360) && (a+360) <= to);
|
|
}
|
|
else {
|
|
// clock direction
|
|
return (to <= a && a <= from) || (to <= (a+360) && (a+360) <= from);
|
|
}
|
|
}
|
|
|
|
/** Angles in ordred inside an arc delimited by (from,to) */
|
|
bool fourAnglesInOrder(float from,float x,float y, float to)
|
|
{
|
|
if (from < to) {
|
|
// trigo direction
|
|
return
|
|
(from<=x && x<=y && y<=to) ||
|
|
(from<=x+360 && x+360<=y+360 && y+360<=to) ||
|
|
(from<=x && x<=y+360 && y+360<=to) ||
|
|
(from<=x+360 && x+360<=y && y<=to);
|
|
} else {
|
|
// clock direction
|
|
return (to <= y && y<=x && x <= from) ||
|
|
(to <= y+360 && y+360<=x+360 && x+360 <= from) ||
|
|
(to <= y+360 && y+360<=x && x <= from) ||
|
|
(to <= y && y<=x+360 && x+360 <= from);
|
|
}
|
|
}
|
|
|
|
/** Product of two vectors for directions */
|
|
float dirProd(float dir1x, float dir1y, float dir2x, float dir2y)
|
|
{
|
|
return dir1x * dir2x + dir1y * dir2y;
|
|
}
|
|
|
|
float dirVProd(float dir1x, float dir1y, float dir2x, float dir2y)
|
|
{
|
|
return dir1x * dir2y - dir1y * dir2x;
|
|
}
|
|
|
|
/** Size of a direction vector */
|
|
float dirNorm(float dir1x, float dir1y)
|
|
{
|
|
/* use of math.h function */
|
|
return hypot(dir1x, dir1y);
|
|
}
|
|
|
|
/** Cos of directions */
|
|
float dirCos(float dir1x, float dir1y, float dir2x, float dir2y)
|
|
{
|
|
return dirProd(dir1x, dir1y, dir2x, dir2y)
|
|
/ (dirNorm(dir1x, dir1y) * dirNorm(dir2x, dir2y));
|
|
}
|
|
|
|
/** Project point (x,y) on line d(p1,p2) and
|
|
* return result on (px, py)
|
|
*/
|
|
void
|
|
dirProjPoint( /* IN */ float x, float y, position_t * p1, position_t * p2,
|
|
/* OUT */ float *px, float *py)
|
|
{
|
|
//compute direction vector for d(p1, p2)
|
|
float dir_x = p2->x - p1->x;
|
|
float dir_y = p2->y - p1->y;
|
|
float dirn = dirNorm(dir_x, dir_y);
|
|
|
|
//compute P1 - H
|
|
float p1h = ((x - p1->x) * dir_x + (y - p1->y) * dir_y) / dirn;
|
|
|
|
//compute h
|
|
(*px) = p1->x + (p1h * dir_x) / dirn;
|
|
(*py) = p1->y + (p1h * dir_y) / dirn;
|
|
|
|
return;
|
|
}
|
|
|
|
/*
|
|
* =========================================================================
|
|
* Parsing
|
|
* =========================================================================
|
|
*/
|
|
|
|
typedef void (*map_segment_line_loader)(FILE *, /* file to read */
|
|
const char *, /* file name */
|
|
void *, /* pointer to the element
|
|
to be filled */
|
|
size_t); /* line number */
|
|
|
|
void map_load_segment(FILE *f,
|
|
const char *filename,
|
|
map_segment_line_loader loader,
|
|
const char *segment_name,
|
|
void *psegment,
|
|
int *segment_size,
|
|
size_t element_size,
|
|
size_t segment_max_size,
|
|
size_t *pline) {
|
|
assert (f);
|
|
assert (filename);
|
|
assert (segment_name);
|
|
assert (psegment);
|
|
assert (segment_size);
|
|
assert (pline);
|
|
|
|
char kword[15], **segment = (char **)psegment;
|
|
int rcode;
|
|
|
|
/* read until "segment_name" encountered */
|
|
while (true) {
|
|
rcode = fscanf(f, "%14s", kword);
|
|
(*pline)++;
|
|
|
|
if ((rcode == EOF) || (strcmp(kword, "end") == 0))
|
|
log_fatal("[map %s] could not find segment %s\n", filename, segment_name);
|
|
|
|
if (strcmp(kword, segment_name) == 0)
|
|
break;
|
|
}
|
|
|
|
/* "segment_name" has been read, now read item number */
|
|
rcode = fscanf(f, "%d", segment_size);
|
|
if (rcode == EOF)
|
|
log_fatal("[map %s] reached end of file while looking"
|
|
" for element count in segment %s (line %zu)\n",
|
|
filename, segment_name, *pline);
|
|
if ((*segment_size < 0) || (*segment_size > segment_max_size))
|
|
log_fatal("[map %s] too many (%d) elements in "
|
|
"segment %s (should be <= %d) (line %zu)\n",
|
|
filename, *segment_size, segment_name, segment_max_size, *pline);
|
|
(*pline)++;
|
|
|
|
/* allocate enough elements */
|
|
*segment = calloc(*segment_size, element_size);
|
|
assert(*segment);
|
|
|
|
log_info("[map %s] starting to read segment %s of size %zu\n",
|
|
filename, segment_name, *segment_size);
|
|
for (size_t i = 0; i < *segment_size; i++, (*pline)++)
|
|
loader(f, filename, &(*segment)[i * element_size], *pline);
|
|
log_info("[map %s] finished reading segment %s\n", filename, segment_name);
|
|
}
|
|
|
|
void rd_segment_line_loader(FILE *f,
|
|
const char *filename,
|
|
void *p,
|
|
size_t line) {
|
|
assert (f);
|
|
assert (filename);
|
|
assert (p);
|
|
|
|
char kword[15];
|
|
road_t *rd = (road_t *)p;
|
|
int rcode = fscanf(f, "%14s", kword);
|
|
|
|
if (rcode == EOF)
|
|
log_fatal("[map %s] 'line|arc' expected (line %zu)\n", filename, line);
|
|
|
|
if (strcmp(kword, "line") == 0) {
|
|
int nbdir = 0;
|
|
/* read "line <dir> <max_speed> <startX> <startY> <endX> <endY>" */
|
|
rcode = fscanf(f, "%d %d %f %f %f %f",
|
|
&nbdir, &rd->max_speed,
|
|
&rd->u.line.startp.x, &rd->u.line.startp.y,
|
|
&rd->u.line.endp.x, &rd->u.line.endp.y);
|
|
if ((rcode == EOF) ||
|
|
(nbdir < 1) ||
|
|
(nbdir > 2) ||
|
|
(rd->max_speed <= 0) ||
|
|
(rd->max_speed >= SPEED_MAX) ||
|
|
(rd->u.line.startp.x <= MIN_X) ||
|
|
(rd->u.line.startp.x >= MAX_X) ||
|
|
(rd->u.line.endp.x <= MIN_X) ||
|
|
(rd->u.line.endp.x >= MAX_X) ||
|
|
(rd->u.line.startp.y <= MIN_Y) ||
|
|
(rd->u.line.startp.y >= MAX_Y) ||
|
|
(rd->u.line.endp.y <= MIN_Y) ||
|
|
(rd->u.line.endp.y >= MAX_Y))
|
|
log_fatal("[map_read %s]: 'line' format error (line %zu)\n",
|
|
filename, line);
|
|
|
|
rd->color = COL_ROADLINE;
|
|
rd->kind = (nbdir == 1) ? RD_LINE_1 : RD_LINE_2;
|
|
rd->dir_x = (rd->u.line.endp.x - rd->u.line.startp.x);
|
|
rd->dir_y = (rd->u.line.endp.y - rd->u.line.startp.y);
|
|
} else if (strcmp(kword, "arc") == 0) {
|
|
int nbdir = 0;
|
|
/* read "arc <dir> <max_speed> <centerX> <centerY>
|
|
<radius> <start_angle> <end_angle>" */
|
|
rcode = fscanf(f, "%d %d %f %f %f %f %f",
|
|
&nbdir, &rd->max_speed,
|
|
&rd->u.arc.center.x, &rd->u.arc.center.y,
|
|
&rd->u.arc.radius,
|
|
&rd->u.arc.start_angle,
|
|
&rd->u.arc.end_angle);
|
|
if ((rcode == EOF) ||
|
|
(nbdir != 1) ||
|
|
(rd->max_speed <= 0) ||
|
|
(rd->max_speed >= SPEED_MAX) ||
|
|
(rd->u.arc.center.x <= MIN_X) ||
|
|
(rd->u.arc.center.x >= MAX_X) ||
|
|
(rd->u.arc.center.y <= MIN_Y) ||
|
|
(rd->u.arc.center.y >= MAX_Y) ||
|
|
(rd->u.arc.radius <= 0) ||
|
|
(rd->u.arc.start_angle < -180.0) ||
|
|
(rd->u.arc.start_angle >= 360.0) ||
|
|
(rd->u.arc.end_angle < -180.0) ||
|
|
(rd->u.arc.end_angle >= 360.0))
|
|
log_fatal("[map %s]: 'arc' format error (line %zu)\n",
|
|
filename, line);
|
|
rd->color = COL_ROADLINE;
|
|
rd->kind = RD_ARC;
|
|
rd->dir_x = 0.0;
|
|
rd->dir_y = (rd->u.arc.start_angle < rd->u.arc.end_angle) ? -1.0 : 1.0;
|
|
} else {
|
|
log_fatal("[map %s] unknown road type %s (line %zu)\n",
|
|
filename, kword, line);
|
|
}
|
|
}
|
|
|
|
void wayp_segment_line_loader(FILE *f,
|
|
const char *filename,
|
|
void *p,
|
|
size_t line) {
|
|
assert (f);
|
|
assert (filename);
|
|
assert (p);
|
|
|
|
int rcode;
|
|
waypoint_t *wp = (waypoint_t *)p;
|
|
|
|
/* read <road> <X> <Y> */
|
|
rcode = fscanf(f, "%d %f %f", &wp->road, &wp->position.x, &wp->position.y);
|
|
if ((rcode == EOF) ||
|
|
(wp->road < 0) ||
|
|
(wp->road >= map->road_sz) ||
|
|
(wp->position.x <= MIN_X) ||
|
|
(wp->position.x >= MAX_X) ||
|
|
(wp->position.y <= MIN_Y) ||
|
|
(wp->position.y >= MAX_Y)) {
|
|
log_fatal("[map %s] waypoint format error (line %zu)\n", filename, line);
|
|
}
|
|
}
|
|
|
|
void tl_segment_line_loader(FILE *f,
|
|
const char *filename,
|
|
void *p,
|
|
size_t line) {
|
|
assert (f);
|
|
assert (filename);
|
|
assert (p);
|
|
|
|
int rcode;
|
|
tlight_t *tl = (tlight_t *)p;
|
|
|
|
/* read <road> <X> <Y> <delayR> <delayA> <delayG> <dephase> */
|
|
rcode = fscanf(f, "%d %f %f %d %d %d %d",
|
|
&tl->road,
|
|
&tl->tl.ptl_pos.x, &tl->tl.ptl_pos.y,
|
|
&tl->tl.ptl_red, &tl->tl.ptl_amber, &tl->tl.ptl_green,
|
|
&tl->tl.ptl_phase);
|
|
if ((rcode == EOF) ||
|
|
(tl->road < 0) ||
|
|
(tl->road >= map->road_sz) ||
|
|
(tl->tl.ptl_pos.x <= MIN_X) ||
|
|
(tl->tl.ptl_pos.x >= MAX_X) ||
|
|
(tl->tl.ptl_pos.y <= MIN_Y) ||
|
|
(tl->tl.ptl_pos.y >= MAX_Y))
|
|
log_fatal("[map %s] traffic light format error (line %zu)\n",
|
|
filename, line);
|
|
}
|
|
|
|
void st_segment_line_loader(FILE *f,
|
|
const char *filename,
|
|
void *p,
|
|
size_t line) {
|
|
assert (f);
|
|
assert (filename);
|
|
assert (p);
|
|
|
|
int rcode;
|
|
stop_t *st = (stop_t *)p;
|
|
|
|
/* read <road> <tlight> <X> <Y> */
|
|
rcode = fscanf(f, "%d %d %f %f",
|
|
&st->road, &st->sema,
|
|
&st->position.x, &st->position.y);
|
|
if ((rcode == EOF) ||
|
|
(st->road < 0) ||
|
|
(st->road >= map->road_sz) ||
|
|
(st->sema < 0) ||
|
|
(st->sema >= map->tlight_sz) ||
|
|
(st->position.x <= MIN_X) ||
|
|
(st->position.x >= MAX_X) ||
|
|
(st->position.y <= MIN_Y) ||
|
|
(st->position.y >= MAX_Y))
|
|
log_fatal("[map %s] stop format error (line %zu)\n", filename, line);
|
|
}
|
|
|
|
void obst_segment_line_loader(FILE *f,
|
|
const char *filename,
|
|
void *p,
|
|
size_t line) {
|
|
assert (f);
|
|
assert (filename);
|
|
assert (p);
|
|
|
|
int rcode;
|
|
obst_t *obst = (obst_t *)p;
|
|
|
|
rcode = fscanf(f, "%f %f %f %f", &obst->pot_pos.x, &obst->pot_pos.y,
|
|
&obst->pot_since, &obst->pot_till);
|
|
if ((rcode == EOF) ||
|
|
(obst->pot_pos.x <= MIN_X) ||
|
|
(obst->pot_pos.x >= MAX_X) ||
|
|
(obst->pot_pos.y <= MIN_Y) ||
|
|
(obst->pot_pos.y >= MAX_Y))
|
|
log_fatal("[map %s] obstacle format error (line %zu)\n", filename, line);
|
|
}
|
|
|
|
void iti_segment_line_loader(FILE *f,
|
|
const char *filename,
|
|
void *p,
|
|
size_t line) {
|
|
assert (f);
|
|
assert (filename);
|
|
assert (p);
|
|
|
|
char kword[15];
|
|
iti_t *iti = (iti_t *)p;
|
|
int rcode = fscanf(f, "%14s %f", kword, &iti->param);
|
|
|
|
if (strcmp(kword, "go") == 0) {
|
|
iti->act = Globals__Go;
|
|
if ((rcode == EOF) || (iti->param <= 0.f) || (iti->param > SPEED_MAX))
|
|
log_fatal("[map %s] itinerary format error (action Go, line %zu)\n",
|
|
filename, line);
|
|
} else if (strcmp(kword, "turn") == 0) {
|
|
iti->act = Globals__Turn;
|
|
if ((rcode == EOF) || (iti->param < -180.0) || (iti->param >= 360.0))
|
|
log_fatal("[map %s] itinerary format error (action Turn, line %zu)\n",
|
|
filename, line);
|
|
} else if (strcmp(kword, "stop") == 0) {
|
|
iti->act = Globals__Stop;
|
|
iti->param = 0.f; /* Dummy */
|
|
} else {
|
|
log_fatal("[map %s] itinerary format error (unknown action %s, line %zu)\n",
|
|
filename, kword, line);
|
|
}
|
|
}
|
|
|
|
void map_read_string_line(FILE *f, const char *filename,
|
|
const char *expected_kw, char *buff, size_t *pline) {
|
|
char kword[15];
|
|
int rcode = fscanf(f, "%14s %249s", kword, buff);
|
|
if ((rcode == EOF) || (strcmp(kword, expected_kw) != 0))
|
|
log_fatal("[map %s] could not read %s (line %zu)\n",
|
|
filename, expected_kw, *pline);
|
|
if (buff[0] != '"' || buff[strlen(buff) - 1] != '"')
|
|
log_fatal("[map %s] string after %s should be between double quotes"
|
|
" (line %zu)\n",
|
|
filename, expected_kw, *pline);
|
|
(*pline)++;
|
|
|
|
/* Remove surrounding double quotes. */
|
|
memmove(buff, buff + 1, strlen(buff) - 2);
|
|
buff[strlen(buff) - 2] = 0;
|
|
|
|
log_info("[map %s] read %s: %s\n", filename, expected_kw, buff);
|
|
}
|
|
|
|
void map_load(const char *filename) {
|
|
map = malloc(sizeof *map);
|
|
assert(map);
|
|
bzero(map, sizeof *map);
|
|
|
|
size_t line = 1;
|
|
FILE *f = fopen(filename, "r");
|
|
if (!f)
|
|
log_fatal("[map %s] could not open file\n", filename);
|
|
|
|
map_read_string_line(f, filename, "map", map->name, &line);
|
|
map_read_string_line(f, filename, "graphics", map->graphics, &line);
|
|
map_read_string_line(f, filename, "guide", map->guide, &line);
|
|
|
|
char kword[15];
|
|
int rcode = fscanf(f, "%14s %f %f %f", kword,
|
|
&map->init_phase.ph_pos.x,
|
|
&map->init_phase.ph_pos.y,
|
|
&map->init_phase.ph_head);
|
|
if ((rcode == EOF) || (strcmp(kword, "init") != 0))
|
|
log_fatal("[map %s] could not read init (line %zu)\n", filename, line);
|
|
line++;
|
|
log_info("[map %s] read init: x = %f, y = %f, head = %f\n",
|
|
filename,
|
|
map->init_phase.ph_pos.x,
|
|
map->init_phase.ph_pos.y,
|
|
map->init_phase.ph_head);
|
|
|
|
/* Read the roads. */
|
|
map_load_segment(f, filename, rd_segment_line_loader, "rd",
|
|
&map->road_arr, &map->road_sz, sizeof *map->road_arr,
|
|
MAX_ROAD_COUNT, &line);
|
|
|
|
/* Read the waypoints. */
|
|
map_load_segment(f, filename, wayp_segment_line_loader, "wp",
|
|
&map->wayp_arr, &map->wayp_sz, sizeof *map->wayp_arr,
|
|
MAX_WAYP_COUNT, &line);
|
|
|
|
/* Read the traffic lights. */
|
|
map_load_segment(f, filename, tl_segment_line_loader, "tl",
|
|
&map->tlight_arr, &map->tlight_sz, sizeof *map->tlight_arr,
|
|
MAX_TL_COUNT, &line);
|
|
|
|
/* Read the stops. */
|
|
map_load_segment(f, filename, st_segment_line_loader, "st",
|
|
&map->stop_arr, &map->stop_sz, sizeof *map->stop_arr,
|
|
MAX_STOP_COUNT, &line);
|
|
|
|
/* Read the obstacles. */
|
|
map_load_segment(f, filename, obst_segment_line_loader, "obst",
|
|
&map->obst_arr, &map->obst_sz, sizeof *map->obst_arr,
|
|
MAX_OBST_COUNT, &line);
|
|
|
|
/* Read the obstacles. */
|
|
map_load_segment(f, filename, iti_segment_line_loader, "iti",
|
|
&map->iti_arr, &map->iti_sz, sizeof *map->iti_arr,
|
|
MAX_ITI_COUNT, &line);
|
|
|
|
/* Close file and return. */
|
|
fclose(f);
|
|
}
|
|
|
|
void map_destroy() {
|
|
if (!map)
|
|
return;
|
|
|
|
/* Fields have been initialized to NULL, it is safe to call free() on them. */
|
|
free(map->road_arr);
|
|
free(map->wayp_arr);
|
|
free(map->tlight_arr);
|
|
free(map->stop_arr);
|
|
free(map->obst_arr);
|
|
free(map);
|
|
map = NULL;
|
|
}
|
|
|
|
void Map__read_obstacles_step(Map__read_obstacles_out *o) {
|
|
/* TODO very inefficient */
|
|
memcpy(o->obst, map->obst_arr, map->obst_sz * sizeof *o->obst);
|
|
|
|
/* The map file might contain fewer obstacles than Globals__obstnum. */
|
|
for (size_t i = map->obst_sz; i < MAX_OBST_COUNT; i++) {
|
|
o->obst[i].pot_pos.x = 0.f;
|
|
o->obst[i].pot_pos.y = 0.f;
|
|
o->obst[i].pot_since = -1.f;
|
|
o->obst[i].pot_till = -1.f;
|
|
}
|
|
}
|
|
|
|
void Map__read_itinerary_step(Map__read_itinerary_out *o) {
|
|
/* TODO very inefficient */
|
|
memcpy(o->iti, map->iti_arr, map->iti_sz * sizeof *o->iti);
|
|
|
|
/* The map file might contain fewer itinerary steps than Globals__itinum. */
|
|
for (size_t i = map->iti_sz; i < MAX_ITI_COUNT; i++) {
|
|
o->iti[i].act = Globals__Stop;
|
|
o->iti[i].param = 0.f;
|
|
}
|
|
}
|
|
|
|
void Map__read_traffic_lights_step(Map__read_traffic_lights_out *o) {
|
|
assert (map->tlight_sz <= Globals__trafnum);
|
|
for (size_t i = 0; i < map->tlight_sz; i++)
|
|
o->tlights[i] = map->tlight_arr[i].tl;
|
|
for (size_t i = map->tlight_sz; i < MAX_TL_COUNT; i++)
|
|
o->tlights[i] = (Globals__param_tlight){ {-100, -100}, 0, 0, 0, 0 };
|
|
}
|
|
|
|
bool colors_equal(const Globals__color *a, const Globals__color *b) {
|
|
return a->red == b->red && a->green == b->green && a->blue == b->blue;
|
|
}
|
|
|
|
bool isOnRoadLine1(road_t *rd, float x, float y,
|
|
Globals__color *col, double *d, float *dir_x, float *dir_y) {
|
|
//test that may be inside road area
|
|
// -compute projection on line
|
|
float px = 0.0;
|
|
float py = 0.0;
|
|
dirProjPoint(x, y, &(rd->u.line.startp), &(rd->u.line.endp), &px, &py);
|
|
//-compare with ends of the segment
|
|
if ((px <= fmax(rd->u.line.startp.x, rd->u.line.endp.x)+EPS) &&
|
|
(px >= fmin(rd->u.line.startp.x, rd->u.line.endp.x)-EPS) &&
|
|
(py <= fmax(rd->u.line.startp.y, rd->u.line.endp.y)+EPS) &&
|
|
(py >= fmin(rd->u.line.startp.y, rd->u.line.endp.y)-EPS))
|
|
//the projection is inside, compute distance to rd
|
|
(*d) = distance(x, y, px, py);
|
|
else
|
|
//the projection is outside the segment
|
|
// let us test the extremities:
|
|
(*d) = fmin(distance(x,y,rd->u.line.startp.x,rd->u.line.startp.y),
|
|
distance(x,y,rd->u.line.endp.x,rd->u.line.endp.y));
|
|
|
|
//-compare with the width of the road
|
|
if ((*d) >= RD_SIZE_HALF_WIDTH) {
|
|
log_debug("[geometry] (%.2f, %.2f) is too far from road %p (dist. %.2f)!\n",
|
|
x, y, rd, *d);
|
|
return false;
|
|
}
|
|
|
|
(*col) = rd->color;
|
|
(*dir_x) = rd->u.line.endp.x - rd->u.line.startp.x;
|
|
(*dir_y) = rd->u.line.endp.y - rd->u.line.startp.y;
|
|
|
|
if ((*d) < RD_SIZE_LINE) {
|
|
//distance less than width of the road line,
|
|
//then on road with one color
|
|
//log_debug("Position on road line: %f!\n", *d);
|
|
}
|
|
else {
|
|
//otherwise, compute the side of the road
|
|
// left or right using direction(x, y)->(px, py)
|
|
double dirPX = px - x; /* -b where b = x1 - x2 */
|
|
double dirPY = py - y; /* a where a = y2 - y1 */
|
|
double sinDir = dirVProd(*dir_x, *dir_y, dirPX, dirPY);
|
|
if (sinDir < 0) {
|
|
//on left
|
|
(*col) = COL_ROADLEFT;
|
|
#ifndef MAP_BMP
|
|
if ((*d) <= RD_SIZE_LINE2)
|
|
col->green = (unsigned int)(255.*((*d)-1.0));
|
|
#endif
|
|
} else {
|
|
//on right
|
|
(*col) = COL_ROADRIGHT;
|
|
#ifndef MAP_BMP
|
|
if ((*d) <= RD_SIZE_LINE2)
|
|
col->red = (unsigned int)(255.*((*d)-1.0));
|
|
#endif
|
|
}
|
|
//log_debug("Position on road: %f!\n", *d);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool isOnRoadLine2(road_t *rd, float x, float y,
|
|
Globals__color *col, double *d, float *dir_x, float *dir_y) {
|
|
/* TODO missing from original project */
|
|
return false;
|
|
}
|
|
|
|
bool isOnRoadArc(/* IN */
|
|
road_t *rd, float x, float y,
|
|
/* OUT */
|
|
Globals__color *col, double *d,
|
|
float *dir_x, float *dir_y)
|
|
{
|
|
//center
|
|
double cx = rd->u.arc.center.x;
|
|
double cy = rd->u.arc.center.y;
|
|
|
|
//compute angle(in degrees) from center
|
|
double a = lineAngle(cx, cy, x, y);
|
|
|
|
//compute signed distance to the circle c
|
|
double dist_c = distance(cx, cy, x, y) - rd->u.arc.radius;
|
|
|
|
if (isInArc(a, rd->u.arc.start_angle,rd->u.arc.end_angle)) {
|
|
//within the angle
|
|
(*d) = fabs(dist_c);
|
|
}
|
|
else {
|
|
//Point outside road angles,
|
|
// consider the distance to extremities
|
|
(*d) = fmin(distance(x,y,
|
|
cx+rd->u.arc.radius*cos(toradian(rd->u.arc.start_angle)),
|
|
cy+rd->u.arc.radius*sin(toradian(rd->u.arc.start_angle))),
|
|
distance(x,y,
|
|
cx+rd->u.arc.radius*cos(toradian(rd->u.arc.end_angle)),
|
|
cy+rd->u.arc.radius*sin(toradian(rd->u.arc.end_angle))));
|
|
}
|
|
|
|
if ((*d) >= RD_SIZE_HALF_WIDTH) {
|
|
//log_debug("Position too far from road: %f!\n", (*d));
|
|
return false;
|
|
}
|
|
|
|
(*col) = rd->color;
|
|
|
|
//direction is normal to d(c, (x, y))
|
|
//its sign depends on rotation
|
|
if (rd->u.arc.start_angle < rd->u.arc.end_angle){//counterclockwise
|
|
(*dir_x) = cy - y;
|
|
(*dir_y) = x - cx;
|
|
}
|
|
else {//clockwise
|
|
(*dir_x) = y - cy; /* a where a = y2 - y1 */
|
|
(*dir_y) = cx - x; /* b where b = x1 - x2 */
|
|
}
|
|
|
|
if ((*d) < RD_SIZE_LINE) {
|
|
//distance less than width of the road line,
|
|
//then on road with one color
|
|
//log_debug("Point on road line: %f!\n", *d);
|
|
} else {
|
|
//otherwise, compute the side of the road
|
|
// left or right using the sense(trigo or clock) of the road
|
|
if (((rd->u.arc.start_angle < rd->u.arc.end_angle) //trigo dir
|
|
&& dist_c < 0) ||
|
|
((rd->u.arc.start_angle > rd->u.arc.end_angle) // clock dir
|
|
&& dist_c > 0)) {
|
|
(*col) = COL_ROADLEFT;
|
|
#ifndef MAP_BMP
|
|
if ((*d) <= RD_SIZE_LINE2)
|
|
col->green = (unsigned int)(255.*((*d)-1.0));
|
|
#endif
|
|
} else {
|
|
(*col) = COL_ROADRIGHT;
|
|
#ifndef MAP_BMP
|
|
if ((*d) <= RD_SIZE_LINE2)
|
|
col->red = (unsigned int)(255.*((*d)-1.0));
|
|
#endif
|
|
}
|
|
}
|
|
//log_debug("Point on road: %f!\n", *d);
|
|
return true;
|
|
}
|
|
|
|
bool isAfterStop(int x, int y, int rid, float dir_x, float dir_y, int* tl)
|
|
{
|
|
(*tl) = -1;
|
|
|
|
if (map->stop_arr == NULL ||
|
|
map->stop_sz <= 0)
|
|
return false;
|
|
//no stop points
|
|
|
|
for (int i = 0; i < map->stop_sz; i++) {
|
|
stop_t *sp = &map->stop_arr[i];
|
|
if (sp->road != rid)
|
|
continue;
|
|
// check the distance to the point
|
|
road_t *rd = &map->road_arr[sp->road];
|
|
if (rd->kind == RD_LINE_1 ||
|
|
rd->kind == RD_LINE_2) {
|
|
// line
|
|
// - compute projection on roadline
|
|
float px = 0.0;
|
|
float py = 0.0;
|
|
dirProjPoint(x, y, &(rd->u.line.startp), &(rd->u.line.endp), &px, &py);
|
|
// - compare with the bounds of the stop point
|
|
double dsp = distance(px, py, sp->position.x, sp->position.y);
|
|
double dir_x = px - sp->position.x;
|
|
double dir_y = py - sp->position.y;
|
|
if (dsp >= (RD_SIZE_STOP-EPS) &&
|
|
dsp <= (STP_AFTER+RD_SIZE_STOP+EPS) &&
|
|
(dirProd(rd->dir_x, rd->dir_y, dir_x, dir_y) >= 0))
|
|
{
|
|
log_debug("[geometry] (%d, %d) is after stop %d (dist %.2f)\n",
|
|
x, y, i, dsp);
|
|
(*tl) = sp->sema;
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
log_debug("Position too far/not AFTER point on line: %lf!\n", dsp);
|
|
log_debug("rd->dir(%lf,%lf) <> dir(%lf,%lf)\n", rd->dir_x, rd->dir_y,
|
|
dir_x, dir_y);
|
|
}
|
|
}
|
|
else {
|
|
// arc
|
|
// -compute angle of point in degrees
|
|
double apoint = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y,
|
|
sp->position.x, sp->position.y);
|
|
double apos = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y, x, y);
|
|
double dsp = toradian(fabs(apoint - apos))*rd->u.arc.radius;
|
|
if (dsp >= (RD_SIZE_STOP-EPS) &&
|
|
dsp <= (STP_AFTER+RD_SIZE_STOP+EPS)) {
|
|
// - check that the direction is AFTER
|
|
if (fourAnglesInOrder(rd->u.arc.start_angle,
|
|
apoint, apos,
|
|
rd->u.arc.end_angle))
|
|
{
|
|
log_debug("Position AFTER point on arc: %lf!\n", apos);
|
|
(*tl) = sp->sema;
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
log_debug("Position too far/not AFTER point on arc: %lf!\n", apos);
|
|
return false;
|
|
}
|
|
}
|
|
else {
|
|
log_debug("Position too far/not AFTER point on arc: %lf degrees!\n", apos);
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool
|
|
isPositionOnPoint(float x, float y, road_t* rd, position_t* p, double pWidth)
|
|
{
|
|
if (rd->kind == RD_LINE_1 ||
|
|
rd->kind == RD_LINE_2) { // line
|
|
// - compute projection on road line
|
|
float px = 0.0;
|
|
float py = 0.0;
|
|
position_t startp = {0, 0};
|
|
position_t endp = {0, 0};
|
|
|
|
startp = (rd->u.line.startp);
|
|
endp = (rd->u.line.endp);
|
|
dirProjPoint(x, y, &(startp), &(endp), &px, &py);
|
|
// - compare with the bounds of the waypoint
|
|
double dwp = distance(px, py, p->x, p->y);
|
|
if (dwp <= pWidth) {
|
|
log_debug("Position near point on line: %lf!\n", dwp);
|
|
return true;
|
|
}
|
|
else {
|
|
//log_debug("Position too far from point on line: %lf!\n", dwp);
|
|
return false;
|
|
}
|
|
}
|
|
else {
|
|
// arc
|
|
// - compute angle of point in degrees
|
|
double apoint = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y, p->x, p->y);
|
|
double apos = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y, x, y);
|
|
double dwp = toradian(fabs(apoint - apos))*rd->u.arc.radius;
|
|
if (dwp <= pWidth) {
|
|
log_debug("Position near point on arc: %lf!\n", dwp);
|
|
return true;
|
|
}
|
|
else {
|
|
log_debug("Position too far from point on arc: %lf!\n", dwp);
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
Globals__color getColorPoint(int rid, float x, float y, int* tl) {
|
|
// first go through waypoints
|
|
log_debug("[geometry] looking for waypoints at (%.2f, %.2f) on road %d\n",
|
|
x, y, rid);
|
|
for (int i = 0; i < map->wayp_sz; i++) {
|
|
waypoint_t *wp = &map->wayp_arr[i];
|
|
if (wp->road != rid)
|
|
{
|
|
log_debug("Waypoint not on road %d!\n", rid);
|
|
continue;
|
|
}
|
|
road_t *rd = &map->road_arr[wp->road];
|
|
// waitpoints are ruban
|
|
if (isPositionOnPoint(x, y, rd, &(wp->position), RD_SIZE_WAYPOINT)) {
|
|
return COL_WAYPOINT;
|
|
//one waypoint by position
|
|
}
|
|
}
|
|
log_debug("[geometry] finished walking waypoints\n");
|
|
|
|
// then go to stop points
|
|
log_debug("[geometry] looking for stops at (%.2f, %.2f) on road %d\n",
|
|
x, y, rid);
|
|
for (int i = 0; i < map->stop_sz; i++) {
|
|
stop_t *sp = &map->stop_arr[i];
|
|
if (sp->road != rid) {
|
|
log_debug("Stop not on road %d!\n", rid);
|
|
continue;
|
|
}
|
|
road_t *rd = &map->road_arr[sp->road];
|
|
// stop points are ruban
|
|
if (isPositionOnPoint(x, y, rd, &sp->position, RD_SIZE_STOP)) {
|
|
log_debug("[geometry] (%.2f, %.2f) at stop %d\n", x, y, i);
|
|
*tl = sp->sema;
|
|
return COL_STOP;
|
|
//one stop by position
|
|
}
|
|
}
|
|
log_debug("[geometry] finished walking stops\n");
|
|
|
|
return COL_OUT;
|
|
}
|
|
|
|
DEFINE_HEPT_FUN(Map, lookup_pos, (Globals__position pos)) {
|
|
float x = pos.x, y = pos.y;
|
|
|
|
out->data.on_road = false;
|
|
out->data.color = COL_OUT;
|
|
out->data.max_speed = SPEED_MIN;
|
|
out->data.tl_number = -1;
|
|
out->data.tl_required = false;
|
|
out->data.dir_x = -1.0;
|
|
out->data.dir_y = 0.0;
|
|
|
|
log_debug("[geometry] querying pos (%.2f, %.2f)\n", x, y);
|
|
if (map == NULL)
|
|
log_fatal("[geometry] map has not been initialized\n");
|
|
|
|
int min_rd = -1;
|
|
double min_d = 700.;
|
|
for (int rid = 0; rid < map->road_sz; rid++) {
|
|
road_t *rd = &map->road_arr[rid];
|
|
double d = 0.;
|
|
Globals__color col = COL_OUT;
|
|
bool onRoad = false;
|
|
float dir_X = 0.0;
|
|
float dir_Y = 0.0;
|
|
switch (rd->kind) {
|
|
case RD_LINE_1:
|
|
onRoad = isOnRoadLine1(rd, x, y, &col, &d, &dir_X, &dir_Y);
|
|
break;
|
|
case RD_LINE_2:
|
|
onRoad = isOnRoadLine2(rd, x, y, &col, &d, &dir_X, &dir_Y);
|
|
break;
|
|
case RD_ARC:
|
|
onRoad = isOnRoadArc(rd, x, y, &col, &d, &dir_X, &dir_Y);
|
|
break;
|
|
case RD_OTHER:
|
|
break;
|
|
}
|
|
|
|
if (onRoad && (d < min_d)) {
|
|
min_d = d;
|
|
min_rd = rid;
|
|
out->data.color = col;
|
|
out->data.dir_x = dir_X;
|
|
out->data.dir_y = dir_Y;
|
|
out->data.max_speed = map->road_arr[min_rd].max_speed;
|
|
}
|
|
}
|
|
|
|
/* Update color when a waypoint or stop. */
|
|
int tl = -1;
|
|
if (min_rd >= 0) {
|
|
Globals__color col = getColorPoint(min_rd, x, y, &tl);
|
|
if (!colors_equal(&col, &COL_OUT))
|
|
out->data.color = col;
|
|
}
|
|
|
|
/* Compute the return type. */
|
|
if (min_rd >= 0) {
|
|
log_debug("[geometry] (%.2f, %.2f) is on road %d\n", x, y, min_rd);
|
|
out->data.on_road = true;
|
|
out->data.tl_number = tl;
|
|
out->data.tl_required = isAfterStop(x, y, min_rd,
|
|
out->data.dir_x, out->data.dir_y, &tl);
|
|
if(out->data.tl_required) {
|
|
if (tl != out->data.tl_number) {
|
|
log_debug("Warning: on TL %ld != ", out->data.tl_number);
|
|
log_debug("after TL %d!\n", tl);
|
|
}
|
|
out->data.tl_number = tl;
|
|
}
|
|
}
|
|
|
|
/* Log the result. */
|
|
log_debug("[geometry] { on_road = %d; color = (%d, %d, %d);"
|
|
" dir = (%2.2f, %2.2f); tl = (%d, %d); }\n",
|
|
out->data.on_road,
|
|
out->data.color.red, out->data.color.green, out->data.color.blue,
|
|
out->data.dir_x, out->data.dir_y,
|
|
out->data.tl_number, out->data.tl_required);
|
|
}
|
|
|
|
|
|
void play_asset_wav(SDL_AudioDeviceID audio_device, asset_wav_t *wav) {
|
|
if (!audio_device) {
|
|
log_info("[sdl] no audio device\n");
|
|
return;
|
|
}
|
|
|
|
if (SDL_QueueAudio(audio_device, wav->buffer, wav->size))
|
|
log_fatal("[sdl] could not queue audio\n");
|
|
}
|
|
|
|
asset_wav_t collision, wrong_dir, exit_road, light_run, speed_excess;
|
|
SDL_AudioDeviceID audio_device = 0;
|
|
|
|
DEFINE_HEPT_FUN(Map, soundEffects, (Globals__event evt, Globals__status sta)) {
|
|
if (sta == Globals__Preparing)
|
|
return;
|
|
if (evt.exitRoad) {
|
|
play_asset_wav(audio_device, &exit_road);
|
|
log_info("[audio] car left the road\n");
|
|
} else if (evt.collisionEvent) {
|
|
play_asset_wav(audio_device, &collision);
|
|
log_info("[audio] car has collided with an obstacle\n");
|
|
} else if (evt.dirEvent) {
|
|
play_asset_wav(audio_device, &wrong_dir);
|
|
log_info("[audio] car goes in the wrong direction\n");
|
|
} else if (evt.lightRun) {
|
|
play_asset_wav(audio_device, &light_run);
|
|
log_info("[audio] car has run a red light\n");
|
|
} else if (evt.speedExcess) {
|
|
play_asset_wav(audio_device, &speed_excess);
|
|
log_info("[audio] car is going too fast\n");
|
|
}
|
|
}
|