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