cc00_reeds_shepp_state_space.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Copyright (c) 2017 - for information on the respective copyright
3 * owner see the NOTICE file and/or the repository
4 *
5 * https://github.com/hbanzhaf/steering_functions.git
6 *
7 * Licensed under the Apache License, Version 2.0 (the "License");
8 * you may not use this file except in compliance with the License.
9 * You may obtain a copy of the License at
10 *
11 * http://www.apache.org/licenses/LICENSE-2.0
12 *
13 * Unless required by applicable law or agreed to in writing, software
14 * distributed under the License is distributed on an "AS IS" BASIS,
15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
16 * implied. See the License for the specific language governing
17 * permissions and limitations under the License.
18 
19 * This source code is derived from Continuous Curvature (CC) Steer.
20 * Copyright (c) 2016, Thierry Fraichard and Institut national de
21 * recherche en informatique et en automatique (Inria), licensed under
22 * the BSD license, cf. 3rd-party-licenses.txt file in the root
23 * directory of this source tree.
24 **********************************************************************/
25 
26 #include <cmath>
27 #include <limits>
28 
29 #include "steering_functions/hc_cc_state_space/cc00_reeds_shepp_state_space.hpp"
30 #include "steering_functions/hc_cc_state_space/configuration.hpp"
31 #include "steering_functions/utilities/utilities.hpp"
32 
33 #define CC_REGULAR false
34 
35 using namespace std;
36 
37 namespace steering
38 {
39 
40 class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp
41 {
42 private:
43  CC00_Reeds_Shepp_State_Space *parent_;
44 
45 public:
46  explicit CC00_Reeds_Shepp(CC00_Reeds_Shepp_State_Space *parent)
47  {
48  parent_ = parent;
49  }
50 
51  double distance = 0.0;
52  double angle = 0.0;
53 
54  // ##### TT ###################################################################
55  bool TT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
56  {
57  if (c1.left == c2.left)
58  {
59  return false;
60  }
61  if (c1.forward == c2.forward)
62  {
63  return false;
64  }
65  return fabs(distance - 2 * c1.radius) < get_epsilon();
66  }
67 
68  void TT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q) const
69  {
70  double x = (c1.xc + c2.xc) / 2;
71  double y = (c1.yc + c2.yc) / 2;
72  double angle = atan2(c2.yc - c1.yc, c2.xc - c1.xc);
73  double theta;
74  if (c1.left)
75  {
76  if (c1.forward)
77  {
78  theta = angle + HALF_PI - c1.mu;
79  }
80  else
81  {
82  theta = angle + HALF_PI + c1.mu;
83  }
84  }
85  else
86  {
87  if (c1.forward)
88  {
89  theta = angle - HALF_PI + c1.mu;
90  }
91  else
92  {
93  theta = angle - HALF_PI - c1.mu;
94  }
95  }
96  *q = new Configuration(x, y, theta, 0);
97  }
98 
99  double TT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q) const
100  {
101  TT_tangent_circles(c1, c2, q);
102  return c1.cc_turn_length(**q) + c2.cc_turn_length(**q);
103  }
104 
105  // ##### TcT ##################################################################
106  bool TcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
107  {
108  if (c1.left == c2.left)
109  {
110  return false;
111  }
112  if (c1.forward != c2.forward)
113  {
114  return false;
115  }
116  return fabs(distance - 2 * c1.radius * c1.cos_mu) < get_epsilon();
117  }
118 
119  void TcT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q) const
120  {
121  double distance = center_distance(c1, c2);
122  double delta_x = 0.5 * distance;
123  double delta_y = sqrt(pow(c1.radius, 2) - pow(delta_x, 2));
124  double angle = atan2(c2.yc - c1.yc, c2.xc - c1.xc);
125  double x, y, theta;
126  if (c1.left)
127  {
128  if (c1.forward)
129  {
130  theta = angle + HALF_PI;
131  global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y);
132  }
133  else
134  {
135  theta = angle + HALF_PI;
136  global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y);
137  }
138  }
139  else
140  {
141  if (c1.forward)
142  {
143  theta = angle - HALF_PI;
144  global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y);
145  }
146  else
147  {
148  theta = angle - HALF_PI;
149  global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y);
150  }
151  }
152  *q = new Configuration(x, y, theta, 0);
153  }
154 
155  double TcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q) const
156  {
157  TcT_tangent_circles(c1, c2, q);
158  return c1.cc_turn_length(**q) + c2.cc_turn_length(**q);
159  }
160 
161  // ##### Reeds-Shepp families: ################################################
162 
163  // ##### TcTcT ################################################################
164  bool TcTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
165  {
166  if (c1.left != c2.left)
167  {
168  return false;
169  }
170  if (c1.forward == c2.forward)
171  {
172  return false;
173  }
174  return distance <= 4 * c1.radius * c1.cos_mu;
175  }
176 
177  void TcTcT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
178  Configuration **q3, Configuration **q4) const
179  {
180  double theta = angle;
181  double r = 2 * c1.radius * c1.cos_mu;
182  double delta_x = 0.5 * distance;
183  double delta_y = sqrt(pow(r, 2) - pow(delta_x, 2));
184  double x, y;
185 
186  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
187  HC_CC_Circle tgt1(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
188  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
189  HC_CC_Circle tgt2(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
190 
191  TcT_tangent_circles(c1, tgt1, q1);
192  TcT_tangent_circles(tgt1, c2, q2);
193  TcT_tangent_circles(c1, tgt2, q3);
194  TcT_tangent_circles(tgt2, c2, q4);
195  }
196 
197  double TcTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
198  HC_CC_Circle **ci) const
199  {
200  Configuration *qa, *qb, *qc, *qd;
201  TcTcT_tangent_circles(c1, c2, &qa, &qb, &qc, &qd);
202  HC_CC_Circle *middle1, *middle2;
203  middle1 = new HC_CC_Circle(*qa, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
204  middle2 = new HC_CC_Circle(*qc, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
205 
206  // select shortest connection
207  double length1 = c1.cc_turn_length(*qa) + middle1->cc_turn_length(*qb) + c2.cc_turn_length(*qb);
208  double length2 = c1.cc_turn_length(*qc) + middle2->cc_turn_length(*qd) + c2.cc_turn_length(*qd);
209  if (length1 < length2)
210  {
211  *q1 = qa;
212  *q2 = qb;
213  *ci = middle1;
214  delete qc;
215  delete qd;
216  delete middle2;
217  return length1;
218  }
219  else
220  {
221  *q1 = qc;
222  *q2 = qd;
223  *ci = middle2;
224  delete qa;
225  delete qb;
226  delete middle1;
227  return length2;
228  }
229  return numeric_limits<double>::max();
230  }
231 
232  // ##### TcTT #################################################################
233  bool TcTT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
234  {
235  if (c1.left != c2.left)
236  {
237  return false;
238  }
239  if (c1.forward != c2.forward)
240  {
241  return false;
242  }
243  return (distance <= 2 * c1.radius * (1 + c1.cos_mu)) && (distance >= 2 * c1.radius * (1 - c1.cos_mu));
244  }
245 
246  void TcTT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
247  Configuration **q3, Configuration **q4) const
248  {
249  double theta = angle;
250  double r1 = 2 * c1.radius * c1.cos_mu;
251  double r2 = 2 * c1.radius;
252  double delta_x = (pow(r1, 2) + pow(distance, 2) - pow(r2, 2)) / (2 * distance);
253  double delta_y = sqrt(pow(r1, 2) - pow(delta_x, 2));
254  double x, y;
255 
256  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
257  HC_CC_Circle tgt1(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
258  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
259  HC_CC_Circle tgt2(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
260 
261  TcT_tangent_circles(c1, tgt1, q1);
262  TT_tangent_circles(tgt1, c2, q2);
263  TcT_tangent_circles(c1, tgt2, q3);
264  TT_tangent_circles(tgt2, c2, q4);
265  }
266 
267  double TcTT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
268  HC_CC_Circle **ci) const
269  {
270  Configuration *qa, *qb, *qc, *qd;
271  TcTT_tangent_circles(c1, c2, &qa, &qb, &qc, &qd);
272  HC_CC_Circle *middle1, *middle2;
273  middle1 = new HC_CC_Circle(*qa, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
274  middle2 = new HC_CC_Circle(*qc, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
275 
276  // select shortest connection
277  double length1 = c1.cc_turn_length(*qa) + middle1->cc_turn_length(*qb) + c2.cc_turn_length(*qb);
278  double length2 = c1.cc_turn_length(*qc) + middle2->cc_turn_length(*qd) + c2.cc_turn_length(*qd);
279  if (length1 < length2)
280  {
281  *q1 = qa;
282  *q2 = qb;
283  *ci = middle1;
284  delete qc;
285  delete qd;
286  delete middle2;
287  return length1;
288  }
289  else
290  {
291  *q1 = qc;
292  *q2 = qd;
293  *ci = middle2;
294  delete qa;
295  delete qb;
296  delete middle1;
297  return length2;
298  }
299  return numeric_limits<double>::max();
300  }
301 
302  // ##### TTcT #################################################################
303  bool TTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
304  {
305  if (c1.left != c2.left)
306  {
307  return false;
308  }
309  if (c1.forward != c2.forward)
310  {
311  return false;
312  }
313  return (distance <= 2 * c1.radius * (1 + c1.cos_mu)) && (distance >= 2 * c1.radius * (1 - c1.cos_mu));
314  }
315 
316  void TTcT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
317  Configuration **q3, Configuration **q4) const
318  {
319  double theta = angle;
320  double r1 = 2 * c1.radius;
321  double r2 = 2 * c1.radius * c1.cos_mu;
322  double delta_x = (pow(r1, 2) + pow(distance, 2) - pow(r2, 2)) / (2 * distance);
323  double delta_y = sqrt(pow(r1, 2) - pow(delta_x, 2));
324  double x, y;
325 
326  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
327  HC_CC_Circle tgt1(x, y, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
328  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
329  HC_CC_Circle tgt2(x, y, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
330 
331  TT_tangent_circles(c1, tgt1, q1);
332  TcT_tangent_circles(tgt1, c2, q2);
333  TT_tangent_circles(c1, tgt2, q3);
334  TcT_tangent_circles(tgt2, c2, q4);
335  }
336 
337  double TTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
338  HC_CC_Circle **ci) const
339  {
340  Configuration *qa, *qb, *qc, *qd;
341  TTcT_tangent_circles(c1, c2, &qa, &qb, &qc, &qd);
342  HC_CC_Circle *middle1, *middle2;
343  middle1 = new HC_CC_Circle(*qa, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
344  middle2 = new HC_CC_Circle(*qc, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
345 
346  // select shortest connection
347  double length1 = c1.cc_turn_length(*qa) + middle1->cc_turn_length(*qb) + c2.cc_turn_length(*qb);
348  double length2 = c1.cc_turn_length(*qc) + middle2->cc_turn_length(*qd) + c2.cc_turn_length(*qd);
349  if (length1 < length2)
350  {
351  *q1 = qa;
352  *q2 = qb;
353  *ci = middle1;
354  delete qc;
355  delete qd;
356  delete middle2;
357  return length1;
358  }
359  else
360  {
361  *q1 = qc;
362  *q2 = qd;
363  *ci = middle2;
364  delete qa;
365  delete qb;
366  delete middle1;
367  return length2;
368  }
369  return numeric_limits<double>::max();
370  }
371 
372  // ##### TST ##################################################################
373  bool TiST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
374  {
375  if (c1.left == c2.left)
376  {
377  return false;
378  }
379  if (c1.forward == c2.forward)
380  {
381  return false;
382  }
383  return (distance >= 2 * c1.radius);
384  }
385 
386  bool TeST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
387  {
388  if (c1.left != c2.left)
389  {
390  return false;
391  }
392  if (c1.forward == c2.forward)
393  {
394  return false;
395  }
396 
397  return (distance >= 2 * c1.radius * c1.sin_mu);
398  }
399 
400  bool TST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
401  {
402  return TiST_exists(c1, c2) || TeST_exists(c1, c2);
403  }
404 
405  void TiST_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1,
406  Configuration **q2) const
407  {
408  double distance = center_distance(c1, c2);
409  double angle = atan2(c2.yc - c1.yc, c2.xc - c1.xc);
410  double alpha = asin(2 * c1.radius * c1.cos_mu / distance);
411  double delta_x = c1.radius * c1.sin_mu;
412  double delta_y = c1.radius * c1.cos_mu;
413  double x, y, theta;
414  if (c1.left && c1.forward)
415  {
416  theta = angle + alpha;
417  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
418  *q1 = new Configuration(x, y, theta, 0);
419  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
420  *q2 = new Configuration(x, y, theta, 0);
421  }
422  if (c1.left && !c1.forward)
423  {
424  theta = angle - alpha;
425  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
426  *q1 = new Configuration(x, y, theta + PI, 0);
427  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
428  *q2 = new Configuration(x, y, theta + PI, 0);
429  }
430  if (!c1.left && c1.forward)
431  {
432  theta = angle - alpha;
433  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
434  *q1 = new Configuration(x, y, theta, 0);
435  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
436  *q2 = new Configuration(x, y, theta, 0);
437  }
438  if (!c1.left && !c1.forward)
439  {
440  theta = angle + alpha;
441  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
442  *q1 = new Configuration(x, y, theta + PI, 0);
443  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
444  *q2 = new Configuration(x, y, theta + PI, 0);
445  }
446  }
447 
448  void TeST_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1,
449  Configuration **q2) const
450  {
451  double delta_x = c1.radius * c1.sin_mu;
452  double delta_y = c1.radius * c1.cos_mu;
453  double theta = atan2(c2.yc - c1.yc, c2.xc - c1.xc);
454  double x, y;
455  if (c1.left && c1.forward)
456  {
457  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
458  *q1 = new Configuration(x, y, theta, 0);
459  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
460  *q2 = new Configuration(x, y, theta, 0);
461  }
462  if (c1.left && !c1.forward)
463  {
464  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
465  *q1 = new Configuration(x, y, theta + PI, 0);
466  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
467  *q2 = new Configuration(x, y, theta + PI, 0);
468  }
469  if (!c1.left && c1.forward)
470  {
471  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
472  *q1 = new Configuration(x, y, theta, 0);
473  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
474  *q2 = new Configuration(x, y, theta, 0);
475  }
476  if (!c1.left && !c1.forward)
477  {
478  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
479  *q1 = new Configuration(x, y, theta + PI, 0);
480  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
481  *q2 = new Configuration(x, y, theta + PI, 0);
482  }
483  }
484 
485  double TiST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
486  {
487  TiST_tangent_circles(c1, c2, q1, q2);
488  return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + c2.cc_turn_length(**q2);
489  }
490 
491  double TeST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
492  {
493  TeST_tangent_circles(c1, c2, q1, q2);
494  return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + c2.cc_turn_length(**q2);
495  }
496 
497  double TST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
498  {
499  if (TiST_exists(c1, c2))
500  {
501  return TiST_path(c1, c2, q1, q2);
502  }
503  if (TeST_exists(c1, c2))
504  {
505  return TeST_path(c1, c2, q1, q2);
506  }
507  return numeric_limits<double>::max();
508  }
509 
510  // ##### TSTcT ################################################################
511  bool TiSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
512  {
513  if (c1.left != c2.left)
514  {
515  return false;
516  }
517  if (c1.forward != c2.forward)
518  {
519  return false;
520  }
521  return (distance >= 2 * c1.radius * sqrt(1 + 2 * c1.sin_mu * c1.cos_mu + pow(c1.cos_mu, 2)));
522  }
523 
524  bool TeSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
525  {
526  if (c1.left == c2.left)
527  {
528  return false;
529  }
530  if (c1.forward != c2.forward)
531  {
532  return false;
533  }
534  return (distance >= 2 * c1.radius * (c1.cos_mu + c1.sin_mu));
535  }
536 
537  bool TSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
538  {
539  return TiSTcT_exists(c1, c2) || TeSTcT_exists(c1, c2);
540  }
541 
542  double TiSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
543  Configuration **q3, HC_CC_Circle **ci) const
544  {
545  double theta = angle;
546  double r = c2.radius * c2.cos_mu;
547  double delta_y = pow(2 * r, 2) / distance;
548  double delta_x = 2 * r * sqrt(1 - delta_y / distance);
549  double x, y;
550 
551  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
552  HC_CC_Circle tgt1(x, y, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
553 
554  TiST_tangent_circles(c1, tgt1, q1, q2);
555  TcT_tangent_circles(tgt1, c2, q3);
556 
557  *ci = new HC_CC_Circle(**q2, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
558 
559  return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + (*ci)->cc_turn_length(**q3) +
560  c2.cc_turn_length(**q3);
561  }
562 
563  double TeSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
564  Configuration **q3, HC_CC_Circle **ci) const
565  {
566  double theta = angle;
567  double delta_x = 2 * c2.radius * c2.cos_mu;
568  double delta_y = 0;
569  double x, y;
570 
571  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
572  HC_CC_Circle tgt1(x, y, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
573 
574  TeST_tangent_circles(c1, tgt1, q1, q2);
575  TcT_tangent_circles(tgt1, c2, q3);
576 
577  *ci = new HC_CC_Circle(**q2, c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
578 
579  return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + (*ci)->cc_turn_length(**q3) +
580  c2.cc_turn_length(**q3);
581  }
582 
583  double TSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
584  Configuration **q3, HC_CC_Circle **ci) const
585  {
586  if (TiSTcT_exists(c1, c2))
587  {
588  return TiSTcT_path(c1, c2, q1, q2, q3, ci);
589  }
590  if (TeSTcT_exists(c1, c2))
591  {
592  return TeSTcT_path(c1, c2, q1, q2, q3, ci);
593  }
594  return numeric_limits<double>::max();
595  }
596 
597  // ##### TcTST ################################################################
598  bool TcTiST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
599  {
600  if (c1.left != c2.left)
601  {
602  return false;
603  }
604  if (c1.forward != c2.forward)
605  {
606  return false;
607  }
608  return (distance >= 2 * c1.radius * sqrt(1 + 2 * c1.sin_mu * c1.cos_mu + pow(c1.cos_mu, 2)));
609  }
610 
611  bool TcTeST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
612  {
613  if (c1.left == c2.left)
614  {
615  return false;
616  }
617  if (c1.forward != c2.forward)
618  {
619  return false;
620  }
621  return (distance >= 2 * c1.radius * (c1.cos_mu + c1.sin_mu));
622  }
623 
624  bool TcTST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
625  {
626  return TcTiST_exists(c1, c2) || TcTeST_exists(c1, c2);
627  }
628 
629  double TcTiST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
630  Configuration **q3, HC_CC_Circle **ci) const
631  {
632  double theta = angle;
633  double r = c1.radius * c1.cos_mu;
634  double delta_y = pow(2 * r, 2) / distance;
635  double delta_x = 2 * r * sqrt(1 - delta_y / distance);
636  double x, y;
637 
638  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
639  HC_CC_Circle tgt1(x, y, !c2.left, !c2.forward, c2.regular, parent_->hc_cc_circle_param_);
640 
641  TcT_tangent_circles(c1, tgt1, q1);
642  TiST_tangent_circles(tgt1, c2, q2, q3);
643 
644  *ci = new HC_CC_Circle(**q1, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
645 
646  return c1.cc_turn_length(**q1) + (*ci)->cc_turn_length(**q2) + configuration_distance(**q2, **q3) +
647  c2.cc_turn_length(**q3);
648  }
649 
650  double TcTeST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
651  Configuration **q3, HC_CC_Circle **ci) const
652  {
653  double theta = angle;
654  double delta_x = 2 * c2.radius * c2.cos_mu;
655  double delta_y = 0;
656  double x, y;
657 
658  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
659  HC_CC_Circle tgt1(x, y, c2.left, !c2.forward, c2.regular, parent_->hc_cc_circle_param_);
660 
661  TcT_tangent_circles(c1, tgt1, q1);
662  TeST_tangent_circles(tgt1, c2, q2, q3);
663 
664  *ci = new HC_CC_Circle(**q1, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
665 
666  return c1.cc_turn_length(**q1) + (*ci)->cc_turn_length(**q2) + configuration_distance(**q2, **q3) +
667  c2.cc_turn_length(**q3);
668  }
669 
670  double TcTST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
671  Configuration **q3, HC_CC_Circle **ci) const
672  {
673  if (TcTiST_exists(c1, c2))
674  {
675  return TcTiST_path(c1, c2, q1, q2, q3, ci);
676  }
677  if (TcTeST_exists(c1, c2))
678  {
679  return TcTeST_path(c1, c2, q1, q2, q3, ci);
680  }
681  return numeric_limits<double>::max();
682  }
683 
684  // ##### TcTSTcT ##############################################################
685  bool TcTiSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
686  {
687  if (c1.left == c2.left)
688  {
689  return false;
690  }
691  if (c1.forward == c2.forward)
692  {
693  return false;
694  }
695  return (distance >= 2 * c1.radius * sqrt(1 + 4 * c1.cos_mu * c1.sin_mu + 4 * pow(c1.cos_mu, 2)));
696  }
697 
698  bool TcTeSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
699  {
700  if (c1.left != c2.left)
701  {
702  return false;
703  }
704  if (c1.forward == c2.forward)
705  {
706  return false;
707  }
708  return (distance >= 2 * c1.radius * (2 * c1.cos_mu + c1.sin_mu));
709  }
710 
711  bool TcTSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
712  {
713  return TcTiSTcT_exists(c1, c2) || TcTeSTcT_exists(c1, c2);
714  }
715 
716  double TcTiSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
717  Configuration **q3, Configuration **q4, HC_CC_Circle **ci1, HC_CC_Circle **ci2) const
718  {
719  double theta = angle;
720  double r = c1.radius * c1.cos_mu;
721  double delta_y = pow(2 * r, 2) / distance;
722  double delta_x = 2 * r * sqrt(1 - delta_y / distance);
723  double x, y;
724 
725  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
726  HC_CC_Circle tgt1(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
727  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
728  HC_CC_Circle tgt2(x, y, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
729 
730  TcT_tangent_circles(c1, tgt1, q1);
731  TiST_tangent_circles(tgt1, tgt2, q2, q3);
732  TcT_tangent_circles(tgt2, c2, q4);
733 
734  *ci1 = new HC_CC_Circle(**q1, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
735  *ci2 = new HC_CC_Circle(**q3, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
736 
737  return c1.cc_turn_length(**q1) + (*ci1)->cc_turn_length(**q2) + configuration_distance(**q2, **q3) +
738  (*ci2)->cc_turn_length(**q4) + c2.cc_turn_length(**q4);
739  }
740 
741  double TcTeSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
742  Configuration **q3, Configuration **q4, HC_CC_Circle **ci1, HC_CC_Circle **ci2) const
743  {
744  double theta = angle;
745  double delta_x = 2 * c1.radius * c1.cos_mu;
746  double delta_y = 0;
747  double x, y;
748 
749  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
750  HC_CC_Circle tgt1(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
751  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
752  HC_CC_Circle tgt2(x, y, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
753 
754  TcT_tangent_circles(c1, tgt1, q1);
755  TeST_tangent_circles(tgt1, tgt2, q2, q3);
756  TcT_tangent_circles(tgt2, c2, q4);
757 
758  *ci1 = new HC_CC_Circle(**q1, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
759  *ci2 = new HC_CC_Circle(**q3, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
760 
761  return c1.cc_turn_length(**q1) + (*ci1)->cc_turn_length(**q2) + configuration_distance(**q2, **q3) +
762  (*ci2)->cc_turn_length(**q4) + c2.cc_turn_length(**q4);
763  }
764 
765  double TcTSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
766  Configuration **q3, Configuration **q4, HC_CC_Circle **ci1, HC_CC_Circle **ci2) const
767  {
768  if (TcTiSTcT_exists(c1, c2))
769  {
770  return TcTiSTcT_path(c1, c2, q1, q2, q3, q4, ci1, ci2);
771  }
772  if (TcTeSTcT_exists(c1, c2))
773  {
774  return TcTeSTcT_path(c1, c2, q1, q2, q3, q4, ci1, ci2);
775  }
776  return numeric_limits<double>::max();
777  }
778 
779  // ##### TTcTT ###############################################################
780  bool TTcTT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
781  {
782  if (c1.left == c2.left)
783  {
784  return false;
785  }
786  if (c1.forward != c2.forward)
787  {
788  return false;
789  }
790  return (distance <= 2 * c1.radius * (c1.cos_mu + 2));
791  }
792 
793  void TTcTT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
794  Configuration **q3, Configuration **q4, Configuration **q5, Configuration **q6) const
795  {
796  double theta = angle;
797  double r1, r2, delta_x, delta_y, x, y;
798  r1 = 2 * c1.radius * c1.cos_mu;
799  r2 = 2 * c1.radius;
800  if (distance < 2 * c1.radius * (-c1.cos_mu + 2))
801  {
802  delta_x = (distance + r1) / 2;
803  delta_y = sqrt(pow(r2, 2) - pow(delta_x, 2));
804  }
805  else
806  {
807  delta_x = (distance - r1) / 2;
808  delta_y = sqrt(pow(r2, 2) - pow(delta_x, 2));
809  }
810 
811  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
812  HC_CC_Circle tgt1(x, y, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
813  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
814  HC_CC_Circle tgt2(x, y, !c2.left, !c2.forward, c2.regular, parent_->hc_cc_circle_param_);
815 
816  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
817  HC_CC_Circle tgt3(x, y, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
818  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
819  HC_CC_Circle tgt4(x, y, !c2.left, !c2.forward, c2.regular, parent_->hc_cc_circle_param_);
820 
821  TT_tangent_circles(c1, tgt1, q1);
822  TcT_tangent_circles(tgt1, tgt2, q2);
823  TT_tangent_circles(tgt2, c2, q3);
824 
825  TT_tangent_circles(c1, tgt3, q4);
826  TcT_tangent_circles(tgt3, tgt4, q5);
827  TT_tangent_circles(tgt4, c2, q6);
828  }
829 
830  double TTcTT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
831  Configuration **q3, HC_CC_Circle **ci1, HC_CC_Circle **ci2) const
832  {
833  Configuration *qa, *qb, *qc, *qd, *qe, *qf;
834  TTcTT_tangent_circles(c1, c2, &qa, &qb, &qc, &qd, &qe, &qf);
835  HC_CC_Circle *middle1, *middle2, *middle3, *middle4;
836  middle1 = new HC_CC_Circle(*qa, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
837  middle2 = new HC_CC_Circle(*qb, !c2.left, !c2.forward, c2.regular, parent_->hc_cc_circle_param_);
838  middle3 = new HC_CC_Circle(*qd, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
839  middle4 = new HC_CC_Circle(*qe, !c2.left, !c2.forward, c2.regular, parent_->hc_cc_circle_param_);
840 
841  // select shortest connection
842  double length1 =
843  c1.cc_turn_length(*qa) + middle1->cc_turn_length(*qb) + middle2->cc_turn_length(*qc) + c2.cc_turn_length(*qc);
844  double length2 =
845  c1.cc_turn_length(*qd) + middle3->cc_turn_length(*qe) + middle4->cc_turn_length(*qf) + c2.cc_turn_length(*qf);
846  if (length1 < length2)
847  {
848  *q1 = qa;
849  *q2 = qb;
850  *q3 = qc;
851  *ci1 = middle1;
852  *ci2 = middle2;
853  delete qd;
854  delete qe;
855  delete qf;
856  delete middle3;
857  delete middle4;
858  return length1;
859  }
860  else
861  {
862  *q1 = qd;
863  *q2 = qe;
864  *q3 = qf;
865  *ci1 = middle3;
866  *ci2 = middle4;
867  delete qa;
868  delete qb;
869  delete qc;
870  delete middle1;
871  delete middle2;
872  return length2;
873  }
874  return numeric_limits<double>::max();
875  }
876 
877  // ##### TcTTcT ###############################################################
878  bool TcTTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
879  {
880  if (c1.left == c2.left)
881  {
882  return false;
883  }
884  if (c1.forward == c2.forward)
885  {
886  return false;
887  }
888  return (distance <= 2 * c1.radius * (2 * c1.cos_mu + 1)) && (distance >= 2 * c1.radius * (2 * c1.cos_mu - 1));
889  }
890 
891  void TcTTcT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
892  Configuration **q3, Configuration **q4, Configuration **q5, Configuration **q6) const
893  {
894  double theta = angle;
895  double r1 = 2 * c1.radius * c1.cos_mu;
896  double r2 = c1.radius;
897  double delta_x = (pow(r1, 2) + pow(distance / 2, 2) - pow(r2, 2)) / distance;
898  double delta_y = sqrt(pow(r1, 2) - pow(delta_x, 2));
899  double x, y;
900 
901  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
902  HC_CC_Circle tgt1(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
903  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
904  HC_CC_Circle tgt2(x, y, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
905 
906  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
907  HC_CC_Circle tgt3(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
908  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
909  HC_CC_Circle tgt4(x, y, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
910 
911  TcT_tangent_circles(c1, tgt1, q1);
912  TT_tangent_circles(tgt1, tgt2, q2);
913  TcT_tangent_circles(tgt2, c2, q3);
914 
915  TcT_tangent_circles(c1, tgt3, q4);
916  TT_tangent_circles(tgt3, tgt4, q5);
917  TcT_tangent_circles(tgt4, c2, q6);
918  }
919 
920  double TcTTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
921  Configuration **q3, HC_CC_Circle **ci1, HC_CC_Circle **ci2) const
922  {
923  Configuration *qa, *qb, *qc, *qd, *qe, *qf;
924  TcTTcT_tangent_circles(c1, c2, &qa, &qb, &qc, &qd, &qe, &qf);
925  HC_CC_Circle *middle1, *middle2, *middle3, *middle4;
926  middle1 = new HC_CC_Circle(*qa, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
927  middle2 = new HC_CC_Circle(*qb, c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
928  middle3 = new HC_CC_Circle(*qd, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
929  middle4 = new HC_CC_Circle(*qe, c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
930 
931  // select shortest connection
932  double length1 =
933  c1.cc_turn_length(*qa) + middle1->cc_turn_length(*qb) + middle2->cc_turn_length(*qc) + c2.cc_turn_length(*qc);
934  double length2 =
935  c1.cc_turn_length(*qd) + middle3->cc_turn_length(*qe) + middle4->cc_turn_length(*qf) + c2.cc_turn_length(*qf);
936  if (length1 < length2)
937  {
938  *q1 = qa;
939  *q2 = qb;
940  *q3 = qc;
941  *ci1 = middle1;
942  *ci2 = middle2;
943  delete qd;
944  delete qe;
945  delete qf;
946  delete middle3;
947  delete middle4;
948  return length1;
949  }
950  else
951  {
952  *q1 = qd;
953  *q2 = qe;
954  *q3 = qf;
955  *ci1 = middle3;
956  *ci2 = middle4;
957  delete qa;
958  delete qb;
959  delete qc;
960  delete middle1;
961  delete middle2;
962  return length2;
963  }
964  return numeric_limits<double>::max();
965  }
966 
967  // ############################################################################
968 
969  // ##### TTT ##################################################################
970  bool TTT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
971  {
972  if (c1.left != c2.left)
973  {
974  return false;
975  }
976  if (c1.forward == c2.forward)
977  {
978  return false;
979  }
980  return distance <= 4 * c1.radius;
981  }
982 
983  void TTT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
984  Configuration **q3, Configuration **q4) const
985  {
986  double theta = angle;
987  double r = 2 * c1.radius;
988  double delta_x = 0.5 * distance;
989  double delta_y = sqrt(pow(r, 2) - pow(delta_x, 2));
990  double x, y;
991 
992  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
993  HC_CC_Circle tgt1(x, y, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
994  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
995  HC_CC_Circle tgt2(x, y, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
996 
997  TT_tangent_circles(c1, tgt1, q1);
998  TT_tangent_circles(tgt1, c2, q2);
999  TT_tangent_circles(c1, tgt2, q3);
1000  TT_tangent_circles(tgt2, c2, q4);
1001  }
1002 
1003  double TTT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
1004  HC_CC_Circle **ci) const
1005  {
1006  Configuration *qa, *qb, *qc, *qd;
1007  TTT_tangent_circles(c1, c2, &qa, &qb, &qc, &qd);
1008  HC_CC_Circle *middle1, *middle2;
1009  middle1 = new HC_CC_Circle(*qa, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
1010  middle2 = new HC_CC_Circle(*qc, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
1011 
1012  // select shortest connection
1013  double length1 = c1.cc_turn_length(*qa) + middle1->cc_turn_length(*qb) + c2.cc_turn_length(*qb);
1014  double length2 = c1.cc_turn_length(*qc) + middle2->cc_turn_length(*qd) + c2.cc_turn_length(*qd);
1015  if (length1 < length2)
1016  {
1017  *q1 = qa;
1018  *q2 = qb;
1019  *ci = middle1;
1020  delete qc;
1021  delete qd;
1022  delete middle2;
1023  return length1;
1024  }
1025  else
1026  {
1027  *q1 = qc;
1028  *q2 = qd;
1029  *ci = middle2;
1030  delete qa;
1031  delete qb;
1032  delete middle1;
1033  return length2;
1034  }
1035  return numeric_limits<double>::max();
1036  }
1037 
1038  // ##### TcST ################################################################
1039  bool TciST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1040  {
1041  if (c1.left == c2.left)
1042  {
1043  return false;
1044  }
1045  if (c1.forward != c2.forward)
1046  {
1047  return false;
1048  }
1049  return distance >= 2 * c1.radius * c1.cos_mu;
1050  }
1051 
1052  bool TceST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1053  {
1054  if (c1.left != c2.left)
1055  {
1056  return false;
1057  }
1058  if (c1.forward != c2.forward)
1059  {
1060  return false;
1061  }
1062  return distance >= get_epsilon();
1063  }
1064 
1065  bool TcST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1066  {
1067  return TciST_exists(c1, c2) || TceST_exists(c1, c2);
1068  }
1069 
1070  double TciST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
1071  {
1072  double alpha = asin(2 * c1.radius * c1.cos_mu / distance);
1073  double delta_x = c1.radius * c1.sin_mu;
1074  double delta_y = c1.radius * c1.cos_mu;
1075  double x, y, theta;
1076  if (c1.left && c1.forward)
1077  {
1078  theta = angle - alpha;
1079  global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y);
1080  *q1 = new Configuration(x, y, theta + PI, 0);
1081  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
1082  *q2 = new Configuration(x, y, theta + PI, 0);
1083  }
1084  if (c1.left && !c1.forward)
1085  {
1086  theta = angle + alpha;
1087  global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y);
1088  *q1 = new Configuration(x, y, theta, 0);
1089  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
1090  *q2 = new Configuration(x, y, theta, 0);
1091  }
1092  if (!c1.left && c1.forward)
1093  {
1094  theta = angle + alpha;
1095  global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y);
1096  *q1 = new Configuration(x, y, theta + PI, 0);
1097  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
1098  *q2 = new Configuration(x, y, theta + PI, 0);
1099  }
1100  if (!c1.left && !c1.forward)
1101  {
1102  theta = angle - alpha;
1103  global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y);
1104  *q1 = new Configuration(x, y, theta, 0);
1105  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
1106  *q2 = new Configuration(x, y, theta, 0);
1107  }
1108  return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + c2.cc_turn_length(**q2);
1109  }
1110 
1111  double TceST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
1112  {
1113  double theta = angle;
1114  double delta_x = c1.radius * c1.sin_mu;
1115  double delta_y = c1.radius * c1.cos_mu;
1116  double x, y;
1117  if (c1.left && c1.forward)
1118  {
1119  global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y);
1120  *q1 = new Configuration(x, y, theta + PI, 0);
1121  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
1122  *q2 = new Configuration(x, y, theta + PI, 0);
1123  }
1124  if (c1.left && !c1.forward)
1125  {
1126  global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y);
1127  *q1 = new Configuration(x, y, theta, 0);
1128  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
1129  *q2 = new Configuration(x, y, theta, 0);
1130  }
1131  if (!c1.left && c1.forward)
1132  {
1133  global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y);
1134  *q1 = new Configuration(x, y, theta + PI, 0);
1135  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
1136  *q2 = new Configuration(x, y, theta + PI, 0);
1137  }
1138  if (!c1.left && !c1.forward)
1139  {
1140  global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y);
1141  *q1 = new Configuration(x, y, theta, 0);
1142  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
1143  *q2 = new Configuration(x, y, theta, 0);
1144  }
1145  return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + c2.cc_turn_length(**q2);
1146  }
1147 
1148  double TcST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
1149  {
1150  if (TciST_exists(c1, c2))
1151  {
1152  return TciST_path(c1, c2, q1, q2);
1153  }
1154  if (TceST_exists(c1, c2))
1155  {
1156  return TceST_path(c1, c2, q1, q2);
1157  }
1158  return numeric_limits<double>::max();
1159  }
1160 
1161  // ##### TScT #################################################################
1162  bool TiScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1163  {
1164  if (c1.left == c2.left)
1165  {
1166  return false;
1167  }
1168  if (c1.forward != c2.forward)
1169  {
1170  return false;
1171  }
1172  return distance >= 2 * c1.radius * c1.cos_mu;
1173  }
1174 
1175  bool TeScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1176  {
1177  if (c1.left != c2.left)
1178  {
1179  return false;
1180  }
1181  if (c1.forward != c2.forward)
1182  {
1183  return false;
1184  }
1185  return distance >= get_epsilon();
1186  }
1187 
1188  bool TScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1189  {
1190  return TiScT_exists(c1, c2) || TeScT_exists(c1, c2);
1191  }
1192 
1193  double TiScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
1194  {
1195  double alpha = asin(2 * c1.radius * c1.cos_mu / distance);
1196  double delta_x = c1.radius * c1.sin_mu;
1197  double delta_y = c1.radius * c1.cos_mu;
1198  double x, y, theta;
1199  if (c1.left && c1.forward)
1200  {
1201  theta = angle + alpha;
1202  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
1203  *q1 = new Configuration(x, y, theta, 0);
1204  global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y);
1205  *q2 = new Configuration(x, y, theta, 0);
1206  }
1207  if (c1.left && !c1.forward)
1208  {
1209  theta = angle - alpha;
1210  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
1211  *q1 = new Configuration(x, y, theta + PI, 0);
1212  global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y);
1213  *q2 = new Configuration(x, y, theta + PI, 0);
1214  }
1215  if (!c1.left && c1.forward)
1216  {
1217  theta = angle - alpha;
1218  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
1219  *q1 = new Configuration(x, y, theta, 0);
1220  global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y);
1221  *q2 = new Configuration(x, y, theta, 0);
1222  }
1223  if (!c1.left && !c1.forward)
1224  {
1225  theta = angle + alpha;
1226  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
1227  *q1 = new Configuration(x, y, theta + PI, 0);
1228  global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y);
1229  *q2 = new Configuration(x, y, theta + PI, 0);
1230  }
1231  return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + c2.cc_turn_length(**q2);
1232  }
1233 
1234  double TeScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
1235  {
1236  double theta = angle;
1237  double delta_x = c1.radius * c1.sin_mu;
1238  double delta_y = c1.radius * c1.cos_mu;
1239  double x, y;
1240  if (c1.left && c1.forward)
1241  {
1242  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
1243  *q1 = new Configuration(x, y, theta, 0);
1244  global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y);
1245  *q2 = new Configuration(x, y, theta, 0);
1246  }
1247  if (c1.left && !c1.forward)
1248  {
1249  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
1250  *q1 = new Configuration(x, y, theta + PI, 0);
1251  global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y);
1252  *q2 = new Configuration(x, y, theta + PI, 0);
1253  }
1254  if (!c1.left && c1.forward)
1255  {
1256  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
1257  *q1 = new Configuration(x, y, theta, 0);
1258  global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y);
1259  *q2 = new Configuration(x, y, theta, 0);
1260  }
1261  if (!c1.left && !c1.forward)
1262  {
1263  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
1264  *q1 = new Configuration(x, y, theta + PI, 0);
1265  global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y);
1266  *q2 = new Configuration(x, y, theta + PI, 0);
1267  }
1268  return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + c2.cc_turn_length(**q2);
1269  }
1270 
1271  double TScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
1272  {
1273  if (TiScT_exists(c1, c2))
1274  {
1275  return TiScT_path(c1, c2, q1, q2);
1276  }
1277  if (TeScT_exists(c1, c2))
1278  {
1279  return TeScT_path(c1, c2, q1, q2);
1280  }
1281  return numeric_limits<double>::max();
1282  }
1283 
1284  // ##### TcScT ################################################################
1285  bool TciScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1286  {
1287  if (c1.left == c2.left)
1288  {
1289  return false;
1290  }
1291  if (c1.forward == c2.forward)
1292  {
1293  return false;
1294  }
1295  return distance >= 2 * c1.radius * c1.cos_mu;
1296  }
1297 
1298  bool TceScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1299  {
1300  if (c1.left != c2.left)
1301  {
1302  return false;
1303  }
1304  if (c1.forward == c2.forward)
1305  {
1306  return false;
1307  }
1308  return distance >= get_epsilon();
1309  }
1310 
1311  bool TcScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1312  {
1313  return TciScT_exists(c1, c2) || TceScT_exists(c1, c2);
1314  }
1315 
1316  double TciScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
1317  {
1318  double alpha = asin(2 * c1.radius * c1.cos_mu / distance);
1319  double delta_x = c1.radius * c1.sin_mu;
1320  double delta_y = c1.radius * c1.cos_mu;
1321  double x, y, theta;
1322  if (c1.left && c1.forward)
1323  {
1324  theta = angle - alpha;
1325  global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y);
1326  *q1 = new Configuration(x, y, theta + PI, 0);
1327  global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y);
1328  *q2 = new Configuration(x, y, theta + PI, 0);
1329  }
1330  if (c1.left && !c1.forward)
1331  {
1332  theta = angle + alpha;
1333  global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y);
1334  *q1 = new Configuration(x, y, theta, 0);
1335  global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y);
1336  *q2 = new Configuration(x, y, theta, 0);
1337  }
1338  if (!c1.left && c1.forward)
1339  {
1340  theta = angle + alpha;
1341  global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y);
1342  *q1 = new Configuration(x, y, theta + PI, 0);
1343  global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y);
1344  *q2 = new Configuration(x, y, theta + PI, 0);
1345  }
1346  if (!c1.left && !c1.forward)
1347  {
1348  theta = angle - alpha;
1349  global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y);
1350  *q1 = new Configuration(x, y, theta, 0);
1351  global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y);
1352  *q2 = new Configuration(x, y, theta, 0);
1353  }
1354  return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + c2.cc_turn_length(**q2);
1355  }
1356 
1357  double TceScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
1358  {
1359  double theta = angle;
1360  double delta_x = c1.radius * c1.sin_mu;
1361  double delta_y = c1.radius * c1.cos_mu;
1362  double x, y;
1363  if (c1.left && c1.forward)
1364  {
1365  global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y);
1366  *q1 = new Configuration(x, y, theta + PI, 0);
1367  global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y);
1368  *q2 = new Configuration(x, y, theta + PI, 0);
1369  }
1370  if (c1.left && !c1.forward)
1371  {
1372  global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y);
1373  *q1 = new Configuration(x, y, theta, 0);
1374  global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y);
1375  *q2 = new Configuration(x, y, theta, 0);
1376  }
1377  if (!c1.left && c1.forward)
1378  {
1379  global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y);
1380  *q1 = new Configuration(x, y, theta + PI, 0);
1381  global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y);
1382  *q2 = new Configuration(x, y, theta + PI, 0);
1383  }
1384  if (!c1.left && !c1.forward)
1385  {
1386  global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y);
1387  *q1 = new Configuration(x, y, theta, 0);
1388  global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y);
1389  *q2 = new Configuration(x, y, theta, 0);
1390  }
1391  return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + c2.cc_turn_length(**q2);
1392  }
1393 
1394  double TcScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
1395  {
1396  if (TciScT_exists(c1, c2))
1397  {
1398  return TciScT_path(c1, c2, q1, q2);
1399  }
1400  if (TceScT_exists(c1, c2))
1401  {
1402  return TceScT_path(c1, c2, q1, q2);
1403  }
1404  return numeric_limits<double>::max();
1405  }
1406 };
1407 
1408 // ############################################################################
1409 
1410 CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp_State_Space(double kappa, double sigma, double discretization)
1411  : HC_CC_State_Space(kappa, sigma, discretization)
1412  , cc00_reeds_shepp_{ unique_ptr<CC00_Reeds_Shepp>(new CC00_Reeds_Shepp(this)) }
1413 {
1414 }
1415 
1416 CC00_Reeds_Shepp_State_Space::~CC00_Reeds_Shepp_State_Space() = default;
1417 
1418 HC_CC_RS_Path *CC00_Reeds_Shepp_State_Space::cc00_circles_rs_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1419 {
1420  // table containing the lengths of the paths, the intermediate configurations and circles
1421  double length[nb_hc_cc_rs_paths];
1422  double_array_init(length, nb_hc_cc_rs_paths, numeric_limits<double>::max());
1423  Configuration *qi1[nb_hc_cc_rs_paths];
1424  pointer_array_init((void **)qi1, nb_hc_cc_rs_paths);
1425  Configuration *qi2[nb_hc_cc_rs_paths];
1426  pointer_array_init((void **)qi2, nb_hc_cc_rs_paths);
1427  Configuration *qi3[nb_hc_cc_rs_paths];
1428  pointer_array_init((void **)qi3, nb_hc_cc_rs_paths);
1429  Configuration *qi4[nb_hc_cc_rs_paths];
1430  pointer_array_init((void **)qi4, nb_hc_cc_rs_paths);
1431  HC_CC_Circle *cstart[nb_hc_cc_rs_paths];
1432  pointer_array_init((void **)cstart, nb_hc_cc_rs_paths);
1433  HC_CC_Circle *ci1[nb_hc_cc_rs_paths];
1434  pointer_array_init((void **)ci1, nb_hc_cc_rs_paths);
1435  HC_CC_Circle *ci2[nb_hc_cc_rs_paths];
1436  pointer_array_init((void **)ci2, nb_hc_cc_rs_paths);
1437  HC_CC_Circle *cend[nb_hc_cc_rs_paths];
1438  pointer_array_init((void **)cend, nb_hc_cc_rs_paths);
1439 
1440  // precomputations
1441  cc00_reeds_shepp_->distance = center_distance(c1, c2);
1442  cc00_reeds_shepp_->angle = atan2(c2.yc - c1.yc, c2.xc - c1.xc);
1443 
1444  // case E
1445  if (configuration_equal(c1.start, c2.start))
1446  {
1447  length[hc_cc_rs::E] = 0;
1448  goto label_end;
1449  }
1450  // case S forwards
1451  if (configuration_aligned(c1.start, c2.start))
1452  {
1453  length[hc_cc_rs::S] = configuration_distance(c1.start, c2.start);
1454  goto label_end;
1455  }
1456  // case S backwards
1457  if (configuration_aligned(c2.start, c1.start))
1458  {
1459  length[hc_cc_rs::S] = configuration_distance(c2.start, c1.start);
1460  goto label_end;
1461  }
1462  // case T
1463  if (configuration_on_hc_cc_circle(c1, c2.start))
1464  {
1465  cstart[hc_cc_rs::T] = new HC_CC_Circle(c1);
1466  length[hc_cc_rs::T] = cstart[hc_cc_rs::T]->cc_turn_length(c2.start);
1467  goto label_end;
1468  }
1469  // case TT
1470  if (cc00_reeds_shepp_->TT_exists(c1, c2))
1471  {
1472  cstart[hc_cc_rs::TT] = new HC_CC_Circle(c1);
1473  cend[hc_cc_rs::TT] = new HC_CC_Circle(c2);
1474  length[hc_cc_rs::TT] = cc00_reeds_shepp_->TT_path(*cstart[hc_cc_rs::TT], *cend[hc_cc_rs::TT], &qi1[hc_cc_rs::TT]);
1475  }
1476  // case TcT
1477  if (cc00_reeds_shepp_->TcT_exists(c1, c2))
1478  {
1479  cstart[hc_cc_rs::TcT] = new HC_CC_Circle(c1);
1480  cend[hc_cc_rs::TcT] = new HC_CC_Circle(c2);
1481  length[hc_cc_rs::TcT] =
1482  cc00_reeds_shepp_->TcT_path(*cstart[hc_cc_rs::TcT], *cend[hc_cc_rs::TcT], &qi1[hc_cc_rs::TcT]);
1483  }
1484  // ##### Reeds-Shepp families: ############################################
1485  // case TcTcT
1486  if (cc00_reeds_shepp_->TcTcT_exists(c1, c2))
1487  {
1488  cstart[hc_cc_rs::TcTcT] = new HC_CC_Circle(c1);
1489  cend[hc_cc_rs::TcTcT] = new HC_CC_Circle(c2);
1490  length[hc_cc_rs::TcTcT] =
1491  cc00_reeds_shepp_->TcTcT_path(*cstart[hc_cc_rs::TcTcT], *cend[hc_cc_rs::TcTcT], &qi1[hc_cc_rs::TcTcT],
1492  &qi2[hc_cc_rs::TcTcT], &ci1[hc_cc_rs::TcTcT]);
1493  }
1494  // case TcTT
1495  if (cc00_reeds_shepp_->TcTT_exists(c1, c2))
1496  {
1497  cstart[hc_cc_rs::TcTT] = new HC_CC_Circle(c1);
1498  cend[hc_cc_rs::TcTT] = new HC_CC_Circle(c2);
1499  length[hc_cc_rs::TcTT] =
1500  cc00_reeds_shepp_->TcTT_path(*cstart[hc_cc_rs::TcTT], *cend[hc_cc_rs::TcTT], &qi1[hc_cc_rs::TcTT],
1501  &qi2[hc_cc_rs::TcTT], &ci1[hc_cc_rs::TcTT]);
1502  }
1503  // case TTcT
1504  if (cc00_reeds_shepp_->TTcT_exists(c1, c2))
1505  {
1506  cstart[hc_cc_rs::TTcT] = new HC_CC_Circle(c1);
1507  cend[hc_cc_rs::TTcT] = new HC_CC_Circle(c2);
1508  length[hc_cc_rs::TTcT] =
1509  cc00_reeds_shepp_->TTcT_path(*cstart[hc_cc_rs::TTcT], *cend[hc_cc_rs::TTcT], &qi1[hc_cc_rs::TTcT],
1510  &qi2[hc_cc_rs::TTcT], &ci1[hc_cc_rs::TTcT]);
1511  }
1512  // case TST
1513  if (cc00_reeds_shepp_->TST_exists(c1, c2))
1514  {
1515  cstart[hc_cc_rs::TST] = new HC_CC_Circle(c1);
1516  cend[hc_cc_rs::TST] = new HC_CC_Circle(c2);
1517  length[hc_cc_rs::TST] = cc00_reeds_shepp_->TST_path(*cstart[hc_cc_rs::TST], *cend[hc_cc_rs::TST],
1518  &qi1[hc_cc_rs::TST], &qi2[hc_cc_rs::TST]);
1519  }
1520  // case TSTcT
1521  if (cc00_reeds_shepp_->TSTcT_exists(c1, c2))
1522  {
1523  cstart[hc_cc_rs::TSTcT] = new HC_CC_Circle(c1);
1524  cend[hc_cc_rs::TSTcT] = new HC_CC_Circle(c2);
1525  length[hc_cc_rs::TSTcT] =
1526  cc00_reeds_shepp_->TSTcT_path(*cstart[hc_cc_rs::TSTcT], *cend[hc_cc_rs::TSTcT], &qi1[hc_cc_rs::TSTcT],
1527  &qi2[hc_cc_rs::TSTcT], &qi3[hc_cc_rs::TSTcT], &ci1[hc_cc_rs::TSTcT]);
1528  }
1529  // case TcTST
1530  if (cc00_reeds_shepp_->TcTST_exists(c1, c2))
1531  {
1532  cstart[hc_cc_rs::TcTST] = new HC_CC_Circle(c1);
1533  cend[hc_cc_rs::TcTST] = new HC_CC_Circle(c2);
1534  length[hc_cc_rs::TcTST] =
1535  cc00_reeds_shepp_->TcTST_path(*cstart[hc_cc_rs::TcTST], *cend[hc_cc_rs::TcTST], &qi1[hc_cc_rs::TcTST],
1536  &qi2[hc_cc_rs::TcTST], &qi3[hc_cc_rs::TcTST], &ci1[hc_cc_rs::TcTST]);
1537  }
1538  // case TcTSTcT
1539  if (cc00_reeds_shepp_->TcTSTcT_exists(c1, c2))
1540  {
1541  cstart[hc_cc_rs::TcTSTcT] = new HC_CC_Circle(c1);
1542  cend[hc_cc_rs::TcTSTcT] = new HC_CC_Circle(c2);
1543  length[hc_cc_rs::TcTSTcT] = cc00_reeds_shepp_->TcTSTcT_path(
1544  *cstart[hc_cc_rs::TcTSTcT], *cend[hc_cc_rs::TcTSTcT], &qi1[hc_cc_rs::TcTSTcT], &qi2[hc_cc_rs::TcTSTcT],
1545  &qi3[hc_cc_rs::TcTSTcT], &qi4[hc_cc_rs::TcTSTcT], &ci1[hc_cc_rs::TcTSTcT], &ci2[hc_cc_rs::TcTSTcT]);
1546  }
1547  // case TTcTT
1548  if (cc00_reeds_shepp_->TTcTT_exists(c1, c2))
1549  {
1550  cstart[hc_cc_rs::TTcTT] = new HC_CC_Circle(c1);
1551  cend[hc_cc_rs::TTcTT] = new HC_CC_Circle(c2);
1552  length[hc_cc_rs::TTcTT] = cc00_reeds_shepp_->TTcTT_path(
1553  *cstart[hc_cc_rs::TTcTT], *cend[hc_cc_rs::TTcTT], &qi1[hc_cc_rs::TTcTT], &qi2[hc_cc_rs::TTcTT],
1554  &qi3[hc_cc_rs::TTcTT], &ci1[hc_cc_rs::TTcTT], &ci2[hc_cc_rs::TTcTT]);
1555  }
1556  // case TcTTcT
1557  if (cc00_reeds_shepp_->TcTTcT_exists(c1, c2))
1558  {
1559  cstart[hc_cc_rs::TcTTcT] = new HC_CC_Circle(c1);
1560  cend[hc_cc_rs::TcTTcT] = new HC_CC_Circle(c2);
1561  length[hc_cc_rs::TcTTcT] = cc00_reeds_shepp_->TcTTcT_path(
1562  *cstart[hc_cc_rs::TcTTcT], *cend[hc_cc_rs::TcTTcT], &qi1[hc_cc_rs::TcTTcT], &qi2[hc_cc_rs::TcTTcT],
1563  &qi3[hc_cc_rs::TcTTcT], &ci1[hc_cc_rs::TcTTcT], &ci2[hc_cc_rs::TcTTcT]);
1564  }
1565  // ############################################################################
1566  // case TTT
1567  if (cc00_reeds_shepp_->TTT_exists(c1, c2))
1568  {
1569  cstart[hc_cc_rs::TTT] = new HC_CC_Circle(c1);
1570  cend[hc_cc_rs::TTT] = new HC_CC_Circle(c2);
1571  length[hc_cc_rs::TTT] = cc00_reeds_shepp_->TTT_path(*cstart[hc_cc_rs::TTT], *cend[hc_cc_rs::TTT],
1572  &qi1[hc_cc_rs::TTT], &qi2[hc_cc_rs::TTT], &ci1[hc_cc_rs::TTT]);
1573  }
1574  // case TcST
1575  if (cc00_reeds_shepp_->TcST_exists(c1, c2))
1576  {
1577  cstart[hc_cc_rs::TcST] = new HC_CC_Circle(c1);
1578  cend[hc_cc_rs::TcST] = new HC_CC_Circle(c2);
1579  length[hc_cc_rs::TcST] = cc00_reeds_shepp_->TcST_path(*cstart[hc_cc_rs::TcST], *cend[hc_cc_rs::TcST],
1580  &qi1[hc_cc_rs::TcST], &qi2[hc_cc_rs::TcST]);
1581  }
1582  // case TScT
1583  if (cc00_reeds_shepp_->TScT_exists(c1, c2))
1584  {
1585  cstart[hc_cc_rs::TScT] = new HC_CC_Circle(c1);
1586  cend[hc_cc_rs::TScT] = new HC_CC_Circle(c2);
1587  length[hc_cc_rs::TScT] = cc00_reeds_shepp_->TScT_path(*cstart[hc_cc_rs::TScT], *cend[hc_cc_rs::TScT],
1588  &qi1[hc_cc_rs::TScT], &qi2[hc_cc_rs::TScT]);
1589  }
1590  // case TcScT
1591  if (cc00_reeds_shepp_->TcScT_exists(c1, c2))
1592  {
1593  cstart[hc_cc_rs::TcScT] = new HC_CC_Circle(c1);
1594  cend[hc_cc_rs::TcScT] = new HC_CC_Circle(c2);
1595  length[hc_cc_rs::TcScT] = cc00_reeds_shepp_->TcScT_path(*cstart[hc_cc_rs::TcScT], *cend[hc_cc_rs::TcScT],
1596  &qi1[hc_cc_rs::TcScT], &qi2[hc_cc_rs::TcScT]);
1597  }
1598 label_end:
1599  // select shortest path
1600  hc_cc_rs::path_type best_path = (hc_cc_rs::path_type)array_index_min(length, nb_hc_cc_rs_paths);
1601  HC_CC_RS_Path *path;
1602  path = new HC_CC_RS_Path(c1.start, c2.start, best_path, kappa_, sigma_, qi1[best_path], qi2[best_path],
1603  qi3[best_path], qi4[best_path], cstart[best_path], cend[best_path], ci1[best_path],
1604  ci2[best_path], length[best_path]);
1605 
1606  // clean up
1607  for (int i = 0; i < nb_hc_cc_rs_paths; i++)
1608  {
1609  if (i != best_path)
1610  {
1611  delete qi1[i];
1612  delete qi2[i];
1613  delete qi3[i];
1614  delete qi4[i];
1615  delete cstart[i];
1616  delete ci1[i];
1617  delete ci2[i];
1618  delete cend[i];
1619  }
1620  }
1621  return path;
1622 }
1623 
1624 HC_CC_RS_Path *CC00_Reeds_Shepp_State_Space::cc00_reeds_shepp(const State &state1, const State &state2) const
1625 {
1626  // compute the 4 circles at the intial and final configuration
1627  Configuration start(state1.x, state1.y, state1.theta, 0.0);
1628  Configuration end(state2.x, state2.y, state2.theta, 0.0);
1629 
1630  HC_CC_Circle *start_circle[4];
1631  HC_CC_Circle *end_circle[4];
1632  start_circle[0] = new HC_CC_Circle(start, true, true, CC_REGULAR, hc_cc_circle_param_);
1633  start_circle[1] = new HC_CC_Circle(start, false, true, CC_REGULAR, hc_cc_circle_param_);
1634  start_circle[2] = new HC_CC_Circle(start, true, false, CC_REGULAR, hc_cc_circle_param_);
1635  start_circle[3] = new HC_CC_Circle(start, false, false, CC_REGULAR, hc_cc_circle_param_);
1636  end_circle[0] = new HC_CC_Circle(end, true, true, CC_REGULAR, hc_cc_circle_param_);
1637  end_circle[1] = new HC_CC_Circle(end, false, true, CC_REGULAR, hc_cc_circle_param_);
1638  end_circle[2] = new HC_CC_Circle(end, true, false, CC_REGULAR, hc_cc_circle_param_);
1639  end_circle[3] = new HC_CC_Circle(end, false, false, CC_REGULAR, hc_cc_circle_param_);
1640 
1641  // compute the shortest path for the 16 combinations (4 circles at the beginning and 4 at the end)
1642  HC_CC_RS_Path *path[] = { nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr,
1643  nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr };
1644 
1645  double lg[] = { numeric_limits<double>::max(), numeric_limits<double>::max(), numeric_limits<double>::max(),
1646  numeric_limits<double>::max(), numeric_limits<double>::max(), numeric_limits<double>::max(),
1647  numeric_limits<double>::max(), numeric_limits<double>::max(), numeric_limits<double>::max(),
1648  numeric_limits<double>::max(), numeric_limits<double>::max(), numeric_limits<double>::max(),
1649  numeric_limits<double>::max(), numeric_limits<double>::max(), numeric_limits<double>::max(),
1650  numeric_limits<double>::max() };
1651 
1652  for (int i = 0; i < 4; i++)
1653  {
1654  for (int j = 0; j < 4; j++)
1655  {
1656  path[4 * i + j] = cc00_circles_rs_path(*start_circle[i], *end_circle[j]);
1657  if (path[4 * i + j])
1658  {
1659  lg[4 * i + j] = path[4 * i + j]->length;
1660  }
1661  }
1662  }
1663 
1664  // select shortest path
1665  int best_path = array_index_min(lg, 16);
1666 
1667  // // display calculations
1668  // cout << "CC00_Reeds_Shepp_State_Space" << endl;
1669  // for (int i = 0; i < 16; i++)
1670  // {
1671  // cout << i << ": ";
1672  // if (path[i])
1673  // {
1674  // path[i]->print(true);
1675  // cout << endl;
1676  // }
1677  // }
1678  // cout << "shortest path: " << (int)best_path << endl;
1679  // path[best_path]->print(true);
1680 
1681  // clean up
1682  for (int i = 0; i < 4; i++)
1683  {
1684  delete start_circle[i];
1685  delete end_circle[i];
1686  }
1687  for (int i = 0; i < 16; i++)
1688  {
1689  if (i != best_path)
1690  {
1691  delete path[i];
1692  }
1693  }
1694  return path[best_path];
1695 }
1696 
1697 double CC00_Reeds_Shepp_State_Space::get_distance(const State &state1, const State &state2) const
1698 {
1699  HC_CC_RS_Path *p = this->cc00_reeds_shepp(state1, state2);
1700  double length = p->length;
1701  delete p;
1702  return length;
1703 }
1704 
1705 vector<Control> CC00_Reeds_Shepp_State_Space::get_controls(const State &state1, const State &state2) const
1706 {
1707  vector<Control> cc_rs_controls;
1708  cc_rs_controls.reserve(13);
1709  HC_CC_RS_Path *p = this->cc00_reeds_shepp(state1, state2);
1710  switch (p->type)
1711  {
1712  case hc_cc_rs::E:
1713  empty_controls(cc_rs_controls);
1714  break;
1715  case hc_cc_rs::S:
1716  straight_controls(p->start, p->end, cc_rs_controls);
1717  break;
1718  case hc_cc_rs::T:
1719  cc_turn_controls(*(p->cstart), p->end, true, cc_rs_controls);
1720  break;
1721  case hc_cc_rs::TT:
1722  case hc_cc_rs::TcT:
1723  cc_turn_controls(*(p->cstart), *(p->qi1), true, cc_rs_controls);
1724  cc_turn_controls(*(p->cend), *(p->qi1), false, cc_rs_controls);
1725  break;
1726  // ##### Reeds-Shepp families: ############################################
1727  case hc_cc_rs::TcTcT:
1728  case hc_cc_rs::TcTT:
1729  case hc_cc_rs::TTcT:
1730  cc_turn_controls(*(p->cstart), *(p->qi1), true, cc_rs_controls);
1731  cc_turn_controls(*(p->ci1), *(p->qi2), true, cc_rs_controls);
1732  cc_turn_controls(*(p->cend), *(p->qi2), false, cc_rs_controls);
1733  break;
1734  case hc_cc_rs::TST:
1735  cc_turn_controls(*(p->cstart), *(p->qi1), true, cc_rs_controls);
1736  straight_controls(*(p->qi1), *(p->qi2), cc_rs_controls);
1737  cc_turn_controls(*(p->cend), *(p->qi2), false, cc_rs_controls);
1738  break;
1739  case hc_cc_rs::TSTcT:
1740  cc_turn_controls(*(p->cstart), *(p->qi1), true, cc_rs_controls);
1741  straight_controls(*(p->qi1), *(p->qi2), cc_rs_controls);
1742  cc_turn_controls(*(p->ci1), *(p->qi3), true, cc_rs_controls);
1743  cc_turn_controls(*(p->cend), *(p->qi3), false, cc_rs_controls);
1744  break;
1745  case hc_cc_rs::TcTST:
1746  cc_turn_controls(*(p->cstart), *(p->qi1), true, cc_rs_controls);
1747  cc_turn_controls(*(p->ci1), *(p->qi2), true, cc_rs_controls);
1748  straight_controls(*(p->qi2), *(p->qi3), cc_rs_controls);
1749  cc_turn_controls(*(p->cend), *(p->qi3), false, cc_rs_controls);
1750  break;
1751  case hc_cc_rs::TcTSTcT:
1752  cc_turn_controls(*(p->cstart), *(p->qi1), true, cc_rs_controls);
1753  cc_turn_controls(*(p->ci1), *(p->qi2), true, cc_rs_controls);
1754  straight_controls(*(p->qi2), *(p->qi3), cc_rs_controls);
1755  cc_turn_controls(*(p->ci2), *(p->qi4), true, cc_rs_controls);
1756  cc_turn_controls(*(p->cend), *(p->qi4), false, cc_rs_controls);
1757  break;
1758  case hc_cc_rs::TTcTT:
1759  case hc_cc_rs::TcTTcT:
1760  cc_turn_controls(*(p->cstart), *(p->qi1), true, cc_rs_controls);
1761  cc_turn_controls(*(p->ci1), *(p->qi2), true, cc_rs_controls);
1762  cc_turn_controls(*(p->ci2), *(p->qi3), true, cc_rs_controls);
1763  cc_turn_controls(*(p->cend), *(p->qi3), false, cc_rs_controls);
1764  break;
1765  // ########################################################################
1766  case hc_cc_rs::TTT:
1767  cc_turn_controls(*(p->cstart), *(p->qi1), true, cc_rs_controls);
1768  cc_turn_controls(*(p->ci1), *(p->qi2), true, cc_rs_controls);
1769  cc_turn_controls(*(p->cend), *(p->qi2), false, cc_rs_controls);
1770  break;
1771  case hc_cc_rs::TcST:
1772  case hc_cc_rs::TScT:
1773  case hc_cc_rs::TcScT:
1774  cc_turn_controls(*(p->cstart), *(p->qi1), true, cc_rs_controls);
1775  straight_controls(*(p->qi1), *(p->qi2), cc_rs_controls);
1776  cc_turn_controls(*(p->cend), *(p->qi2), false, cc_rs_controls);
1777  break;
1778  default:
1779  break;
1780  }
1781  delete p;
1782  return cc_rs_controls;
1783 }
1784 
1785 } // namespace steering
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcT_exists
bool TcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:106
steering::hc_cc_rs::TTcT
@ TTcT
Definition: paths.hpp:133
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TiSTcT_path
double TiSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, HC_CC_Circle **ci) const
Definition: cc00_reeds_shepp_state_space.cpp:542
plot_states.alpha
alpha
Definition: plot_states.py:107
steering::Path::length
double length
Path length.
Definition: paths.hpp:98
steering::HC_CC_State_Space::hc_cc_circle_param_
HC_CC_Circle_Param hc_cc_circle_param_
Parameters of a hc-/cc-circle.
Definition: hc_cc_state_space.hpp:101
steering::hc_cc_rs::TcScT
@ TcScT
Definition: paths.hpp:144
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTST_exists
bool TcTST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:624
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TTT_exists
bool TTT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:970
steering::State
Description of a kinematic car's state.
Definition: steering_functions.hpp:44
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TSTcT_path
double TSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, HC_CC_Circle **ci) const
Definition: cc00_reeds_shepp_state_space.cpp:583
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::parent_
CC00_Reeds_Shepp_State_Space * parent_
Definition: cc00_reeds_shepp_state_space.cpp:43
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TiST_exists
bool TiST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:373
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTSTcT_exists
bool TcTSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:711
steering::HC_CC_RS_Path::type
hc_cc_rs::path_type type
Path type.
Definition: paths.hpp:164
steering::HC_CC_State_Space::sigma_
double sigma_
Definition: hc_cc_state_space.hpp:95
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTT_path
double TcTT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, HC_CC_Circle **ci) const
Definition: cc00_reeds_shepp_state_space.cpp:267
plot_states.path
path
Definition: plot_states.py:88
steering::HC_CC_Circle_Param::mu
double mu
Angle between the initial orientation and the tangent to the circle at the initial position.
Definition: hc_cc_circle.hpp:94
steering::hc_cc_rs::TcTT
@ TcTT
Definition: paths.hpp:132
steering::hc_cc_rs::S
@ S
Definition: paths.hpp:126
steering::State::theta
double theta
Orientation of the robot.
Definition: steering_functions.hpp:70
configuration.hpp
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TT_path
double TT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q) const
Definition: cc00_reeds_shepp_state_space.cpp:99
steering::HC_CC_RS_Path::cend
HC_CC_Circle * cend
Definition: paths.hpp:170
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTST_path
double TcTST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, HC_CC_Circle **ci) const
Definition: cc00_reeds_shepp_state_space.cpp:670
steering::HC_CC_Circle::left
bool left
Turning direction: left/right.
Definition: hc_cc_circle.hpp:123
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTcT_path
double TcTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, HC_CC_Circle **ci) const
Definition: cc00_reeds_shepp_state_space.cpp:197
steering::empty_controls
void empty_controls(std::vector< Control > &controls)
Appends controls with 0 input.
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTiST_exists
bool TcTiST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:598
steering::CC00_Reeds_Shepp_State_Space::get_controls
std::vector< Control > get_controls(const State &state1, const State &state2) const
Returns controls of the shortest path from state1 to state2.
Definition: cc00_reeds_shepp_state_space.cpp:1705
steering::hc_cc_rs::TcTTcT
@ TcTTcT
Definition: paths.hpp:139
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTeSTcT_path
double TcTeSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4, HC_CC_Circle **ci1, HC_CC_Circle **ci2) const
Definition: cc00_reeds_shepp_state_space.cpp:741
steering::hc_cc_rs::TT
@ TT
Definition: paths.hpp:128
steering::HC_CC_Circle
Definition: hc_cc_circle.hpp:80
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTeSTcT_exists
bool TcTeSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:698
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TceScT_path
double TceScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:1357
steering::HC_CC_Circle_Param::cos_mu
double cos_mu
Definition: hc_cc_circle.hpp:97
steering::State::y
double y
Position in y of the robot.
Definition: steering_functions.hpp:67
steering::HC_CC_Circle::xc
double xc
Center of the circle.
Definition: hc_cc_circle.hpp:132
steering::hc_cc_rs::TScT
@ TScT
Definition: paths.hpp:143
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TeST_path
double TeST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:491
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcST_exists
bool TcST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:1065
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTiSTcT_exists
bool TcTiSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:685
steering::hc_cc_rs::TST
@ TST
Definition: paths.hpp:134
steering::HC_CC_RS_Path
Definition: paths.hpp:149
steering::configuration_distance
double configuration_distance(const Configuration &q1, const Configuration &q2)
Cartesian distance between two configurations.
Definition: configuration.cpp:54
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TciScT_exists
bool TciScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:1285
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TTcT_path
double TTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, HC_CC_Circle **ci) const
Definition: cc00_reeds_shepp_state_space.cpp:337
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TTT_path
double TTT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, HC_CC_Circle **ci) const
Definition: cc00_reeds_shepp_state_space.cpp:1003
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TeSTcT_path
double TeSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, HC_CC_Circle **ci) const
Definition: cc00_reeds_shepp_state_space.cpp:563
steering::CC00_Reeds_Shepp_State_Space
An implementation of continuous curvature (CC) steer for a Reeds-Shepp car with zero curvature at the...
Definition: cc00_reeds_shepp_state_space.hpp:74
steering::hc_cc_rs::TcTcT
@ TcTcT
Definition: paths.hpp:131
steering::HC_CC_Circle::yc
double yc
Definition: hc_cc_circle.hpp:132
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TciST_path
double TciST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:1070
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TceST_path
double TceST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:1111
steering::global_frame_change
void global_frame_change(double x, double y, double theta, double local_x, double local_y, double *global_x, double *global_y)
Transformation of (local_x, local_y) from local coordinate system to global one.
Definition: utilities.cpp:223
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TiST_tangent_circles
void TiST_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:405
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TceScT_exists
bool TceScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:1298
steering::HC_CC_Circle::forward
bool forward
Driving direction: forwards/backwards.
Definition: hc_cc_circle.hpp:126
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTTcT_path
double TcTTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, HC_CC_Circle **ci1, HC_CC_Circle **ci2) const
Definition: cc00_reeds_shepp_state_space.cpp:920
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TST_path
double TST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:497
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTT_exists
bool TcTT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:233
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TSTcT_exists
bool TSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:537
steering::HC_CC_RS_Path::qi1
Configuration * qi1
Intermediate configurations.
Definition: paths.hpp:167
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TT_exists
bool TT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:55
steering::State::x
double x
Position in x of the robot.
Definition: steering_functions.hpp:64
steering::hc_cc_rs::TTcTT
@ TTcTT
Definition: paths.hpp:138
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TeScT_exists
bool TeScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:1175
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTTcT_exists
bool TcTTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:878
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TTT_tangent_circles
void TTT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4) const
Definition: cc00_reeds_shepp_state_space.cpp:983
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TT_tangent_circles
void TT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q) const
Definition: cc00_reeds_shepp_state_space.cpp:68
steering::HC_CC_RS_Path::ci1
HC_CC_Circle * ci1
Definition: paths.hpp:170
steering::pointer_array_init
void pointer_array_init(void *array[], int size)
Initialize an array with nullptr.
Definition: utilities.cpp:264
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TScT_exists
bool TScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:1188
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TeSTcT_exists
bool TeSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:524
steering::HC_CC_State_Space::kappa_
double kappa_
Curvature, sharpness of clothoid.
Definition: hc_cc_state_space.hpp:95
steering::straight_controls
void straight_controls(const Configuration &q1, const Configuration &q2, std::vector< Control > &controls)
Appends controls with a straight line.
steering::hc_cc_rs::TcTSTcT
@ TcTSTcT
Definition: paths.hpp:137
distance
double distance(double x0, double y0, double x1, double y1)
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TceST_exists
bool TceST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:1052
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TiScT_exists
bool TiScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:1162
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTcT_exists
bool TcTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:164
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TiST_path
double TiST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:485
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TTcT_exists
bool TTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:303
steering::CC00_Reeds_Shepp_State_Space::cc00_reeds_shepp_
std::unique_ptr< CC00_Reeds_Shepp > cc00_reeds_shepp_
Pimpl Idiom: unique pointer on class with families
Definition: cc00_reeds_shepp_state_space.hpp:120
steering::HC_CC_Circle_Param::sin_mu
double sin_mu
Sine and cosine of mu.
Definition: hc_cc_circle.hpp:97
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTTcT_tangent_circles
void TcTTcT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4, Configuration **q5, Configuration **q6) const
Definition: cc00_reeds_shepp_state_space.cpp:891
steering::HC_CC_State_Space
Definition: hc_cc_state_space.hpp:45
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcT_tangent_circles
void TcT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q) const
Definition: cc00_reeds_shepp_state_space.cpp:119
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TciScT_path
double TciScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:1316
steering::Configuration
Definition: configuration.hpp:55
steering::array_index_min
int array_index_min(double array[], int size)
Find index with minimal value in double array.
Definition: utilities.cpp:241
start
ROSCPP_DECL void start()
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TeST_exists
bool TeST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:386
steering::HC_CC_Circle::cc_turn_length
double cc_turn_length(const Configuration &q) const
Length of a cc-turn.
Definition: hc_cc_circle.cpp:238
steering::center_distance
double center_distance(const HC_CC_Circle &c1, const HC_CC_Circle &c2)
Cartesian distance between the centers of two circles.
Definition: hc_cc_circle.cpp:307
steering::HC_CC_Circle::regular
bool regular
Type of the circle: regular/irregular.
Definition: hc_cc_circle.hpp:129
steering::HC_CC_RS_Path::qi3
Configuration * qi3
Definition: paths.hpp:167
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcST_path
double TcST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:1148
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTiST_path
double TcTiST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, HC_CC_Circle **ci) const
Definition: cc00_reeds_shepp_state_space.cpp:629
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::CC00_Reeds_Shepp
CC00_Reeds_Shepp(CC00_Reeds_Shepp_State_Space *parent)
Definition: cc00_reeds_shepp_state_space.cpp:46
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcT_path
double TcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q) const
Definition: cc00_reeds_shepp_state_space.cpp:155
steering::HC_CC_RS_Path::ci2
HC_CC_Circle * ci2
Definition: paths.hpp:170
steering::nb_hc_cc_rs_paths
const int nb_hc_cc_rs_paths
Definition: paths.hpp:147
steering::get_epsilon
double get_epsilon()
Return value of epsilon.
Definition: utilities.cpp:57
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTSTcT_path
double TcTSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4, HC_CC_Circle **ci1, HC_CC_Circle **ci2) const
Definition: cc00_reeds_shepp_state_space.cpp:765
std
steering::CC00_Reeds_Shepp_State_Space::cc00_reeds_shepp
HC_CC_RS_Path * cc00_reeds_shepp(const State &state1, const State &state2) const
Returns a sequence of turns and straight lines connecting a start and an end configuration.
Definition: cc00_reeds_shepp_state_space.cpp:1624
steering::CC00_Reeds_Shepp_State_Space::~CC00_Reeds_Shepp_State_Space
~CC00_Reeds_Shepp_State_Space()
Destructor.
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTeST_exists
bool TcTeST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:611
plot_states.kappa
kappa
Definition: plot_states.py:106
steering::hc_cc_rs::TcT
@ TcT
Definition: paths.hpp:129
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TiScT_path
double TiScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:1193
steering::hc_cc_rs::TcTST
@ TcTST
Definition: paths.hpp:136
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTT_tangent_circles
void TcTT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4) const
Definition: cc00_reeds_shepp_state_space.cpp:246
steering::hc_cc_rs::TTT
@ TTT
Definition: paths.hpp:141
steering::hc_cc_rs::E
@ E
Definition: paths.hpp:125
steering::double_array_init
void double_array_init(double array[], int size, double value)
Initialize an array with a given value.
Definition: utilities.cpp:256
steering
Definition: dubins_state_space.hpp:70
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTeST_path
double TcTeST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, HC_CC_Circle **ci) const
Definition: cc00_reeds_shepp_state_space.cpp:650
steering::HC_CC_RS_Path::cstart
HC_CC_Circle * cstart
Start, end and intermediate circles.
Definition: paths.hpp:170
steering::cc_turn_controls
void cc_turn_controls(const HC_CC_Circle &c, const Configuration &q, bool order, std::vector< Control > &controls)
Appends controls with a cc-turn.
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TciST_exists
bool TciST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:1039
steering::HC_CC_RS_Path::qi4
Configuration * qi4
Definition: paths.hpp:167
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTcT_tangent_circles
void TcTcT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4) const
Definition: cc00_reeds_shepp_state_space.cpp:177
steering::hc_cc_rs::path_type
path_type
Definition: paths.hpp:123
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcTiSTcT_path
double TcTiSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4, HC_CC_Circle **ci1, HC_CC_Circle **ci2) const
Definition: cc00_reeds_shepp_state_space.cpp:716
steering::CC00_Reeds_Shepp_State_Space::cc00_circles_rs_path
HC_CC_RS_Path * cc00_circles_rs_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Returns a sequence of turns and straight lines connecting the two circles c1 and c2.
Definition: cc00_reeds_shepp_state_space.cpp:1418
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TeST_tangent_circles
void TeST_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:448
cc00_reeds_shepp_state_space.hpp
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcScT_exists
bool TcScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:1311
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TTcTT_tangent_circles
void TTcTT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4, Configuration **q5, Configuration **q6) const
Definition: cc00_reeds_shepp_state_space.cpp:793
steering::configuration_aligned
bool configuration_aligned(const Configuration &q1, const Configuration &q2)
Are two configurations aligned?
Definition: configuration.cpp:59
PI
#define PI
Definition: utilities.hpp:31
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp
Definition: cc00_reeds_shepp_state_space.cpp:40
steering::configuration_on_hc_cc_circle
bool configuration_on_hc_cc_circle(const HC_CC_Circle &c, const Configuration &q)
Configuration on the circle?
Definition: hc_cc_circle.cpp:312
steering::configuration_equal
bool configuration_equal(const Configuration &q1, const Configuration &q2)
Are two configurations equal?
Definition: configuration.cpp:69
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TcScT_path
double TcScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:1394
steering::CC00_Reeds_Shepp_State_Space::get_distance
double get_distance(const State &state1, const State &state2) const
Returns shortest path length from state1 to state2.
Definition: cc00_reeds_shepp_state_space.cpp:1697
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TeScT_path
double TeScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:1234
CC_REGULAR
#define CC_REGULAR
Definition: cc00_reeds_shepp_state_space.cpp:33
steering::hc_cc_rs::TcST
@ TcST
Definition: paths.hpp:142
HALF_PI
#define HALF_PI
Definition: utilities.hpp:32
steering::HC_CC_Circle_Param::radius
double radius
Radius of the outer circle.
Definition: hc_cc_circle.hpp:91
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TTcTT_path
double TTcTT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, HC_CC_Circle **ci1, HC_CC_Circle **ci2) const
Definition: cc00_reeds_shepp_state_space.cpp:830
steering::Path::start
Configuration start
Start and end configuration.
Definition: paths.hpp:92
steering::Path::end
Configuration end
Definition: paths.hpp:92
steering::HC_CC_Circle::start
Configuration start
Start configuration.
Definition: hc_cc_circle.hpp:120
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TScT_path
double TScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2) const
Definition: cc00_reeds_shepp_state_space.cpp:1271
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TiSTcT_exists
bool TiSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:511
utilities.hpp
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TTcTT_exists
bool TTcTT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:780
steering::hc_cc_rs::T
@ T
Definition: paths.hpp:127
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TST_exists
bool TST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: cc00_reeds_shepp_state_space.cpp:400
steering::CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp::TTcT_tangent_circles
void TTcT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4) const
Definition: cc00_reeds_shepp_state_space.cpp:316
steering::HC_CC_RS_Path::qi2
Configuration * qi2
Definition: paths.hpp:167
steering::hc_cc_rs::TSTcT
@ TSTcT
Definition: paths.hpp:135

steering_functions
Author(s): Holger Banzhaf
autogenerated on Mon Dec 11 2023 03:27:43