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.cc82
1 files changed, 60 insertions, 22 deletions
diff --git a/2004/i/nono/src/motor/goto_hermite.cc b/2004/i/nono/src/motor/goto_hermite.cc
index a1fc6c5..9e47296 100644
--- a/2004/i/nono/src/motor/goto_hermite.cc
+++ b/2004/i/nono/src/motor/goto_hermite.cc
@@ -29,7 +29,18 @@
/// Constructeur.
/// fa : angle final.
GotoHermite::GotoHermite (double fa)
- : step_ (0.0), ia_ (0.0), fa_ (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 ());
}
@@ -38,7 +49,19 @@ GotoHermite::GotoHermite (double fa)
void
GotoHermite::add (double x, double y)
{
- points_.push_back (Point (x, 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.
@@ -47,7 +70,7 @@ GotoHermite::init (const Tracker &t)
{
double x, y, a;
t.getPos (x, y, a);
- points_[0] = Point (x, y);
+ lastPoint_ = points_[0] = Point (x, y);
ia_ = a;
}
@@ -62,7 +85,22 @@ bool
GotoHermite::get (const Tracker &t, double distmax, double eps, double &dist,
double &x, double &y)
{
- return false;
+ 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.
@@ -70,17 +108,19 @@ void
GotoHermite::test (std::ostream &os) const
{
Point p;
+ double dist;
for (double s = 0.0; ; s += stepSize_)
{
- if (!computePoint (s, p))
+ if (!computePoint (s, p, dist))
break;
- std::cout << p << std::endl;
+ std::cout << p << ' ' << dist << std::endl;
}
}
-/// Calcule le point au pas step, renvois faux si fini.
+/// Calcule le point au pas step et la distance restant à parcourir,
+/// renvois faux si fini.
bool
-GotoHermite::computePoint (double step, Point &p) const
+GotoHermite::computePoint (double step, Point &p, double &dist) const
{
// Vérifications d'overflow.
if (step < 0.0)
@@ -108,12 +148,9 @@ GotoHermite::computePoint (double step, Point &p) const
}
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);
+ t1.x = cos (ia_) * di_;
+ t1.y = sin (ia_) * di_;
}
Point t2;
if (i < static_cast<int> (points_.size ()) - 2)
@@ -123,18 +160,19 @@ GotoHermite::computePoint (double step, Point &p) const
}
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);
+ t2.x = cos (fa_) * df_;
+ t2.y = sin (fa_) * df_;
}
- // Aplique Hermite.
+ // Applique Hermite.
+ t1 *= h3;
+ t2 *= h4;
p = p1 * h1;
- p+= t1 * h3;
- p+= t2 * h4;
- p+= p2 * h2;
+ p += t1;
+ p += t2;
+ p += p2 * h2;
+ // Calcule la distance au point final.
+ dist = dists_[i] + p.distTo (points_[i + 1]);
return true;
}