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
41namespace 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