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

steering_functions
Author(s): Holger Banzhaf
autogenerated on Thu Aug 18 2022 02:09:46