1 // Copyright (c) 2016, Guan-Horng Liu.
2 // SPDX-FileCopyrightText: 2016 Guan-Horng Liu
4 // SPDX-License-Identifier: BSD-3-Clause
6 #ifndef SPACES_REEDS_SHEPP_STATE_SPACE_
7 #define SPACES_REEDS_SHEPP_STATE_SPACE_
12 typedef int (*ReedsSheppPathSamplingCallback)(double q[3], void* user_data);
13 typedef int (*ReedsSheppPathTypeCallback)(int t, void* user_data);
15 class ReedsSheppStateSpace
19 /** \brief The Reeds-Shepp path segment types */
20 enum ReedsSheppPathSegmentType { RS_NOP=0, RS_LEFT=1, RS_STRAIGHT=2, RS_RIGHT=3 };
22 /** \brief Reeds-Shepp path types */
23 static const ReedsSheppPathSegmentType reedsSheppPathType[18][5];
25 /** \brief Complete description of a ReedsShepp path */
29 ReedsSheppPath(const ReedsSheppPathSegmentType* type=reedsSheppPathType[0],
30 double t=std::numeric_limits<double>::max(), double u=0., double v=0.,
31 double w=0., double x=0.);
33 double length() const { return totalLength_; }
35 /** Path segment types */
36 const ReedsSheppPathSegmentType* type_;
37 /** Path segment lengths */
43 ReedsSheppStateSpace(double turningRadius) : rho_(turningRadius) {}
45 double distance(double q0[3], double q1[3]);
47 void type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data);
49 void sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data);
51 /** \brief Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2 */
52 ReedsSheppPath reedsShepp(double q0[3], double q1[3]);
55 void interpolate(double q0[3], ReedsSheppPath &path, double seg, double q[4]);
57 /** \brief Turning radius */