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
6ebf1138
Commit
6ebf1138
authored
Nov 25, 2016
by
Steve Tonneau
Browse files
no longer using pointers in exact_cubic. had to lose const parameter
parent
eb75a2c8
Changes
3
Hide whitespace changes
Inline
Side-by-side
include/spline/cubic_zero_vel_acc.h
View file @
6ebf1138
...
...
@@ -78,8 +78,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
In
it
(
wayPointsBegin
),
next
(
wayPointsBegin
);
++
next
;
Numeric
t_previous
((
*
it
).
first
);
++
next
;
for
(
std
::
size_t
i
(
0
);
next
!=
wayPointsEnd
;
++
next
,
++
it
,
++
i
)
{
...
...
include/spline/exact_cubic.h
View file @
6ebf1138
...
...
@@ -45,9 +45,9 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef
Time
time_t
;
typedef
Numeric
num_t
;
typedef
spline_curve
<
time_t
,
Numeric
,
Dim
,
Safe
,
point_t
,
t_point_t
>
spline_t
;
typedef
typename
std
::
vector
<
spline_t
*
>
T_cubic
;
typedef
typename
T_cubic
::
iterator
IT_cubic
;
typedef
typename
T_cubic
::
const_iterator
CIT_cubic
;
typedef
typename
std
::
vector
<
spline_t
>
t_spline_t
;
typedef
typename
t_spline_t
::
iterator
it_spline_t
;
typedef
typename
t_spline_t
::
const_iterator
cit_spline_t
;
/* Constructors - destructors */
public:
...
...
@@ -56,89 +56,92 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template
<
typename
In
>
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
);
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
;
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
((
*
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
)
{
add_cubic
(
a
.
row
(
i
),
b
.
row
(
i
),
c
.
row
(
i
),
d
.
row
(
i
),(
*
it
).
first
,
(
*
next
).
first
);
}
add_cubic
(
a
.
row
(
size
-
1
),
b
.
row
(
size
-
1
),
c
.
row
(
size
-
1
),
d
.
row
(
size
-
1
),(
*
it
).
first
,
(
*
it
).
first
);
:
subSplines_
(
computeWayPoints
<
In
>
(
wayPointsBegin
,
wayPointsEnd
))
{
}
///\brief Destructor
~
exact_cubic
()
~
exact_cubic
(){}
protected:
template
<
typename
In
>
t_spline_t
computeWayPoints
(
In
wayPointsBegin
,
In
wayPointsEnd
)
const
{
for
(
IT_cubic
it
=
subSplines_
.
begin
();
it
!=
subSplines_
.
end
();
++
it
)
std
::
size_t
const
size
(
std
::
distance
(
wayPointsBegin
,
wayPointsEnd
));
if
(
Safe
&&
size
<
1
)
{
delete
(
*
it
);
throw
;
// TODO
}
}
t_spline_t
subSplines
;
subSplines
.
reserve
(
size
);
// 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
);
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
);
private:
void
add_cubic
(
point_t
const
&
a
,
point_t
const
&
b
,
point_t
const
&
c
,
point_t
const
&
d
,
const
time_t
min
,
const
time_t
max
)
In
it
(
wayPointsBegin
),
next
(
wayPointsBegin
);
++
next
;
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
((
*
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
)
{
add_cubic
(
a
.
row
(
i
),
b
.
row
(
i
),
c
.
row
(
i
),
d
.
row
(
i
),(
*
it
).
first
,
(
*
next
).
first
,
subSplines
);
}
add_cubic
(
a
.
row
(
size
-
1
),
b
.
row
(
size
-
1
),
c
.
row
(
size
-
1
),
d
.
row
(
size
-
1
),(
*
it
).
first
,
(
*
it
).
first
,
subSplines
);
return
subSplines
;
}
void
add_cubic
(
point_t
const
&
a
,
point_t
const
&
b
,
point_t
const
&
c
,
point_t
const
&
d
,
const
time_t
min
,
const
time_t
max
,
t_spline_t
&
subSplines
)
const
{
t_point_t
coeffs
=
make_cubic_vector
<
point_t
,
t_point_t
>
(
a
,
b
,
c
,
d
);
subSplines
_
.
push_back
(
new
spline_t
(
coeffs
.
begin
(),
coeffs
.
end
(),
min
,
max
));
subSplines
.
push_back
(
spline_t
(
coeffs
.
begin
(),
coeffs
.
end
(),
min
,
max
));
}
private:
...
...
@@ -153,13 +156,12 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param return : the value x(t)
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
(
Safe
&&
(
t
<
subSplines_
.
front
().
t_min_
||
t
>
subSplines_
.
back
().
t_max_
)){
throw
std
::
out_of_range
(
"TODO"
);}
for
(
cit_spline_t
it
=
subSplines_
.
begin
();
it
!=
subSplines_
.
end
();
++
it
)
{
if
(
t
>=
(
(
*
it
)
->
t_min_
)
&&
t
<=
(
(
*
it
)
->
t_max_
))
if
(
t
>=
(
it
->
t_min_
)
&&
t
<=
(
it
->
t_max_
))
{
return
(
*
it
)
->
operator
()(
t
);
return
it
->
operator
()(
t
);
}
}
}
...
...
@@ -167,13 +169,13 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Helpers*/
public:
num_t
virtual
min
()
const
{
return
subSplines_
.
front
()
->
t_min_
;}
num_t
virtual
max
()
const
{
return
subSplines_
.
back
()
->
t_max_
;}
num_t
virtual
min
()
const
{
return
subSplines_
.
front
()
.
t_min_
;}
num_t
virtual
max
()
const
{
return
subSplines_
.
back
()
.
t_max_
;}
/*Helpers*/
/*Attributes*/
private
:
T_cubic
subSplines_
;
public
:
const
t_spline_t
subSplines_
;
/*Attributes*/
};
}
...
...
include/spline/spline_curve.h
View file @
6ebf1138
...
...
@@ -98,7 +98,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
,
coefficients_
(
other
.
coefficients_
)
{}
private:
spline_curve
&
operator
=
(
const
spline_curve
&
other
);
//
spline_curve& operator=(const spline_curve& other);
/* Constructors - destructors */
...
...
@@ -130,10 +130,10 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Attributes*/
public:
const
t_point_t
coefficients_
;
const
time_t
t_min_
,
t_max_
;
const
std
::
size_t
dim_
;
const
std
::
size_t
order_
;
t_point_t
coefficients_
;
time_t
t_min_
,
t_max_
;
std
::
size_t
dim_
;
std
::
size_t
order_
;
/*Attributes*/
private:
...
...
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