display.cpp

/*
** sdl.cc
** Login : <mouret@mexico>
** Started on  Sat Jan 12 20:40:12 2008 Jeanbaptiste MOURET
** $Id$
**
** Copyright (C) 2008 Jeanbaptiste 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 "display.hpp"
#include <bitset>

#ifdef USE_SDL
namespace fastsim {

  void Display :: _events() {
    SDL_Event event;
    while (SDL_PollEvent(&event)) {
      switch (event.type) {
      case SDL_QUIT:
        _quit();
      case SDL_KEYDOWN:
        if (event.key.keysym.sym == SDLK_ESCAPE)
          _quit();
        break;
      }
    }
  }

  void Display :: _blit_map() {
    for (unsigned i = 0; i < _map->get_pixel_w(); ++i)
      for (unsigned j = 0; j < _map->get_pixel_h(); ++j)
        if (_map->get_pixel(i, j) == Map::obstacle)
          _put_pixel(_map_bmp, i, j, 0, 0, 0);
        else
          _put_pixel(_map_bmp, i, j, 255, 255, 255);
    SDL_BlitSurface(_map_bmp, 0, _screen, 0);
    SDL_UpdateRect(_screen, 0, 0, _w, _h);
  }

  Display :: Display(const boost::shared_ptr<Map>& m, const Robot& r) :
    _map(m), _robot(r) {
    _w = _map->get_pixel_w();
    _h = _map->get_pixel_h();
    if (SDL_Init(SDL_INIT_VIDEO) == -1)
      throw Exception(SDL_GetError());
    _screen = SDL_SetVideoMode(_w, _h,
                               32, SDL_SWSURFACE);
    if (!_screen)
      throw Exception(SDL_GetError());

    Uint32 rmask, gmask, bmask, amask;
#if SDL_BYTEORDER == SDL_BIG_ENDIAN
    rmask = 0xff000000;
    gmask = 0x00ff0000;
    bmask = 0x0000ff00;
    amask = 0x000000ff;
#else
    rmask = 0x000000ff;
    gmask = 0x0000ff00;
    bmask = 0x00ff0000;
    amask = 0xff000000;
#endif

    _map_bmp = SDL_CreateRGBSurface(SDL_SWSURFACE, _w, _h, 32,
                                    rmask, gmask, bmask, amask);
    _blit_map();
    _bb_to_sdl(_robot.get_bb(), &_prev_bb);
  }

  void Display :: _line(SDL_Surface* surf,
                        int x0, int y0, int x1, int y1,
                        Uint32 color) {
    x0 = std::max(x0, 0);
    x0 = std::min(x0, surf->w - 1);
    x1 = std::max(x1, 0);
    x1 = std::min(x1, surf->w - 1);
    y0 = std::max(y0, 0);
    y0 = std::min(y0, surf->h - 1);
    y1 = std::max(y1, 0);
    y1 = std::min(y1, surf->h - 1);
    int dy = y1 - y0;
    int dx = x1 - x0;
    int stepx, stepy;

    if (dy < 0) {
      dy = -dy;
      stepy = -1;
    } else {
      stepy = 1;
    }
    if (dx < 0) {
      dx = -dx;
      stepx = -1;
    } else {
      stepx = 1;
    }
    dy <<= 1; // dy is now 2*dy
    dx <<= 1; // dx is now 2*dx

    _put_pixel(surf, color, x0, y0);
    if (dx > dy) {
      int fraction = dy - (dx >> 1); // same as 2*dy - dx
      while (x0 != x1) {
        if (fraction >= 0) {
          y0 += stepy;
          fraction -= dx; // -= 2*dx
        }
        x0 += stepx;
        fraction += dy; //  -= 2*dy
        _put_pixel(surf, color, x0, y0);
      }
    } else {
      int fraction = dx - (dy >> 1);
      while (y0 != y1) {
        if (fraction >= 0) {
          x0 += stepx;
          fraction -= dy;
        }
        y0 += stepy;
        fraction += dx;
        _put_pixel(surf, color, x0, y0);
      }
    }
  }

  void Display :: _try_pixel(bool& res,
                             SDL_Surface* surf,
                             Uint32 color, int x, int y) {

    if (x >= 0 && y >= 0
        && x < _map->get_pixel_w()
        && y < _map->get_pixel_h()
        && _map->get_pixel(x, y) == Map::free) {
      _put_pixel(surf, color, x, y);
      res = false;
    } else
      res = true;
  }

  void Display :: _circle_points(SDL_Surface* surf,
                                 int cx, int cy, int x, int y, Uint32 color) {

    if (x == 0) {
      _put_pixel(surf, color, cx, cy + y);
      _put_pixel(surf, color, cx, cy - y);
      _put_pixel(surf, color, cx + y, cy);
      _put_pixel(surf, color, cx - y, cy);
    } else if (x == y) {
      _put_pixel(surf, color, cx + x, cy + y);
      _put_pixel(surf, color, cx - x, cy + y);
      _put_pixel(surf, color, cx + x, cy - y);
      _put_pixel(surf, color, cx - x, cy - y);
    } else if (x < y) {
      _put_pixel(surf, color, cx + x, cy + y);
      _put_pixel(surf, color, cx - x, cy + y);
      _put_pixel(surf, color, cx + x, cy - y);
      _put_pixel(surf, color, cx - x, cy - y);
      _put_pixel(surf, color, cx + y, cy + x);
      _put_pixel(surf, color, cx - y, cy + x);
      _put_pixel(surf, color, cx + y, cy - x);
      _put_pixel(surf, color, cx - y, cy - x);
    }
  }

  void Display :: _circle(SDL_Surface *surf,
                          int x_center, int y_center, int radius,
                          Uint32 color) {
    int x = 0;
    int y = radius;
    int p = (5 - radius * 4) / 4;

    _circle_points(surf, x_center, y_center, x, y, color);
    while (x < y) {
      x++;
      if (p < 0)
        p += 2 * x + 1;
      else {
        y--;
        p += 2 * (x - y) + 1;
      }
      _circle_points(surf, x_center, y_center, x, y, color);
    }
  }
  /// see http://en.wikipedia.org/wiki/Midpoint_circle_algorithm
  // transform setPixel(x0 + x, y0 + y); setPixel(x0 - x, y0 + y);
  // to  _line(x0 - x,, y0+y, x0 + x, y0 + y)
  void Display :: _disc(SDL_Surface *surf,
                        int x_center, int y_center, int radius,
                        Uint32 color) {
    int f = 1 - radius;
    int ddF_x = 1;
    int ddF_y = -2 * radius;
    int x = 0;
    int y = radius;
    int x0 = x_center;
    int y0 = y_center;

    _line(surf, x0, y0 - radius, x0, y0 + radius, color);
    _line(surf, x0 - radius, y0, x0 + radius, y0, color);
    while(x < y) {
      // ddF_x == 2 * x + 1;
      // ddF_y == -2 * y;
      // f == x*x + y*y - radius*radius + 2*x - y + 1;
      if(f >= 0) {
        y--;
        ddF_y += 2;
        f += ddF_y;
      }
      x++;
      ddF_x += 2;
      f += ddF_x;
      _line(surf, x0 - x, y0 + y, x0 + x, y0 + y, color);
      _line(surf, x0 - x, y0 - y, x0 + x, y0 - y, color);
      _line(surf, x0 - y, y0 + x, x0 + y, y0 + x, color);
      _line(surf, x0 - y, y0 - x, x0 + y, y0 - x, color);
    }
  }



  void Display :: _disp_bb() {
    unsigned x = _map->real_to_pixel(_robot.get_bb().x);
    unsigned y = _map->real_to_pixel(_robot.get_bb().y);
    unsigned w = _map->real_to_pixel(_robot.get_bb().w);
    unsigned h = _map->real_to_pixel(_robot.get_bb().h);

    assert(x >= 0);
    assert(y >= 0);
    assert(x + w < _screen->w);
    assert(y + h < _screen->h);
    _line(_screen, x, y, x + w, y, 0);
    _line(_screen, x + w, y, x + w, y + h, 0);
    _line(_screen, x + w, y + h, x, y + h, 0);
    _line(_screen, x, y + h, x, y, 0);
  }

  void Display :: _disp_goals() {
    for (size_t i = 0; i < _map->get_goals().size(); ++i) {
      const Goal& goal = _map->get_goals()[i];
      unsigned x = _map->real_to_pixel(goal.get_x());
      unsigned y = _map->real_to_pixel(goal.get_y());
      unsigned diam = _map->real_to_pixel(goal.get_diam());
      Uint8 r = 0, g = 0, b = 0;
      switch (goal.get_color()) {
      case 0:
        r = 255;
        break;
      case 1:
        g = 255;
        break;
      case 2:
        b = 255;
        break;
      default:
        assert(0);
      }
      _circle(_screen, x, y, diam, r, g, b);
    }
  }

  void Display :: _disp_radars() {
    unsigned r = _map->real_to_pixel(_robot.get_radius())  / 2;
    unsigned x = _map->real_to_pixel(_robot.get_pos().x());
    unsigned y = _map->real_to_pixel(_robot.get_pos().y());

    for (size_t i = 0; i < _robot.get_radars().size(); ++i) {
      const Radar& radar = _robot.get_radars()[i];
      if (radar.get_activated_slice() != -1) {
        float a1 = _robot.get_pos().theta() + radar.get_inc() * radar.get_activated_slice();
        float a2 = _robot.get_pos().theta() + radar.get_inc() * (radar.get_activated_slice() + 1);
        _line(_screen,
              cos(a1) * r + x, sin(a1) * r + y,
              cos(a2) * r + x, sin(a2) * r + y,
              0x0000FF);
        const Goal& g = _map->get_goals()[radar.get_color()];
        unsigned gx = _map->real_to_pixel(g.get_x());
        unsigned gy = _map->real_to_pixel(g.get_y());
        _line(_screen, x, y, gx, gy, 0x0000FF);
      }

    }

  }
  void Display :: _disp_bumpers() {
    // convert to pixel
    unsigned x = _map->real_to_pixel(_robot.get_pos().x());
    unsigned y = _map->real_to_pixel(_robot.get_pos().y());
    unsigned r = _map->real_to_pixel(_robot.get_radius());
    float theta = _robot.get_pos().theta();
    Uint32 cb_left = SDL_MapRGB(_screen->format, _robot.get_left_bumper() ? 255 : 0, 0, 0);
    Uint32 cb_right = SDL_MapRGB(_screen->format, _robot.get_right_bumper() ? 255 : 0, 0, 0);
    _line(_screen,
          (int) (r * cosf(theta + M_PI / 2.0f) + x),
          (int) (r * sinf(theta + M_PI / 2.0f) + y),
          (int) (r * cosf(theta) + x),
          (int) (r * sinf(theta) + y),
          cb_left);
    _line(_screen,
          (int) (r * cosf(theta - M_PI / 2.0f) + x),
          (int) (r * sinf(theta - M_PI / 2.0f) + y),
          (int) (r * cosf(theta) + x),
          (int) (r * sinf(theta) + y),
          cb_right);
  }

  Uint32 Display :: _color_from_id(SDL_Surface* surf, int x) {
    std::bitset<4> bs = x;
    Uint8 k = 1 - bs[4];
    Uint8 r = bs[0] * 128 + 127 * k;
    Uint8 g = bs[1] * 128 + 127 * k;
    Uint8 b = bs[2] * 128 + 127 * k;

    return SDL_MapRGB(surf->format, r, g, b);
  }

  void Display :: _disp_switches() {
    for (size_t i = 0; i < _map->get_illuminated_switches().size(); ++i) {
      const IlluminatedSwitch& sw = *_map->get_illuminated_switches()[i];
      unsigned x = _map->real_to_pixel(sw.get_x());
      unsigned y = _map->real_to_pixel(sw.get_y());
      unsigned rad = _map->real_to_pixel(sw.get_radius());
      Uint32 color = _color_from_id(_screen, sw.get_color());

      _circle(_screen, x, y, rad, color);
      if (sw.get_on())
        _disc(_screen, x, y, rad, color);
    }
  }

  void Display :: _disp_lasers() {
    for (size_t i = 0; i < _robot.get_lasers().size(); ++i) {
      unsigned x_laser = _map->real_to_pixel(_robot.get_pos().x()
                                             + _robot.get_lasers()[i].get_gap_dist()
                                             * cosf(_robot.get_pos().theta()
                                                 + _robot.get_lasers()[i].get_gap_angle()));
      unsigned y_laser = _map->real_to_pixel(_robot.get_pos().y()
                                             + _robot.get_lasers()[i].get_gap_dist()
                                             * sinf(_robot.get_pos().theta()
                                                 + _robot.get_lasers()[i].get_gap_angle()));
      _line(_screen, x_laser, y_laser,
            _robot.get_lasers()[i].get_x_pixel(),
            _robot.get_lasers()[i].get_y_pixel(),
            0xFF00000);
    }
  }

  void Display :: _disp_light_sensors() {
    for (size_t i = 0; i < _robot.get_light_sensors().size(); ++i) {
      const LightSensor& ls = _robot.get_light_sensors()[i];
      unsigned x_ls = _map->real_to_pixel(_robot.get_pos().x());
      unsigned y_ls = _map->real_to_pixel(_robot.get_pos().y());
      unsigned x_ls1 = _map->real_to_pixel(_robot.get_pos().x()
                                           + 200./(float)ls.get_color()
                                           * cosf(_robot.get_pos().theta()
                                               + ls.get_angle()-ls.get_range()/2.0));
      unsigned y_ls1 = _map->real_to_pixel(_robot.get_pos().y()
                                           + 200./(float)ls.get_color()
                                           * sinf(_robot.get_pos().theta()
                                               + ls.get_angle()-ls.get_range()/2.0));
      _line(_screen, x_ls, y_ls, x_ls1, y_ls1, _color_from_id(_screen, ls.get_color()));
      unsigned x_ls2 = _map->real_to_pixel(_robot.get_pos().x()
                                           + 200./(float)ls.get_color()
                                           * cosf(_robot.get_pos().theta()
                                               + ls.get_angle()+ls.get_range()/2.0));
      unsigned y_ls2 = _map->real_to_pixel(_robot.get_pos().y()
                                           + 200./(float)ls.get_color()
                                           * sinf(_robot.get_pos().theta()
                                               + ls.get_angle()+ls.get_range()/2.0));
      _line(_screen, x_ls, y_ls, x_ls2, y_ls2, _color_from_id(_screen, ls.get_color()));
      _line(_screen, x_ls1, y_ls1, x_ls2, y_ls2, _color_from_id(_screen, ls.get_color()));

      if (ls.get_activated()) {
        const IlluminatedSwitch& is =
          *_map->get_illuminated_switches()[ls.get_num()];
        unsigned x_is = _map->real_to_pixel(is.get_x());
        unsigned y_is = _map->real_to_pixel(is.get_y());
        _line(_screen, x_ls, y_ls, x_is, y_is, _color_from_id(_screen, is.get_color()));
      }
    }
  }

  void Display :: _disp_camera() {
    BOOST_STATIC_CONSTEXPR int pw = 20;
    if (!_robot.use_camera())
      return;
    unsigned x_ls = _map->real_to_pixel(_robot.get_pos().x());
    unsigned y_ls = _map->real_to_pixel(_robot.get_pos().y());
    float a1 = _robot.get_pos().theta() + _robot.get_camera().get_angular_range() / 2.0;
    _line(_screen, x_ls, y_ls, cos(a1) * 200 + x_ls,
          sin(a1) * 200 + y_ls, 0x0000ff);
    float a2 = _robot.get_pos().theta() - _robot.get_camera().get_angular_range() / 2.0;
    _line(_screen, x_ls, y_ls, cos(a2) * 200 + x_ls,
          sin(a2) * 200 + y_ls, 0x0000ff);

    for (size_t i = 0; i < _robot.get_camera().pixels().size(); ++i) {
      int pix = _robot.get_camera().pixels()[i];
      Uint32 color = pix == -1 ? 0xffffff : _color_from_id(_screen, pix);
      SDL_Rect r;
      r.x = i * pw;
      r.y = 0;
      r.w = pw;
      r.h = pw;
      SDL_FillRect(_screen, &r, color);
    }

  }

  void Display :: update() {
    _events();
    // convert to pixel
    unsigned x = _map->real_to_pixel(_robot.get_pos().x());
    unsigned y = _map->real_to_pixel(_robot.get_pos().y());
    unsigned r = _map->real_to_pixel(_robot.get_radius());
    float theta = _robot.get_pos().theta();

    // erase robot
    SDL_BlitSurface(_map_bmp, &_prev_bb, _screen, &_prev_bb);
    // erase all
    SDL_BlitSurface(_map_bmp, 0, _screen, 0);

    // lasers
    _disp_lasers();

    // goals
    _disp_goals();

    // illuminated switches
    _disp_switches();

    // light sensor
    _disp_light_sensors();

    // radars
    _disp_radars();

    // camera
    _disp_camera();

    // draw the circle again (robot)
    unsigned int col=_robot.color();
    _disc(_screen, x, y, r, _color_from_id(_screen,col));
    _circle(_screen,x,y,r,255,0,0);
    // direction
    Uint32 color = SDL_MapRGB(_screen->format, 0, 255, 0);
    _line(_screen, x, y, (int) (r * cosf(theta) + x), (int)(r * sinf(theta) + y), color);

    // bumpers
    _disp_bumpers();

    SDL_Rect rect;
    _bb_to_sdl(_robot.get_bb(), &rect);
    using namespace std;
    rect.x = max(0, min((int)rect.x, (int)_prev_bb.x));
    rect.y = max(0, min((int)rect.y, (int)_prev_bb.y));
    rect.w = max(rect.w, _prev_bb.w);
    rect.h = max(rect.h, _prev_bb.h);

    if (rect.x + rect.w > _w) rect.w = _w;
    if (rect.y + rect.h > _h) rect.h = _h;

    // the fast one
    SDL_UpdateRect(_screen, rect.x, rect.y, rect.w, rect.h);
    // the slow one
    SDL_UpdateRect(_screen, 0, 0, _screen->w, _screen->h);

    _bb_to_sdl(_robot.get_bb(), &_prev_bb);

  }



}

#else
#warning "SDL is disabled (no display for fastim)""
#endif