WFMath  0.3.12
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 
00042 #include <cmath>
00043 
00044 namespace WFMath {
00045 
00046 #ifdef ATLAS_MESSAGE_ELEMENT_H
00047 
00048 typedef Atlas::Message::WrongTypeException _AtlasBadParse;
00049 typedef Atlas::Message::Element _AtlasMessageType;
00050 typedef Atlas::Message::FloatType _AtlasFloatType;
00051 typedef Atlas::Message::ListType _AtlasListType;
00052 typedef Atlas::Message::MapType _AtlasMapType;
00053 
00054 inline bool _isNum(const _AtlasMessageType& a) {return a.isNum();}
00055 inline _AtlasFloatType _asNum(const _AtlasMessageType& a) {return a.asNum();}
00056 
00057 #else
00058 #error "You must include Atlas/Message/Element.h before wfmath/atlasconv.h"
00059 #endif
00060 
00061 class AtlasInType
00062 {
00063  public:
00064   AtlasInType(const _AtlasMessageType& val) : m_val(val) {}
00065   // allow nice conversions when necessary
00066   template<class C> AtlasInType(C c) : m_obj(c), m_val(m_obj) {}
00067   operator const _AtlasMessageType&() const {return m_val;}
00068   bool IsList() const {return m_val.isList();}
00069   const _AtlasListType& AsList() const {return m_val.asList();}
00070  private:
00071   _AtlasMessageType m_obj;
00072   const _AtlasMessageType& m_val;
00073 };
00074 
00075 class AtlasOutType
00076 {
00077  public:
00078   AtlasOutType(const _AtlasListType& l) : m_val(l) {}
00079   AtlasOutType(const _AtlasMapType& l) : m_val(l) {}
00080   operator _AtlasMessageType&() {return m_val;}
00081   operator const _AtlasMessageType&() const {return m_val;}
00082  private:
00083   _AtlasMessageType m_val;
00084 };
00085 
00086 inline AtlasOutType _ArrayToAtlas(const CoordType* array, unsigned len)
00087 {
00088   _AtlasListType a(len);
00089 
00090   for(unsigned i = 0; i < len; ++i)
00091     a[i] = array[i];
00092 
00093   return a;
00094 }
00095 
00096 inline void _ArrayFromAtlas(CoordType* array, unsigned len, const AtlasInType& a)
00097 {
00098   if(!a.IsList())
00099     throw _AtlasBadParse();
00100 
00101   const _AtlasListType& list(a.AsList());
00102 
00103   if(list.size() != (unsigned int) len)
00104     throw _AtlasBadParse();
00105 
00106   for(unsigned i = 0; i < len; ++i)
00107     array[i] = _asNum(list[i]);
00108 }
00109 
00110 template<int dim>
00111 inline Vector<dim>::Vector(const AtlasInType& a)
00112 {
00113   fromAtlas(a);
00114 }
00115 
00116 template<int dim>
00117 inline void Vector<dim>::fromAtlas(const AtlasInType& a)
00118 {
00119   _ArrayFromAtlas(m_elem, dim, a);
00120   m_valid = true;
00121 }
00122 
00123 template<int dim>
00124 inline AtlasOutType Vector<dim>::toAtlas() const
00125 {
00126   return _ArrayToAtlas(m_elem, dim);
00127 }
00128 
00129 inline void Quaternion::fromAtlas(const AtlasInType& a)
00130 {
00131   if(!a.IsList())
00132     throw _AtlasBadParse();
00133 
00134 
00135   const _AtlasListType& list(a.AsList());
00136 
00137   if(list.size() != 4)
00138     throw _AtlasBadParse();
00139 
00140 
00141   for(int i = 0; i < 3; ++i)
00142     m_vec[i] = _asNum(list[i]);
00143 
00144   m_w = _asNum(list[3]);
00145 
00146   CoordType norm = std::sqrt(m_w * m_w + m_vec.sqrMag());
00147 
00148   if (norm <= WFMATH_EPSILON) {
00149     m_valid = false;
00150     m_vec.setValid(false);
00151     return;
00152   }
00153 
00154   m_vec /= norm;
00155   m_w /= norm;
00156 
00157   m_valid = true;
00158   m_age = 1;
00159   m_vec.setValid();
00160 }
00161 
00162 inline AtlasOutType Quaternion::toAtlas() const
00163 {
00164   _AtlasListType a(4);
00165 
00166   for(int i = 0; i < 3; ++i)
00167     a[i] = m_vec[i];
00168   a[3] = m_w;
00169 
00170   return a;
00171 }
00172 
00173 template<int dim>
00174 inline Point<dim>::Point(const AtlasInType& a)
00175 {
00176   fromAtlas(a);
00177 }
00178 
00179 template<int dim>
00180 inline void Point<dim>::fromAtlas(const AtlasInType& a)
00181 {
00182   _ArrayFromAtlas(m_elem, dim, a);
00183   m_valid = true;
00184 }
00185 
00186 template<int dim>
00187 inline AtlasOutType Point<dim>::toAtlas() const
00188 {
00189   return _ArrayToAtlas(m_elem, dim);
00190 }
00191 
00192 template<int dim>
00193 inline AxisBox<dim>::AxisBox(const AtlasInType& a)
00194 {
00195   fromAtlas(a);
00196 }
00197 
00198 template<int dim>
00199 inline void AxisBox<dim>::fromAtlas(const AtlasInType& a)
00200 {
00201   if(!a.IsList())
00202     throw _AtlasBadParse();
00203 
00204   const _AtlasListType& list(a.AsList());
00205 
00206   switch(list.size()) {
00207     case dim:
00208       m_low.setToOrigin();
00209       m_high.fromAtlas(a);
00210       break;
00211     case (2 * dim):
00212       for(int i = 0; i < dim; ++i) {
00213         m_low[i] = _asNum(list[i]);
00214         m_high[i] = _asNum(list[i+dim]);
00215       }
00216       m_low.setValid();
00217       m_high.setValid();
00218       break;
00219     default:
00220       throw _AtlasBadParse();
00221   }
00222 
00223   for(int i = 0; i < dim; ++i) {
00224     if(m_low[i] > m_high[i]) { // spec may allow this?
00225       CoordType tmp = m_low[i];
00226       m_low[i] = m_high[i];
00227       m_high[i] = tmp;
00228     }
00229   }
00230 }
00231 
00232 template<int dim>
00233 inline AtlasOutType AxisBox<dim>::toAtlas() const
00234 {
00235   int i;
00236 
00237   for(i = 0; i < dim; ++i)
00238     if(m_low[i] != 0)
00239       break;
00240 
00241   if(i == dim)
00242     return m_high.toAtlas(); // matches case 'dim' above
00243 
00244   // Do case '2 * dim' above
00245 
00246   _AtlasListType a(2*dim);
00247   for(i = 0; i < dim; ++i) {
00248     a[i] = m_low[i];
00249     a[dim+i] = m_high[i];
00250   }
00251 
00252   return a;
00253 }
00254 
00255 template<int dim>
00256 inline void Ball<dim>::fromAtlas(const AtlasInType& a)
00257 {
00258   const _AtlasMessageType& message(a);
00259   if (message.isMap()) {
00260     const Atlas::Message::MapType& shapeElement(message.asMap());
00261     // Get sphere's radius
00262     Atlas::Message::MapType::const_iterator shape_I = shapeElement.find("radius");
00263     if (shape_I != shapeElement.end()) {
00264       const Atlas::Message::Element& shapeRadiusElem(shape_I->second);
00265       if (shapeRadiusElem.isNum()) {
00266         m_radius = shapeRadiusElem.asNum();
00267       }
00268     }
00269     Atlas::Message::MapType::const_iterator pos_I = shapeElement.find("position");
00270     if (pos_I != shapeElement.end()) {
00271       const Atlas::Message::Element& posElem(pos_I->second);
00272       if (posElem.isList()) {
00273         m_center.fromAtlas(posElem);
00274       }
00275     }
00276   }
00277 }
00278 
00279 template<int dim>
00280 inline AtlasOutType Ball<dim>::toAtlas() const
00281 {
00282   Atlas::Message::MapType map;
00283   map.insert(Atlas::Message::MapType::value_type("radius", _AtlasFloatType(m_radius)));
00284   map.insert(Atlas::Message::MapType::value_type("position", m_center.toAtlas()));
00285   return map;
00286 }
00287 
00288 template<int dim>
00289 inline Ball<dim>::Ball(const AtlasInType& a) : m_center(Point<dim>::ZERO()),
00290                                                m_radius(0)
00291 {
00292   fromAtlas(a);
00293 }
00294 
00295 template<template <int> class ShapeT, int dim>
00296 inline void _CornersFromAtlas(ShapeT<dim> & shape,
00297                               const _AtlasMessageType& message)
00298 {
00299   if (message.isList()) {
00300     const Atlas::Message::ListType& pointsData(message.asList());
00301     
00302     for (size_t p = 0; p < pointsData.size(); ++p) {
00303       if (!pointsData[p].isList()) {
00304         continue;
00305       }
00306       
00307       const Atlas::Message::ListType& point(pointsData[p].asList());
00308       if ((point.size() < dim) || !point[0].isNum() || !point[1].isNum()) {
00309         continue;
00310       }
00311       
00312       WFMath::Point<dim> wpt(point[0].asNum(), point[1].asNum());
00313       shape.addCorner(shape.numCorners(), wpt);
00314     }
00315   }
00316 }
00317 
00318 inline void Polygon<2>::fromAtlas(const AtlasInType& a)
00319 {
00320   const _AtlasMessageType& message(a);
00321   if (message.isMap()) {
00322     const Atlas::Message::MapType& shapeElement(message.asMap());
00323     Atlas::Message::MapType::const_iterator it = shapeElement.find("points");
00324     if ((it != shapeElement.end()) && it->second.isList()) {
00325       _CornersFromAtlas(*this, it->second);
00326       if (numCorners() > 2) {
00327         return;
00328       }
00329     }
00330   } else if (message.isList()) {
00331     _CornersFromAtlas(*this, message);
00332     if (numCorners() > 2) {
00333       return;
00334     }
00335   }
00336   throw _AtlasBadParse();
00337 }
00338 
00339 inline AtlasOutType Polygon<2>::toAtlas() const
00340 {
00341   Atlas::Message::ListType points;
00342   for (theConstIter I = m_points.begin(); I != m_points.end(); ++I) 
00343   {
00344     points.push_back(I->toAtlas());
00345   }
00346   Atlas::Message::MapType map;
00347   map.insert(Atlas::Message::MapType::value_type("points", points));
00348   return map;
00349 }
00350 
00351 
00352 template<int dim>
00353 inline void RotBox<dim>::fromAtlas(const AtlasInType& a)
00354 {
00355   const _AtlasMessageType& message(a);
00356   if (message.isMap()) {
00357     const Atlas::Message::MapType& shapeElement(message.asMap());
00358     // Get rotbox's position
00359     Atlas::Message::MapType::const_iterator shape_I = shapeElement.find("point");
00360     if (shape_I != shapeElement.end()) {
00361       const Atlas::Message::Element& shapePointElem(shape_I->second);
00362       WFMath::Point<dim> shapePoint;
00363       shapePoint.fromAtlas(shapePointElem);
00364       // Get rotbox's vector
00365       shape_I = shapeElement.find("size");
00366       if (shape_I != shapeElement.end()) {
00367         const Atlas::Message::Element& shapeVectorElem(shape_I->second);
00368         WFMath::Vector<dim> shapeVector;
00369         shapeVector.fromAtlas(shapeVectorElem);
00370         m_corner0 = shapePoint;
00371         m_size = shapeVector;
00372         m_orient = WFMath::RotMatrix<dim>().identity(); //TODO: parse rotation matrix (is it needed?)
00373         return;
00374       }
00375     }
00376   }
00377   throw _AtlasBadParse();
00378 }
00379 
00380 template<int dim>
00381 inline AtlasOutType RotBox<dim>::toAtlas() const
00382 {
00383   Atlas::Message::MapType map;
00384   map.insert(Atlas::Message::MapType::value_type("point", m_corner0.toAtlas()));
00385   map.insert(Atlas::Message::MapType::value_type("size", m_size.toAtlas()));
00386   //TODO: also add the rotmatrix
00387   return map;
00388 }
00389 
00390 template<int dim>
00391 inline RotBox<dim>::RotBox(const AtlasInType& a) {
00392   fromAtlas(a);
00393 }
00394 
00395 } // namespace WFMath
00396 
00397 #endif // WFMATH_ATLAS_CONV_H