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"
11 // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
13 const double pi = M_PI;
14 const double twopi = 2. * pi;
15 const double RS_EPS = 1e-6;
16 const double ZERO = 10*std::numeric_limits<double>::epsilon();
18 inline double mod2pi(double x)
20 double v = fmod(x, twopi);
28 inline void polar(double x, double y, double &r, double &theta)
33 inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
35 double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
36 double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
37 tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
38 omega = mod2pi(tau - u + v - phi) ;
41 // formula 8.1 in Reeds-Shepp paper
42 inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
44 polar(x - sin(phi), y - 1. + cos(phi), u, t);
50 assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
51 assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
52 assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
59 inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
62 polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
69 t = mod2pi(t1 + theta);
71 assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
72 assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
73 assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
74 return t>=-ZERO && v>=-ZERO;
78 void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
80 double t, u, v, Lmin = path.length(), L;
81 if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
83 path = ReedsSheppStateSpace::ReedsSheppPath(
84 ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
87 if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
89 path = ReedsSheppStateSpace::ReedsSheppPath(
90 ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
93 if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
95 path = ReedsSheppStateSpace::ReedsSheppPath(
96 ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
99 if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
101 path = ReedsSheppStateSpace::ReedsSheppPath(
102 ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
105 if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
107 path = ReedsSheppStateSpace::ReedsSheppPath(
108 ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
111 if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
113 path = ReedsSheppStateSpace::ReedsSheppPath(
114 ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
117 if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
119 path = ReedsSheppStateSpace::ReedsSheppPath(
120 ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
123 if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
124 path = ReedsSheppStateSpace::ReedsSheppPath(
125 ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
127 // formula 8.3 / 8.4 *** TYPO IN PAPER ***
128 inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
130 double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
131 polar(xi, eta, u1, theta);
134 u = -2.*asin(.25 * u1);
135 t = mod2pi(theta + .5 * u + pi);
136 v = mod2pi(phi - t + u);
137 assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
138 assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
139 assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
140 return t>=-ZERO && u<=ZERO;
144 void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
146 double t, u, v, Lmin = path.length(), L;
147 if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
149 path = ReedsSheppStateSpace::ReedsSheppPath(
150 ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
153 if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
155 path = ReedsSheppStateSpace::ReedsSheppPath(
156 ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
159 if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
161 path = ReedsSheppStateSpace::ReedsSheppPath(
162 ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
165 if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
167 path = ReedsSheppStateSpace::ReedsSheppPath(
168 ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
173 double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
174 if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
176 path = ReedsSheppStateSpace::ReedsSheppPath(
177 ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
180 if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
182 path = ReedsSheppStateSpace::ReedsSheppPath(
183 ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
186 if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
188 path = ReedsSheppStateSpace::ReedsSheppPath(
189 ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
192 if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
193 path = ReedsSheppStateSpace::ReedsSheppPath(
194 ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
197 inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
199 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
203 tauOmega(u, -u, xi, eta, phi, t, v);
204 assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
205 assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
206 assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
207 return t>=-ZERO && v<=ZERO;
212 inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
214 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
215 if (rho>=0 && rho<=1)
220 tauOmega(u, u, xi, eta, phi, t, v);
221 assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
222 assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
223 assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
224 return t>=-ZERO && v>=-ZERO;
229 void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
231 double t, u, v, Lmin = path.length(), L;
232 if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
234 path = ReedsSheppStateSpace::ReedsSheppPath(
235 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
238 if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
240 path = ReedsSheppStateSpace::ReedsSheppPath(
241 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
244 if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
246 path = ReedsSheppStateSpace::ReedsSheppPath(
247 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
250 if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
252 path = ReedsSheppStateSpace::ReedsSheppPath(
253 ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
257 if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
259 path = ReedsSheppStateSpace::ReedsSheppPath(
260 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
263 if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
265 path = ReedsSheppStateSpace::ReedsSheppPath(
266 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
269 if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
271 path = ReedsSheppStateSpace::ReedsSheppPath(
272 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
275 if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
276 path = ReedsSheppStateSpace::ReedsSheppPath(
277 ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
280 inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
282 double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
283 polar(xi, eta, rho, theta);
286 double r = sqrt(rho*rho - 4.);
288 t = mod2pi(theta + atan2(r, -2.));
289 v = mod2pi(phi - .5*pi - t);
290 assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
291 assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
292 assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
293 return t>=-ZERO && u<=ZERO && v<=ZERO;
298 inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
300 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
301 polar(-eta, xi, rho, theta);
306 v = mod2pi(t + .5*pi - phi);
307 assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
308 assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
309 assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
310 return t>=-ZERO && u<=ZERO && v<=ZERO;
314 void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
316 double t, u, v, Lmin = path.length() - .5*pi, L;
317 if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
319 path = ReedsSheppStateSpace::ReedsSheppPath(
320 ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
323 if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
325 path = ReedsSheppStateSpace::ReedsSheppPath(
326 ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
329 if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
331 path = ReedsSheppStateSpace::ReedsSheppPath(
332 ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
335 if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
337 path = ReedsSheppStateSpace::ReedsSheppPath(
338 ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
342 if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
344 path = ReedsSheppStateSpace::ReedsSheppPath(
345 ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
348 if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
350 path = ReedsSheppStateSpace::ReedsSheppPath(
351 ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
354 if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
356 path = ReedsSheppStateSpace::ReedsSheppPath(
357 ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
360 if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
362 path = ReedsSheppStateSpace::ReedsSheppPath(
363 ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
368 double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
369 if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
371 path = ReedsSheppStateSpace::ReedsSheppPath(
372 ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
375 if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
377 path = ReedsSheppStateSpace::ReedsSheppPath(
378 ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
381 if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
383 path = ReedsSheppStateSpace::ReedsSheppPath(
384 ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
387 if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
389 path = ReedsSheppStateSpace::ReedsSheppPath(
390 ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
394 if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
396 path = ReedsSheppStateSpace::ReedsSheppPath(
397 ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
400 if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
402 path = ReedsSheppStateSpace::ReedsSheppPath(
403 ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
406 if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
408 path = ReedsSheppStateSpace::ReedsSheppPath(
409 ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
412 if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
413 path = ReedsSheppStateSpace::ReedsSheppPath(
414 ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
416 // formula 8.11 *** TYPO IN PAPER ***
417 inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
419 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
420 polar(xi, eta, rho, theta);
423 u = 4. - sqrt(rho*rho - 4.);
426 t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
428 assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
429 assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
430 assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
431 return t>=-ZERO && v>=-ZERO;
436 void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
438 double t, u, v, Lmin = path.length() - pi, L;
439 if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
441 path = ReedsSheppStateSpace::ReedsSheppPath(
442 ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
445 if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
447 path = ReedsSheppStateSpace::ReedsSheppPath(
448 ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
451 if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
453 path = ReedsSheppStateSpace::ReedsSheppPath(
454 ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
457 if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
458 path = ReedsSheppStateSpace::ReedsSheppPath(
459 ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
462 ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
464 ReedsSheppStateSpace::ReedsSheppPath path;
465 CSC(x, y, phi, path);
466 CCC(x, y, phi, path);
467 CCCC(x, y, phi, path);
468 CCSC(x, y, phi, path);
469 CCSCC(x, y, phi, path);
474 const ReedsSheppStateSpace::ReedsSheppPathSegmentType
475 ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
476 { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 0
477 { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP }, // 1
478 { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 2
479 { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP }, // 3
480 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 4
481 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 5
482 { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 6
483 { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 7
484 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 8
485 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 9
486 { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 10
487 { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 11
488 { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 12
489 { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 13
490 { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 14
491 { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 15
492 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT }, // 16
493 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT } // 17
496 ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
497 double t, double u, double v, double w, double x)
500 length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
501 totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
505 double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
507 return rho_ * reedsShepp(q0, q1).length();
510 ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
512 double dx = q1[0] - q0[0], dy = q1[1] - q0[1], dth = q1[2] - q0[2];
513 double c = cos(q0[2]), s = sin(q0[2]);
514 double x = c*dx + s*dy, y = -s*dx + c*dy;
515 return ::reedsShepp(x/rho_, y/rho_, dth);
518 void ReedsSheppStateSpace::type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data)
520 ReedsSheppPath path = reedsShepp(q0, q1);
521 for (int i=0;i<5;++i)
522 cb(path.type_[i], user_data);
526 void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data)
528 ReedsSheppPath path = reedsShepp(q0, q1);
529 double dist = rho_ * path.length();
531 for (double seg=0.0; seg<=dist; seg+=step_size){
533 interpolate(q0, path, seg/rho_, qnew);
534 cb( qnew, user_data);
539 void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[5])
542 if (seg < 0.0) seg = 0.0;
543 if (seg > path.length()) seg = path.length();
550 for (unsigned int i=0; i<5 && seg>0; ++i)
552 if (path.length_[i]<0)
554 v = std::max(-seg, path.length_[i]);
559 v = std::min(seg, path.length_[i]);
563 switch(path.type_[i])
566 s[0] += ( sin(phi+v) - sin(phi));
567 s[1] += (-cos(phi+v) + cos(phi));
572 s[0] += (-sin(phi-v) + sin(phi));
573 s[1] += ( cos(phi-v) - cos(phi));
578 s[0] += (v * cos(phi));
579 s[1] += (v * sin(phi));
587 s[0] = s[0] * rho_ + q0[0];
588 s[1] = s[1] * rho_ + q0[1];