vehicle_platform/reeds_shepp.cpp

   1 /*********************************************************************
   2 *
   3 * Software License Agreement (BSD License)
   4 *
   5 *  Copyright (c) 2016, Guan-Horng Liu.
   6 *  All rights reserved.
   7 *
   8 *  Redistribution and use in source and binary forms, with or without
   9 *  modification, are permitted provided that the following conditions
  10 *  are met:
  11 *
  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.
  21 *
  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.
  34 *
  35 * Author:  Guan-Horng Liu
  36 *********************************************************************/
  37
  38
  39 #include "reeds_shepp.h"
  40 #include <boost/math/constants/constants.hpp>
  41
  42
  43 namespace
  44 {
  45     // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
  46
  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();
  51
  52     inline double mod2pi(double x)
  53     {
  54         double v = fmod(x, twopi);
  55         if (v < -pi)
  56             v += twopi;
  57         else
  58             if (v > pi)
  59                 v -= twopi;
  60         return v;
  61     }
  62     inline void polar(double x, double y, double &r, double &theta)
  63     {
  64         r = sqrt(x*x + y*y);
  65         theta = atan2(y, x);
  66     }
  67     inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
  68     {
  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) ;
  73     }
  74
  75     // formula 8.1 in Reeds-Shepp paper
  76     inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
  77     {
  78         polar(x - sin(phi), y - 1. + cos(phi), u, t);
  79         if (t >= -ZERO)
  80         {
  81             v = mod2pi(phi - t);
  82             if (v >= -ZERO)
  83             {
  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);
  87                 return true;
  88             }
  89         }
  90         return false;
  91     }
  92     // formula 8.2
  93     inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
  94     {
  95         double t1, u1;
  96         polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
  97         u1 = u1*u1;
  98         if (u1 >= 4.)
  99         {
 100             double theta;
 101             u = sqrt(u1 - 4.);
 102             theta = atan2(2., u);
 103             t = mod2pi(t1 + theta);
 104             v = mod2pi(t - phi);
 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;
 109         }
 110         return false;
 111     }
 112     void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
 113     {
 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)))
 116         {
 117             path = ReedsSheppStateSpace::ReedsSheppPath(
 118                 ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
 119             Lmin = L;
 120         }
 121         if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 122         {
 123             path = ReedsSheppStateSpace::ReedsSheppPath(
 124                 ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
 125             Lmin = L;
 126         }
 127         if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 128         {
 129             path = ReedsSheppStateSpace::ReedsSheppPath(
 130                 ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
 131             Lmin = L;
 132         }
 133         if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
 134         {
 135             path = ReedsSheppStateSpace::ReedsSheppPath(
 136                 ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
 137             Lmin = L;
 138         }
 139         if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
 140         {
 141             path = ReedsSheppStateSpace::ReedsSheppPath(
 142                 ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
 143             Lmin = L;
 144         }
 145         if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 146         {
 147             path = ReedsSheppStateSpace::ReedsSheppPath(
 148                 ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
 149             Lmin = L;
 150         }
 151         if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 152         {
 153             path = ReedsSheppStateSpace::ReedsSheppPath(
 154                 ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
 155             Lmin = L;
 156         }
 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);
 160     }
 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)
 163     {
 164         double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
 165         polar(xi, eta, u1, theta);
 166         if (u1 <= 4.)
 167         {
 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;
 175         }
 176         return false;
 177     }
 178     void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
 179     {
 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)))
 182         {
 183             path = ReedsSheppStateSpace::ReedsSheppPath(
 184                 ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
 185             Lmin = L;
 186         }
 187         if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 188         {
 189             path = ReedsSheppStateSpace::ReedsSheppPath(
 190                 ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
 191             Lmin = L;
 192         }
 193         if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 194         {
 195             path = ReedsSheppStateSpace::ReedsSheppPath(
 196                 ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
 197             Lmin = L;
 198         }
 199         if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
 200         {
 201             path = ReedsSheppStateSpace::ReedsSheppPath(
 202                 ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
 203             Lmin = L;
 204         }
 205
 206         // backwards
 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)))
 209         {
 210             path = ReedsSheppStateSpace::ReedsSheppPath(
 211                 ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
 212             Lmin = L;
 213         }
 214         if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 215         {
 216             path = ReedsSheppStateSpace::ReedsSheppPath(
 217                 ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
 218             Lmin = L;
 219         }
 220         if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 221         {
 222             path = ReedsSheppStateSpace::ReedsSheppPath(
 223                 ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
 224             Lmin = L;
 225         }
 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);
 229     }
 230     // formula 8.7
 231     inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
 232     {
 233         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
 234         if (rho <= 1.)
 235         {
 236             u = acos(rho);
 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;
 242         }
 243         return false;
 244     }
 245     // formula 8.8
 246     inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
 247     {
 248         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
 249         if (rho>=0 && rho<=1)
 250         {
 251             u = -acos(rho);
 252             if (u >= -.5 * pi)
 253             {
 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;
 259             }
 260         }
 261         return false;
 262     }
 263     void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
 264     {
 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)))
 267         {
 268             path = ReedsSheppStateSpace::ReedsSheppPath(
 269                 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
 270             Lmin = L;
 271         }
 272         if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
 273         {
 274             path = ReedsSheppStateSpace::ReedsSheppPath(
 275                 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
 276             Lmin = L;
 277         }
 278         if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
 279         {
 280             path = ReedsSheppStateSpace::ReedsSheppPath(
 281                 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
 282             Lmin = L;
 283         }
 284         if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
 285         {
 286             path = ReedsSheppStateSpace::ReedsSheppPath(
 287                 ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
 288             Lmin = L;
 289         }
 290
 291         if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
 292         {
 293             path = ReedsSheppStateSpace::ReedsSheppPath(
 294                 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
 295             Lmin = L;
 296         }
 297         if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
 298         {
 299             path = ReedsSheppStateSpace::ReedsSheppPath(
 300                 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
 301             Lmin = L;
 302         }
 303         if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
 304         {
 305             path = ReedsSheppStateSpace::ReedsSheppPath(
 306                 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
 307             Lmin = L;
 308         }
 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);
 312     }
 313     // formula 8.9
 314     inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
 315     {
 316         double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
 317         polar(xi, eta, rho, theta);
 318         if (rho >= 2.)
 319         {
 320             double r = sqrt(rho*rho - 4.);
 321             u = 2. - r;
 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;
 328         }
 329         return false;
 330     }
 331     // formula 8.10
 332     inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
 333     {
 334         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
 335         polar(-eta, xi, rho, theta);
 336         if (rho >= 2.)
 337         {
 338             t = theta;
 339             u = 2. - rho;
 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;
 345         }
 346         return false;
 347     }
 348     void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
 349     {
 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)))
 352         {
 353             path = ReedsSheppStateSpace::ReedsSheppPath(
 354                 ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
 355             Lmin = L;
 356         }
 357         if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 358         {
 359             path = ReedsSheppStateSpace::ReedsSheppPath(
 360                 ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
 361             Lmin = L;
 362         }
 363         if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 364         {
 365             path = ReedsSheppStateSpace::ReedsSheppPath(
 366                 ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
 367             Lmin = L;
 368         }
 369         if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
 370         {
 371             path = ReedsSheppStateSpace::ReedsSheppPath(
 372                 ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
 373             Lmin = L;
 374         }
 375
 376         if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
 377         {
 378             path = ReedsSheppStateSpace::ReedsSheppPath(
 379                 ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
 380             Lmin = L;
 381         }
 382         if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 383         {
 384             path = ReedsSheppStateSpace::ReedsSheppPath(
 385                 ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
 386             Lmin = L;
 387         }
 388         if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 389         {
 390             path = ReedsSheppStateSpace::ReedsSheppPath(
 391                 ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
 392             Lmin = L;
 393         }
 394         if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
 395         {
 396             path = ReedsSheppStateSpace::ReedsSheppPath(
 397                 ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
 398             Lmin = L;
 399         }
 400
 401         // backwards
 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)))
 404         {
 405             path = ReedsSheppStateSpace::ReedsSheppPath(
 406                 ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
 407             Lmin = L;
 408         }
 409         if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 410         {
 411             path = ReedsSheppStateSpace::ReedsSheppPath(
 412                 ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
 413             Lmin = L;
 414         }
 415         if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 416         {
 417             path = ReedsSheppStateSpace::ReedsSheppPath(
 418                 ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
 419             Lmin = L;
 420         }
 421         if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
 422         {
 423             path = ReedsSheppStateSpace::ReedsSheppPath(
 424                 ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
 425             Lmin = L;
 426         }
 427
 428         if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
 429         {
 430             path = ReedsSheppStateSpace::ReedsSheppPath(
 431                 ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
 432             Lmin = L;
 433         }
 434         if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 435         {
 436             path = ReedsSheppStateSpace::ReedsSheppPath(
 437                 ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
 438             Lmin = L;
 439         }
 440         if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 441         {
 442             path = ReedsSheppStateSpace::ReedsSheppPath(
 443                 ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
 444             Lmin = L;
 445         }
 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);
 449     }
 450     // formula 8.11 *** TYPO IN PAPER ***
 451     inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
 452     {
 453         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
 454         polar(xi, eta, rho, theta);
 455         if (rho >= 2.)
 456         {
 457             u = 4. - sqrt(rho*rho - 4.);
 458             if (u <= ZERO)
 459             {
 460                 t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
 461                 v = mod2pi(t - phi);
 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;
 466             }
 467         }
 468         return false;
 469     }
 470     void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
 471     {
 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)))
 474         {
 475             path = ReedsSheppStateSpace::ReedsSheppPath(
 476                 ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
 477             Lmin = L;
 478         }
 479         if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
 480         {
 481             path = ReedsSheppStateSpace::ReedsSheppPath(
 482                 ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
 483             Lmin = L;
 484         }
 485         if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
 486         {
 487             path = ReedsSheppStateSpace::ReedsSheppPath(
 488                 ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
 489             Lmin = L;
 490         }
 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);
 494     }
 495
 496     ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
 497     {
 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);
 504         return path;
 505     }
 506 }
 507
 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
 528 };
 529
 530 ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
 531     double t, double u, double v, double w, double x)
 532     : type_(type)
 533 {
 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);
 536 }
 537
 538
 539 double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
 540 {
 541     return rho_ * reedsShepp(q0, q1).length();
 542 }
 543
 544 ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
 545 {
 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);
 550 }
 551
 552 void ReedsSheppStateSpace::type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data)
 553 {
 554     ReedsSheppPath path = reedsShepp(q0, q1);
 555     for (int i=0;i<5;++i)
 556         cb(path.type_[i], user_data);
 557     return;
 558 }
 559
 560 void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data)
 561 {
 562     ReedsSheppPath path = reedsShepp(q0, q1);
 563     double dist = rho_ * path.length(); // TODO shorten path here
 564
 565     for (double seg=0.0; seg<=dist; seg+=step_size){
 566         double qnew[4] = {};
 567         interpolate(q0, path, seg/rho_, qnew);
 568         cb( qnew, user_data);
 569     }
 570     return;
 571 }
 572
 573 void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[4])
 574 {
 575
 576     if (seg < 0.0) seg = 0.0;
 577     if (seg > path.length()) seg = path.length();
 578
 579     double phi, v = 0;
 580
 581     s[0] = s[1] = 0.0;
 582     s[2] = q0[2];
 583     s[3] = 0;
 584
 585     for (unsigned int i=0; i<5 && seg>0; ++i)
 586     {
 587         if (path.length_[i]<0)
 588         {
 589             v = std::max(-seg, path.length_[i]);
 590             seg += v;
 591         }
 592         else
 593         {
 594             v = std::min(seg, path.length_[i]);
 595             seg -= v;
 596         }
 597         phi = s[2];
 598         switch(path.type_[i])
 599         {
 600             case RS_LEFT:
 601                 s[0] += ( sin(phi+v) - sin(phi));
 602                 s[1] += (-cos(phi+v) + cos(phi));
 603                 s[2] = phi + v;
 604                 break;
 605             case RS_RIGHT:
 606                 s[0] += (-sin(phi-v) + sin(phi));
 607                 s[1] += ( cos(phi-v) - cos(phi));
 608                 s[2] = phi - v;
 609                 break;
 610             case RS_STRAIGHT:
 611                 s[0] += (v * cos(phi));
 612                 s[1] += (v * sin(phi));
 613                 break;
 614             case RS_NOP:
 615                 break;
 616         }
 617     }
 618
 619     s[0] = s[0] * rho_ + q0[0];
 620     s[1] = s[1] * rho_ + q0[1];
 621     s[3] = v;
 622 }