cheshirekow  v0.1.0
RSL.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Josh Bialkowski (jbialk@mit.edu)
3  *
4  * This file is part of mpblocks.
5  *
6  * mpblocks is free software: you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation, either version 3 of the License, or
9  * (at your option) any later version.
10  *
11  * mpblocks is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with mpblocks. If not, see <http://www.gnu.org/licenses/>.
18  */
26 #ifndef MPBLOCKS_DUBINS_CURVES_EIGEN_HYPER_TWO_THETA_X_RSL_H_
27 #define MPBLOCKS_DUBINS_CURVES_EIGEN_HYPER_TWO_THETA_X_RSL_H_
28 
29 namespace mpblocks {
30 namespace dubins {
31 namespace curves_eigen {
32 namespace hyper {
33 namespace two_theta_x {
34 
36 template <int xSpec, int tSpec, typename Format_t>
37 struct Solver<xSpec, tSpec, RSL, Format_t> {
38  typedef Eigen::Matrix<Format_t, 3, 1> Vector3d_t;
39  typedef Eigen::Matrix<Format_t, 2, 1> Vector2d_t;
42 
43  static const unsigned int SPEC = SpecPack<xSpec, OFF, tSpec>::Result;
44 
45  static Result_t solve(const Vector3d_t& q0, const Hyper_t& h,
46  const Format_t r) {
47  Result_t out[3];
48  for (int i = 0; i < 3; i++) {
49  out[i].id = RSL;
50  }
51 
52  // calculate the center of the turn
53  const Vector2d_t c0 = rightCenter(q0, r);
54 
55  // the goal point (we'll vary the x coordinate)
56  Vector3d_t q1;
57  q1 << get_constraint<xSpec, 0>(h), q0[1], get_constraint<tSpec, 2>(h);
58 
59  // the center of the goal point (we'll vary the x coordinate)
60  Vector2d_t c1 = leftCenter(q1, r);
61 
62  // vertical distance between centers
63  const Format_t dx = c1[0] - c0[0];
64 
66  Format_t dist, dist_i, d1, arc0, arc1;
67 
69  Format_t target;
70  if (dx > 0)
71  target = 0;
72  else
73  target = M_PI;
74 
75  d1 = std::abs(dx);
76  arc0 = cwArc(q0[2], target);
77  arc1 = ccwArc(target, q1[2]);
78  dist = r * (arc0 + arc1) + d1;
79 
80  out[0] = Vector3d_t(arc0, d1, arc1);
81  int iBest = 0;
82 
87  Format_t cosTheta = std::cos(q0[2]);
88  if ((dx > 0 && cosTheta > 0) || (dx < 0 && cosTheta < 0)) {
89  d1 = (dx - 2 * r * std::cos(q0[2] + M_PI / 2)) / cosTheta;
90  if (d1 > 0) {
91  arc0 = 0;
92  arc1 = ccwArc(q0[2], q1[2]);
93  dist_i = r * (arc0 + arc1) + d1;
94 
95  out[1] = Vector3d_t(arc0, d1, arc1);
96  if (dist_i < dist) {
97  iBest = 1;
98  dist = dist_i;
99  }
100  }
101  }
102 
103  cosTheta = std::cos(q1[2]);
105  if ((dx > 0 && cosTheta > 0) || (dx < 0 && cosTheta < 0)) {
106  d1 = (dx - 2 * r * std::cos(q1[2] + M_PI / 2)) / cosTheta;
107  if (d1 > 0) {
108  arc0 = cwArc(q0[2], q1[2]);
109  arc1 = 0;
110  dist_i = r * (arc0 + arc1) + d1;
111 
112  out[2] = Vector3d_t(arc0, d1, arc1);
113  if (dist_i < dist) {
114  iBest = 2;
115  dist = dist_i;
116  }
117  }
118  }
119 
120  return out[iBest];
121  };
122 };
123 
124 } // namespace two_theta_x
125 } // namespace hyper
126 } // namespace curves_eigen
127 } // namespace dubins
128 } // namespace mpblocks
129 
130 #endif // MPBLOCKS_DUBINS_CURVES_EIGEN_HYPER_TWO_THETA_X_RSL_H_
__host__ __device__ Format_t cwArc(Format_t a, Format_t b)
returns the clockwise (right) distance from a to b
Definition: funcs.hpp:72
__host__ __device__ Format_t ccwArc(Format_t a, Format_t b)
returns the counter clockwise (left) distance from a to b
Definition: funcs.hpp:51
static Result_t solve(const Vector3d_t &q0, const Hyper_t &h, const Format_t r)
Definition: RSL.hpp:45
interface for variants of solvers, default template is never instantiated
Definition: Solver.hpp:37
int id
identifies the type of path
Definition: path.h:45
A hyper-rectangle in dubins space: A rectangular prism in R^3.
Definition: hyper_rect.h:44
Eigen::Matrix< Scalar, 2, 1 > rightCenter(const Eigen::Matrix< Scalar, 3, 1 > &q, Scalar r)
return the center of a clockwise (right) circle coincident to q with radius r
Definition: funcs.hpp:76
Encodes a dubins path primitive, which is three connected arc segments.
Definition: path.h:42
Eigen::Matrix< Scalar, 2, 1 > leftCenter(const Eigen::Matrix< Scalar, 3, 1 > &q, Scalar r)
return the center of a counter clockwise (left) circle coincident to q with radius r ...
Definition: funcs.hpp:54