Directory: | ./ |
---|---|
File: | src/task/multi-bound.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 121 | 172 | 70.3% |
Branches: | 67 | 223 | 30.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Copyright 2010, | ||
3 | * François Bleibel, | ||
4 | * Olivier Stasse, | ||
5 | * | ||
6 | * CNRS/AIST | ||
7 | * | ||
8 | */ | ||
9 | |||
10 | #include <sot/core/multi-bound.hh> | ||
11 | //#define VP_DEBUG_MODE 25 | ||
12 | #include <sot/core/debug.hh> | ||
13 | |||
14 | using namespace dynamicgraph::sot; | ||
15 | |||
16 | 1589 | MultiBound::MultiBound(const double x) | |
17 | 1589 | : mode(MODE_SINGLE), | |
18 | 1589 | boundSingle(x), | |
19 | 1589 | boundSup(0), | |
20 | 1589 | boundInf(0), | |
21 | 1589 | boundSupSetup(false), | |
22 | 1589 | boundInfSetup(false) {} | |
23 | |||
24 | 1 | MultiBound::MultiBound(const double xi, const double xs) | |
25 | 1 | : mode(MODE_DOUBLE), | |
26 | 1 | boundSingle(0), | |
27 | 1 | boundSup(xs), | |
28 | 1 | boundInf(xi), | |
29 | 1 | boundSupSetup(true), | |
30 | 1 | boundInfSetup(true) {} | |
31 | |||
32 | 2 | MultiBound::MultiBound(const double x, const MultiBound::SupInfType bound) | |
33 | 2 | : mode(MODE_DOUBLE), | |
34 | 2 | boundSingle(0), | |
35 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
|
2 | boundSup((bound == BOUND_SUP) ? x : 0), |
36 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
|
2 | boundInf((bound == BOUND_INF) ? x : 0), |
37 | 2 | boundSupSetup(bound == BOUND_SUP), | |
38 | 2 | boundInfSetup(bound == BOUND_INF) {} | |
39 | |||
40 | ✗ | MultiBound::MultiBound(const MultiBound &clone) | |
41 | ✗ | : mode(clone.mode), | |
42 | ✗ | boundSingle(clone.boundSingle), | |
43 | ✗ | boundSup(clone.boundSup), | |
44 | ✗ | boundInf(clone.boundInf), | |
45 | ✗ | boundSupSetup(clone.boundSupSetup), | |
46 | ✗ | boundInfSetup(clone.boundInfSetup) {} | |
47 | |||
48 | ✗ | MultiBound::MultiBoundModeType MultiBound::getMode(void) const { return mode; } | |
49 | 1500 | double MultiBound::getSingleBound(void) const { | |
50 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1500 times.
|
1500 | if (MODE_SINGLE != mode) { |
51 | ✗ | SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE, | |
52 | ✗ | "Accessing single bound of a non-single type."); | |
53 | } | ||
54 | 1500 | return boundSingle; | |
55 | } | ||
56 | ✗ | double MultiBound::getDoubleBound(const MultiBound::SupInfType bound) const { | |
57 | ✗ | if (MODE_DOUBLE != mode) { | |
58 | ✗ | SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE, | |
59 | ✗ | "Accessing double bound of a non-double type."); | |
60 | } | ||
61 | ✗ | switch (bound) { | |
62 | ✗ | case BOUND_SUP: { | |
63 | ✗ | if (!boundSupSetup) { | |
64 | ✗ | SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE, | |
65 | ✗ | "Accessing un-setup sup bound."); | |
66 | } | ||
67 | ✗ | return boundSup; | |
68 | } | ||
69 | ✗ | case BOUND_INF: { | |
70 | ✗ | if (!boundInfSetup) { | |
71 | ✗ | SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE, | |
72 | ✗ | "Accessing un-setup inf bound"); | |
73 | } | ||
74 | ✗ | return boundInf; | |
75 | } | ||
76 | } | ||
77 | ✗ | return 0; | |
78 | } | ||
79 | ✗ | bool MultiBound::getDoubleBoundSetup(const MultiBound::SupInfType bound) const { | |
80 | ✗ | if (MODE_DOUBLE != mode) { | |
81 | ✗ | SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE, | |
82 | ✗ | "Accessing double bound of a non-double type."); | |
83 | } | ||
84 | ✗ | switch (bound) { | |
85 | ✗ | case BOUND_SUP: | |
86 | ✗ | return boundSupSetup; | |
87 | ✗ | case BOUND_INF: | |
88 | ✗ | return boundInfSetup; | |
89 | } | ||
90 | ✗ | return false; | |
91 | } | ||
92 | 8 | void MultiBound::setDoubleBound(SupInfType boundType, double boundValue) { | |
93 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 6 times.
|
8 | if (MODE_DOUBLE != mode) { |
94 | 2 | mode = MODE_DOUBLE; | |
95 | 2 | boundSupSetup = false; | |
96 | 2 | boundInfSetup = false; | |
97 | } | ||
98 |
2/3✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | switch (boundType) { |
99 | 4 | case BOUND_INF: | |
100 | 4 | boundInfSetup = true; | |
101 | 4 | boundInf = boundValue; | |
102 | 4 | break; | |
103 | 4 | case BOUND_SUP: | |
104 | 4 | boundSupSetup = true; | |
105 | 4 | boundSup = boundValue; | |
106 | 4 | break; | |
107 | } | ||
108 | 8 | } | |
109 | 4 | void MultiBound::unsetDoubleBound(SupInfType boundType) { | |
110 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
|
4 | if (MODE_DOUBLE != mode) { |
111 | 1 | mode = MODE_DOUBLE; | |
112 | 1 | boundSupSetup = false; | |
113 | 1 | boundInfSetup = false; | |
114 | } else { | ||
115 |
2/3✓ Branch 0 taken 1 times.
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
3 | switch (boundType) { |
116 | 1 | case BOUND_INF: | |
117 | 1 | boundInfSetup = false; | |
118 | 1 | break; | |
119 | 2 | case BOUND_SUP: | |
120 | 2 | boundSupSetup = false; | |
121 | 2 | break; | |
122 | } | ||
123 | } | ||
124 | 4 | } | |
125 | 2 | void MultiBound::setSingleBound(double boundValue) { | |
126 | 2 | mode = MODE_SINGLE; | |
127 | 2 | boundSingle = boundValue; | |
128 | 2 | } | |
129 | |||
130 | 7 | inline static void SOT_MULTI_BOUND_CHECK_C(std::istream &is, char check, | |
131 | VectorMultiBound &v) { | ||
132 | char c; | ||
133 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | is.get(c); |
134 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
|
7 | if (c != check) { |
135 | ✗ | v.resize(0); | |
136 | ✗ | sotERROR << "Error while parsing vector multi-bound. Waiting for a '" | |
137 | ✗ | << check << "'. Get '" << c << "' instead. " << std::endl; | |
138 | ✗ | SOT_THROW ExceptionTask(ExceptionTask::PARSER_MULTI_BOUND, | |
139 | ✗ | "Error parsing vector multi-bound."); | |
140 | } | ||
141 | 7 | } | |
142 | |||
143 | namespace dynamicgraph { | ||
144 | namespace sot { | ||
145 | |||
146 | 44 | std::ostream &operator<<(std::ostream &os, const MultiBound &m) { | |
147 |
2/3✓ Branch 0 taken 29 times.
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
44 | switch (m.mode) { |
148 | 29 | case MultiBound::MODE_SINGLE: { | |
149 | 29 | os << m.boundSingle; | |
150 | 29 | break; | |
151 | } | ||
152 | 15 | case MultiBound::MODE_DOUBLE: { | |
153 | 15 | os << "("; | |
154 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 5 times.
|
15 | if (m.boundInfSetup) |
155 | 10 | os << m.boundInf; | |
156 | else | ||
157 | 5 | os << "--"; | |
158 | 15 | os << ","; | |
159 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 5 times.
|
15 | if (m.boundSupSetup) |
160 | 10 | os << m.boundSup; | |
161 | else | ||
162 | 5 | os << "--"; | |
163 | 15 | os << ")"; | |
164 | 15 | break; | |
165 | } | ||
166 | } | ||
167 | 44 | return os; | |
168 | } | ||
169 | |||
170 | 8 | std::istream &operator>>(std::istream &is, MultiBound &m) { | |
171 | sotDEBUGIN(15); | ||
172 | char c; | ||
173 | double val; | ||
174 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | is.get(c); |
175 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 2 times.
|
8 | if (c == '(') { |
176 | sotDEBUG(15) << "Double" << std::endl; | ||
177 | char c2[3]; | ||
178 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | is.get(c2, 3); |
179 |
3/4✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 4 times.
✓ Branch 8 taken 2 times.
|
6 | if (std::string(c2, 2) != "--") { |
180 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | is.putback(c2[1]); |
181 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | is.putback(c2[0]); |
182 | //{char strbuf[256]; is.getline(strbuf,256); sotDEBUG(1) << | ||
183 | //"#"<<strbuf<<"#"<<std::endl;} | ||
184 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | is >> val; |
185 | sotDEBUG(15) << "First val = " << val << std::endl; | ||
186 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | m.setDoubleBound(MultiBound::BOUND_INF, val); |
187 | } else { | ||
188 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | m.unsetDoubleBound(MultiBound::BOUND_INF); |
189 | } | ||
190 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | is.get(c); |
191 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
|
6 | if (c != ',') { |
192 | ✗ | sotERROR << "Error while parsing multi-bound. Waiting for a ','. Get '" | |
193 | ✗ | << c << "' instead. " << std::endl; | |
194 | ✗ | SOT_THROW ExceptionTask( | |
195 | ✗ | ExceptionTask::PARSER_MULTI_BOUND, | |
196 | ✗ | "Error parsing multi-bound, while waiting for a ','."); | |
197 | } | ||
198 | |||
199 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | is.get(c2, 3); |
200 |
3/4✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 4 times.
✓ Branch 8 taken 2 times.
|
6 | if (std::string(c2, 2) != "--") { |
201 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | is.putback(c2[1]); |
202 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | is.putback(c2[0]); |
203 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | is >> val; |
204 | sotDEBUG(15) << "Second val = " << val << std::endl; | ||
205 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | m.setDoubleBound(MultiBound::BOUND_SUP, val); |
206 | } else { | ||
207 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | m.unsetDoubleBound(MultiBound::BOUND_SUP); |
208 | } | ||
209 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | is.get(c); |
210 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
|
6 | if (c != ')') { |
211 | ✗ | sotERROR << "Error while parsing multi-bound. Waiting for a ')'. Get '" | |
212 | ✗ | << c << "' instead. " << std::endl; | |
213 | ✗ | SOT_THROW ExceptionTask( | |
214 | ✗ | ExceptionTask::PARSER_MULTI_BOUND, | |
215 | ✗ | "Error parsing multi-bound, while waiting for a ')'."); | |
216 | } | ||
217 | } else { | ||
218 | sotDEBUG(15) << "Single ('" << c << "')" << std::endl; | ||
219 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | is.putback(c); |
220 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | is >> val; |
221 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | m.setSingleBound(val); |
222 | } | ||
223 | |||
224 | sotDEBUGOUT(15); | ||
225 | 8 | return is; | |
226 | } | ||
227 | |||
228 | 5 | std::ostream &operator<<(std::ostream &os, const VectorMultiBound &v) { | |
229 | 5 | os << "[" << v.size() << "]("; | |
230 |
2/2✓ Branch 3 taken 28 times.
✓ Branch 4 taken 5 times.
|
33 | for (VectorMultiBound::const_iterator iter = v.begin(); iter != v.end(); |
231 | 28 | ++iter) { | |
232 |
3/4✓ Branch 2 taken 23 times.
✓ Branch 3 taken 5 times.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
|
28 | if (iter != v.begin()) os << ","; |
233 |
1/2✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
|
28 | os << (*iter); |
234 | } | ||
235 | 5 | return os << ")"; | |
236 | } | ||
237 | |||
238 | 1 | std::istream &operator>>(std::istream &is, VectorMultiBound &v) { | |
239 | unsigned int vali; | ||
240 | |||
241 | /* Read the vector size. */ | ||
242 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | SOT_MULTI_BOUND_CHECK_C(is, '[', v); |
243 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | is >> vali; |
244 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | v.resize(vali); |
245 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | SOT_MULTI_BOUND_CHECK_C(is, ']', v); |
246 | |||
247 | /* Loop for the vals. */ | ||
248 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | SOT_MULTI_BOUND_CHECK_C(is, '(', v); |
249 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
5 | for (unsigned int i = 0; i < vali; ++i) { |
250 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | is >> v[i]; |
251 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
|
4 | if (i != vali - 1) { |
252 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | SOT_MULTI_BOUND_CHECK_C(is, ',', v); |
253 | } else { | ||
254 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | SOT_MULTI_BOUND_CHECK_C(is, ')', v); |
255 | } | ||
256 | } | ||
257 | |||
258 | 1 | return is; | |
259 | } | ||
260 | |||
261 | } /* namespace sot */ | ||
262 | } /* namespace dynamicgraph */ | ||
263 |