Commit 16f04625 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

Implement SO3Linear::compute_derivate

parent c5fb7c7f
......@@ -4,7 +4,7 @@
#include "MathDefs.h"
#include "curve_abc.h"
#include "fwd.h"
#include "constant_curve.h"
#include <Eigen/Geometry>
#include <boost/math/constants/constants.hpp>
......@@ -25,7 +25,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
typedef Eigen::Quaternion<Scalar> quaternion_t;
typedef Time time_t;
typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t;
typedef polynomial<Time, Numeric, Safe, point_derivate_t, point_derivate_t> curve_derivate_t;
typedef constant_curve<Time, Numeric, Safe, point_derivate_t> curve_derivate_t;
typedef SO3Linear<Time, Numeric, Safe> SO3Linear_t;
......@@ -164,8 +164,8 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
}
}
curve_derivate_t compute_derivate(const std::size_t /*order*/) const {
throw std::logic_error("Compute derivate for SO3Linear is not implemented yet.");
curve_derivate_t compute_derivate(const std::size_t order) const {
return curve_derivate_t(derivate(T_min_, order), T_min_, T_max_);
}
/// \brief Compute the derived curve at order N.
......
......@@ -1679,6 +1679,20 @@ void so3LinearTest(bool& error) {
error = true;
std::cout << "Angular velocity around y and z axis should be null" << std::endl;
}
constant3_t so3Derivate1 = so3Traj.compute_derivate(1);
if (so3Derivate1(1.) != so3Traj.derivate(1., 1)) {
error = true;
std::cout << "compute_derivate curve do not equal derivate call" << std::endl;
}
constant3_t so3Derivate2 = so3Traj.compute_derivate(2);
if (so3Derivate2(1.) != point3_t::Zero(3)) {
error = true;
std::cout << "compute_derivate curve do not equal derivate call" << std::endl;
}
// check if errors are correctly raised :
try {
so3Traj(-0.1);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment