summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/goto_hermite.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/motor/goto_hermite.cc')
-rw-r--r--2004/i/nono/src/motor/goto_hermite.cc140
1 files changed, 140 insertions, 0 deletions
diff --git a/2004/i/nono/src/motor/goto_hermite.cc b/2004/i/nono/src/motor/goto_hermite.cc
new file mode 100644
index 0000000..a1fc6c5
--- /dev/null
+++ b/2004/i/nono/src/motor/goto_hermite.cc
@@ -0,0 +1,140 @@
+// 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 <iostream>
+
+/// 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<int> (step);
+ if (i >= static_cast<int> (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<int> (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;
+}
+