GCC Code Coverage Report

Directory: ./
File: src/extracted-path.hh
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 26 87 29.9%
Branches: 11 74 14.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 #ifndef HPP_CORE_EXTRACTED_PATH_HH
31 #define HPP_CORE_EXTRACTED_PATH_HH
32
33 #include <hpp/core/path.hh>
34
35 namespace hpp {
36 namespace core {
37 /// Result of restriction of a path to a sub interval or of reversion
38 /// \note Decorator design pattern
39 class ExtractedPath : public Path {
40 public:
41 typedef Path parent_t;
42
43 180 virtual ~ExtractedPath() {}
44
45 /// Return a shared pointer to a copy of this
46 ✗ virtual PathPtr_t copy() const { return createCopy(weak_.lock()); }
47
48 /// Return a shared pointer to a copy of this and set constraints
49 ///
50 /// \param constraints constraints to apply to the copy
51 /// \precond *this should not have constraints.
52 ✗ virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
53 ✗ return createCopy(weak_.lock(), constraints);
54 }
55
56 45 static ExtractedPathPtr_t create(const PathPtr_t& original,
57 const interval_t& subInterval) {
58
1/2
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
 45 ExtractedPath* ptr = new ExtractedPath(original, subInterval);
59 45 ExtractedPathPtr_t shPtr(ptr);
60
1/2
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
 45 ptr->init(shPtr);
61 45 return shPtr;
62 }
63
64 ✗ static ExtractedPathPtr_t createCopy(const ExtractedPathPtr_t& path) {
65 ✗ ExtractedPath* ptr = new ExtractedPath(*path);
66 ✗ ExtractedPathPtr_t shPtr(ptr);
67 ✗ ptr->init(shPtr);
68 ✗ return shPtr;
69 }
70
71 ✗ static ExtractedPathPtr_t createCopy(const ExtractedPathPtr_t& path,
72 const ConstraintSetPtr_t& constraints) {
73 ✗ ExtractedPath* ptr = new ExtractedPath(*path, constraints);
74 ✗ ExtractedPathPtr_t shPtr(ptr);
75 ✗ ptr->init(shPtr);
76 ✗ return shPtr;
77 }
78
79 ✗ virtual bool impl_compute(ConfigurationOut_t result, value_type param) const {
80 ✗ param = sInOriginalPath(param);
81 ✗ return original_->impl_compute(result, param);
82 }
83
84 ✗ virtual void impl_derivative(vectorOut_t result, const value_type& s,
85 size_type order) const {
86 ✗ if (reversed_) {
87 ✗ value_type param = (paramRange().second - s);
88 ✗ original_->impl_derivative(result, param, order);
89 ✗ if (order % 2 == 1) result *= -1.;
90 } else {
91 ✗ original_->impl_derivative(result, s, order);
92 }
93 }
94
95 ✗ void impl_velocityBound(vectorOut_t result, const value_type& t0,
96 const value_type& t1) const override {
97 ✗ value_type tmin = sInOriginalPath(t0), tmax = sInOriginalPath(t1);
98 ✗ if (tmin > tmax) std::swap(tmin, tmax);
99 ✗ original_->velocityBound(result, tmin, tmax);
100 }
101
102 ✗ virtual PathPtr_t impl_extract(const interval_t& subInterval) const {
103 ✗ ExtractedPathPtr_t path = createCopy(weak_.lock());
104 ✗ value_type tmin = sInOriginalPath(subInterval.first),
105 ✗ tmax = sInOriginalPath(subInterval.second);
106 ✗ path->reversed_ = tmin > tmax;
107 ✗ if (path->reversed_) std::swap(tmin, tmax);
108 // path->reversed_ = ((this->reversed_) && (!reversed)) ||
109 // ((!this->reversed_) && (reversed));
110 ✗ path->timeParameterization(TimeParameterizationPtr_t(),
111 ✗ std::make_pair(tmin, tmax));
112 ✗ assert(path->timeRange().first >=
113 timeRange().first - std::numeric_limits<float>::epsilon());
114 ✗ assert(path->timeRange().second <=
115 timeRange().second + std::numeric_limits<float>::epsilon());
116 ✗ return path;
117 }
118
119 /// Get the initial configuration
120 36 inline Configuration_t initial() const {
121 bool success;
122 36 return original_->eval(reversed_ ? timeRange().second : timeRange().first,
123
2/4
✗ Branch 1 not taken.
✓ Branch 2 taken 36 times.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 108 success);
124 }
125
126 /// Get the final configuration
127 ✗ inline Configuration_t end() const {
128 bool success;
129 ✗ return original_->eval(reversed_ ? timeRange().first : timeRange().second,
130 ✗ success);
131 }
132
133 protected:
134 /// Print path in a stream
135 ✗ virtual std::ostream& print(std::ostream& os) const {
136 ✗ os << "Extracted Path:" << std::endl;
137 ✗ Path::print(os);
138 ✗ os << "original path:" << std::endl;
139 ✗ os << *original_ << std::endl;
140 ✗ return os;
141 }
142
143 /// Constructor
144 ///
145 /// \param original Path to extract,
146 /// \param subInterval definition interval of the extracted path
147 /// \note If subInterval.first is bigger than subInterval.second, then,
148 /// the path is reversed.
149 45 ExtractedPath(const PathPtr_t& original, const interval_t& subInterval)
150 45 : Path(std::pair<value_type, value_type>(0, 0), original->outputSize(),
151 original->outputDerivativeSize(), original->constraints()),
152
1/2
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
 90 original_(original) {
153 45 reversed_ = subInterval.first <= subInterval.second ? false : true;
154 45 interval_t tr;
155
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 42 times.
 45 if (reversed_) {
156 3 tr.first = subInterval.second;
157 3 tr.second = subInterval.first;
158 } else {
159 42 tr = subInterval;
160 }
161
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 timeRange(tr);
162
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 45 times.
 45 assert(timeRange().first >= original->timeRange().first);
163
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 45 times.
 45 assert(timeRange().second <= original->timeRange().second);
164 45 }
165
166 ✗ ExtractedPath(const ExtractedPath& path)
167 ✗ : Path(path),
168 ✗ original_(path.original_),
169 ✗ reversed_(path.reversed_),
170 ✗ weak_() {}
171
172 ✗ ExtractedPath(const ExtractedPath& path,
173 const ConstraintSetPtr_t& constraints)
174 ✗ : Path(path, constraints),
175 ✗ original_(path.original_),
176 ✗ reversed_(path.reversed_),
177 ✗ weak_() {}
178
179 45 void init(ExtractedPathPtr_t self) {
180
1/2
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
 45 parent_t::init(self);
181 45 weak_ = self;
182 45 }
183
184 /// For serialization only.
185 ✗ ExtractedPath() {}
186
187 private:
188 ✗ inline value_type sInOriginalPath(const value_type& s) const {
189 ✗ assert(paramRange().first <= s && s <= paramRange().second);
190 ✗ if (!reversed_) return s;
191 ✗ return paramRange().second - s;
192 }
193
194 PathPtr_t original_;
195 bool reversed_;
196 ExtractedPathWkPtr_t weak_;
197
198 HPP_SERIALIZABLE();
199 };
200 } // namespace core
201 } // namespace hpp
202 #endif // HPP_CORE_EXTRACTED_PATH_HH
203