WFMath  0.3.12
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, double epsilon) const
77 {
78  CoordType delta = (CoordType) _ScaleEpsilon(m_elem, p.m_elem, dim, epsilon);
79 
80  for(int i = 0; i < dim; ++i) {
81  if(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+(const Point<dim>& c, const Vector<dim>& v)
117 {
118  Point<dim> out(c);
119 
120  out += v;
121 
122  return out;
123 }
124 
125 template<int dim>
126 inline Point<dim> operator+(const Vector<dim>& v, const Point<dim>& c)
127 {
128  Point<dim> out(c);
129 
130  out += v;
131 
132  return out;
133 }
134 
135 template<int dim>
136 inline Point<dim>& operator-=(Point<dim>& p, const Vector<dim> &rhs)
137 {
138  for(int i = 0; i < dim; ++i) {
139  p.m_elem[i] -= rhs.m_elem[i];
140  }
141 
142  p.m_valid = p.m_valid && rhs.m_valid;
143 
144  return p;
145 }
146 
147 template<int dim>
148 inline Point<dim> operator-(const Point<dim>& c, const Vector<dim>& v)
149 {
150  Point<dim> out(c);
151 
152  out -= v;
153 
154  return out;
155 }
156 
157 template<int dim>
158 inline Point<dim>& Point<dim>::operator=(const Point<dim>& rhs)
159 {
160  // Compare pointer addresses
161  if (this == &rhs) {
162  return *this;
163  }
164 
165  for(int i = 0; i < dim; ++i) {
166  m_elem[i] = rhs.m_elem[i];
167  }
168 
169  m_valid = rhs.m_valid;
170 
171  return *this;
172 }
173 
174 template<int dim>
175 inline CoordType SquaredDistance(const Point<dim>& p1, const Point<dim>& p2)
176 {
177  CoordType ans = 0;
178 
179  for(int i = 0; i < dim; ++i) {
180  CoordType diff = p1.m_elem[i] - p2.m_elem[i];
181  ans += diff * diff;
182  }
183 
184  return (fabs(ans) >= _ScaleEpsilon(p1.m_elem, p2.m_elem, dim)) ? ans : 0;
185 }
186 
187 template<int dim, template<class, class> class container,
188  template<class, class> class container2>
189 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c,
190  const container2<CoordType, std::allocator<CoordType> >& weights)
191 {
192  // FIXME become friend
193 
194  typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator c_i = c.begin(), c_end = c.end();
195  typename container2<CoordType, std::allocator<CoordType> >::const_iterator w_i = weights.begin(),
196  w_end = weights.end();
197 
198  Point<dim> out;
199 
200  if (c_i == c_end || w_i == w_end) {
201  return out;
202  }
203 
204  bool valid = c_i->isValid();
205 
206  CoordType tot_weight = *w_i, max_weight = std::fabs(*w_i);
207  for(int j = 0; j < dim; ++j) {
208  out[j] = (*c_i)[j] * *w_i;
209  }
210 
211  while(++c_i != c_end && ++w_i != w_end) {
212  tot_weight += *w_i;
213  CoordType val = std::fabs(*w_i);
214  if(val > max_weight)
215  max_weight = val;
216  if(!c_i->isValid())
217  valid = false;
218  for(int j = 0; j < dim; ++j)
219  out[j] += (*c_i)[j] * *w_i;
220  }
221 
222  // Make sure the weights don't add up to zero
223  if (max_weight <= 0 || fabs(tot_weight) <= max_weight * WFMATH_EPSILON) {
224  return out;
225  }
226 
227  for(int j = 0; j < dim; ++j) {
228  out[j] /= tot_weight;
229  }
230 
231  out.setValid(valid);
232 
233  return out;
234 }
235 
236 template<int dim, template<class, class> class container>
237 Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c)
238 {
239  // FIXME become friend
240 
241  typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator i = c.begin(), end = c.end();
242 
243  if (i == end) {
244  return Point<dim>();
245  }
246 
247  Point<dim> out = *i;
248  float num_points = 1;
249 
250  bool valid = i->isValid();
251 
252  while(++i != end) {
253  ++num_points;
254  if(!i->isValid())
255  valid = false;
256  for(int j = 0; j < dim; ++j)
257  out[j] += (*i)[j];
258  }
259 
260  for(int j = 0; j < dim; ++j) {
261  out[j] /= num_points;
262  }
263 
264  out.setValid(valid);
265 
266  return out;
267 }
268 
269 template<int dim>
270 inline Point<dim> Midpoint(const Point<dim>& p1, const Point<dim>& p2, CoordType dist)
271 {
272  Point<dim> out;
273  CoordType conj_dist = 1 - dist;
274 
275  for(int i = 0; i < dim; ++i) {
276  out.m_elem[i] = p1.m_elem[i] * conj_dist + p2.m_elem[i] * dist;
277  }
278 
279  out.m_valid = p1.m_valid && p2.m_valid;
280 
281  return out;
282 }
283 
284 template<> Point<2>::Point(CoordType x, CoordType y) : m_valid(true)
285 {
286  m_elem[0] = x;
287  m_elem[1] = y;
288 }
289 
290 template<> Point<3>::Point(CoordType x, CoordType y, CoordType z) : m_valid(true)
291 {
292  m_elem[0] = x;
293  m_elem[1] = y;
294  m_elem[2] = z;
295 }
296 
297 template<> Point<2>& Point<2>::polar(CoordType r, CoordType theta);
298 template<> void Point<2>::asPolar(CoordType& r, CoordType& theta) const;
299 
300 template<> Point<3>& Point<3>::polar(CoordType r, CoordType theta,
301  CoordType z);
302 template<> void Point<3>::asPolar(CoordType& r, CoordType& theta,
303  CoordType& z) const;
304 template<> Point<3>& Point<3>::spherical(CoordType r, CoordType theta,
305  CoordType phi);
306 template<> void Point<3>::asSpherical(CoordType& r, CoordType& theta,
307  CoordType& phi) const;
308 
309 } // namespace WFMath
310 
311 #endif // WFMATH_POINT_FUNCS_H