47 template<
class ScalarType>
75 const State& start_state,
77 const State& end_state)
79 init(start_time, start_state, end_time, end_state);
103 const State& start_state,
104 const Time& end_time,
105 const State& end_state);
124 typedef typename std::vector<SplineCoefficients>::const_iterator ConstIterator;
125 for(ConstIterator coefs_it =
coefs_.begin(); coefs_it !=
coefs_.end(); ++coefs_it)
127 const typename std::vector<Scalar>::size_type
id = std::distance(
coefs_.begin(), coefs_it);
184 template<
class ScalarType>
186 const State& start_state,
187 const Time& end_time,
188 const State& end_state)
191 if (end_time < start_time)
193 throw(std::invalid_argument(
"Quintic spline segment can't be constructed: end_time < start_time."));
197 throw(std::invalid_argument(
"Quintic spline segment can't be constructed: Endpoint positions can't be empty."));
201 throw(std::invalid_argument(
"Quintic spline segment can't be constructed: Endpoint positions size mismatch."));
204 const unsigned int dim = start_state.
position.size();
205 const bool has_velocity = !start_state.
velocity.empty() && !end_state.
velocity.empty();
208 if (has_velocity && dim != start_state.
velocity.size())
210 throw(std::invalid_argument(
"Quintic spline segment can't be constructed: Start state velocity size mismatch."));
212 if (has_velocity && dim != end_state.
velocity.size())
214 throw(std::invalid_argument(
"Quintic spline segment can't be constructed: End state velocity size mismatch."));
216 if (has_acceleration && dim!= start_state.
acceleration.size())
218 throw(std::invalid_argument(
"Quintic spline segment can't be constructed: Start state acceleration size mismatch."));
220 if (has_acceleration && dim != end_state.
acceleration.size())
222 throw(std::invalid_argument(
"Quintic spline segment can't be constructed: End state acceleratios size mismatch."));
226 start_time_ = start_time;
227 duration_ = end_time - start_time;
233 typedef typename std::vector<SplineCoefficients>::iterator Iterator;
237 for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
239 const typename std::vector<Scalar>::size_type
id = std::distance(coefs_.begin(), coefs_it);
241 computeCoefficients(start_state.
position[
id],
247 else if (!has_acceleration)
250 for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
252 const typename std::vector<Scalar>::size_type
id = std::distance(coefs_.begin(), coefs_it);
263 for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
265 const typename std::vector<Scalar>::size_type
id = std::distance(coefs_.begin(), coefs_it);
275 template<
class ScalarType>
279 for (
int i=1; i<=n; ++i)
281 powers[i] = powers[i-1]*x;
285 template<
class ScalarType>
292 coefficients[0] = start_pos;
293 coefficients[1] = (time == 0.0) ? 0.0 : (end_pos - start_pos) / time;
294 coefficients[2] = 0.0;
295 coefficients[3] = 0.0;
296 coefficients[4] = 0.0;
297 coefficients[5] = 0.0;
300 template<
class ScalarType>
309 coefficients[0] = start_pos;
310 coefficients[1] = start_vel;
311 coefficients[2] = 0.0;
312 coefficients[3] = 0.0;
317 generatePowers(3, time, T);
319 coefficients[0] = start_pos;
320 coefficients[1] = start_vel;
321 coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
322 coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
324 coefficients[4] = 0.0;
325 coefficients[5] = 0.0;
328 template<
class ScalarType>
337 coefficients[0] = start_pos;
338 coefficients[1] = start_vel;
339 coefficients[2] = 0.5*start_acc;
340 coefficients[3] = 0.0;
341 coefficients[4] = 0.0;
342 coefficients[5] = 0.0;
347 generatePowers(5, time, T);
349 coefficients[0] = start_pos;
350 coefficients[1] = start_vel;
351 coefficients[2] = 0.5*start_acc;
352 coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
353 12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
354 coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
355 16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
356 coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
357 6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
361 template<
class ScalarType>
368 generatePowers(5, time, t);
370 position = t[0]*coefficients[0] +
371 t[1]*coefficients[1] +
372 t[2]*coefficients[2] +
373 t[3]*coefficients[3] +
374 t[4]*coefficients[4] +
375 t[5]*coefficients[5];
377 velocity = t[0]*coefficients[1] +
378 2.0*t[1]*coefficients[2] +
379 3.0*t[2]*coefficients[3] +
380 4.0*t[3]*coefficients[4] +
381 5.0*t[4]*coefficients[5];
383 acceleration = 2.0*t[0]*coefficients[2] +
384 6.0*t[1]*coefficients[3] +
385 12.0*t[2]*coefficients[4] +
386 20.0*t[3]*coefficients[5];
389 template<
class ScalarType>
397 sample(coefficients, 0.0, position, _, _);
401 else if (time > duration)
404 sample(coefficients, duration, position, _, _);
410 sample(coefficients, time,
411 position, velocity, acceleration);