WFMath  1.0.1
atlasconv.h
1 // atlasconv.h (Functions to convert WFMath library object to/from an Atlas Message)
2 //
3 // The WorldForge Project
4 // Copyright (C) 2001 The WorldForge Project
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU General Public License as published by
8 // the Free Software Foundation; either version 2 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 //
20 // For information about WorldForge and its authors, please contact
21 // the Worldforge Web Site at http://www.worldforge.org.
22 
23 // Author: Ron Steinke
24 // Created: 2001-12-11
25 
26 // Since we don't want WFMath and Atlas to depend on each other,
27 // we're putting all the atlas interface functions into this header.
28 
29 // WARNING! WARNING! Do not include this file in any other file in wfmath.
30 
31 #ifndef WFMATH_ATLAS_CONV_H
32 #define WFMATH_ATLAS_CONV_H
33 
34 #include <wfmath/point.h>
35 #include <wfmath/vector.h>
36 #include <wfmath/quaternion.h>
37 #include <wfmath/axisbox.h>
38 #include <wfmath/polygon.h>
39 #include <wfmath/ball.h>
40 #include <wfmath/rotbox.h>
41 #include <wfmath/line.h>
42 
43 #include <cmath>
44 
45 namespace WFMath {
46 
47 #ifndef ATLAS_MESSAGE_ELEMENT_H
48 #error "You must include Atlas/Message/Element.h before wfmath/atlasconv.h"
49 #endif
50 
51 typedef Atlas::Message::WrongTypeException _AtlasBadParse;
52 
53 class AtlasInType
54 {
55  public:
56  AtlasInType(const Atlas::Message::Element& val) : m_val(val) {}
57  // allow nice conversions when necessary
58  template<class C> AtlasInType(C c) : m_obj(c), m_val(m_obj) {}
59  operator const Atlas::Message::Element&() const {return m_val;}
60  bool IsList() const {return m_val.isList();}
61  const Atlas::Message::ListType& AsList() const {return m_val.asList();}
62  private:
63  Atlas::Message::Element m_obj;
64  const Atlas::Message::Element& m_val;
65 };
66 
67 class AtlasOutType
68 {
69  public:
70  AtlasOutType(const Atlas::Message::ListType& l) : m_val(l) {}
71  AtlasOutType(const Atlas::Message::MapType& l) : m_val(l) {}
72  operator Atlas::Message::Element&() {return m_val;}
73  operator const Atlas::Message::Element&() const {return m_val;}
74  private:
75  Atlas::Message::Element m_val;
76 };
77 
78 inline AtlasOutType _ArrayToAtlas(const CoordType* array, unsigned len)
79 {
80  Atlas::Message::ListType a(len);
81 
82  for(unsigned i = 0; i < len; ++i)
83  a[i] = array[i];
84 
85  return a;
86 }
87 
88 inline void _ArrayFromAtlas(CoordType* array, unsigned len, const AtlasInType& a)
89 {
90  if(!a.IsList())
91  throw _AtlasBadParse();
92 
93  const Atlas::Message::ListType& list(a.AsList());
94 
95  if(list.size() != (unsigned int) len)
96  throw _AtlasBadParse();
97 
98  for(unsigned i = 0; i < len; ++i)
99  array[i] = list[i].asNum();
100 }
101 
102 template<int dim>
103 inline Vector<dim>::Vector(const AtlasInType& a)
104 {
105  fromAtlas(a);
106 }
107 
108 template<int dim>
109 inline void Vector<dim>::fromAtlas(const AtlasInType& a)
110 {
111  _ArrayFromAtlas(m_elem, dim, a);
112  m_valid = true;
113 }
114 
115 template<int dim>
116 inline AtlasOutType Vector<dim>::toAtlas() const
117 {
118  return _ArrayToAtlas(m_elem, dim);
119 }
120 
121 inline void Quaternion::fromAtlas(const AtlasInType& a)
122 {
123  if(!a.IsList())
124  throw _AtlasBadParse();
125 
126 
127  const Atlas::Message::ListType& list(a.AsList());
128 
129  if(list.size() != 4)
130  throw _AtlasBadParse();
131 
132 
133  for(int i = 0; i < 3; ++i)
134  m_vec[i] = list[i].asNum();
135 
136  m_w = list[3].asNum();
137 
138  CoordType norm = std::sqrt(m_w * m_w + m_vec.sqrMag());
139 
140  if (norm <= numeric_constants<CoordType>::epsilon()) {
141  m_valid = false;
142  m_vec.setValid(false);
143  return;
144  }
145 
146  m_vec /= norm;
147  m_w /= norm;
148 
149  m_valid = true;
150  m_age = 1;
151  m_vec.setValid();
152 }
153 
154 inline AtlasOutType Quaternion::toAtlas() const
155 {
156  Atlas::Message::ListType a(4);
157 
158  for(int i = 0; i < 3; ++i)
159  a[i] = m_vec[i];
160  a[3] = m_w;
161 
162  return a;
163 }
164 
165 template<int dim>
166 inline Point<dim>::Point(const AtlasInType& a)
167 {
168  fromAtlas(a);
169 }
170 
171 template<int dim>
172 inline void Point<dim>::fromAtlas(const AtlasInType& a)
173 {
174  _ArrayFromAtlas(m_elem, dim, a);
175  m_valid = true;
176 }
177 
178 template<int dim>
179 inline AtlasOutType Point<dim>::toAtlas() const
180 {
181  return _ArrayToAtlas(m_elem, dim);
182 }
183 
184 template<int dim>
185 inline AxisBox<dim>::AxisBox(const AtlasInType& a)
186 {
187  fromAtlas(a);
188 }
189 
190 template<int dim>
191 inline void AxisBox<dim>::fromAtlas(const AtlasInType& a)
192 {
193  if(!a.IsList())
194  throw _AtlasBadParse();
195 
196  const Atlas::Message::ListType& list(a.AsList());
197 
198  switch(list.size()) {
199  case dim:
200  m_low.setToOrigin();
201  m_high.fromAtlas(a);
202  break;
203  case (2 * dim):
204  for(int i = 0; i < dim; ++i) {
205  m_low[i] = list[i].asNum();
206  m_high[i] = list[i+dim].asNum();
207  }
208  m_low.setValid();
209  m_high.setValid();
210  break;
211  default:
212  throw _AtlasBadParse();
213  }
214 
215  for(int i = 0; i < dim; ++i) {
216  if(m_low[i] > m_high[i]) { // spec may allow this?
217  CoordType tmp = m_low[i];
218  m_low[i] = m_high[i];
219  m_high[i] = tmp;
220  }
221  }
222 }
223 
224 template<int dim>
225 inline AtlasOutType AxisBox<dim>::toAtlas() const
226 {
227  int i;
228 
229  for(i = 0; i < dim; ++i)
230  if(m_low[i] != 0)
231  break;
232 
233  if(i == dim)
234  return m_high.toAtlas(); // matches case 'dim' above
235 
236  // Do case '2 * dim' above
237 
238  Atlas::Message::ListType a(2*dim);
239  for(i = 0; i < dim; ++i) {
240  a[i] = m_low[i];
241  a[dim+i] = m_high[i];
242  }
243 
244  return a;
245 }
246 
247 template<int dim>
248 inline void Ball<dim>::fromAtlas(const AtlasInType& a)
249 {
250  const Atlas::Message::Element& message(a);
251  if (message.isMap()) {
252  const Atlas::Message::MapType& shapeElement(message.asMap());
253  // Get sphere's radius
254  Atlas::Message::MapType::const_iterator shape_I = shapeElement.find("radius");
255  if (shape_I != shapeElement.end()) {
256  const Atlas::Message::Element& shapeRadiusElem(shape_I->second);
257  if (shapeRadiusElem.isNum()) {
258  m_radius = shapeRadiusElem.asNum();
259  }
260  }
261  Atlas::Message::MapType::const_iterator pos_I = shapeElement.find("position");
262  if (pos_I != shapeElement.end()) {
263  const Atlas::Message::Element& posElem(pos_I->second);
264  if (posElem.isList()) {
265  m_center.fromAtlas(posElem);
266  }
267  }
268  }
269 }
270 
271 template<int dim>
272 inline AtlasOutType Ball<dim>::toAtlas() const
273 {
274  Atlas::Message::MapType map;
275  map.insert(Atlas::Message::MapType::value_type("radius", m_radius));
276  map.insert(Atlas::Message::MapType::value_type("position", m_center.toAtlas()));
277  return map;
278 }
279 
280 template<int dim>
281 inline Ball<dim>::Ball(const AtlasInType& a) : m_center(Point<dim>::ZERO()),
282  m_radius(0)
283 {
284  fromAtlas(a);
285 }
286 
287 inline bool _ListNumCheck(const Atlas::Message::ListType & list, int dim)
288 {
289  for(int i = 0; i < dim; ++i) {
290  if (!list[i].isNum()) {
291  return false;
292  }
293  }
294  return true;
295 }
296 
297 template<template <int> class ShapeT>
298 inline void _AddCorner(ShapeT<3> & shape,
299  const Atlas::Message::ListType & point)
300 {
301  Point<3> wpt(point[0].asNum(), point[1].asNum(), point[2].asNum());
302  shape.addCorner(shape.numCorners(), wpt);
303 }
304 
305 template<template <int> class ShapeT>
306 inline void _AddCorner(ShapeT<2> & shape,
307  const Atlas::Message::ListType & point)
308 {
309  Point<2> wpt(point[0].asNum(), point[1].asNum());
310  shape.addCorner(shape.numCorners(), wpt);
311 }
312 
313 template<template <int> class ShapeT, int dim>
314 inline void _CornersFromAtlas(ShapeT<dim> & shape,
315  const Atlas::Message::Element& message)
316 {
317  if (message.isList()) {
318  const Atlas::Message::ListType& pointsData(message.asList());
319 
320  for (size_t p = 0; p < pointsData.size(); ++p) {
321  if (!pointsData[p].isList()) {
322  continue;
323  }
324 
325  const Atlas::Message::ListType& point(pointsData[p].asList());
326  if ((point.size() < dim) || !_ListNumCheck(point, dim)) {
327  continue;
328  }
329 
330  _AddCorner(shape, point);
331  }
332  }
333 }
334 
335 inline void Polygon<2>::fromAtlas(const AtlasInType& a)
336 {
337  const Atlas::Message::Element& message(a);
338  if (message.isMap()) {
339  const Atlas::Message::MapType& shapeElement(message.asMap());
340  Atlas::Message::MapType::const_iterator it = shapeElement.find("points");
341  if ((it != shapeElement.end()) && it->second.isList()) {
342  _CornersFromAtlas(*this, it->second);
343  if (numCorners() > 2) {
344  return;
345  }
346  }
347  } else if (message.isList()) {
348  _CornersFromAtlas(*this, message);
349  if (numCorners() > 2) {
350  return;
351  }
352  }
353  throw _AtlasBadParse();
354 }
355 
356 inline AtlasOutType Polygon<2>::toAtlas() const
357 {
358  Atlas::Message::ListType points;
359  for (theConstIter I = m_points.begin(); I != m_points.end(); ++I)
360  {
361  points.push_back(I->toAtlas());
362  }
363  Atlas::Message::MapType map;
364  map.insert(Atlas::Message::MapType::value_type("points", points));
365  return map;
366 }
367 
368 template<int dim>
369 inline void Line<dim>::fromAtlas(const AtlasInType& a)
370 {
371  const Atlas::Message::Element& message(a);
372  if (message.isMap()) {
373  const Atlas::Message::MapType& shapeElement(message.asMap());
374  Atlas::Message::MapType::const_iterator it = shapeElement.find("points");
375  if ((it != shapeElement.end()) && it->second.isList()) {
376  _CornersFromAtlas(*this, it->second);
377  if (numCorners() > 0) {
378  return;
379  }
380  }
381  } else if (message.isList()) {
382  _CornersFromAtlas(*this, message);
383  if (numCorners() > 0) {
384  return;
385  }
386  }
387  throw _AtlasBadParse();
388 }
389 
390 template<int dim>
391 inline AtlasOutType Line<dim>::toAtlas() const
392 {
393  Atlas::Message::ListType points;
394  for (const_iterator I = m_points.begin(); I != m_points.end(); ++I)
395  {
396  points.push_back(I->toAtlas());
397  }
398  Atlas::Message::MapType map;
399  map.insert(Atlas::Message::MapType::value_type("points", points));
400  return map;
401 }
402 
403 template<int dim>
404 inline Line<dim>::Line(const AtlasInType& a) {
405  fromAtlas(a);
406 }
407 
408 template<int dim>
409 inline void RotBox<dim>::fromAtlas(const AtlasInType& a)
410 {
411  const Atlas::Message::Element& message(a);
412  if (message.isMap()) {
413  const Atlas::Message::MapType& shapeElement(message.asMap());
414  // Get rotbox's position
415  Atlas::Message::MapType::const_iterator shape_I = shapeElement.find("point");
416  if (shape_I != shapeElement.end()) {
417  const Atlas::Message::Element& shapePointElem(shape_I->second);
418  Point<dim> shapePoint;
419  shapePoint.fromAtlas(shapePointElem);
420  // Get rotbox's vector
421  shape_I = shapeElement.find("size");
422  if (shape_I != shapeElement.end()) {
423  const Atlas::Message::Element& shapeVectorElem(shape_I->second);
424  Vector<dim> shapeVector;
425  shapeVector.fromAtlas(shapeVectorElem);
426  m_corner0 = shapePoint;
427  m_size = shapeVector;
428  m_orient = RotMatrix<dim>().identity(); //TODO: parse rotation matrix (is it needed?)
429  return;
430  }
431  }
432  }
433  throw _AtlasBadParse();
434 }
435 
436 template<int dim>
437 inline AtlasOutType RotBox<dim>::toAtlas() const
438 {
439  Atlas::Message::MapType map;
440  map.insert(Atlas::Message::MapType::value_type("point", m_corner0.toAtlas()));
441  map.insert(Atlas::Message::MapType::value_type("size", m_size.toAtlas()));
442  //TODO: also add the rotmatrix
443  return map;
444 }
445 
446 template<int dim>
447 inline RotBox<dim>::RotBox(const AtlasInType& a) {
448  fromAtlas(a);
449 }
450 
451 } // namespace WFMath
452 
453 #endif // WFMATH_ATLAS_CONV_H