From 8a89707ba0a0ff4d3ffac2620df39b311dfdb1cf Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 28 Nov 2014 15:07:42 +0100
Subject: [PATCH] Fix bug in PathProjector.
---
src/path-projector.cc | 20 ++++++++++++++------
1 file changed, 14 insertions(+), 6 deletions(-)
diff --git a/src/path-projector.cc b/src/path-projector.cc
index 3424c6cb..40a43443 100644
--- a/src/path-projector.cc
+++ b/src/path-projector.cc
@@ -38,26 +38,34 @@ namespace hpp {
bool PathProjector::apply (const PathPtr_t path, PathPtr_t& proj) const
{
assert (path);
+ bool success = false;
PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST (PathVector, path);
if (!pv) {
StraightPathPtr_t sp = HPP_DYNAMIC_PTR_CAST (StraightPath, path);
if (!sp) throw std::invalid_argument ("Unknow inherited class of Path");
- return impl_apply (sp, proj);
- }
- else {
+ success = impl_apply (sp, proj);
+ } else {
PathVectorPtr_t res = PathVector::create (pv->outputSize());
PathPtr_t part;
+ success = true;
for (size_t i = 0; i < pv->numberPaths (); i++) {
if (!apply (pv->pathAtRank (i), part)) {
- if (part && part->length () > 0) res->appendPath (part);
- return false;
+ // We add the path only if part is not NULL and:
+ // - either its length is not zero,
+ // - or it's not the first one.
+ if (part && (part->length () > 0 || i == 0)) res->appendPath (part);
+ success = false;
+ break;
}
res->appendPath (part);
}
+ //if (res->numberPaths ()==0) proj = path.extract (std::make_pair ());
proj = res;
}
assert (proj);
- return true;
+ assert (((*proj)(proj->timeRange ().first) - (*path)(path->timeRange ().first)).isZero());
+ assert (!success || ((*proj)(proj->timeRange ().second) - (*path)(path->timeRange ().second)).isZero());
+ return success;
}
} // namespace manipulation
} // namespace hpp
--
GitLab