WFMath 0.3.11

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