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
d1757d2a
Commit
d1757d2a
authored
Nov 24, 2016
by
Steve Tonneau
Committed by
Steve Tonneau
Nov 25, 2016
Browse files
implemented generic splines. no error
parent
3b8434c8
Changes
3
Hide whitespace changes
Inline
Side-by-side
include/spline/cubic_function.h
View file @
d1757d2a
...
...
@@ -31,59 +31,59 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>
>
struct
cubic_function
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
Point
point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
/* Constructors - destructors */
public:
///\brief Constructor
cubic_function
(
point_t
const
&
a
,
point_t
const
&
b
,
point_t
const
&
c
,
point_t
const
&
d
,
time_t
min
,
time_t
max
)
:
a_
(
a
),
b_
(
b
),
c_
(
c
),
d_
(
d
),
t_min_
(
min
),
t_max_
(
max
)
:
a_
(
a
),
b_
(
b
),
c_
(
c
),
d_
(
d
),
t_min_
(
min
),
t_max_
(
max
)
{
if
(
t_min_
>
t_max_
&&
Safe
)
{
std
::
out_of_range
(
"TODO"
);
}
}
if
(
t_min_
>
t_max_
&&
Safe
)
{
std
::
out_of_range
(
"TODO"
);
}
}
///\brief Destructor
~
cubic_function
()
{
// NOTHING
}
///\brief Destructor
~
cubic_function
()
{
// NOTHING
}
private:
cubic_function
(
const
cubic_function
&
);
cubic_function
&
operator
=
(
const
cubic_function
&
);
private:
cubic_function
(
const
cubic_function
&
);
cubic_function
&
operator
=
(
const
cubic_function
&
);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual
point_t
operator
()(
time_t
t
)
const
{
if
((
t
<
t_min_
||
t
>
t_max_
)
&&
Safe
){
throw
std
::
out_of_range
(
"TODO"
);}
time_t
const
dt
(
t
-
t_min_
);
return
a_
+
b_
*
dt
+
c_
*
dt
*
dt
+
d_
*
dt
*
dt
*
dt
;
}
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual
point_t
operator
()(
time_t
t
)
const
{
if
((
t
<
t_min_
||
t
>
t_max_
)
&&
Safe
){
throw
std
::
out_of_range
(
"TODO"
);}
time_t
const
dt
(
t
-
t_min_
);
return
a_
+
b_
*
dt
+
c_
*
dt
*
dt
+
d_
*
dt
*
dt
*
dt
;
}
/*Operations*/
/*Helpers*/
public:
/// \brief Returns the minimum time for wich curve is defined
num_t
virtual
min
()
const
{
return
t_min_
;}
/// \brief Returns the maximum time for wich curve is defined
num_t
virtual
max
()
const
{
return
t_max_
;}
public:
/// \brief Returns the minimum time for wich curve is defined
num_t
virtual
min
()
const
{
return
t_min_
;}
/// \brief Returns the maximum time for wich curve is defined
num_t
virtual
max
()
const
{
return
t_max_
;}
/*Helpers*/
/*Attributes*/
public:
const
point_t
a_
,
b_
,
c_
,
d_
;
const
time_t
t_min_
,
t_max_
;
public:
const
point_t
a_
,
b_
,
c_
,
d_
;
const
time_t
t_min_
,
t_max_
;
/*Attributes*/
};
//class CubicFunction
};
//class CubicFunction
}
#endif //_STRUCT_CUBICFUNCTION
include/spline/spline_curve.h
0 → 100644
View file @
d1757d2a
/**
* \file cubic_function.h
* \brief Definition of a cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the CubicFunction struct.
* It allows the creation and evaluation of natural
* smooth cubic splines of arbitrary dimension
*/
#ifndef _STRUCT_SPLINE
#define _STRUCT_SPLINE
#include
"MathDefs.h"
#include
"curve_abc.h"
#include
<iostream>
#include
<algorithm>
#include
<functional>
#include
<stdexcept>
namespace
spline
{
/// \class spline_curve
/// \brief Represents a spline curve of arbitrary order defined on the interval
/// [tBegin, tEnd]. It follows the equation
/// x(t) = a + b(t - t_min_) + ... + d(t - t_min_)^N, where N is the order
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
std
::
size_t
Dim
=
3
,
std
::
size_t
Order
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>,
typename
T_Point
=
std
::
vector
<
Point
,
Eigen
::
aligned_allocator
<
Point
>
>
>
struct
spline_curve
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
T_Point
t_point_t
;
typedef
typename
t_point_t
::
const_iterator
cit_point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
/* Constructors - destructors */
public:
///\brief Constructor
///\param coefficients : a container containing all coefficients of the spline, starting
/// with the zero order coefficient, up to the highest order
///\param min: LOWER bound on interval definition of the spline
///\param max: UPPER bound on interval definition of the spline
spline_curve
(
const
T_Point
&
coefficients
,
time_t
min
,
time_t
max
)
:
coefficients_
(
coefficients
),
t_min_
(
min
),
t_max_
(
max
),
dim_
(
Dim
),
order_
(
Order
)
{
if
(
t_min_
>
t_max_
&&
Safe
)
{
std
::
out_of_range
(
"TODO"
);
}
}
///\brief Constructor
///\param zeroOrderCoefficient : an iterator pointing to the first element of a structure containing the coefficients
/// it corresponds to the zero degree coefficient
///\param out : an iterator pointing to the last element of a structure ofcoefficients
///\param min: LOWER bound on interval definition of the spline
///\param max: UPPER bound on interval definition of the spline
template
<
typename
In
>
spline_curve
(
In
zeroOrderCoefficient
,
In
out
,
time_t
min
,
time_t
max
)
:
coefficients_
(
init_coeffs
(
zeroOrderCoefficient
,
out
)),
t_min_
(
min
),
t_max_
(
max
),
dim_
(
Dim
),
order_
(
Order
)
{
if
(
t_min_
>
t_max_
&&
Safe
)
{
std
::
out_of_range
(
"TODO"
);
}
}
///\brief Destructor
~
spline_curve
()
{
// NOTHING
}
private:
spline_curve
(
const
spline_curve
&
);
spline_curve
&
operator
=
(
const
spline_curve
&
);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual
point_t
operator
()(
time_t
t
)
const
{
if
((
t
<
t_min_
||
t
>
t_max_
)
&&
Safe
){
throw
std
::
out_of_range
(
"TODO"
);}
time_t
const
dt
(
t
-
t_min_
);
time_t
cdt
(
dt
);
cit_point_t
cit
=
coefficients_
.
begin
();
point_t
currentPoint_
=
*
cit
;
++
cit
;
for
(;
cit
!=
coefficients_
.
end
();
++
cit
,
cdt
*=
dt
)
currentPoint_
+=
cdt
*
(
*
cit
);
return
currentPoint_
;
}
/*Operations*/
/*Helpers*/
public:
/// \brief Returns the minimum time for wich curve is defined
num_t
virtual
min
()
const
{
return
t_min_
;}
/// \brief Returns the maximum time for wich curve is defined
num_t
virtual
max
()
const
{
return
t_max_
;}
/*Helpers*/
/*Attributes*/
public:
const
t_point_t
coefficients_
;
const
time_t
t_min_
,
t_max_
;
const
std
::
size_t
dim_
;
const
std
::
size_t
order_
;
/*Attributes*/
private:
template
<
typename
In
>
t_point_t
init_coeffs
(
In
zeroOrderCoefficient
,
In
highestOrderCoefficient
)
{
t_point_t
res
(
Order
+
1
);
std
::
copy
(
zeroOrderCoefficient
,
highestOrderCoefficient
,
res
.
begin
());
return
res
;
}
};
//class spline_curve
}
#endif //_STRUCT_SPLINE
src/tests/spline_test/Main.cpp
View file @
d1757d2a
#include
"spline/cubic_function.h"
#include
"spline/exact_cubic.h"
#include
"spline/bezier_curve.h"
#include
"spline/spline_curve.h"
#include
<string>
#include
<iostream>
...
...
@@ -12,7 +12,8 @@ using namespace std;
namespace
spline
{
typedef
Eigen
::
Vector3d
point_t
;
typedef
cubic_function
<
double
,
double
,
3
,
true
,
point_t
>
cubic_function_t
;
typedef
std
::
vector
<
point_t
,
Eigen
::
aligned_allocator
<
point_t
>
>
t_point_t
;
typedef
spline_curve
<
double
,
double
,
3
,
3
,
true
,
point_t
,
t_point_t
>
cubic_function_t
;
typedef
exact_cubic
<
double
,
double
,
3
,
true
,
point_t
>
exact_cubic_t
;
typedef
bezier_curve
<
double
,
double
,
3
,
true
,
point_t
>
bezier_curve_t
;
typedef
std
::
pair
<
double
,
point_t
>
Waypoint
;
...
...
@@ -20,7 +21,7 @@ typedef std::vector<Waypoint> T_Waypoint;
typedef
Eigen
::
Matrix
<
double
,
1
,
1
>
point_one
;
typedef
cubic_function
<
double
,
double
,
1
,
true
,
point_one
>
cubic_function_one
;
typedef
spline_curve
<
double
,
double
,
1
,
3
,
true
,
point_one
>
cubic_function_one
;
typedef
exact_cubic
<
double
,
double
,
1
,
true
,
point_one
>
exact_cubic_one
;
typedef
std
::
pair
<
double
,
point_one
>
WaypointOne
;
typedef
std
::
vector
<
WaypointOne
>
T_WaypointOne
;
...
...
@@ -59,7 +60,7 @@ void ComparePoints(const point_t& pt1, const point_t& pt2, const std::string& er
if
(
!
(
pt1
==
pt2
))
{
error
=
true
;
std
::
cout
<<
errmsg
<<
pt1
<<
" ; "
<<
pt2
<<
"
\n
"
;
std
::
cout
<<
errmsg
<<
pt1
<<
" ; "
<<
pt2
<<
std
::
endl
;
}
}
...
...
@@ -68,7 +69,7 @@ void ComparePoints(const point_one& pt1, const point_one& pt2, const std::string
if
(
!
(
pt1
==
pt2
))
{
error
=
true
;
std
::
cout
<<
errmsg
<<
pt1
<<
" ; "
<<
pt2
<<
"
\n
"
;
std
::
cout
<<
errmsg
<<
pt1
<<
" ; "
<<
pt2
<<
std
::
endl
;
}
}
...
...
@@ -81,25 +82,33 @@ void CubicFunctionTest(bool& error)
point_t
b
(
2
,
3
,
4
);
point_t
c
(
3
,
4
,
5
);
point_t
d
(
3
,
6
,
7
);
cubic_function_t
cf
(
a
,
b
,
c
,
d
,
0
,
1
);
t_point_t
vec
;
vec
.
push_back
(
a
);
vec
.
push_back
(
b
);
vec
.
push_back
(
c
);
vec
.
push_back
(
d
);
cubic_function_t
cf
(
vec
.
begin
(),
vec
.
end
(),
0
,
1
);
point_t
res1
;
res1
=
cf
(
0
);
point_t
x0
(
1
,
2
,
3
);
ComparePoints
(
x0
,
res1
,
errMsg
+
"(0) "
,
error
);
point_t
x1
(
9
,
15
,
19
);
ComparePoints
(
x0
,
res1
,
errMsg
+
"(0) "
,
error
);
res1
=
cf
(
1
);
ComparePoints
(
x1
,
res1
,
errMsg
+
"(1) "
,
error
);
point_t
x1
(
9
,
15
,
19
);
res1
=
cf
(
1
);
ComparePoints
(
x1
,
res1
,
errMsg
+
"(1) "
,
error
);
point_t
x2
(
3.125
,
5.25
,
7.125
);
res1
=
cf
(
0.5
);
ComparePoints
(
x2
,
res1
,
errMsg
+
"(0.5) "
,
error
);
cubic_function_t
cf2
(
a
,
b
,
c
,
d
,
0.5
,
1
);
ComparePoints
(
x2
,
res1
,
errMsg
+
"(0.5) "
,
error
);
vec
.
clear
();
vec
.
push_back
(
a
);
vec
.
push_back
(
b
);
vec
.
push_back
(
c
);
vec
.
push_back
(
d
);
cubic_function_t
cf2
(
vec
,
0.5
,
1
);
res1
=
cf2
(
0.5
);
ComparePoints
(
x0
,
res1
,
errMsg
+
"x3 "
,
error
);
ComparePoints
(
x0
,
res1
,
errMsg
+
"x3 "
,
error
);
error
=
true
;
try
{
...
...
@@ -157,29 +166,29 @@ void BezierCurveTest(bool& error)
point_t
res1
;
res1
=
cf
(
0
);
point_t
x20
=
a
;
ComparePoints
(
x20
,
res1
,
errMsg
+
"2(0) "
,
error
);
ComparePoints
(
x20
,
res1
,
errMsg
+
"2(0) "
,
error
);
point_t
x21
=
b
;
res1
=
cf
(
1
);
ComparePoints
(
x21
,
res1
,
errMsg
+
"2(1) "
,
error
);
ComparePoints
(
x21
,
res1
,
errMsg
+
"2(1) "
,
error
);
//3d curve
params
.
push_back
(
c
);
bezier_curve_t
cf3
(
params
.
begin
(),
params
.
end
());
res1
=
cf3
(
0
);
ComparePoints
(
a
,
res1
,
errMsg
+
"3(0) "
,
error
);
ComparePoints
(
a
,
res1
,
errMsg
+
"3(0) "
,
error
);
res1
=
cf3
(
1
);
ComparePoints
(
c
,
res1
,
errMsg
+
"3(1) "
,
error
);
ComparePoints
(
c
,
res1
,
errMsg
+
"3(1) "
,
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
);
ComparePoints
(
a
,
res1
,
errMsg
+
"3(0) "
,
error
);
res1
=
cf4
(
2
);
ComparePoints
(
d
,
res1
,
errMsg
+
"3(1) "
,
error
);
ComparePoints
(
d
,
res1
,
errMsg
+
"3(1) "
,
error
);
try
{
...
...
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