// goto_hermite.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 "goto_hermite.h" #include /// Constructeur. /// fa : angle final. GotoHermite::GotoHermite (double fa) : step_ (0.0), lastDist_ (0.0), ia_ (0.0), fa_ (fa), di_ (defaultDi_), df_ (defaultDf_) { points_.push_back (Point ()); } /// Constructeur. /// fa : angle final. /// di : longueur de la tangente initiale. /// df : longueur de la tangente finale. GotoHermite::GotoHermite (double fa, double di, double df) : step_ (0.0), lastDist_ (0.0), ia_ (0.0), fa_ (fa), di_ (di), df_ (df) { points_.push_back (Point ()); } /// Ajoute un point au chemin. void GotoHermite::add (double x, double y) { Point newPoint (x, y); // Met à jour le tableau des distances. double dist = newPoint.distTo (points_.back ()); for (Dists::iterator i = dists_.begin (); i != dists_.end (); ++i) { *i += dist; } dists_.push_back (0.0); lastDist_ += dist; // Ajoute le point. points_.push_back (newPoint); } /// Initialise le Goto, appelé au début de la trajectoire. void GotoHermite::init (const Tracker &t) { double x, y, a; t.getPos (x, y, a); lastPoint_ = points_[0] = Point (x, y); ia_ = a; } /// Fournit la distance au point final (mm), le point le plus loin à moins /// de distmax (mm) et renvois false si le Goto est fini. /// eps : distance (mm) en dessous de laquelle on considère que la destination /// est atteinte. /// distmax : distance (mm) maximale du point. /// dist : distance (mm) au point final. /// (x, y) : point intermédiaire (mm). bool GotoHermite::get (const Tracker &t, double distmax, double eps, double &dist, double &x, double &y) { Point cur (t.getX (), t.getY ()); // Trouve le point intermédiaire. double sqDistMax = distmax * distmax; while (cur.sqDistTo (lastPoint_) < sqDistMax) { step_ += stepSize_; if (!computePoint (step_, lastPoint_, lastDist_)) break; } // Trouve la distance au point final. dist = lastDist_ + cur.distTo (lastPoint_); if (eps > dist) return false; x = lastPoint_.x; y = lastPoint_.y; return true; } /// Test le GotoHermite en affichant la liste des points générés. void GotoHermite::test (std::ostream &os) const { Point p; double dist; for (double s = 0.0; ; s += stepSize_) { if (!computePoint (s, p, dist)) break; std::cout << p << ' ' << dist << std::endl; } } /// Calcule le point au pas step et la distance restant à parcourir, /// renvois faux si fini. bool GotoHermite::computePoint (double step, Point &p, double &dist) const { // Vérifications d'overflow. if (step < 0.0) step = 0.0; // Détermine la section correspondante. int i = static_cast (step); if (i >= static_cast (points_.size ()) - 1) return false; step -= i; const Point &p1 = points_[i]; const Point &p2 = points_[i + 1]; // Calcule les fonction d'hermite. double step2 = step * step; double step3 = step2 * step; double h2 = -2 * step3 + 3 * step2; double h1 = -h2 + 1; double h3 = step3 - 2 * step2 + step; double h4 = step3 - step2; // Calcule les tangeantes. Point t1; if (i > 0) { // Méthode de Catmull-Rom. t1 = (p2 - points_[i - 1]) * tightness_; } else { t1 = p1; t1.x = cos (ia_) * di_; t1.y = sin (ia_) * di_; } Point t2; if (i < static_cast (points_.size ()) - 2) { // Méthode de Catmull-Rom. t2 = (points_[i + 2] - p1) * tightness_; } else { t2 = p2; t2.x = cos (fa_) * df_; t2.y = sin (fa_) * df_; } // Applique Hermite. t1 *= h3; t2 *= h4; p = p1 * h1; p += t1; p += t2; p += p2 * h2; // Calcule la distance au point final. dist = dists_[i] + p.distTo (points_[i + 1]); return true; }