wfmath  1.0.3
A math library for the Worldforge system.
miniball_funcs.h
1 
2 
3 // Copright (C) 1999
4 // $Revision$
5 // $Date$
6 //
7 // This program is free software; you can redistribute it and/or modify
8 // it under the terms of the GNU General Public License as published by
9 // the Free Software Foundation; either version 2 of the License, or
10 // (at your option) any later version.
11 //
12 // This program is distributed in the hope that it will be useful,
13 // but WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 // GNU General Public License for more details.
16 //
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA,
20 // or download the License terms from prep.ai.mit.edu/pub/gnu/COPYING-2.0.
21 //
22 // Contact:
23 // --------
24 // Bernd Gaertner
25 // Institut f. Informatik
26 // ETH Zuerich
27 // ETH-Zentrum
28 // CH-8092 Zuerich, Switzerland
29 // http://www.inf.ethz.ch/personal/gaertner
30 //
31 
32 // 2001-1-9: included in WFMath backend. Namespace wrapping added
33 // and filename changed to follow WFMath conventions, but otherwise
34 // unchanged.
35 
36 #ifndef WFMATH_MINIBALL_FUNCS_H
37 #define WFMATH_MINIBALL_FUNCS_H
38 
39 #include <cassert>
40 
41 namespace WFMath { namespace _miniball {
42 
43  // Miniball
44  // --------
45 
46  template <int d>
47  void Miniball<d>::check_in (const Point& p)
48  {
49  L.push_back(p);
50  }
51 
52 
53  template <int d>
54  void Miniball<d>::build (bool pivoting)
55  {
56  B.reset();
57  support_end = L.begin();
58  if (pivoting)
59  pivot_mb (L.end());
60  else
61  mtf_mb (L.end());
62  }
63 
64 
65  template <int d>
66  void Miniball<d>::mtf_mb (It i)
67  {
68  support_end = L.begin();
69  if ((B.size())==d+1) return;
70  for (It k=L.begin(); k!=i;) {
71  It j=k++;
72  if (B.excess(*j) > 0) {
73  if (B.push(*j)) {
74  mtf_mb (j);
75  B.pop();
76  move_to_front(j);
77  }
78  }
79  }
80  }
81 
82  template <int d>
83  void Miniball<d>::move_to_front (It j)
84  {
85  if (support_end == j)
86  support_end++;
87  L.splice (L.begin(), L, j);
88  }
89 
90 
91  template <int d>
92  void Miniball<d>::pivot_mb (It i)
93  {
94  It t = ++L.begin();
95  mtf_mb (t);
96  double max_e, old_sqr_r;
97  do {
98  It pivot;
99  max_e = max_excess (t, i, pivot);
100  if (max_e <= 0)
101  break;
102  t = support_end;
103  if (t==pivot) ++t;
104  old_sqr_r = B.squared_radius();
105  B.push (*pivot);
106  mtf_mb (support_end);
107  B.pop();
108  move_to_front (pivot);
109  } while (B.squared_radius() > old_sqr_r);
110  }
111 
112 
113  template <int d>
114  double Miniball<d>::max_excess (It t, It i, It& pivot) const
115  {
116  const double *c = B.center(), sqr_r = B.squared_radius();
117  double e, max_e = 0;
118  for (It k=t; k!=i; ++k) {
119  const double *p = (*k).begin();
120  e = -sqr_r;
121  for (int j=0; j<d; ++j)
122  e += sqr(p[j]-c[j]);
123  if (e > max_e) {
124  max_e = e;
125  pivot = k;
126  }
127  }
128  return max_e;
129  }
130 
131 
132 
133  template <int d>
134  typename Miniball<d>::Point Miniball<d>::center () const
135  {
136  return Point(B.center());
137  }
138 
139  template <int d>
140  double Miniball<d>::squared_radius () const
141  {
142  return B.squared_radius();
143  }
144 
145 
146  template <int d>
147  int Miniball<d>::nr_points () const
148  {
149  return L.size();
150  }
151 
152  template <int d>
153  typename Miniball<d>::Cit Miniball<d>::points_begin () const
154  {
155  return L.begin();
156  }
157 
158  template <int d>
159  typename Miniball<d>::Cit Miniball<d>::points_end () const
160  {
161  return L.end();
162  }
163 
164 
165  template <int d>
166  int Miniball<d>::nr_support_points () const
167  {
168  return B.support_size();
169  }
170 
171  template <int d>
172  typename Miniball<d>::Cit Miniball<d>::support_points_begin () const
173  {
174  return L.begin();
175  }
176 
177  template <int d>
178  typename Miniball<d>::Cit Miniball<d>::support_points_end () const
179  {
180  return support_end;
181  }
182 
183 
184 
185  template <int d>
186  double Miniball<d>::accuracy (double& slack) const
187  {
188  double e, max_e = 0;
189  int n_supp=0;
190  Cit i;
191  for (i=L.begin(); i!=support_end; ++i,++n_supp)
192  if ((e = abs (B.excess (*i))) > max_e)
193  max_e = e;
194 
195  // you've found a non-numerical problem if the following ever fails
196  assert (n_supp == nr_support_points());
197 
198  for (i=support_end; i!=L.end(); ++i)
199  if ((e = B.excess (*i)) > max_e)
200  max_e = e;
201 
202  slack = B.slack();
203  return (max_e/squared_radius());
204  }
205 
206 
207  template <int d>
208  bool Miniball<d>::is_valid (double tolerance) const
209  {
210  double slack;
211  return ( (accuracy (slack) < tolerance) && (slack == 0) );
212  }
213 
214 
215 
216  // Basis
217  // -----
218 
219  template <int d>
220  const double* Basis<d>::center () const
221  {
222  return current_c;
223  }
224 
225  template <int d>
226  double Basis<d>::squared_radius() const
227  {
228  return current_sqr_r;
229  }
230 
231  template <int d>
232  int Basis<d>::size() const
233  {
234  return m;
235  }
236 
237  template <int d>
238  int Basis<d>::support_size() const
239  {
240  return s;
241  }
242 
243  template <int d>
244  double Basis<d>::excess (const Point& p) const
245  {
246  double e = -current_sqr_r;
247  for (int k=0; k<d; ++k)
248  e += sqr(p[k]-current_c[k]);
249  return e;
250  }
251 
252 
253 
254  template <int d>
255  void Basis<d>::reset ()
256  {
257  m = s = 0;
258  // we misuse c[0] for the center of the empty sphere
259  for (int j=0; j<d; ++j)
260  c[0][j]=0;
261  current_c = c[0];
262  current_sqr_r = -1;
263  }
264 
265 
266  template <int d>
267  Basis<d>::Basis () : m(0), s(0), current_c(0), current_sqr_r(-1.)
268  {
269  reset();
270  }
271 
272 
273  template <int d>
274  void Basis<d>::pop ()
275  {
276  --m;
277  }
278 
279 
280  template <int d>
281  bool Basis<d>::push (const Point& p)
282  {
283  int i, j;
284  double eps = 1e-32;
285  if (m==0) {
286  for (i=0; i<d; ++i)
287  q0[i] = p[i];
288  for (i=0; i<d; ++i)
289  c[0][i] = q0[i];
290  sqr_r[0] = 0;
291  } else {
292  // set v_m to Q_m
293  for (i=0; i<d; ++i)
294  v[m][i] = p[i]-q0[i];
295 
296  // compute the a_{m,i}, i< m
297  for (i=1; i<m; ++i) {
298  a[m][i] = 0;
299  for (j=0; j<d; ++j)
300  a[m][i] += v[i][j] * v[m][j];
301  a[m][i]*=(2/z[i]);
302  }
303 
304  // update v_m to Q_m-\bar{Q}_m
305  for (i=1; i<m; ++i) {
306  for (j=0; j<d; ++j)
307  v[m][j] -= a[m][i]*v[i][j];
308  }
309 
310  // compute z_m
311  z[m]=0;
312  for (j=0; j<d; ++j)
313  z[m] += sqr(v[m][j]);
314  z[m]*=2;
315 
316  // reject push if z_m too small
317  if (z[m]<eps*current_sqr_r) {
318  return false;
319  }
320 
321  // update c, sqr_r
322  double e = -sqr_r[m-1];
323  for (i=0; i<d; ++i)
324  e += sqr(p[i]-c[m-1][i]);
325  f[m]=e/z[m];
326 
327  for (i=0; i<d; ++i)
328  c[m][i] = c[m-1][i]+f[m]*v[m][i];
329  sqr_r[m] = sqr_r[m-1] + e*f[m]/2;
330  }
331  current_c = c[m];
332  current_sqr_r = sqr_r[m];
333  s = ++m;
334  return true;
335  }
336 
337 
338 
339  template <int d>
340  double Basis<d>::slack () const
341  {
342  double l[d+1], min_l=0;
343  l[0] = 1;
344  for (int i=s-1; i>0; --i) {
345  l[i] = f[i];
346  for (int k=s-1; k>i; --k)
347  l[i]-=a[k][i]*l[k];
348  if (l[i] < min_l) min_l = l[i];
349  l[0] -= l[i];
350  }
351  if (l[0] < min_l) min_l = l[0];
352  return ( (min_l < 0) ? -min_l : 0);
353  }
354 
355 }} // namespace WFMath::_miniball
356 
357 #endif // WFMATH_MINIBALL_FUNCS_H
Generic library namespace.
Definition: shape.h:41