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
Jason Chemin
curves
Commits
3005c737
Commit
3005c737
authored
Mar 14, 2017
by
Steve Tonneau
Browse files
added derivations for bezier curves
parent
4d962ec0
Changes
5
Hide whitespace changes
Inline
Side-by-side
include/spline/bezier_curve.h
View file @
3005c737
...
...
@@ -27,12 +27,13 @@ namespace spline
///
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
>
struct
bezier_curve
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
std
::
vector
<
point_t
,
Eigen
::
aligned_allocator
<
point_t
>
>
t_point_t
;
typedef
bezier_curve
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
bezier_curve_t
;
/* Constructors - destructors */
public:
...
...
@@ -48,7 +49,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
assert
(
bernstein_
.
size
()
==
size_
);
In
it
(
PointsBegin
);
if
(
Safe
&&
(
size_
<
=
1
||
minBound
=
=
maxBound
))
if
(
Safe
&&
(
size_
<
1
||
minBound
>
=
maxBound
))
{
throw
std
::
out_of_range
(
"can't create bezier min bound is higher than max bound"
);
// TODO
}
...
...
@@ -85,7 +86,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
num_t
dt
=
(
1
-
nT
);
switch
(
size_
)
{
{
case
1
:
return
pts_
[
0
];
case
2
:
return
pts_
[
0
]
*
dt
+
nT
*
pts_
[
1
];
break
;
...
...
@@ -106,6 +109,33 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
}
/// \brief Computes the derivative curve at order N.
/// \param order : order of the derivative
/// \param return : the value x(t)
bezier_curve_t
compute_derivate
(
const
std
::
size_t
order
)
const
{
if
(
order
==
0
)
return
*
this
;
t_point_t
derived_wp
;
for
(
typename
t_point_t
::
const_iterator
pit
=
pts_
.
begin
();
pit
!=
pts_
.
end
()
-
1
;
++
pit
)
derived_wp
.
push_back
(
*
(
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_
);
return
deriv
.
compute_derivate
(
order
-
1
);
}
/// \brief Evaluates the derivative at order N of the curve.
/// If the derivative is to be evaluated several times, it is
/// rather recommended to compute the derivative curve using compute_derivate
/// \param order : order of the derivative
/// \param t : the time when to evaluate the spine
/// \param return : the value x(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 Evaluates all Bernstein polynomes for a certain degree
///
...
...
include/spline/curve_abc.h
View file @
3005c737
...
...
@@ -43,6 +43,13 @@ struct curve_abc : std::unary_function<Time, Point>
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual
point_t
operator
()(
const
time_t
t
)
const
=
0
;
/// \brief Evaluation of the derivative spline at time t.
/// \param t : the time when to evaluate the spline
/// \param order : order of the derivative
/// \param return : the value x(t)
virtual
point_t
derivate
(
const
time_t
t
,
const
std
::
size_t
order
)
const
=
0
;
/*Operations*/
/*Helpers*/
...
...
include/spline/exact_cubic.h
View file @
3005c737
...
...
@@ -172,8 +172,9 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \brief Evaluation of the derivative spline at time t.
/// \param t : the time when to evaluate the spline
/// \param order : order of the derivative
/// \param return : the value x(t)
virtual
point_t
derivate
(
time_t
t
,
std
::
size_t
order
)
const
virtual
point_t
derivate
(
const
time_t
t
,
const
std
::
size_t
order
)
const
{
if
(
Safe
&&
(
t
<
subSplines_
.
front
().
min
()
||
t
>
subSplines_
.
back
().
max
())){
throw
std
::
out_of_range
(
"TODO"
);}
for
(
cit_spline_t
it
=
subSplines_
.
begin
();
it
!=
subSplines_
.
end
();
++
it
)
...
...
include/spline/spline_curve.h
View file @
3005c737
...
...
@@ -130,9 +130,10 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \brief Evaluation of the derivative spline at time t.
/// \param t : the time when to evaluate the spine
/// \param t : the time when to evaluate the spline
/// \param order : order of the derivative
/// \param return : the value x(t)
point_t
derivate
(
time_t
t
,
std
::
size_t
order
)
const
virtual
point_t
derivate
(
const
time_t
t
,
const
std
::
size_t
order
)
const
{
if
((
t
<
t_min_
||
t
>
t_max_
)
&&
Safe
){
throw
std
::
out_of_range
(
"TODO"
);}
time_t
const
dt
(
t
-
t_min_
);
...
...
src/tests/spline_test/Main.cpp
View file @
3005c737
...
...
@@ -55,9 +55,9 @@ ostream& operator<<(ostream& os, const point_t& pt)
return
os
;
}
void
ComparePoints
(
const
Eigen
::
VectorXd
&
pt1
,
const
Eigen
::
VectorXd
&
pt2
,
const
std
::
string
&
errmsg
,
bool
&
error
)
void
ComparePoints
(
const
Eigen
::
VectorXd
&
pt1
,
const
Eigen
::
VectorXd
&
pt2
,
const
std
::
string
&
errmsg
,
bool
&
error
,
bool
notequal
=
false
)
{
if
((
pt1
-
pt2
).
norm
()
>
margin
)
if
((
pt1
-
pt2
).
norm
()
>
margin
&&
!
notequal
)
{
error
=
true
;
std
::
cout
<<
errmsg
<<
pt1
<<
" ; "
<<
pt2
<<
std
::
endl
;
...
...
@@ -228,6 +228,27 @@ void BezierCurveTest(bool& error)
}
}
void
BezierDerivativeCurveTest
(
bool
&
error
)
{
std
::
string
errMsg
(
"In test BezierDerivativeCurveTest ; unexpected result for x "
);
point_t
a
(
1
,
2
,
3
);
point_t
b
(
2
,
3
,
4
);
point_t
c
(
3
,
4
,
5
);
std
::
vector
<
point_t
>
params
;
params
.
push_back
(
a
);
params
.
push_back
(
b
);
params
.
push_back
(
c
);
bezier_curve_t
cf3
(
params
.
begin
(),
params
.
end
());
ComparePoints
(
cf3
(
0
),
cf3
.
derivate
(
0.
,
0
),
errMsg
,
error
);
ComparePoints
(
cf3
(
0
),
cf3
.
derivate
(
0.
,
1
),
errMsg
,
error
,
true
);
ComparePoints
(
point_t
::
Zero
(),
cf3
.
derivate
(
0.
,
100
),
errMsg
,
error
);
}
/*Exact Cubic Function tests*/
void
ExactCubicNoErrorTest
(
bool
&
error
)
{
...
...
@@ -577,6 +598,7 @@ int main(int /*argc*/, char** /*argv[]*/)
TestReparametrization
(
error
);
EffectorSplineRotationWayPointRotationTest
(
error
);
BezierCurveTest
(
error
);
BezierDerivativeCurveTest
(
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