Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Jason Chemin
curves
Commits
a9efbf46
Commit
a9efbf46
authored
Sep 23, 2019
by
Pierre Fernbach
Browse files
Merge remote-tracking branch 'jchemin/topic/add_serialization_boost' into devel
parents
7ea3d69b
b6633721
Changes
39
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
a9efbf46
...
...
@@ -19,7 +19,7 @@ SETUP_HPP_PROJECT()
ADD_REQUIRED_DEPENDENCY
(
eigen3
)
SET
(
BOOST_COMPONENTS unit_test_framework
)
SET
(
BOOST_COMPONENTS unit_test_framework
serialization
)
OPTION
(
BUILD_PYTHON_INTERFACE
"Build the python binding"
ON
)
IF
(
BUILD_PYTHON_INTERFACE
)
...
...
@@ -30,13 +30,18 @@ IF(BUILD_PYTHON_INTERFACE)
ADD_REQUIRED_DEPENDENCY
(
"eigenpy"
)
SET
(
BOOST_COMPONENTS
${
BOOST_COMPONENTS
}
python
)
ADD_SUBDIRECTORY
(
python
)
ENDIF
(
BUILD_PYTHON_INTERFACE
)
#find_package(Boost 1.58 REQUIRED unit_test_framework system serialization)
#SET(BOOST_COMPONENTS unit_test_framework serialization)
SEARCH_FOR_BOOST
()
INCLUDE_DIRECTORIES
(
SYSTEM
${
Boost_INCLUDE_DIRS
}
)
IF
(
BUILD_PYTHON_INTERFACE
)
ADD_SUBDIRECTORY
(
python
)
ENDIF
(
BUILD_PYTHON_INTERFACE
)
ADD_SUBDIRECTORY
(
include/curves
)
ADD_SUBDIRECTORY
(
tests
)
...
...
include/curves/CMakeLists.txt
View file @
a9efbf46
...
...
@@ -20,3 +20,4 @@ INSTALL(FILES
)
ADD_SUBDIRECTORY
(
helpers
)
ADD_SUBDIRECTORY
(
serialization
)
include/curves/MathDefs.h
View file @
a9efbf46
...
...
@@ -21,26 +21,22 @@
#include <vector>
#include <utility>
namespace
curves
{
//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template
<
typename
_Matrix_Type_
>
void
PseudoInverse
(
_Matrix_Type_
&
pinvmat
)
{
//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template
<
typename
_Matrix_Type_
>
void
PseudoInverse
(
_Matrix_Type_
&
pinvmat
)
{
Eigen
::
JacobiSVD
<
_Matrix_Type_
>
svd
(
pinvmat
,
Eigen
::
ComputeFullU
|
Eigen
::
ComputeFullV
);
_Matrix_Type_
m_sigma
=
svd
.
singularValues
();
double
pinvtoler
=
1.e-6
;
// choose your tolerance widely!
_Matrix_Type_
m_sigma_inv
=
_Matrix_Type_
::
Zero
(
pinvmat
.
cols
(),
pinvmat
.
rows
());
for
(
long
i
=
0
;
i
<
m_sigma
.
rows
();
++
i
)
{
if
(
m_sigma
(
i
)
>
pinvtoler
)
{
m_sigma_inv
(
i
,
i
)
=
1.0
/
m_sigma
(
i
);
}
if
(
m_sigma
(
i
)
>
pinvtoler
)
{
m_sigma_inv
(
i
,
i
)
=
1.0
/
m_sigma
(
i
);
}
}
pinvmat
=
(
svd
.
matrixV
()
*
m_sigma_inv
*
svd
.
matrixU
().
transpose
());
}
}
}
// namespace curves
#endif //_SPLINEMATH
include/curves/bernstein.h
View file @
a9efbf46
...
...
@@ -20,55 +20,70 @@
namespace
curves
{
/// \brief Computes a binomial coefficient.
/// \param n : an unsigned integer.
/// \param k : an unsigned integer.
/// \return \f$\binom{n}{k}f$
///
inline
unsigned
int
bin
(
const
unsigned
int
n
,
const
unsigned
int
k
)
{
if
(
k
>
n
)
throw
std
::
runtime_error
(
"binomial coefficient higher than degree"
);
if
(
k
==
0
)
return
1
;
if
(
k
>
n
/
2
)
return
bin
(
n
,
n
-
k
);
/// \brief Computes a binomial coefficient .
/// \param n : an unsigned integer.
/// \param k : an unsigned integer.
/// \return \f$\binom{n}{k}f$
///
inline
unsigned
int
bin
(
const
unsigned
int
n
,
const
unsigned
int
k
)
{
if
(
k
>
n
)
throw
std
::
runtime_error
(
"binomial coefficient higher than degree"
);
if
(
k
==
0
)
return
1
;
if
(
k
>
n
/
2
)
return
bin
(
n
,
n
-
k
);
return
n
*
bin
(
n
-
1
,
k
-
1
)
/
k
;
}
}
/// \class Bernstein.
/// \brief Computes a Bernstein polynome.
///
template
<
typename
Numeric
=
double
>
struct
Bern
{
Bern
(
const
unsigned
int
m
,
const
unsigned
int
i
)
:
m_minus_i
(
m
-
i
)
,
i_
(
i
)
,
bin_m_i_
(
bin
(
m
,
i
))
{}
/// \class Bernstein.
/// \brief Computes a Bernstein polynome.
///
template
<
typename
Numeric
=
double
>
struct
Bern
{
Bern
(){}
Bern
(
const
unsigned
int
m
,
const
unsigned
int
i
)
:
m_minus_i
(
m
-
i
),
i_
(
i
),
bin_m_i_
(
bin
(
m
,
i
))
{}
~
Bern
(){}
~
Bern
(){}
Numeric
operator
()(
const
Numeric
u
)
const
{
assert
(
u
>=
0.
&&
u
<=
1.
);
return
bin_m_i_
*
(
pow
(
u
,
i_
))
*
pow
((
1
-
u
),
m_minus_i
);
}
Numeric
operator
()(
const
Numeric
u
)
const
{
assert
(
u
>=
0.
&&
u
<=
1.
);
return
bin_m_i_
*
(
pow
(
u
,
i_
))
*
pow
((
1
-
u
),
m_minus_i
);
}
Numeric
m_minus_i
;
Numeric
i_
;
Numeric
bin_m_i_
;
};
/* Attributes */
Numeric
m_minus_i
;
Numeric
i_
;
Numeric
bin_m_i_
;
/* Attributes */
/// \brief Computes all Bernstein polynomes for a certain degree.
///
template
<
typename
Numeric
>
std
::
vector
<
Bern
<
Numeric
>
>
makeBernstein
(
const
unsigned
int
n
)
{
// Serialization of the class
friend
class
boost
::
serialization
::
access
;
template
<
class
Archive
>
void
serialize
(
Archive
&
ar
,
const
unsigned
int
version
){
if
(
version
)
{
// Do something depending on version ?
}
ar
&
boost
::
serialization
::
make_nvp
(
"m_minus_i"
,
m_minus_i
);
ar
&
boost
::
serialization
::
make_nvp
(
"i"
,
i_
);
ar
&
boost
::
serialization
::
make_nvp
(
"bin_m_i"
,
bin_m_i_
);
}
};
// End struct Bern
/// \brief Computes all Bernstein polynomes for a certain degree.
///
template
<
typename
Numeric
>
std
::
vector
<
Bern
<
Numeric
>
>
makeBernstein
(
const
unsigned
int
n
)
{
std
::
vector
<
Bern
<
Numeric
>
>
res
;
for
(
unsigned
int
i
=
0
;
i
<=
n
;
++
i
)
res
.
push_back
(
Bern
<
Numeric
>
(
n
,
i
));
{
res
.
push_back
(
Bern
<
Numeric
>
(
n
,
i
));
}
return
res
;
}
}
}
// namespace curves
#endif //_CLASS_BERNSTEIN
include/curves/bezier_curve.h
View file @
a9efbf46
...
...
@@ -23,15 +23,15 @@
namespace
curves
{
/// \class BezierCurve.
/// \brief Represents a Bezier curve of arbitrary dimension and order.
/// For degree lesser than 4, the evaluation is analitycal. Otherwise
/// the bernstein polynoms are used to evaluate the spline at a given location.
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
std
::
size_t
Dim
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>
>
struct
bezier_curve
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
/// \class BezierCurve.
/// \brief Represents a Bezier curve of arbitrary dimension and order.
/// For degree lesser than 4, the evaluation is analitycal. Otherwise
/// the bernstein polynoms are used to evaluate the spline at a given location.
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
std
::
size_t
Dim
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
1
>
>
struct
bezier_curve
:
public
curve_abc
<
Time
,
Numeric
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
...
...
@@ -40,125 +40,139 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef
typename
t_point_t
::
const_iterator
cit_point_t
;
typedef
bezier_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
bezier_curve_t
;
/* Constructors - destructors */
/* Constructors - destructors */
public:
/// \brief Constructor.
/// Given the first and last point of a control points set, create the bezier curve.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param T : upper bound of time which is between \f$[0;T]\f$ (default \f$[0;1]\f$).
/// \param mult_T : ... (default value is 1.0).
///
template
<
typename
In
>
bezier_curve
(
In
PointsBegin
,
In
PointsEnd
,
const
time_t
T_min
=
0.
,
const
time_t
T_max
=
1.
,
const
time_t
mult_T
=
1.
)
:
T_min_
(
T_min
)
,
T_max_
(
T_max
)
,
mult_T_
(
mult_T
)
,
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
))
,
degree_
(
size_
-
1
)
,
bernstein_
(
curves
::
makeBernstein
<
num_t
>
((
unsigned
int
)
degree_
))
{
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
bezier_curve
()
:
T_min_
(
0
),
T_max_
(
0
)
{}
/// \brief Constructor.
/// Given the first and last point of a control points set, create the bezier curve.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param T : upper bound of time which is between \f$[0;T]\f$ (default \f$[0;1]\f$).
/// \param mult_T : ... (default value is 1.0).
///
template
<
typename
In
>
bezier_curve
(
In
PointsBegin
,
In
PointsEnd
,
const
time_t
T_min
=
0.
,
const
time_t
T_max
=
1.
,
const
time_t
mult_T
=
1.
)
:
T_min_
(
T_min
),
T_max_
(
T_max
),
mult_T_
(
mult_T
),
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
)),
degree_
(
size_
-
1
),
bernstein_
(
curves
::
makeBernstein
<
num_t
>
((
unsigned
int
)
degree_
))
{
assert
(
bernstein_
.
size
()
==
size_
);
In
it
(
PointsBegin
);
if
(
Safe
&&
(
size_
<
1
||
T_max_
<=
T_min_
))
{
throw
std
::
invalid_argument
(
"can't create bezier min bound is higher than max bound"
);
// TODO
throw
std
::
invalid_argument
(
"can't create bezier min bound is higher than max bound"
);
// TODO
}
for
(;
it
!=
PointsEnd
;
++
it
)
{
control_points_
.
push_back
(
*
it
);
control_points_
.
push_back
(
*
it
);
}
}
/// \brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
/// to ensure that velocity and acceleration constraints are respected.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param constraints : constraints applying on start / end velocities and acceleration.
///
template
<
typename
In
>
bezier_curve
(
In
PointsBegin
,
In
PointsEnd
,
const
curve_constraints_t
&
constraints
,
const
time_t
T_min
=
0.
,
const
time_t
T_max
=
1.
,
const
time_t
mult_T
=
1.
)
:
T_min_
(
T_min
)
,
T_max_
(
T_max
)
,
mult_T_
(
mult_T
)
,
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
)
+
4
)
,
degree_
(
size_
-
1
)
,
bernstein_
(
curves
::
makeBernstein
<
num_t
>
((
unsigned
int
)
degree_
))
{
}
/// \brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
/// to ensure that velocity and acceleration constraints are respected.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param constraints : constraints applying on start / end velocities and acceleration.
///
template
<
typename
In
>
bezier_curve
(
In
PointsBegin
,
In
PointsEnd
,
const
curve_constraints_t
&
constraints
,
const
time_t
T_min
=
0.
,
const
time_t
T_max
=
1.
,
const
time_t
mult_T
=
1.
)
:
T_min_
(
T_min
)
,
T_max_
(
T_max
)
,
mult_T_
(
mult_T
)
,
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
)
+
4
)
,
degree_
(
size_
-
1
)
,
bernstein_
(
curves
::
makeBernstein
<
num_t
>
((
unsigned
int
)
degree_
))
{
if
(
Safe
&&
(
size_
<
1
||
T_max_
<=
T_min_
))
{
throw
std
::
invalid_argument
(
"can't create bezier min bound is higher than max bound"
);
throw
std
::
invalid_argument
(
"can't create bezier min bound is higher than max bound"
);
}
t_point_t
updatedList
=
add_constraints
<
In
>
(
PointsBegin
,
PointsEnd
,
constraints
);
for
(
cit_point_t
cit
=
updatedList
.
begin
();
cit
!=
updatedList
.
end
();
++
cit
)
{
control_points_
.
push_back
(
*
cit
);
control_points_
.
push_back
(
*
cit
);
}
}
///\brief Destructor
~
bezier_curve
()
{
}
bezier_curve
(
const
bezier_curve
&
other
)
:
T_min_
(
other
.
T_min_
),
T_max_
(
other
.
T_max_
),
mult_T_
(
other
.
mult_T_
),
size_
(
other
.
size_
),
degree_
(
other
.
degree_
),
bernstein_
(
other
.
bernstein_
),
control_points_
(
other
.
control_points_
)
{}
///\brief Destructor
~
bezier_curve
()
{
// NOTHING
}
private:
// bezier_curve(const bezier_curve&);
// bezier_curve& operator=(const bezier_curve&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the bezier curve at time t.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual
point_t
operator
()(
const
time_t
t
)
const
{
}
/*Operations*/
/// \brief Evaluation of the bezier curve at time t.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual
point_t
operator
()(
const
time_t
t
)
const
{
check_if_not_empty
();
if
(
Safe
&!
(
T_min_
<=
t
&&
t
<=
T_max_
))
{
throw
std
::
invalid_argument
(
"can't evaluate bezier curve, time t is out of range"
);
// TODO
throw
std
::
invalid_argument
(
"can't evaluate bezier curve, time t is out of range"
);
// TODO
}
if
(
size_
==
1
)
{
return
mult_T_
*
control_points_
[
0
];
}
else
}
else
{
return
evalHorner
(
t
);
}
}
/// \brief Compute the derived curve at order N.
/// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
/// \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
bezier_curve_t
compute_derivate
(
const
std
::
size_t
order
)
const
{
}
/// \brief Compute the derived curve at order N.
/// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
/// \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
bezier_curve_t
compute_derivate
(
const
std
::
size_t
order
)
const
{
check_if_not_empty
();
if
(
order
==
0
)
{
return
*
this
;
return
*
this
;
}
t_point_t
derived_wp
;
for
(
typename
t_point_t
::
const_iterator
pit
=
control_points_
.
begin
();
pit
!=
control_points_
.
end
()
-
1
;
++
pit
)
derived_wp
.
push_back
((
num_t
)
degree_
*
(
*
(
pit
+
1
)
-
(
*
pit
)));
{
derived_wp
.
push_back
((
num_t
)
degree_
*
(
*
(
pit
+
1
)
-
(
*
pit
)));
}
if
(
derived_wp
.
empty
())
derived_wp
.
push_back
(
point_t
::
Zero
(
Dim
));
{
derived_wp
.
push_back
(
point_t
::
Zero
(
Dim
));
}
bezier_curve_t
deriv
(
derived_wp
.
begin
(),
derived_wp
.
end
(),
T_min_
,
T_max_
,
mult_T_
*
(
1.
/
(
T_max_
-
T_min_
))
);
return
deriv
.
compute_derivate
(
order
-
1
);
}
/// \brief Compute the primitive of the curve at order N.
/// Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. <br>
/// At order \f$N=1\f$, the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
/// \param order : order of the primitive.
/// \return primitive at order N of x(t).
bezier_curve_t
compute_primitive
(
const
std
::
size_t
order
)
const
{
}
/// \brief Compute the primitive of the curve at order N.
/// Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. <br>
/// At order \f$N=1\f$, the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
/// \param order : order of the primitive.
/// \return primitive at order N of x(t).
bezier_curve_t
compute_primitive
(
const
std
::
size_t
order
)
const
{
check_if_not_empty
();
if
(
order
==
0
)
{
return
*
this
;
return
*
this
;
}
num_t
new_degree
=
(
num_t
)(
degree_
+
1
);
t_point_t
n_wp
;
...
...
@@ -168,61 +182,61 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
n_wp
.
push_back
(
current_sum
);
for
(
typename
t_point_t
::
const_iterator
pit
=
control_points_
.
begin
();
pit
!=
control_points_
.
end
();
++
pit
)
{
current_sum
+=
*
pit
;
n_wp
.
push_back
(
current_sum
/
new_degree
);
current_sum
+=
*
pit
;
n_wp
.
push_back
(
current_sum
/
new_degree
);
}
bezier_curve_t
integ
(
n_wp
.
begin
(),
n_wp
.
end
(),
T_min_
,
T_max_
,
mult_T_
*
(
T_max_
-
T_min_
));
return
integ
.
compute_primitive
(
order
-
1
);
}
/// \brief Evaluate the derivative order N of curve at time t.
/// If derivative is to be evaluated several times, it is
/// rather recommended to compute derived curve using compute_derivate.
/// \param order : order of derivative.
/// \param t : time when to evaluate the curve.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
///
virtual
point_t
derivate
(
const
time_t
t
,
const
std
::
size_t
order
)
const
{
}
/// \brief Evaluate the derivative order N of curve at time t.
/// If derivative is to be evaluated several times, it is
/// rather recommended to compute derived curve using compute_derivate.
/// \param order : order of derivative.
/// \param t : time when to evaluate the curve.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
///
virtual
point_t
derivate
(
const
time_t
t
,
const
std
::
size_t
order
)
const
{
bezier_curve_t
deriv
=
compute_derivate
(
order
);
return
deriv
(
t
);
}
/// \brief Evaluate all Bernstein polynomes for a certain degree.
/// A bezier curve with N control points is represented by : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$
/// with \f$ B_i^N(t) = \binom{N}{i}t^i (1-t)^{N-i} \f$.<br/>
/// Warning: the horner scheme is about 100 times faster than this method.<br>
/// This method will probably be removed in the future as the computation of bernstein polynomial is very costly.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t
evalBernstein
(
const
Numeric
t
)
const
{
}
/// \brief Evaluate all Bernstein polynomes for a certain degree.
/// A bezier curve with N control points is represented by : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$
/// with \f$ B_i^N(t) = \binom{N}{i}t^i (1-t)^{N-i} \f$.<br/>
/// Warning: the horner scheme is about 100 times faster than this method.<br>
/// This method will probably be removed in the future as the computation of bernstein polynomial is very costly.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t
evalBernstein
(
const
Numeric
t
)
const
{
const
Numeric
u
=
(
t
-
T_min_
)
/
(
T_max_
-
T_min_
);
point_t
res
=
point_t
::
Zero
(
Dim
);
typename
t_point_t
::
const_iterator
control_points_it
=
control_points_
.
begin
();
for
(
typename
std
::
vector
<
Bern
<
Numeric
>
>::
const_iterator
cit
=
bernstein_
.
begin
();
cit
!=
bernstein_
.
end
();
++
cit
,
++
control_points_it
)
cit
!=
bernstein_
.
end
();
++
cit
,
++
control_points_it
)
{
res
+=
cit
->
operator
()(
u
)
*
(
*
control_points_it
);
res
+=
cit
->
operator
()(
u
)
*
(
*
control_points_it
);
}
return
res
*
mult_T_
;
}
/// \brief Evaluate all Bernstein polynomes for a certain degree using Horner's scheme.
/// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.<br>
/// To evaluate the position on curve at time t,we can apply the Horner's scheme : <br>
/// \f$ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.<br>
/// Horner's scheme : for a polynom of degree N expressed by : <br>
/// \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
/// where \f$number of additions = N\f$ / f$number of multiplication = N!\f$<br>
/// Using Horner's method, the polynom is transformed into : <br>
/// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N additions and multiplications.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t
evalHorner
(
const
Numeric
t
)
const
{
}
/// \brief Evaluate all Bernstein polynomes for a certain degree using Horner's scheme.
/// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.<br>
/// To evaluate the position on curve at time t,we can apply the Horner's scheme : <br>
/// \f$ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.<br>
/// Horner's scheme : for a polynom of degree N expressed by : <br>
/// \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
/// where \f$number of additions = N\f$ / f$number of multiplication = N!\f$<br>
/// Using Horner's method, the polynom is transformed into : <br>
/// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N additions and multiplications.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t
evalHorner
(
const
Numeric
t
)
const
{
const
Numeric
u
=
(
t
-
T_min_
)
/
(
T_max_
-
T_min_
);
typename
t_point_t
::
const_iterator
control_points_it
=
control_points_
.
begin
();
Numeric
u_op
,
bc
,
tn
;
...
...
@@ -232,75 +246,80 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
point_t
tmp
=
(
*
control_points_it
)
*
u_op
;
++
control_points_it
;
for
(
unsigned
int
i
=
1
;
i
<
degree_
;
i
++
,
++
control_points_it
)
{
tn
=
tn
*
u
;
bc
=
bc
*
((
num_t
)(
degree_
-
i
+
1
))
/
i
;
tmp
=
(
tmp
+
tn
*
bc
*
(
*
control_points_it
))
*
u_op
;
tn
=
tn
*
u
;
bc
=
bc
*
((
num_t
)(
degree_
-
i
+
1
))
/
i
;
tmp
=
(
tmp
+
tn
*
bc
*
(
*
control_points_it
))
*
u_op
;
}
return
(
tmp
+
tn
*
u
*
(
*
control_points_it
))
*
mult_T_
;
}
const
t_point_t
&
waypoints
()
const
{
return
control_points_
;}
/// \brief Evaluate the curve value at time t using deCasteljau algorithm.
/// The algorithm will compute the \f$N-1\f$ centroids of parameters \f${t,1-t}\f$ of consecutive \f$N\f$ control points
/// of bezier curve, and perform it iteratively until getting one point in the list which will be the evaluation of bezier
/// curve at time \f$t\f$.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t
evalDeCasteljau
(
const
Numeric
t
)
const
{
}
const
t_point_t
&
waypoints
()
const
{
return
control_points_
;}
/// \brief Evaluate the curve value at time t using deCasteljau algorithm.
/// The algorithm will compute the \f$N-1\f$ centroids of parameters \f${t,1-t}\f$ of consecutive \f$N\f$ control points
/// of bezier curve, and perform it iteratively until getting one point in the list which will be the evaluation of bezier
/// curve at time \f$t\f$.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t
evalDeCasteljau
(
const
Numeric
t
)
const
{
// normalize time :
const
Numeric
u
=
(
t
-
T_min_
)
/
(
T_max_
-
T_min_
);
t_point_t
pts
=
deCasteljauReduction
(
waypoints
(),
u
);
while
(
pts
.
size
()
>
1
)
{
pts
=
deCasteljauReduction
(
pts
,
u
);
pts
=
deCasteljauReduction
(
pts
,
u
);
}
return
pts
[
0
]
*
mult_T_
;
}
}
t_point_t
deCasteljauReduction
(
const
Numeric
t
)
const
{
t_point_t
deCasteljauReduction
(
const
Numeric
t
)
const
{
const
Numeric
u
=
(
t
-
T_min_
)
/
(
T_max_
-
T_min_
);
return
deCasteljauReduction
(
waypoints
(),
u
);
}
/// \brief Compute de Casteljau's reduction of the given list of points at time t.
/// For the list \f$pts\f$ of N points, compute a new list of points of size N-1 :<br>
/// \f$<br>( pts[0]*(1-t)+pts[1], pts[1]*(1-t)+pts[2], ..., pts[0]*(N-2)+pts[N-1] )\f$<br>
/// with t the time when to evaluate bezier curve.<br>\ The new list contains centroid of
/// parameters \f${t,1-t}\f$ of consecutive points in the list.
/// \param pts : list of points.
/// \param u : NORMALIZED time when to evaluate the curve.
/// \return reduced list of point (size of pts - 1).
///
t_point_t
deCasteljauReduction
(
const
t_point_t
&
pts
,
const
Numeric
u
)
const
{
}
/// \brief Compute de Casteljau's reduction of the given list of points at time t.