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