Commit 32bd5feb authored by JasonChmn's avatar JasonChmn Committed by Pierre Fernbach
Browse files

[piecewise_curve] Create conversions for piecewise_curve, add tests OK

parent 5822b3b6
......@@ -18,6 +18,7 @@ SET(CXX_DISABLE_WERROR True)
SETUP_HPP_PROJECT()
ADD_REQUIRED_DEPENDENCY(eigen3)
ADD_REQUIRED_DEPENDENCY(protobuf)
SET(BOOST_COMPONENTS unit_test_framework)
......
......@@ -20,3 +20,4 @@ INSTALL(FILES
)
ADD_SUBDIRECTORY(helpers)
ADD_SUBDIRECTORY(proto)
......@@ -99,6 +99,11 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>
}
}
bezier_curve(const bezier_curve& other)
: t_min_(other.t_min_), t_max_(other.t_max_), mult_T_(other.mult_T_), size_(other.size_),
order_(other.order_), bernstein_(other.bernstein_), control_points_(other.control_points_)
{}
///\brief Destructor
~bezier_curve()
{
......
......@@ -38,29 +38,6 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
typedef std::vector< pair_point_tangent_t ,Eigen::aligned_allocator<Point> > t_pair_point_tangent_t;
typedef std::vector<Time> vector_time_t;
typedef Numeric num_t;
/*Attributes*/
public:
/// Vector of pair < Point, Tangent >.
t_pair_point_tangent_t control_points_;
/// Vector of Time corresponding to time of each N control points : time at \f$P_0, P_1, P_2, ..., P_N\f$.
/// Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.
vector_time_t time_control_points_;
/// Vector of Time corresponding to time duration of each subspline.<br>
/// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
/// duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} )<br>
/// It contains \f$N-1\f$ durations.
vector_time_t duration_splines_;
/// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
/*const*/ Time T_min_;
/// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
/*const*/ Time T_max_;
/// Number of control points (pairs).
std::size_t size_;
/// Degree (Cubic so degree 3)
static const std::size_t degree_ = 3;
/*Attributes*/
public:
/// \brief Constructor.
......@@ -70,6 +47,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
///
template<typename In>
cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t & time_control_points)
: order_(3)
{
// Check size of pairs container.
std::size_t const size(std::distance(PairsBegin, PairsEnd));
......@@ -87,6 +65,12 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
setTime(time_control_points);
}
cubic_hermite_spline(const cubic_hermite_spline& other)
: t_min_(other.t_min_), t_max_(other.t_max_), size_(other.size_), order_(other.order_),
control_points_(other.control_points_), time_control_points_(other.time_control_points_),
duration_splines_(other.duration_splines_)
{}
/// \brief Destructor.
virtual ~cubic_hermite_spline(){}
......@@ -98,7 +82,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
///
virtual Point operator()(const Time t) const
{
if(Safe &! (T_min_ <= t && t <= T_max_))
if(Safe &! (t_min_ <= t && t <= t_max_))
{
throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
}
......@@ -130,8 +114,8 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
void setTime(const vector_time_t & time_control_points)
{
time_control_points_ = time_control_points;
T_min_ = time_control_points_.front();
T_max_ = time_control_points_.back();
t_min_ = time_control_points_.front();
t_max_ = time_control_points_.back();
if (time_control_points.size() != size())
{
throw std::length_error("size of time control points should be equal to number of control points");
......@@ -365,6 +349,28 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
Time virtual max() const{return time_control_points_.back();}
/*Helpers*/
/*Attributes*/
/// Vector of pair < Point, Tangent >.
t_pair_point_tangent_t control_points_;
/// Vector of Time corresponding to time of each N control points : time at \f$P_0, P_1, P_2, ..., P_N\f$.
/// Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.
vector_time_t time_control_points_;
/// Vector of Time corresponding to time duration of each subspline.<br>
/// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
/// duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} )<br>
/// It contains \f$N-1\f$ durations.
vector_time_t duration_splines_;
/// Starting time of cubic hermite spline : t_min_ is equal to first time of control points.
/*const*/ Time t_min_;
/// Ending time of cubic hermite spline : t_max_ is equal to last time of control points.
/*const*/ Time t_max_;
/// Number of control points (pairs).
std::size_t size_;
/// Degree (Cubic so degree 3)
std::size_t order_;
/*Attributes*/
};
} // namespace curve
......
......@@ -11,8 +11,6 @@
#include "curve_abc.h"
#include "curve_conversion.h"
#include <typeinfo>
namespace curves
{
/// \class PiecewiseCurve.
......@@ -80,7 +78,8 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>
}
/// \brief Add a new curve to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where \f$T_{min}\f$
/// is equal to \f$T_{max}\f$ of the actual piecewise curve.
/// is equal to \f$T_{max}\f$ of the actual piecewise curve. The curve added should be of type Curve as defined
/// in the template.
/// \param cf : curve to add.
///
void add_curve(const curve_t& cf)
......@@ -130,6 +129,56 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>
return isContinuous;
}
template<typename Bezier>
piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Bezier> convert_piecewise_curve_to_bezier()
{
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Bezier> piecewise_curve_out_t;
// Get first curve (segment)
curve_t first_curve = curves_.at(0);
Bezier first_curve_output = bezier_from_curve<Bezier, curve_t>(first_curve);
// Create piecewise curve
piecewise_curve_out_t pc_res(first_curve_output);
// Convert and add all other curves (segments)
for (std::size_t i=1; i<size_; i++)
{
pc_res.add_curve(bezier_from_curve<Bezier, curve_t>(curves_.at(i)));
}
return pc_res;
}
template<typename Hermite>
piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Hermite> convert_piecewise_curve_to_cubic_hermite()
{
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Hermite> piecewise_curve_out_t;
// Get first curve (segment)
curve_t first_curve = curves_.at(0);
Hermite first_curve_output = hermite_from_curve<Hermite, curve_t>(first_curve);
// Create piecewise curve
piecewise_curve_out_t pc_res(first_curve_output);
// Convert and add all other curves (segments)
for (std::size_t i=1; i<size_; i++)
{
pc_res.add_curve(hermite_from_curve<Hermite, curve_t>(curves_.at(i)));
}
return pc_res;
}
template<typename Polynomial>
piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> convert_piecewise_curve_to_polynomial()
{
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
// Get first curve (segment)
curve_t first_curve = curves_.at(0);
Polynomial first_curve_output = polynomial_from_curve<Polynomial, curve_t>(first_curve);
// Create piecewise curve
piecewise_curve_out_t pc_res(first_curve_output);
// Convert and add all other curves (segments)
for (std::size_t i=1; i<size_; i++)
{
pc_res.add_curve(polynomial_from_curve<Polynomial, curve_t>(curves_.at(i)));
}
return pc_res;
}
private:
......
......@@ -95,7 +95,8 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point>
polynomial(const polynomial& other)
: coefficients_(other.coefficients_),
dim_(other.dim_), order_(other.order_), t_min_(other.t_min_), t_max_(other.t_max_){}
dim_(other.dim_), order_(other.order_), t_min_(other.t_min_), t_max_(other.t_max_)
{}
//polynomial& operator=(const polynomial& other);
......
INCLUDE(FindProtobuf)
FIND_PACKAGE(Protobuf REQUIRED)
INCLUDE_DIRECTORIES(${PROTOBUF_INCLUDE_DIR})
PROTOBUF_GENERATE_CPP(PROTO_SRC PROTO_HEADER Curves_proto.proto)
ADD_LIBRARY(proto ${PROTO_HEADER} ${PROTO_SRC})
This diff is collapsed.
// Generated by the protocol buffer compiler. DO NOT EDIT!
// source: Curves_proto.proto
#ifndef PROTOBUF_Curves_5fproto_2eproto__INCLUDED
#define PROTOBUF_Curves_5fproto_2eproto__INCLUDED
#include <string>
#include <google/protobuf/stubs/common.h>
#if GOOGLE_PROTOBUF_VERSION < 3005000
#error This file was generated by a newer version of protoc which is
#error incompatible with your Protocol Buffer headers. Please update
#error your headers.
#endif
#if 3005001 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
#error This file was generated by an older version of protoc which is
#error incompatible with your Protocol Buffer headers. Please
#error regenerate this file with a newer version of protoc.
#endif
#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/arena.h>
#include <google/protobuf/arenastring.h>
#include <google/protobuf/generated_message_table_driven.h>
#include <google/protobuf/generated_message_util.h>
#include <google/protobuf/metadata.h>
#include <google/protobuf/message.h>
#include <google/protobuf/repeated_field.h> // IWYU pragma: export
#include <google/protobuf/extension_set.h> // IWYU pragma: export
#include <google/protobuf/unknown_field_set.h>
// @@protoc_insertion_point(includes)
namespace protobuf_Curves_5fproto_2eproto {
// Internal implementation detail -- do not use these members.
struct TableStruct {
static const ::google::protobuf::internal::ParseTableField entries[];
static const ::google::protobuf::internal::AuxillaryParseTableField aux[];
static const ::google::protobuf::internal::ParseTable schema[2];
static const ::google::protobuf::internal::FieldMetadata field_metadata[];
static const ::google::protobuf::internal::SerializationTable serialization_table[];
static const ::google::protobuf::uint32 offsets[];
};
void AddDescriptors();
void InitDefaultsMatrixImpl();
void InitDefaultsMatrix();
void InitDefaultsPiecewise_polynomial_curveImpl();
void InitDefaultsPiecewise_polynomial_curve();
inline void InitDefaults() {
InitDefaultsMatrix();
InitDefaultsPiecewise_polynomial_curve();
}
} // namespace protobuf_Curves_5fproto_2eproto
namespace Curves_proto {
class Matrix;
class MatrixDefaultTypeInternal;
extern MatrixDefaultTypeInternal _Matrix_default_instance_;
class Piecewise_polynomial_curve;
class Piecewise_polynomial_curveDefaultTypeInternal;
extern Piecewise_polynomial_curveDefaultTypeInternal _Piecewise_polynomial_curve_default_instance_;
} // namespace Curves_proto
namespace Curves_proto {
// ===================================================================
class Matrix : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:Curves_proto.Matrix) */ {
public:
Matrix();
virtual ~Matrix();
Matrix(const Matrix& from);
inline Matrix& operator=(const Matrix& from) {
CopyFrom(from);
return *this;
}
#if LANG_CXX11
Matrix(Matrix&& from) noexcept
: Matrix() {
*this = ::std::move(from);
}
inline Matrix& operator=(Matrix&& from) noexcept {
if (GetArenaNoVirtual() == from.GetArenaNoVirtual()) {
if (this != &from) InternalSwap(&from);
} else {
CopyFrom(from);
}
return *this;
}
#endif
static const ::google::protobuf::Descriptor* descriptor();
static const Matrix& default_instance();
static void InitAsDefaultInstance(); // FOR INTERNAL USE ONLY
static inline const Matrix* internal_default_instance() {
return reinterpret_cast<const Matrix*>(
&_Matrix_default_instance_);
}
static PROTOBUF_CONSTEXPR int const kIndexInFileMessages =
0;
void Swap(Matrix* other);
friend void swap(Matrix& a, Matrix& b) {
a.Swap(&b);
}
// implements Message ----------------------------------------------
inline Matrix* New() const PROTOBUF_FINAL { return New(NULL); }
Matrix* New(::google::protobuf::Arena* arena) const PROTOBUF_FINAL;
void CopyFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
void MergeFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
void CopyFrom(const Matrix& from);
void MergeFrom(const Matrix& from);
void Clear() PROTOBUF_FINAL;
bool IsInitialized() const PROTOBUF_FINAL;
size_t ByteSizeLong() const PROTOBUF_FINAL;
bool MergePartialFromCodedStream(
::google::protobuf::io::CodedInputStream* input) PROTOBUF_FINAL;
void SerializeWithCachedSizes(
::google::protobuf::io::CodedOutputStream* output) const PROTOBUF_FINAL;
::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
bool deterministic, ::google::protobuf::uint8* target) const PROTOBUF_FINAL;
int GetCachedSize() const PROTOBUF_FINAL { return _cached_size_; }
private:
void SharedCtor();
void SharedDtor();
void SetCachedSize(int size) const PROTOBUF_FINAL;
void InternalSwap(Matrix* other);
private:
inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
return NULL;
}
inline void* MaybeArenaPtr() const {
return NULL;
}
public:
::google::protobuf::Metadata GetMetadata() const PROTOBUF_FINAL;
// nested types ----------------------------------------------------
// accessors -------------------------------------------------------
// repeated double data = 3 [packed = true];
int data_size() const;
void clear_data();
static const int kDataFieldNumber = 3;
double data(int index) const;
void set_data(int index, double value);
void add_data(double value);
const ::google::protobuf::RepeatedField< double >&
data() const;
::google::protobuf::RepeatedField< double >*
mutable_data();
// uint32 rows = 1;
void clear_rows();
static const int kRowsFieldNumber = 1;
::google::protobuf::uint32 rows() const;
void set_rows(::google::protobuf::uint32 value);
// uint32 cols = 2;
void clear_cols();
static const int kColsFieldNumber = 2;
::google::protobuf::uint32 cols() const;
void set_cols(::google::protobuf::uint32 value);
// @@protoc_insertion_point(class_scope:Curves_proto.Matrix)
private:
::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
::google::protobuf::RepeatedField< double > data_;
mutable int _data_cached_byte_size_;
::google::protobuf::uint32 rows_;
::google::protobuf::uint32 cols_;
mutable int _cached_size_;
friend struct ::protobuf_Curves_5fproto_2eproto::TableStruct;
friend void ::protobuf_Curves_5fproto_2eproto::InitDefaultsMatrixImpl();
};
// -------------------------------------------------------------------
class Piecewise_polynomial_curve : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:Curves_proto.Piecewise_polynomial_curve) */ {
public:
Piecewise_polynomial_curve();
virtual ~Piecewise_polynomial_curve();
Piecewise_polynomial_curve(const Piecewise_polynomial_curve& from);
inline Piecewise_polynomial_curve& operator=(const Piecewise_polynomial_curve& from) {
CopyFrom(from);
return *this;
}
#if LANG_CXX11
Piecewise_polynomial_curve(Piecewise_polynomial_curve&& from) noexcept
: Piecewise_polynomial_curve() {
*this = ::std::move(from);
}
inline Piecewise_polynomial_curve& operator=(Piecewise_polynomial_curve&& from) noexcept {
if (GetArenaNoVirtual() == from.GetArenaNoVirtual()) {
if (this != &from) InternalSwap(&from);
} else {
CopyFrom(from);
}
return *this;
}
#endif
static const ::google::protobuf::Descriptor* descriptor();
static const Piecewise_polynomial_curve& default_instance();
static void InitAsDefaultInstance(); // FOR INTERNAL USE ONLY
static inline const Piecewise_polynomial_curve* internal_default_instance() {
return reinterpret_cast<const Piecewise_polynomial_curve*>(
&_Piecewise_polynomial_curve_default_instance_);
}
static PROTOBUF_CONSTEXPR int const kIndexInFileMessages =
1;
void Swap(Piecewise_polynomial_curve* other);
friend void swap(Piecewise_polynomial_curve& a, Piecewise_polynomial_curve& b) {
a.Swap(&b);
}
// implements Message ----------------------------------------------
inline Piecewise_polynomial_curve* New() const PROTOBUF_FINAL { return New(NULL); }
Piecewise_polynomial_curve* New(::google::protobuf::Arena* arena) const PROTOBUF_FINAL;
void CopyFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
void MergeFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
void CopyFrom(const Piecewise_polynomial_curve& from);
void MergeFrom(const Piecewise_polynomial_curve& from);
void Clear() PROTOBUF_FINAL;
bool IsInitialized() const PROTOBUF_FINAL;
size_t ByteSizeLong() const PROTOBUF_FINAL;
bool MergePartialFromCodedStream(
::google::protobuf::io::CodedInputStream* input) PROTOBUF_FINAL;
void SerializeWithCachedSizes(
::google::protobuf::io::CodedOutputStream* output) const PROTOBUF_FINAL;
::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
bool deterministic, ::google::protobuf::uint8* target) const PROTOBUF_FINAL;
int GetCachedSize() const PROTOBUF_FINAL { return _cached_size_; }
private:
void SharedCtor();
void SharedDtor();
void SetCachedSize(int size) const PROTOBUF_FINAL;
void InternalSwap(Piecewise_polynomial_curve* other);
private:
inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
return NULL;
}
inline void* MaybeArenaPtr() const {
return NULL;
}
public:
::google::protobuf::Metadata GetMetadata() const PROTOBUF_FINAL;
// nested types ----------------------------------------------------
// accessors -------------------------------------------------------
// repeated double time = 3 [packed = true];
int time_size() const;
void clear_time();
static const int kTimeFieldNumber = 3;
double time(int index) const;
void set_time(int index, double value);
void add_time(double value);
const ::google::protobuf::RepeatedField< double >&
time() const;
::google::protobuf::RepeatedField< double >*
mutable_time();
// repeated .Curves_proto.Matrix curves = 4;
int curves_size() const;
void clear_curves();
static const int kCurvesFieldNumber = 4;
const ::Curves_proto::Matrix& curves(int index) const;
::Curves_proto::Matrix* mutable_curves(int index);
::Curves_proto::Matrix* add_curves();
::google::protobuf::RepeatedPtrField< ::Curves_proto::Matrix >*
mutable_curves();
const ::google::protobuf::RepeatedPtrField< ::Curves_proto::Matrix >&
curves() const;
// uint32 number_of_curves = 1;
void clear_number_of_curves();
static const int kNumberOfCurvesFieldNumber = 1;
::google::protobuf::uint32 number_of_curves() const;
void set_number_of_curves(::google::protobuf::uint32 value);
// uint32 dimension = 2;
void clear_dimension();
static const int kDimensionFieldNumber = 2;
::google::protobuf::uint32 dimension() const;
void set_dimension(::google::protobuf::uint32 value);
// @@protoc_insertion_point(class_scope:Curves_proto.Piecewise_polynomial_curve)
private:
::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
::google::protobuf::RepeatedField< double > time_;
mutable int _time_cached_byte_size_;
::google::protobuf::RepeatedPtrField< ::Curves_proto::Matrix > curves_;
::google::protobuf::uint32 number_of_curves_;
::google::protobuf::uint32 dimension_;
mutable int _cached_size_;
friend struct ::protobuf_Curves_5fproto_2eproto::TableStruct;
friend void ::protobuf_Curves_5fproto_2eproto::InitDefaultsPiecewise_polynomial_curveImpl();
};
// ===================================================================
// ===================================================================
#if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
#endif // __GNUC__
// Matrix
// uint32 rows = 1;
inline void Matrix::clear_rows() {
rows_ = 0u;
}
inline ::google::protobuf::uint32 Matrix::rows() const {
// @@protoc_insertion_point(field_get:Curves_proto.Matrix.rows)
return rows_;
}
inline void Matrix::set_rows(::google::protobuf::uint32 value) {
rows_ = value;
// @@protoc_insertion_point(field_set:Curves_proto.Matrix.rows)
}
// uint32 cols = 2;
inline void Matrix::clear_cols() {
cols_ = 0u;
}
inline ::google::protobuf::uint32 Matrix::cols() const {
// @@protoc_insertion_point(field_get:Curves_proto.Matrix.cols)
return cols_;
}
inline void Matrix::set_cols(::google::protobuf::uint32 value) {
cols_ = value;
// @@protoc_insertion_point(field_set:Curves_proto.Matrix.cols)
}
// repeated double data = 3 [packed = true];
inline int Matrix::data_size() const {
return data_.size();
}
inline void Matrix::clear_data() {
data_.Clear();
}
inline double Matrix::data(int index) const {
// @@protoc_insertion_point(field_get:Curves_proto.Matrix.data)
return data_.Get(index);
}
inline void Matrix::set_data(int index, double value) {
data_.Set(index, value);
// @@protoc_insertion_point(field_set:Curves_proto.Matrix.data)
}
inline void Matrix::add_data(double value) {
data_.Add(value);
// @@protoc_insertion_point(field_add:Curves_proto.Matrix.data)
}
inline const ::google::protobuf::RepeatedField< double >&
Matrix::data() const {
// @@protoc_insertion_point(field_list:Curves_proto.Matrix.data)
return data_;
}
inline ::google::protobuf::RepeatedField< double >*
Matrix::mutable_data() {
// @@protoc_insertion_point(field_mutable_list:Curves_proto.Matrix.data)
return