1 /*********************************************************************
3 * Software License Agreement (BSD License)
5 * Copyright (c) 2016, Guan-Horng Liu.
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
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.
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.
35 * Author: Guan-Horng Liu
36 *********************************************************************/
38 #ifndef SPACES_REEDS_SHEPP_STATE_SPACE_
39 #define SPACES_REEDS_SHEPP_STATE_SPACE_
41 #include <boost/math/constants/constants.hpp>
44 typedef int (*ReedsSheppPathSamplingCallback)(double q[3], void* user_data);
45 typedef int (*ReedsSheppPathTypeCallback)(int t, void* user_data);
47 class ReedsSheppStateSpace
51 /** \brief The Reeds-Shepp path segment types */
52 enum ReedsSheppPathSegmentType { RS_NOP=0, RS_LEFT=1, RS_STRAIGHT=2, RS_RIGHT=3 };
54 /** \brief Reeds-Shepp path types */
55 static const ReedsSheppPathSegmentType reedsSheppPathType[18][5];
57 /** \brief Complete description of a ReedsShepp path */
61 ReedsSheppPath(const ReedsSheppPathSegmentType* type=reedsSheppPathType[0],
62 double t=std::numeric_limits<double>::max(), double u=0., double v=0.,
63 double w=0., double x=0.);
65 double length() const { return totalLength_; }
67 /** Path segment types */
68 const ReedsSheppPathSegmentType* type_;
69 /** Path segment lengths */
75 ReedsSheppStateSpace(double turningRadius) : rho_(turningRadius) {}
77 double distance(double q0[3], double q1[3]);
79 void type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data);
81 void sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data);
83 /** \brief Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2 */
84 ReedsSheppPath reedsShepp(double q0[3], double q1[3]);
87 void interpolate(double q0[3], ReedsSheppPath &path, double seg, double q[4]);
89 /** \brief Turning radius */