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
4c1d45e3
Commit
4c1d45e3
authored
Jun 27, 2013
by
stonneau
Browse files
library now just header only (templated)
parent
20b7bd84
Changes
13
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
4c1d45e3
...
...
@@ -3,12 +3,10 @@ project(spline)
set
(
CMAKE_MODULE_PATH
"
${
PROJECT_SOURCE_DIR
}
/build/"
)
set
(
LIBRARY_OUTPUT_PATH
"
${
PROJECT_SOURCE_DIR
}
/lib/"
)
set
(
CMAKE_RUNTIME_OUTPUT_DIRECTORY
"
${
PROJECT_SOURCE_DIR
}
/bin/"
)
find_package
(
Eigen3 REQUIRED
)
include_directories
(
${
EIGEN3_INCLUDE_DIR
}
)
add_subdirectory
(
src/spline
)
add_subdirectory
(
src/tests/spline_test
)
src/spline/API/BezierCurve.h
View file @
4c1d45e3
/**
* \file
B
ezier
C
urve.h
* \file
b
ezier
_c
urve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
...
...
@@ -15,51 +15,104 @@
#include
"Exports.h"
#include
"MathDefs.h"
#include
<memory>
#include
<vector>
namespace
spline
{
/// \class BezierCurve
/// \brief Represents a curve
///
class
BezierCurve
:
public
Curve_ABC
{
/// \class BezierCurve
/// \brief Represents a curve
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
int
Dim
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>
>
struct
bezier_curve
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
/* Constructors - destructors */
public:
///\brief Constructor
///\param points: the points parametering the Bezier curve
///\TODO : sor far size above 3 is ignored
SPLINE_API
BezierCurve
(
const
T_Vector
&
/*points*/
,
const
Real
minBound
=
0
,
const
Real
maxBound
=
1
);
///\brief Constructor
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///\TODO : so far size above 3 is ignored
template
<
typename
In
>
SPLINE_API
bezier_curve
(
In
PointsBegin
,
In
PointsEnd
,
const
time_t
minBound
=
0
,
const
time_t
maxBound
=
1
)
:
minBound_
(
minBound
)
,
maxBound_
(
maxBound
)
,
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
))
{
In
it
(
PointsBegin
);
if
(
Safe
&&
(
size_
<=
1
||
minBound
==
maxBound
))
{
throw
;
// TODO
}
for
(;
it
!=
PointsEnd
;
++
it
)
{
pts_
.
push_back
(
*
it
);
}
}
///\brief Destructor
SPLINE_API
~
BezierCurve
();
///\brief Destructor
SPLINE_API
~
bezier_curve
()
{
// NOTHING
}
private:
B
ezier
C
urve
(
const
B
ezier
C
urve
&
);
B
ezier
C
urve
&
operator
=
(
const
B
ezier
C
urve
&
);
b
ezier
_c
urve
(
const
b
ezier
_c
urve
&
);
b
ezier
_c
urve
&
operator
=
(
const
b
ezier
_c
urve
&
);
/* Constructors - destructors */
/*Operations*/
public:
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t)
/// \param return : true if evaluation is successful, false if t is out of range
SPLINE_API
virtual
bool
Evaluate
(
const
Real
/*t*/
,
Vector3
&
/*result*/
)
const
;
/// \param return : the value x(t)
SPLINE_API
virtual
point_t
operator
()(
time_t
t
)
const
{
num_t
nT
=
(
t
-
minBound_
)
/
(
maxBound_
-
minBound_
);
if
(
Safe
&!
(
0
<=
nT
&&
nT
<=
1
))
{
throw
;
// TODO
}
else
{
num_t
dt
=
(
1
-
nT
);
switch
(
size_
)
{
case
2
:
return
pts_
[
0
]
*
dt
+
nT
*
pts_
[
1
];
break
;
case
3
:
return
pts_
[
0
]
*
dt
*
dt
+
2
*
pts_
[
1
]
*
nT
*
dt
+
pts_
[
2
]
*
nT
*
nT
;
break
;
default
:
return
pts_
[
0
]
*
dt
*
dt
*
dt
+
3
*
pts_
[
1
]
*
nT
*
dt
*
dt
+
3
*
pts_
[
2
]
*
nT
*
nT
*
dt
+
pts_
[
3
]
*
nT
*
nT
*
nT
;
break
;
}
}
}
/*Operations*/
SPLINE_API
virtual
Real
MinBound
()
const
;
SPLINE_API
virtual
Real
MaxBound
()
const
;
/*Helpers*/
SPLINE_API
virtual
time_t
MinBound
()
const
{
return
minBound_
;}
SPLINE_API
virtual
time_t
MaxBound
()
const
{
return
minBound_
;}
/*Helpers*/
public:
const
int
size_
;
const
Real
minBound_
,
maxBound_
;
const
int
size_
;
const
time_t
minBound_
,
maxBound_
;
private:
const
T_Vector
pts_
;
};
typedef
std
::
vector
<
Point
,
Eigen
::
aligned_allocator
<
Point
>
>
T_Vector
;
T_Vector
pts_
;
};
}
#endif //_CLASS_BEZIERCURVE
src/spline/API/CubicFunction.h
View file @
4c1d45e3
...
...
@@ -5,63 +5,84 @@
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the CubicFunction
class
.
* It allows the creation and evaluation of natural
3D
* smooth cubic splines
* This file contains definitions for the CubicFunction
struct
.
* It allows the creation and evaluation of natural
* smooth cubic splines
of arbitrary dimension
*/
#ifndef _
CLASS
_CUBICFUNCTION
IMP
#define _
CLASS
_CUBICFUNCTION
IMP
#ifndef _
STRUCT
_CUBICFUNCTION
#define _
STRUCT
_CUBICFUNCTION
#include
"Exports.h"
#include
"MathDefs.h"
#include
"Curve_ABC.h"
#include
<stdexcept>
namespace
spline
{
class
SplineVisitor
;
/// \class CubicFunction
/// \brief Represents a cubic spline defined on the interval
/// [tBegin, tEnd]. It follows the equation
/// x(t) = a + b(t - t
Beg
in) + c(t - t
Beg
in)^2 + d(t - t
Beg
in)^3
/// x(t) = a + b(t - t
_m
in
_
) + c(t - t
_m
in
_
)^2 + d(t - t
_m
in
_
)^3
///
class
CubicFunction
:
public
Curve_ABC
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
int
Dim
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>
>
struct
cubic_function
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
/* Constructors - destructors */
public:
///\brief Constructor
SPLINE_API
CubicFunction
(
const
Vector3
&
/*a*/
,
const
Vector3
&
/*b*/
,
const
Vector3
&
/*c*/
,
const
Vector3
&
/*d*/
,
const
Real
/*tBegin*/
,
const
Real
/*tEnd*/
);
SPLINE_API
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
)
{
if
(
t_min_
>=
t_max_
&
Safe
)
{
std
::
out_of_range
(
"TODO"
);
}
}
///\brief Destructor
SPLINE_API
~
CubicFunction
();
SPLINE_API
~
cubic_function
()
{
// NOTHING
}
private:
C
ubic
F
unction
(
const
C
ubic
F
unction
&
);
C
ubic
F
unction
&
operator
=
(
const
C
ubic
F
unction
&
);
//c
ubic
_f
unction(const
c
ubic
_f
unction&);
//c
ubic
_f
unction& operator=(const
c
ubic
_f
unction&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t)
SPLINE_API
virtual
bool
Evaluate
(
const
Real
/*t*/
,
Vector3
&
/*result*/
)
const
;
/// \param return : the value x(t)
SPLINE_API
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:
SPLINE_API
Real
virtual
MinBound
()
const
;
SPLINE_API
Real
virtual
MaxBound
()
const
;
SPLINE_API
num_t
virtual
MinBound
()
const
{
return
t_min_
;}
SPLINE_API
num_t
virtual
MaxBound
()
const
{
return
t_max_
;}
/*Helpers*/
/*Attributes*/
p
rivate
:
const
Vector3
a_
,
b_
,
c_
,
d_
;
const
Real
tBeg
in_
,
t
End
_
;
p
ublic
:
const
point_t
a_
,
b_
,
c_
,
d_
;
const
time_t
t_m
in_
,
t
_max
_
;
/*Attributes*/
};
//class CubicFunction
}
#endif //_
CLASS
_CUBICFUNCTION
IMP
#endif //_
STRUCT
_CUBICFUNCTION
src/spline/API/Curve_ABC.h
View file @
4c1d45e3
/**
* \file
C
urve_
ABC
.h
* \brief
class allowing to create an Exact cubic spline
.
* \file
c
urve_
abc
.h
* \brief
interface for a Curve of arbitrary dimension
.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
...
...
@@ -9,67 +9,49 @@
*/
#ifndef _CLASS_CURVEABC
#define _CLASS_CURVEABC
#include
"SplineVisitor.h"
#ifndef _STRUCT_CURVE_ABC
#define _STRUCT_CURVE_ABC
#include
"Exports.h"
#include
"MathDefs.h"
#include
<memory>
#include
<vector>
#include
<functional>
namespace
spline
{
/// \class Curve_ABC
/// \brief Represents a curve
///
class
Curve_ABC
{
/// \struct curve_abc
/// \brief Represents a curve of dimension Dim
/// is Safe is false, no verification is made on the evaluation of the curve.
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
int
Dim
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>
>
struct
curve_abc
:
std
::
unary_function
<
Time
,
Point
>
{
typedef
Point
point_t
;
typedef
Time
time_t
;
/* Constructors - destructors */
public:
///\brief Constructor
SPLINE_API
Curve_ABC
(){};
///\brief Destructor
SPLINE_API
~
Curve_ABC
(){};
///\brief Constructor
SPLINE_API
curve_abc
(){};
private:
Curve_ABC
(
const
Curve_ABC
&
);
Curve_ABC
&
operator
=
(
const
Curve_ABC
&
);
///\brief Destructor
SPLINE_API
~
curve_abc
(){};
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t)
/// \param return : true if evaluation is successful, false if t is out of range
SPLINE_API
virtual
bool
Evaluate
(
const
Real
/*t*/
,
Vector3
&
/*result*/
)
const
=
0
;
/// \param return : the value x(t)
SPLINE_API
virtual
point_t
operator
()(
time_t
t
)
const
=
0
;
/*Operations*/
/*Helpers*/
public:
/// \brief Given a timestep dt, returns a set of values for the exact spline
/// separated by this timestep
/// \param visitor : an object called for each timestep in the spline interval.
/// \param dt : the timestep
/// \param result : a reference to the Point set to the x(t)
SPLINE_API
void
Accept
(
SplineVisitor
&
visitor
,
Real
dt
)
const
{
for
(
Real
ti
=
MinBound
();
ti
<=
MaxBound
();
ti
=
ti
+
dt
)
{
Vector3
res
;
Evaluate
(
ti
,
res
);
visitor
.
Visit
(
ti
,
res
);
}
}
SPLINE_API
virtual
Real
MinBound
()
const
=
0
;
SPLINE_API
virtual
Real
MaxBound
()
const
=
0
;
SPLINE_API
virtual
time_t
MinBound
()
const
=
0
;
SPLINE_API
virtual
time_t
MaxBound
()
const
=
0
;
/*Helpers*/
};
}
#endif //_
CLASS_EXA
CTCUB
I
C
#endif //_
STRU
CT
_
CU
RVE_A
BC
src/spline/API/ExactCubic.h
View file @
4c1d45e3
/**
* \file
E
xact
C
ubic.h
* \file
e
xact
_c
ubic.h
* \brief class allowing to create an Exact cubic spline.
* \author Steve T.
* \version 0.1
...
...
@@ -21,56 +21,157 @@
#define _CLASS_EXACTCUBIC
#include
"Curve_ABC.h"
#include
"CubicFunction.h"
#include
"Exports.h"
#include
"MathDefs.h"
#include
<
memory
>
#include
<
functional
>
#include
<vector>
#include
<Eigen/StdVector>
#include
<iostream>
namespace
spline
{
struct
CubicPImpl
;
// private implementation
/// \class ExactCubic
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization
///
struct
ExactCubic
:
public
Curve_ABC
{
/* Constructors - destructors */
/// \class ExactCubic
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization
///
template
<
typename
Time
=
double
,
typename
Numeric
=
Time
,
int
Dim
=
3
,
bool
Safe
=
false
,
typename
Point
=
Eigen
::
Matrix
<
Numeric
,
Dim
,
1
>
>
struct
exact_cubic
:
public
curve_abc
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
>
{
typedef
Point
point_t
;
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
MatrixX
;
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
cubic_function
<
time_t
,
Numeric
,
Dim
,
Safe
,
Point
>
cubic_function_t
;
typedef
typename
std
::
vector
<
cubic_function_t
*>
T_cubic
;
typedef
typename
T_cubic
::
iterator
IT_cubic
;
typedef
typename
T_cubic
::
const_iterator
CIT_cubic
;
/* Constructors - destructors */
public:
///\brief Constructor
///\param waypoints : a list comprising at least 2 waypoints in ascending time order
SPLINE_API
ExactCubic
(
const
T_Waypoint
&
/*waypoints*/
);
///\brief Constructor
///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template
<
typename
In
>
SPLINE_API
exact_cubic
(
In
wayPointsBegin
,
In
wayPointsEnd
)
{
std
::
size_t
const
size
(
std
::
distance
(
wayPointsBegin
,
wayPointsEnd
));
if
(
Safe
&&
size
<
1
)
{
throw
;
// TODO
}
// refer to the paper to understand all this.
MatrixX
h1
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
h2
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
h3
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
h4
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
h5
=
MatrixX
::
Zero
(
size
,
size
);
MatrixX
h6
=
MatrixX
::
Zero
(
size
,
size
);
///\brief Destructor
SPLINE_API
~
ExactCubic
();
MatrixX
a
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
b
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
c
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
d
=
MatrixX
::
Zero
(
size
,
Dim
);
MatrixX
x
=
MatrixX
::
Zero
(
size
,
Dim
);
In
it
(
wayPointsBegin
),
next
(
wayPointsBegin
);
++
next
;
Numeric
t_previous
((
*
it
).
first
);
for
(
std
::
size_t
i
(
0
);
next
!=
wayPointsEnd
;
++
next
,
++
it
,
++
i
)
{
num_t
const
dTi
((
*
next
).
first
-
(
*
it
).
first
);
num_t
const
dTi_sqr
(
dTi
*
dTi
);
num_t
const
dTi_cube
(
dTi_sqr
*
dTi
);
// filling matrices values
h3
(
i
,
i
)
=
-
3
/
dTi_sqr
;
h3
(
i
,
i
+
1
)
=
3
/
dTi_sqr
;
h4
(
i
,
i
)
=
-
2
/
dTi
;
h4
(
i
,
i
+
1
)
=
-
1
/
dTi
;
h5
(
i
,
i
)
=
2
/
dTi_cube
;
h5
(
i
,
i
+
1
)
=
-
2
/
dTi_cube
;
h6
(
i
,
i
)
=
1
/
dTi_sqr
;
h6
(
i
,
i
+
1
)
=
1
/
dTi_sqr
;
if
(
i
+
2
<
size
)
{
In
it2
(
next
);
++
it2
;
num_t
const
dTi_1
(
1
/
((
*
it2
).
first
-
(
*
next
).
first
));
num_t
const
dTi_1sqr
(
dTi_1
*
dTi_1
);
// this can be optimized but let's focus on clarity as long as not needed
h1
(
i
+
1
,
i
)
=
2
/
dTi
;
h1
(
i
+
1
,
i
+
1
)
=
4
/
dTi
+
4
/
dTi_1
;
h1
(
i
+
1
,
i
+
2
)
=
2
/
dTi_1
;
h2
(
i
+
1
,
i
)
=
-
6
/
dTi_sqr
;
h2
(
i
+
1
,
i
+
1
)
=
(
6
/
dTi_1sqr
)
-
(
6
/
dTi_sqr
);
h2
(
i
+
1
,
i
+
2
)
=
6
/
dTi_1sqr
;
}
x
.
row
(
i
)
=
(
*
it
).
second
.
transpose
();
}
// adding last x
x
.
row
(
size
-
1
)
=
(
*
it
).
second
.
transpose
();
a
=
x
;
PseudoInverse
(
h1
);
b
=
h1
*
h2
*
x
;
//h1 * b = h2 * x => b = (h1)^-1 * h2 * x
c
=
h3
*
x
+
h4
*
b
;
d
=
h5
*
x
+
h6
*
b
;
it
=
wayPointsBegin
,
next
=
wayPointsBegin
;
++
next
;
for
(
int
i
=
0
;
next
!=
wayPointsEnd
;
++
i
,
++
it
,
++
next
)
{
subSplines_
.
push_back
(
new
cubic_function_t
(
a
.
row
(
i
),
b
.
row
(
i
),
c
.
row
(
i
),
d
.
row
(
i
),
(
*
it
).
first
,
(
*
next
).
first
));
}
subSplines_
.
push_back
(
new
cubic_function_t
(
a
.
row
(
size
-
1
),
b
.
row
(
size
-
1
),
c
.
row
(
size
-
1
),
d
.
row
(
size
-
1
),
(
*
it
).
first
,
(
*
it
).
first
));
}
///\brief Destructor
SPLINE_API
~
exact_cubic
()
{
for
(
IT_cubic
it
=
subSplines_
.
begin
();
it
!=
subSplines_
.
end
();
++
it
)
{
delete
(
*
it
);
}
}
private:
E
xact
C
ubic
(
const
E
xact
C
ubic
&
);
E
xact
C
ubic
&
operator
=
(
const
E
xact
C
ubic
&
);
/* Constructors - destructors */
e
xact
_c
ubic
(
const
e
xact
_c
ubic
&
);
e
xact
_c
ubic
&
operator
=
(
const
e
xact
_c
ubic
&
);
/* Constructors - destructors */
/*Operations*/
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t)
/// \return : true if evaluation is successful, false if t is out of range
SPLINE_API
virtual
bool
Evaluate
(
const
Real
/*t*/
,
Vector3
&
/*result*/
)
const
;
/*Operations*/
/// \param return : the value x(t)
SPLINE_API
virtual
point_t
operator
()(
time_t
t
)
const
{
if
(
Safe
&&
(
t
<
subSplines_
.
front
()
->
t_min_
||
t
>
subSplines_
.
back
()
->
t_max_
)){
throw
std
::
out_of_range
(
"TODO"
);}
for
(
CIT_cubic
it
=
subSplines_
.
begin
();
it
!=
subSplines_
.
end
();
++
it
)
{
if
(
t
>=
((
*
it
)
->
t_min_
)
&&
t
<=
((
*
it
)
->
t_max_
))
{
return
(
*
it
)
->
operator
()(
t
);
}
}
}
/*Operations*/
/*Helpers*/
/*Helpers*/
public:
SPLINE_API
Real
virtual
MinBound
()
const
;
SPLINE_API
Real
virtual
MaxBound
()
const
;
/*Helpers*/
SPLINE_API
num_t
virtual
MinBound
()
const
{
return
subSplines_
.
front
()
->
t_min_
;}
SPLINE_API
num_t
virtual
MaxBound
()
const
{
return
subSplines_
.
back
()
->
t_max_
;}
/*Helpers*/
/*Attributes*/
/*Attributes*/
private:
std
::
auto_ptr
<
CubicPImpl
>
pImpl
_
;
/*Attributes*/
};
T_cubic
subSplines
_
;
/*Attributes*/
};
}
#endif //_CLASS_EXACTCUBIC
src/spline/API/MathDefs.h
View file @
4c1d45e3
...
...
@@ -24,35 +24,17 @@
#include
<utility>
namespace
spline
{
#if (USEFLOAT)
typedef
float
Real
;
typedef
Eigen
::
Vector3f
Vector3
;
typedef
Eigen
::
Vector2f
Vector2
;
typedef
Eigen
::
VectorXf
VectorX
;
typedef
Eigen
::
MatrixXf
MatrixX
;
typedef
Eigen
::
Matrix4f
Matrix4
;
typedef
Eigen
::
Matrix3f
Matrix3
;
#else
typedef
double
Real
;
typedef
Eigen
::
Vector3d
Vector3
;
typedef
Eigen
::
Vector2d
Vector2
;
typedef
Eigen
::
VectorXd
VectorX
;
typedef
Eigen
::
MatrixXd
MatrixX
;
typedef
Eigen
::
Matrix4d
Matrix4
;
typedef
Eigen
::
Matrix3d
Matrix3
;
#endif
//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
);
VectorX
m_sigma
=
svd
.
singularValues
();
_Matrix_Type_
m_sigma
=
svd
.
singularValues
();
Real
pinvtoler
=
1.e-6
;
// choose your tolerance widely!
double
pinvtoler
=
1.e-6
;
// choose your tolerance widely!
Matrix
X
m_sigma_inv
=
Matrix
X
::
Zero
(
pinvmat
.
cols
(),
pinvmat
.
rows
());
_
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
)
...
...
@@ -61,14 +43,6 @@ void PseudoInverse(_Matrix_Type_& pinvmat)
pinvmat
=
(
svd
.
matrixV
()
*
m_sigma_inv
*
svd
.
matrixU
().
transpose
());
}
typedef
std
::
vector
<
Vector3
,
Eigen
::
aligned_allocator
<
Vector3
>
>
T_Vector
;
/** Definition for a waypoint */
typedef
std
::
pair
<
Real
,
Vector3
>
Waypoint
;
typedef
std
::
vector
<
Waypoint
>
T_Waypoint
;
typedef
T_Waypoint
::
iterator
IT_Waypoint
;
typedef
T_Waypoint
::
const_iterator
CIT_Waypoint
;
}
// namespace spline
#endif //_SPLINEMATH
src/spline/API/SplineVisitor.h
deleted
100644 → 0
View file @
20b7bd84
/**
* \file SplineVisitor.h
* \brief Visitor for the Spline classes
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/