19 #ifndef _CLASS_EXACTCUBIC 20 #define _CLASS_EXACTCUBIC 36 template <
typename Time = double,
typename Numeric =
Time, std::size_t Dim = 3,
bool Safe =
false,
37 typename Point = Eigen::Matrix<Numeric, Dim, 1>,
38 typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
39 typename SplineBase = polynom<Time, Numeric, Dim, Safe, Point, T_Point> >
43 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic>
MatrixX;
57 template <
typename In>
59 : curve_abc_t(),
subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
72 template <
typename In>
73 t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd)
const {
74 std::size_t
const size(std::distance(wayPointsBegin, wayPointsEnd));
75 if (Safe && size < 1) {
78 t_spline_t subSplines;
79 subSplines.reserve(size);
82 MatrixX h1 = MatrixX::Zero(size, size);
83 MatrixX h2 = MatrixX::Zero(size, size);
84 MatrixX h3 = MatrixX::Zero(size, size);
85 MatrixX h4 = MatrixX::Zero(size, size);
86 MatrixX h5 = MatrixX::Zero(size, size);
87 MatrixX h6 = MatrixX::Zero(size, size);
89 MatrixX a = MatrixX::Zero(size, Dim);
90 MatrixX b = MatrixX::Zero(size, Dim);
91 MatrixX c = MatrixX::Zero(size, Dim);
92 MatrixX d = MatrixX::Zero(size, Dim);
93 MatrixX x = MatrixX::Zero(size, Dim);
95 In it(wayPointsBegin), next(wayPointsBegin);
98 for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) {
99 num_t
const dTi((*next).first - (*it).first);
100 num_t
const dTi_sqr(dTi * dTi);
101 num_t
const dTi_cube(dTi_sqr * dTi);
103 h3(i, i) = -3 / dTi_sqr;
104 h3(i, i + 1) = 3 / dTi_sqr;
106 h4(i, i + 1) = -1 / dTi;
107 h5(i, i) = 2 / dTi_cube;
108 h5(i, i + 1) = -2 / dTi_cube;
109 h6(i, i) = 1 / dTi_sqr;
110 h6(i, i + 1) = 1 / dTi_sqr;
114 num_t
const dTi_1((*it2).first - (*next).first);
115 num_t
const dTi_1sqr(dTi_1 * dTi_1);
117 h1(i + 1, i) = 2 / dTi;
118 h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1;
119 h1(i + 1, i + 2) = 2 / dTi_1;
120 h2(i + 1, i) = -6 / dTi_sqr;
121 h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
122 h2(i + 1, i + 2) = 6 / dTi_1sqr;
124 x.row(i) = (*it).second.transpose();
127 x.row(size - 1) = (*it).second.transpose();
133 it = wayPointsBegin, next = wayPointsBegin;
135 for (
int i = 0; next != wayPointsEnd; ++i, ++it, ++next) {
136 subSplines.push_back(create_cubic<Time, Numeric, Dim, Safe, Point, T_Point>(
137 a.row(i), b.row(i), c.row(i), d.row(i), (*it).first, (*next).first));
139 subSplines.push_back(create_cubic<Time, Numeric, Dim, Safe, Point, T_Point>(
140 a.row(size - 1), b.row(size - 1), c.row(size - 1), d.row(size - 1), (*it).first, (*it).first));
155 throw std::out_of_range(
"TODO");
158 if ((t >= (it->min()) && t <= (it->max())) || it + 1 ==
subSplines_.end())
return it->operator()(t);
161 throw std::runtime_error(
"Exact cubic evaluation failed; t is outside bounds");
168 virtual point_t
derivate(
const time_t t,
const std::size_t order)
const {
170 throw std::out_of_range(
"TODO");
173 if ((t >= (it->min()) && t <= (it->max())) || it + 1 ==
subSplines_.end())
return it->derivate(t, order);
176 throw std::runtime_error(
"Exact cubic evaluation failed; t is outside bounds");
192 #endif //_CLASS_EXACTCUBIC virtual ~exact_cubic()
Destructor.
Definition: exact_cubic.h:69
Definition of a cubic spline.
SplineBase spline_t
Definition: exact_cubic.h:46
Definition: bernstein.h:20
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
virtual num_t max() const
Returns the maximum time for wich curve is defined.
Definition: exact_cubic.h:183
Numeric num_t
Definition: exact_cubic.h:45
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: exact_cubic.h:43
t_spline_t::iterator it_spline_t
Definition: exact_cubic.h:48
interface for a Curve of arbitrary dimension.
t_spline_t::const_iterator cit_spline_t
Definition: exact_cubic.h:49
std::vector< spline_t > t_spline_t
Definition: exact_cubic.h:47
double Time
Definition: effector_spline.h:27
double Numeric
Definition: effector_spline.h:26
Definition: exact_cubic.h:40
exact_cubic(const exact_cubic &other)
Copy Constructor.
Definition: exact_cubic.h:66
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: exact_cubic.h:153
void PseudoInverse(_Matrix_Type_ &pinvmat)
Definition: MathDefs.h:28
virtual num_t min() const
Returns the minimum time for wich curve is defined.
Definition: exact_cubic.h:182
T_Point t_point_t
Definition: exact_cubic.h:42
exact_cubic(In wayPointsBegin, In wayPointsEnd)
Constructor.
Definition: exact_cubic.h:58
t_spline_t subSplines_
Definition: exact_cubic.h:188
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative spline at time t.
Definition: exact_cubic.h:168
Point point_t
Definition: exact_cubic.h:41
Represents a curve of dimension Dim is Safe is false, no verification is made on the evaluation of th...
Definition: curve_abc.h:24
Time time_t
Definition: exact_cubic.h:44
curve_abc< Time, Numeric, Dim, Safe, Point > curve_abc_t
Definition: exact_cubic.h:50
exact_cubic(const t_spline_t &subSplines)
Constructor.
Definition: exact_cubic.h:63
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28