GCC Code Coverage Report


Directory: ./
File: src/path-projector/dichotomy.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 0 80 0.0%
Branches: 0 191 0.0%

Line Branch Exec Source
1 // Copyright (c) 2014, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include "hpp/core/path-projector/dichotomy.hh"
30
31 #include <hpp/core/config-projector.hh>
32 #include <hpp/core/path-vector.hh>
33 #include <hpp/core/straight-path.hh>
34 #include <queue>
35 #include <stack>
36 #include <stdexcept>
37
38 namespace hpp {
39 namespace core {
40 namespace pathProjector {
41 Dichotomy::Dichotomy(const DistancePtr_t& distance,
42 const SteeringMethodPtr_t& steeringMethod,
43 value_type maxPathLength)
44 : PathProjector(distance, steeringMethod), maxPathLength_(maxPathLength) {}
45
46 bool Dichotomy::impl_apply(const PathPtr_t& path, PathPtr_t& proj) const {
47 assert(path);
48 bool success = false;
49 PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST(PathVector, path);
50 if (!pv) {
51 StraightPathPtr_t sp = HPP_DYNAMIC_PTR_CAST(StraightPath, path);
52 if (!sp) throw std::invalid_argument("Unknow inherited class of Path");
53 success = applyToStraightPath(sp, proj);
54 } else {
55 PathVectorPtr_t res =
56 PathVector::create(pv->outputSize(), pv->outputDerivativeSize());
57 PathPtr_t part;
58 success = true;
59 for (size_t i = 0; i < pv->numberPaths(); i++) {
60 if (!apply(pv->pathAtRank(i), part)) {
61 // We add the path only if part is not NULL and:
62 // - either its length is not zero,
63 // - or it's not the first one.
64 if (part && (part->length() > 0 || i == 0)) {
65 res->appendPath(part);
66 }
67 success = false;
68 break;
69 }
70 res->appendPath(part);
71 }
72 proj = res;
73 }
74 assert(proj);
75 assert((proj->initial() - path->initial()).isZero());
76 assert(!success || (proj->end() - path->end()).isZero());
77 return success;
78 }
79
80 bool Dichotomy::applyToStraightPath(const StraightPathPtr_t& path,
81 PathPtr_t& projection) const {
82 ConstraintSetPtr_t constraints = path->constraints();
83 const ConfigProjectorPtr_t& cp = constraints->configProjector();
84 core::interval_t timeRange = path->timeRange();
85 const Configuration_t& q1 = path->initial();
86 const Configuration_t& q2 = path->end();
87 if (cp) cp->rightHandSideFromConfig(q1);
88 if (!constraints->isSatisfied(q1) || !constraints->isSatisfied(q2)) {
89 return false;
90 }
91 if (!cp || cp->dimension() == 0) {
92 projection = path;
93 return true;
94 }
95
96 bool pathIsFullyProjected = true;
97 std::queue<PathPtr_t> paths;
98 std::stack<PathPtr_t> pathToSplit;
99 Configuration_t qi(q1.size());
100 PathPtr_t sPath = steer(q1, q2);
101 pathToSplit.push(sPath);
102 while (!pathToSplit.empty()) {
103 sPath = pathToSplit.top();
104 pathToSplit.pop();
105 double l = sPath->length();
106 if (l < maxPathLength_) {
107 paths.push(sPath);
108 continue;
109 }
110 timeRange = sPath->timeRange();
111 const Configuration_t& qb = sPath->initial();
112 sPath->eval(qi, timeRange.first + l / 2);
113 const Configuration_t& qe = sPath->end();
114 if (!constraints->apply(qi)) {
115 pathIsFullyProjected = false;
116 break;
117 }
118 PathPtr_t firstPart = steer(qb, qi);
119 PathPtr_t secondPart = steer(qi, qe);
120 if (secondPart->length() == 0 || firstPart->length() == 0) {
121 pathIsFullyProjected = false;
122 break;
123 }
124 pathToSplit.push(secondPart);
125 pathToSplit.push(firstPart);
126 }
127 switch (paths.size()) {
128 case 0:
129 return false;
130 break;
131 case 1:
132 projection = paths.front()->copy(constraints);
133 break;
134 default:
135 core::PathVectorPtr_t pv = core::PathVector::create(
136 path->outputSize(), path->outputDerivativeSize());
137 while (!paths.empty()) {
138 pv->appendPath(paths.front()->copy(constraints));
139 paths.pop();
140 }
141 projection = pv;
142 break;
143 }
144 return pathIsFullyProjected;
145 }
146 } // namespace pathProjector
147 } // namespace core
148 } // namespace hpp
149