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

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