cheshirekow  v0.1.0
RSR.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_Y_RSR_H_
27 #define MPBLOCKS_DUBINS_CURVES_EIGEN_HYPER_TWO_THETA_Y_RSR_H_
28 
29 namespace mpblocks {
30 namespace dubins {
31 namespace curves_eigen {
32 namespace hyper {
33 namespace two_theta_y {
34 
36 template <int ySpec, int tSpec, typename Format_t>
37 struct Solver<ySpec, tSpec, RSR, 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<OFF, ySpec, 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 = RSR;
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 << q0[0], get_constraint<ySpec, 1>(h), get_constraint<tSpec, 2>(h);
58 
59  // the center of the goal point (we'll vary the x coordinate)
60  Vector2d_t c1 = rightCenter(q1, r);
61 
62  // vertical distance between centers
63  const Format_t dy = c1[1] - c0[1];
64 
66  Format_t dist, dist_i, d1, arc0, arc1;
67 
69  Format_t target;
70  if (dy > 0)
71  target = M_PI / 2;
72  else
73  target = -M_PI / 2;
74 
75  d1 = std::abs(dy);
76  arc0 = cwArc(q0[2], target);
77  arc1 = cwArc(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 sinTheta = std::sin(q0[2]);
88  if ((dy > 0 && sinTheta > 0) || (dy < 0 && sinTheta < 0)) {
89  d1 = dy / sinTheta;
90  arc0 = 0;
91  arc1 = cwArc(q0[2], q1[2]);
92  dist_i = r * (arc0 + arc1) + d1;
93 
94  out[1] = Vector3d_t(arc0, d1, arc1);
95  if (dist_i < dist) {
96  iBest = 1;
97  dist = dist_i;
98  }
99  }
100 
101  sinTheta = std::sin(q1[2]);
102  if ((dy > 0 && sinTheta > 0) || (dy < 0 && sinTheta < 0)) {
103  d1 = dy / sinTheta;
104  arc0 = cwArc(q0[2], q1[2]);
105  arc1 = 0;
106  dist_i = r * (arc0 + arc1) + d1;
107 
108  out[2] = Vector3d_t(arc0, d1, arc1);
109  if (dist_i < dist) {
110  iBest = 2;
111  dist = dist_i;
112  }
113  }
114 
115  return out[iBest];
116  };
117 };
118 
119 } // namespace two_theta_y
120 } // namespace hyper
121 } // namespace curves_eigen
122 } // namespace dubins
123 } // namespace mpblocks
124 
125 #endif // MPBLOCKS_DUBINS_CURVES_EIGEN_HYPER_TWO_THETA_Y_RSR_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
interface for variants of solvers, default template is never instantiated
Definition: Solver.hpp:41
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
static Result_t solve(const Vector3d_t &q0, const Hyper_t &h, const Format_t r)
Definition: RSR.hpp:45