WFMath  1.0.2
stream.h
00001 // stream.h (Functions in the WFMath library that use streams)
00002 //
00003 //  The WorldForge Project
00004 //  Copyright (C) 2001,2002  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-7
00025 
00026 #ifndef WFMATH_STREAM_H
00027 #define WFMATH_STREAM_H
00028 
00029 #include <wfmath/vector.h>
00030 #include <wfmath/rotmatrix.h>
00031 #include <wfmath/point.h>
00032 #include <wfmath/axisbox.h>
00033 #include <wfmath/ball.h>
00034 #include <wfmath/segment.h>
00035 #include <wfmath/rotbox.h>
00036 #include <wfmath/polygon.h>
00037 #include <wfmath/line.h>
00038 #include <wfmath/error.h>
00039 #include <string>
00040 #include <iostream>
00041 #include <list> // For Polygon<>::operator>>()
00042 
00043 #include <cassert>
00044 
00045 namespace WFMath {
00046 
00047 // sstream vs. strstream compatibility wrapper
00048 
00049 namespace _IOWrapper {
00050 
00051   // Need separate read/write classes, since one is const C& and the other is C&
00052 
00053   class BaseRead {
00054    public:
00055     virtual ~BaseRead() {}
00056 
00057     virtual void read(std::istream& is) = 0;
00058   };
00059 
00060   class BaseWrite {
00061    public:
00062     virtual ~BaseWrite() {}
00063 
00064     virtual void write(std::ostream& os) const = 0;
00065   };
00066 
00067   template<class C>
00068   class ImplRead : public BaseRead {
00069    public:
00070     ImplRead(C& c) : m_data(c) {}
00071     virtual ~ImplRead() {}
00072 
00073     virtual void read(std::istream& is) {is >> m_data;}
00074 
00075    private:
00076     C &m_data;
00077   };
00078 
00079   template<class C>
00080   class ImplWrite : public BaseWrite {
00081    public:
00082     ImplWrite(const C& c) : m_data(c) {}
00083     virtual ~ImplWrite() {}
00084 
00085     virtual void write(std::ostream& os) const {os << m_data;}
00086 
00087    private:
00088     const C &m_data;
00089   };
00090 
00091   std::string ToStringImpl(const BaseWrite& b, std::streamsize precision);
00092   void FromStringImpl(BaseRead& b, const std::string& s, std::streamsize precision);
00093 }
00094 
00096 
00099 template<class C>
00100 inline std::string ToString(const C& c, std::streamsize precision = 6)
00101 {
00102   return _IOWrapper::ToStringImpl(_IOWrapper::ImplWrite<C>(c), precision);
00103 }
00104 
00106 
00109 template<class C>
00110 inline void FromString(C& c, const std::string& s, std::streamsize = 6)
00111 {
00112   _IOWrapper::ImplRead<C> i(c);
00113   _IOWrapper::FromStringImpl(i, s, 6);
00114 }
00115 
00116 void _ReadCoordList(std::istream& is, CoordType* d, const int num);
00117 void _WriteCoordList(std::ostream& os, const CoordType* d, const int num);
00118 CoordType _GetEpsilon(std::istream& is);
00119 
00120 template<int dim>
00121 inline std::ostream& operator<<(std::ostream& os, const Vector<dim>& v)
00122 {
00123   _WriteCoordList(os, v.m_elem, dim);
00124   return os;
00125 }
00126 
00127 template<int dim>
00128 inline std::istream& operator>>(std::istream& is, Vector<dim>& v)
00129 {
00130   _ReadCoordList(is, v.m_elem, dim);
00131   v.m_valid = true;
00132   return is;
00133 }
00134 
00135 template<int dim>
00136 inline std::ostream& operator<<(std::ostream& os, const RotMatrix<dim>& m)
00137 {
00138   os << '(';
00139 
00140   for(int i = 0; i < dim; ++i) {
00141     _WriteCoordList(os, m.m_elem[i], dim);
00142     os << (i < (dim - 1) ? ',' : ')');
00143   }
00144 
00145   return os;
00146 }
00147 
00148 template<int dim>
00149 inline std::istream& operator>>(std::istream& is, RotMatrix<dim>& m)
00150 {
00151   CoordType d[dim*dim];
00152   char next;
00153 
00154   is >> next;
00155   if(next != '(')
00156     throw ParseError();
00157 
00158   for(int i = 0; i < dim; ++i) {
00159     _ReadCoordList(is, d + i * dim, dim);
00160     is >> next;
00161     char want = (i == dim - 1) ? ')' : ',';
00162     if(next != want)
00163       throw ParseError();
00164   }
00165 
00166   if(!m._setVals(d, FloatMax(numeric_constants<CoordType>::epsilon(), _GetEpsilon(is))))
00167     throw ParseError();
00168 
00169   return is;
00170 }
00171 
00172 template<int dim>
00173 inline std::ostream& operator<<(std::ostream& os, const Point<dim>& p)
00174 {
00175   _WriteCoordList(os, p.m_elem, dim);
00176   return os;
00177 }
00178 
00179 template<int dim>
00180 inline std::istream& operator>>(std::istream& is, Point<dim>& p)
00181 {
00182   _ReadCoordList(is, p.m_elem, dim);
00183   p.m_valid = true;
00184   return is;
00185 }
00186 
00187 template<int dim>
00188 inline std::ostream& operator<<(std::ostream& os, const AxisBox<dim>& a)
00189 {
00190   return os << "AxisBox: m_low = " << a.m_low << ", m_high = " << a.m_high;
00191 }
00192 
00193 template<int dim>
00194 inline std::istream& operator>>(std::istream& is, AxisBox<dim>& a)
00195 {
00196   char next;
00197 
00198   do {
00199     is >> next;
00200   } while(next != '=');
00201 
00202   is >> a.m_low;
00203 
00204   do {
00205     is >> next;
00206   } while(next != '=');
00207 
00208   is >> a.m_high;
00209 
00210   return is;
00211 }
00212 
00213 template<int dim>
00214 inline std::ostream& operator<<(std::ostream& os, const Ball<dim>& b)
00215 {
00216   return os << "Ball: m_center = " << b.m_center <<
00217           + ", m_radius = " << b.m_radius;
00218 }
00219 
00220 template<int dim>
00221 inline std::istream& operator>>(std::istream& is, Ball<dim>& b)
00222 {
00223   char next;
00224 
00225   do {
00226     is >> next;
00227   } while(next != '=');
00228 
00229   is >> b.m_center;
00230 
00231   do {
00232     is >> next;
00233   } while(next != '=');
00234 
00235   is >> b.m_radius;
00236 
00237   return is;
00238 }
00239 
00240 template<int dim>
00241 inline std::ostream& operator<<(std::ostream& os, const Segment<dim>& s)
00242 {
00243   return os << "Segment: m_p1 = " << s.m_p1 << ", m_p2 = " << s.m_p2;
00244 }
00245 
00246 template<int dim>
00247 inline std::istream& operator>>(std::istream& is, Segment<dim>& s)
00248 {
00249   char next;
00250 
00251   do {
00252     is >> next;
00253   } while(next != '=');
00254 
00255   is >> s.m_p1;
00256 
00257   do {
00258     is >> next;
00259   } while(next != '=');
00260 
00261   is >> s.m_p2;
00262 
00263   return is;
00264 }
00265 
00266 template<int dim>
00267 inline std::ostream& operator<<(std::ostream& os, const RotBox<dim>& r)
00268 {
00269   return os << "RotBox: m_corner0 = " << r.m_corner0
00270          << ", m_size = " << r.m_size
00271          << ", m_orient = " << r.m_orient;
00272 }
00273 
00274 template<int dim>
00275 inline std::istream& operator>>(std::istream& is, RotBox<dim>& r)
00276 {
00277   char next;
00278 
00279   do {
00280     is >> next;
00281   } while(next != '=');
00282 
00283   is >> r.m_corner0;
00284 
00285   do {
00286     is >> next;
00287   } while(next != '=');
00288 
00289   is >> r.m_size;
00290 
00291   do {
00292     is >> next;
00293   } while(next != '=');
00294 
00295   is >> r.m_orient;
00296 
00297   return is;
00298 }
00299 
00300 template<> std::ostream& operator<<(std::ostream& os, const Polygon<2>& r);
00301 template<> std::istream& operator>>(std::istream& is, Polygon<2>& r);
00302 
00303 template<int dim>
00304 inline std::ostream& operator<<(std::ostream& os, const Polygon<dim>& r)
00305 {
00306   size_t size = r.m_poly.numCorners();
00307 
00308   if(size == 0) {
00309     os << "<empty>";
00310     return os;
00311   }
00312 
00313   os << "Polygon: (";
00314 
00315   for(size_t i = 0; i < size; ++i)
00316     os << r.getCorner(i) << (i < (dim - 1) ? ',' : ')');
00317 
00318   return os;
00319 }
00320 
00321 // Can't stick this in operator>>(std::istream&, Polygon<>&), because
00322 // we use it as a template argument for list<>. Why isn't that allowed?
00323 template<int dim> struct _PolyReader
00324 {
00325   Point<dim> pd;
00326   Point<2> p2;
00327 };
00328 
00329 template<int dim>
00330 std::istream& operator>>(std::istream& is, Polygon<dim>& r)
00331 {
00332   char next;
00333   _PolyReader<dim> read;
00334   std::list<_PolyReader<dim> > read_list;
00335 
00336   // Read in the points
00337 
00338   do {
00339     is >> next;
00340     if(next == '<') { // empty polygon
00341        do {
00342          is >> next;
00343        } while(next != '>');
00344        return is;
00345     }
00346   } while(next != '(');
00347 
00348   while(true) {
00349     is >> read.pd;
00350     read_list.push_back(read);
00351     is >> next;
00352     if(next == ')')
00353       break;
00354     if(next != ',')
00355       throw ParseError();
00356   }
00357 
00358   // Convert to internal format. Be careful about the order points are
00359   // added to the orientation. If the first few points are too close together,
00360   // round off error can skew the plane, and later points that are further
00361   // away may fail.
00362 
00363   typename std::list<_PolyReader<dim> >::iterator i, end = read_list.end();
00364   bool succ;
00365 
00366   std::streamsize str_prec = is.precision();
00367   float str_eps = 1;
00368   while(--str_prec > 0) // Precision of 6 gives epsilon = 1e-5
00369     str_eps /= 10;
00370   CoordType epsilon = FloatMax(str_eps, numeric_constants<CoordType>::epsilon());
00371 
00372   r.m_orient = _Poly2Orient<dim>();
00373 
00374   if(read_list.size() < 3) { // This will always work
00375     for(i = read_list.begin(); i != end; ++i) {
00376       succ = r.m_orient.expand(i->pd, i->p2, epsilon);
00377       assert(succ);
00378     }
00379   }
00380   else { // Find the three furthest apart points
00381     typename std::list<_PolyReader<dim> >::iterator p1 = end, p2 = end, p3 = end, j; // invalid values
00382     CoordType dist = -1;
00383 
00384     for(i = read_list.begin(); i != end; ++i) {
00385       for(j = i, ++j; j != end; ++j) {
00386         CoordType new_dist = SloppyDistance(i->pd, j->pd);
00387         if(new_dist > dist) {
00388           p1 = i;
00389           p2 = j;
00390           dist = new_dist;
00391         }
00392       }
00393     }
00394 
00395     assert(p1 != end);
00396     assert(p2 != end);
00397 
00398     dist = -1;
00399 
00400     for(i = read_list.begin(); i != end; ++i) {
00401       // Don't want to be near either p1 or p2
00402       if(i == p1 || i == p2)
00403         continue;
00404       CoordType new_dist = FloatMin(SloppyDistance(i->pd, p1->pd),
00405                                     SloppyDistance(i->pd, p2->pd));
00406       if(new_dist > dist) {
00407         p3 = i;
00408         dist = new_dist;
00409       }
00410     }
00411 
00412     assert(p3 != end);
00413 
00414     // Add p1, p2, p3 first
00415 
00416     succ = r.m_orient.expand(p1->pd, p1->p2, epsilon);
00417     assert(succ);
00418     succ = r.m_orient.expand(p2->pd, p2->p2, epsilon);
00419     assert(succ);
00420     succ = r.m_orient.expand(p3->pd, p3->p2, epsilon);
00421     assert(succ);
00422 
00423     // Try to add the rest
00424 
00425     for(i = read_list.begin(); i != end; ++i) {
00426       if(i == p1 || i == p2 || i == p3) // Did these already
00427         continue;
00428       succ = r.m_orient.expand(i->pd, i->p2, epsilon);
00429       if(!succ) {
00430         r.clear();
00431         throw ParseError();
00432       }
00433     }
00434   }
00435 
00436   // Got valid points, add them to m_poly
00437 
00438   r.m_poly.resize(read_list.size());
00439 
00440   int pnum;
00441   for(i = read_list.begin(), pnum = 0; i != end; ++i, ++pnum)
00442     r.m_poly[pnum] = i->p2;
00443 
00444   return is;
00445 }
00446 
00447 template<int dim>
00448 inline std::ostream& operator<<(std::ostream& os, const Line<dim>& r)
00449 {
00450   size_t size = r.numCorners();
00451 
00452   if(size == 0) {
00453     os << "<empty>";
00454     return os;
00455   }
00456 
00457   os << "Line: (";
00458 
00459   for(size_t i = 0; i < size; ++i)
00460     os << r.getCorner(i) << (i < (dim - 1) ? ',' : ')');
00461 
00462   return os;
00463 }
00464 
00465 } // namespace WFMath
00466 
00467 #endif // WFMATH_STREAM_H