Robotics Library  0.7.0
CircularVector2.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2009, Andre Gaschler
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 //
14 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
18 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24 // POSSIBILITY OF SUCH DAMAGE.
25 //
26 
27 #ifndef RL_MATH_CIRCULARVECTOR2_H
28 #define RL_MATH_CIRCULARVECTOR2_H
29 
30 #define EIGEN_MATRIXBASE_PLUGIN <rl/math/MatrixBaseAddons.h>
31 #define EIGEN_QUATERNIONBASE_PLUGIN <rl/math/QuaternionBaseAddons.h>
32 #define EIGEN_TRANSFORM_PLUGIN <rl/math/TransformAddons.h>
33 
34 #include <cassert>
35 #include <cmath>
36 #include <limits>
37 #include <Eigen/Dense>
38 #include <Eigen/LU>
39 
40 #include "Circular.h"
41 #include "Function.h"
42 #include "Matrix.h"
43 #include "Vector.h"
44 
45 namespace rl
46 {
47  namespace math
48  {
54  template<>
55  class Circular<Vector2> : public Function<Vector2>
56  {
57  public:
60  angle(0)
61  {
62  }
63 
64  virtual ~Circular<Vector2>()
65  {
66  }
67 
73  static Circular<Vector2> ThreePoints(const Vector2& y0, const Vector2& yi, const Vector2& y1, const Real& x1 = 1)
74  {
76 
77  ::Eigen::Matrix<Real, 2, 3> points2d;
78  points2d << y0, yi, y1;
79 
80  // Solve substituted system similar to http://www.arndt-bruenner.de/mathe/scripts/kreis3p.htm
81  Matrix33 A;
82  A << Matrix::Ones(3, 1), -points2d.transpose();
83  Vector3 b = - (points2d.transpose() * points2d).diagonal();
84  Vector3 x = A.fullPivLu().solve(b);
85  Vector2 center2d = x.bottomRows(2) / 2;
86  //Real radiusSquared = center2d.transpose() * center2d - x(0);
87  //Real radius = ::std::sqrt(radiusSquared);
88  assert((A * x - b).norm() < 1e-8 && "Circular motion: Linear system cannot be solved. (Points must not be colinear.)");
89 
90  f.center = center2d;
91  f.axisX = y0 - f.center;
92  f.axisY = Vector2(f.axisX(1), -f.axisX(0));
93  Real angleVia = ::std::atan2(f.axisY.transpose() * (yi - f.center), f.axisX.transpose() * (yi - f.center));
94 
95  if (angleVia < 0)
96  {
97  f.axisY *= -1.0;
98  }
99 
100  f.angle = ::std::atan2(f.axisY.transpose() * (y1 - f.center), f.axisX.transpose() * (y1 - f.center));
101 
102  if (f.angle < 0)
103  {
104  f.angle += 2 * static_cast<Real>(M_PI);
105  }
106 
107  f.x1 = x1;
108 
109  return f;
110  }
111 
124  static Circular<Vector2> ThreePointsAngle(const Vector2& y0, const Vector2& yi, const Vector2& y1, const Real& angle, const Real& x1 = 1)
125  {
126  Circular<Vector2> c = ThreePoints(y0, yi, y1, x1);
127  c.angle = angle;
128  return c;
129  }
130 
132  {
133  return new Circular<Vector2>(*this);
134  }
135 
136  Real getAngle() const
137  {
138  return this->angle;
139  }
140 
142  {
143  return this->axisX;
144  }
145 
147  {
148  return this->axisY;
149  }
150 
152  {
153  return this->center;
154  }
155 
162  Vector2 operator()(const Real& x, const ::std::size_t& derivative = 0) const
163  {
164  assert(x >= this->lower() - FUNCTION_BOUNDARY);
165  assert(x <= this->upper() + FUNCTION_BOUNDARY);
166  assert(derivative <= 2 && "Circular: higher derivatives not implemented");
167 
168  Real c = this->angle / this->x1;
169 
170  if (derivative == 0)
171  {
172  return this->center + ::std::cos(c * x) * this->axisX + ::std::sin(c * x) * this->axisY;
173  }
174  else if (derivative == 1)
175  {
176  return -c * ::std::sin(c * x) * this->axisX + c * ::std::cos(c * x) * this->axisY;
177  }
178  else if (derivative == 2)
179  {
180  return -::std::pow(c, 2) * ::std::cos(c * x) * this->axisX - ::std::pow(c, 2) * ::std::sin(c * x) * this->axisY;
181  }
182  else
183  {
184  return this->axisX * ::std::numeric_limits<Real>::signaling_NaN();
185  }
186  }
187 
188  protected:
191 
194 
197 
200 
201  private:
202 
203  };
204  }
205 }
206 
207 #endif // RL_MATH_CIRCULARVECTOR2_H
rl::math::Circular< Vector2 >::axisY
Vector2 axisY
Second axis of the circular motion.
Definition: CircularVector2.h:196
pow
Quaternion< Scalar > pow(const Scalar &t) const
Definition: QuaternionBaseAddons.h:128
rl::math::Circular
Circular segment function that maps from a time x to a point on a circular trajectory.
Definition: Circular.h:43
rl::math::Circular< Vector2 >::getCenter
Vector2 getCenter() const
Definition: CircularVector2.h:151
rl::math::Circular< Vector2 >::operator()
Vector2 operator()(const Real &x, const ::std::size_t &derivative=0) const
Evaluates the circular segment function for a given x.
Definition: CircularVector2.h:162
rl::math::Function
A mathematical mapping from Real -> ArrayX.
Definition: Function.h:45
rl::math::Function::x1
Real x1
Definition: Function.h:104
rl::math::Vector3
::Eigen::Matrix< Real, 3, 1 > Vector3
Definition: Vector.h:46
rl::math::Circular< Vector2 >
Circular segment function that maps from a time x to a point in 2D on a circular trajectory.
Definition: CircularVector2.h:56
rl::math::Circular< Vector2 >::clone
Circular< Vector2 > * clone() const
Definition: CircularVector2.h:131
rl::math::Circular< Vector2 >::getAxisX
Vector2 getAxisX() const
Definition: CircularVector2.h:141
Vector.h
Circular.h
rl::math::Function::upper
Real & upper()
Definition: Function.h:74
rl::math::Circular< Vector2 >::ThreePointsAngle
static Circular< Vector2 > ThreePointsAngle(const Vector2 &y0, const Vector2 &yi, const Vector2 &y1, const Real &angle, const Real &x1=1)
Generates a circular segment through three given points in 2D with a given segment angle.
Definition: CircularVector2.h:124
rl::math::Circular< Vector2 >::ThreePoints
static Circular< Vector2 > ThreePoints(const Vector2 &y0, const Vector2 &yi, const Vector2 &y1, const Real &x1=1)
Generates a circular segment function in 2D through three given points.
Definition: CircularVector2.h:73
rl::math::Circular< Vector2 >::axisX
Vector2 axisX
First axis of the circular motion.
Definition: CircularVector2.h:193
rl::math::Vector2
::Eigen::Matrix< Real, 2, 1 > Vector2
Definition: Vector.h:44
Matrix.h
rl::math::Circular< Vector2 >::getAngle
Real getAngle() const
Definition: CircularVector2.h:136
rl::math::Circular< Vector2 >::getAxisY
Vector2 getAxisY() const
Definition: CircularVector2.h:146
rl::math::Circular< Vector2 >::center
Vector2 center
Center of the circle.
Definition: CircularVector2.h:199
rl::math::FUNCTION_BOUNDARY
static const Real FUNCTION_BOUNDARY
Definition: Function.h:110
rl::math::Circular< Vector2 >::angle
Real angle
Angle of circular motion.
Definition: CircularVector2.h:190
rl::math::Function::lower
Real & lower()
Definition: Function.h:64
rl::math::Real
double Real
Definition: Real.h:42
Function.h
rl::math::Matrix33
::Eigen::Matrix< Real, 3, 3 > Matrix33
Definition: Matrix.h:46
rl
Robotics Library.
Definition: AnalogInput.cpp:30