From b097c16be082ddd505db55bf9531d98cb2eadc65 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 27 Jun 2004 14:27:24 +0000 Subject: Trouve le plus court chemin. --- 2004/i/nono/src/path/map_graph.cc | 130 +++++++++++++++++++++++--------------- 1 file changed, 79 insertions(+), 51 deletions(-) (limited to '2004/i/nono/src/path/map_graph.cc') diff --git a/2004/i/nono/src/path/map_graph.cc b/2004/i/nono/src/path/map_graph.cc index 014d80f..ba4f502 100644 --- a/2004/i/nono/src/path/map_graph.cc +++ b/2004/i/nono/src/path/map_graph.cc @@ -121,6 +121,85 @@ MapGraph::buildGraph (void) } } +/// 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 @@ -226,55 +305,4 @@ operator<< (std::ostream &os, const MapGraph &mg) 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 -- cgit v1.2.3