multi-bound.cpp
Go to the documentation of this file.
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 MultiBound::MultiBound(const double x)
17  : mode(MODE_SINGLE),
18  boundSingle(x),
19  boundSup(0),
20  boundInf(0),
21  boundSupSetup(false),
22  boundInfSetup(false) {}
23 
24 MultiBound::MultiBound(const double xi, const double xs)
25  : mode(MODE_DOUBLE),
26  boundSingle(0),
27  boundSup(xs),
28  boundInf(xi),
29  boundSupSetup(true),
30  boundInfSetup(true) {}
31 
32 MultiBound::MultiBound(const double x, const MultiBound::SupInfType bound)
33  : mode(MODE_DOUBLE),
34  boundSingle(0),
35  boundSup((bound == BOUND_SUP) ? x : 0),
36  boundInf((bound == BOUND_INF) ? x : 0),
37  boundSupSetup(bound == BOUND_SUP),
38  boundInfSetup(bound == BOUND_INF) {}
39 
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 
49 double MultiBound::getSingleBound(void) const {
50  if (MODE_SINGLE != mode) {
52  "Accessing single bound of a non-single type.");
53  }
54  return boundSingle;
55 }
57  if (MODE_DOUBLE != mode) {
59  "Accessing double bound of a non-double type.");
60  }
61  switch (bound) {
62  case BOUND_SUP: {
63  if (!boundSupSetup) {
65  "Accessing un-setup sup bound.");
66  }
67  return boundSup;
68  }
69  case BOUND_INF: {
70  if (!boundInfSetup) {
72  "Accessing un-setup inf bound");
73  }
74  return boundInf;
75  }
76  }
77  return 0;
78 }
80  if (MODE_DOUBLE != mode) {
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 void MultiBound::setDoubleBound(SupInfType boundType, double boundValue) {
93  if (MODE_DOUBLE != mode) {
94  mode = MODE_DOUBLE;
95  boundSupSetup = false;
96  boundInfSetup = false;
97  }
98  switch (boundType) {
99  case BOUND_INF:
100  boundInfSetup = true;
101  boundInf = boundValue;
102  break;
103  case BOUND_SUP:
104  boundSupSetup = true;
105  boundSup = boundValue;
106  break;
107  }
108 }
110  if (MODE_DOUBLE != mode) {
111  mode = MODE_DOUBLE;
112  boundSupSetup = false;
113  boundInfSetup = false;
114  } else {
115  switch (boundType) {
116  case BOUND_INF:
117  boundInfSetup = false;
118  break;
119  case BOUND_SUP:
120  boundSupSetup = false;
121  break;
122  }
123  }
124 }
125 void MultiBound::setSingleBound(double boundValue) {
126  mode = MODE_SINGLE;
127  boundSingle = boundValue;
128 }
129 
130 inline static void SOT_MULTI_BOUND_CHECK_C(std::istream &is, char check,
131  VectorMultiBound &v) {
132  char c;
133  is.get(c);
134  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;
139  "Error parsing vector multi-bound.");
140  }
141 }
142 
143 namespace dynamicgraph {
144 namespace sot {
145 
146 std::ostream &operator<<(std::ostream &os, const MultiBound &m) {
147  switch (m.mode) {
149  os << m.boundSingle;
150  break;
151  }
153  os << "(";
154  if (m.boundInfSetup)
155  os << m.boundInf;
156  else
157  os << "--";
158  os << ",";
159  if (m.boundSupSetup)
160  os << m.boundSup;
161  else
162  os << "--";
163  os << ")";
164  break;
165  }
166  }
167  return os;
168 }
169 
170 std::istream &operator>>(std::istream &is, MultiBound &m) {
171  sotDEBUGIN(15);
172  char c;
173  double val;
174  is.get(c);
175  if (c == '(') {
176  sotDEBUG(15) << "Double" << std::endl;
177  char c2[3];
178  is.get(c2, 3);
179  if (std::string(c2, 2) != "--") {
180  is.putback(c2[1]);
181  is.putback(c2[0]);
182  //{char strbuf[256]; is.getline(strbuf,256); sotDEBUG(1) <<
183  //"#"<<strbuf<<"#"<<std::endl;}
184  is >> val;
185  sotDEBUG(15) << "First val = " << val << std::endl;
186  m.setDoubleBound(MultiBound::BOUND_INF, val);
187  } else {
188  m.unsetDoubleBound(MultiBound::BOUND_INF);
189  }
190  is.get(c);
191  if (c != ',') {
192  sotERROR << "Error while parsing multi-bound. Waiting for a ','. Get '"
193  << c << "' instead. " << std::endl;
196  "Error parsing multi-bound, while waiting for a ','.");
197  }
198 
199  is.get(c2, 3);
200  if (std::string(c2, 2) != "--") {
201  is.putback(c2[1]);
202  is.putback(c2[0]);
203  is >> val;
204  sotDEBUG(15) << "Second val = " << val << std::endl;
205  m.setDoubleBound(MultiBound::BOUND_SUP, val);
206  } else {
207  m.unsetDoubleBound(MultiBound::BOUND_SUP);
208  }
209  is.get(c);
210  if (c != ')') {
211  sotERROR << "Error while parsing multi-bound. Waiting for a ')'. Get '"
212  << c << "' instead. " << std::endl;
215  "Error parsing multi-bound, while waiting for a ')'.");
216  }
217  } else {
218  sotDEBUG(15) << "Single ('" << c << "')" << std::endl;
219  is.putback(c);
220  is >> val;
221  m.setSingleBound(val);
222  }
223 
224  sotDEBUGOUT(15);
225  return is;
226 }
227 
228 std::ostream &operator<<(std::ostream &os, const VectorMultiBound &v) {
229  os << "[" << v.size() << "](";
230  for (VectorMultiBound::const_iterator iter = v.begin(); iter != v.end();
231  ++iter) {
232  if (iter != v.begin()) os << ",";
233  os << (*iter);
234  }
235  return os << ")";
236 }
237 
238 std::istream &operator>>(std::istream &is, VectorMultiBound &v) {
239  std::size_t vali;
240 
241  /* Read the vector size. */
242  SOT_MULTI_BOUND_CHECK_C(is, '[', v);
243  is >> vali;
244  v.resize(vali);
245  SOT_MULTI_BOUND_CHECK_C(is, ']', v);
246 
247  /* Loop for the vals. */
248  SOT_MULTI_BOUND_CHECK_C(is, '(', v);
249  for (std::size_t i = 0; i < vali; ++i) {
250  is >> v[i];
251  if (i != vali - 1) {
252  SOT_MULTI_BOUND_CHECK_C(is, ',', v);
253  } else {
254  SOT_MULTI_BOUND_CHECK_C(is, ')', v);
255  }
256  }
257 
258  return is;
259 }
260 
262  mode = other.mode;
263  boundSingle = other.boundSingle;
264  boundSup = other.boundSup;
265  boundInf = other.boundInf;
268  return *this;
269 }
270 } /* namespace sot */
271 } /* namespace dynamicgraph */
m
float m
dynamicgraph::sot::MultiBound::MODE_DOUBLE
@ MODE_DOUBLE
Definition: multi-bound.hh:37
dynamicgraph::sot::MultiBound::BOUND_INF
@ BOUND_INF
Definition: multi-bound.hh:38
dynamicgraph::sot::operator>>
SOT_CORE_EXPORT std::istream & operator>>(std::istream &os, VectorMultiBound &v)
Definition: multi-bound.cpp:238
dynamicgraph::sot::MultiBound::unsetDoubleBound
void unsetDoubleBound(SupInfType boundType)
Definition: multi-bound.cpp:109
dynamicgraph::sot::MultiBound::boundSup
double boundSup
Definition: multi-bound.hh:43
multi-bound.hh
dynamicgraph
c
Vec3f c
dynamicgraph::sot::VectorMultiBound
std::vector< MultiBound > VectorMultiBound
Definition: multi-bound.hh:72
i
int i
sotERROR
#define sotERROR
Definition: debug.hh:178
dynamicgraph::sot::MultiBound::boundSingle
double boundSingle
Definition: multi-bound.hh:42
dynamicgraph::sot::MultiBound::boundInf
double boundInf
Definition: multi-bound.hh:43
dynamicgraph::sot::ExceptionTask::PARSER_MULTI_BOUND
@ PARSER_MULTI_BOUND
Definition: exception-task.hh:39
debug.hh
sotDEBUGOUT
#define sotDEBUGOUT(level)
Definition: debug.hh:215
dynamicgraph::sot::MultiBound::operator=
MultiBound & operator=(const MultiBound &other)
Definition: multi-bound.cpp:261
dynamicgraph::sot::MultiBound::boundInfSetup
bool boundInfSetup
Definition: multi-bound.hh:44
SOT_MULTI_BOUND_CHECK_C
static void SOT_MULTI_BOUND_CHECK_C(std::istream &is, char check, VectorMultiBound &v)
Definition: multi-bound.cpp:130
dynamicgraph::sot::MultiBound::getMode
MultiBoundModeType getMode(void) const
Definition: multi-bound.cpp:48
dynamicgraph::sot::MultiBound::SupInfType
SupInfType
Definition: multi-bound.hh:38
sotDEBUGIN
#define sotDEBUGIN(level)
Definition: debug.hh:214
x
x
dynamicgraph::sot::MultiBound::MultiBoundModeType
MultiBoundModeType
Definition: multi-bound.hh:37
dynamicgraph::sot::MultiBound::getDoubleBound
double getDoubleBound(const SupInfType bound) const
Definition: multi-bound.cpp:56
dynamicgraph::sot::ExceptionTask
Definition: exception-task.hh:29
dynamicgraph::sot::MultiBound::getSingleBound
double getSingleBound(void) const
Definition: multi-bound.cpp:49
dynamicgraph::sot::MultiBound::getDoubleBoundSetup
bool getDoubleBoundSetup(const SupInfType bound) const
Definition: multi-bound.cpp:79
clone
virtual CollisionGeometry * clone() const=0
dynamicgraph::sot::operator<<
SOT_CORE_EXPORT std::ostream & operator<<(std::ostream &os, const VectorMultiBound &v)
Definition: multi-bound.cpp:228
check
bool check(const T &value, const T &other)
dynamicgraph::sot::MultiBound::setDoubleBound
void setDoubleBound(SupInfType boundType, double boundValue)
Definition: multi-bound.cpp:92
dynamicgraph::sot::MultiBound::setSingleBound
void setSingleBound(double boundValue)
Definition: multi-bound.cpp:125
dynamicgraph::sot::MultiBound::MODE_SINGLE
@ MODE_SINGLE
Definition: multi-bound.hh:37
dynamicgraph::sot::MultiBound
Definition: multi-bound.hh:35
dynamicgraph::sot
v
v
dynamicgraph::sot::MultiBound::BOUND_SUP
@ BOUND_SUP
Definition: multi-bound.hh:38
dynamicgraph::sot::MultiBound::mode
MultiBoundModeType mode
Definition: multi-bound.hh:41
c2
c2
dynamicgraph::sot::MultiBound::boundSupSetup
bool boundSupSetup
Definition: multi-bound.hh:44
SOT_THROW
#define SOT_THROW
Definition: exception-abstract.hh:130
dynamicgraph::sot::ExceptionTask::BOUND_TYPE
@ BOUND_TYPE
Definition: exception-task.hh:38
dynamicgraph::sot::MultiBound::MultiBound
MultiBound(const double x=0.)
Definition: multi-bound.cpp:16
sotDEBUG
#define sotDEBUG(level)
Definition: debug.hh:168


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Tue Oct 24 2023 02:26:31