hcpm0_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/hcpm0_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 HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp
42 {
43 private:
44  HCpm0_Reeds_Shepp_State_Space *parent_;
45 
46 public:
47  explicit HCpm0_Reeds_Shepp(HCpm0_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 * c2.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 - c2.mu;
80  }
81  else
82  {
83  theta = angle + HALF_PI + c2.mu;
84  }
85  }
86  else
87  {
88  if (c1.forward)
89  {
90  theta = angle - HALF_PI + c2.mu;
91  }
92  else
93  {
94  theta = angle - HALF_PI - c2.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, q2);
104  *cstart = new HC_CC_Circle(**q2, !c2.left, c2.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
105  *cend = new HC_CC_Circle(c2.start, c2.left, c2.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
106  *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa);
107  return (*cstart)->hc_turn_length(**q1) + (*cend)->cc_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)->rs_turn_length(**q) + (*cend)->hc_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)->rs_turn_length(*qa) + middle1->rs_turn_length(*qb) + (*cend)->hc_turn_length(*qb);
219  double length2 = (*cstart)->rs_turn_length(*qc) + middle2->rs_turn_length(*qd) + (*cend)->hc_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 * c2.radius + 2 * fabs(c2.kappa_inv)) && (distance >= 2 * c2.radius - 2 * fabs(c2.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(c2.kappa_inv);
262  double r2 = 2 * c2.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 *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 
287  *cstart = new HC_CC_Circle(c1);
288  *cend = new HC_CC_Circle(c2.start, c2.left, c2.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
289 
290  // select shortest connection
291  double length1 = (*cstart)->rs_turn_length(*qa) + middle1->hc_turn_length(*qa) + (*cend)->cc_turn_length(*qb);
292  double length2 = (*cstart)->rs_turn_length(*qc) + middle2->hc_turn_length(*qc) + (*cend)->cc_turn_length(*qd);
293  if (length1 < length2)
294  {
295  *q1 = qa;
296  *q2 = qb;
297  *ci = middle1;
298  delete qc;
299  delete qd;
300  delete middle2;
301  return length1;
302  }
303  else
304  {
305  *q1 = qc;
306  *q2 = qd;
307  *ci = middle2;
308  delete qa;
309  delete qb;
310  delete middle1;
311  return length2;
312  }
313  return numeric_limits<double>::max();
314  }
315 
316  // ##### TTcT #################################################################
317  bool TTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
318  {
319  if (c1.left != c2.left)
320  {
321  return false;
322  }
323  if (c1.forward != c2.forward)
324  {
325  return false;
326  }
327  return (distance <= 2 * c2.radius + 2 * fabs(c2.kappa_inv)) && (distance >= 2 * c2.radius - 2 * fabs(c2.kappa_inv));
328  }
329 
330  void TTcT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
331  Configuration **q3, Configuration **q4) const
332  {
333  double theta = angle;
334  double r1 = 2 * c2.radius;
335  double r2 = 2 * fabs(c2.kappa_inv);
336  double delta_x = (pow(r1, 2) + pow(distance, 2) - pow(r2, 2)) / (2 * distance);
337  double delta_y = sqrt(pow(r1, 2) - pow(delta_x, 2));
338  double x, y;
339 
340  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
341  HC_CC_Circle tgt1(x, y, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
342  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
343  HC_CC_Circle tgt2(x, y, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
344 
345  TT_tangent_circles(c1, tgt1, q1);
346  TcT_tangent_circles(tgt1, c2, q2);
347  TT_tangent_circles(c1, tgt2, q3);
348  TcT_tangent_circles(tgt2, c2, q4);
349  }
350 
351  double TTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
352  Configuration **q1, Configuration **q2, HC_CC_Circle **ci) const
353  {
354  Configuration *qa, *qb, *qc, *qd;
355  TTcT_tangent_circles(c1, c2, &qa, &qb, &qc, &qd);
356  HC_CC_Circle *start1, *start2, *middle1, *middle2;
357  start1 = new HC_CC_Circle(*qa, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
358  middle1 = new HC_CC_Circle(*qa, !c1.left, c1.forward, true, parent_->hc_cc_circle_param_);
359  start2 = new HC_CC_Circle(*qc, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
360  middle2 = new HC_CC_Circle(*qc, !c1.left, c1.forward, true, parent_->hc_cc_circle_param_);
361 
362  *cend = new HC_CC_Circle(c2);
363  *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa);
364 
365  // select shortest connection
366  double length1 = start1->hc_turn_length(**q1) + middle1->hc_turn_length(*qb) + (*cend)->hc_turn_length(*qb);
367  double length2 = start2->hc_turn_length(**q1) + middle2->hc_turn_length(*qd) + (*cend)->hc_turn_length(*qd);
368  if (length1 < length2)
369  {
370  *cstart = start1;
371  *q2 = qb;
372  *ci = middle1;
373  delete qa;
374  delete qc;
375  delete qd;
376  delete start2;
377  delete middle2;
378  return length1;
379  }
380  else
381  {
382  *cstart = start2;
383  *q2 = qd;
384  *ci = middle2;
385  delete qa;
386  delete qb;
387  delete qc;
388  delete start1;
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 * c2.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 * c2.radius * c2.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 * c2.radius * c2.cos_mu / distance);
433  double delta_x = c2.radius * c2.sin_mu;
434  double delta_y = c2.radius * c2.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 = c2.radius * c2.sin_mu;
474  double delta_y = c2.radius * c2.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, q2, q3);
511  *cstart = new HC_CC_Circle(**q2, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
512  *cend = new HC_CC_Circle(c2.start, c2.left, c2.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
513  *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa);
514  return (*cstart)->hc_turn_length(**q1) + configuration_distance(**q2, **q3) + (*cend)->cc_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, q2, q3);
521  *cstart = new HC_CC_Circle(**q2, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
522  *cend = new HC_CC_Circle(c2.start, c2.left, c2.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
523  *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa);
524  return (*cstart)->hc_turn_length(**q1) + configuration_distance(**q2, **q3) + (*cend)->cc_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 * c2.radius * c2.sin_mu + 2 * fabs(c2.kappa_inv), 2) + pow(2 * c2.radius * c2.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(c2.kappa_inv) + c2.radius * c2.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, Configuration **q4,
576  HC_CC_Circle **ci) const
577  {
578  double theta = angle;
579  double delta_y = (4 * c2.radius * c2.cos_mu) / (fabs(c2.kappa) * distance);
580  double delta_x = sqrt(pow(2 * c2.kappa_inv, 2) - pow(delta_y, 2));
581  double x, y;
582 
583  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
584  HC_CC_Circle tgt1(x, y, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
585 
586  TiST_tangent_circles(c1, tgt1, q2, q3);
587  TcT_tangent_circles(tgt1, c2, q4);
588 
589  *cstart = new HC_CC_Circle(**q2, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
590  *cend = new HC_CC_Circle(c2);
591  *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa);
592  *ci = new HC_CC_Circle(**q3, !c1.left, c1.forward, true, parent_->hc_cc_circle_param_);
593 
594  return (*cstart)->hc_turn_length(**q1) + configuration_distance(**q2, **q3) + (*ci)->hc_turn_length(**q4) +
595  (*cend)->hc_turn_length(**q4);
596  }
597 
598  double TeSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
599  Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4,
600  HC_CC_Circle **ci) const
601  {
602  double theta = angle;
603  double delta_x = 2 * fabs(c2.kappa_inv);
604  double delta_y = 0;
605  double x, y;
606 
607  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
608  HC_CC_Circle tgt1(x, y, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
609 
610  TeST_tangent_circles(c1, tgt1, q2, q3);
611  TcT_tangent_circles(tgt1, c2, q4);
612 
613  *cstart = new HC_CC_Circle(**q2, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
614  *cend = new HC_CC_Circle(c2);
615  *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa);
616  *ci = new HC_CC_Circle(**q3, !c2.left, c2.forward, true, parent_->hc_cc_circle_param_);
617 
618  return (*cstart)->hc_turn_length(**q1) + configuration_distance(**q2, **q3) + (*ci)->hc_turn_length(**q4) +
619  (*cend)->hc_turn_length(**q4);
620  }
621 
622  double TSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
623  Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4,
624  HC_CC_Circle **ci) const
625  {
626  if (TiSTcT_exists(c1, c2))
627  {
628  return TiSTcT_path(c1, c2, cstart, cend, q1, q2, q3, q4, ci);
629  }
630  if (TeSTcT_exists(c1, c2))
631  {
632  return TeSTcT_path(c1, c2, cstart, cend, q1, q2, q3, q4, ci);
633  }
634  return numeric_limits<double>::max();
635  }
636 
637  // ##### TcTST ################################################################
638  bool TcTiST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
639  {
640  if (c1.left != c2.left)
641  {
642  return false;
643  }
644  if (c1.forward != c2.forward)
645  {
646  return false;
647  }
648  return (distance >=
649  sqrt(pow(2 * c2.radius * c2.sin_mu + 2 * fabs(c2.kappa_inv), 2) + pow(2 * c2.radius * c2.cos_mu, 2)));
650  }
651 
652  bool TcTeST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
653  {
654  if (c1.left == c2.left)
655  {
656  return false;
657  }
658  if (c1.forward != c2.forward)
659  {
660  return false;
661  }
662  return (distance >= 2 * (fabs(c2.kappa_inv) + c2.radius * c2.sin_mu));
663  }
664 
665  bool TcTST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
666  {
667  return TcTiST_exists(c1, c2) || TcTeST_exists(c1, c2);
668  }
669 
670  double TcTiST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
671  Configuration **q1, Configuration **q2, Configuration **q3, HC_CC_Circle **ci) const
672  {
673  double theta = angle;
674  double delta_y = (4 * c2.radius * c2.cos_mu) / (fabs(c2.kappa) * distance);
675  double delta_x = sqrt(pow(2 * c2.kappa_inv, 2) - pow(delta_y, 2));
676  double x, y;
677 
678  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
679  HC_CC_Circle tgt1(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
680 
681  TcT_tangent_circles(c1, tgt1, q1);
682  TiST_tangent_circles(tgt1, c2, q2, q3);
683 
684  *cstart = new HC_CC_Circle(c1);
685  *cend = new HC_CC_Circle(c2.start, c2.left, c2.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
686  *ci = new HC_CC_Circle(**q2, !c1.left, c1.forward, true, parent_->hc_cc_circle_param_);
687 
688  return (*cstart)->rs_turn_length(**q1) + (*ci)->hc_turn_length(**q1) + configuration_distance(**q2, **q3) +
689  (*cend)->cc_turn_length(**q3);
690  }
691 
692  double TcTeST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
693  Configuration **q1, Configuration **q2, Configuration **q3, HC_CC_Circle **ci) const
694  {
695  double theta = angle;
696  double delta_x = 2 * fabs(c2.kappa_inv);
697  double delta_y = 0;
698  double x, y;
699 
700  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
701  HC_CC_Circle tgt1(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
702 
703  TcT_tangent_circles(c1, tgt1, q1);
704  TeST_tangent_circles(tgt1, c2, q2, q3);
705 
706  *cstart = new HC_CC_Circle(c1);
707  *cend = new HC_CC_Circle(c2.start, c2.left, c2.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
708  *ci = new HC_CC_Circle(**q2, !c1.left, c1.forward, true, parent_->hc_cc_circle_param_);
709 
710  return (*cstart)->rs_turn_length(**q1) + (*ci)->hc_turn_length(**q1) + configuration_distance(**q2, **q3) +
711  (*cend)->cc_turn_length(**q3);
712  }
713 
714  double TcTST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
715  Configuration **q1, Configuration **q2, Configuration **q3, HC_CC_Circle **ci) const
716  {
717  if (TcTiST_exists(c1, c2))
718  {
719  return TcTiST_path(c1, c2, cstart, cend, q1, q2, q3, ci);
720  }
721  if (TcTeST_exists(c1, c2))
722  {
723  return TcTeST_path(c1, c2, cstart, cend, q1, q2, q3, 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 * c2.radius, 2) + 16 * c2.radius * c2.sin_mu * fabs(c2.kappa_inv) + pow(4 * c2.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(c2.kappa_inv) + 2 * c2.radius * c2.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 * c2.radius * c2.cos_mu) / (distance * fabs(c2.kappa));
767  double delta_x = sqrt(pow(2 * c2.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)->rs_turn_length(**q1) + (*ci1)->hc_turn_length(**q1) + configuration_distance(**q2, **q3) +
785  (*ci2)->hc_turn_length(**q4) + (*cend)->hc_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)->rs_turn_length(**q1) + (*ci1)->hc_turn_length(**q1) + configuration_distance(**q2, **q3) +
812  (*ci2)->hc_turn_length(**q4) + (*cend)->hc_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 * c2.radius + 2 * fabs(c2.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(c2.kappa_inv);
850  r2 = 2 * c2.radius;
851  if (distance < 4 * c2.radius - 2 * fabs(c2.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 *start1, *start2, *middle1, *middle2, *middle3, *middle4;
888  start1 = new HC_CC_Circle(*qa, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
889  middle1 = new HC_CC_Circle(*qa, !c1.left, c1.forward, true, parent_->hc_cc_circle_param_);
890  middle2 = new HC_CC_Circle(*qc, !c2.left, c2.forward, true, parent_->hc_cc_circle_param_);
891  start2 = new HC_CC_Circle(*qd, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
892  middle3 = new HC_CC_Circle(*qd, !c1.left, c1.forward, true, parent_->hc_cc_circle_param_);
893  middle4 = new HC_CC_Circle(*qf, !c2.left, c2.forward, true, parent_->hc_cc_circle_param_);
894 
895  *cend = new HC_CC_Circle(c2.start, c2.left, c2.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
896  *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa);
897 
898  // select shortest connection
899  double length1 = start1->hc_turn_length(**q1) + middle1->hc_turn_length(*qb) + middle2->hc_turn_length(*qb) +
900  (*cend)->cc_turn_length(*qc);
901  double length2 = start2->hc_turn_length(**q1) + middle3->hc_turn_length(*qe) + middle4->hc_turn_length(*qe) +
902  (*cend)->cc_turn_length(*qf);
903  if (length1 < length2)
904  {
905  *cstart = start1;
906  *q2 = qb;
907  *q3 = qc;
908  *ci1 = middle1;
909  *ci2 = middle2;
910  delete qa;
911  delete qd;
912  delete qe;
913  delete qf;
914  delete start2;
915  delete middle3;
916  delete middle4;
917  return length1;
918  }
919  else
920  {
921  *cstart = start2;
922  *q2 = qe;
923  *q3 = qf;
924  *ci1 = middle3;
925  *ci2 = middle4;
926  delete qa;
927  delete qb;
928  delete qc;
929  delete qd;
930  delete start1;
931  delete middle1;
932  delete middle2;
933  return length2;
934  }
935 
936  return numeric_limits<double>::max();
937  }
938 
939  // ##### TcTTcT ###############################################################
940  bool TcTTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
941  {
942  if (c1.left == c2.left)
943  {
944  return false;
945  }
946  if (c1.forward == c2.forward)
947  {
948  return false;
949  }
950  return (distance <= 4 * fabs(c2.kappa_inv) + 2 * c2.radius) && (distance >= 4 * fabs(c2.kappa_inv) - 2 * c2.radius);
951  }
952 
953  void TcTTcT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
954  Configuration **q3, Configuration **q4, Configuration **q5, Configuration **q6) const
955  {
956  double theta = angle;
957  double r1 = 2 * fabs(c2.kappa_inv);
958  double r2 = c2.radius;
959  double delta_x = (pow(r1, 2) + pow(distance / 2, 2) - pow(r2, 2)) / distance;
960  double delta_y = sqrt(pow(r1, 2) - pow(delta_x, 2));
961  double x, y;
962 
963  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
964  HC_CC_Circle tgt1(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
965  global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y);
966  HC_CC_Circle tgt2(x, y, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
967 
968  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
969  HC_CC_Circle tgt3(x, y, !c1.left, !c1.forward, c1.regular, parent_->hc_cc_circle_param_);
970  global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y);
971  HC_CC_Circle tgt4(x, y, !c2.left, c2.forward, c2.regular, parent_->hc_cc_circle_param_);
972 
973  TcT_tangent_circles(c1, tgt1, q1);
974  TT_tangent_circles(tgt1, tgt2, q2);
975  TcT_tangent_circles(tgt2, c2, q3);
976 
977  TcT_tangent_circles(c1, tgt3, q4);
978  TT_tangent_circles(tgt3, tgt4, q5);
979  TcT_tangent_circles(tgt4, c2, q6);
980  }
981 
982  double TcTTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
983  Configuration **q1, Configuration **q2, HC_CC_Circle **ci1, HC_CC_Circle **ci2) const
984  {
985  Configuration *qa, *qb, *qc, *qd, *qe, *qf;
986  TcTTcT_tangent_circles(c1, c2, &qa, &qb, &qc, &qd, &qe, &qf);
987  HC_CC_Circle *middle1, *middle2, *middle3, *middle4;
988  middle1 = new HC_CC_Circle(*qb, !c1.left, c1.forward, true, parent_->hc_cc_circle_param_);
989  middle2 = new HC_CC_Circle(*qb, c1.left, !c1.forward, true, parent_->hc_cc_circle_param_);
990  middle3 = new HC_CC_Circle(*qe, !c1.left, c1.forward, true, parent_->hc_cc_circle_param_);
991  middle4 = new HC_CC_Circle(*qe, c1.left, !c1.forward, true, parent_->hc_cc_circle_param_);
992 
993  *cstart = new HC_CC_Circle(c1);
994  *cend = new HC_CC_Circle(c2);
995 
996  // select shortest connection
997  double length1 = (*cstart)->rs_turn_length(*qa) + middle1->hc_turn_length(*qa) + middle2->hc_turn_length(*qc) +
998  (*cend)->hc_turn_length(*qc);
999  double length2 = (*cstart)->rs_turn_length(*qd) + middle3->hc_turn_length(*qd) + middle4->hc_turn_length(*qf) +
1000  (*cend)->hc_turn_length(*qf);
1001  if (length1 < length2)
1002  {
1003  *q1 = qa;
1004  *q2 = qc;
1005  *ci1 = middle1;
1006  *ci2 = middle2;
1007  delete qb;
1008  delete qd;
1009  delete qe;
1010  delete qf;
1011  delete middle3;
1012  delete middle4;
1013  return length1;
1014  }
1015  else
1016  {
1017  *q1 = qd;
1018  *q2 = qf;
1019  *ci1 = middle3;
1020  *ci2 = middle4;
1021  delete qa;
1022  delete qb;
1023  delete qc;
1024  delete qe;
1025  delete middle1;
1026  delete middle2;
1027  return length2;
1028  }
1029  return numeric_limits<double>::max();
1030  }
1031 
1032  // ############################################################################
1033 
1034  // ##### TTT ##################################################################
1035  bool TTT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1036  {
1037  if (c1.left != c2.left)
1038  {
1039  return false;
1040  }
1041  if (c1.forward == c2.forward)
1042  {
1043  return false;
1044  }
1045  return distance <= 4 * c2.radius;
1046  }
1047 
1048  void TTT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q1, Configuration **q2,
1049  Configuration **q3, Configuration **q4) const
1050  {
1051  double theta = angle;
1052  double r = 2 * c2.radius;
1053  double delta_x = 0.5 * distance;
1054  double delta_y = sqrt(pow(r, 2) - pow(delta_x, 2));
1055  double x, y;
1056 
1057  global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y);
1058  HC_CC_Circle tgt1(x, y, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
1059  global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y);
1060  HC_CC_Circle tgt2(x, y, !c1.left, c1.forward, c1.regular, parent_->hc_cc_circle_param_);
1061 
1062  TT_tangent_circles(c1, tgt1, q1);
1063  TT_tangent_circles(tgt1, c2, q2);
1064  TT_tangent_circles(c1, tgt2, q3);
1065  TT_tangent_circles(tgt2, c2, q4);
1066  }
1067 
1068  double TTT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
1069  Configuration **q1, Configuration **q2, HC_CC_Circle **ci) const
1070  {
1071  Configuration *qa, *qb, *qc, *qd;
1072  TTT_tangent_circles(c1, c2, &qa, &qb, &qc, &qd);
1073  HC_CC_Circle *start1, *start2, *middle1, *middle2;
1074  start1 = new HC_CC_Circle(*qa, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
1075  middle1 = new HC_CC_Circle(*qa, !c1.left, c1.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
1076  start2 = new HC_CC_Circle(*qc, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
1077  middle2 = new HC_CC_Circle(*qc, !c1.left, c1.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
1078 
1079  *cend = new HC_CC_Circle(c2.start, c2.left, c2.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
1080  *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa);
1081 
1082  // select shortest connection
1083  double length1 = start1->hc_turn_length(**q1) + middle1->cc_turn_length(*qb) + (*cend)->cc_turn_length(*qb);
1084  double length2 = start2->hc_turn_length(**q1) + middle2->cc_turn_length(*qd) + (*cend)->cc_turn_length(*qd);
1085  if (length1 < length2)
1086  {
1087  *cstart = start1;
1088  *q2 = qb;
1089  *ci = middle1;
1090  delete qa;
1091  delete qc;
1092  delete qd;
1093  delete start2;
1094  delete middle2;
1095  return length1;
1096  }
1097  else
1098  {
1099  *cstart = start2;
1100  *q2 = qd;
1101  *ci = middle2;
1102  delete qa;
1103  delete qb;
1104  delete qc;
1105  delete start1;
1106  delete middle1;
1107  return length2;
1108  }
1109  return numeric_limits<double>::max();
1110  }
1111 
1112  // ##### TcST ################################################################
1113  bool TciST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1114  {
1115  if (c1.left == c2.left)
1116  {
1117  return false;
1118  }
1119  if (c1.forward != c2.forward)
1120  {
1121  return false;
1122  }
1123  return distance >= sqrt(pow(c2.radius * c2.sin_mu, 2) + pow(c2.radius * c2.cos_mu + fabs(c2.kappa_inv), 2));
1124  }
1125 
1126  bool TceST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1127  {
1128  if (c1.left != c2.left)
1129  {
1130  return false;
1131  }
1132  if (c1.forward != c2.forward)
1133  {
1134  return false;
1135  }
1136  return distance >= sqrt(pow(c2.radius * c2.sin_mu, 2) + pow(c2.radius * c2.cos_mu - fabs(c2.kappa_inv), 2));
1137  }
1138 
1139  bool TcST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1140  {
1141  return TciST_exists(c1, c2) || TceST_exists(c1, c2);
1142  }
1143 
1144  double TciST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
1145  Configuration **q1, Configuration **q2) const
1146  {
1147  double alpha = asin((c2.radius * c2.cos_mu + fabs(c2.kappa_inv)) / distance);
1148  double delta_x1 = 0.0;
1149  double delta_y1 = fabs(c2.kappa_inv);
1150  double delta_x2 = c2.radius * c2.sin_mu;
1151  double delta_y2 = c2.radius * c2.cos_mu;
1152  double x, y, theta;
1153  if (c1.left && c1.forward)
1154  {
1155  theta = angle - alpha;
1156  global_frame_change(c1.xc, c1.yc, theta, -delta_x1, delta_y1, &x, &y);
1157  *q1 = new Configuration(x, y, theta + PI, c1.kappa);
1158  global_frame_change(c2.xc, c2.yc, theta, -delta_x2, -delta_y2, &x, &y);
1159  *q2 = new Configuration(x, y, theta + PI, 0);
1160  }
1161  if (c1.left && !c1.forward)
1162  {
1163  theta = angle + alpha;
1164  global_frame_change(c1.xc, c1.yc, theta, -delta_x1, -delta_y1, &x, &y);
1165  *q1 = new Configuration(x, y, theta, c1.kappa);
1166  global_frame_change(c2.xc, c2.yc, theta, -delta_x2, delta_y2, &x, &y);
1167  *q2 = new Configuration(x, y, theta, 0);
1168  }
1169  if (!c1.left && c1.forward)
1170  {
1171  theta = angle + alpha;
1172  global_frame_change(c1.xc, c1.yc, theta, -delta_x1, -delta_y1, &x, &y);
1173  *q1 = new Configuration(x, y, theta + PI, c1.kappa);
1174  global_frame_change(c2.xc, c2.yc, theta, -delta_x2, delta_y2, &x, &y);
1175  *q2 = new Configuration(x, y, theta + PI, 0);
1176  }
1177  if (!c1.left && !c1.forward)
1178  {
1179  theta = angle - alpha;
1180  global_frame_change(c1.xc, c1.yc, theta, -delta_x1, delta_y1, &x, &y);
1181  *q1 = new Configuration(x, y, theta, c1.kappa);
1182  global_frame_change(c2.xc, c2.yc, theta, -delta_x2, -delta_y2, &x, &y);
1183  *q2 = new Configuration(x, y, theta, 0);
1184  }
1185  *cstart = new HC_CC_Circle(c1);
1186  *cend = new HC_CC_Circle(c2.start, c2.left, c2.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
1187  return (*cstart)->rs_turn_length(**q1) + configuration_distance(**q1, **q2) + (*cend)->cc_turn_length(**q2);
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) const
1192  {
1193  double alpha = asin((c2.radius * c2.cos_mu - fabs(c2.kappa_inv)) / distance);
1194  double delta_x1 = 0.0;
1195  double delta_y1 = fabs(c2.kappa_inv);
1196  double delta_x2 = c2.radius * c2.sin_mu;
1197  double delta_y2 = c2.radius * c2.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  *cstart = new HC_CC_Circle(c1);
1232  *cend = new HC_CC_Circle(c2.start, c2.left, c2.forward, CC_REGULAR, parent_->hc_cc_circle_param_);
1233  return (*cstart)->rs_turn_length(**q1) + configuration_distance(**q1, **q2) + (*cend)->cc_turn_length(**q2);
1234  }
1235 
1236  double TcST_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
1237  Configuration **q1, Configuration **q2) const
1238  {
1239  if (TciST_exists(c1, c2))
1240  {
1241  return TciST_path(c1, c2, cstart, cend, q1, q2);
1242  }
1243  if (TceST_exists(c1, c2))
1244  {
1245  return TceST_path(c1, c2, cstart, cend, q1, q2);
1246  }
1247  return numeric_limits<double>::max();
1248  }
1249 
1250  // ##### TScT #################################################################
1251  bool TiScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1252  {
1253  if (c1.left == c2.left)
1254  {
1255  return false;
1256  }
1257  if (c1.forward != c2.forward)
1258  {
1259  return false;
1260  }
1261  return distance >= sqrt(pow(c2.radius * c2.sin_mu, 2) + pow(c2.radius * c2.cos_mu + fabs(c2.kappa_inv), 2));
1262  }
1263 
1264  bool TeScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1265  {
1266  if (c1.left != c2.left)
1267  {
1268  return false;
1269  }
1270  if (c1.forward != c2.forward)
1271  {
1272  return false;
1273  }
1274  return distance >= sqrt(pow(c2.radius * c2.sin_mu, 2) + pow(c2.radius * c2.cos_mu - fabs(c2.kappa_inv), 2));
1275  }
1276 
1277  bool TScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
1278  {
1279  return TiScT_exists(c1, c2) || TeScT_exists(c1, c2);
1280  }
1281 
1282  double TiScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
1283  Configuration **q1, Configuration **q2, Configuration **q3) const
1284  {
1285  double alpha = asin((c2.radius * c2.cos_mu + fabs(c2.kappa_inv)) / distance);
1286  double delta_x1 = c2.radius * c2.sin_mu;
1287  double delta_y1 = c2.radius * c2.cos_mu;
1288  double delta_x2 = 0.0;
1289  double delta_y2 = fabs(c2.kappa_inv);
1290  double x, y, theta;
1291  if (c1.left && c1.forward)
1292  {
1293  theta = angle + alpha;
1294  global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y);
1295  *q2 = new Configuration(x, y, theta, 0);
1296  global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y);
1297  *q3 = new Configuration(x, y, theta, c2.kappa);
1298  }
1299  if (c1.left && !c1.forward)
1300  {
1301  theta = angle - alpha;
1302  global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y);
1303  *q2 = new Configuration(x, y, theta + PI, 0);
1304  global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y);
1305  *q3 = new Configuration(x, y, theta + PI, c2.kappa);
1306  }
1307  if (!c1.left && c1.forward)
1308  {
1309  theta = angle - alpha;
1310  global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y);
1311  *q2 = new Configuration(x, y, theta, 0);
1312  global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y);
1313  *q3 = new Configuration(x, y, theta, c2.kappa);
1314  }
1315  if (!c1.left && !c1.forward)
1316  {
1317  theta = angle + alpha;
1318  global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y);
1319  *q2 = new Configuration(x, y, theta + PI, 0);
1320  global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y);
1321  *q3 = new Configuration(x, y, theta + PI, c2.kappa);
1322  }
1323  *cstart = new HC_CC_Circle(**q2, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
1324  *cend = new HC_CC_Circle(c2);
1325  *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa);
1326  return (*cstart)->hc_turn_length(**q1) + configuration_distance(**q2, **q3) + (*cend)->hc_turn_length(**q3);
1327  }
1328 
1329  double TeScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend,
1330  Configuration **q1, Configuration **q2, Configuration **q3) const
1331  {
1332  double alpha = asin((c2.radius * c2.cos_mu - fabs(c2.kappa_inv)) / distance);
1333  double delta_x1 = c2.radius * c2.sin_mu;
1334  double delta_y1 = c2.radius * c2.cos_mu;
1335  double delta_x2 = 0.0;
1336  double delta_y2 = fabs(c2.kappa_inv);
1337  double x, y, theta;
1338  if (c1.left && c1.forward)
1339  {
1340  theta = angle + alpha;
1341  global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y);
1342  *q2 = new Configuration(x, y, theta, 0);
1343  global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y);
1344  *q3 = new Configuration(x, y, theta, c2.kappa);
1345  }
1346  if (c1.left && !c1.forward)
1347  {
1348  theta = angle - alpha;
1349  global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y);
1350  *q2 = new Configuration(x, y, theta + PI, 0);
1351  global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y);
1352  *q3 = new Configuration(x, y, theta + PI, c2.kappa);
1353  }
1354  if (!c1.left && c1.forward)
1355  {
1356  theta = angle - alpha;
1357  global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y);
1358  *q2 = new Configuration(x, y, theta, 0);
1359  global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y);
1360  *q3 = new Configuration(x, y, theta, c2.kappa);
1361  }
1362  if (!c1.left && !c1.forward)
1363  {
1364  theta = angle + alpha;
1365  global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y);
1366  *q2 = new Configuration(x, y, theta + PI, 0);
1367  global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y);
1368  *q3 = new Configuration(x, y, theta + PI, c2.kappa);
1369  }
1370  *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa);
1371  *cstart = new HC_CC_Circle(**q2, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_);
1372  *cend = new HC_CC_Circle(c2);
1373  return (*cstart)->hc_turn_length(**q1) + configuration_distance(**q2, **q3) + (*cend)->hc_turn_length(**q3);
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, Configuration **q3) const
1378  {
1379  if (TiScT_exists(c1, c2))
1380  {
1381  return TiScT_path(c1, c2, cstart, cend, q1, q2, q3);
1382  }
1383  if (TeScT_exists(c1, c2))
1384  {
1385  return TeScT_path(c1, c2, cstart, cend, q1, q2, q3);
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(c2.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(c2.kappa) * distance));
1426  double delta_x = 0.0;
1427  double delta_y = fabs(c2.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)->rs_turn_length(**q1) + configuration_distance(**q1, **q2) + (*cend)->hc_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(c2.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)->rs_turn_length(**q1) + configuration_distance(**q1, **q2) + (*cend)->hc_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 HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp_State_Space(double kappa, double sigma, double discretization)
1524  : HC_CC_State_Space(kappa, sigma, discretization)
1525  , hcpm0_reeds_shepp_{ unique_ptr<HCpm0_Reeds_Shepp>(new HCpm0_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 HCpm0_Reeds_Shepp_State_Space::~HCpm0_Reeds_Shepp_State_Space() = default;
1533 
1534 HC_CC_RS_Path *HCpm0_Reeds_Shepp_State_Space::hcpm0_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  hcpm0_reeds_shepp_->distance = center_distance(c1, c2);
1559  hcpm0_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 (hcpm0_reeds_shepp_->distance < get_epsilon())
1569  {
1570  cend[hc_cc_rs::T] = new HC_CC_Circle(c2.start, c2.left, c2.forward, HC_REGULAR, hc_cc_circle_param_);
1571  length[hc_cc_rs::T] = cend[hc_cc_rs::T]->hc_turn_length(c1.start);
1572  goto label_end;
1573  }
1574  // case TT
1575  if (hcpm0_reeds_shepp_->TT_exists(c1, c2))
1576  {
1577  length[hc_cc_rs::TT] = hcpm0_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 (hcpm0_reeds_shepp_->TcT_exists(c1, c2))
1582  {
1583  length[hc_cc_rs::TcT] =
1584  hcpm0_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 (hcpm0_reeds_shepp_->TcTcT_exists(c1, c2))
1589  {
1590  length[hc_cc_rs::TcTcT] =
1591  hcpm0_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 (hcpm0_reeds_shepp_->TcTT_exists(c1, c2))
1596  {
1597  length[hc_cc_rs::TcTT] =
1598  hcpm0_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 (hcpm0_reeds_shepp_->TTcT_exists(c1, c2))
1603  {
1604  length[hc_cc_rs::TTcT] =
1605  hcpm0_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 (hcpm0_reeds_shepp_->TST_exists(c1, c2))
1610  {
1611  length[hc_cc_rs::TST] = hcpm0_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 (hcpm0_reeds_shepp_->TSTcT_exists(c1, c2))
1616  {
1617  length[hc_cc_rs::TSTcT] = hcpm0_reeds_shepp_->TSTcT_path(
1618  c1, c2, &cstart[hc_cc_rs::TSTcT], &cend[hc_cc_rs::TSTcT], &qi1[hc_cc_rs::TSTcT], &qi2[hc_cc_rs::TSTcT],
1619  &qi3[hc_cc_rs::TSTcT], &qi4[hc_cc_rs::TSTcT], &ci1[hc_cc_rs::TSTcT]);
1620  }
1621  // case TcTST
1622  if (hcpm0_reeds_shepp_->TcTST_exists(c1, c2))
1623  {
1624  length[hc_cc_rs::TcTST] =
1625  hcpm0_reeds_shepp_->TcTST_path(c1, c2, &cstart[hc_cc_rs::TcTST], &cend[hc_cc_rs::TcTST], &qi1[hc_cc_rs::TcTST],
1626  &qi2[hc_cc_rs::TcTST], &qi3[hc_cc_rs::TcTST], &ci1[hc_cc_rs::TcTST]);
1627  }
1628  // case TcTSTcT
1629  if (hcpm0_reeds_shepp_->TcTSTcT_exists(c1, c2))
1630  {
1631  length[hc_cc_rs::TcTSTcT] = hcpm0_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 (hcpm0_reeds_shepp_->TTcTT_exists(c1, c2))
1637  {
1638  length[hc_cc_rs::TTcTT] = hcpm0_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 (hcpm0_reeds_shepp_->TcTTcT_exists(c1, c2))
1644  {
1645  length[hc_cc_rs::TcTTcT] = hcpm0_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 (hcpm0_reeds_shepp_->TTT_exists(c1, c2))
1652  {
1653  length[hc_cc_rs::TTT] = hcpm0_reeds_shepp_->TTT_path(c1, c2, &cstart[hc_cc_rs::TTT], &cend[hc_cc_rs::TTT],
1654  &qi1[hc_cc_rs::TTT], &qi2[hc_cc_rs::TTT], &ci1[hc_cc_rs::TTT]);
1655  }
1656  // case TcST
1657  if (hcpm0_reeds_shepp_->TcST_exists(c1, c2))
1658  {
1659  length[hc_cc_rs::TcST] = hcpm0_reeds_shepp_->TcST_path(c1, c2, &cstart[hc_cc_rs::TcST], &cend[hc_cc_rs::TcST],
1660  &qi1[hc_cc_rs::TcST], &qi2[hc_cc_rs::TcST]);
1661  }
1662  // case TScT
1663  if (hcpm0_reeds_shepp_->TScT_exists(c1, c2))
1664  {
1665  length[hc_cc_rs::TScT] =
1666  hcpm0_reeds_shepp_->TScT_path(c1, c2, &cstart[hc_cc_rs::TScT], &cend[hc_cc_rs::TScT], &qi1[hc_cc_rs::TScT],
1667  &qi2[hc_cc_rs::TScT], &qi3[hc_cc_rs::TScT]);
1668  }
1669  // case TcScT
1670  if (hcpm0_reeds_shepp_->TcScT_exists(c1, c2))
1671  {
1672  length[hc_cc_rs::TcScT] = hcpm0_reeds_shepp_->TcScT_path(c1, c2, &cstart[hc_cc_rs::TcScT], &cend[hc_cc_rs::TcScT],
1673  &qi1[hc_cc_rs::TcScT], &qi2[hc_cc_rs::TcScT]);
1674  }
1675 label_end:
1676  // select shortest path
1677  hc_cc_rs::path_type best_path = (hc_cc_rs::path_type)array_index_min(length, nb_hc_cc_rs_paths);
1678  HC_CC_RS_Path *path;
1679  path = new HC_CC_RS_Path(c1.start, c2.start, best_path, kappa_, sigma_, qi1[best_path], qi2[best_path],
1680  qi3[best_path], qi4[best_path], cstart[best_path], cend[best_path], ci1[best_path],
1681  ci2[best_path], length[best_path]);
1682 
1683  // clean up
1684  for (int i = 0; i < nb_hc_cc_rs_paths; i++)
1685  {
1686  if (i != best_path)
1687  {
1688  delete qi1[i];
1689  delete qi2[i];
1690  delete qi3[i];
1691  delete qi4[i];
1692  delete cstart[i];
1693  delete ci1[i];
1694  delete ci2[i];
1695  delete cend[i];
1696  }
1697  }
1698  return path;
1699 }
1700 
1701 HC_CC_RS_Path *HCpm0_Reeds_Shepp_State_Space::hcpm0_reeds_shepp(const State &state1, const State &state2) const
1702 {
1703  // compute the 4 circles at the intial and final configuration
1704  Configuration start1(state1.x, state1.y, state1.theta, kappa_);
1705  Configuration start2(state1.x, state1.y, state1.theta, -kappa_);
1706  Configuration end(state2.x, state2.y, state2.theta, 0.0);
1707 
1708  HC_CC_Circle *start_circle[4];
1709  HC_CC_Circle *end_circle[4];
1710  start_circle[0] = new HC_CC_Circle(start1, true, true, true, rs_circle_param_);
1711  start_circle[1] = new HC_CC_Circle(start2, false, true, true, rs_circle_param_);
1712  start_circle[2] = new HC_CC_Circle(start1, true, false, true, rs_circle_param_);
1713  start_circle[3] = new HC_CC_Circle(start2, false, false, true, rs_circle_param_);
1714  end_circle[0] = new HC_CC_Circle(end, true, true, true, hc_cc_circle_param_);
1715  end_circle[1] = new HC_CC_Circle(end, false, true, true, hc_cc_circle_param_);
1716  end_circle[2] = new HC_CC_Circle(end, true, false, true, hc_cc_circle_param_);
1717  end_circle[3] = new HC_CC_Circle(end, false, false, true, hc_cc_circle_param_);
1718 
1719  // compute the shortest path for the 16 combinations (4 circles at the beginning and 4 at the end)
1720  HC_CC_RS_Path *path[] = { nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr,
1721  nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr };
1722 
1723  double lg[] = { numeric_limits<double>::max(), numeric_limits<double>::max(), numeric_limits<double>::max(),
1724  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() };
1729 
1730  for (int i = 0; i < 4; i++)
1731  {
1732  // skip circle at the beginning for curvature continuity
1733  if (i == 0 && state1.kappa < 0)
1734  continue;
1735  else if (i == 1 && state1.kappa > 0)
1736  continue;
1737  else if (i == 2 && state1.kappa < 0)
1738  continue;
1739  else if (i == 3 && state1.kappa > 0)
1740  continue;
1741  for (int j = 0; j < 4; j++)
1742  {
1743  path[4 * i + j] = hcpm0_circles_rs_path(*start_circle[i], *end_circle[j]);
1744  if (path[4 * i + j])
1745  {
1746  lg[4 * i + j] = path[4 * i + j]->length;
1747  }
1748  }
1749  }
1750 
1751  // determine shortest path
1752  int best_path = array_index_min(lg, 16);
1753 
1754  // // display calculations
1755  // cout << "HCpm0_Reeds_Shepp_State_Space" << endl;
1756  // for (int i = 0; i < 16; i++)
1757  // {
1758  // cout << i << ": ";
1759  // if (path[i])
1760  // {
1761  // path[i]->print(true);
1762  // }
1763  // }
1764  // cout << "shortest path: " << (int)best_path << endl;
1765  // path[best_path]->print(true);
1766 
1767  // clean up
1768  for (int i = 0; i < 4; i++)
1769  {
1770  delete start_circle[i];
1771  delete end_circle[i];
1772  }
1773  for (int i = 0; i < 16; i++)
1774  {
1775  if (i != best_path)
1776  {
1777  delete path[i];
1778  }
1779  }
1780  return path[best_path];
1781 }
1782 
1783 double HCpm0_Reeds_Shepp_State_Space::get_distance(const State &state1, const State &state2) const
1784 {
1785  HC_CC_RS_Path *p = this->hcpm0_reeds_shepp(state1, state2);
1786  double length = p->length;
1787  delete p;
1788  return length;
1789 }
1790 
1791 vector<Control> HCpm0_Reeds_Shepp_State_Space::get_controls(const State &state1, const State &state2) const
1792 {
1793  vector<Control> hc_rs_controls;
1794  hc_rs_controls.reserve(9);
1795  HC_CC_RS_Path *p = this->hcpm0_reeds_shepp(state1, state2);
1796  switch (p->type)
1797  {
1798  case hc_cc_rs::E:
1799  empty_controls(hc_rs_controls);
1800  break;
1801  case hc_cc_rs::T:
1802  hc_turn_controls(*(p->cend), p->start, false, hc_rs_controls);
1803  break;
1804  case hc_cc_rs::TT:
1805  hc_turn_controls(*(p->cstart), *(p->qi1), false, hc_rs_controls);
1806  cc_turn_controls(*(p->cend), *(p->qi2), false, hc_rs_controls);
1807  break;
1808  case hc_cc_rs::TcT:
1809  rs_turn_controls(*(p->cstart), *(p->qi1), true, hc_rs_controls);
1810  hc_turn_controls(*(p->cend), *(p->qi1), false, hc_rs_controls);
1811  break;
1812  case hc_cc_rs::TcTcT:
1813  rs_turn_controls(*(p->cstart), *(p->qi1), true, hc_rs_controls);
1814  rs_turn_controls(*(p->ci1), *(p->qi2), true, hc_rs_controls);
1815  hc_turn_controls(*(p->cend), *(p->qi2), false, hc_rs_controls);
1816  break;
1817  case hc_cc_rs::TcTT:
1818  rs_turn_controls(*(p->cstart), *(p->qi1), true, hc_rs_controls);
1819  hc_turn_controls(*(p->ci1), *(p->qi1), false, hc_rs_controls);
1820  cc_turn_controls(*(p->cend), *(p->qi2), false, hc_rs_controls);
1821  break;
1822  case hc_cc_rs::TTcT:
1823  hc_turn_controls(*(p->cstart), *(p->qi1), false, hc_rs_controls);
1824  hc_turn_controls(*(p->ci1), *(p->qi2), true, hc_rs_controls);
1825  hc_turn_controls(*(p->cend), *(p->qi2), false, hc_rs_controls);
1826  break;
1827  case hc_cc_rs::TST:
1828  hc_turn_controls(*(p->cstart), *(p->qi1), false, hc_rs_controls);
1829  straight_controls(*(p->qi2), *(p->qi3), hc_rs_controls);
1830  cc_turn_controls(*(p->cend), *(p->qi3), false, hc_rs_controls);
1831  break;
1832  case hc_cc_rs::TSTcT:
1833  hc_turn_controls(*(p->cstart), *(p->qi1), false, hc_rs_controls);
1834  straight_controls(*(p->qi2), *(p->qi3), hc_rs_controls);
1835  hc_turn_controls(*(p->ci1), *(p->qi4), true, hc_rs_controls);
1836  hc_turn_controls(*(p->cend), *(p->qi4), false, hc_rs_controls);
1837  break;
1838  case hc_cc_rs::TcTST:
1839  rs_turn_controls(*(p->cstart), *(p->qi1), true, hc_rs_controls);
1840  hc_turn_controls(*(p->ci1), *(p->qi1), false, hc_rs_controls);
1841  straight_controls(*(p->qi2), *(p->qi3), hc_rs_controls);
1842  cc_turn_controls(*(p->cend), *(p->qi3), false, hc_rs_controls);
1843  break;
1844  case hc_cc_rs::TcTSTcT:
1845  rs_turn_controls(*(p->cstart), *(p->qi1), true, hc_rs_controls);
1846  hc_turn_controls(*(p->ci1), *(p->qi1), false, hc_rs_controls);
1847  straight_controls(*(p->qi2), *(p->qi3), hc_rs_controls);
1848  hc_turn_controls(*(p->ci2), *(p->qi4), true, hc_rs_controls);
1849  hc_turn_controls(*(p->cend), *(p->qi4), false, hc_rs_controls);
1850  break;
1851  case hc_cc_rs::TTcTT:
1852  hc_turn_controls(*(p->cstart), *(p->qi1), false, hc_rs_controls);
1853  hc_turn_controls(*(p->ci1), *(p->qi2), true, hc_rs_controls);
1854  hc_turn_controls(*(p->ci2), *(p->qi2), false, hc_rs_controls);
1855  cc_turn_controls(*(p->cend), *(p->qi3), false, hc_rs_controls);
1856  break;
1857  case hc_cc_rs::TcTTcT:
1858  rs_turn_controls(*(p->cstart), *(p->qi1), true, hc_rs_controls);
1859  hc_turn_controls(*(p->ci1), *(p->qi1), false, hc_rs_controls);
1860  hc_turn_controls(*(p->ci2), *(p->qi2), true, hc_rs_controls);
1861  hc_turn_controls(*(p->cend), *(p->qi2), false, hc_rs_controls);
1862  break;
1863  // ########################################################################
1864  case hc_cc_rs::TTT:
1865  hc_turn_controls(*(p->cstart), *(p->qi1), false, hc_rs_controls);
1866  cc_turn_controls(*(p->ci1), *(p->qi2), true, hc_rs_controls);
1867  cc_turn_controls(*(p->cend), *(p->qi2), false, hc_rs_controls);
1868  break;
1869  case hc_cc_rs::TcST:
1870  rs_turn_controls(*(p->cstart), *(p->qi1), true, hc_rs_controls);
1871  straight_controls(*(p->qi1), *(p->qi2), hc_rs_controls);
1872  cc_turn_controls(*(p->cend), *(p->qi2), false, hc_rs_controls);
1873  break;
1874  case hc_cc_rs::TScT:
1875  hc_turn_controls(*(p->cstart), *(p->qi1), false, hc_rs_controls);
1876  straight_controls(*(p->qi2), *(p->qi3), hc_rs_controls);
1877  hc_turn_controls(*(p->cend), *(p->qi3), false, hc_rs_controls);
1878  break;
1879  case hc_cc_rs::TcScT:
1880  rs_turn_controls(*(p->cstart), *(p->qi1), true, hc_rs_controls);
1881  straight_controls(*(p->qi1), *(p->qi2), hc_rs_controls);
1882  hc_turn_controls(*(p->cend), *(p->qi2), false, hc_rs_controls);
1883  break;
1884  default:
1885  break;
1886  }
1887  delete p;
1888  return hc_rs_controls;
1889 }
1890 
1891 } // namespace steering
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:953
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::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TTcT_exists
bool TTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:317
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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, HC_CC_Circle **ci) const
Definition: hcpm0_reeds_shepp_state_space.cpp:714
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::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:881
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp
Definition: hcpm0_reeds_shepp_state_space.cpp:41
steering::State
Description of a kinematic car's state.
Definition: steering_functions.hpp:44
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1236
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TSTcT_exists
bool TSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:569
CC_REGULAR
#define CC_REGULAR
Definition: hcpm0_reeds_shepp_state_space.cpp:34
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
steering::HC_CC_RS_Path::type
hc_cc_rs::path_type type
Path type.
Definition: paths.hpp:164
steering::HCpm0_Reeds_Shepp_State_Space::hcpm0_reeds_shepp
HC_CC_RS_Path * hcpm0_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: hcpm0_reeds_shepp_state_space.cpp:1701
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:427
steering::HC_CC_State_Space::sigma_
double sigma_
Definition: hc_cc_state_space.hpp:95
plot_states.path
path
Definition: plot_states.py:88
steering::HC_CC_Circle_Param::mu
double mu
Angle between the initial orientation and the tangent to the circle at the initial position.
Definition: hc_cc_circle.hpp:94
steering::hc_cc_rs::TcTT
@ TcTT
Definition: paths.hpp:132
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:330
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TST_exists
bool TST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:422
steering::State::theta
double theta
Orientation of the robot.
Definition: steering_functions.hpp:70
configuration.hpp
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcTST_exists
bool TcTST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:665
steering::HC_CC_RS_Path::cend
HC_CC_Circle * cend
Definition: paths.hpp:170
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcScT_exists
bool TcScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1417
steering::HC_CC_Circle::left
bool left
Turning direction: left/right.
Definition: hc_cc_circle.hpp:123
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:185
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcTiST_exists
bool TcTiST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:638
steering::empty_controls
void empty_controls(std::vector< Control > &controls)
Appends controls with 0 input.
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TTT_exists
bool TTT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1035
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TceST_exists
bool TceST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1126
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcTSTcT_exists
bool TcTSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:756
steering::State::kappa
double kappa
Curvature at position (x,y)
Definition: steering_functions.hpp:73
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::HCpm0_Reeds_Shepp
HCpm0_Reeds_Shepp(HCpm0_Reeds_Shepp_State_Space *parent)
Definition: hcpm0_reeds_shepp_state_space.cpp:47
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TSTcT_path
double TSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4, HC_CC_Circle **ci) const
Definition: hcpm0_reeds_shepp_state_space.cpp:622
steering::HCpm0_Reeds_Shepp_State_Space::hcpm0_reeds_shepp_
std::unique_ptr< HCpm0_Reeds_Shepp > hcpm0_reeds_shepp_
Pimpl Idiom: unique pointer on class with families
Definition: hcpm0_reeds_shepp_state_space.hpp:120
steering::hc_cc_rs::TcTTcT
@ TcTTcT
Definition: paths.hpp:139
steering::hc_cc_rs::TT
@ TT
Definition: paths.hpp:128
steering::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:1783
steering::HC_CC_Circle
Definition: hc_cc_circle.hpp:80
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:1422
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:761
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TceScT_exists
bool TceScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1404
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:788
steering::HC_CC_Circle_Param::cos_mu
double cos_mu
Definition: hc_cc_circle.hpp:97
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TScT_exists
bool TScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1277
steering::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:1791
HC_REGULAR
#define HC_REGULAR
Definition: hcpm0_reeds_shepp_state_space.cpp:33
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::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:527
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::parent_
HCpm0_Reeds_Shepp_State_Space * parent_
Definition: hcpm0_reeds_shepp_state_space.cpp:44
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TT_tangent_circles
void TT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q) const
Definition: hcpm0_reeds_shepp_state_space.cpp:69
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:351
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::HC_CC_RS_Path
Definition: paths.hpp:149
steering::configuration_distance
double configuration_distance(const Configuration &q1, const Configuration &q2)
Cartesian distance between two configurations.
Definition: configuration.cpp:54
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcT_tangent_circles
void TcT_tangent_circles(const HC_CC_Circle &c1, const HC_CC_Circle &c2, Configuration **q) const
Definition: hcpm0_reeds_shepp_state_space.cpp:124
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcTeST_exists
bool TcTeST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:652
steering::hc_cc_rs::TcTcT
@ TcTcT
Definition: paths.hpp:131
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcTcT_exists
bool TcTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:172
steering::HC_CC_Circle::yc
double yc
Definition: hc_cc_circle.hpp:132
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TiSTcT_path
double TiSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4, HC_CC_Circle **ci) const
Definition: hcpm0_reeds_shepp_state_space.cpp:574
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:982
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcTiSTcT_exists
bool TcTiSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:729
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TiST_exists
bool TiST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:396
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::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:815
steering::HC_CC_Circle::forward
bool forward
Driving direction: forwards/backwards.
Definition: hc_cc_circle.hpp:126
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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, HC_CC_Circle **ci) const
Definition: hcpm0_reeds_shepp_state_space.cpp:692
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:507
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1144
steering::HCpm0_Reeds_Shepp_State_Space::rs_circle_param_
HC_CC_Circle_Param rs_circle_param_
Parameter of a rs-circle.
Definition: hcpm0_reeds_shepp_state_space.hpp:126
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::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TeST_exists
bool TeST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:409
steering::HC_CC_RS_Path::qi1
Configuration * qi1
Intermediate configurations.
Definition: paths.hpp:167
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcTeSTcT_exists
bool TcTeSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:743
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:517
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::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:205
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:1048
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TTcTT_exists
bool TTcTT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:831
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TiScT_path
double TiScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend, Configuration **q1, Configuration **q2, Configuration **q3) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1282
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::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
hcpm0_reeds_shepp_state_space.hpp
distance
double distance(double x0, double y0, double x1, double y1)
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:1506
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:160
steering::HCpm0_Reeds_Shepp_State_Space
An implementation of hybrid curvature (HC) steer with either positive (p) or negative (n) max....
Definition: hcpm0_reeds_shepp_state_space.hpp:74
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TiSTcT_exists
bool TiSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:542
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TciScT_exists
bool TciScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1391
steering::HC_CC_Circle_Param::sin_mu
double sin_mu
Sine and cosine of mu.
Definition: hc_cc_circle.hpp:97
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:257
steering::HC_CC_State_Space
Definition: hc_cc_state_space.hpp:45
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TiScT_exists
bool TiScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1251
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
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcT_exists
bool TcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:111
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::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::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:470
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TeSTcT_exists
bool TeSTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:556
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1190
std
steering::HC_CC_Circle_Param::kappa
double kappa
Max. curvature, inverse of max. curvature, max. sharpness.
Definition: hc_cc_circle.hpp:88
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:278
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:100
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:844
plot_states.kappa
kappa
Definition: plot_states.py:106
steering::hc_cc_rs::TcT
@ TcT
Definition: paths.hpp:129
steering::hc_cc_rs::TcTST
@ TcTST
Definition: paths.hpp:136
steering::HCpm0_Reeds_Shepp_State_Space::hcpm0_circles_rs_path
HC_CC_RS_Path * hcpm0_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: hcpm0_reeds_shepp_state_space.cpp:1534
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcTT_exists
bool TcTT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:244
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TScT_path
double TScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend, Configuration **q1, Configuration **q2, Configuration **q3) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1376
steering::HCpm0_Reeds_Shepp_State_Space::~HCpm0_Reeds_Shepp_State_Space
~HCpm0_Reeds_Shepp_State_Space()
Destructor.
steering::hc_cc_rs::TTT
@ TTT
Definition: paths.hpp:141
steering::hc_cc_rs::E
@ E
Definition: paths.hpp:125
steering::double_array_init
void double_array_init(double array[], int size, double value)
Initialize an array with a given value.
Definition: utilities.cpp:256
steering
Definition: dubins_state_space.hpp:70
steering::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::HCpm0_Reeds_Shepp_State_Space::HCpm0_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, HC_CC_Circle **ci) const
Definition: hcpm0_reeds_shepp_state_space.cpp:670
steering::hc_cc_rs::path_type
path_type
Definition: paths.hpp:123
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TT_exists
bool TT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:56
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TciST_exists
bool TciST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1113
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcTTcT_exists
bool TcTTcT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:940
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TeSTcT_path
double TeSTcT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend, Configuration **q1, Configuration **q2, Configuration **q3, Configuration **q4, HC_CC_Circle **ci) const
Definition: hcpm0_reeds_shepp_state_space.cpp:598
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::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::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TeScT_path
double TeScT_path(const HC_CC_Circle &c1, const HC_CC_Circle &c2, HC_CC_Circle **cstart, HC_CC_Circle **cend, Configuration **q1, Configuration **q2, Configuration **q3) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1329
steering::HC_CC_Circle_Param::radius
double radius
Radius of the outer circle.
Definition: hc_cc_circle.hpp:91
steering::Path::start
Configuration start
Start and end configuration.
Definition: paths.hpp:92
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_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: hcpm0_reeds_shepp_state_space.cpp:1466
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::HCpm0_Reeds_Shepp_State_Space::HCpm0_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, HC_CC_Circle **ci) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1068
utilities.hpp
steering::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TeScT_exists
bool TeScT_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1264
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::HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp::TcST_exists
bool TcST_exists(const HC_CC_Circle &c1, const HC_CC_Circle &c2) const
Definition: hcpm0_reeds_shepp_state_space.cpp:1139
steering::HC_CC_RS_Path::qi2
Configuration * qi2
Definition: paths.hpp:167
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