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

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