// 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), ia_ (0.0), fa_ (fa) { points_.push_back (Point ()); } /// Ajoute un point au chemin. void GotoHermite::add (double x, double y) { points_.push_back (Point (x, y)); } /// 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); 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) { return false; } /// Test le GotoHermite en affichant la liste des points générés. void GotoHermite::test (std::ostream &os) const { Point p; for (double s = 0.0; ; s += stepSize_) { if (!computePoint (s, p)) break; std::cout << p << std::endl; } } /// Calcule le point au pas step, renvois faux si fini. bool GotoHermite::computePoint (double step, Point &p) 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 { // Projette le vecteur vers le point suivant sur le vecteur angle. t1 = p1; double cia = cos (ia_); double sia = sin (ia_); t1.x += cia * (p2.x - p1.x) + sia * (p2.y - p1.y); t1.x += -sia * (p2.x - p1.x) + cia * (p2.y - p1.y); } Point t2; if (i < static_cast (points_.size ()) - 2) { // Méthode de Catmull-Rom. t2 = (points_[i + 2] - p1) * tightness_; } else { // Projette le vecteur depuis le point précédent sur le vecteur angle. t2 = p2; double cia = cos (fa_); double sia = sin (fa_); t2.x += cia * (p2.x - p1.x) + sia * (p2.y - p1.y); t2.x += -sia * (p2.x - p1.x) + cia * (p2.y - p1.y); } // Aplique Hermite. p = p1 * h1; p+= t1 * h3; p+= t2 * h4; p+= p2 * h2; return true; }