Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
ndcurves
Commits
a976e461
Commit
a976e461
authored
Nov 14, 2017
by
t steve
Browse files
Merge branch 'packaging'
parents
d270a50a
950e634c
Changes
6
Hide whitespace changes
Inline
Side-by-side
include/spline/bezier_curve.h
View file @
a976e461
...
...
@@ -46,21 +46,60 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
template
<
typename
In
>
bezier_curve
(
In
PointsBegin
,
In
PointsEnd
,
const
time_t
minBound
=
0
,
const
time_t
maxBound
=
1
)
:
minBound_
(
minBound
)
,
maxBound_
(
maxBound
)
bezier_curve
(
In
PointsBegin
,
In
PointsEnd
)
:
T_
(
1.
)
,
mult_T_
(
1.
)
,
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
))
,
degree_
(
size_
-
1
)
,
bernstein_
(
spline
::
makeBernstein
<
num_t
>
(
degree_
))
{
assert
(
bernstein_
.
size
()
==
size_
);
In
it
(
PointsBegin
);
if
(
Safe
&&
(
size_
<
1
||
minBound
>=
maxBound
))
if
(
Safe
&&
(
size_
<
1
||
T_
<=
0.
))
throw
std
::
out_of_range
(
"can't create bezier min bound is higher than max bound"
);
// TODO
for
(;
it
!=
PointsEnd
;
++
it
)
pts_
.
push_back
(
*
it
);
}
///\brief Constructor
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
template
<
typename
In
>
bezier_curve
(
In
PointsBegin
,
In
PointsEnd
,
const
time_t
T
)
:
T_
(
T
)
,
mult_T_
(
1.
)
,
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
))
,
degree_
(
size_
-
1
)
,
bernstein_
(
spline
::
makeBernstein
<
num_t
>
(
degree_
))
{
assert
(
bernstein_
.
size
()
==
size_
);
In
it
(
PointsBegin
);
if
(
Safe
&&
(
size_
<
1
||
T_
<=
0.
))
throw
std
::
out_of_range
(
"can't create bezier min bound is higher than max bound"
);
// TODO
for
(;
it
!=
PointsEnd
;
++
it
)
pts_
.
push_back
(
*
it
);
}
///\brief Constructor
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
template
<
typename
In
>
bezier_curve
(
In
PointsBegin
,
In
PointsEnd
,
const
time_t
T
,
const
time_t
mult_T
)
:
T_
(
T
)
,
mult_T_
(
mult_T
)
,
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
))
,
degree_
(
size_
-
1
)
,
bernstein_
(
spline
::
makeBernstein
<
num_t
>
(
degree_
))
{
assert
(
bernstein_
.
size
()
==
size_
);
In
it
(
PointsBegin
);
if
(
Safe
&&
(
size_
<
1
||
T_
<=
0.
))
throw
std
::
out_of_range
(
"can't create bezier min bound is higher than max bound"
);
// TODO
for
(;
it
!=
PointsEnd
;
++
it
)
pts_
.
push_back
(
*
it
);
}
///\brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
...
...
@@ -69,14 +108,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\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
minBound
=
0
,
const
time_t
maxBound
=
1
)
:
minBound_
(
minBound
)
,
m
axBound_
(
maxBound
)
bezier_curve
(
In
PointsBegin
,
In
PointsEnd
,
const
curve_constraints_t
&
constraints
,
const
time_t
T
=
1
.
)
:
T_
(
T
)
,
m
ult_T_
(
1.
)
,
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
)
+
4
)
,
degree_
(
size_
-
1
)
,
bernstein_
(
spline
::
makeBernstein
<
num_t
>
(
degree_
))
{
if
(
Safe
&&
(
size_
<
1
||
minBound
>=
maxBound
))
if
(
Safe
&&
(
size_
<
1
||
T_
<=
0.
))
throw
std
::
out_of_range
(
"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
)
...
...
@@ -101,7 +140,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param return : the value x(t)
virtual
point_t
operator
()(
const
time_t
t
)
const
{
num_t
nT
=
(
t
-
minBound_
)
/
(
maxBound_
-
minBound_
)
;
num_t
nT
=
t
/
T_
;
if
(
Safe
&!
(
0
<=
nT
&&
nT
<=
1
))
{
throw
std
::
out_of_range
(
"can't evaluate bezier curve, out of range"
);
// TODO
...
...
@@ -112,22 +151,22 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
switch
(
size_
)
{
case
1
:
return
pts_
[
0
];
return
mult_T_
*
pts_
[
0
];
case
2
:
return
pts_
[
0
]
*
dt
+
nT
*
pts_
[
1
];
return
mult_T_
*
(
pts_
[
0
]
*
dt
+
nT
*
pts_
[
1
]
)
;
break
;
case
3
:
return
pts_
[
0
]
*
dt
*
dt
return
mult_T_
*
(
pts_
[
0
]
*
dt
*
dt
+
2
*
pts_
[
1
]
*
nT
*
dt
+
pts_
[
2
]
*
nT
*
nT
;
+
pts_
[
2
]
*
nT
*
nT
)
;
break
;
case
4
:
return
pts_
[
0
]
*
dt
*
dt
*
dt
return
mult_T_
*
(
pts_
[
0
]
*
dt
*
dt
*
dt
+
3
*
pts_
[
1
]
*
nT
*
dt
*
dt
+
3
*
pts_
[
2
]
*
nT
*
nT
*
dt
+
pts_
[
3
]
*
nT
*
nT
*
nT
;
+
pts_
[
3
]
*
nT
*
nT
*
nT
)
;
default
:
return
evalHorner
(
nT
);
return
mult_T_
*
evalHorner
(
nT
);
break
;
}
}
...
...
@@ -144,7 +183,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
derived_wp
.
push_back
(
degree_
*
(
*
(
pit
+
1
)
-
(
*
pit
)));
if
(
derived_wp
.
empty
())
derived_wp
.
push_back
(
point_t
::
Zero
());
bezier_curve_t
deriv
(
derived_wp
.
begin
(),
derived_wp
.
end
(),
minBound_
,
maxBound_
);
bezier_curve_t
deriv
(
derived_wp
.
begin
(),
derived_wp
.
end
(),
T_
,
mult_T_
*
(
1.
/
T_
)
);
return
deriv
.
compute_derivate
(
order
-
1
);
}
...
...
@@ -165,7 +204,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
current_sum
+=
*
pit
;
n_wp
.
push_back
(
current_sum
/
new_degree
);
}
bezier_curve_t
integ
(
n_wp
.
begin
(),
n_wp
.
end
(),
minBound_
,
maxBound
_
);
bezier_curve_t
integ
(
n_wp
.
begin
(),
n_wp
.
end
(),
T_
,
mult_T_
*
T
_
);
return
integ
.
compute_primitive
(
order
-
1
);
}
...
...
@@ -183,6 +222,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
/// \brief Evaluates all Bernstein polynomes for a certain degree
/// Warning: the horner scheme is about 100 times faster than this method.
/// This method will probably be removed in the future
///
point_t
evalBernstein
(
const
Numeric
u
)
const
{
...
...
@@ -246,15 +287,16 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Helpers*/
public:
virtual
time_t
min
()
const
{
return
minBound_
;}
virtual
time_t
max
()
const
{
return
maxBound
_
;}
virtual
time_t
min
()
const
{
return
0.
;}
virtual
time_t
max
()
const
{
return
T
_
;}
/*Helpers*/
public:
const
time_t
minBound_
,
maxBound_
;
const
std
::
size_t
size_
;
const
std
::
size_t
degree_
;
const
std
::
vector
<
Bern
<
Numeric
>
>
bernstein_
;
/*const*/
time_t
T_
;
/*const*/
time_t
mult_T_
;
/*const*/
std
::
size_t
size_
;
/*const*/
std
::
size_t
degree_
;
/*const*/
std
::
vector
<
Bern
<
Numeric
>
>
bernstein_
;
private:
t_point_t
pts_
;
...
...
include/spline/bezier_polynom_conversion.h
0 → 100644
View file @
a976e461
/**
* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
#ifndef _BEZIER_POLY_CONVERSION
#define _BEZIER_POLY_CONVERSION
#include
"curve_abc.h"
#include
"bernstein.h"
#include
"curve_constraint.h"
#include
"MathDefs.h"
#include
<vector>
#include
<stdexcept>
#include
<iostream>
namespace
spline
{
/// \brief Provides methods for converting a curve from a bernstein representation
/// to a polynom representation
///
///
///
///\brief Converts a Bezier curve to a polynom
///\param bezier: the Bezier curve to be converted from
///\return the equivalent polynom
template
<
typename
Bezier
,
typename
Polynom
>
Polynom
from_bezier
(
const
Bezier
&
curve
)
{
typedef
typename
Polynom
::
t_point_t
t_point_t
;
typedef
typename
Polynom
::
num_t
num_t
;
assert
(
curve
.
min
()
==
0.
);
assert
(
curve
.
max
()
==
1.
);
t_point_t
coefficients
;
Bezier
current
(
curve
);
coefficients
.
push_back
(
curve
(
0.
));
num_t
fact
=
1
;
for
(
std
::
size_t
i
=
1
;
i
<=
curve
.
degree_
;
++
i
)
{
current
=
current
.
compute_derivate
(
1
);
fact
*=
i
;
coefficients
.
push_back
(
current
(
0.
)
/
fact
);
}
return
Polynom
(
coefficients
,
curve
.
min
(),
curve
.
max
());
}
///\brief Converts a polynom to a Bezier curve
///\param polynom: the polynom to be converted from
///\return the equivalent Bezier curve
/*template<typename Bezier, typename Polynom>
Bezier from_polynom(const Polynom& polynom)
{
typedef Bezier::point_t point_t;
typedef Bezier::time_t time_t;
typedef Bezier::num_t num_t;
typedef Bezier::curve_constraints_t curve_constraints_t;
typedef Bezier::t_point_t t_point_t;
typedef Bezier::cit_point_t cit_point_t;
typedef Bezier::bezier_curve_t bezier_curve_t;
}*/
}
#endif //_BEZIER_POLY_CONVERSION
include/spline/polynom.h
View file @
a976e461
...
...
@@ -35,6 +35,7 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
struct
polynom
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
T_Point
t_point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
curve_abc_t
;
...
...
python/spline_python.cpp
View file @
a976e461
...
...
@@ -3,6 +3,9 @@
#include
"spline/exact_cubic.h"
#include
"spline/spline_deriv_constraint.h"
#include
"spline/curve_constraint.h"
#include
"spline/bezier_polynom_conversion.h"
#include
"spline/bernstein.h"
#include
<vector>
...
...
@@ -35,12 +38,15 @@ typedef polynom_t::coeff_t coeff_t;
typedef
std
::
pair
<
real
,
point_t
>
waypoint_t
;
typedef
std
::
vector
<
waypoint_t
,
Eigen
::
aligned_allocator
<
point_t
>
>
t_waypoint_t
;
typedef
spline
::
Bern
<
double
>
bernstein_t
;
typedef
spline
::
spline_deriv_constraint
<
real
,
real
,
3
,
true
,
point_t
,
t_point_t
>
spline_deriv_constraint_t
;
typedef
spline
::
curve_constraints
<
point_t
>
curve_constraints_t
;
typedef
spline
::
curve_constraints
<
point6_t
>
curve_constraints6_t
;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
bernstein_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
bezier_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
bezier6_t
)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
polynom_t
)
...
...
@@ -61,17 +67,17 @@ T_Point vectorFromEigenArray(const PointList& array)
}
template
<
typename
Bezier
,
typename
PointList
,
typename
T_Point
>
Bezier
*
wrapBezierConstructorTemplate
(
const
PointList
&
array
,
const
real
lb
=
0.
,
const
real
ub
=
1.
)
Bezier
*
wrapBezierConstructorTemplate
(
const
PointList
&
array
,
const
real
ub
=
1.
)
{
T_Point
asVector
=
vectorFromEigenArray
<
PointList
,
T_Point
>
(
array
);
return
new
Bezier
(
asVector
.
begin
(),
asVector
.
end
(),
lb
,
ub
);
return
new
Bezier
(
asVector
.
begin
(),
asVector
.
end
(),
ub
);
}
template
<
typename
Bezier
,
typename
PointList
,
typename
T_Point
,
typename
CurveConstraints
>
Bezier
*
wrapBezierConstructorConstraintsTemplate
(
const
PointList
&
array
,
const
CurveConstraints
&
constraints
,
const
real
lb
=
0.
,
const
real
ub
=
1.
)
Bezier
*
wrapBezierConstructorConstraintsTemplate
(
const
PointList
&
array
,
const
CurveConstraints
&
constraints
,
const
real
ub
=
1.
)
{
T_Point
asVector
=
vectorFromEigenArray
<
PointList
,
T_Point
>
(
array
);
return
new
Bezier
(
asVector
.
begin
(),
asVector
.
end
(),
constraints
,
lb
,
ub
);
return
new
Bezier
(
asVector
.
begin
(),
asVector
.
end
(),
constraints
,
ub
);
}
/*3D constructors */
...
...
@@ -79,17 +85,17 @@ bezier_t* wrapBezierConstructor(const point_list_t& array)
{
return
wrapBezierConstructorTemplate
<
bezier_t
,
point_list_t
,
t_point_t
>
(
array
)
;
}
bezier_t
*
wrapBezierConstructorBounds
(
const
point_list_t
&
array
,
const
real
lb
,
const
real
ub
)
bezier_t
*
wrapBezierConstructorBounds
(
const
point_list_t
&
array
,
const
real
ub
)
{
return
wrapBezierConstructorTemplate
<
bezier_t
,
point_list_t
,
t_point_t
>
(
array
,
lb
,
ub
)
;
return
wrapBezierConstructorTemplate
<
bezier_t
,
point_list_t
,
t_point_t
>
(
array
,
ub
)
;
}
bezier_t
*
wrapBezierConstructorConstraints
(
const
point_list_t
&
array
,
const
curve_constraints_t
&
constraints
)
{
return
wrapBezierConstructorConstraintsTemplate
<
bezier_t
,
point_list_t
,
t_point_t
,
curve_constraints_t
>
(
array
,
constraints
)
;
}
bezier_t
*
wrapBezierConstructorBoundsConstraints
(
const
point_list_t
&
array
,
const
curve_constraints_t
&
constraints
,
const
real
lb
,
const
real
ub
)
bezier_t
*
wrapBezierConstructorBoundsConstraints
(
const
point_list_t
&
array
,
const
curve_constraints_t
&
constraints
,
const
real
ub
)
{
return
wrapBezierConstructorConstraintsTemplate
<
bezier_t
,
point_list_t
,
t_point_t
,
curve_constraints_t
>
(
array
,
constraints
,
lb
,
ub
)
;
return
wrapBezierConstructorConstraintsTemplate
<
bezier_t
,
point_list_t
,
t_point_t
,
curve_constraints_t
>
(
array
,
constraints
,
ub
)
;
}
/*END 3D constructors */
/*6D constructors */
...
...
@@ -97,17 +103,17 @@ bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
{
return
wrapBezierConstructorTemplate
<
bezier6_t
,
point_list6_t
,
t_point6_t
>
(
array
)
;
}
bezier6_t
*
wrapBezierConstructorBounds6
(
const
point_list6_t
&
array
,
const
real
lb
,
const
real
ub
)
bezier6_t
*
wrapBezierConstructorBounds6
(
const
point_list6_t
&
array
,
const
real
ub
)
{
return
wrapBezierConstructorTemplate
<
bezier6_t
,
point_list6_t
,
t_point6_t
>
(
array
,
lb
,
ub
)
;
return
wrapBezierConstructorTemplate
<
bezier6_t
,
point_list6_t
,
t_point6_t
>
(
array
,
ub
)
;
}
bezier6_t
*
wrapBezierConstructor6Constraints
(
const
point_list6_t
&
array
,
const
curve_constraints6_t
&
constraints
)
{
return
wrapBezierConstructorConstraintsTemplate
<
bezier6_t
,
point_list6_t
,
t_point6_t
,
curve_constraints6_t
>
(
array
,
constraints
)
;
}
bezier6_t
*
wrapBezierConstructorBounds6Constraints
(
const
point_list6_t
&
array
,
const
curve_constraints6_t
&
constraints
,
const
real
lb
,
const
real
ub
)
bezier6_t
*
wrapBezierConstructorBounds6Constraints
(
const
point_list6_t
&
array
,
const
curve_constraints6_t
&
constraints
,
const
real
ub
)
{
return
wrapBezierConstructorConstraintsTemplate
<
bezier6_t
,
point_list6_t
,
t_point6_t
,
curve_constraints6_t
>
(
array
,
constraints
,
lb
,
ub
)
;
return
wrapBezierConstructorConstraintsTemplate
<
bezier6_t
,
point_list6_t
,
t_point6_t
,
curve_constraints6_t
>
(
array
,
constraints
,
ub
)
;
}
/*END 6D constructors */
...
...
@@ -300,6 +306,17 @@ BOOST_PYTHON_MODULE(spline)
;
/** END spline_deriv_constraints**/
/** BEGIN bernstein polynom**/
class_
<
bernstein_t
>
(
"bernstein"
,
init
<
const
unsigned
int
,
const
unsigned
int
>
())
.
def
(
"__call__"
,
&
bernstein_t
::
operator
())
;
/** END bernstein polynom**/
/** BEGIN Bezier to polynom conversion**/
def
(
"from_bezier"
,
from_bezier
<
bezier_t
,
polynom_t
>
);
/** END Bezier to polynom conversion**/
}
...
...
python/test/test.py
View file @
a976e461
from
spline
import
bezier
,
bezier6
,
polynom
,
exact_cubic
,
curve_constraints
,
spline_deriv_constraint
from
spline
import
bezier
,
bezier6
,
polynom
,
exact_cubic
,
curve_constraints
,
spline_deriv_constraint
,
from_bezier
from
numpy
import
matrix
from
numpy.linalg
import
norm
__EPS
=
1e-6
waypoints
=
matrix
([[
1.
,
2.
,
3.
],[
4.
,
5.
,
6.
]]).
transpose
()
waypoints6
=
matrix
([[
1.
,
2.
,
3.
,
7.
,
5.
,
5.
],[
4.
,
5.
,
6.
,
4.
,
5.
,
6.
]]).
transpose
()
time_waypoints
=
matrix
([
0.
,
1.
])
#testing bezier curve
a
=
bezier6
(
waypoints6
)
a
=
bezier
(
waypoints
,
-
1.
,
3.
)
a
=
bezier
(
waypoints
,
3.
)
assert
(
a
.
degree
==
a
.
nbWaypoints
-
1
)
a
.
min
()
...
...
@@ -21,6 +23,7 @@ a = a.compute_derivate(100)
prim
=
a
.
compute_primitive
(
1
)
for
i
in
range
(
10
):
t
=
float
(
i
)
/
10.
assert
(
a
(
t
)
==
prim
.
derivate
(
t
,
1
)).
all
()
...
...
@@ -32,6 +35,20 @@ for i in range(10):
assert
(
a
(
t
)
==
prim
.
derivate
(
t
,
2
)).
all
()
assert
(
prim
(
0
)
==
matrix
([
0.
,
0.
,
0.
])).
all
()
waypoints
=
matrix
([[
1.
,
2.
,
3.
],[
4.
,
5.
,
6.
],[
4.
,
5.
,
6.
],[
4.
,
5.
,
6.
],[
4.
,
5.
,
6.
]]).
transpose
()
a0
=
bezier
(
waypoints
)
a1
=
bezier
(
waypoints
,
3.
)
prim0
=
a0
.
compute_primitive
(
1
)
prim1
=
a1
.
compute_primitive
(
1
)
for
i
in
range
(
10
):
t
=
float
(
i
)
/
10.
assert
norm
(
a0
(
t
)
-
a1
(
3
*
t
))
<
__EPS
assert
norm
(
a0
.
derivate
(
t
,
1
)
-
a1
.
derivate
(
3
*
t
,
1
)
*
3.
)
<
__EPS
assert
norm
(
a0
.
derivate
(
t
,
2
)
-
a1
.
derivate
(
3
*
t
,
2
)
*
9.
)
<
__EPS
assert
norm
(
prim0
(
t
)
-
prim1
(
t
*
3
)
/
3.
)
<
__EPS
assert
(
prim
(
0
)
==
matrix
([
0.
,
0.
,
0.
])).
all
()
#testing bezier with constraints
c
=
curve_constraints
();
...
...
@@ -40,7 +57,7 @@ c.end_vel = matrix([0.,1.,1.]);
c
.
init_acc
=
matrix
([
0.
,
1.
,
-
1.
]);
c
.
end_acc
=
matrix
([
0.
,
100.
,
1.
]);
waypoints
=
matrix
([[
1.
,
2.
,
3.
],[
4.
,
5.
,
6.
]]).
transpose
()
a
=
bezier
(
waypoints
,
c
)
assert
norm
(
a
.
derivate
(
0
,
1
)
-
c
.
init_vel
)
<
1e-10
assert
norm
(
a
.
derivate
(
1
,
2
)
-
c
.
end_acc
)
<
1e-10
...
...
@@ -78,3 +95,9 @@ c.end_acc = matrix([0.,1.,1.]);
a
=
spline_deriv_constraint
(
waypoints
,
time_waypoints
)
a
=
spline_deriv_constraint
(
waypoints
,
time_waypoints
,
c
)
#converting bezier to polynom
a
=
bezier
(
waypoints
)
a_pol
=
from_bezier
(
a
)
assert
norm
(
a
(
0.3
)
-
a_pol
(
0.3
))
<
__EPS
src/tests/spline_test/Main.cpp
View file @
a976e461
...
...
@@ -5,6 +5,7 @@
#include
"spline/spline_deriv_constraint.h"
#include
"spline/helpers/effector_spline.h"
#include
"spline/helpers/effector_spline_rotation.h"
#include
"spline/bezier_polynom_conversion.h"
#include
<string>
#include
<iostream>
...
...
@@ -174,9 +175,7 @@ void BezierCurveTest(bool& error)
//4d curve
params
.
push_back
(
d
);
bezier_curve_t
cf4
(
params
.
begin
(),
params
.
end
(),
0.4
,
2
);
res1
=
cf4
(
0.4
);
ComparePoints
(
a
,
res1
,
errMsg
+
"3(0) "
,
error
);
bezier_curve_t
cf4
(
params
.
begin
(),
params
.
end
(),
2
);
res1
=
cf4
(
2
);
ComparePoints
(
d
,
res1
,
errMsg
+
"3(1) "
,
error
);
...
...
@@ -344,6 +343,32 @@ void BezierDerivativeCurveTest(bool& error)
ComparePoints
(
point_t
::
Zero
(),
cf3
.
derivate
(
0.
,
100
),
errMsg
,
error
);
}
void
BezierDerivativeCurveTimeReparametrizationTest
(
bool
&
error
)
{
std
::
string
errMsg
(
"In test BezierDerivativeCurveTimeReparametrizationTest ; unexpected result for x "
);
point_t
a
(
1
,
2
,
3
);
point_t
b
(
2
,
3
,
4
);
point_t
c
(
3
,
4
,
5
);
point_t
d
(
3
,
4
,
5
);
point_t
e
(
3
,
4
,
5
);
point_t
f
(
3
,
4
,
5
);
std
::
vector
<
point_t
>
params
;
params
.
push_back
(
a
);
params
.
push_back
(
b
);
params
.
push_back
(
c
);
params
.
push_back
(
d
);
params
.
push_back
(
e
);
params
.
push_back
(
f
);
double
T
=
2.
;
bezier_curve_t
cf
(
params
.
begin
(),
params
.
end
());
bezier_curve_t
cfT
(
params
.
begin
(),
params
.
end
(),
T
);
ComparePoints
(
cf
(
0.5
),
cfT
(
1
),
errMsg
,
error
);
ComparePoints
(
cf
.
derivate
(
0.5
,
1
),
cfT
.
derivate
(
1
,
1
)
*
T
,
errMsg
,
error
);
ComparePoints
(
cf
.
derivate
(
0.5
,
2
),
cfT
.
derivate
(
1
,
2
)
*
T
*
T
,
errMsg
,
error
);
}
void
BezierDerivativeCurveConstraintTest
(
bool
&
error
)
{
std
::
string
errMsg
(
"In test BezierDerivativeCurveConstraintTest ; unexpected result for x "
);
...
...
@@ -377,6 +402,39 @@ void BezierDerivativeCurveConstraintTest(bool& error)
}
void
BezierToPolynomConversionTest
(
bool
&
error
)
{
std
::
string
errMsg
(
"In test BezierToPolynomConversionTest ; unexpected result for x "
);
point_t
a
(
1
,
2
,
3
);
point_t
b
(
2
,
3
,
4
);
point_t
c
(
3
,
4
,
5
);
point_t
d
(
3
,
6
,
7
);
point_t
e
(
3
,
61
,
7
);
point_t
f
(
3
,
56
,
7
);
point_t
g
(
3
,
36
,
7
);
point_t
h
(
43
,
6
,
7
);
point_t
i
(
3
,
6
,
77
);
std
::
vector
<
point_t
>
params
;
params
.
push_back
(
a
);
params
.
push_back
(
b
);
params
.
push_back
(
c
);
params
.
push_back
(
d
);
params
.
push_back
(
e
);
params
.
push_back
(
f
);
params
.
push_back
(
g
);
params
.
push_back
(
h
);
params
.
push_back
(
i
);
bezier_curve_t
cf
(
params
.
begin
(),
params
.
end
());
polynom_t
pol
=
from_bezier
<
bezier_curve_t
,
polynom_t
>
(
cf
);
for
(
double
i
=
0.
;
i
<
1.
;
i
+=
0.01
)
{
ComparePoints
(
cf
(
i
),
pol
(
i
),
errMsg
,
error
,
true
);
ComparePoints
(
cf
(
i
),
pol
(
i
),
errMsg
,
error
,
false
);
}
}
/*Exact Cubic Function tests*/
void
ExactCubicNoErrorTest
(
bool
&
error
)
{
...
...
@@ -714,7 +772,7 @@ int main(int /*argc*/, char** /*argv[]*/)
{
std
::
cout
<<
"performing tests...
\n
"
;
bool
error
=
false
;
/*
CubicFunctionTest(error);
CubicFunctionTest
(
error
);
ExactCubicNoErrorTest
(
error
);
ExactCubicPointsCrossedTest
(
error
);
// checks that given wayPoints are crossed
ExactCubicTwoPointsTest
(
error
);
...
...
@@ -727,8 +785,10 @@ int main(int /*argc*/, char** /*argv[]*/)
EffectorSplineRotationWayPointRotationTest
(
error
);
BezierCurveTest
(
error
);
BezierDerivativeCurveTest
(
error
);
BezierDerivativeCurveConstraintTest(error);
*/
BezierDerivativeCurveConstraintTest
(
error
);
BezierCurveTestCompareHornerAndBernstein
(
error
);
BezierDerivativeCurveTimeReparametrizationTest
(
error
);
BezierToPolynomConversionTest
(
error
);
if
(
error
)
{
std
::
cout
<<
"There were some errors
\n
"
;
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment