Ticket #702: osd_core.c.speedcam.diff

File osd_core.c.speedcam.diff, 24.1 KB (added by tegzed, 9 years ago)

add support for cameras in navit binfile maps

  • osd_core.c

     
    4949#include "osd.h"
    5050#include "speech.h"
    5151#include "event.h"
     52#include "mapset.h"
    5253
     54//prototypes
    5355struct odometer;
    5456
    5557static void osd_odometer_reset(struct odometer *this);
    5658static void osd_cmd_odometer_reset(struct navit *this, char *function, struct attr **in, struct attr ***out, int *valid);
    5759static void osd_odometer_draw(struct odometer *this, struct navit *nav, struct vehicle *v);
     60struct quadtree_node* quadtree_node_new(struct quadtree_node* parent, double xmin, double xmax, double ymin, double ymax );
     61struct quadtree_item* quadtree_find_nearest_flood(struct quadtree_node* this_, struct quadtree_item* item, double current_max, struct quadtree_node* toSkip);
     62struct quadtree_item* quadtree_find_nearest(struct quadtree_node* this_, struct quadtree_item* item);
     63void quadtree_split(struct quadtree_node* this_);
     64void quadtree_add(struct quadtree_node* this_, struct quadtree_item* item);
     65void quadtree_destroy(struct quadtree_node* this_);
     66struct quadtree_node* osd_speed_cam_load(char* fn);
    5867
    5968struct compass {
    6069        struct osd_item osd_item;
     
    12391248        return (struct osd_priv *) this;
    12401249}
    12411250
     1251enum camera_t {CAM_FIXED=1, CAM_TRAFFIC_LAMP, CAM_RED, CAM_SECTION, CAM_MOBILE, CAM_RAIL, CAM_TRAFFIPAX};
     1252char*camera_t_strs[] = {"None","Fix","Traffic lamp","Red detect","Section","Mobile","Rail","Traffipax(non persistent)"};
     1253char*camdir_t_strs[] = {"All dir.","UNI-dir","BI-dir"};
     1254enum cam_dir_t {CAMDIR_ALL=0, CAMDIR_ONE, CAMDIR_TWO};
     1255
     1256struct quadtree_item {
     1257    double longitude;
     1258    double latitude;
     1259    void*  data;
     1260};
     1261
     1262#define QUADTREE_NODE_CAPACITY 10
     1263
     1264struct quadtree_node {
     1265    int node_num;
     1266    struct quadtree_item items[QUADTREE_NODE_CAPACITY];
     1267    struct quadtree_node *aa;
     1268    struct quadtree_node *ab;
     1269    struct quadtree_node *ba;
     1270    struct quadtree_node *bb;
     1271    double xmin, xmax, ymin, ymax;
     1272    int is_leaf;
     1273    struct quadtree_node*parent;
     1274};
     1275
     1276struct quadtree_node*
     1277quadtree_node_new(struct quadtree_node* parent, double xmin, double xmax, double ymin, double ymax ) {
     1278    struct quadtree_node*ret = calloc(1, sizeof(struct quadtree_node));
     1279    ret->xmin = xmin;
     1280    ret->xmax = xmax;
     1281    ret->ymin = ymin;
     1282    ret->ymax = ymax;
     1283    ret->is_leaf = 1;
     1284    ret->parent = parent;
     1285    return ret;
     1286}
     1287
     1288#define MAX_DOUBLE 9999999
     1289
     1290static double
     1291dist_sq(double x1,double y1,double x2,double y2)
     1292{
     1293  return (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1);
     1294}
     1295
     1296/*
     1297 * searches all four subnodes recursively for the closest item
     1298 */
     1299struct quadtree_item*
     1300quadtree_find_nearest_flood(struct quadtree_node* this_, struct quadtree_item* item, double current_max, struct quadtree_node* toSkip) {
     1301    struct quadtree_node* nodes[4] = { this_->aa, this_->ab, this_->ba, this_->bb };
     1302    struct quadtree_item*res = NULL;
     1303    if( this_->is_leaf ) {
     1304        int i;
     1305        double distance_sq = current_max;
     1306        for(i=0;i<this_->node_num;++i) {
     1307            double curr_dist_sq = dist_sq(item->longitude,item->latitude,this_->items[i].longitude,this_->items[i].latitude);
     1308            if(curr_dist_sq<distance_sq) {
     1309                distance_sq = curr_dist_sq;
     1310                res = &this_->items[i];
     1311            }
     1312        }
     1313    }
     1314    else {
     1315      int i;
     1316      for( i=0;i<4;++i) {
     1317        if(nodes[i] && nodes[i]!=toSkip) {
     1318          //limit flooding
     1319          struct quadtree_item*res_tmp = NULL;
     1320          if(
     1321             dist_sq(nodes[i]->xmin,nodes[i]->ymin,item->longitude,item->latitude)<current_max ||
     1322             dist_sq(nodes[i]->xmax,nodes[i]->ymin,item->longitude,item->latitude)<current_max ||
     1323             dist_sq(nodes[i]->xmax,nodes[i]->ymax,item->longitude,item->latitude)<current_max ||
     1324             dist_sq(nodes[i]->xmin,nodes[i]->ymax,item->longitude,item->latitude)<current_max
     1325            ) {
     1326            res_tmp = quadtree_find_nearest_flood(nodes[i],item,current_max,NULL);
     1327          }
     1328          if(res_tmp) {
     1329            res = res_tmp;
     1330            double curr_dist_sq = dist_sq(item->longitude,item->latitude,res->longitude,res->latitude);
     1331            if(curr_dist_sq<current_max) {
     1332                current_max = curr_dist_sq;
     1333            }
     1334          }
     1335        }
     1336      }
     1337    }
     1338  return res;
     1339}
     1340
     1341/*
     1342 * tries to find closest item, first it descend into the quadtree as much as possible, then if no point is found
     1343 */
     1344struct quadtree_item*
     1345quadtree_find_nearest(struct quadtree_node* this_, struct quadtree_item* item) {
     1346    if( ! this_ ) {
     1347      return NULL;
     1348    }
     1349    struct quadtree_item*res = NULL;
     1350    double distance_sq = MAX_DOUBLE;
     1351    if( this_->is_leaf ) {
     1352        int i;
     1353        for(i=0;i<this_->node_num;++i) {
     1354            double curr_dist_sq = dist_sq(item->longitude,item->latitude,this_->items[i].longitude,this_->items[i].latitude);
     1355            if(curr_dist_sq<distance_sq) {
     1356                distance_sq = curr_dist_sq;
     1357                res = &this_->items[i];
     1358            }
     1359        }
     1360      //go up n levels
     1361          if(!res && this_->parent) {
     1362                  struct quadtree_node* anchestor = this_->parent;
     1363                  int cnt = 0;
     1364                  while (anchestor->parent && cnt<4) {
     1365                    anchestor = anchestor->parent;
     1366                    ++cnt;
     1367                  }
     1368                struct quadtree_item*res2 = NULL;
     1369                res2 = quadtree_find_nearest_flood(anchestor,item,distance_sq,NULL);
     1370                if(res2) {
     1371                  res = res2;
     1372                }
     1373          }
     1374    }
     1375    else {
     1376        if(
     1377           this_->aa &&
     1378           this_->aa->xmin<=item->longitude && item->longitude<this_->aa->xmax &&
     1379           this_->aa->ymin<=item->latitude && item->latitude<this_->aa->ymax
     1380           ) {
     1381          res = quadtree_find_nearest(this_->aa,item);
     1382        }
     1383        else if(
     1384           this_->ab &&
     1385           this_->ab->xmin<=item->longitude && item->longitude<this_->ab->xmax &&
     1386           this_->ab->ymin<=item->latitude && item->latitude<this_->ab->ymax
     1387           ) {
     1388          res = quadtree_find_nearest(this_->ab,item);
     1389        }
     1390        else if(
     1391           this_->ba &&
     1392           this_->ba->xmin<=item->longitude && item->longitude<this_->ba->xmax &&
     1393           this_->ba->ymin<=item->latitude && item->latitude<this_->ba->ymax
     1394           ) {
     1395          res = quadtree_find_nearest(this_->ba,item);
     1396        }
     1397        else if(
     1398           this_->bb &&
     1399           this_->bb->xmin<=item->longitude && item->longitude<this_->bb->xmax &&
     1400           this_->bb->ymin<=item->latitude && item->latitude<this_->bb->ymax
     1401           ) {
     1402          res = quadtree_find_nearest(this_->bb,item);
     1403        }
     1404        else {
     1405        if(this_->parent) {
     1406            //go up two levels
     1407            struct quadtree_node* anchestor = this_->parent;
     1408            int cnt = 0;
     1409            while (anchestor->parent && cnt<4) {
     1410              anchestor = anchestor->parent;
     1411              ++cnt;
     1412            }
     1413        res = quadtree_find_nearest_flood(anchestor,item,distance_sq,NULL);
     1414        }
     1415      }
     1416    }
     1417  return res;
     1418}
     1419
     1420void
     1421quadtree_add(struct quadtree_node* this_, struct quadtree_item* item) {
     1422    if( this_->is_leaf ) {
     1423        this_->items[this_->node_num++] = *item;
     1424        if(QUADTREE_NODE_CAPACITY == this_->node_num) {
     1425            quadtree_split(this_);
     1426        }
     1427    }
     1428    else {
     1429        if(
     1430           this_->xmin<=item->longitude && item->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
     1431           this_->ymin<=item->latitude && item->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
     1432           ) {
     1433            if(!this_->aa) {
     1434              this_->aa = quadtree_node_new( this_, this_->xmin, this_->xmin+(this_->xmax-this_->xmin)/2.0 , this_->ymin, this_->ymin+(this_->ymax-this_->ymin)/2.0 );
     1435            }
     1436          quadtree_add(this_->aa,item);
     1437        }
     1438        else if(
     1439           this_->xmin+(this_->xmax-this_->xmin)/2.0<=item->longitude && item->longitude<this_->xmax &&
     1440           this_->ymin<=item->latitude && item->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
     1441           ) {
     1442            if(!this_->ab) {
     1443              this_->ab = quadtree_node_new( this_, this_->xmin+(this_->xmax-this_->xmin)/2.0, this_->xmax , this_->ymin, this_->ymin+(this_->ymax-this_->ymin)/2.0 );
     1444            }
     1445          quadtree_add(this_->ab,item);
     1446        }
     1447        else if(
     1448           this_->xmin<=item->longitude && item->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
     1449           this_->ymin+(this_->ymax-this_->ymin)/2.0<=item->latitude && item->latitude<this_->ymax
     1450           ) {
     1451            if(!this_->ba) {
     1452              this_->ba = quadtree_node_new( this_, this_->xmin, this_->xmin+(this_->xmax-this_->xmin)/2.0 , this_->ymin+(this_->ymax-this_->ymin)/2.0 , this_->ymax);
     1453            }
     1454          quadtree_add(this_->ba,item);
     1455        }
     1456        else if(
     1457           this_->xmin+(this_->xmax-this_->xmin)/2.0<=item->longitude && item->longitude<this_->xmax &&
     1458           this_->ymin+(this_->ymax-this_->ymin)/2.0<=item->latitude && item->latitude<this_->ymax
     1459           ) {
     1460            if(!this_->bb) {
     1461              this_->bb = quadtree_node_new( this_, this_->xmin+(this_->xmax-this_->xmin)/2.0, this_->xmax , this_->ymin+(this_->ymax-this_->ymin)/2.0 , this_->ymax);
     1462            }
     1463          quadtree_add(this_->bb,item);
     1464        }
     1465    }
     1466}
     1467
     1468void
     1469quadtree_split(struct quadtree_node* this_) {
     1470    int i;
     1471    this_->is_leaf = 0;
     1472    for(i=0;i<this_->node_num;++i) {
     1473        if(
     1474           this_->xmin<=this_->items[i].longitude && this_->items[i].longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
     1475           this_->ymin<=this_->items[i].latitude && this_->items[i].latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
     1476           ) {
     1477          if(!this_->aa) {
     1478            this_->aa = quadtree_node_new( this_, this_->xmin, this_->xmin+(this_->xmax-this_->xmin)/2.0 , this_->ymin, this_->ymin+(this_->ymax-this_->ymin)/2.0 );
     1479          }
     1480          quadtree_add(this_->aa,&this_->items[i]);
     1481        }
     1482        else if(
     1483           this_->xmin+(this_->xmax-this_->xmin)/2.0<=this_->items[i].longitude && this_->items[i].longitude<this_->xmax &&
     1484           this_->ymin<=this_->items[i].latitude && this_->items[i].latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
     1485           ) {
     1486          if(!this_->ab) {
     1487            this_->ab = quadtree_node_new( this_, this_->xmin+(this_->xmax-this_->xmin)/2.0, this_->xmax , this_->ymin, this_->ymin+(this_->ymax-this_->ymin)/2.0 );
     1488          }
     1489          quadtree_add(this_->ab,&this_->items[i]);
     1490        }
     1491        else if(
     1492           this_->xmin<=this_->items[i].longitude && this_->items[i].longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
     1493           this_->ymin+(this_->ymax-this_->ymin)/2.0<=this_->items[i].latitude && this_->items[i].latitude<this_->ymax
     1494           ) {
     1495          if(!this_->ba) {
     1496            this_->ba = quadtree_node_new( this_, this_->xmin, this_->xmin+(this_->xmax-this_->xmin)/2.0 , this_->ymin+(this_->ymax-this_->ymin)/2.0 , this_->ymax);
     1497          }
     1498          quadtree_add(this_->ba,&this_->items[i]);
     1499        }
     1500        else if(
     1501           this_->xmin+(this_->xmax-this_->xmin)/2.0<=this_->items[i].longitude && this_->items[i].longitude<this_->xmax &&
     1502           this_->ymin+(this_->ymax-this_->ymin)/2.0<=this_->items[i].latitude && this_->items[i].latitude<this_->ymax
     1503           ) {
     1504          if(!this_->bb) {
     1505            this_->bb = quadtree_node_new( this_, this_->xmin+(this_->xmax-this_->xmin)/2.0, this_->xmax , this_->ymin+(this_->ymax-this_->ymin)/2.0 , this_->ymax);
     1506          }
     1507          quadtree_add(this_->bb,&this_->items[i]);
     1508        }
     1509    }
     1510    this_->node_num = 0;
     1511}
     1512
     1513void
     1514quadtree_destroy(struct quadtree_node* this_) {
     1515    if(this_->aa) {
     1516        quadtree_destroy(this_->aa);
     1517    }
     1518    if(this_->ab) {
     1519        quadtree_destroy(this_->ab);
     1520    }
     1521    if(this_->ba) {
     1522        quadtree_destroy(this_->ba);
     1523    }
     1524    if(this_->bb) {
     1525        quadtree_destroy(this_->bb);
     1526    }
     1527    free(this_);
     1528}
     1529
     1530
     1531
     1532struct osd_speed_cam_entry {
     1533        double lon;
     1534        double lat;
     1535        enum camera_t cam_type;
     1536        int speed_limit;
     1537        enum cam_dir_t cam_dir;
     1538        int direction;
     1539};
     1540
     1541enum eAnnounceState {eNoWarn=0,eWarningTold=1};
     1542
     1543struct osd_speed_cam {
     1544  struct osd_item item;
     1545  int width;
     1546  struct graphics_gc *white,*orange;
     1547  struct graphics_gc *red;
     1548  struct color idle_color;
     1549
     1550  struct quadtree_node*root_node;
     1551  int announce_on;
     1552  enum eAnnounceState announce_state;
     1553  char *text;                 //text of label attribute for this osd
     1554};
     1555
     1556static double
     1557angle_diff(firstAngle,secondAngle)
     1558{
     1559        double difference = secondAngle - firstAngle;
     1560        while (difference < -180) difference += 360;
     1561        while (difference > 180) difference -= 360;
     1562        return difference;
     1563}
     1564
     1565static void
     1566osd_speed_cam_draw(struct osd_speed_cam *this_, struct navit *navit, struct vehicle *v)
     1567{
     1568  struct attr position_attr,vehicle_attr;
     1569  struct point p, bbox[4];
     1570  struct attr speed_attr;
     1571  struct vehicle* curr_vehicle = v;
     1572  struct coord curr_coord;
     1573  struct coord cam_coord;
     1574  if(navit) {
     1575    navit_get_attr(navit, attr_vehicle, &vehicle_attr, NULL);
     1576  }
     1577  else {
     1578    return;
     1579  }
     1580  if (vehicle_attr.u.vehicle) {
     1581    curr_vehicle = vehicle_attr.u.vehicle;
     1582  }
     1583
     1584  if(0==curr_vehicle)
     1585    return;
     1586
     1587  int ret_attr = 0;
     1588  osd_std_draw(&this_->item);
     1589
     1590  ret_attr = vehicle_get_attr(curr_vehicle, attr_position_coord_geo,&position_attr, NULL);
     1591  if(0==ret_attr) {
     1592    return;
     1593  }
     1594  transform_from_geo(projection_mg, position_attr.u.coord_geo, &curr_coord);
     1595
     1596  double dCurrDist = -1;
     1597  int dir_idx = -1;
     1598  int dir = -1;
     1599  int spd = -1;
     1600  int idx = -1;
     1601  double speed = -1;
     1602
     1603  if (this_->root_node) {
     1604    struct quadtree_item qi;
     1605    qi.longitude = position_attr.u.coord_geo->lng;
     1606    qi.latitude = position_attr.u.coord_geo->lat;
     1607    struct quadtree_item*qi_res = quadtree_find_nearest(this_->root_node,&qi);
     1608    struct coord_geo coord_geo_cam;
     1609    if(qi_res) {  //found nearest camera
     1610      coord_geo_cam.lng = qi_res->longitude;
     1611      coord_geo_cam.lat = qi_res->latitude;
     1612      transform_from_geo(projection_mg, &coord_geo_cam, &cam_coord);
     1613      dCurrDist = transform_distance(projection_mg, &curr_coord, &cam_coord);
     1614      idx = ((struct osd_speed_cam_entry*)qi_res->data)->cam_type;
     1615      if(idx>CAM_TRAFFIPAX) {
     1616        return;
     1617      }
     1618      dir_idx = ((struct osd_speed_cam_entry*)qi_res->data)->cam_dir;
     1619      dir = ((struct osd_speed_cam_entry*)qi_res->data)->direction;
     1620      spd = ((struct osd_speed_cam_entry*)qi_res->data)->speed_limit;
     1621    }
     1622  }
     1623  else if(navit_get_mapset(navit)) {
     1624
     1625  int dst=2000;
     1626  int dstsq=dst*dst;
     1627  struct map_selection sel;
     1628  struct map_rect *mr;
     1629  struct mapset_handle *msh;
     1630  struct map *map;
     1631  struct item *item;
     1632
     1633  sel.next=NULL;
     1634  sel.order=18;
     1635  sel.range.min=type_tec_common;
     1636  sel.range.max=type_tec_common;
     1637  sel.u.c_rect.lu.x=curr_coord.x-dst;
     1638  sel.u.c_rect.lu.y=curr_coord.y+dst;
     1639  sel.u.c_rect.rl.x=curr_coord.x+dst;
     1640  sel.u.c_rect.rl.y=curr_coord.y-dst;
     1641 
     1642  msh=mapset_open(navit_get_mapset(navit));
     1643  while ((map=mapset_next(msh, 1))) {
     1644    mr=map_rect_new(map, &sel);
     1645    if (!mr)
     1646      continue;
     1647    while ((item=map_rect_get_item(mr))) {
     1648      struct coord cn;
     1649      if (item->type == type_tec_common && item_coord_get(item, &cn, 1)) {
     1650        int dist=transform_distance_sq(&cn, &curr_coord);
     1651        if (dist < dstsq) { 
     1652          dstsq=dist;
     1653          dCurrDist = sqrt(dist);
     1654          cam_coord = cn;
     1655          struct attr tec_attr;
     1656          idx = -1;
     1657          if(item_attr_get(item,attr_tec_type,&tec_attr)) {
     1658            idx = tec_attr.u.num;
     1659          }
     1660          dir_idx = -1;
     1661          if(item_attr_get(item,attr_tec_dirtype,&tec_attr)) {
     1662            dir_idx = tec_attr.u.num;
     1663          }
     1664          dir= 0;
     1665          if(item_attr_get(item,attr_tec_direction,&tec_attr)) {
     1666            dir = tec_attr.u.num;
     1667          }
     1668          spd= 0;
     1669          if(item_attr_get(item,attr_maxspeed,&tec_attr)) {
     1670            spd = tec_attr.u.num;
     1671          }
     1672        }
     1673      }
     1674    }
     1675    map_rect_destroy(mr);
     1676  }
     1677  mapset_close(msh);
     1678
     1679    dCurrDist = transform_distance(projection_mg, &curr_coord, &cam_coord);
     1680
     1681  }
     1682
     1683  dCurrDist = transform_distance(projection_mg, &curr_coord, &cam_coord);
     1684  ret_attr = vehicle_get_attr(curr_vehicle,attr_position_speed,&speed_attr, NULL);
     1685  if(0==ret_attr) {
     1686    return;
     1687  }
     1688  speed = *speed_attr.u.numd;
     1689  if(dCurrDist <= speed*750.0/130.0) {  //at speed 130 distance limit is 750m
     1690    if(this_->announce_state==eNoWarn && this_->announce_on) {
     1691      this_->announce_state=eWarningTold; //warning told
     1692      navit_say(navit, _("Look out! Camera!"));
     1693    }
     1694  }
     1695  else {
     1696    this_->announce_state=eNoWarn;
     1697  }
     1698
     1699  char buffer [64+1]="";
     1700  char buffer2[64+1]="";
     1701  buffer [0] = 0;
     1702  buffer2[0] = 0;
     1703  if(this_->text) {
     1704    str_replace(buffer,this_->text,"${distance}",format_distance(dCurrDist,""));
     1705    str_replace(buffer2,buffer,"${camera_type}",camera_t_strs[idx]);
     1706    str_replace(buffer,buffer2,"${camera_dir}",camdir_t_strs[dir_idx]);
     1707    char dir_str[16];
     1708    char spd_str[16];
     1709    sprintf(dir_str,"%d",dir);
     1710    sprintf(spd_str,"%d",spd);
     1711    str_replace(buffer2,buffer,"${direction}",dir_str);
     1712    str_replace(buffer,buffer2,"${speed_limit}",spd_str);
     1713
     1714  graphics_get_text_bbox(this_->item.gr, this_->item.font, buffer, 0x10000, 0, bbox, 0);
     1715  p.x=(this_->item.w-bbox[2].x)/2;
     1716  p.y = this_->item.h-this_->item.h/10;
     1717  struct graphics_gc *curr_color = this_->orange;
     1718  struct attr attr_dir;
     1719  //tolerance is +-20 degrees
     1720  if(
     1721    dir_idx==CAMDIR_ONE &&
     1722    dCurrDist <= speed*750.0/130.0 &&
     1723    vehicle_get_attr(v, attr_position_direction, &attr_dir, NULL) &&
     1724    fabs(angle_diff(dir,*attr_dir.u.numd))<=20 ) {
     1725      curr_color = this_->red;
     1726  }
     1727  //tolerance is +-20 degrees in both directions
     1728  else if(
     1729    dir_idx==CAMDIR_TWO &&
     1730    dCurrDist <= speed*750.0/130.0 &&
     1731    vehicle_get_attr(v, attr_position_direction, &attr_dir, NULL) &&
     1732    (fabs(angle_diff(dir,*attr_dir.u.numd))<=20 || fabs(angle_diff(dir+180,*attr_dir.u.numd))<=20 )) {
     1733      curr_color = this_->red;
     1734  }
     1735        else if(dCurrDist <= speed*750.0/130.0) {
     1736      curr_color = this_->red;
     1737        }
     1738  graphics_draw_text(this_->item.gr, curr_color, NULL, this_->item.font, buffer, &p, 0x10000, 0);
     1739    }
     1740  graphics_draw_mode(this_->item.gr, draw_mode_end);
     1741}
     1742
     1743static void
     1744osd_speed_cam_init(struct osd_speed_cam *this, struct navit *nav)
     1745{
     1746  osd_set_std_graphic(nav, &this->item, (struct osd_priv *)this);
     1747
     1748  this->red = graphics_gc_new(this->item.gr);
     1749  graphics_gc_set_foreground(this->red, &(struct color) {0xffff,0x0000,0x0000,0xffff});
     1750  graphics_gc_set_linewidth(this->red, this->width);
     1751
     1752  this->orange = graphics_gc_new(this->item.gr);
     1753  graphics_gc_set_foreground(this->orange, &this->idle_color);
     1754  graphics_gc_set_linewidth(this->orange, this->width);
     1755
     1756  this->white = graphics_gc_new(this->item.gr);
     1757  graphics_gc_set_foreground(this->white, &this->item.text_color);
     1758  graphics_gc_set_linewidth(this->white, this->width);
     1759
     1760
     1761  graphics_gc_set_linewidth(this->item.graphic_fg_white, this->width);
     1762
     1763  navit_add_callback(nav, callback_new_attr_1(callback_cast(osd_speed_cam_draw), attr_position_coord_geo, this));
     1764
     1765}
     1766
     1767struct quadtree_node*
     1768osd_speed_cam_load(char* fn)
     1769{
     1770    FILE*fp;
     1771    if((fp=fopen(fn,"rt"))) {
     1772        char  line[128];
     1773        char* line2;
     1774        char* lat_str;
     1775        char* lon_str;
     1776        char* type_str;
     1777        char* spd_str;
     1778        char* dirtype_str;
     1779        char* direction_str;
     1780        struct quadtree_node*root_node = quadtree_node_new(NULL,-90,90,-90,90);
     1781        /*skip header lines*/
     1782        fgets(line,127,fp);
     1783
     1784        while(fgets(line,127,fp)) {
     1785          if(!strcmp(line,"")) {
     1786          continue;
     1787        }
     1788            line2 = g_strdup(line);
     1789            /*count tokens*/
     1790            int tok_cnt = 1;
     1791            if( strtok(line2,",")) {
     1792              while( strtok(NULL,",")) {
     1793                ++tok_cnt;
     1794              }
     1795            }
     1796            g_free(line2);
     1797            if (7==tok_cnt) {
     1798              strtok(line,","); //skip idx
     1799              if(lon_str) {
     1800              }
     1801              else {
     1802                  continue;
     1803              }
     1804              lon_str = strtok(NULL,",");
     1805              if(lon_str) {
     1806              }
     1807              else {
     1808                  continue;
     1809              }
     1810            }
     1811            else if (6==tok_cnt) {
     1812              lon_str = strtok(line,",");
     1813              if(lon_str) {
     1814              }
     1815              else {
     1816                  continue;
     1817              }
     1818            }
     1819            else { //illegal format
     1820              continue;
     1821            }
     1822
     1823            lat_str = strtok(NULL,",");
     1824            if(lat_str) {
     1825            }
     1826            else {
     1827                continue;
     1828            }
     1829
     1830            type_str = strtok(NULL,",");
     1831            if(type_str) {
     1832            }
     1833            else {
     1834                continue;
     1835            }
     1836
     1837            spd_str = strtok(NULL,",");
     1838            if(spd_str) {
     1839            }
     1840            else {
     1841                continue;
     1842            }
     1843            dirtype_str = strtok(NULL,",");
     1844            if(dirtype_str) {
     1845            }
     1846            else {
     1847                continue;
     1848            }
     1849            direction_str = strtok(NULL,",");
     1850            if(direction_str) {
     1851            }
     1852            else {
     1853                continue;
     1854            }
     1855
     1856            struct quadtree_item item;
     1857            item.longitude = atof(lon_str);
     1858            item.latitude  = atof(lat_str);
     1859            if(item.longitude==0 && item.latitude==0) {
     1860              continue;
     1861            }
     1862            struct osd_speed_cam_entry*sc_entry = malloc(sizeof(struct osd_speed_cam_entry));
     1863            sc_entry->lon         = item.longitude;
     1864            sc_entry->lat         = item.latitude;
     1865            sc_entry->cam_type    = atoi(type_str);
     1866            sc_entry->speed_limit = atoi(spd_str);
     1867            sc_entry->cam_dir     = atoi(dirtype_str);
     1868            sc_entry->direction   = atoi(direction_str);
     1869            item.data = sc_entry;
     1870            if(sc_entry->lon != 0) {
     1871              quadtree_add(root_node,&item);
     1872            }
     1873        }
     1874        return root_node;
     1875    }
     1876    else {
     1877        return NULL;
     1878    }
     1879  return NULL;
     1880}
     1881
     1882
     1883static struct osd_priv *
     1884osd_speed_cam_new(struct navit *nav, struct osd_methods *meth, struct attr **attrs)
     1885{
     1886  struct osd_speed_cam *this = g_new0(struct osd_speed_cam, 1);
     1887  struct attr *attr;
     1888  this->item.p.x = 120;
     1889  this->item.p.y = 20;
     1890  this->item.w = 60;
     1891  this->item.h = 80;
     1892  this->item.navit = nav;
     1893  this->item.font_size = 200;
     1894  this->item.meth.draw = osd_draw_cast(osd_speed_cam_draw);
     1895
     1896  osd_set_std_attr(attrs, &this->item, 2);
     1897  attr = attr_search(attrs, NULL, attr_width);
     1898  this->width=attr ? attr->u.num : 2;
     1899  attr = attr_search(attrs, NULL, attr_idle_color);
     1900  this->idle_color=attr ? *attr->u.color : ((struct color) {0xffff,0xa5a5,0x0000,0xffff}); // text idle_color defaults to orange
     1901
     1902  attr = attr_search(attrs, NULL, attr_label);
     1903  //FIXME find some way to free text!!!!
     1904  if (attr) {
     1905    this->text = g_strdup(attr->u.str);
     1906  }
     1907  else
     1908    this->text = NULL;
     1909
     1910  attr = attr_search(attrs, NULL, attr_path);
     1911  char*fn;
     1912  if (attr) {
     1913    fn = attr->u.str;
     1914  }
     1915  else {
     1916    fn = g_strdup_printf("%s/speedcam.txt",navit_get_user_data_directory(TRUE));
     1917  }
     1918  this->root_node = osd_speed_cam_load(fn);
     1919
     1920  attr = attr_search(attrs, NULL, attr_announce_on);
     1921  if (attr) {
     1922    this->announce_on = attr->u.num;
     1923  }
     1924  else {
     1925    this->announce_on = 1;    //announce by default
     1926  }
     1927
     1928  navit_add_callback(nav, callback_new_attr_1(callback_cast(osd_speed_cam_init), attr_graphics_ready, this));
     1929  return (struct osd_priv *) this;
     1930}
    12421931struct osd_speed_warner {
    12431932        struct osd_item item;
    12441933        struct graphics_gc *red;
     
    12521941        double speed_exceed_limit_offset;
    12531942        double speed_exceed_limit_percent;
    12541943        int announce_on;
    1255         enum eAnnounceState {eNoWarn=0,eWarningTold=1};
    12561944        enum eAnnounceState announce_state;
    12571945        int bTextOnly;
    12581946};
     
    24843172        plugin_register_osd_type("button", osd_button_new);
    24853173        plugin_register_osd_type("toggle_announcer", osd_nav_toggle_announcer_new);
    24863174        plugin_register_osd_type("speed_warner", osd_speed_warner_new);
     3175        plugin_register_osd_type("speed_cam", osd_speed_cam_new);
    24873176        plugin_register_osd_type("text", osd_text_new);
    24883177        plugin_register_osd_type("gps_status", osd_gps_status_new);
    24893178        plugin_register_osd_type("volume", osd_volume_new);