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>
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) {
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");
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