WFMath  1.0.2
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, CoordType epsilon) const
00077 {
00078   CoordType delta = _ScaleEpsilon(m_elem, p.m_elem, dim, epsilon);
00079 
00080   for(int i = 0; i < dim; ++i) {
00081     if(std::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-=(Point<dim>& p, const Vector<dim> &rhs)
00117 {
00118     for(int i = 0; i < dim; ++i) {
00119       p.m_elem[i] -= rhs.m_elem[i];
00120     }
00121 
00122     p.m_valid = p.m_valid && rhs.m_valid;
00123 
00124     return p;
00125 }
00126 
00127 template<int dim>
00128 inline Point<dim>& Point<dim>::operator=(const Point<dim>& rhs)
00129 {
00130     // Compare pointer addresses
00131     if (this == &rhs) {
00132       return *this;
00133     }
00134 
00135     for(int i = 0; i < dim; ++i) {
00136       m_elem[i] = rhs.m_elem[i];
00137     }
00138 
00139     m_valid = rhs.m_valid;
00140 
00141     return *this;
00142 }
00143 
00144 template<int dim>
00145 inline CoordType SquaredDistance(const Point<dim>& p1, const Point<dim>& p2)
00146 {
00147   CoordType ans = 0;
00148 
00149   for(int i = 0; i < dim; ++i) {
00150     CoordType diff = p1.m_elem[i] - p2.m_elem[i];
00151     ans += diff * diff;
00152   }
00153 
00154   return (std::fabs(ans) >= _ScaleEpsilon(p1.m_elem, p2.m_elem, dim)) ? ans : 0;
00155 }
00156 
00157 template<int dim, template<class, class> class container,
00158                         template<class, class> class container2>
00159 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c,
00160                       const container2<CoordType, std::allocator<CoordType> >& weights)
00161 {
00162   // FIXME become friend
00163 
00164   typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator c_i = c.begin(), c_end = c.end();
00165   typename container2<CoordType, std::allocator<CoordType> >::const_iterator w_i = weights.begin(),
00166                                                  w_end = weights.end();
00167 
00168   Point<dim> out;
00169 
00170   if (c_i == c_end || w_i == w_end) {
00171     return out;
00172   }
00173 
00174   bool valid = c_i->isValid();
00175 
00176   CoordType tot_weight = *w_i, max_weight = std::fabs(*w_i);
00177   for(int j = 0; j < dim; ++j) {
00178     out[j] = (*c_i)[j] * *w_i;
00179   }
00180 
00181   while(++c_i != c_end && ++w_i != w_end) {
00182     tot_weight += *w_i;
00183     CoordType val = std::fabs(*w_i);
00184     if(val > max_weight)
00185       max_weight = val;
00186     if(!c_i->isValid())
00187       valid = false;
00188     for(int j = 0; j < dim; ++j)
00189       out[j] += (*c_i)[j] * *w_i;
00190   }
00191 
00192   // Make sure the weights don't add up to zero
00193   if (max_weight <= 0 || std::fabs(tot_weight) <= max_weight * numeric_constants<CoordType>::epsilon()) {
00194     return out;
00195   }
00196 
00197   for(int j = 0; j < dim; ++j) {
00198     out[j] /= tot_weight;
00199   }
00200 
00201   out.setValid(valid);
00202 
00203   return out;
00204 }
00205 
00206 template<int dim, template<class, class> class container>
00207 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c)
00208 {
00209   // FIXME become friend
00210 
00211   typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator i = c.begin(), end = c.end();
00212 
00213   if (i == end) {
00214     return Point<dim>();
00215   }
00216 
00217   Point<dim> out = *i;
00218   float num_points = 1;
00219 
00220   bool valid = i->isValid();
00221 
00222   while(++i != end) {
00223     ++num_points;
00224     if(!i->isValid())
00225       valid = false;
00226     for(int j = 0; j < dim; ++j)
00227       out[j] += (*i)[j];
00228   }
00229 
00230   for(int j = 0; j < dim; ++j) {
00231     out[j] /= num_points;
00232   }
00233 
00234   out.setValid(valid);
00235 
00236   return out;
00237 }
00238 
00239 template<int dim>
00240 inline Point<dim> Midpoint(const Point<dim>& p1, const Point<dim>& p2, CoordType dist)
00241 {
00242   Point<dim> out;
00243   CoordType conj_dist = 1 - dist;
00244 
00245   for(int i = 0; i < dim; ++i) {
00246     out.m_elem[i] = p1.m_elem[i] * conj_dist + p2.m_elem[i] * dist;
00247   }
00248 
00249   out.m_valid = p1.m_valid && p2.m_valid;
00250 
00251   return out;
00252 }
00253 
00254 template<> Point<2>& Point<2>::polar(CoordType r, CoordType theta);
00255 template<> void Point<2>::asPolar(CoordType& r, CoordType& theta) const;
00256 
00257 template<> Point<3>& Point<3>::polar(CoordType r, CoordType theta,
00258                                      CoordType z);
00259 template<> void Point<3>::asPolar(CoordType& r, CoordType& theta,
00260                                   CoordType& z) const;
00261 template<> Point<3>& Point<3>::spherical(CoordType r, CoordType theta,
00262                                          CoordType phi);
00263 template<> void Point<3>::asSpherical(CoordType& r, CoordType& theta,
00264                                       CoordType& phi) const;
00265 
00266 } // namespace WFMath
00267 
00268 #endif  // WFMATH_POINT_FUNCS_H