src/dubins.c

   1 /*
   2  * SPDX-FileCopyrightText: 2008-2018 Andrew Walker
   3  *
   4  * SPDX-License-Identifier: MIT
   5  */
   6
   7 #ifdef WIN32
   8 #define _USE_MATH_DEFINES
   9 #endif
  10 #include <math.h>
  11 #include "dubins.h"
  12
  13 #define EPSILON (10e-10)
  14
  15 typedef enum
  16 {
  17     L_SEG = 0,
  18     S_SEG = 1,
  19     R_SEG = 2
  20 } SegmentType;
  21
  22 /* The segment types for each of the Path types */
  23 const SegmentType DIRDATA[][3] = {
  24     { L_SEG, S_SEG, L_SEG },
  25     { L_SEG, S_SEG, R_SEG },
  26     { R_SEG, S_SEG, L_SEG },
  27     { R_SEG, S_SEG, R_SEG },
  28     { R_SEG, L_SEG, R_SEG },
  29     { L_SEG, R_SEG, L_SEG }
  30 };
  31
  32 typedef struct
  33 {
  34     double alpha;
  35     double beta;
  36     double d;
  37     double sa;
  38     double sb;
  39     double ca;
  40     double cb;
  41     double c_ab;
  42     double d_sq;
  43 } DubinsIntermediateResults;
  44
  45
  46 int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
  47 int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);
  48
  49 /**
  50  * Floating point modulus suitable for rings
  51  *
  52  * fmod doesn't behave correctly for angular quantities, this function does
  53  */
  54 double fmodr( double x, double y)
  55 {
  56     return x - y*floor(x/y);
  57 }
  58
  59 double mod2pi( double theta )
  60 {
  61     return fmodr( theta, 2 * M_PI );
  62 }
  63
  64 int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho)
  65 {
  66     int i, errcode;
  67     DubinsIntermediateResults in;
  68     double params[3];
  69     double cost;
  70     double best_cost = INFINITY;
  71     int best_word = -1;
  72     errcode = dubins_intermediate_results(&in, q0, q1, rho);
  73     if(errcode != EDUBOK) {
  74         return errcode;
  75     }
  76
  77
  78     path->qi[0] = q0[0];
  79     path->qi[1] = q0[1];
  80     path->qi[2] = q0[2];
  81     path->rho = rho;
  82
  83     for( i = 0; i < 6; i++ ) {
  84         DubinsPathType pathType = (DubinsPathType)i;
  85         errcode = dubins_word(&in, pathType, params);
  86         if(errcode == EDUBOK) {
  87             cost = params[0] + params[1] + params[2];
  88             if(cost < best_cost) {
  89                 best_word = i;
  90                 best_cost = cost;
  91                 path->param[0] = params[0];
  92                 path->param[1] = params[1];
  93                 path->param[2] = params[2];
  94                 path->type = pathType;
  95             }
  96         }
  97     }
  98     if(best_word == -1) {
  99         return EDUBNOPATH;
 100     }
 101     return EDUBOK;
 102 }
 103
 104 int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType)
 105 {
 106     int errcode;
 107     DubinsIntermediateResults in;
 108     errcode = dubins_intermediate_results(&in, q0, q1, rho);
 109     if(errcode == EDUBOK) {
 110         double params[3];
 111         errcode = dubins_word(&in, pathType, params);
 112         if(errcode == EDUBOK) {
 113             path->param[0] = params[0];
 114             path->param[1] = params[1];
 115             path->param[2] = params[2];
 116             path->qi[0] = q0[0];
 117             path->qi[1] = q0[1];
 118             path->qi[2] = q0[2];
 119             path->rho = rho;
 120             path->type = pathType;
 121         }
 122     }
 123     return errcode;
 124 }
 125
 126 double dubins_path_length( DubinsPath* path )
 127 {
 128     double length = 0.;
 129     length += path->param[0];
 130     length += path->param[1];
 131     length += path->param[2];
 132     length = length * path->rho;
 133     return length;
 134 }
 135
 136 double dubins_segment_length( DubinsPath* path, int i )
 137 {
 138     if( (i < 0) || (i > 2) )
 139     {
 140         return INFINITY;
 141     }
 142     return path->param[i] * path->rho;
 143 }
 144
 145 double dubins_segment_length_normalized( DubinsPath* path, int i )
 146 {
 147     if( (i < 0) || (i > 2) )
 148     {
 149         return INFINITY;
 150     }
 151     return path->param[i];
 152 }
 153
 154 DubinsPathType dubins_path_type( DubinsPath* path )
 155 {
 156     return path->type;
 157 }
 158
 159 void dubins_segment( double t, double qi[3], double qt[3], SegmentType type)
 160 {
 161     double st = sin(qi[2]);
 162     double ct = cos(qi[2]);
 163     if( type == L_SEG ) {
 164         qt[0] = +sin(qi[2]+t) - st;
 165         qt[1] = -cos(qi[2]+t) + ct;
 166         qt[2] = t;
 167     }
 168     else if( type == R_SEG ) {
 169         qt[0] = -sin(qi[2]-t) + st;
 170         qt[1] = +cos(qi[2]-t) - ct;
 171         qt[2] = -t;
 172     }
 173     else if( type == S_SEG ) {
 174         qt[0] = ct * t;
 175         qt[1] = st * t;
 176         qt[2] = 0.0;
 177     }
 178     qt[0] += qi[0];
 179     qt[1] += qi[1];
 180     qt[2] += qi[2];
 181 }
 182
 183 int dubins_path_sample( DubinsPath* path, double t, double q[3] )
 184 {
 185     /* tprime is the normalised variant of the parameter t */
 186     double tprime = t / path->rho;
 187     double qi[3]; /* The translated initial configuration */
 188     double q1[3]; /* end-of segment 1 */
 189     double q2[3]; /* end-of segment 2 */
 190     const SegmentType* types = DIRDATA[path->type];
 191     double p1, p2;
 192
 193     if( t < 0 || t > dubins_path_length(path) ) {
 194         return EDUBPARAM;
 195     }
 196
 197     /* initial configuration */
 198     qi[0] = 0.0;
 199     qi[1] = 0.0;
 200     qi[2] = path->qi[2];
 201
 202     /* generate the target configuration */
 203     p1 = path->param[0];
 204     p2 = path->param[1];
 205     dubins_segment( p1,      qi,    q1, types[0] );
 206     dubins_segment( p2,      q1,    q2, types[1] );
 207     if( tprime < p1 ) {
 208         dubins_segment( tprime, qi, q, types[0] );
 209     }
 210     else if( tprime < (p1+p2) ) {
 211         dubins_segment( tprime-p1, q1, q,  types[1] );
 212     }
 213     else {
 214         dubins_segment( tprime-p1-p2, q2, q,  types[2] );
 215     }
 216
 217     /* scale the target configuration, translate back to the original starting point */
 218     q[0] = q[0] * path->rho + path->qi[0];
 219     q[1] = q[1] * path->rho + path->qi[1];
 220     q[2] = mod2pi(q[2]);
 221
 222     return EDUBOK;
 223 }
 224
 225 int dubins_path_sample_many(DubinsPath* path, double stepSize,
 226                             DubinsPathSamplingCallback cb, void* user_data)
 227 {
 228     int retcode;
 229     double q[3];
 230     double x = 0.0;
 231     double length = dubins_path_length(path);
 232     while( x <  length ) {
 233         dubins_path_sample( path, x, q );
 234         retcode = cb(q, x, user_data);
 235         if( retcode != 0 ) {
 236             return retcode;
 237         }
 238         x += stepSize;
 239     }
 240     return 0;
 241 }
 242
 243 int dubins_path_endpoint( DubinsPath* path, double q[3] )
 244 {
 245     return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
 246 }
 247
 248 int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
 249 {
 250     /* calculate the true parameter */
 251     double tprime = t / path->rho;
 252
 253     if((t < 0) || (t > dubins_path_length(path)))
 254     {
 255         return EDUBPARAM;
 256     }
 257
 258     /* copy most of the data */
 259     newpath->qi[0] = path->qi[0];
 260     newpath->qi[1] = path->qi[1];
 261     newpath->qi[2] = path->qi[2];
 262     newpath->rho   = path->rho;
 263     newpath->type  = path->type;
 264
 265     /* fix the parameters */
 266     newpath->param[0] = fmin( path->param[0], tprime );
 267     newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
 268     newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
 269     return 0;
 270 }
 271
 272 int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
 273 {
 274     double dx, dy, D, d, theta, alpha, beta;
 275     if( rho <= 0.0 ) {
 276         return EDUBBADRHO;
 277     }
 278
 279     dx = q1[0] - q0[0];
 280     dy = q1[1] - q0[1];
 281     D = sqrt( dx * dx + dy * dy );
 282     d = D / rho;
 283     theta = 0;
 284
 285     /* test required to prevent domain errors if dx=0 and dy=0 */
 286     if(d > 0) {
 287         theta = mod2pi(atan2( dy, dx ));
 288     }
 289     alpha = mod2pi(q0[2] - theta);
 290     beta  = mod2pi(q1[2] - theta);
 291
 292     in->alpha = alpha;
 293     in->beta  = beta;
 294     in->d     = d;
 295     in->sa    = sin(alpha);
 296     in->sb    = sin(beta);
 297     in->ca    = cos(alpha);
 298     in->cb    = cos(beta);
 299     in->c_ab  = cos(alpha - beta);
 300     in->d_sq  = d * d;
 301
 302     return EDUBOK;
 303 }
 304
 305 int dubins_LSL(DubinsIntermediateResults* in, double out[3])
 306 {
 307     double tmp0, tmp1, p_sq;
 308
 309     tmp0 = in->d + in->sa - in->sb;
 310     p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));
 311
 312     if(p_sq >= 0) {
 313         tmp1 = atan2( (in->cb - in->ca), tmp0 );
 314         out[0] = mod2pi(tmp1 - in->alpha);
 315         out[1] = sqrt(p_sq);
 316         out[2] = mod2pi(in->beta - tmp1);
 317         return EDUBOK;
 318     }
 319     return EDUBNOPATH;
 320 }
 321
 322
 323 int dubins_RSR(DubinsIntermediateResults* in, double out[3])
 324 {
 325     double tmp0 = in->d - in->sa + in->sb;
 326     double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
 327     if( p_sq >= 0 ) {
 328         double tmp1 = atan2( (in->ca - in->cb), tmp0 );
 329         out[0] = mod2pi(in->alpha - tmp1);
 330         out[1] = sqrt(p_sq);
 331         out[2] = mod2pi(tmp1 -in->beta);
 332         return EDUBOK;
 333     }
 334     return EDUBNOPATH;
 335 }
 336
 337 int dubins_LSR(DubinsIntermediateResults* in, double out[3])
 338 {
 339     double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
 340     if( p_sq >= 0 ) {
 341         double p    = sqrt(p_sq);
 342         double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
 343         out[0] = mod2pi(tmp0 - in->alpha);
 344         out[1] = p;
 345         out[2] = mod2pi(tmp0 - mod2pi(in->beta));
 346         return EDUBOK;
 347     }
 348     return EDUBNOPATH;
 349 }
 350
 351 int dubins_RSL(DubinsIntermediateResults* in, double out[3])
 352 {
 353     double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
 354     if( p_sq >= 0 ) {
 355         double p    = sqrt(p_sq);
 356         double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
 357         out[0] = mod2pi(in->alpha - tmp0);
 358         out[1] = p;
 359         out[2] = mod2pi(in->beta - tmp0);
 360         return EDUBOK;
 361     }
 362     return EDUBNOPATH;
 363 }
 364
 365 int dubins_RLR(DubinsIntermediateResults* in, double out[3])
 366 {
 367     double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 8.;
 368     double phi  = atan2( in->ca - in->cb, in->d - in->sa + in->sb );
 369     if( fabs(tmp0) <= 1) {
 370         double p = mod2pi((2*M_PI) - acos(tmp0) );
 371         double t = mod2pi(in->alpha - phi + mod2pi(p/2.));
 372         out[0] = t;
 373         out[1] = p;
 374         out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
 375         return EDUBOK;
 376     }
 377     return EDUBNOPATH;
 378 }
 379
 380 int dubins_LRL(DubinsIntermediateResults* in, double out[3])
 381 {
 382     double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
 383     double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
 384     if( fabs(tmp0) <= 1) {
 385         double p = mod2pi( 2*M_PI - acos( tmp0) );
 386         double t = mod2pi(-in->alpha - phi + p/2.);
 387         out[0] = t;
 388         out[1] = p;
 389         out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
 390         return EDUBOK;
 391     }
 392     return EDUBNOPATH;
 393 }
 394
 395 int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3])
 396 {
 397     int result;
 398     switch(pathType)
 399     {
 400     case LSL:
 401         result = dubins_LSL(in, out);
 402         break;
 403     case RSL:
 404         result = dubins_RSL(in, out);
 405         break;
 406     case LSR:
 407         result = dubins_LSR(in, out);
 408         break;
 409     case RSR:
 410         result = dubins_RSR(in, out);
 411         break;
 412     case LRL:
 413         result = dubins_LRL(in, out);
 414         break;
 415     case RLR:
 416         result = dubins_RLR(in, out);
 417         break;
 418     default:
 419         result = EDUBNOPATH;
 420     }
 421     return result;
 422 }