// 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])); } } } /// 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; } } } } } /// Trouve le prochain point cible à partir de p. Renvois true si trouvé. bool MapGraph::nextPoint (const Point &p, Point &target) { bool found = false; double dmin; int pmin; for (unsigned int i = 0; i < points_.size (); ++i) { if (distsMin_[i] > 0.0) { double d = computeDist (points_[i], p); if (d > 0.0) { d += distsMin_[i]; if (!found || d < dmin) { dmin = d; pmin = i; found = true; } } } } target = pmin; return found; } /// 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; } } // namespace Path