cheshirekow  v0.1.0
generic_artist.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Josh Bialkowski (jbialk@mit.edu)
3  *
4  * This file is part of mpblocks.
5  *
6  * mpblocks is free software: you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation, either version 3 of the License, or
9  * (at your option) any later version.
10  *
11  * mpblocks is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with mpblocks. If not, see <http://www.gnu.org/licenses/>.
18  */
27 #ifndef MPBLOCKS_DUBINS_CURVES_EIGEN_GENERICARTIST_HPP_
28 #define MPBLOCKS_DUBINS_CURVES_EIGEN_GENERICARTIST_HPP_
29 
30 #include <mpblocks/gtk.h>
31 
32 namespace mpblocks {
33 namespace dubins {
34 namespace curves_eigen {
35 
37 template <typename Format_t>
38 struct GenericArtist {
39  typedef Eigen::Matrix<Format_t, 3, 1> Vector3d_t;
40  typedef Eigen::Matrix<Format_t, 2, 1> Vector2d_t;
41  typedef Integrate<Format_t> Integrate_t;
42  typedef Path<Format_t> Path_t;
43 
44  static void drawLRL(const Vector3d_t& q0, const Format_t r, const Path_t soln,
45  const DrawOpts& opts) {
46  if (!soln.f) return;
47 
48  Cairo::RefPtr<Cairo::Context> ctx = opts.ctx;
49  gtk::EigenCairo ectx(ctx);
50 
51  Vector3d_t q1 = Integrate_t::L(q0, r, soln.s[0]);
52  Vector3d_t q2 = Integrate_t::R(q1, r, soln.s[1]);
53 
54  Vector2d_t c[3];
55  c[0] = leftCenter(q0, r);
56  c[1] = rightCenter(q1, r);
57  c[2] = leftCenter(q2, r);
58 
59  Vector3d_t alpha;
60  alpha[0] = leftAngleOf(q0);
61  alpha[1] = rightAngleOf(q1);
62  alpha[2] = leftAngleOf(q2);
63 
64  Vector3d_t arc = soln.s;
65 
66  if (opts.drawBalls) {
67  ctx->set_dash(opts.dash, opts.dashOffExtra);
68  ctx->set_source(opts.patL);
69  ectx.circle(c[0], r);
70  ctx->stroke();
71  ectx.circle(c[2], r);
72  ctx->stroke();
73  ctx->set_source(opts.patR);
74  ectx.circle(c[1], r);
75  ctx->stroke();
76  ctx->unset_dash();
77  }
78 
79  ctx->set_source(opts.patPath);
80  ctx->move_to(q0[0], q0[1]);
81  ectx.arc( c[0], r, alpha[0], alpha[0] + arc[0]);
82  ectx.arc_negative(c[1], r, alpha[1], alpha[1] - arc[1]);
83  ectx.arc(c[2], r, alpha[2], alpha[2] + arc[2]);
84  ctx->stroke();
85  }
86 
87  static void drawRLR(const Vector3d_t& q0, const Format_t r,
88  const Path_t& soln, const DrawOpts& opts) {
89  if (!soln.f) return;
90 
91  Cairo::RefPtr<Cairo::Context> ctx = opts.ctx;
92  gtk::EigenCairo ectx(ctx);
93 
94  Vector3d_t q1 = Integrate_t::R(q0, r, soln.s[0]);
95  Vector3d_t q2 = Integrate_t::L(q1, r, soln.s[1]);
96 
97  Vector2d_t c[3];
98  c[0] = rightCenter(q0, r);
99  c[1] = leftCenter(q1, r);
100  c[2] = rightCenter(q2, r);
101 
102  Vector3d_t alpha;
103  alpha[0] = rightAngleOf(q0);
104  alpha[1] = leftAngleOf(q1);
105  alpha[2] = rightAngleOf(q2);
106 
107  Vector3d_t arc = soln.s;
108 
109  if (opts.drawBalls) {
110  ctx->set_dash(opts.dash, opts.dashOffExtra);
111  ctx->set_source(opts.patR);
112  ectx.circle(c[0], r);
113  ctx->stroke();
114  ectx.circle(c[2], r);
115  ctx->stroke();
116  ctx->set_source(opts.patL);
117  ectx.circle(c[1], r);
118  ctx->stroke();
119  ctx->unset_dash();
120  }
121 
122  ctx->set_source(opts.patPath);
123  ctx->move_to(q0[0], q0[1]);
124  ectx.arc_negative(c[0], r, alpha[0], alpha[0] - arc[0]);
125  ectx.arc(c[1], r, alpha[1], alpha[1] + arc[1]);
126  ectx.arc_negative(c[2], r, alpha[2], alpha[2] - arc[2]);
127  ctx->stroke();
128  }
129 
130  static void drawLSL(const Vector3d_t& q0, const Format_t r,
131  const Path_t& soln, const DrawOpts& opts) {
132  if (!soln.f) return;
133 
134  Cairo::RefPtr<Cairo::Context> ctx = opts.ctx;
135  gtk::EigenCairo ectx(ctx);
136 
137  Vector3d_t q1 = Integrate_t::L(q0, r, soln.s[0]);
138  Vector3d_t q2 = Integrate_t::S(q1, soln.s[1]);
139  Vector3d_t q3 = Integrate_t::L(q2, r, soln.s[2]);
140 
141  Vector2d_t c[2];
142  c[0] = leftCenter(q0, r);
143  c[1] = leftCenter(q3, r);
144 
145  Format_t alpha[2];
146  alpha[0] = leftAngleOf(q0);
147  alpha[1] = leftAngleOf(q2);
148 
149  if (opts.drawBalls) {
150  ctx->set_dash(opts.dash, opts.dashOffExtra);
151  ctx->set_source(opts.patL);
152  ectx.circle(c[0], r);
153  ctx->stroke();
154  ectx.circle(c[1], r);
155  ctx->stroke();
156  ctx->unset_dash();
157  }
158 
159  // get the angle of the straight segment
160  Format_t angle = leftAngle_inv(alpha[0] + soln.s[0]);
161 
162  // create a vector
163  Vector2d_t v = soln.s[1] * Vector2d_t(std::cos(angle), std::sin(angle));
164 
165  ctx->set_source(opts.patPath);
166  ctx->move_to(q0[0], q0[1]);
167  ectx.arc(c[0], r, alpha[0], alpha[0] + soln.s[0]);
168  ectx.rel_line_to(v);
169  ectx.arc(c[1], r, alpha[1], alpha[1] + soln.s[2]);
170  ctx->stroke();
171  }
172 
173  static void drawRSR(const Vector3d_t& q0, const Format_t r,
174  const Path_t& soln, const DrawOpts& opts) {
175  if (!soln.f) return;
176 
177  Cairo::RefPtr<Cairo::Context> ctx = opts.ctx;
178  gtk::EigenCairo ectx(ctx);
179 
180  Vector3d_t q1 = Integrate_t::R(q0, r, soln.s[0]);
181  Vector3d_t q2 = Integrate_t::S(q1, soln.s[1]);
182 
183  Vector2d_t c[2];
184  c[0] = rightCenter(q0, r);
185  c[1] = rightCenter(q2, r);
186 
187  Vector3d_t alpha;
188  alpha[0] = rightAngleOf(q0);
189  alpha[1] = rightAngleOf(q2);
190 
191  if (opts.drawBalls) {
192  ctx->set_dash(opts.dash, opts.dashOffExtra);
193  ctx->set_source(opts.patR);
194  ectx.circle(c[0], r);
195  ctx->stroke();
196  ectx.circle(c[1], r);
197  ctx->stroke();
198  ctx->unset_dash();
199  }
200 
201  // get the angle of the straight segment
202  Format_t angle = rightAngle_inv(alpha[0] - soln.s[0]);
203 
204  // create a vector
205  Vector2d_t v = soln.s[1] * Vector2d_t(std::cos(angle), std::sin(angle));
206 
207  ctx->set_source(opts.patPath);
208  ctx->move_to(q0[0], q0[1]);
209  ectx.arc_negative(c[0], r, alpha[0], alpha[0] - soln.s[0]);
210  ectx.rel_line_to(v);
211  ectx.arc_negative(c[1], r, alpha[1], alpha[1] - soln.s[2]);
212  ctx->stroke();
213  }
214 
215  static void drawLSR(const Vector3d_t& q0, const Format_t r,
216  const Path_t& soln, const DrawOpts& opts) {
217  if (!soln.f) return;
218 
219  Cairo::RefPtr<Cairo::Context> ctx = opts.ctx;
220  gtk::EigenCairo ectx(ctx);
221 
222  Vector3d_t q1 = Integrate_t::L(q0, r, soln.s[0]);
223  Vector3d_t q2 = Integrate_t::S(q1, soln.s[1]);
224 
225  Vector2d_t c[2];
226  c[0] = leftCenter(q0, r);
227  c[1] = rightCenter(q2, r);
228 
229  Vector3d_t alpha;
230  alpha[0] = leftAngleOf(q0);
231  alpha[1] = rightAngleOf(q2);
232 
233  if (opts.drawBalls) {
234  ctx->set_dash(opts.dash, opts.dashOffExtra);
235  ctx->set_source(opts.patL);
236  ectx.circle(c[0], r);
237  ctx->stroke();
238  ctx->set_source(opts.patR);
239  ectx.circle(c[1], r);
240  ctx->stroke();
241  ctx->unset_dash();
242  }
243 
244  // get the angle of the straight segment
245  Format_t angle = leftAngle_inv(alpha[0] + soln.s[0]);
246 
247  // create a vector
248  Vector2d_t v = soln.s[1] * Vector2d_t(std::cos(angle), std::sin(angle));
249 
250  ctx->set_source(opts.patPath);
251  ctx->move_to(q0[0], q0[1]);
252  ectx.arc(c[0], r, alpha[0], alpha[0] + soln.s[0]);
253  ectx.rel_line_to(v);
254  ectx.arc_negative(c[1], r, alpha[1], alpha[1] - soln.s[2]);
255  ctx->stroke();
256  }
257 
258  static void drawRSL(const Vector3d_t& q0, const Format_t r,
259  const Path_t& soln, const DrawOpts& opts) {
260  if (!soln.f) return;
261 
262  Cairo::RefPtr<Cairo::Context> ctx = opts.ctx;
263  gtk::EigenCairo ectx(ctx);
264 
265  Vector3d_t q1 = Integrate_t::R(q0, r, soln.s[0]);
266  Vector3d_t q2 = Integrate_t::S(q1, soln.s[1]);
267 
268  Vector2d_t c[2];
269  c[0] = rightCenter(q0, r);
270  c[1] = leftCenter(q2, r);
271 
272  Vector3d_t alpha;
273  alpha[0] = rightAngleOf(q0);
274  alpha[1] = leftAngleOf(q2);
275 
276  if (opts.drawBalls) {
277  ctx->set_dash(opts.dash, opts.dashOffExtra);
278  ctx->set_source(opts.patR);
279  ectx.circle(c[0], r);
280  ctx->stroke();
281  ctx->set_source(opts.patL);
282  ectx.circle(c[1], r);
283  ctx->stroke();
284  ctx->unset_dash();
285  }
286 
287  // get the angle of the straight segment
288  Format_t angle = rightAngle_inv(alpha[0] - soln.s[0]);
289 
290  // create a vector
291  Vector2d_t v = soln.s[1] * Vector2d_t(std::cos(angle), std::sin(angle));
292 
293  ctx->set_source(opts.patPath);
294  ctx->move_to(q0[0], q0[1]);
295  ectx.arc_negative(c[0], r, alpha[0], alpha[0] - soln.s[0]);
296  ectx.rel_line_to(v);
297  ectx.arc(c[1], r, alpha[1], alpha[1] + soln.s[2]);
298  ctx->stroke();
299  }
300 
301  static void drawLS(const Vector3d_t& q0, const Format_t r, const Path_t& soln,
302  const DrawOpts& opts) {
303  if (!soln.f) return;
304 
305  Cairo::RefPtr<Cairo::Context> ctx = opts.ctx;
306  gtk::EigenCairo ectx(ctx);
307 
308  Vector3d_t q1 = Integrate_t::L(q0, r, soln.s[0]);
309  Vector3d_t q2 = Integrate_t::S(q1, soln.s[1]);
310 
311  Vector2d_t c = leftCenter(q0, r);
312 
313  if (opts.drawBalls) {
314  ctx->set_dash(opts.dash, opts.dashOffExtra);
315  ctx->set_source(opts.patL);
316  ectx.circle(c, r);
317  ctx->stroke();
318  ctx->unset_dash();
319  }
320 
321  ctx->set_source(opts.patPath);
322 
323  Format_t a1 = leftAngleOf(q0);
324  ctx->move_to(q0[0], q0[1]);
325  ectx.arc(c, r, a1, a1 + soln.s[0]);
326  Vector2d_t v = soln.s[1] * Vector2d_t(std::cos(q1[2]), std::sin(q1[2]));
327  ectx.rel_line_to(v);
328  ctx->stroke();
329  }
330 
331  static void drawRS(const Vector3d_t& q0, const Format_t r, const Path_t& soln,
332  const DrawOpts& opts) {
333  if (!soln.f) return;
334 
335  Cairo::RefPtr<Cairo::Context> ctx = opts.ctx;
336  gtk::EigenCairo ectx(ctx);
337 
338  Vector3d_t q1 = Integrate_t::R(q0, r, soln.s[0]);
339  Vector3d_t q2 = Integrate_t::S(q1, soln.s[1]);
340 
341  Vector2d_t c = rightCenter(q0, r);
342 
343  if (opts.drawBalls) {
344  ctx->set_dash(opts.dash, opts.dashOffExtra);
345  ctx->set_source(opts.patR);
346  ectx.circle(c, r);
347  ctx->stroke();
348  ctx->unset_dash();
349  }
350 
351  ctx->set_source(opts.patPath);
352 
353  Format_t a1 = rightAngleOf(q0);
354  ctx->move_to(q0[0], q0[1]);
355  ectx.arc_negative(c, r, a1, a1 - soln.s[0]);
356 
357  Vector2d_t v = soln.d1 * Vector2d_t(std::cos(q1[2]), std::sin(q1[2]));
358  ectx.rel_line_to(v);
359  ctx->stroke();
360  }
361 
363  static void draw(const Vector3d_t& q0, const Format_t r, const Path_t& soln,
364  const DrawOpts& opts) {
365  switch (soln.id) {
366  case LRLa:
367  case LRLb:
368  drawLRL(q0, r, soln, opts);
369  return;
370 
371  case RLRa:
372  case RLRb:
373  drawRLR(q0, r, soln, opts);
374  return;
375 
376  case LSL:
377  drawLSL(q0, r, soln, opts);
378  return;
379 
380  case RSR:
381  drawRSR(q0, r, soln, opts);
382  return;
383 
384  case LSR:
385  drawLSR(q0, r, soln, opts);
386  return;
387 
388  case RSL:
389  drawRSL(q0, r, soln, opts);
390  return;
391  }
392  }
393 };
394 
395 template <typename Format_t>
396 void draw(const Eigen::Matrix<Format_t, 3, 1>& q0, const Format_t r,
397  const Path<Format_t>& soln, const DrawOpts& opts) {
398  GenericArtist<Format_t>::draw(q0, r, soln, opts);
399 }
400 
401 } // curves_eigen
402 } // dubins
403 } // mpblocks
404 
405 #endif // MPBLOCKS_DUBINS_CURVES_EIGEN_GENERICARTIST_HPP_
mpblocks::dubins::Path::f
bool f
is feasible
Definition: path.h:46
mpblocks::dubins::curves_eigen::GenericArtist::drawRSR
static void drawRSR(const Vector3d_t &q0, const Format_t r, const Path_t &soln, const DrawOpts &opts)
Definition: generic_artist.hpp:173
mpblocks::dubins::curves_eigen::DrawOpts::patR
Cairo::RefPtr< Cairo::Pattern > patR
Definition: draw_opts.h:38
mpblocks::dubins::curves_eigen::GenericArtist::drawRS
static void drawRS(const Vector3d_t &q0, const Format_t r, const Path_t &soln, const DrawOpts &opts)
Definition: generic_artist.hpp:331
mpblocks::gtk::EigenCairo::arc
void arc(const Eigen::MatrixBase< Derived > &c, double radius, double angle1, double angle2)
Definition: EigenCairo.hpp:77
mpblocks::dubins::curves_eigen::GenericArtist::Vector2d_t
Eigen::Matrix< Format_t, 2, 1 > Vector2d_t
Definition: generic_artist.hpp:40
mpblocks::dubins::curves_eigen::leftAngle_inv
Scalar leftAngle_inv(const Scalar alpha)
Definition: funcs.hpp:97
mpblocks::dubins::curves_eigen::leftAngleOf
Scalar leftAngleOf(const Scalar q_theta)
return the angle of the vector from the center of the counter clockwise (left) circle coincident to q...
Definition: funcs.hpp:87
mpblocks::dubins::curves_eigen::GenericArtist::drawRSL
static void drawRSL(const Vector3d_t &q0, const Format_t r, const Path_t &soln, const DrawOpts &opts)
Definition: generic_artist.hpp:258
mpblocks::dubins::curves_eigen::Integrate::R
static Vector3d R(const Vector3d &q0, const Scalar r, const Scalar arc)
Definition: integrator.h:54
mpblocks::dubins::curves_eigen::GenericArtist::Path_t
Path< Format_t > Path_t
Definition: generic_artist.hpp:42
mpblocks::dubins::curves_eigen::DrawOpts::ctx
Cairo::RefPtr< Cairo::Context > ctx
Definition: draw_opts.h:41
mpblocks::dubins::Path::s
Vector3d s
lengths of each segment, how it's interpreted depends on id
Definition: path.h:47
mpblocks::dubins::curves_eigen::DrawOpts::drawBalls
bool drawBalls
whether or not to draw balls
Definition: draw_opts.h:49
mpblocks::dubins::curves_eigen::DrawOpts::patPath
Cairo::RefPtr< Cairo::Pattern > patPath
Definition: draw_opts.h:40
mpblocks::dubins::curves_eigen::Integrate::L
static Vector3d L(const Vector3d &q0, const Scalar r, const Scalar arc)
Definition: integrator.h:44
mpblocks::dubins::curves_eigen::GenericArtist::drawLS
static void drawLS(const Vector3d_t &q0, const Format_t r, const Path_t &soln, const DrawOpts &opts)
Definition: generic_artist.hpp:301
mpblocks::dubins::curves_eigen::draw
void draw(const Eigen::Matrix< Format_t, 3, 1 > &q0, const Format_t r, const Path< Format_t > &soln, const DrawOpts &opts)
Definition: generic_artist.hpp:396
mpblocks::dubins::Path::id
int id
identifies the type of path
Definition: path.h:45
mpblocks::dubins::curves_eigen::GenericArtist::draw
static void draw(const Vector3d_t &q0, const Format_t r, const Path_t &soln, const DrawOpts &opts)
dispatch the artist
Definition: generic_artist.hpp:363
mpblocks::dubins::curves_eigen::rightAngle_inv
Scalar rightAngle_inv(const Scalar alpha)
Definition: funcs.hpp:117
mpblocks::dubins::curves_eigen::rightAngleOf
Scalar rightAngleOf(const Scalar q_theta)
return the angle of the vector from the center of the clockwise (right) circle coincident to q...
Definition: funcs.hpp:107
mpblocks::dubins::curves_eigen::DrawOpts::patL
Cairo::RefPtr< Cairo::Pattern > patL
Definition: draw_opts.h:37
mpblocks::dubins::curves_eigen::DrawOpts::dash
std::vector< double > dash
Definition: draw_opts.h:56
mpblocks::dubins::curves_eigen::GenericArtist::Integrate_t
Integrate< Format_t > Integrate_t
Definition: generic_artist.hpp:41
mpblocks::gtk::EigenCairo::arc_negative
void arc_negative(const Eigen::MatrixBase< Derived > &c, double radius, double angle1, double angle2)
Definition: EigenCairo.hpp:83
mpblocks::dubins::curves_eigen::GenericArtist::drawRLR
static void drawRLR(const Vector3d_t &q0, const Format_t r, const Path_t &soln, const DrawOpts &opts)
Definition: generic_artist.hpp:87
mpblocks::dubins::curves_eigen::rightCenter
Eigen::Matrix< Scalar, 2, 1 > rightCenter(const Eigen::Matrix< Scalar, 3, 1 > &q, Scalar r)
return the center of a clockwise (right) circle coincident to q with radius r
Definition: funcs.hpp:76
mpblocks::dubins::curves_eigen::Integrate::S
static Vector3d S(const Vector3d &q0, const Scalar d1)
Definition: integrator.h:64
mpblocks::dubins::curves_eigen::Integrate
Definition: integrator.h:39
mpblocks::dubins::curves_eigen::GenericArtist::Vector3d_t
Eigen::Matrix< Format_t, 3, 1 > Vector3d_t
Definition: generic_artist.hpp:39
mpblocks::dubins::Path
Encodes a dubins path primitive, which is three connected arc segments.
Definition: path.h:42
mpblocks::dubins::curves_eigen::GenericArtist::drawLSR
static void drawLSR(const Vector3d_t &q0, const Format_t r, const Path_t &soln, const DrawOpts &opts)
Definition: generic_artist.hpp:215
mpblocks::dubins::curves_eigen::GenericArtist::drawLSL
static void drawLSL(const Vector3d_t &q0, const Format_t r, const Path_t &soln, const DrawOpts &opts)
Definition: generic_artist.hpp:130
mpblocks::dubins::curves_eigen::DrawOpts
encapsulates patterns for drawing
Definition: draw_opts.h:36
mpblocks::dubins::curves_eigen::leftCenter
Eigen::Matrix< Scalar, 2, 1 > leftCenter(const Eigen::Matrix< Scalar, 3, 1 > &q, Scalar r)
return the center of a counter clockwise (left) circle coincident to q with radius r ...
Definition: funcs.hpp:54
mpblocks::dubins::curves_eigen::GenericArtist
interface for different solutions
Definition: generic_artist.hpp:38
mpblocks::dubins::curves_eigen::DrawOpts::dashOffExtra
double dashOffExtra
Definition: draw_opts.h:46
mpblocks::gtk::EigenCairo::circle
void circle(const Eigen::MatrixBase< Derived > &c, double r)
Definition: EigenCairo.hpp:115
mpblocks::dubins::curves_eigen::GenericArtist::drawLRL
static void drawLRL(const Vector3d_t &q0, const Format_t r, const Path_t soln, const DrawOpts &opts)
Definition: generic_artist.hpp:44
mpblocks::gtk::EigenCairo
wrapper for cairo context which adds methods allowing for eigen vector typed points ...
Definition: EigenCairo.h:38
mpblocks::gtk::EigenCairo::rel_line_to
void rel_line_to(const Eigen::MatrixBase< Derived > &v)
Definition: EigenCairo.hpp:94