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