map.cpp

/*
** map.cc
** Login : <mouret@asuncion.lip6.fr>
** Started on  Mon Jan 14 16:39:08 2008 Jean-Baptiste MOURET
** $Id$
**
** Copyright (C) 2008 Jean-Baptiste MOURET
** This program 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.
**
** This program 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 this program; if not, write to the Free Software
** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/

#include <iostream>
#include <sstream>
#include "map.hpp"

namespace fastsim {
  void Map::_read_file(const std::string& fname) {
    std::string str;
    std::ifstream ifs(fname.c_str());
    if (!ifs.good())
      throw Exception(std::string("cannot open map :") + fname);

    ifs >>str;
    if (str != "P4")
      throw Exception("wrong file type for map");
    char line[256];
    ifs.getline(line,256);
    while ((line[0]=='\0')||(line[0] == '#')) {
      ifs.getline(line,256);
    }
    std::istringstream istr(line);
    istr>> _w>>_h;
    if ((_w<=0)||(_h<=0)) {
      std::cerr<<"ERROR: the size of your map is not valid. There may be a problem with your file."<<std::endl;
      std::cerr<<" w="<<_w<<" h="<<_h<<std::endl;
      std::cerr<<" file="<<fname<<std::endl;
      exit(1);
    }
    _data.resize(_w * _h);
    if (_w % 8 != 0)
      throw Exception("wrong map size");
    int k = _w * _h / 8;
    char buffer[k];

    ifs.read((char*)buffer, k);
    for (int i = 0; i < k; ++i)
      for (int j = 0; j < 8; ++j)
        _data[i * 8 + j] = _get_bit(buffer[i + 1], j) ? obstacle : free;

  }

  // we use the "triangle method"
  // concept :
  // * area = base * height / 2
  // * area = cross product
  // -> if height < radius, intersection
  bool Map::_check_inter_ray_circle(float x1, float y1,
                                    float x2, float y2,
                                    float xr, float yr, float radius) const {
    // check if the object is on the right side of the camera
    float dot = (x2 - x1) * (xr - x1) + (y2 - y1) * (yr - y1);
    if (dot < 0)
      return false;
    float area2 = fabs((x2 - x1) * (yr - y1) -  (y2 - y1) * (xr - x1));
    float dx = x2 - x1;
    float dy = y2 - y1;
    float lab = sqrt(dx * dx + dy * dy);
    float h = area2 / lab;
    if (h < radius)
      return true;
    return false;
  }

  int Map::check_inter_is(float x1, float y1,
                          float x2, float y2) const {
    // list intersections with rays
    std::vector<ill_sw_t> res;
    for (size_t i = 0; i < _illuminated_switches.size(); ++i) {
      ill_sw_t isv = _illuminated_switches[i];
      float xtmp, ytmp;
      if (isv->get_on()
          && _check_inter_ray_circle(x1, y1, x2, y2,
                                     isv->get_x(), isv->get_y(),
                                     isv->get_radius())
          && !check_inter_real(x1, y1, isv->get_x(), isv->get_y(), xtmp, ytmp))
        res.push_back(isv);
    }
    if (res.empty())
      return -1;
    // return the closest
    std::sort(res.begin(), res.end(), ClosestSwitch_f(x1, y1));
    return res[0]->get_color();
  }


  bool Map :: _try_pixel(int x, int y) const {
    if (x >= 0 && y >= 0
        && x < get_pixel_w()
        && y < get_pixel_h()
        && get_pixel(x, y) == free)
      return false;
    else
      return true;
  }



  // see
  // http://lifc.univ-fcomte.fr/~dedu/projects/bresenham/index.html
  // In PIXEL coordinates
  bool Map :: check_inter_pixel(int y1, int x1,
                                int y2, int x2,
                                int& y_res, int& x_res) const {
    int i;               // loop counter
    int ystep, xstep;    // the step on y and x axis
    int error;           // the error accumulated during the increment
    int errorprev;       // *vision the previous value of the error variable
    int y = y1, x = x1;  // the line points
    int ddy, ddx;        // compulsory variables: the double values of dy and dx
    int dx = x2 - x1;
    int dy = y2 - y1;
    bool inter = _try_pixel(y1, x1);
    if (dy < 0) {
      ystep = -1;
      dy = -dy;
    } else ystep = 1;
    if (dx < 0) {
      xstep = -1;
      dx = -dx;
    } else xstep = 1;
    ddy = dy * 2;
    ddx = dx * 2;
    if (ddx >= ddy) { // first octant (0 <= slope <= 1)
      errorprev = error = dx;
      for (i = 0 ; i < dx ; i++) {
        // do not use the first point (already done)
        x += xstep;
        error += ddy;
        if (error > ddx) {
          y += ystep;
          error -= ddx;
          if (error + errorprev < ddx)  // bottom square also
            inter = inter || _try_pixel(y - ystep, x);
          else if (error + errorprev > ddx)  // left square also
            inter = inter || _try_pixel(y, x - xstep);
          else { // corner: bottom and left squares also
            inter = inter || _try_pixel(y - ystep, x);
            inter = inter || _try_pixel(y, x - xstep);
          }
        }
        inter = inter || _try_pixel(y, x);
        errorprev = error;
        if (inter) {
          x_res = x;
          y_res = y;
          return true;
        }
      }
    } else {
      // the same as above
      errorprev = error = dy;
      for (i = 0 ; i < dy ; i++) {
        y += ystep;
        error += ddx;
        if (error > ddy) {
          x += xstep;
          error -= ddy;
          if (error + errorprev < ddy)
            inter = inter || _try_pixel(y, x - xstep);
          else if (error + errorprev > ddy)
            inter = inter || _try_pixel(y - ystep, x);
          else {
            inter = inter || _try_pixel(y, x - xstep);
            inter = inter || _try_pixel(y -  ystep, x);
          }
        }
        inter = inter || _try_pixel(y, x);
        errorprev = error;
        if (inter) {
          x_res = x;
          y_res = y;
          return true;
        }
      }
    }
    return false;
  }

  // Draws a rectangle with (x,y) the upper left point and (lx,ly) the size
  void Map:: draw_rect(int x, int y, int lx, int ly) {
    int i,j;

    for (i=0; i<lx; i++)
      for (j=0; j<ly; j++) {
        if ((x+i) >= 0 && (y+j) >= 0
            && (x+i) < get_pixel_w()
            && (y+j) < get_pixel_h())
          set_pixel(x+i,y+j,obstacle);
      }
  }

}