summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/path/map_graph.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/path/map_graph.cc')
-rw-r--r--2004/i/nono/src/path/map_graph.cc280
1 files changed, 280 insertions, 0 deletions
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 <iterator>
+#include <algorithm>
+#include <iomanip>
+
+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<Point> (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<double> (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<int> (mg.points_[i].x)
+ << "|" << static_cast<int> (mg.points_[i].y)
+ << "}}\",pos=\"" << static_cast<int> (mg.points_[i].x) * 3
+ << "," << static_cast<int> (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<int> (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<int> (mg.points_[i].x)
+ << "|" << static_cast<int> (mg.points_[i].y)
+ << "} |" << static_cast<int> (mg.distsMin_[i])
+ << "}\",pos=\"" << static_cast<int> (mg.points_[i].x) * 3
+ << "," << static_cast<int> (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<int> (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