From 4db35babdd051cb1abebfd91f1c3cc0edd15974b Mon Sep 17 00:00:00 2001 From: schodet Date: Thu, 13 May 2004 19:08:53 +0000 Subject: Ajout du PathFinder. --- 2004/i/nono/src/path/map_segm.cc | 153 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 153 insertions(+) create mode 100644 2004/i/nono/src/path/map_segm.cc (limited to '2004/i/nono/src/path/map_segm.cc') diff --git a/2004/i/nono/src/path/map_segm.cc b/2004/i/nono/src/path/map_segm.cc new file mode 100644 index 0000000..682b2c2 --- /dev/null +++ b/2004/i/nono/src/path/map_segm.cc @@ -0,0 +1,153 @@ +// map_segm.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// 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 "map_segm.h" + +#include +#include +#include +#include + +namespace Path { + +/// Constructeur. +MapSegm::MapSegm (int minx, int miny, int maxx, int maxy) + : minx_ (minx), miny_ (miny), maxx_ (maxx), maxy_ (maxy) +{ + mapLimitsX_.push_back (minx); + mapLimitsX_.push_back (maxx); + mapLimitsY_.push_back (miny); + mapLimitsY_.push_back (maxy); + mapCells_.push_back (std::vector (1, 0)); +} + +/// Place un obstacle (mm). +void +MapSegm::putObstacle (int minx, int miny, int maxx, int maxy) +{ + // Clip. + if (minx < minx_) + minx = minx_; + if (miny < miny_) + miny = miny_; + if (maxx > maxx_) + maxx = maxx_; + if (maxy > maxy_) + maxy = maxy_; + // Split. + int mincol = findCol (minx); + if (mapLimitsX_[mincol] != minx) + splitCol (mincol, minx); + int maxcol = findCol (maxx); + if (mapLimitsX_[maxcol] != maxx) + splitCol (maxcol, maxx); + int minrow = findRow (miny); + if (mapLimitsY_[minrow] != miny) + splitRow (minrow, miny); + int maxrow = findRow (maxy); + if (mapLimitsY_[maxrow] != maxy) + splitRow (maxrow, maxy); + std::cout << "putObs " << mincol << ' ' << maxcol << ' ' << minrow << ' ' + << maxrow << std::endl; + // Remplit la zone. + for (int j = minrow; j < maxrow; ++j) + { + MapRow &r = mapCells_[j]; + for (int i = mincol; i < maxcol; ++i) + { + r[i] = 1; + } + } +} + +/// Split la carte à une colonne donnée. +void +MapSegm::splitCol (int col, int x) +{ + // Met à jour le tableau des abscisse. + mapLimitsX_.insert (mapLimitsX_.begin () + col, x); + // Pour chaque ligne. + MapCells::iterator first = mapCells_.begin (); + MapCells::iterator last = mapCells_.end (); + for ( ; first != last; ++first) + { + // Split la case. + (*first).insert ((*first).begin () + col, (*first)[col - 1]); + } +} + +/// Split la carte à une ligne donnée. +void +MapSegm::splitRow (int row, int y) +{ + // Met à jour le tableau des ordonnées. + mapLimitsY_.insert (mapLimitsY_.begin () + row, y); + // Split la ligne. + mapCells_.insert (mapCells_.begin () + row, mapCells_[row - 1]); +} + +/// Trouve la colonne correspondante à une abscisse (mm). +int +MapSegm::findCol (int x) const +{ + MapLimits::const_iterator it = + std::lower_bound (mapLimitsX_.begin (), mapLimitsX_.end (), x); + return it - mapLimitsX_.begin (); +} + +/// Trouve la ligne correspondante à une ordonnée (mm). +int +MapSegm::findRow (int y) const +{ + MapLimits::const_iterator it = + std::lower_bound (mapLimitsY_.begin (), mapLimitsY_.end (), y); + return it - mapLimitsY_.begin (); +} + +/// Sort sur std::ostream. +std::ostream & +operator<< (std::ostream &os, const MapSegm &m) +{ + using namespace std; + os << "limit x: "; + copy (m.mapLimitsX_.begin (), m.mapLimitsX_.end (), + ostream_iterator (os, " ")); + os << endl; + os << "limit y: "; + copy (m.mapLimitsY_.begin (), m.mapLimitsY_.end (), + ostream_iterator (os, " ")); + os << endl; + os << "map:" << endl; + MapSegm::MapCells::const_iterator first = m.mapCells_.begin (); + MapSegm::MapCells::const_iterator last = m.mapCells_.end (); + for ( ; first != last; ++first) + { + copy ((*first).begin (), (*first).end (), + ostream_iterator (os, " ")); + os << endl; + } + return os << endl; +} + +} // namespace Path -- cgit v1.2.3