1 // Copyright (c) 2016, Guan-Horng Liu.
2 // SPDX-FileCopyrightText: 2016 Guan-Horng Liu
4 // SPDX-License-Identifier: BSD-3-Clause
6 #include "reeds_shepp.h"
7 #include <boost/math/constants/constants.hpp>
12 // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
14 const double pi = boost::math::constants::pi<double>();
15 const double twopi = 2. * pi;
16 const double RS_EPS = 1e-6;
17 const double ZERO = 10*std::numeric_limits<double>::epsilon();
19 inline double mod2pi(double x)
21 double v = fmod(x, twopi);
29 inline void polar(double x, double y, double &r, double &theta)
34 inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
36 double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
37 double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
38 tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
39 omega = mod2pi(tau - u + v - phi) ;
42 // formula 8.1 in Reeds-Shepp paper
43 inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
45 polar(x - sin(phi), y - 1. + cos(phi), u, t);
51 assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
52 assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
53 assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
60 inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
63 polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
70 t = mod2pi(t1 + theta);
72 assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
73 assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
74 assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
75 return t>=-ZERO && v>=-ZERO;
79 void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
81 double t, u, v, Lmin = path.length(), L;
82 if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
84 path = ReedsSheppStateSpace::ReedsSheppPath(
85 ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
88 if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
90 path = ReedsSheppStateSpace::ReedsSheppPath(
91 ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
94 if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
96 path = ReedsSheppStateSpace::ReedsSheppPath(
97 ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
100 if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
102 path = ReedsSheppStateSpace::ReedsSheppPath(
103 ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
106 if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
108 path = ReedsSheppStateSpace::ReedsSheppPath(
109 ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
112 if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
114 path = ReedsSheppStateSpace::ReedsSheppPath(
115 ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
118 if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
120 path = ReedsSheppStateSpace::ReedsSheppPath(
121 ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
124 if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
125 path = ReedsSheppStateSpace::ReedsSheppPath(
126 ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
128 // formula 8.3 / 8.4 *** TYPO IN PAPER ***
129 inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
131 double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
132 polar(xi, eta, u1, theta);
135 u = -2.*asin(.25 * u1);
136 t = mod2pi(theta + .5 * u + pi);
137 v = mod2pi(phi - t + u);
138 assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
139 assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
140 assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
141 return t>=-ZERO && u<=ZERO;
145 void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
147 double t, u, v, Lmin = path.length(), L;
148 if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
150 path = ReedsSheppStateSpace::ReedsSheppPath(
151 ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
154 if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
156 path = ReedsSheppStateSpace::ReedsSheppPath(
157 ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
160 if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
162 path = ReedsSheppStateSpace::ReedsSheppPath(
163 ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
166 if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
168 path = ReedsSheppStateSpace::ReedsSheppPath(
169 ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
174 double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
175 if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
177 path = ReedsSheppStateSpace::ReedsSheppPath(
178 ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
181 if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
183 path = ReedsSheppStateSpace::ReedsSheppPath(
184 ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
187 if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
189 path = ReedsSheppStateSpace::ReedsSheppPath(
190 ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
193 if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
194 path = ReedsSheppStateSpace::ReedsSheppPath(
195 ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
198 inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
200 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
204 tauOmega(u, -u, xi, eta, phi, t, v);
205 assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
206 assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
207 assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
208 return t>=-ZERO && v<=ZERO;
213 inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
215 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
216 if (rho>=0 && rho<=1)
221 tauOmega(u, u, xi, eta, phi, t, v);
222 assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
223 assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
224 assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
225 return t>=-ZERO && v>=-ZERO;
230 void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
232 double t, u, v, Lmin = path.length(), L;
233 if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
235 path = ReedsSheppStateSpace::ReedsSheppPath(
236 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
239 if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
241 path = ReedsSheppStateSpace::ReedsSheppPath(
242 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
245 if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
247 path = ReedsSheppStateSpace::ReedsSheppPath(
248 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
251 if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
253 path = ReedsSheppStateSpace::ReedsSheppPath(
254 ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
258 if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
260 path = ReedsSheppStateSpace::ReedsSheppPath(
261 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
264 if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
266 path = ReedsSheppStateSpace::ReedsSheppPath(
267 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
270 if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
272 path = ReedsSheppStateSpace::ReedsSheppPath(
273 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
276 if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
277 path = ReedsSheppStateSpace::ReedsSheppPath(
278 ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
281 inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
283 double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
284 polar(xi, eta, rho, theta);
287 double r = sqrt(rho*rho - 4.);
289 t = mod2pi(theta + atan2(r, -2.));
290 v = mod2pi(phi - .5*pi - t);
291 assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
292 assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
293 assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
294 return t>=-ZERO && u<=ZERO && v<=ZERO;
299 inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
301 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
302 polar(-eta, xi, rho, theta);
307 v = mod2pi(t + .5*pi - phi);
308 assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
309 assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
310 assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
311 return t>=-ZERO && u<=ZERO && v<=ZERO;
315 void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
317 double t, u, v, Lmin = path.length() - .5*pi, L;
318 if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
320 path = ReedsSheppStateSpace::ReedsSheppPath(
321 ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
324 if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
326 path = ReedsSheppStateSpace::ReedsSheppPath(
327 ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
330 if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
332 path = ReedsSheppStateSpace::ReedsSheppPath(
333 ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
336 if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
338 path = ReedsSheppStateSpace::ReedsSheppPath(
339 ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
343 if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
345 path = ReedsSheppStateSpace::ReedsSheppPath(
346 ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
349 if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
351 path = ReedsSheppStateSpace::ReedsSheppPath(
352 ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
355 if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
357 path = ReedsSheppStateSpace::ReedsSheppPath(
358 ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
361 if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
363 path = ReedsSheppStateSpace::ReedsSheppPath(
364 ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
369 double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
370 if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
372 path = ReedsSheppStateSpace::ReedsSheppPath(
373 ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
376 if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
378 path = ReedsSheppStateSpace::ReedsSheppPath(
379 ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
382 if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
384 path = ReedsSheppStateSpace::ReedsSheppPath(
385 ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
388 if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
390 path = ReedsSheppStateSpace::ReedsSheppPath(
391 ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
395 if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
397 path = ReedsSheppStateSpace::ReedsSheppPath(
398 ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
401 if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
403 path = ReedsSheppStateSpace::ReedsSheppPath(
404 ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
407 if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
409 path = ReedsSheppStateSpace::ReedsSheppPath(
410 ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
413 if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
414 path = ReedsSheppStateSpace::ReedsSheppPath(
415 ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
417 // formula 8.11 *** TYPO IN PAPER ***
418 inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
420 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
421 polar(xi, eta, rho, theta);
424 u = 4. - sqrt(rho*rho - 4.);
427 t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
429 assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
430 assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
431 assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
432 return t>=-ZERO && v>=-ZERO;
437 void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
439 double t, u, v, Lmin = path.length() - pi, L;
440 if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
442 path = ReedsSheppStateSpace::ReedsSheppPath(
443 ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
446 if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
448 path = ReedsSheppStateSpace::ReedsSheppPath(
449 ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
452 if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
454 path = ReedsSheppStateSpace::ReedsSheppPath(
455 ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
458 if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
459 path = ReedsSheppStateSpace::ReedsSheppPath(
460 ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
463 ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
465 ReedsSheppStateSpace::ReedsSheppPath path;
466 CSC(x, y, phi, path);
467 CCC(x, y, phi, path);
468 CCCC(x, y, phi, path);
469 CCSC(x, y, phi, path);
470 CCSCC(x, y, phi, path);
475 const ReedsSheppStateSpace::ReedsSheppPathSegmentType
476 ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
477 { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 0
478 { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP }, // 1
479 { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 2
480 { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP }, // 3
481 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 4
482 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 5
483 { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 6
484 { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 7
485 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 8
486 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 9
487 { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 10
488 { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 11
489 { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 12
490 { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 13
491 { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 14
492 { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 15
493 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT }, // 16
494 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT } // 17
497 ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
498 double t, double u, double v, double w, double x)
501 length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
502 totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
506 double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
508 return rho_ * reedsShepp(q0, q1).length();
511 ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
513 double dx = q1[0] - q0[0], dy = q1[1] - q0[1], dth = q1[2] - q0[2];
514 double c = cos(q0[2]), s = sin(q0[2]);
515 double x = c*dx + s*dy, y = -s*dx + c*dy;
516 return ::reedsShepp(x/rho_, y/rho_, dth);
519 void ReedsSheppStateSpace::type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data)
521 ReedsSheppPath path = reedsShepp(q0, q1);
522 for (int i=0;i<5;++i)
523 cb(path.type_[i], user_data);
527 void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data)
529 ReedsSheppPath path = reedsShepp(q0, q1);
530 double dist = rho_ * path.length();
532 for (double seg=0.0; seg<=dist; seg+=step_size){
534 interpolate(q0, path, seg/rho_, qnew);
535 cb( qnew, user_data);
540 void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[4])
543 if (seg < 0.0) seg = 0.0;
544 if (seg > path.length()) seg = path.length();
551 for (unsigned int i=0; i<5 && seg>0; ++i)
553 if (path.length_[i]<0)
555 v = std::max(-seg, path.length_[i]);
560 v = std::min(seg, path.length_[i]);
564 switch(path.type_[i])
567 s[0] += ( sin(phi+v) - sin(phi));
568 s[1] += (-cos(phi+v) + cos(phi));
572 s[0] += (-sin(phi-v) + sin(phi));
573 s[1] += ( cos(phi-v) - cos(phi));
577 s[0] += (v * cos(phi));
578 s[1] += (v * sin(phi));
585 s[0] = s[0] * rho_ + q0[0];
586 s[1] = s[1] * rho_ + q0[1];