WFMath  1.0.1
point_funcs.h
1 // point_funcs.h (point class copied from libCoal, subsequently modified)
2 //
3 // The WorldForge Project
4 // Copyright (C) 2000, 2001, 2002 The WorldForge Project
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU General Public License as published by
8 // the Free Software Foundation; either version 2 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 //
20 // For information about WorldForge and its authors, please contact
21 // the Worldforge Web Site at http://www.worldforge.org.
22 //
23 
24 // Author: Ron Steinke
25 
26 
27 #ifndef WFMATH_POINT_FUNCS_H
28 #define WFMATH_POINT_FUNCS_H
29 
30 #include <wfmath/point.h>
31 
32 #include <wfmath/vector.h>
33 #include <wfmath/zero.h>
34 
35 #include <cmath>
36 
37 namespace WFMath {
38 
39 template<int dim>
40 inline Point<dim>::Point(const Point<dim>& p) : m_valid(p.m_valid)
41 {
42  for(int i = 0; i < dim; ++i) {
43  m_elem[i] = p.m_elem[i];
44  }
45 }
46 
47 template<int dim>
48 inline Point<dim>::Point(const Vector<dim>& v) : m_valid(v.isValid())
49 {
50  for(int i = 0; i < dim; ++i) {
51  m_elem[i] = v.elements()[i];
52  }
53 }
54 
55 template<int dim>
57 {
58  static ZeroPrimitive<Point<dim> > zeroPoint(dim);
59  return zeroPoint.getShape();
60 }
61 
62 
63 template<int dim>
65 {
66  for(int i = 0; i < dim; ++i) {
67  m_elem[i] = 0;
68  }
69 
70  m_valid = true;
71 
72  return *this;
73 }
74 
75 template<int dim>
76 inline bool Point<dim>::isEqualTo(const Point<dim> &p, CoordType epsilon) const
77 {
78  CoordType delta = _ScaleEpsilon(m_elem, p.m_elem, dim, epsilon);
79 
80  for(int i = 0; i < dim; ++i) {
81  if(std::fabs(m_elem[i] - p.m_elem[i]) > delta) {
82  return false;
83  }
84  }
85 
86  return true;
87 }
88 
89 template<int dim>
90 inline Vector<dim> operator-(const Point<dim>& c1, const Point<dim>& c2)
91 {
92  Vector<dim> out;
93 
94  for(int i = 0; i < dim; ++i) {
95  out.m_elem[i] = c1.m_elem[i] - c2.m_elem[i];
96  }
97 
98  out.m_valid = c1.m_valid && c2.m_valid;
99 
100  return out;
101 }
102 
103 template<int dim>
104 inline Point<dim>& operator+=(Point<dim>& p, const Vector<dim> &rhs)
105 {
106  for(int i = 0; i < dim; ++i) {
107  p.m_elem[i] += rhs.m_elem[i];
108  }
109 
110  p.m_valid = p.m_valid && rhs.m_valid;
111 
112  return p;
113 }
114 
115 template<int dim>
116 inline Point<dim>& operator-=(Point<dim>& p, const Vector<dim> &rhs)
117 {
118  for(int i = 0; i < dim; ++i) {
119  p.m_elem[i] -= rhs.m_elem[i];
120  }
121 
122  p.m_valid = p.m_valid && rhs.m_valid;
123 
124  return p;
125 }
126 
127 template<int dim>
128 inline Point<dim>& Point<dim>::operator=(const Point<dim>& rhs)
129 {
130  // Compare pointer addresses
131  if (this == &rhs) {
132  return *this;
133  }
134 
135  for(int i = 0; i < dim; ++i) {
136  m_elem[i] = rhs.m_elem[i];
137  }
138 
139  m_valid = rhs.m_valid;
140 
141  return *this;
142 }
143 
144 template<int dim>
145 inline CoordType SquaredDistance(const Point<dim>& p1, const Point<dim>& p2)
146 {
147  CoordType ans = 0;
148 
149  for(int i = 0; i < dim; ++i) {
150  CoordType diff = p1.m_elem[i] - p2.m_elem[i];
151  ans += diff * diff;
152  }
153 
154  return (std::fabs(ans) >= _ScaleEpsilon(p1.m_elem, p2.m_elem, dim)) ? ans : 0;
155 }
156 
157 template<int dim, template<class, class> class container,
158  template<class, class> class container2>
159 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c,
160  const container2<CoordType, std::allocator<CoordType> >& weights)
161 {
162  // FIXME become friend
163 
164  typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator c_i = c.begin(), c_end = c.end();
165  typename container2<CoordType, std::allocator<CoordType> >::const_iterator w_i = weights.begin(),
166  w_end = weights.end();
167 
168  Point<dim> out;
169 
170  if (c_i == c_end || w_i == w_end) {
171  return out;
172  }
173 
174  bool valid = c_i->isValid();
175 
176  CoordType tot_weight = *w_i, max_weight = std::fabs(*w_i);
177  for(int j = 0; j < dim; ++j) {
178  out[j] = (*c_i)[j] * *w_i;
179  }
180 
181  while(++c_i != c_end && ++w_i != w_end) {
182  tot_weight += *w_i;
183  CoordType val = std::fabs(*w_i);
184  if(val > max_weight)
185  max_weight = val;
186  if(!c_i->isValid())
187  valid = false;
188  for(int j = 0; j < dim; ++j)
189  out[j] += (*c_i)[j] * *w_i;
190  }
191 
192  // Make sure the weights don't add up to zero
193  if (max_weight <= 0 || std::fabs(tot_weight) <= max_weight * numeric_constants<CoordType>::epsilon()) {
194  return out;
195  }
196 
197  for(int j = 0; j < dim; ++j) {
198  out[j] /= tot_weight;
199  }
200 
201  out.setValid(valid);
202 
203  return out;
204 }
205 
206 template<int dim, template<class, class> class container>
207 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c)
208 {
209  // FIXME become friend
210 
211  typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator i = c.begin(), end = c.end();
212 
213  if (i == end) {
214  return Point<dim>();
215  }
216 
217  Point<dim> out = *i;
218  float num_points = 1;
219 
220  bool valid = i->isValid();
221 
222  while(++i != end) {
223  ++num_points;
224  if(!i->isValid())
225  valid = false;
226  for(int j = 0; j < dim; ++j)
227  out[j] += (*i)[j];
228  }
229 
230  for(int j = 0; j < dim; ++j) {
231  out[j] /= num_points;
232  }
233 
234  out.setValid(valid);
235 
236  return out;
237 }
238 
239 template<int dim>
240 inline Point<dim> Midpoint(const Point<dim>& p1, const Point<dim>& p2, CoordType dist)
241 {
242  Point<dim> out;
243  CoordType conj_dist = 1 - dist;
244 
245  for(int i = 0; i < dim; ++i) {
246  out.m_elem[i] = p1.m_elem[i] * conj_dist + p2.m_elem[i] * dist;
247  }
248 
249  out.m_valid = p1.m_valid && p2.m_valid;
250 
251  return out;
252 }
253 
254 template<> Point<2>& Point<2>::polar(CoordType r, CoordType theta);
255 template<> void Point<2>::asPolar(CoordType& r, CoordType& theta) const;
256 
257 template<> Point<3>& Point<3>::polar(CoordType r, CoordType theta,
258  CoordType z);
259 template<> void Point<3>::asPolar(CoordType& r, CoordType& theta,
260  CoordType& z) const;
261 template<> Point<3>& Point<3>::spherical(CoordType r, CoordType theta,
262  CoordType phi);
263 template<> void Point<3>::asSpherical(CoordType& r, CoordType& theta,
264  CoordType& phi) const;
265 
266 } // namespace WFMath
267 
268 #endif // WFMATH_POINT_FUNCS_H