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 namespace WFMath { namespace _miniball {
40 
41  // Miniball
42  // --------
43 
44  template <int d>
45  void Miniball<d>::check_in (const Point& p)
46  {
47  L.push_back(p);
48  }
49 
50 
51  template <int d>
52  void Miniball<d>::build (bool pivoting)
53  {
54  B.reset();
55  support_end = L.begin();
56  if (pivoting)
57  pivot_mb (L.end());
58  else
59  mtf_mb (L.end());
60  }
61 
62 
63  template <int d>
64  void Miniball<d>::mtf_mb (It i)
65  {
66  support_end = L.begin();
67  if ((B.size())==d+1) return;
68  for (It k=L.begin(); k!=i;) {
69  It j=k++;
70  if (B.excess(*j) > 0) {
71  if (B.push(*j)) {
72  mtf_mb (j);
73  B.pop();
74  move_to_front(j);
75  }
76  }
77  }
78  }
79 
80  template <int d>
81  void Miniball<d>::move_to_front (It j)
82  {
83  if (support_end == j)
84  support_end++;
85  L.splice (L.begin(), L, j);
86  }
87 
88 
89  template <int d>
90  void Miniball<d>::pivot_mb (It i)
91  {
92  It t = ++L.begin();
93  mtf_mb (t);
94  double max_e, old_sqr_r;
95  do {
96  It pivot;
97  max_e = max_excess (t, i, pivot);
98  if (max_e <= 0)
99  break;
100  t = support_end;
101  if (t==pivot) ++t;
102  old_sqr_r = B.squared_radius();
103  B.push (*pivot);
104  mtf_mb (support_end);
105  B.pop();
106  move_to_front (pivot);
107  } while (B.squared_radius() > old_sqr_r);
108  }
109 
110 
111  template <int d>
112  double Miniball<d>::max_excess (It t, It i, It& pivot) const
113  {
114  const double *c = B.center(), sqr_r = B.squared_radius();
115  double e, max_e = 0;
116  for (It k=t; k!=i; ++k) {
117  const double *p = (*k).begin();
118  e = -sqr_r;
119  for (int j=0; j<d; ++j)
120  e += sqr(p[j]-c[j]);
121  if (e > max_e) {
122  max_e = e;
123  pivot = k;
124  }
125  }
126  return max_e;
127  }
128 
129 
130 
131  template <int d>
132  typename Miniball<d>::Point Miniball<d>::center () const
133  {
134  return Point(B.center());
135  }
136 
137  template <int d>
138  double Miniball<d>::squared_radius () const
139  {
140  return B.squared_radius();
141  }
142 
143 
144  template <int d>
145  int Miniball<d>::nr_points () const
146  {
147  return L.size();
148  }
149 
150  template <int d>
151  typename Miniball<d>::Cit Miniball<d>::points_begin () const
152  {
153  return L.begin();
154  }
155 
156  template <int d>
157  typename Miniball<d>::Cit Miniball<d>::points_end () const
158  {
159  return L.end();
160  }
161 
162 
163  template <int d>
164  int Miniball<d>::nr_support_points () const
165  {
166  return B.support_size();
167  }
168 
169  template <int d>
170  typename Miniball<d>::Cit Miniball<d>::support_points_begin () const
171  {
172  return L.begin();
173  }
174 
175  template <int d>
176  typename Miniball<d>::Cit Miniball<d>::support_points_end () const
177  {
178  return support_end;
179  }
180 
181 
182 
183  template <int d>
184  double Miniball<d>::accuracy (double& slack) const
185  {
186  double e, max_e = 0;
187  int n_supp=0;
188  Cit i;
189  for (i=L.begin(); i!=support_end; ++i,++n_supp)
190  if ((e = abs (B.excess (*i))) > max_e)
191  max_e = e;
192 
193  // you've found a non-numerical problem if the following ever fails
194  assert (n_supp == nr_support_points());
195 
196  for (i=support_end; i!=L.end(); ++i)
197  if ((e = B.excess (*i)) > max_e)
198  max_e = e;
199 
200  slack = B.slack();
201  return (max_e/squared_radius());
202  }
203 
204 
205  template <int d>
206  bool Miniball<d>::is_valid (double tolerance) const
207  {
208  double slack;
209  return ( (accuracy (slack) < tolerance) && (slack == 0) );
210  }
211 
212 
213 
214  // Basis
215  // -----
216 
217  template <int d>
218  const double* Basis<d>::center () const
219  {
220  return current_c;
221  }
222 
223  template <int d>
224  double Basis<d>::squared_radius() const
225  {
226  return current_sqr_r;
227  }
228 
229  template <int d>
230  int Basis<d>::size() const
231  {
232  return m;
233  }
234 
235  template <int d>
236  int Basis<d>::support_size() const
237  {
238  return s;
239  }
240 
241  template <int d>
242  double Basis<d>::excess (const Point& p) const
243  {
244  double e = -current_sqr_r;
245  for (int k=0; k<d; ++k)
246  e += sqr(p[k]-current_c[k]);
247  return e;
248  }
249 
250 
251 
252  template <int d>
253  void Basis<d>::reset ()
254  {
255  m = s = 0;
256  // we misuse c[0] for the center of the empty sphere
257  for (int j=0; j<d; ++j)
258  c[0][j]=0;
259  current_c = c[0];
260  current_sqr_r = -1;
261  }
262 
263 
264  template <int d>
265  Basis<d>::Basis () : m(0), s(0), current_c(0), current_sqr_r(-1.)
266  {
267  reset();
268  }
269 
270 
271  template <int d>
272  void Basis<d>::pop ()
273  {
274  --m;
275  }
276 
277 
278  template <int d>
279  bool Basis<d>::push (const Point& p)
280  {
281  int i, j;
282  double eps = 1e-32;
283  if (m==0) {
284  for (i=0; i<d; ++i)
285  q0[i] = p[i];
286  for (i=0; i<d; ++i)
287  c[0][i] = q0[i];
288  sqr_r[0] = 0;
289  } else {
290  // set v_m to Q_m
291  for (i=0; i<d; ++i)
292  v[m][i] = p[i]-q0[i];
293 
294  // compute the a_{m,i}, i< m
295  for (i=1; i<m; ++i) {
296  a[m][i] = 0;
297  for (j=0; j<d; ++j)
298  a[m][i] += v[i][j] * v[m][j];
299  a[m][i]*=(2/z[i]);
300  }
301 
302  // update v_m to Q_m-\bar{Q}_m
303  for (i=1; i<m; ++i) {
304  for (j=0; j<d; ++j)
305  v[m][j] -= a[m][i]*v[i][j];
306  }
307 
308  // compute z_m
309  z[m]=0;
310  for (j=0; j<d; ++j)
311  z[m] += sqr(v[m][j]);
312  z[m]*=2;
313 
314  // reject push if z_m too small
315  if (z[m]<eps*current_sqr_r) {
316  return false;
317  }
318 
319  // update c, sqr_r
320  double e = -sqr_r[m-1];
321  for (i=0; i<d; ++i)
322  e += sqr(p[i]-c[m-1][i]);
323  f[m]=e/z[m];
324 
325  for (i=0; i<d; ++i)
326  c[m][i] = c[m-1][i]+f[m]*v[m][i];
327  sqr_r[m] = sqr_r[m-1] + e*f[m]/2;
328  }
329  current_c = c[m];
330  current_sqr_r = sqr_r[m];
331  s = ++m;
332  return true;
333  }
334 
335 
336 
337  template <int d>
338  double Basis<d>::slack () const
339  {
340  double l[d+1], min_l=0;
341  l[0] = 1;
342  for (int i=s-1; i>0; --i) {
343  l[i] = f[i];
344  for (int k=s-1; k>i; --k)
345  l[i]-=a[k][i]*l[k];
346  if (l[i] < min_l) min_l = l[i];
347  l[0] -= l[i];
348  }
349  if (l[0] < min_l) min_l = l[0];
350  return ( (min_l < 0) ? -min_l : 0);
351  }
352 
353 }} // namespace WFMath::_miniball
354 
355 #endif // WFMATH_MINIBALL_FUNCS_H
Generic library namespace.
Definition: shape.h:41