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/Makefile.defs | 9 ++ 2004/i/nono/src/path/map_graph.cc | 280 +++++++++++++++++++++++++++++++++ 2004/i/nono/src/path/map_graph.h | 86 ++++++++++ 2004/i/nono/src/path/map_segm.cc | 153 ++++++++++++++++++ 2004/i/nono/src/path/map_segm.h | 67 ++++++++ 2004/i/nono/src/path/test_map.cc | 61 +++++++ 2004/i/nono/src/path/test_map_graph.cc | 87 ++++++++++ 7 files changed, 743 insertions(+) create mode 100644 2004/i/nono/src/path/Makefile.defs create mode 100644 2004/i/nono/src/path/map_graph.cc create mode 100644 2004/i/nono/src/path/map_graph.h create mode 100644 2004/i/nono/src/path/map_segm.cc create mode 100644 2004/i/nono/src/path/map_segm.h create mode 100644 2004/i/nono/src/path/test_map.cc create mode 100644 2004/i/nono/src/path/test_map_graph.cc (limited to '2004/i') diff --git a/2004/i/nono/src/path/Makefile.defs b/2004/i/nono/src/path/Makefile.defs new file mode 100644 index 0000000..8e06df5 --- /dev/null +++ b/2004/i/nono/src/path/Makefile.defs @@ -0,0 +1,9 @@ +TARGETS += test_map_graph +LIBS += path.a +test_map_graph_SOURCES = test_map_graph.cc path.a +path_a_SOURCES = map_graph.cc + +test_map_graph: $(test_map_graph_SOURCES:%.cc=%.o) + +path.a: ${path_a_SOURCES:%.cc=path.a(%.o)} + diff --git a/2004/i/nono/src/path/map_graph.cc b/2004/i/nono/src/path/map_graph.cc new file mode 100644 index 0000000..014d80f --- /dev/null +++ b/2004/i/nono/src/path/map_graph.cc @@ -0,0 +1,280 @@ +// map_graph.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_graph.h" + +#include +#include +#include + +namespace Path { + +/// Constructeur. +MapGraph::MapGraph (const Point &min, const Point &max) + : min_ (min), max_ (max) +{ + // Le point cible. + points_.push_back (Point ()); +} + +/// Ajoute un obstacle. +void +MapGraph::add (const Point &c, double r) +{ + obstacles_.push_back (Obstacle (c, r)); +} + +/// Paramètre le point d'arrivée. +void +MapGraph::setGoal (const Point &goal) +{ + if (goal < min_) + points_[0] = min_; + else if (goal > max_) + points_[0] = max_; + else + points_[0] = goal; +} + +/// Place les points correspondants aux obstacles. +void +MapGraph::placePoints (void) +{ + // Netois les anciens résultats. + points_.erase (points_.begin () + 1, points_.end ()); + // Place les points. + for (Obstacles::const_iterator i = obstacles_.begin (); + i != obstacles_.end (); ++i) + { + const Obstacle &obs = *i; + double r = obs.r * margin_; + double ra = r * 0.5 * sqrt2_; + Point n (obs.c.x, obs.c.y - r); + if (min_ <= n && n < max_) + points_.push_back (n); + Point ne (obs.c.x + ra, obs.c.y - ra); + if (min_ <= ne && ne < max_) + points_.push_back (ne); + Point e (obs.c.x + r, obs.c.y); + if (min_ <= e && e < max_) + points_.push_back (e); + Point se (obs.c.x + ra, obs.c.y + ra); + if (min_ <= se && se < max_) + points_.push_back (se); + Point s (obs.c.x, obs.c.y + r); + if (min_ <= s && s < max_) + points_.push_back (s); + Point so (obs.c.x - ra, obs.c.y + ra); + if (min_ <= so && so < max_) + points_.push_back (so); + Point o (obs.c.x - r, obs.c.y); + if (min_ <= o && o < max_) + points_.push_back (o); + Point no (obs.c.x - ra, obs.c.y - ra); + if (min_ <= no && no < max_) + points_.push_back (no); + } +} + +/// Construit le graph à partir des points en prenant en compte les +/// obstacles. +void +MapGraph::buildGraph (void) +{ + // Netoie l'ancien graph. + arcs_.clear (); + // Reconstruit le graph. + arcs_.resize (points_.size (), ArcsLine ()); + for (unsigned int i = 0; i < points_.size (); ++i) + { + ArcsLine &al = arcs_[i]; + for (unsigned int j = 0; j < i; ++j) + { + al.push_back (arcs_[j][i]); + } + al.push_back (-1.0); + for (unsigned int j = i + 1; j < points_.size (); ++j) + { + al.push_back (computeDist (points_[i], points_[j])); + } + } +} + +/// Calcule la distance entre deux points en prenant les obstacles en +/// compte. +double +MapGraph::computeDist (const Point &a, const Point &b) const +{ + Point u (b - a); + double ab = u.norm (); + u *= 1.0 / ab; + Point n (u); + n.ccw (); + for (Obstacles::const_iterator i = obstacles_.begin (); + i != obstacles_.end (); ++i) + { + Point vac ((*i).c - a); + double d = fabs (vac * n); + double r = (*i).r; + if (d > r) + { + // La droite ne passe pas dans le cercle. + } + else + { + // La droite passe dans le cercle, regarder le segment. + double m = vac * u; + double f = sqrt (r * r - d * d); + // Il faut que les points soient du même coté des deux + // intersections. + if ((0 < m - f && 0 < m + f && ab < m - f && ab < m + f) + || (0 > m - f && 0 > m + f && ab > m - f && ab > m + f)) + { + // C'est bon on est dehors. + } + else + { + // Dedans. + return -1.0; + } + } + } + return a.distTo (b); +} + +/// Sort sur un std::ostream. +std::ostream & +operator<< (std::ostream &os, const MapGraph &mg) +{ + using namespace std; + os << "Points :\n"; + copy (mg.points_.begin (), mg.points_.end (), + ostream_iterator (os, "\n")); + os << "Arcs :\n"; + for (MapGraph::Arcs::const_iterator i = mg.arcs_.begin (); + i != mg.arcs_.end (); ++i) + { + copy ((*i).begin (), (*i).end (), + ostream_iterator (os, " ")); + os << "\n"; + } + os << "graph map {\n" + " overlap=scale\n" + " sep=0.5\n"; + for (unsigned int i = 0; i < mg.points_.size (); ++i) + { + os << " " << i << " [shape=Mrecord,label=\"{" << i + << "| {" << static_cast (mg.points_[i].x) + << "|" << static_cast (mg.points_[i].y) + << "}}\",pos=\"" << static_cast (mg.points_[i].x) * 3 + << "," << static_cast (mg.points_[i].y) * 3 << "\"]\n"; + for (unsigned int j = 0; j < i; ++j) + { + double d = mg.arcs_[i][j]; + if (d > 0.0) + { + os << " " << i << " -- " << j << " [label=\"" + << static_cast (d) << "\"];\n"; + } + } + } + os << "}\n"; + os << "graph path {\n" + " overlap=scale\n" + " sep=0.5\n"; + for (unsigned int i = 0; i < mg.points_.size (); ++i) + { + os << " " << i << " [shape=" + << (mg.E[i] ? "M" : "") << "record,label=\"{" << i + << "| {" << static_cast (mg.points_[i].x) + << "|" << static_cast (mg.points_[i].y) + << "} |" << static_cast (mg.distsMin_[i]) + << "}\",pos=\"" << static_cast (mg.points_[i].x) * 3 + << "," << static_cast (mg.points_[i].y) * 3 << "\"]\n"; + } + for (unsigned int i = 0; i < mg.points_.size (); ++i) + { + if (mg.parents_[i] >= 0) + { + os << " " << i << " -- " << mg.parents_[i] + << " [label=\"" + << static_cast (mg.distsMin_[i]) << "\"];\n"; + } + } + os << "}\n"; + return os << endl; +} + +/// Trouve les plus cours chemins vers le points d'arrivée. +void +MapGraph::findPath (int steps/*-1*/) +{ + // Nettoie les derniers résultats. + distsMin_.clear (); + parents_.clear (); + E.clear (); + // Initalise E, d et p. + E.resize (points_.size (), 0); + distsMin_ = arcs_[0]; + for (unsigned int i = 0; i < points_.size (); ++i) + parents_.push_back (distsMin_[i] > 0.0 ? 0 : -1); + E[0] = 1; + distsMin_[0] = 0.0; + // Dijkstra. + if (steps == -1) steps = points_.size () - 2; + for (int i = 0; i < steps; ++i) + { + // Trouve le sommet t ayant le plus petit d dans S-E. + double dmin = 0.0; + int t = -1; + for (unsigned int j = 0; j < points_.size (); ++j) + { + if (E[j] == 0 && (distsMin_[j] > 0.0 && (t == -1 || dmin > distsMin_[j]))) + { + dmin = distsMin_[j]; + t = j; + } + } + if (t == -1) + break; + // Ajouter t à E. + E[t] = 1; + // Pour tout j dans S. + for (unsigned int j = 0; j < points_.size (); ++j) + { + if (arcs_[t][j] > 0.0) + { + double d = distsMin_[t] + arcs_[t][j]; + // Relaxe. + if (distsMin_[j] < 0.0 || distsMin_[j] > d) + { + distsMin_[j] = d; + parents_[j] = t; + } + } + } + } +} + +} // namespace Path diff --git a/2004/i/nono/src/path/map_graph.h b/2004/i/nono/src/path/map_graph.h new file mode 100644 index 0000000..13452f8 --- /dev/null +++ b/2004/i/nono/src/path/map_graph.h @@ -0,0 +1,86 @@ +#ifndef map_graph_h +#define map_graph_h +// map_graph.h +// 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 "utils/point.h" + +#include +#include +#include + +namespace Path { + +/// Carte vectorielle permettant de trouver son chemin. +class MapGraph +{ + struct Obstacle + { + Point c; + double r; + Obstacle (const Point &c_, double r_) : c (c_), r (r_) { } + }; + typedef std::list Obstacles; + Obstacles obstacles_; + typedef std::vector Points; + Points points_; + typedef std::vector ArcsLine; + typedef std::vector Arcs; + Arcs arcs_; + static const double sqrt2_ = 1.41421356237309504880; + static const double margin_ = 1.1; + Point min_, max_; + typedef std::vector Parent; + Parent parents_; + typedef std::vector Dists; + Dists distsMin_; + std::vector E; + public: + /// Constructeur. + MapGraph (const Point &min, const Point &max); + /// Ajoute un obstacle. + void add (const Point &c, double r); + /// Paramètre le point d'arrivée. + void setGoal (const Point &goal); + /// Sort sur un std::ostream. + friend std::ostream &operator<< (std::ostream &os, const MapGraph &mg); +// private: + /// Place les points correspondants aux obstacles. + void placePoints (void); + /// Construit le graph à partir des points en prenant en compte les + /// obstacles. + void buildGraph (void); + /// Calcule la distance entre deux points en prenant les obstacles en + /// compte. + double computeDist (const Point &a, const Point &b) const; + /// Trouve les plus cours chemins vers le points d'arrivée. + void findPath (int steps = -1); +}; + +/// Sort sur un std::ostream. +std::ostream &operator<< (std::ostream &os, const MapGraph &mg); + +} // namespace Path + +#endif // map_graph_h 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 diff --git a/2004/i/nono/src/path/map_segm.h b/2004/i/nono/src/path/map_segm.h new file mode 100644 index 0000000..78b87e7 --- /dev/null +++ b/2004/i/nono/src/path/map_segm.h @@ -0,0 +1,67 @@ +#ifndef map_segm_h +#define map_segm_h +// map_segm.h +// 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 +#include + +namespace Path { + +/// Carte de path finding. +class MapSegm +{ + /// Limites des cases. + int minx_, miny_, maxx_, maxy_; + typedef std::vector MapLimits; + MapLimits mapLimitsX_, mapLimitsY_; + /// Contenu des cases. + typedef std::vector MapRow; + typedef std::vector MapCells; + MapCells mapCells_; + public: + /// Constructeur. + MapSegm (int minx, int miny, int maxx, int maxy); + /// Place un obstacle (mm). + void putObstacle (int minx, int miny, int maxx, int maxy); + /// Sort sur std::ostream. + friend std::ostream &operator<< (std::ostream &os, const Map &m); + protected: + /// Split la carte à une colonne donnée. + void splitCol (int col, int x); + /// Split la carte à une ligne donnée. + void splitRow (int row, int y); + /// Trouve la colonne correspondante à une abscisse (mm). + int findCol (int x) const; + /// Trouve la ligne correspondante à une ordonnée (mm). + int findRow (int y) const; +}; + +/// Sort sur std::ostream. +std::ostream &operator<< (std::ostream &os, const MapSegm &m); + +} // namespace Path + +#endif // map_segm_h diff --git a/2004/i/nono/src/path/test_map.cc b/2004/i/nono/src/path/test_map.cc new file mode 100644 index 0000000..255859b --- /dev/null +++ b/2004/i/nono/src/path/test_map.cc @@ -0,0 +1,61 @@ +// test_map.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.h" + +#include +#include +#include + +int +main (int argc, char **argv) +{ + if (argc < 5 || argc % 4 != 1) + { + std::cerr << "test_map - Teste Path::Map.\n" + " place un obstacle." + << std::endl; + return 1; + } + try + { + Path::Map m (0, 0, 300, 300); + int i = 1; + while (i < argc - 3) + { + m.putObstacle (atoi (argv[i]), + atoi (argv[i + 1]), + atoi (argv[i + 2]), + atoi (argv[i + 3])); + i += 4; + } + std::cout << m << std::endl; + return 0; + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + return 1; + } +} diff --git a/2004/i/nono/src/path/test_map_graph.cc b/2004/i/nono/src/path/test_map_graph.cc new file mode 100644 index 0000000..e2484d9 --- /dev/null +++ b/2004/i/nono/src/path/test_map_graph.cc @@ -0,0 +1,87 @@ +// test_map_graph.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_graph.h" + +#include +#include +#include + +void +syntax (void) +{ + std::cout << "test_map_graph - Teste MapGraph.\n" + " o place un obstacle.\n" + " s change le nombre de pas de l'algo." + << std::endl; +} + +int +main (int argc, char **argv) +{ + try + { + Path::MapGraph mg (Point (0, 0), Point (300, 300)); + int i = 1; + int steps = -1; + int x, y, r; + while (i < argc) + { + switch (argv[i][0]) + { + case 'o': + i++; + if (i >= argc) break; + x = atoi (argv[i++]); + if (i >= argc) break; + y = atoi (argv[i++]); + if (i >= argc) break; + r = atoi (argv[i++]); + mg.add (Point (x, y), r); + break; + case 's': + i++; + if (i >= argc) break; + steps = atoi (argv[i++]); + break; + default: + case '?': + i++; + syntax (); + return 0; + } + } + mg.setGoal (Point ()); + mg.placePoints (); + mg.buildGraph (); + mg.findPath (steps); + std::cout << mg << std::endl; + return 0; + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + return 1; + } +} -- cgit v1.2.3