src/reeds_shepp.cpp

   1 // Copyright (c) 2016, Guan-Horng Liu.
   2 // SPDX-FileCopyrightText: 2016 Guan-Horng Liu
   3 //
   4 // SPDX-License-Identifier: BSD-3-Clause
   5
   6 #include "reeds_shepp.h"
   7 #include <cmath>
   8
   9 namespace
  10 {
  11     // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
  12
  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();
  17
  18     inline double mod2pi(double x)
  19     {
  20         double v = fmod(x, twopi);
  21         if (v < -pi)
  22             v += twopi;
  23         else
  24             if (v > pi)
  25                 v -= twopi;
  26         return v;
  27     }
  28     inline void polar(double x, double y, double &r, double &theta)
  29     {
  30         r = sqrt(x*x + y*y);
  31         theta = atan2(y, x);
  32     }
  33     inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
  34     {
  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) ;
  39     }
  40
  41     // formula 8.1 in Reeds-Shepp paper
  42     inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
  43     {
  44         polar(x - sin(phi), y - 1. + cos(phi), u, t);
  45         if (t >= -ZERO)
  46         {
  47             v = mod2pi(phi - t);
  48             if (v >= -ZERO)
  49             {
  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);
  53                 return true;
  54             }
  55         }
  56         return false;
  57     }
  58     // formula 8.2
  59     inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
  60     {
  61         double t1, u1;
  62         polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
  63         u1 = u1*u1;
  64         if (u1 >= 4.)
  65         {
  66             double theta;
  67             u = sqrt(u1 - 4.);
  68             theta = atan2(2., u);
  69             t = mod2pi(t1 + theta);
  70             v = mod2pi(t - phi);
  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;
  75         }
  76         return false;
  77     }
  78     void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
  79     {
  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)))
  82         {
  83             path = ReedsSheppStateSpace::ReedsSheppPath(
  84                 ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
  85             Lmin = L;
  86         }
  87         if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
  88         {
  89             path = ReedsSheppStateSpace::ReedsSheppPath(
  90                 ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
  91             Lmin = L;
  92         }
  93         if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
  94         {
  95             path = ReedsSheppStateSpace::ReedsSheppPath(
  96                 ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
  97             Lmin = L;
  98         }
  99         if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
 100         {
 101             path = ReedsSheppStateSpace::ReedsSheppPath(
 102                 ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
 103             Lmin = L;
 104         }
 105         if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
 106         {
 107             path = ReedsSheppStateSpace::ReedsSheppPath(
 108                 ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
 109             Lmin = L;
 110         }
 111         if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 112         {
 113             path = ReedsSheppStateSpace::ReedsSheppPath(
 114                 ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
 115             Lmin = L;
 116         }
 117         if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 118         {
 119             path = ReedsSheppStateSpace::ReedsSheppPath(
 120                 ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
 121             Lmin = L;
 122         }
 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);
 126     }
 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)
 129     {
 130         double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
 131         polar(xi, eta, u1, theta);
 132         if (u1 <= 4.)
 133         {
 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;
 141         }
 142         return false;
 143     }
 144     void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
 145     {
 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)))
 148         {
 149             path = ReedsSheppStateSpace::ReedsSheppPath(
 150                 ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
 151             Lmin = L;
 152         }
 153         if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 154         {
 155             path = ReedsSheppStateSpace::ReedsSheppPath(
 156                 ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
 157             Lmin = L;
 158         }
 159         if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 160         {
 161             path = ReedsSheppStateSpace::ReedsSheppPath(
 162                 ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
 163             Lmin = L;
 164         }
 165         if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
 166         {
 167             path = ReedsSheppStateSpace::ReedsSheppPath(
 168                 ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
 169             Lmin = L;
 170         }
 171
 172         // backwards
 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)))
 175         {
 176             path = ReedsSheppStateSpace::ReedsSheppPath(
 177                 ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
 178             Lmin = L;
 179         }
 180         if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 181         {
 182             path = ReedsSheppStateSpace::ReedsSheppPath(
 183                 ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
 184             Lmin = L;
 185         }
 186         if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 187         {
 188             path = ReedsSheppStateSpace::ReedsSheppPath(
 189                 ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
 190             Lmin = L;
 191         }
 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);
 195     }
 196     // formula 8.7
 197     inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
 198     {
 199         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
 200         if (rho <= 1.)
 201         {
 202             u = acos(rho);
 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;
 208         }
 209         return false;
 210     }
 211     // formula 8.8
 212     inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
 213     {
 214         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
 215         if (rho>=0 && rho<=1)
 216         {
 217             u = -acos(rho);
 218             if (u >= -.5 * pi)
 219             {
 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;
 225             }
 226         }
 227         return false;
 228     }
 229     void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
 230     {
 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)))
 233         {
 234             path = ReedsSheppStateSpace::ReedsSheppPath(
 235                 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
 236             Lmin = L;
 237         }
 238         if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
 239         {
 240             path = ReedsSheppStateSpace::ReedsSheppPath(
 241                 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
 242             Lmin = L;
 243         }
 244         if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
 245         {
 246             path = ReedsSheppStateSpace::ReedsSheppPath(
 247                 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
 248             Lmin = L;
 249         }
 250         if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
 251         {
 252             path = ReedsSheppStateSpace::ReedsSheppPath(
 253                 ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
 254             Lmin = L;
 255         }
 256
 257         if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
 258         {
 259             path = ReedsSheppStateSpace::ReedsSheppPath(
 260                 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
 261             Lmin = L;
 262         }
 263         if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
 264         {
 265             path = ReedsSheppStateSpace::ReedsSheppPath(
 266                 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
 267             Lmin = L;
 268         }
 269         if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
 270         {
 271             path = ReedsSheppStateSpace::ReedsSheppPath(
 272                 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
 273             Lmin = L;
 274         }
 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);
 278     }
 279     // formula 8.9
 280     inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
 281     {
 282         double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
 283         polar(xi, eta, rho, theta);
 284         if (rho >= 2.)
 285         {
 286             double r = sqrt(rho*rho - 4.);
 287             u = 2. - r;
 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;
 294         }
 295         return false;
 296     }
 297     // formula 8.10
 298     inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
 299     {
 300         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
 301         polar(-eta, xi, rho, theta);
 302         if (rho >= 2.)
 303         {
 304             t = theta;
 305             u = 2. - rho;
 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;
 311         }
 312         return false;
 313     }
 314     void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
 315     {
 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)))
 318         {
 319             path = ReedsSheppStateSpace::ReedsSheppPath(
 320                 ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
 321             Lmin = L;
 322         }
 323         if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 324         {
 325             path = ReedsSheppStateSpace::ReedsSheppPath(
 326                 ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
 327             Lmin = L;
 328         }
 329         if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 330         {
 331             path = ReedsSheppStateSpace::ReedsSheppPath(
 332                 ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
 333             Lmin = L;
 334         }
 335         if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
 336         {
 337             path = ReedsSheppStateSpace::ReedsSheppPath(
 338                 ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
 339             Lmin = L;
 340         }
 341
 342         if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
 343         {
 344             path = ReedsSheppStateSpace::ReedsSheppPath(
 345                 ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
 346             Lmin = L;
 347         }
 348         if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 349         {
 350             path = ReedsSheppStateSpace::ReedsSheppPath(
 351                 ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
 352             Lmin = L;
 353         }
 354         if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 355         {
 356             path = ReedsSheppStateSpace::ReedsSheppPath(
 357                 ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
 358             Lmin = L;
 359         }
 360         if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
 361         {
 362             path = ReedsSheppStateSpace::ReedsSheppPath(
 363                 ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
 364             Lmin = L;
 365         }
 366
 367         // backwards
 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)))
 370         {
 371             path = ReedsSheppStateSpace::ReedsSheppPath(
 372                 ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
 373             Lmin = L;
 374         }
 375         if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 376         {
 377             path = ReedsSheppStateSpace::ReedsSheppPath(
 378                 ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
 379             Lmin = L;
 380         }
 381         if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 382         {
 383             path = ReedsSheppStateSpace::ReedsSheppPath(
 384                 ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
 385             Lmin = L;
 386         }
 387         if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
 388         {
 389             path = ReedsSheppStateSpace::ReedsSheppPath(
 390                 ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
 391             Lmin = L;
 392         }
 393
 394         if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
 395         {
 396             path = ReedsSheppStateSpace::ReedsSheppPath(
 397                 ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
 398             Lmin = L;
 399         }
 400         if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 401         {
 402             path = ReedsSheppStateSpace::ReedsSheppPath(
 403                 ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
 404             Lmin = L;
 405         }
 406         if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 407         {
 408             path = ReedsSheppStateSpace::ReedsSheppPath(
 409                 ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
 410             Lmin = L;
 411         }
 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);
 415     }
 416     // formula 8.11 *** TYPO IN PAPER ***
 417     inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
 418     {
 419         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
 420         polar(xi, eta, rho, theta);
 421         if (rho >= 2.)
 422         {
 423             u = 4. - sqrt(rho*rho - 4.);
 424             if (u <= ZERO)
 425             {
 426                 t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
 427                 v = mod2pi(t - phi);
 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;
 432             }
 433         }
 434         return false;
 435     }
 436     void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
 437     {
 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)))
 440         {
 441             path = ReedsSheppStateSpace::ReedsSheppPath(
 442                 ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
 443             Lmin = L;
 444         }
 445         if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 446         {
 447             path = ReedsSheppStateSpace::ReedsSheppPath(
 448                 ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
 449             Lmin = L;
 450         }
 451         if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 452         {
 453             path = ReedsSheppStateSpace::ReedsSheppPath(
 454                 ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
 455             Lmin = L;
 456         }
 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);
 460     }
 461
 462     ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
 463     {
 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);
 470         return path;
 471     }
 472 }
 473
 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
 494 };
 495
 496 ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
 497     double t, double u, double v, double w, double x)
 498     : type_(type)
 499 {
 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);
 502 }
 503
 504
 505 double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
 506 {
 507     return rho_ * reedsShepp(q0, q1).length();
 508 }
 509
 510 ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
 511 {
 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);
 516 }
 517
 518 void ReedsSheppStateSpace::type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data)
 519 {
 520     ReedsSheppPath path = reedsShepp(q0, q1);
 521     for (int i=0;i<5;++i)
 522         cb(path.type_[i], user_data);
 523     return;
 524 }
 525
 526 void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data)
 527 {
 528     ReedsSheppPath path = reedsShepp(q0, q1);
 529     double dist = rho_ * path.length();
 530
 531     for (double seg=0.0; seg<=dist; seg+=step_size){
 532         double qnew[4] = {};
 533         interpolate(q0, path, seg/rho_, qnew);
 534         cb( qnew, user_data);
 535     }
 536     return;
 537 }
 538
 539 void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[4])
 540 {
 541
 542     if (seg < 0.0) seg = 0.0;
 543     if (seg > path.length()) seg = path.length();
 544
 545     double phi, v;
 546
 547     s[0] = s[1] = 0.0;
 548     s[2] = q0[2];
 549
 550     for (unsigned int i=0; i<5 && seg>0; ++i)
 551     {
 552         if (path.length_[i]<0)
 553         {
 554             v = std::max(-seg, path.length_[i]);
 555             seg += v;
 556         }
 557         else
 558         {
 559             v = std::min(seg, path.length_[i]);
 560             seg -= v;
 561         }
 562         phi = s[2];
 563         switch(path.type_[i])
 564         {
 565             case RS_LEFT:
 566                 s[0] += ( sin(phi+v) - sin(phi));
 567                 s[1] += (-cos(phi+v) + cos(phi));
 568                 s[2] = phi + v;
 569                 break;
 570             case RS_RIGHT:
 571                 s[0] += (-sin(phi-v) + sin(phi));
 572                 s[1] += ( cos(phi-v) - cos(phi));
 573                 s[2] = phi - v;
 574                 break;
 575             case RS_STRAIGHT:
 576                 s[0] += (v * cos(phi));
 577                 s[1] += (v * sin(phi));
 578                 break;
 579             case RS_NOP:
 580                 break;
 581         }
 582     }
 583
 584     s[0] = s[0] * rho_ + q0[0];
 585     s[1] = s[1] * rho_ + q0[1];
 586     s[3] = v;
 587 }