2 * Copyright (c) 2008-2018, Andrew Walker
4 * Permission is hereby granted, free of charge, to any person obtaining a copy
5 * of this software and associated documentation files (the "Software"), to deal
6 * in the Software without restriction, including without limitation the rights
7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8 * copies of the Software, and to permit persons to whom the Software is
9 * furnished to do so, subject to the following conditions:
11 * The above copyright notice and this permission notice shall be included in
12 * all copies or substantial portions of the Software.
14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
23 #define _USE_MATH_DEFINES
28 #define EPSILON (10e-10)
37 /* The segment types for each of the Path types */
38 const SegmentType DIRDATA[][3] = {
39 { L_SEG, S_SEG, L_SEG },
40 { L_SEG, S_SEG, R_SEG },
41 { R_SEG, S_SEG, L_SEG },
42 { R_SEG, S_SEG, R_SEG },
43 { R_SEG, L_SEG, R_SEG },
44 { L_SEG, R_SEG, L_SEG }
58 } DubinsIntermediateResults;
61 int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
62 int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);
65 * Floating point modulus suitable for rings
67 * fmod doesn't behave correctly for angular quantities, this function does
69 double fmodr( double x, double y)
71 return x - y*floor(x/y);
74 double mod2pi( double theta )
76 return fmodr( theta, 2 * M_PI );
79 int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho)
82 DubinsIntermediateResults in;
85 double best_cost = INFINITY;
87 errcode = dubins_intermediate_results(&in, q0, q1, rho);
88 if(errcode != EDUBOK) {
98 for( i = 0; i < 6; i++ ) {
99 DubinsPathType pathType = (DubinsPathType)i;
100 errcode = dubins_word(&in, pathType, params);
101 if(errcode == EDUBOK) {
102 cost = params[0] + params[1] + params[2];
103 if(cost < best_cost) {
106 path->param[0] = params[0];
107 path->param[1] = params[1];
108 path->param[2] = params[2];
109 path->type = pathType;
113 if(best_word == -1) {
119 int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType)
122 DubinsIntermediateResults in;
123 errcode = dubins_intermediate_results(&in, q0, q1, rho);
124 if(errcode == EDUBOK) {
126 errcode = dubins_word(&in, pathType, params);
127 if(errcode == EDUBOK) {
128 path->param[0] = params[0];
129 path->param[1] = params[1];
130 path->param[2] = params[2];
135 path->type = pathType;
141 double dubins_path_length( DubinsPath* path )
144 length += path->param[0];
145 length += path->param[1];
146 length += path->param[2];
147 length = length * path->rho;
151 double dubins_segment_length( DubinsPath* path, int i )
153 if( (i < 0) || (i > 2) )
157 return path->param[i] * path->rho;
160 double dubins_segment_length_normalized( DubinsPath* path, int i )
162 if( (i < 0) || (i > 2) )
166 return path->param[i];
169 DubinsPathType dubins_path_type( DubinsPath* path )
174 void dubins_segment( double t, double qi[3], double qt[3], SegmentType type)
176 double st = sin(qi[2]);
177 double ct = cos(qi[2]);
178 if( type == L_SEG ) {
179 qt[0] = +sin(qi[2]+t) - st;
180 qt[1] = -cos(qi[2]+t) + ct;
183 else if( type == R_SEG ) {
184 qt[0] = -sin(qi[2]-t) + st;
185 qt[1] = +cos(qi[2]-t) - ct;
188 else if( type == S_SEG ) {
198 int dubins_path_sample( DubinsPath* path, double t, double q[3] )
200 /* tprime is the normalised variant of the parameter t */
201 double tprime = t / path->rho;
202 double qi[3]; /* The translated initial configuration */
203 double q1[3]; /* end-of segment 1 */
204 double q2[3]; /* end-of segment 2 */
205 const SegmentType* types = DIRDATA[path->type];
208 if( t < 0 || t > dubins_path_length(path) ) {
212 /* initial configuration */
217 /* generate the target configuration */
220 dubins_segment( p1, qi, q1, types[0] );
221 dubins_segment( p2, q1, q2, types[1] );
223 dubins_segment( tprime, qi, q, types[0] );
225 else if( tprime < (p1+p2) ) {
226 dubins_segment( tprime-p1, q1, q, types[1] );
229 dubins_segment( tprime-p1-p2, q2, q, types[2] );
232 /* scale the target configuration, translate back to the original starting point */
233 q[0] = q[0] * path->rho + path->qi[0];
234 q[1] = q[1] * path->rho + path->qi[1];
240 int dubins_path_sample_many(DubinsPath* path, double stepSize,
241 DubinsPathSamplingCallback cb, void* user_data)
246 double length = dubins_path_length(path);
247 while( x < length ) {
248 dubins_path_sample( path, x, q );
249 retcode = cb(q, x, user_data);
258 int dubins_path_endpoint( DubinsPath* path, double q[3] )
260 return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
263 int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
265 /* calculate the true parameter */
266 double tprime = t / path->rho;
268 if((t < 0) || (t > dubins_path_length(path)))
273 /* copy most of the data */
274 newpath->qi[0] = path->qi[0];
275 newpath->qi[1] = path->qi[1];
276 newpath->qi[2] = path->qi[2];
277 newpath->rho = path->rho;
278 newpath->type = path->type;
280 /* fix the parameters */
281 newpath->param[0] = fmin( path->param[0], tprime );
282 newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
283 newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
287 int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
289 double dx, dy, D, d, theta, alpha, beta;
296 D = sqrt( dx * dx + dy * dy );
300 /* test required to prevent domain errors if dx=0 and dy=0 */
302 theta = mod2pi(atan2( dy, dx ));
304 alpha = mod2pi(q0[2] - theta);
305 beta = mod2pi(q1[2] - theta);
314 in->c_ab = cos(alpha - beta);
320 int dubins_LSL(DubinsIntermediateResults* in, double out[3])
322 double tmp0, tmp1, p_sq;
324 tmp0 = in->d + in->sa - in->sb;
325 p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));
328 tmp1 = atan2( (in->cb - in->ca), tmp0 );
329 out[0] = mod2pi(tmp1 - in->alpha);
331 out[2] = mod2pi(in->beta - tmp1);
338 int dubins_RSR(DubinsIntermediateResults* in, double out[3])
340 double tmp0 = in->d - in->sa + in->sb;
341 double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
343 double tmp1 = atan2( (in->ca - in->cb), tmp0 );
344 out[0] = mod2pi(in->alpha - tmp1);
346 out[2] = mod2pi(tmp1 -in->beta);
352 int dubins_LSR(DubinsIntermediateResults* in, double out[3])
354 double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
356 double p = sqrt(p_sq);
357 double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
358 out[0] = mod2pi(tmp0 - in->alpha);
360 out[2] = mod2pi(tmp0 - mod2pi(in->beta));
366 int dubins_RSL(DubinsIntermediateResults* in, double out[3])
368 double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
370 double p = sqrt(p_sq);
371 double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
372 out[0] = mod2pi(in->alpha - tmp0);
374 out[2] = mod2pi(in->beta - tmp0);
380 int dubins_RLR(DubinsIntermediateResults* in, double out[3])
382 double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 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 + mod2pi(p/2.));
389 out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
395 int dubins_LRL(DubinsIntermediateResults* in, double out[3])
397 double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
398 double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
399 if( fabs(tmp0) <= 1) {
400 double p = mod2pi( 2*M_PI - acos( tmp0) );
401 double t = mod2pi(-in->alpha - phi + p/2.);
404 out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
410 int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3])
416 result = dubins_LSL(in, out);
419 result = dubins_RSL(in, out);
422 result = dubins_LSR(in, out);
425 result = dubins_RSR(in, out);
428 result = dubins_LRL(in, out);
431 result = dubins_RLR(in, out);