WFMath  1.0.1
atlasconv.h
00001 // atlasconv.h (Functions to convert WFMath library object to/from an Atlas Message)
00002 //
00003 //  The WorldForge Project
00004 //  Copyright (C) 2001  The WorldForge Project
00005 //
00006 //  This program is free software; you can redistribute it and/or modify
00007 //  it under the terms of the GNU General Public License as published by
00008 //  the Free Software Foundation; either version 2 of the License, or
00009 //  (at your option) any later version.
00010 //
00011 //  This program is distributed in the hope that it will be useful,
00012 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 //  GNU General Public License for more details.
00015 //
00016 //  You should have received a copy of the GNU General Public License
00017 //  along with this program; if not, write to the Free Software
00018 //  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00019 //
00020 //  For information about WorldForge and its authors, please contact
00021 //  the Worldforge Web Site at http://www.worldforge.org.
00022 
00023 // Author: Ron Steinke
00024 // Created: 2001-12-11
00025 
00026 // Since we don't want WFMath and Atlas to depend on each other,
00027 // we're putting all the atlas interface functions into this header.
00028 
00029 // WARNING! WARNING! Do not include this file in any other file in wfmath.
00030 
00031 #ifndef WFMATH_ATLAS_CONV_H
00032 #define WFMATH_ATLAS_CONV_H
00033 
00034 #include <wfmath/point.h>
00035 #include <wfmath/vector.h>
00036 #include <wfmath/quaternion.h>
00037 #include <wfmath/axisbox.h>
00038 #include <wfmath/polygon.h>
00039 #include <wfmath/ball.h>
00040 #include <wfmath/rotbox.h>
00041 #include <wfmath/line.h>
00042 
00043 #include <cmath>
00044 
00045 namespace WFMath {
00046 
00047 #ifndef ATLAS_MESSAGE_ELEMENT_H
00048 #error "You must include Atlas/Message/Element.h before wfmath/atlasconv.h"
00049 #endif
00050 
00051 typedef Atlas::Message::WrongTypeException _AtlasBadParse;
00052 
00053 class AtlasInType
00054 {
00055  public:
00056   AtlasInType(const Atlas::Message::Element& val) : m_val(val) {}
00057   // allow nice conversions when necessary
00058   template<class C> AtlasInType(C c) : m_obj(c), m_val(m_obj) {}
00059   operator const Atlas::Message::Element&() const {return m_val;}
00060   bool IsList() const {return m_val.isList();}
00061   const Atlas::Message::ListType& AsList() const {return m_val.asList();}
00062  private:
00063   Atlas::Message::Element m_obj;
00064   const Atlas::Message::Element& m_val;
00065 };
00066 
00067 class AtlasOutType
00068 {
00069  public:
00070   AtlasOutType(const Atlas::Message::ListType& l) : m_val(l) {}
00071   AtlasOutType(const Atlas::Message::MapType& l) : m_val(l) {}
00072   operator Atlas::Message::Element&() {return m_val;}
00073   operator const Atlas::Message::Element&() const {return m_val;}
00074  private:
00075   Atlas::Message::Element m_val;
00076 };
00077 
00078 inline AtlasOutType _ArrayToAtlas(const CoordType* array, unsigned len)
00079 {
00080   Atlas::Message::ListType a(len);
00081 
00082   for(unsigned i = 0; i < len; ++i)
00083     a[i] = array[i];
00084 
00085   return a;
00086 }
00087 
00088 inline void _ArrayFromAtlas(CoordType* array, unsigned len, const AtlasInType& a)
00089 {
00090   if(!a.IsList())
00091     throw _AtlasBadParse();
00092 
00093   const Atlas::Message::ListType& list(a.AsList());
00094 
00095   if(list.size() != (unsigned int) len)
00096     throw _AtlasBadParse();
00097 
00098   for(unsigned i = 0; i < len; ++i)
00099     array[i] = list[i].asNum();
00100 }
00101 
00102 template<int dim>
00103 inline Vector<dim>::Vector(const AtlasInType& a)
00104 {
00105   fromAtlas(a);
00106 }
00107 
00108 template<int dim>
00109 inline void Vector<dim>::fromAtlas(const AtlasInType& a)
00110 {
00111   _ArrayFromAtlas(m_elem, dim, a);
00112   m_valid = true;
00113 }
00114 
00115 template<int dim>
00116 inline AtlasOutType Vector<dim>::toAtlas() const
00117 {
00118   return _ArrayToAtlas(m_elem, dim);
00119 }
00120 
00121 inline void Quaternion::fromAtlas(const AtlasInType& a)
00122 {
00123   if(!a.IsList())
00124     throw _AtlasBadParse();
00125 
00126 
00127   const Atlas::Message::ListType& list(a.AsList());
00128 
00129   if(list.size() != 4)
00130     throw _AtlasBadParse();
00131 
00132 
00133   for(int i = 0; i < 3; ++i)
00134     m_vec[i] = list[i].asNum();
00135 
00136   m_w = list[3].asNum();
00137 
00138   CoordType norm = std::sqrt(m_w * m_w + m_vec.sqrMag());
00139 
00140   if (norm <= numeric_constants<CoordType>::epsilon()) {
00141     m_valid = false;
00142     m_vec.setValid(false);
00143     return;
00144   }
00145 
00146   m_vec /= norm;
00147   m_w /= norm;
00148 
00149   m_valid = true;
00150   m_age = 1;
00151   m_vec.setValid();
00152 }
00153 
00154 inline AtlasOutType Quaternion::toAtlas() const
00155 {
00156   Atlas::Message::ListType a(4);
00157 
00158   for(int i = 0; i < 3; ++i)
00159     a[i] = m_vec[i];
00160   a[3] = m_w;
00161 
00162   return a;
00163 }
00164 
00165 template<int dim>
00166 inline Point<dim>::Point(const AtlasInType& a)
00167 {
00168   fromAtlas(a);
00169 }
00170 
00171 template<int dim>
00172 inline void Point<dim>::fromAtlas(const AtlasInType& a)
00173 {
00174   _ArrayFromAtlas(m_elem, dim, a);
00175   m_valid = true;
00176 }
00177 
00178 template<int dim>
00179 inline AtlasOutType Point<dim>::toAtlas() const
00180 {
00181   return _ArrayToAtlas(m_elem, dim);
00182 }
00183 
00184 template<int dim>
00185 inline AxisBox<dim>::AxisBox(const AtlasInType& a)
00186 {
00187   fromAtlas(a);
00188 }
00189 
00190 template<int dim>
00191 inline void AxisBox<dim>::fromAtlas(const AtlasInType& a)
00192 {
00193   if(!a.IsList())
00194     throw _AtlasBadParse();
00195 
00196   const Atlas::Message::ListType& list(a.AsList());
00197 
00198   switch(list.size()) {
00199     case dim:
00200       m_low.setToOrigin();
00201       m_high.fromAtlas(a);
00202       break;
00203     case (2 * dim):
00204       for(int i = 0; i < dim; ++i) {
00205         m_low[i] = list[i].asNum();
00206         m_high[i] = list[i+dim].asNum();
00207       }
00208       m_low.setValid();
00209       m_high.setValid();
00210       break;
00211     default:
00212       throw _AtlasBadParse();
00213   }
00214 
00215   for(int i = 0; i < dim; ++i) {
00216     if(m_low[i] > m_high[i]) { // spec may allow this?
00217       CoordType tmp = m_low[i];
00218       m_low[i] = m_high[i];
00219       m_high[i] = tmp;
00220     }
00221   }
00222 }
00223 
00224 template<int dim>
00225 inline AtlasOutType AxisBox<dim>::toAtlas() const
00226 {
00227   int i;
00228 
00229   for(i = 0; i < dim; ++i)
00230     if(m_low[i] != 0)
00231       break;
00232 
00233   if(i == dim)
00234     return m_high.toAtlas(); // matches case 'dim' above
00235 
00236   // Do case '2 * dim' above
00237 
00238   Atlas::Message::ListType a(2*dim);
00239   for(i = 0; i < dim; ++i) {
00240     a[i] = m_low[i];
00241     a[dim+i] = m_high[i];
00242   }
00243 
00244   return a;
00245 }
00246 
00247 template<int dim>
00248 inline void Ball<dim>::fromAtlas(const AtlasInType& a)
00249 {
00250   const Atlas::Message::Element& message(a);
00251   if (message.isMap()) {
00252     const Atlas::Message::MapType& shapeElement(message.asMap());
00253     // Get sphere's radius
00254     Atlas::Message::MapType::const_iterator shape_I = shapeElement.find("radius");
00255     if (shape_I != shapeElement.end()) {
00256       const Atlas::Message::Element& shapeRadiusElem(shape_I->second);
00257       if (shapeRadiusElem.isNum()) {
00258         m_radius = shapeRadiusElem.asNum();
00259       }
00260     }
00261     Atlas::Message::MapType::const_iterator pos_I = shapeElement.find("position");
00262     if (pos_I != shapeElement.end()) {
00263       const Atlas::Message::Element& posElem(pos_I->second);
00264       if (posElem.isList()) {
00265         m_center.fromAtlas(posElem);
00266       }
00267     }
00268   }
00269 }
00270 
00271 template<int dim>
00272 inline AtlasOutType Ball<dim>::toAtlas() const
00273 {
00274   Atlas::Message::MapType map;
00275   map.insert(Atlas::Message::MapType::value_type("radius", m_radius));
00276   map.insert(Atlas::Message::MapType::value_type("position", m_center.toAtlas()));
00277   return map;
00278 }
00279 
00280 template<int dim>
00281 inline Ball<dim>::Ball(const AtlasInType& a) : m_center(Point<dim>::ZERO()),
00282                                                m_radius(0)
00283 {
00284   fromAtlas(a);
00285 }
00286 
00287 inline bool _ListNumCheck(const Atlas::Message::ListType & list, int dim)
00288 {
00289   for(int i = 0; i < dim; ++i) {
00290     if (!list[i].isNum()) {
00291       return false;
00292     }
00293   }
00294   return true;
00295 }
00296 
00297 template<template <int> class ShapeT>
00298 inline void _AddCorner(ShapeT<3> & shape,
00299                        const Atlas::Message::ListType & point)
00300 {
00301   Point<3> wpt(point[0].asNum(), point[1].asNum(), point[2].asNum());
00302   shape.addCorner(shape.numCorners(), wpt);
00303 }
00304 
00305 template<template <int> class ShapeT>
00306 inline void _AddCorner(ShapeT<2> & shape,
00307                        const Atlas::Message::ListType & point)
00308 {
00309   Point<2> wpt(point[0].asNum(), point[1].asNum());
00310   shape.addCorner(shape.numCorners(), wpt);
00311 }
00312 
00313 template<template <int> class ShapeT, int dim>
00314 inline void _CornersFromAtlas(ShapeT<dim> & shape,
00315                               const Atlas::Message::Element& message)
00316 {
00317   if (message.isList()) {
00318     const Atlas::Message::ListType& pointsData(message.asList());
00319     
00320     for (size_t p = 0; p < pointsData.size(); ++p) {
00321       if (!pointsData[p].isList()) {
00322         continue;
00323       }
00324       
00325       const Atlas::Message::ListType& point(pointsData[p].asList());
00326       if ((point.size() < dim) || !_ListNumCheck(point, dim)) {
00327         continue;
00328       }
00329       
00330       _AddCorner(shape, point);
00331     }
00332   }
00333 }
00334 
00335 inline void Polygon<2>::fromAtlas(const AtlasInType& a)
00336 {
00337   const Atlas::Message::Element& message(a);
00338   if (message.isMap()) {
00339     const Atlas::Message::MapType& shapeElement(message.asMap());
00340     Atlas::Message::MapType::const_iterator it = shapeElement.find("points");
00341     if ((it != shapeElement.end()) && it->second.isList()) {
00342       _CornersFromAtlas(*this, it->second);
00343       if (numCorners() > 2) {
00344         return;
00345       }
00346     }
00347   } else if (message.isList()) {
00348     _CornersFromAtlas(*this, message);
00349     if (numCorners() > 2) {
00350       return;
00351     }
00352   }
00353   throw _AtlasBadParse();
00354 }
00355 
00356 inline AtlasOutType Polygon<2>::toAtlas() const
00357 {
00358   Atlas::Message::ListType points;
00359   for (theConstIter I = m_points.begin(); I != m_points.end(); ++I) 
00360   {
00361     points.push_back(I->toAtlas());
00362   }
00363   Atlas::Message::MapType map;
00364   map.insert(Atlas::Message::MapType::value_type("points", points));
00365   return map;
00366 }
00367 
00368 template<int dim>
00369 inline void Line<dim>::fromAtlas(const AtlasInType& a)
00370 {
00371   const Atlas::Message::Element& message(a);
00372   if (message.isMap()) {
00373     const Atlas::Message::MapType& shapeElement(message.asMap());
00374     Atlas::Message::MapType::const_iterator it = shapeElement.find("points");
00375     if ((it != shapeElement.end()) && it->second.isList()) {
00376       _CornersFromAtlas(*this, it->second);
00377       if (numCorners() > 0) {
00378         return;
00379       }
00380     }
00381   } else if (message.isList()) {
00382     _CornersFromAtlas(*this, message);
00383     if (numCorners() > 0) {
00384       return;
00385     }
00386   }
00387   throw _AtlasBadParse();
00388 }
00389 
00390 template<int dim>
00391 inline AtlasOutType Line<dim>::toAtlas() const
00392 {
00393   Atlas::Message::ListType points;
00394   for (const_iterator I = m_points.begin(); I != m_points.end(); ++I) 
00395   {
00396     points.push_back(I->toAtlas());
00397   }
00398   Atlas::Message::MapType map;
00399   map.insert(Atlas::Message::MapType::value_type("points", points));
00400   return map;
00401 }
00402 
00403 template<int dim>
00404 inline Line<dim>::Line(const AtlasInType& a) {
00405   fromAtlas(a);
00406 }
00407 
00408 template<int dim>
00409 inline void RotBox<dim>::fromAtlas(const AtlasInType& a)
00410 {
00411   const Atlas::Message::Element& message(a);
00412   if (message.isMap()) {
00413     const Atlas::Message::MapType& shapeElement(message.asMap());
00414     // Get rotbox's position
00415     Atlas::Message::MapType::const_iterator shape_I = shapeElement.find("point");
00416     if (shape_I != shapeElement.end()) {
00417       const Atlas::Message::Element& shapePointElem(shape_I->second);
00418       Point<dim> shapePoint;
00419       shapePoint.fromAtlas(shapePointElem);
00420       // Get rotbox's vector
00421       shape_I = shapeElement.find("size");
00422       if (shape_I != shapeElement.end()) {
00423         const Atlas::Message::Element& shapeVectorElem(shape_I->second);
00424         Vector<dim> shapeVector;
00425         shapeVector.fromAtlas(shapeVectorElem);
00426         m_corner0 = shapePoint;
00427         m_size = shapeVector;
00428         m_orient = RotMatrix<dim>().identity(); //TODO: parse rotation matrix (is it needed?)
00429         return;
00430       }
00431     }
00432   }
00433   throw _AtlasBadParse();
00434 }
00435 
00436 template<int dim>
00437 inline AtlasOutType RotBox<dim>::toAtlas() const
00438 {
00439   Atlas::Message::MapType map;
00440   map.insert(Atlas::Message::MapType::value_type("point", m_corner0.toAtlas()));
00441   map.insert(Atlas::Message::MapType::value_type("size", m_size.toAtlas()));
00442   //TODO: also add the rotmatrix
00443   return map;
00444 }
00445 
00446 template<int dim>
00447 inline RotBox<dim>::RotBox(const AtlasInType& a) {
00448   fromAtlas(a);
00449 }
00450 
00451 } // namespace WFMath
00452 
00453 #endif // WFMATH_ATLAS_CONV_H