1 /*********************************************************************
3 * Software License Agreement (BSD License)
5 * Copyright (c) 2016, Guan-Horng Liu.
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the the copyright holder nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
35 * Author: Guan-Horng Liu
36 *********************************************************************/
39 #include "reeds_shepp.h"
40 #include <boost/math/constants/constants.hpp>
45 // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
47 const double pi = boost::math::constants::pi<double>();
48 const double twopi = 2. * pi;
49 const double RS_EPS = 1e-6;
50 const double ZERO = 10*std::numeric_limits<double>::epsilon();
52 inline double mod2pi(double x)
54 double v = fmod(x, twopi);
62 inline void polar(double x, double y, double &r, double &theta)
67 inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
69 double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
70 double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
71 tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
72 omega = mod2pi(tau - u + v - phi) ;
75 // formula 8.1 in Reeds-Shepp paper
76 inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
78 polar(x - sin(phi), y - 1. + cos(phi), u, t);
84 assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
85 assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
86 assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
93 inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
96 polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
102 theta = atan2(2., u);
103 t = mod2pi(t1 + theta);
105 assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
106 assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
107 assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
108 return t>=-ZERO && v>=-ZERO;
112 void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
114 double t, u, v, Lmin = path.length(), L;
115 if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
117 path = ReedsSheppStateSpace::ReedsSheppPath(
118 ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
121 if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
123 path = ReedsSheppStateSpace::ReedsSheppPath(
124 ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
127 if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
129 path = ReedsSheppStateSpace::ReedsSheppPath(
130 ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
133 if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
135 path = ReedsSheppStateSpace::ReedsSheppPath(
136 ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
139 if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
141 path = ReedsSheppStateSpace::ReedsSheppPath(
142 ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
145 if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
147 path = ReedsSheppStateSpace::ReedsSheppPath(
148 ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
151 if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
153 path = ReedsSheppStateSpace::ReedsSheppPath(
154 ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
157 if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
158 path = ReedsSheppStateSpace::ReedsSheppPath(
159 ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
161 // formula 8.3 / 8.4 *** TYPO IN PAPER ***
162 inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
164 double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
165 polar(xi, eta, u1, theta);
168 u = -2.*asin(.25 * u1);
169 t = mod2pi(theta + .5 * u + pi);
170 v = mod2pi(phi - t + u);
171 assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
172 assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
173 assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
174 return t>=-ZERO && u<=ZERO;
178 void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
180 double t, u, v, Lmin = path.length(), L;
181 if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
183 path = ReedsSheppStateSpace::ReedsSheppPath(
184 ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
187 if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
189 path = ReedsSheppStateSpace::ReedsSheppPath(
190 ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
193 if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
195 path = ReedsSheppStateSpace::ReedsSheppPath(
196 ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
199 if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
201 path = ReedsSheppStateSpace::ReedsSheppPath(
202 ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
207 double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
208 if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
210 path = ReedsSheppStateSpace::ReedsSheppPath(
211 ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
214 if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
216 path = ReedsSheppStateSpace::ReedsSheppPath(
217 ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
220 if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
222 path = ReedsSheppStateSpace::ReedsSheppPath(
223 ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
226 if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
227 path = ReedsSheppStateSpace::ReedsSheppPath(
228 ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
231 inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
233 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
237 tauOmega(u, -u, xi, eta, phi, t, v);
238 assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
239 assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
240 assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
241 return t>=-ZERO && v<=ZERO;
246 inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
248 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
249 if (rho>=0 && rho<=1)
254 tauOmega(u, u, xi, eta, phi, t, v);
255 assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
256 assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
257 assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
258 return t>=-ZERO && v>=-ZERO;
263 void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
265 double t, u, v, Lmin = path.length(), L;
266 if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
268 path = ReedsSheppStateSpace::ReedsSheppPath(
269 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
272 if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
274 path = ReedsSheppStateSpace::ReedsSheppPath(
275 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
278 if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
280 path = ReedsSheppStateSpace::ReedsSheppPath(
281 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
284 if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
286 path = ReedsSheppStateSpace::ReedsSheppPath(
287 ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
291 if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
293 path = ReedsSheppStateSpace::ReedsSheppPath(
294 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
297 if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
299 path = ReedsSheppStateSpace::ReedsSheppPath(
300 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
303 if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
305 path = ReedsSheppStateSpace::ReedsSheppPath(
306 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
309 if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
310 path = ReedsSheppStateSpace::ReedsSheppPath(
311 ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
314 inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
316 double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
317 polar(xi, eta, rho, theta);
320 double r = sqrt(rho*rho - 4.);
322 t = mod2pi(theta + atan2(r, -2.));
323 v = mod2pi(phi - .5*pi - t);
324 assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
325 assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
326 assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
327 return t>=-ZERO && u<=ZERO && v<=ZERO;
332 inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
334 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
335 polar(-eta, xi, rho, theta);
340 v = mod2pi(t + .5*pi - phi);
341 assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
342 assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
343 assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
344 return t>=-ZERO && u<=ZERO && v<=ZERO;
348 void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
350 double t, u, v, Lmin = path.length() - .5*pi, L;
351 if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
353 path = ReedsSheppStateSpace::ReedsSheppPath(
354 ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
357 if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
359 path = ReedsSheppStateSpace::ReedsSheppPath(
360 ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
363 if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
365 path = ReedsSheppStateSpace::ReedsSheppPath(
366 ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
369 if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
371 path = ReedsSheppStateSpace::ReedsSheppPath(
372 ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
376 if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
378 path = ReedsSheppStateSpace::ReedsSheppPath(
379 ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
382 if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
384 path = ReedsSheppStateSpace::ReedsSheppPath(
385 ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
388 if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
390 path = ReedsSheppStateSpace::ReedsSheppPath(
391 ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
394 if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
396 path = ReedsSheppStateSpace::ReedsSheppPath(
397 ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
402 double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
403 if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
405 path = ReedsSheppStateSpace::ReedsSheppPath(
406 ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
409 if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
411 path = ReedsSheppStateSpace::ReedsSheppPath(
412 ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
415 if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
417 path = ReedsSheppStateSpace::ReedsSheppPath(
418 ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
421 if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
423 path = ReedsSheppStateSpace::ReedsSheppPath(
424 ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
428 if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
430 path = ReedsSheppStateSpace::ReedsSheppPath(
431 ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
434 if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
436 path = ReedsSheppStateSpace::ReedsSheppPath(
437 ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
440 if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
442 path = ReedsSheppStateSpace::ReedsSheppPath(
443 ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
446 if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
447 path = ReedsSheppStateSpace::ReedsSheppPath(
448 ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
450 // formula 8.11 *** TYPO IN PAPER ***
451 inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
453 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
454 polar(xi, eta, rho, theta);
457 u = 4. - sqrt(rho*rho - 4.);
460 t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
462 assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
463 assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
464 assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
465 return t>=-ZERO && v>=-ZERO;
470 void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
472 double t, u, v, Lmin = path.length() - pi, L;
473 if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
475 path = ReedsSheppStateSpace::ReedsSheppPath(
476 ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
479 if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
481 path = ReedsSheppStateSpace::ReedsSheppPath(
482 ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
485 if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
487 path = ReedsSheppStateSpace::ReedsSheppPath(
488 ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
491 if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
492 path = ReedsSheppStateSpace::ReedsSheppPath(
493 ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
496 ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
498 ReedsSheppStateSpace::ReedsSheppPath path;
499 CSC(x, y, phi, path);
500 CCC(x, y, phi, path);
501 CCCC(x, y, phi, path);
502 CCSC(x, y, phi, path);
503 CCSCC(x, y, phi, path);
508 const ReedsSheppStateSpace::ReedsSheppPathSegmentType
509 ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
510 { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 0
511 { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP }, // 1
512 { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 2
513 { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP }, // 3
514 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 4
515 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 5
516 { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 6
517 { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 7
518 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 8
519 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 9
520 { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 10
521 { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 11
522 { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 12
523 { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 13
524 { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 14
525 { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 15
526 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT }, // 16
527 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT } // 17
530 ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
531 double t, double u, double v, double w, double x)
534 length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
535 totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
539 double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
541 return rho_ * reedsShepp(q0, q1).length();
544 ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
546 double dx = q1[0] - q0[0], dy = q1[1] - q0[1], dth = q1[2] - q0[2];
547 double c = cos(q0[2]), s = sin(q0[2]);
548 double x = c*dx + s*dy, y = -s*dx + c*dy;
549 return ::reedsShepp(x/rho_, y/rho_, dth);
552 void ReedsSheppStateSpace::type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data)
554 ReedsSheppPath path = reedsShepp(q0, q1);
555 for (int i=0;i<5;++i)
556 cb(path.type_[i], user_data);
560 void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data)
562 ReedsSheppPath path = reedsShepp(q0, q1);
563 double dist = rho_ * path.length(); // TODO shorten path here
565 for (double seg=0.0; seg<=dist; seg+=step_size){
567 interpolate(q0, path, seg/rho_, qnew);
568 cb( qnew, user_data);
573 void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[4])
576 if (seg < 0.0) seg = 0.0;
577 if (seg > path.length()) seg = path.length();
585 for (unsigned int i=0; i<5 && seg>0; ++i)
587 if (path.length_[i]<0)
589 v = std::max(-seg, path.length_[i]);
594 v = std::min(seg, path.length_[i]);
598 switch(path.type_[i])
601 s[0] += ( sin(phi+v) - sin(phi));
602 s[1] += (-cos(phi+v) + cos(phi));
606 s[0] += (-sin(phi-v) + sin(phi));
607 s[1] += ( cos(phi-v) - cos(phi));
611 s[0] += (v * cos(phi));
612 s[1] += (v * sin(phi));
619 s[0] = s[0] * rho_ + q0[0];
620 s[1] = s[1] * rho_ + q0[1];