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
7884ae26
Unverified
Commit
7884ae26
authored
Feb 10, 2020
by
Fernbach Pierre
Committed by
GitHub
Feb 10, 2020
Browse files
Merge pull request #32 from pFernbach/devel
Fix when t_min == t_max
parents
8d201862
7f8ae09c
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/curves/polynomial.h
View file @
7884ae26
...
...
@@ -106,6 +106,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
///
polynomial
(
const
Point
&
init
,
const
Point
&
end
,
const
time_t
min
,
const
time_t
max
)
:
dim_
(
init
.
size
()),
degree_
(
1
),
T_min_
(
min
),
T_max_
(
max
)
{
if
(
T_min_
>=
T_max_
)
throw
std
::
invalid_argument
(
"T_min must be strictly lower than T_max"
);
if
(
init
.
size
()
!=
end
.
size
())
throw
std
::
invalid_argument
(
"init and end points must have the same dimensions."
);
t_point_t
coeffs
;
coeffs
.
push_back
(
init
);
...
...
@@ -127,6 +128,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
polynomial
(
const
Point
&
init
,
const
Point
&
d_init
,
const
Point
&
end
,
const
Point
&
d_end
,
const
time_t
min
,
const
time_t
max
)
:
dim_
(
init
.
size
()),
degree_
(
3
),
T_min_
(
min
),
T_max_
(
max
)
{
if
(
T_min_
>=
T_max_
)
throw
std
::
invalid_argument
(
"T_min must be strictly lower than T_max"
);
if
(
init
.
size
()
!=
end
.
size
())
throw
std
::
invalid_argument
(
"init and end points must have the same dimensions."
);
if
(
init
.
size
()
!=
d_init
.
size
())
throw
std
::
invalid_argument
(
"init and d_init points must have the same dimensions."
);
...
...
@@ -170,6 +172,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
polynomial
(
const
Point
&
init
,
const
Point
&
d_init
,
const
Point
&
dd_init
,
const
Point
&
end
,
const
Point
&
d_end
,
const
Point
&
dd_end
,
const
time_t
min
,
const
time_t
max
)
:
dim_
(
init
.
size
()),
degree_
(
5
),
T_min_
(
min
),
T_max_
(
max
)
{
if
(
T_min_
>=
T_max_
)
throw
std
::
invalid_argument
(
"T_min must be strictly lower than T_max"
);
if
(
init
.
size
()
!=
end
.
size
())
throw
std
::
invalid_argument
(
"init and end points must have the same dimensions."
);
if
(
init
.
size
()
!=
d_init
.
size
())
throw
std
::
invalid_argument
(
"init and d_init points must have the same dimensions."
);
...
...
include/curves/so3_linear.h
View file @
7884ae26
...
...
@@ -38,7 +38,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
dim_
(
3
),
init_rot_
(
init_rot
),
end_rot_
(
end_rot
),
angular_vel_
(
log3
(
init_rot
.
toRotationMatrix
()
.
transpose
()
*
end_rot
.
toRotationMatrix
()
)
/
(
t_max
-
t_m
in
)),
angular_vel_
(
computeAngularVelocity
(
init_rot
.
toRotationMatrix
()
,
end_rot
.
toRotationMatrix
()
,
t_min
,
t_m
ax
)),
T_min_
(
t_min
),
T_max_
(
t_max
)
{
safe_check
();
...
...
@@ -50,7 +50,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
dim_
(
3
),
init_rot_
(
quaternion_t
(
init_rot
)),
end_rot_
(
quaternion_t
(
end_rot
)),
angular_vel_
(
log3
(
init_rot
.
transpose
()
*
end_rot
)
/
(
t_max
-
t_m
in
)),
angular_vel_
(
computeAngularVelocity
(
init_rot
,
end_rot
,
t_min
,
t_m
ax
)),
T_min_
(
t_min
),
T_max_
(
t_max
)
{
safe_check
();
...
...
@@ -62,7 +62,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
dim_
(
3
),
init_rot_
(
init_rot
),
end_rot_
(
end_rot
),
angular_vel_
(
log3
(
init_rot
.
toRotationMatrix
()
.
transpose
()
*
end_rot
.
toRotationMatrix
())),
angular_vel_
(
computeAngularVelocity
(
init_rot
.
toRotationMatrix
()
,
end_rot
.
toRotationMatrix
()
,
0.
,
1.
)),
T_min_
(
0.
),
T_max_
(
1.
)
{
safe_check
();
...
...
@@ -74,7 +74,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
dim_
(
3
),
init_rot_
(
quaternion_t
(
init_rot
)),
end_rot_
(
quaternion_t
(
end_rot
)),
angular_vel_
(
log3
(
init_rot
.
transpose
()
*
end_rot
)),
angular_vel_
(
computeAngularVelocity
(
init_rot
,
end_rot
,
0.
,
1.
)),
T_min_
(
0.
),
T_max_
(
1.
)
{
safe_check
();
...
...
@@ -93,12 +93,20 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
T_min_
(
other
.
T_min_
),
T_max_
(
other
.
T_max_
)
{}
point3_t
computeAngularVelocity
(
const
matrix3_t
&
init_rot
,
const
matrix3_t
&
end_rot
,
const
double
t_min
,
const
double
t_max
){
if
(
t_min
==
t_max
){
return
point3_t
::
Zero
();
}
else
{
return
log3
(
init_rot
.
transpose
()
*
end_rot
)
/
(
t_max
-
t_min
);
}
}
quaternion_t
computeAsQuaternion
(
const
time_t
t
)
const
{
if
(
Safe
&
!
(
T_min_
<=
t
&&
t
<=
T_max_
))
{
throw
std
::
invalid_argument
(
"can't evaluate bezier curve, time t is out of range"
);
// TODO
}
if
(
t
>
T_max_
)
return
end_rot_
;
if
(
t
<
T_min_
)
return
init_rot_
;
if
(
t
>
=
T_max_
)
return
end_rot_
;
if
(
t
<
=
T_min_
)
return
init_rot_
;
Scalar
u
=
(
t
-
T_min_
)
/
(
T_max_
-
T_min_
);
return
init_rot_
.
slerp
(
u
,
end_rot_
);
}
...
...
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