cheshirekow  v0.1.0
SolutionRLRa.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_SOLUTION_RLR_A_HPP_
27 #define MPBLOCKS_DUBINS_CURVES_EIGEN_SOLUTION_RLR_A_HPP_
28 
32 
33 namespace mpblocks {
34 namespace dubins {
35 namespace curves_eigen {
36 
38 
49 template <typename Scalar>
50 struct Solver<RLRa, Scalar> {
51  typedef Eigen::Matrix<Scalar, 3, 1> Vector3d;
52  typedef Eigen::Matrix<Scalar, 2, 1> Vector2d;
53 
54  static Path<Scalar> solve(const Vector3d& q0, const Vector3d& q1,
55  const Scalar r) {
56  Path<Scalar> out(RLRa);
57  Vector2d c[3];
58  Vector3d s;
59 
60  // calculate the center of the circle to which q0 is tangent
61  c[0] = rightCenter(q0, r);
62 
63  // calculate the center of the circle to which q1 is tangent
64  c[2] = rightCenter(q1, r);
65 
66  // the distance between the centers of these two circles
67  Scalar d = (c[0] - c[2]).norm();
68 
69  // if the distance is too large, then this primitive is not the solution,
70  // and we can bail here
71  if (d > 4 * r) {
72  return out;
73  }
74 
75  // if the distance is zero then the geometry degenerates
76  if (d == 0) {
77  out = Vector3d(0, 0, 0);
78  return out;
79  }
80 
81  // the base angle of the isosceles triangle whose vertices are the centers
82  // of the the three circles, note acos returns [0,pi]
83  Scalar a = acos(d / (4 * r));
84 
85  // create a clockwise rotation of magnitude alpha
86  Eigen::Rotation2D<Scalar> R(-a);
87 
88  // we find the third vertex of this triangle by taking the vector between
89  // the two circle centers, normalizing it, and rotating it by alpha, and
90  // scaling it to magnitude 2r, then it points from the center of
91  // one the circle tangent to q1 to the third vertex
92  c[1] = c[0] + R * (c[2] - c[0]).normalized() * 2 * r;
93 
94  // calculate the arc distance we travel on the first circle
95  Vector2d dc = c[1] - c[0]; //< vector between centers of circles
96  Scalar a0 = rightAngleOf(q0); //< arc location of q0
97  Scalar a1 = std::atan2(dc[1], dc[0]); //< arc location of tangent
98  s[0] = cwArc(a0, a1); //< ccwise distance
99 
100  // calculate the arc distance we travel on the third circle
101  s[1] = M_PI + 2 * a;
102 
103  // calculate the arc distance we travel on the second circle
104  dc = c[1] - c[2]; //< vector between centers of circles
105  a0 = std::atan2(dc[1], dc[0]); //< angle of that vector
106  a1 = rightAngleOf(q1); //< angle of vector from center to q1
107  s[2] = cwArc(a0, a1); //< ccwise distance
108 
109  out = s;
110  return out;
111  }
112 };
113 
114 } // curves_eigen
115 } // dubins
116 } // mpblocks
117 
118 #endif // MPBLOCKS_DUBINS_CURVES_EIGEN_SOLUTION_RLR_A_HPP_
__host__ __device__ Format_t cwArc(Format_t a, Format_t b)
returns the clockwise (right) distance from a to b
Definition: funcs.hpp:72
Matrix< double, 3, 1 > Vector3d
Definition: matrix.h:145
empty struct used to template "variant" of three arc primitives
Definition: types.h:43
empty struct used to template "right turn" primitive
Definition: types.h:37
Scalar rightAngleOf(const Scalar q_theta)
return the angle of the vector from the center of the clockwise (right) circle coincident to q...
Definition: funcs.hpp:107
interface for different solutions, this is specialized for each Id in the SolutionId enum ...
Definition: solver.h:39
__device__ __host__ Scalar norm(const RValue< Scalar, ROWS, COLS, Exp > &M)
compute the norm
Definition: Norm.h:140
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
__host__ __device__ Normalized< Scalar, Exp, Spec > normalized(const RValue< Scalar, Exp, Spec > &exp)
Definition: Normalized.h:72
Encodes a dubins path primitive, which is three connected arc segments.
Definition: path.h:42
static Path< Scalar > solve(const Vector3d &q0, const Vector3d &q1, const Scalar r)