WFMath 0.3.11
point_funcs.h
00001 // point_funcs.h (point class copied from libCoal, subsequently modified)
00002 //
00003 //  The WorldForge Project
00004 //  Copyright (C) 2000, 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 
00024 // Author: Ron Steinke
00025 
00026 
00027 #ifndef WFMATH_POINT_FUNCS_H
00028 #define WFMATH_POINT_FUNCS_H
00029 
00030 #include <wfmath/point.h>
00031 
00032 #include <wfmath/vector.h>
00033 
00034 #include <cmath>
00035 
00036 namespace WFMath {
00037 
00038 template<const int dim>
00039 inline Point<dim>::Point(const Point<dim>& p) : m_valid(p.m_valid)
00040 {
00041   for(int i = 0; i < dim; ++i) {
00042     m_elem[i] = p.m_elem[i];
00043   }
00044 }
00045 
00046 template<const int dim>
00047 inline Point<dim>::Point(const Vector<dim>& v) : m_valid(v.isValid())
00048 {
00049   for(int i = 0; i < dim; ++i) {
00050     m_elem[i] = v.elements()[i];
00051   }
00052 }
00053 
00054 template<const int dim>
00055 inline Point<dim>& Point<dim>::setToOrigin()
00056 {
00057   for(int i = 0; i < dim; ++i) {
00058     m_elem[i] = 0;
00059   }
00060 
00061   m_valid = true;
00062 
00063   return *this;
00064 }
00065 
00066 template<const int dim>
00067 inline bool Point<dim>::isEqualTo(const Point<dim> &p, double epsilon) const
00068 {
00069   CoordType delta = (CoordType) _ScaleEpsilon(m_elem, p.m_elem, dim, epsilon);
00070 
00071   for(int i = 0; i < dim; ++i) {
00072     if(fabs(m_elem[i] - p.m_elem[i]) > delta) {
00073       return false;
00074     }
00075   }
00076 
00077   return true;
00078 }
00079 
00080 template<const int dim>
00081 inline Vector<dim> operator-(const Point<dim>& c1, const Point<dim>& c2)
00082 {
00083   Vector<dim> out;
00084 
00085   for(int i = 0; i < dim; ++i) {
00086     out.m_elem[i] = c1.m_elem[i] - c2.m_elem[i];
00087   }
00088 
00089   out.m_valid = c1.m_valid && c2.m_valid;
00090 
00091   return out;
00092 }
00093 
00094 template<const int dim>
00095 inline Point<dim>& operator+=(Point<dim>& p, const Vector<dim> &rhs)
00096 {
00097     for(int i = 0; i < dim; ++i) {
00098       p.m_elem[i] += rhs.m_elem[i];
00099     }
00100 
00101     p.m_valid = p.m_valid && rhs.m_valid;
00102 
00103     return p;
00104 }
00105 
00106 template<const int dim>
00107 inline Point<dim> operator+(const Point<dim>& c, const Vector<dim>& v)
00108 {
00109   Point<dim> out(c);
00110 
00111   out += v;
00112 
00113   return out;
00114 }
00115 
00116 template<const int dim>
00117 inline Point<dim> operator+(const Vector<dim>& v, const Point<dim>& c)
00118 {
00119   Point<dim> out(c);
00120 
00121   out += v;
00122 
00123   return out;
00124 }
00125 
00126 template<const int dim>
00127 inline Point<dim>& operator-=(Point<dim>& p, const Vector<dim> &rhs)
00128 {
00129     for(int i = 0; i < dim; ++i) {
00130       p.m_elem[i] -= rhs.m_elem[i];
00131     }
00132 
00133     p.m_valid = p.m_valid && rhs.m_valid;
00134 
00135     return p;
00136 }
00137 
00138 template<const int dim>
00139 inline Point<dim> operator-(const Point<dim>& c, const Vector<dim>& v)
00140 {
00141   Point<dim> out(c);
00142 
00143   out -= v;
00144 
00145   return out;
00146 }
00147 
00148 template<const int dim>
00149 inline Point<dim>& Point<dim>::operator=(const Point<dim>& rhs)
00150 {
00151     // Compare pointer addresses
00152     if (this == &rhs) {
00153       return *this;
00154     }
00155 
00156     for(int i = 0; i < dim; ++i) {
00157       m_elem[i] = rhs.m_elem[i];
00158     }
00159 
00160     m_valid = rhs.m_valid;
00161 
00162     return *this;
00163 }
00164 
00165 template<const int dim>
00166 inline CoordType SquaredDistance(const Point<dim>& p1, const Point<dim>& p2)
00167 {
00168   CoordType ans = 0;
00169 
00170   for(int i = 0; i < dim; ++i) {
00171     CoordType diff = p1.m_elem[i] - p2.m_elem[i];
00172     ans += diff * diff;
00173   }
00174 
00175   return (fabs(ans) >= _ScaleEpsilon(p1.m_elem, p2.m_elem, dim)) ? ans : 0;
00176 }
00177 
00178 #ifndef WFMATH_NO_TEMPLATES_AS_TEMPLATE_PARAMETERS
00179 template<const int dim, template<class, class> class container,
00180                         template<class, class> class container2>
00181 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c,
00182                       const container2<CoordType, std::allocator<CoordType> >& weights)
00183 {
00184   // FIXME become friend
00185 
00186   typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator c_i = c.begin(), c_end = c.end();
00187   typename container2<CoordType, std::allocator<CoordType> >::const_iterator w_i = weights.begin(),
00188                                                  w_end = weights.end();
00189 
00190   Point<dim> out;
00191 
00192   if (c_i == c_end || w_i == w_end) {
00193     return out;
00194   }
00195 
00196   bool valid = c_i->isValid();
00197 
00198   CoordType tot_weight = *w_i, max_weight = fabs(*w_i);
00199   for(int j = 0; j < dim; ++j) {
00200     out[j] = (*c_i)[j] * *w_i;
00201   }
00202 
00203   while(++c_i != c_end && ++w_i != w_end) {
00204     tot_weight += *w_i;
00205     CoordType val = fabs(*w_i);
00206     if(val > max_weight)
00207       max_weight = val;
00208     if(!c_i->isValid())
00209       valid = false;
00210     for(int j = 0; j < dim; ++j)
00211       out[j] += (*c_i)[j] * *w_i;
00212   }
00213 
00214   // Make sure the weights don't add up to zero
00215   if (max_weight <= 0 || fabs(tot_weight) <= max_weight * WFMATH_EPSILON) {
00216     return out;
00217   }
00218 
00219   for(int j = 0; j < dim; ++j) {
00220     out[j] /= tot_weight;
00221   }
00222 
00223   out.setValid(valid);
00224 
00225   return out;
00226 }
00227 
00228 template<const int dim, template<class, class> class container>
00229 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c)
00230 {
00231   // FIXME become friend
00232 
00233   typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator i = c.begin(), end = c.end();
00234 
00235   if (i == end) {
00236     return Point<dim>();
00237   }
00238 
00239   Point<dim> out = *i;
00240   int num_points = 1;
00241 
00242   bool valid = i->isValid();
00243 
00244   while(++i != end) {
00245     ++num_points;
00246     if(!i->isValid())
00247       valid = false;
00248     for(int j = 0; j < dim; ++j)
00249       out[j] += (*i)[j];
00250   }
00251 
00252   for(int j = 0; j < dim; ++j) {
00253     out[j] /= num_points;
00254   }
00255 
00256   out.setValid(valid);
00257 
00258   return out;
00259 }
00260 #endif
00261 
00262 template<const int dim>
00263 inline Point<dim> Midpoint(const Point<dim>& p1, const Point<dim>& p2, CoordType dist)
00264 {
00265   Point<dim> out;
00266   CoordType conj_dist = 1 - dist;
00267 
00268   for(int i = 0; i < dim; ++i) {
00269     out.m_elem[i] = p1.m_elem[i] * conj_dist + p2.m_elem[i] * dist;
00270   }
00271 
00272   out.m_valid = p1.m_valid && p2.m_valid;
00273 
00274   return out;
00275 }
00276 
00277 template<> Point<2>::Point(CoordType x, CoordType y) : m_valid(true)
00278 {
00279   m_elem[0] = x;
00280   m_elem[1] = y;
00281 }
00282 
00283 template<> Point<3>::Point(CoordType x, CoordType y, CoordType z) : m_valid(true)
00284 {
00285   m_elem[0] = x;
00286   m_elem[1] = y;
00287   m_elem[2] = z;
00288 }
00289 
00290 #ifndef WFMATH_NO_CLASS_FUNCTION_SPECIALIZATION
00291 template<> Point<2>& Point<2>::polar(CoordType r, CoordType theta);
00292 template<> void Point<2>::asPolar(CoordType& r, CoordType& theta) const;
00293 
00294 template<> Point<3>& Point<3>::polar(CoordType r, CoordType theta,
00295                                      CoordType z);
00296 template<> void Point<3>::asPolar(CoordType& r, CoordType& theta,
00297                                   CoordType& z) const;
00298 template<> Point<3>& Point<3>::spherical(CoordType r, CoordType theta,
00299                                          CoordType phi);
00300 template<> void Point<3>::asSpherical(CoordType& r, CoordType& theta,
00301                                       CoordType& phi) const;
00302 #else
00303 void _NCFS_Point2_polar(CoordType *m_elem, CoordType r, CoordType theta);
00304 void _NCFS_Point2_asPolar(CoordType *m_elem, CoordType& r, CoordType& theta);
00305 
00306 void _NCFS_Point3_polar(CoordType *m_elem, CoordType r, CoordType theta,
00307                         CoordType z);
00308 void _NCFS_Point3_asPolar(CoordType *m_elem, CoordType& r, CoordType& theta,
00309                           CoordType& z);
00310 void _NCFS_Point3_spherical(CoordType *m_elem, CoordType r, CoordType theta,
00311                             CoordType phi);
00312 void _NCFS_Point3_asSpherical(CoordType *m_elem, CoordType& r, CoordType& theta,
00313                               CoordType& phi);
00314 
00315 template<>
00316 inline Point<2>& Point<2>::polar(CoordType r, CoordType theta)
00317 {
00318   _NCFS_Point2_polar((CoordType*) m_elem, r, theta);
00319   m_valid = true;
00320   return *this;
00321 }
00322 
00323 template<>
00324 inline void Point<2>::asPolar(CoordType& r, CoordType& theta) const
00325 {
00326   _NCFS_Point2_asPolar((CoordType*) m_elem, r, theta);
00327 }
00328 
00329 template<>
00330 inline Point<3>& Point<3>::polar(CoordType r, CoordType theta, CoordType z)
00331 {
00332   _NCFS_Point3_polar((CoordType*) m_elem, r, theta, z);
00333   m_valid = true;
00334   return *this;
00335 }
00336 
00337 template<>
00338 inline void Point<3>::asPolar(CoordType& r, CoordType& theta, CoordType& z) const
00339 {
00340   _NCFS_Point3_asPolar((CoordType*) m_elem, r, theta, z);
00341 }
00342 
00343 template<>
00344 inline Point<3>& Point<3>::spherical(CoordType r, CoordType theta, CoordType phi)
00345 {
00346   _NCFS_Point3_spherical((CoordType*) m_elem, r, theta, phi);
00347   m_valid = true;
00348   return *this;
00349 }
00350 
00351 template<>
00352 inline void Point<3>::asSpherical(CoordType& r, CoordType& theta, CoordType& phi) const
00353 {
00354   _NCFS_Point3_asSpherical((CoordType*) m_elem, r, theta, phi);
00355 }
00356 #endif
00357 
00358 } // namespace WFMath
00359 
00360 #endif  // WFMATH_POINT_FUNCS_H