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
5b73cb39
Commit
5b73cb39
authored
Sep 28, 2020
by
Steve T
Browse files
curve and point addition cpp
parent
a5f47719
Changes
3
Hide whitespace changes
Inline
Side-by-side
include/curves/bezier_curve.h
View file @
5b73cb39
...
...
@@ -446,6 +446,38 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return
c_split
.
second
.
split
(
t2
).
first
;
}
/// \brief Compute the cross product of the current bezier curve by another bezier curve.
/// The cross product p1Xp2 of 2 bezier curves p1 and p2 is defined such that
/// forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product.
/// This method of course only makes sense for dimension 3 curves.
/// It assumes that a method point_t cross(const point_t&, const point_t&) has been defined
/// \param pOther other polynomial to compute the cross product with.
/// \return a new polynomial defining the cross product between this and pother
bezier_curve_t
cross
(
const
bezier_curve_t
&
g
)
const
{
//See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and
//http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
assert_operator_compatible
(
g
);
if
(
dim
()
!=
3
)
throw
std
::
invalid_argument
(
"Can't perform cross product on Bezier curves with dimensions != 3 "
);
int
m
=
(
int
)(
degree
());
int
n
=
(
int
)(
g
.
degree
());
unsigned
int
mj
,
n_ij
,
mn_i
;
t_point_t
new_waypoints
;
for
(
int
i
=
0
;
i
<=
m
+
n
;
++
i
)
{
bezier_curve_t
::
point_t
current_point
=
bezier_curve_t
::
point_t
::
Zero
(
dim
());
for
(
int
j
=
std
::
max
(
0
,
i
-
n
);
j
<=
std
::
min
(
m
,
i
);
++
j
){
mj
=
bin
(
m
,
j
);
n_ij
=
bin
(
n
,
i
-
j
);
mn_i
=
bin
(
m
+
n
,
i
);
num_t
mul
=
num_t
(
mj
*
n_ij
)
/
num_t
(
mn_i
);
current_point
+=
mul
*
curves
::
cross
(
waypointAtIndex
(
j
),
g
.
waypointAtIndex
(
i
-
j
));
}
new_waypoints
.
push_back
(
current_point
);
}
return
bezier_curve_t
(
new_waypoints
.
begin
(),
new_waypoints
.
end
(),
min
(),
max
(),
mult_T_
*
g
.
mult_T_
);
}
bezier_curve_t
&
operator
+=
(
const
bezier_curve_t
&
other
)
{
assert_operator_compatible
(
other
);
bezier_curve_t
other_elevated
=
other
*
(
other
.
mult_T_
/
this
->
mult_T_
);
// TODO remove mult_T_ from Bezier
...
...
@@ -478,6 +510,20 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return
*
this
;
}
bezier_curve_t
&
operator
+=
(
const
bezier_curve_t
::
point_t
&
point
)
{
for
(
typename
t_point_t
::
iterator
it
=
control_points_
.
begin
();
it
!=
control_points_
.
end
();
++
it
){
(
*
it
)
+=
point
;
}
return
*
this
;
}
bezier_curve_t
&
operator
-=
(
const
bezier_curve_t
::
point_t
&
point
)
{
for
(
typename
t_point_t
::
iterator
it
=
control_points_
.
begin
();
it
!=
control_points_
.
end
();
++
it
){
(
*
it
)
-=
point
;
}
return
*
this
;
}
bezier_curve_t
&
operator
/=
(
const
double
d
)
{
for
(
typename
t_point_t
::
iterator
it
=
control_points_
.
begin
();
it
!=
control_points_
.
end
();
++
it
){
(
*
it
)
/=
d
;
...
...
@@ -493,38 +539,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
}
/// \brief Compute the cross product of the current bezier curve by another bezier curve.
/// The cross product p1Xp2 of 2 bezier curves p1 and p2 is defined such that
/// forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product.
/// This method of course only makes sense for dimension 3 curves.
/// It assumes that a method point_t cross(const point_t&, const point_t&) has been defined
/// \param pOther other polynomial to compute the cross product with.
/// \return a new polynomial defining the cross product between this and pother
bezier_curve_t
cross
(
const
bezier_curve_t
&
g
)
const
{
//See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and
//http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
assert_operator_compatible
(
g
);
if
(
dim
()
!=
3
)
throw
std
::
invalid_argument
(
"Can't perform cross product on Bezier curves with dimensions != 3 "
);
int
m
=
(
int
)(
degree
());
int
n
=
(
int
)(
g
.
degree
());
unsigned
int
mj
,
n_ij
,
mn_i
;
t_point_t
new_waypoints
;
for
(
int
i
=
0
;
i
<=
m
+
n
;
++
i
)
{
bezier_curve_t
::
point_t
current_point
=
bezier_curve_t
::
point_t
::
Zero
(
dim
());
for
(
int
j
=
std
::
max
(
0
,
i
-
n
);
j
<=
std
::
min
(
m
,
i
);
++
j
){
mj
=
bin
(
m
,
j
);
n_ij
=
bin
(
n
,
i
-
j
);
mn_i
=
bin
(
m
+
n
,
i
);
num_t
mul
=
num_t
(
mj
*
n_ij
)
/
num_t
(
mn_i
);
current_point
+=
mul
*
curves
::
cross
(
waypointAtIndex
(
j
),
g
.
waypointAtIndex
(
i
-
j
));
}
new_waypoints
.
push_back
(
current_point
);
}
return
bezier_curve_t
(
new_waypoints
.
begin
(),
new_waypoints
.
end
(),
min
(),
max
(),
mult_T_
*
g
.
mult_T_
);
}
private:
/// \brief Ensure constraints of bezier curve.
/// Add 4 points (2 after the first one, 2 before the last one) to biezer curve
...
...
@@ -652,6 +666,31 @@ bezier_curve<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1, const bezier_cu
return
res
-=
p2
;
}
template
<
typename
T
,
typename
N
,
bool
S
,
typename
P
>
bezier_curve
<
T
,
N
,
S
,
P
>
operator
-
(
const
bezier_curve
<
T
,
N
,
S
,
P
>&
p1
,
const
typename
bezier_curve
<
T
,
N
,
S
,
P
>::
point_t
&
point
)
{
bezier_curve
<
T
,
N
,
S
,
P
>
res
(
p1
);
return
res
-=
point
;
}
template
<
typename
T
,
typename
N
,
bool
S
,
typename
P
>
bezier_curve
<
T
,
N
,
S
,
P
>
operator
-
(
const
typename
bezier_curve
<
T
,
N
,
S
,
P
>::
point_t
&
point
,
const
bezier_curve
<
T
,
N
,
S
,
P
>&
p1
)
{
bezier_curve
<
T
,
N
,
S
,
P
>
res
(
-
p1
);
return
res
+=
point
;
}
template
<
typename
T
,
typename
N
,
bool
S
,
typename
P
>
bezier_curve
<
T
,
N
,
S
,
P
>
operator
+
(
const
bezier_curve
<
T
,
N
,
S
,
P
>&
p1
,
const
typename
bezier_curve
<
T
,
N
,
S
,
P
>::
point_t
&
point
)
{
bezier_curve
<
T
,
N
,
S
,
P
>
res
(
p1
);
return
res
+=
point
;
}
template
<
typename
T
,
typename
N
,
bool
S
,
typename
P
>
bezier_curve
<
T
,
N
,
S
,
P
>
operator
+
(
const
typename
bezier_curve
<
T
,
N
,
S
,
P
>::
point_t
&
point
,
const
bezier_curve
<
T
,
N
,
S
,
P
>&
p1
)
{
bezier_curve
<
T
,
N
,
S
,
P
>
res
(
p1
);
return
res
+=
point
;
}
template
<
typename
T
,
typename
N
,
bool
S
,
typename
P
>
bezier_curve
<
T
,
N
,
S
,
P
>
operator
/
(
const
bezier_curve
<
T
,
N
,
S
,
P
>&
p1
,
const
double
k
)
{
bezier_curve
<
T
,
N
,
S
,
P
>
res
(
p1
);
...
...
include/curves/polynomial.h
View file @
5b73cb39
...
...
@@ -428,6 +428,16 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
return
*
this
;
}
polynomial_t
&
operator
+=
(
const
polynomial_t
::
point_t
&
point
)
{
coefficients_
.
col
(
0
)
+=
point
;
return
*
this
;
}
polynomial_t
&
operator
-=
(
const
polynomial_t
::
point_t
&
point
)
{
coefficients_
.
col
(
0
)
-=
point
;
return
*
this
;
}
polynomial_t
&
operator
/=
(
const
double
d
)
{
coefficients_
/=
d
;
return
*
this
;
...
...
@@ -519,6 +529,31 @@ polynomial<T,N,S,P,TP> operator+(const polynomial<T,N,S,P,TP>& p1, const polynom
return
res
+=
p2
;
}
template
<
typename
T
,
typename
N
,
bool
S
,
typename
P
,
typename
TP
>
polynomial
<
T
,
N
,
S
,
P
,
TP
>
operator
+
(
const
polynomial
<
T
,
N
,
S
,
P
,
TP
>&
p1
,
const
typename
polynomial
<
T
,
N
,
S
,
P
,
TP
>::
point_t
&
point
)
{
polynomial
<
T
,
N
,
S
,
P
,
TP
>
res
(
p1
);
return
res
+=
point
;
}
template
<
typename
T
,
typename
N
,
bool
S
,
typename
P
,
typename
TP
>
polynomial
<
T
,
N
,
S
,
P
,
TP
>
operator
+
(
const
typename
polynomial
<
T
,
N
,
S
,
P
,
TP
>::
point_t
&
point
,
const
polynomial
<
T
,
N
,
S
,
P
,
TP
>&
p1
)
{
polynomial
<
T
,
N
,
S
,
P
,
TP
>
res
(
p1
);
return
res
+=
point
;
}
template
<
typename
T
,
typename
N
,
bool
S
,
typename
P
,
typename
TP
>
polynomial
<
T
,
N
,
S
,
P
,
TP
>
operator
-
(
const
polynomial
<
T
,
N
,
S
,
P
,
TP
>&
p1
,
const
typename
polynomial
<
T
,
N
,
S
,
P
,
TP
>::
point_t
&
point
)
{
polynomial
<
T
,
N
,
S
,
P
,
TP
>
res
(
p1
);
return
res
-=
point
;
}
template
<
typename
T
,
typename
N
,
bool
S
,
typename
P
,
typename
TP
>
polynomial
<
T
,
N
,
S
,
P
,
TP
>
operator
-
(
const
typename
polynomial
<
T
,
N
,
S
,
P
,
TP
>::
point_t
&
point
,
const
polynomial
<
T
,
N
,
S
,
P
,
TP
>&
p1
)
{
polynomial
<
T
,
N
,
S
,
P
,
TP
>
res
(
-
p1
);
return
res
+=
point
;
}
template
<
typename
T
,
typename
N
,
bool
S
,
typename
P
,
typename
TP
>
polynomial
<
T
,
N
,
S
,
P
,
TP
>
operator
-
(
const
polynomial
<
T
,
N
,
S
,
P
,
TP
>&
p1
)
{
typename
polynomial
<
T
,
N
,
S
,
P
,
TP
>::
coeff_t
res
=
-
p1
.
coeff
();
...
...
tests/test-operations.cpp
View file @
5b73cb39
...
...
@@ -116,6 +116,31 @@ BOOST_AUTO_TEST_CASE(bezierOperations, * boost::unit_test::tolerance(0.001)) {
}
}
BOOST_AUTO_TEST_CASE
(
bezierPointOperations
,
*
boost
::
unit_test
::
tolerance
(
0.001
))
{
t_pointX_t
vec1
;
for
(
int
i
=
0
;
i
<
6
;
++
i
)
{
vec1
.
push_back
(
Eigen
::
Vector3d
::
Random
());
}
bezier_t
p1
(
vec1
.
begin
(),
vec1
.
end
(),
0.
,
1.
);
Eigen
::
Vector3d
point
;
point
<<
1.
,
1.
,
1.
;
//bezier_t::point_t point = bezier_t::point_t::Random(3);
bezier_t
pSum
=
p1
+
point
;
bezier_t
pSumR
=
point
+
p1
;
bezier_t
pSub
=
p1
-
point
;
bezier_t
pSubR
=
point
-
p1
;
for
(
double
i
=
0.
;
i
<=
100.
;
++
i
){
double
dt
=
i
/
100.
;
BOOST_TEST
((
pSum
(
dt
)
-
(
p1
(
dt
)
+
point
)).
norm
()
==
0.
);
BOOST_TEST
((
pSumR
(
dt
)
-
(
p1
(
dt
)
+
point
)).
norm
()
==
0.
);
BOOST_TEST
((
pSub
(
dt
)
-
(
p1
(
dt
)
-
point
)).
norm
()
==
0.
);
BOOST_TEST
((
pSubR
(
dt
)
-
(
point
-
p1
(
dt
))).
norm
()
==
0.
);
}
}
BOOST_AUTO_TEST_CASE
(
crossPoductLinearVariable
,
*
boost
::
unit_test
::
tolerance
(
0.001
))
{
linear_variable_t
l1
(
Eigen
::
Matrix3d
::
Identity
()
*
5.
,
Eigen
::
Vector3d
::
Random
());
...
...
@@ -168,6 +193,24 @@ BOOST_AUTO_TEST_CASE(crossProductBezierLinearVariable, * boost::unit_test::toler
}
BOOST_AUTO_TEST_CASE
(
polynomialPointOperations
,
*
boost
::
unit_test
::
tolerance
(
0.001
))
{
polynomial_t
::
coeff_t
coeffs1
=
Eigen
::
MatrixXd
::
Random
(
3
,
5
);
polynomial_t
::
point_t
point
=
polynomial_t
::
point_t
::
Random
(
3
);
polynomial_t
p1
(
coeffs1
,
0.
,
1.
);
polynomial_t
pSum
=
p1
+
point
;
polynomial_t
pSumR
=
point
+
p1
;
polynomial_t
pSub
=
p1
-
point
;
polynomial_t
pSubR
=
point
-
p1
;
for
(
double
i
=
0.
;
i
<=
100.
;
++
i
){
double
dt
=
i
/
100.
;
BOOST_TEST
((
pSum
(
dt
)
-
(
p1
(
dt
)
+
point
)).
norm
()
==
0.
);
BOOST_TEST
((
pSumR
(
dt
)
-
(
p1
(
dt
)
+
point
)).
norm
()
==
0.
);
BOOST_TEST
((
pSub
(
dt
)
-
(
p1
(
dt
)
-
point
)).
norm
()
==
0.
);
BOOST_TEST
((
pSubR
(
dt
)
-
(
point
-
p1
(
dt
))).
norm
()
==
0.
);
}
}
BOOST_AUTO_TEST_CASE
(
polynomialOperations
,
*
boost
::
unit_test
::
tolerance
(
0.001
))
{
polynomial_t
::
coeff_t
coeffs1
=
Eigen
::
MatrixXd
::
Random
(
3
,
5
);
polynomial_t
::
coeff_t
coeffs2
=
Eigen
::
MatrixXd
::
Random
(
3
,
2
);
...
...
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