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