/*  gridsim2 (c) 2006 Kris Beevers  This file is part of gridsim2.  gridsim2 is free software; you can redistribute it and/or modify it  under the terms of the GNU General Public License as published by  the Free Software Foundation; either version 2 of the License, or  (at your option) any later version.  gridsim2 is distributed in the hope that it will be useful, but  WITHOUT ANY WARRANTY; without even the implied warranty of  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU  General Public License for more details.  You should have received a copy of the GNU General Public License  along with gridsim2; if not, write to the Free Software Foundation,  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA*/#include "sim.hpp"#include <ext/hash_set>// the sensing assumes a toroidal environment, i.e., sensor beams// "wrap" from one edge of the grid to the opposite edge.  (actually,// implementing this adds some complications but it's an assumption// made in our analytical model for simplicity so we'll implement it// that way here.)double eps_scale; // computed in sim.cpp from as a function of deltabool fixed_update = false; // set in sim.cppdouble p_0 = 0.01; // set in sim.cppstruct tor_cell{  tor_cell(int32_t _x, int32_t _y) : x(_x), y(_y) {}  bool operator==(const tor_cell &c) const { return cell(x,y)==cell(c.x,c.y); }  int32_t x, y;};struct tor_cell_hash{  size_t operator()(const tor_cell &c) const { return cell(c.x,c.y); }};// toroidal 8-neighbors: the cells may not actually be on the grid// (values may be < 0 or > W/H); they will be wrapped later when// actually updating the gridvoid get_cell_neighbors8(int32_t x, int32_t y, std::vector<tor_cell> &Nm){  Nm.clear();  Nm.push_back(tor_cell(x-1,y-1));  Nm.push_back(tor_cell(x,y-1));  Nm.push_back(tor_cell(x+1,y-1));  Nm.push_back(tor_cell(x-1,y));  Nm.push_back(tor_cell(x+1,y));  Nm.push_back(tor_cell(x-1,y+1));  Nm.push_back(tor_cell(x,y+1));  Nm.push_back(tor_cell(x+1,y+1));}uint8_t sense(const sensor_t &s, uint32_t m){  if(grid[m] == F)    return (drand48() < s.e_e) ? E : F;  else    return (drand48() < s.e_f) ? F : E;}// returns range readingdouble get_beam_cells  (const pose_t &p, const sensor_t &s, double delta_b, double a, double rmax,   bool dosense, __gnu_cxx::hash_set<uint32_t> &C){  double min_occ = std::numeric_limits<double>::max();  point_t n_point;  double r;  std::vector<tor_cell> Nm;  // get the set C of cells that intersect the beam up to a given  // range and if necessary simulate false neg/pos observations of  // cells and only include cells that are not occluded  std::list<tor_cell> Q;  __gnu_cxx::hash_set<tor_cell, tor_cell_hash> been_in_Q;  tor_cell m((int32_t)((p.x-xmin)/delta), (int32_t)((p.y-ymin)/delta));  Q.push_back(m);  been_in_Q.insert(m);  C.clear();  C.insert(cell(m.x,m.y));  while(Q.size() != 0) {    m = Q.front(); Q.pop_front();    get_cell_neighbors8(m.x, m.y, Nm);    for(uint32_t i = 0; i < Nm.size(); ++i) {      const tor_cell &n = Nm[i];      if(been_in_Q.find(n) != been_in_Q.end())	continue;      n_point = cell_to_point(n.x, n.y);      r = distance(n_point, p);      if(r > rmax)	continue;      if(dosense && r > min_occ + s.sigma_r + eps_scale)	continue;      // check if cell n intersects the beam      if(r > eps_scale &&	 fabs(angular_distance(atan2(n_point.y-p.y, n_point.x-p.x), a)) > delta_b + asin(eps_scale/r))	continue;      // yep, so add it to the set      C.insert(cell(n.x,n.y));      Q.push_back(n);      been_in_Q.insert(n);      // simulate sensing if necessary      if(dosense && sense(s, cell(n.x,n.y)) == F)	if(min_occ > r) min_occ = r;    }  }  // add range error  min_occ += s.sigma_r * (2*drand48()-1);  min_occ = std::max(0.0, min_occ);  return min_occ;}void beam_update(const pose_t &p, const sensor_t &s, double a, double r){  static __gnu_cxx::hash_set<uint32_t> C, C_u; // static for speed  __gnu_cxx::hash_set<uint32_t>::iterator i;  get_beam_cells(p, s, s.delta_b, a, std::min(r,s.rmax)+s.sigma_r, false, C);  get_beam_cells(p, s, s.delta_b+s.sigma_b, a, std::min(r,s.rmax)+s.sigma_r, false, C_u);  // divide C_u into C_E and C_F, and count #cells in C_E^*  static std::vector<uint32_t> C_E, C_F; // static for speed  C_E.clear(); C_F.clear();  uint32_t C_Estar = 0;  for(i = C_u.begin(); i != C_u.end(); ++i) {    ++update_freq[*i]; // update count of number of updates of this cell    if(distance(cell_to_point(*i), p) <= std::max(0.0, r-s.sigma_r-eps_scale)) {      C_E.push_back(*i);      if(C.find(*i) != C.end()) ++C_Estar;    } else if(distance(cell_to_point(*i), p) <= r+s.sigma_r+eps_scale)      C_F.push_back(*i);  }  // do updates for all the cells  double p_f = 0.5 + (0.5-s.e_f)/C_F.size();  double p_e = 0.5 + (double(C_Estar)/double(C_E.size())) * (0.5-s.e_e);  for(uint32_t j = 0; j < C_F.size(); ++j) {    double &p = map[C_F[j]];    if(fixed_update)      p = std::min(1.0, p+p_0);    else {      p = p*p_f;      if(p > 0) p /= ((1-p)*(1-p_f) + p*p_f); // normalize    }  }  for(uint32_t j = 0; j < C_E.size(); ++j) {    double &p = map[C_E[j]];    if(fixed_update)      p = std::max(0.0, p-p_0);    else {      p = p*(1-p_e);      if(p > 0) p /= ((1-p)*p_e + p*(1-p_e)); // normalize    }  }}void beam_sense_and_update  (const pose_t &p, const sensor_t &s, double expected_a, double noisy_a){  static __gnu_cxx::hash_set<uint32_t> C; // static for speed  __gnu_cxx::hash_set<uint32_t>::iterator i;  double r = get_beam_cells(p, s, s.delta_b, noisy_a, s.rmax, true, C);  for(i = C.begin(); i != C.end(); ++i)    ++real_freq[*i]; // update count of number of actual observations of this cell  beam_update(p, s, expected_a, r);}void sense_and_update(const pose_t &p, const sensor_t &s){  if(grid[cell((uint32_t)((p.x-xmin)/delta), (uint32_t)((p.y-ymin)/delta))] == F)    return; // can't sense if the robot is in an occupied cell  double a = p.t, noisy_a;  const double da = 2*M_PI / double(s.rho);  for(uint32_t i = 0; i < s.rho; ++i, a += da) {    noisy_a = a + s.sigma_b * (2*drand48()-1); // add bearing error    beam_sense_and_update(p, s, a, noisy_a);  }}void update_without_simulation  (const pose_t &p, const sensor_t &s, const std::vector<double> &ranges,   double amin, double amax, double ares){  assert(p.x >= xmin && p.x <= xmax && p.y >= ymin && p.y <= ymax);  double a = p.t + amin;  for(uint32_t i = 0; i < s.rho; ++i, a += ares) {    if(ranges[i] < s.rmax)      beam_update(p, s, a, ranges[i]);  }}