GCC Code Coverage Report

Directory: ./
File: src/path-vector.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 83 123 67.5%
Branches: 65 186 34.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <boost/serialization/vector.hpp>
31 #include <boost/serialization/weak_ptr.hpp>
32 #include <hpp/core/path-vector.hh>
33 #include <hpp/util/indent.hh>
34 #include <hpp/util/serialization.hh>
35 #include <stdexcept>
36
37 namespace hpp {
38 namespace core {
39
40 2385 std::size_t PathVector::rankAtParam(const value_type& param,
41 value_type& localParam) const {
42
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2385 times.
 2385 assert(!timeParameterization());
43
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 2385 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 2385 if (paths_.empty()) throw std::runtime_error("PathVector is empty.");
44 2385 std::size_t res = 0;
45 2385 localParam = param;
46 2385 bool finished = false;
47
48
6/6
✓ Branch 1 taken 6414 times.
✓ Branch 2 taken 623 times.
✓ Branch 3 taken 4652 times.
✓ Branch 4 taken 1762 times.
✓ Branch 5 taken 4652 times.
✓ Branch 6 taken 2385 times.
 7037 while (res + 1 < paths_.size() && !finished) {
49
2/2
✓ Branch 3 taken 2890 times.
✓ Branch 4 taken 1762 times.
 4652 if (localParam > paths_[res]->length()) {
50 2890 localParam -= paths_[res]->length();
51 2890 res++;
52 } else {
53 1762 finished = true;
54 }
55 }
56
2/2
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 2382 times.
 2385 if (localParam > paths_[res]->length()) {
57
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 3 if (res != paths_.size() - 1) {
58 ✗ throw std::runtime_error("localparam out of range.");
59 }
60 3 localParam = paths_[res]->timeRange().second;
61 } else {
62 2382 localParam += paths_[res]->timeRange().first;
63 }
64
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 2385 times.
 2385 assert(localParam >= paths_[res]->timeRange().first -
65 std::numeric_limits<float>::epsilon());
66
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 2385 times.
 2385 assert(localParam <= paths_[res]->timeRange().second +
67 std::numeric_limits<float>::epsilon());
68 2385 return res;
69 }
70
71 1852848 void PathVector::appendPath(const PathPtr_t& path) {
72
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1852848 times.
 1852848 assert(!timeParameterization());
73
1/2
✓ Branch 1 taken 1852848 times.
✗ Branch 2 not taken.
 1852848 paths_.push_back(path);
74 1852848 interval_t tr = timeRange();
75
1/2
✓ Branch 2 taken 1852848 times.
✗ Branch 3 not taken.
 1852848 tr.second += path->length();
76
1/2
✓ Branch 1 taken 1852848 times.
✗ Branch 2 not taken.
 1852848 timeRange(tr);
77 1852848 }
78
79 926604 PathPtr_t PathVector::pathAtRank(std::size_t rank) const {
80 926604 PathPtr_t copy;
81
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 926604 times.
 926604 if (constraints()) {
82 ✗ if (paths_[rank]->constraints()) {
83 ✗ throw std::runtime_error(
84 "Attempt to extract a path from a path vector where both "
85 ✗ "are subject to constraints. This is not supported.");
86 } else {
87 ✗ ConstraintPtr_t constraintCopy(constraints()->copy());
88 ✗ HPP_STATIC_CAST_REF_CHECK(ConstraintSet, *constraintCopy);
89 ✗ copy = paths_[rank]->copy(
90 ✗ HPP_STATIC_PTR_CAST(ConstraintSet, constraintCopy));
91 }
92 } else {
93
1/2
✓ Branch 3 taken 926604 times.
✗ Branch 4 not taken.
 926604 copy = paths_[rank]->copy();
94 }
95 926604 return copy;
96 }
97
98 64 void PathVector::concatenate(const PathVectorPtr_t& path) {
99
2/2
✓ Branch 2 taken 926296 times.
✓ Branch 3 taken 64 times.
 926360 for (std::size_t i = 0; i < path->numberPaths(); ++i) {
100
2/4
✓ Branch 4 taken 926296 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 926296 times.
✗ Branch 8 not taken.
 926296 appendPath(path->pathAtRank(i)->copy());
101 }
102 64 }
103
104 15 void PathVector::flatten(PathVectorPtr_t p) const {
105
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 15 times.
 15 assert(!timeParameterization());
106
2/2
✓ Branch 1 taken 51 times.
✓ Branch 2 taken 15 times.
 66 for (std::size_t i = 0; i < numberPaths(); ++i) {
107
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 PathPtr_t path = pathAtRank(i);
108 51 PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST(PathVector, path);
109
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 51 times.
 51 if (pv)
110 ✗ pv->flatten(p);
111 else
112
1/2
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
 51 p->appendPath(path);
113 51 }
114 15 }
115
116 720 bool PathVector::impl_compute(ConfigurationOut_t result,
117 value_type param) const {
118
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 720 times.
 720 assert(!timeParameterization());
119 // Find direct path in vector corresponding to parameter.
120 size_t rank;
121 value_type localParam;
122
1/2
✓ Branch 1 taken 720 times.
✗ Branch 2 not taken.
 720 rank = rankAtParam(param, localParam);
123
124 720 PathPtr_t subpath = paths_[rank];
125
2/4
✓ Branch 2 taken 720 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 720 times.
✗ Branch 6 not taken.
 1440 return subpath->eval(result, localParam);
126 720 }
127
128 1653 void PathVector::impl_derivative(vectorOut_t result, const value_type& param,
129 size_type order) const {
130
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1653 times.
 1653 assert(!timeParameterization());
131 // Find direct path in vector corresponding to parameter.
132 size_t rank;
133 value_type localParam;
134
1/2
✓ Branch 1 taken 1653 times.
✗ Branch 2 not taken.
 1653 rank = rankAtParam(param, localParam);
135
136 1653 PathPtr_t subpath = paths_[rank];
137
2/4
✓ Branch 2 taken 1653 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1653 times.
✗ Branch 6 not taken.
 1653 subpath->derivative(result, localParam, order);
138 1653 }
139
140 4 inline const value_type& Iinit(const interval_t& I, bool reverse) {
141
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
 4 return (reverse ? I.second : I.first);
142 }
143 4 inline const value_type& Iend(const interval_t& I, bool reverse) {
144
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
 4 return (reverse ? I.first : I.second);
145 }
146
147 ✗ void PathVector::impl_velocityBound(vectorOut_t bound, const value_type& param0,
148 const value_type& param1) const {
149 ✗ assert(!timeParameterization());
150
151 ✗ bool reversed = param0 > param1 ? true : false;
152 value_type localtinit, localtend;
153 ✗ std::size_t iinit = rankAtParam(param0, localtinit),
154 ✗ iend = rankAtParam(param1, localtend);
155 ✗ if (iinit == iend) {
156 ✗ paths_[iinit]->velocityBound(bound, localtinit, localtend);
157 } else {
158 ✗ paths_[iinit]->velocityBound(bound, localtinit,
159 ✗ Iend(paths_[iinit]->timeRange(), reversed));
160 ✗ int one = (reversed ? -1 : 1);
161 ✗ vector_t localbound(vector_t::Zero(bound.size()));
162 ✗ for (std::size_t i = iinit + one; (reversed && i > iend) || i < iend;
163 ✗ i += one) {
164 ✗ paths_[i]->velocityBound(localbound, paths_[i]->timeRange().first,
165 ✗ paths_[i]->timeRange().second);
166 ✗ bound = bound.cwiseMax(localbound);
167 }
168 ✗ paths_[iend]->velocityBound(
169 ✗ localbound, Iinit(paths_[iend]->timeRange(), reversed), localtend);
170 ✗ bound = bound.cwiseMax(localbound);
171 }
172 }
173
174 6 PathPtr_t PathVector::impl_extract(const interval_t& subInterval) const {
175
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
 6 assert(!timeParameterization());
176
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 PathVectorPtr_t path = create(outputSize(), outputDerivativeSize());
177 6 bool reversed = subInterval.first > subInterval.second ? true : false;
178
179 value_type localtinit, localtend;
180
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 std::size_t iinit = rankAtParam(subInterval.first, localtinit),
181
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 iend = rankAtParam(subInterval.second, localtend);
182
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 4 times.
 6 if (iinit == iend) {
183
2/4
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 path->appendPath(paths_[iinit]->extract(localtinit, localtend));
184 } else {
185
2/4
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 path->appendPath(paths_[iinit]->extract(
186 4 localtinit, Iend(paths_[iinit]->timeRange(), reversed)));
187
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
 4 int one = (reversed ? -1 : 1);
188
4/6
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 4 times.
 4 for (std::size_t i = iinit + one; (reversed && i > iend) || i < iend;
189 ✗ i += one)
190 ✗ path->appendPath(reversed ? paths_[i]->reverse() : paths_[i]);
191
2/4
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 path->appendPath(paths_[iend]->extract(
192 4 Iinit(paths_[iend]->timeRange(), reversed), localtend));
193 }
194 12 return path;
195 6 }
196
197 3 PathPtr_t PathVector::reverse() const {
198
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 3 assert(!timeParameterization());
199
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 PathVectorPtr_t ret = create(outputSize(), outputDerivativeSize());
200
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 std::for_each(paths_.rbegin(), paths_.rend(), [&ret](const PathPtr_t& path) {
201
1/2
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 ret->appendPath(path->reverse());
202 8 });
203 6 return ret;
204 3 }
205
206 ✗ std::ostream& PathVector::print(std::ostream& os) const {
207 ✗ Path::print(os << "PathVector:") << incendl;
208 ✗ for (Paths_t::const_iterator itPath = paths_.begin(); itPath != paths_.end();
209 ✗ ++itPath) {
210 ✗ os << (**itPath) << iendl;
211 }
212 ✗ return os << decindent;
213 }
214
215 template <class Archive>
216 ✗ void PathVector::serialize(Archive& ar, const unsigned int version) {
217 using namespace boost::serialization;
218 (void)version;
219 ✗ ar& make_nvp("base", base_object<Path>(*this));
220 ✗ ar& BOOST_SERIALIZATION_NVP(paths_);
221 ✗ ar& BOOST_SERIALIZATION_NVP(weak_);
222 }
223
224 HPP_SERIALIZATION_IMPLEMENT(PathVector);
225 } // namespace core
226 } // namespace hpp
227
228 BOOST_CLASS_EXPORT_IMPLEMENT(hpp::core::PathVector)
229