Commit 5d4786c9 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Reimplement ConstantCurvature::reverse.

parent 9ef64fe6
......@@ -86,6 +86,8 @@ namespace hpp {
return end_;
}
virtual PathPtr_t reverse () const;
protected:
/// Print path in a stream
virtual std::ostream& print (std::ostream &os) const;
......
......@@ -375,19 +375,42 @@ namespace hpp {
curveLength = ((tmax - tmin)/L * curveLength_);
pathLength = fabs (tmax - tmin)/L * length ();
}
Configuration_t init (robot_->configSize ());
bool res (impl_compute (init, tmin)); assert (res); (void)res;
Configuration_t end (robot_->configSize ());
res = impl_compute (end,tmax); assert (res);
Configuration_t init (robot_->configSize ()),
end (robot_->configSize ());
auto tr = timeRange();
bool res (true);
if (tmin == tr.first) init = initial_;
else if (tmin == tr.second) init = end_;
else res = impl_compute (init, tmin);
assert (res); (void)res;
if (tmax == tr.first) end = initial_;
else if (tmax == tr.second) end = end_;
else res = impl_compute (end,tmax);
assert (res);
ConstantCurvaturePtr_t result (createCopy (weak_.lock ()));
result->initial_ = init;
result->end_ = end;
// swap to avoid memory allocation.
result->initial_.swap(init);
result->end_.swap(end);
result->curveLength_ = curveLength;
result->timeRange (interval_t (0, pathLength));
result->forward_ = curveLength > 0 ? 1 : -1;
return result;
}
PathPtr_t ConstantCurvature::reverse () const
{
assert (!timeParameterization ());
assert (timeRange ().second - timeRange ().first >= 0);
ConstantCurvaturePtr_t result (createCopy (weak_.lock ()));
result->initial_ = end_;
result->end_ = initial_;
result->curveLength_ = - curveLength_;
result->timeRange (interval_t (0, length()));
result->forward_ = curveLength_ < 0 ? 1 : -1;
return result;
}
inline value_type meanBounds(const JointPtr_t& j, const size_type& i)
{
return (j->upperBound(i) + j->lowerBound(i))/2;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment