Miniball.java
Go to the documentation of this file.
00001 /*******************************************************************************
00002  * Copyright (c) 2012 Bernd Gaertner. All rights reserved. This program and the accompanying
00003  * materials are made available under the terms of the GNU Public License v3.0 which accompanies
00004  * this distribution, and is available at http://www.gnu.org/licenses/gpl.html
00005  * 
00006  * Contributors: Bernd Gaertner - initial API and implementation, Year: 2006 Stefan Profanter -
00007  * Ported original C code to Java, Year: 2012
00008  ******************************************************************************/
00009 package edu.tum.cs.vis.model.util.algorithm;
00010 
00011 import java.util.ArrayList;
00012 
00070 @SuppressWarnings("javadoc")
00071 public class Miniball {
00072 
00073         class Miniball_b {
00074 
00075                 int                     m, s;                                                           // size and number of support points
00076                 double[]        q0                      = new double[d];
00077                 double[]        z                       = new double[d + 1];
00078                 double[]        f                       = new double[d + 1];
00079                 double[][]      v                       = new double[d + 1][d];
00080                 double[][]      a                       = new double[d + 1][d];
00081                 double[][]      c                       = new double[d + 1][d];
00082                 double[]        sqr_r           = new double[d + 1];
00083                 double[]        current_c       = new double[d];                // refers to some c[j]
00084                 double          current_sqr_r;
00085 
00086                 double excess(double[] p) {
00087                         double e = -current_sqr_r;
00088                         for (int k = 0; k < d; ++k) {
00089                                 e += mb_sqr(p[k] - current_c[k]);
00090                         }
00091                         return e;
00092                 }
00093 
00094                 double[] getCenter() {
00095                         return (current_c);
00096                 }
00097 
00098                 void pop() {
00099                         --m;
00100                 }
00101 
00102                 boolean push(double[] p) {
00103                         // System.out.println("Miniball_b:push");
00104                         int i, j;
00105                         double eps = 1e-32;
00106 
00107                         if (m == 0) {
00108                                 for (i = 0; i < d; ++i) {
00109                                         q0[i] = p[i];
00110                                 }
00111                                 for (i = 0; i < d; ++i) {
00112                                         c[0][i] = q0[i];
00113                                 }
00114                                 sqr_r[0] = 0;
00115                         } else {
00116                                 // set v_m to Q_m
00117                                 for (i = 0; i < d; ++i) {
00118                                         v[m][i] = p[i] - q0[i];
00119                                 }
00120 
00121                                 // compute the a_{m,i}, i< m
00122                                 for (i = 1; i < m; ++i) {
00123                                         a[m][i] = 0;
00124                                         for (j = 0; j < d; ++j) {
00125                                                 a[m][i] += v[i][j] * v[m][j];
00126                                         }
00127                                         a[m][i] *= (2 / z[i]);
00128                                 }
00129 
00130                                 // update v_m to Q_m-\bar{Q}_m
00131                                 for (i = 1; i < m; ++i) {
00132                                         for (j = 0; j < d; ++j) {
00133                                                 v[m][j] -= a[m][i] * v[i][j];
00134                                         }
00135                                 }
00136 
00137                                 // compute z_m
00138                                 z[m] = 0;
00139                                 for (j = 0; j < d; ++j) {
00140                                         z[m] += mb_sqr(v[m][j]);
00141                                 }
00142                                 z[m] *= 2;
00143 
00144                                 // reject push if z_m too small
00145                                 if (z[m] < eps * current_sqr_r) {
00146                                         return false;
00147                                 }
00148 
00149                                 // update c, sqr_r
00150                                 double e = -sqr_r[m - 1];
00151                                 for (i = 0; i < d; ++i) {
00152                                         e += mb_sqr(p[i] - c[m - 1][i]);
00153                                 }
00154                                 f[m] = e / z[m];
00155 
00156                                 for (i = 0; i < d; ++i) {
00157                                         c[m][i] = c[m - 1][i] + f[m] * v[m][i];
00158                                 }
00159                                 sqr_r[m] = sqr_r[m - 1] + e * f[m] / 2;
00160                         }
00161                         current_c = c[m];
00162                         current_sqr_r = sqr_r[m];
00163                         s = ++m;
00164                         return true;
00165                 }
00166 
00167                 void reset() {
00168                         m = 0;
00169                         s = 0;
00170                         // we misuse c[0] for the center of the empty sphere
00171                         for (int j = 0; j < d; j++) {
00172                                 c[0][j] = 0;
00173                         }
00174                         current_c = c[0];
00175                         current_sqr_r = -1;
00176                 }
00177 
00178                 int size() {
00179                         return m;
00180                 }
00181 
00182                 double slack() {
00183                         double min_l = 0;
00184                         double[] l = new double[d + 1];
00185                         l[0] = 1;
00186                         for (int i = s - 1; i > 0; --i) {
00187                                 l[i] = f[i];
00188                                 for (int k = s - 1; k > i; --k) {
00189                                         l[i] -= a[k][i] * l[k];
00190                                 }
00191                                 if (l[i] < min_l) {
00192                                         min_l = l[i];
00193                                 }
00194                                 l[0] -= l[i];
00195                         }
00196                         if (l[0] < min_l) {
00197                                 min_l = l[0];
00198                         }
00199                         return ((min_l < 0) ? -min_l : 0);
00200                 }
00201 
00202                 double squared_radius() {
00203                         return current_sqr_r;
00204                 }
00205 
00206                 int support_size() {
00207                         return s;
00208                 }
00209         }
00210 
00211         class pvt {
00212                 int     val;
00213 
00214                 pvt() {
00215                         val = 0;
00216                 }
00217 
00218                 int getVal() {
00219                         return (val);
00220                 }
00221 
00222                 void setVal(int i) {
00223                         val = i;
00224                 }
00225         }
00226 
00227         static double mb_sqr(double r) {
00228                 return r * r;
00229         }
00230 
00231         static int points_begin() {
00232                 return (0);
00233         }
00234 
00235         static int support_points_begin() {
00236                 return (0);
00237         }
00238 
00239         int                     d;
00240 
00241         @SuppressWarnings("rawtypes")
00242         ArrayList       L;
00243 
00244         Miniball_b      B;
00245 
00246         int                     support_end     = 0;
00247 
00248         @SuppressWarnings("rawtypes")
00249         public Miniball(int dim) {
00250                 d = dim;
00251                 L = new ArrayList();
00252                 B = new Miniball_b();
00253         }
00254 
00255         double accuracy(double slack) {
00256                 double e, max_e = 0;
00257                 int n_supp = 0;
00258 
00259                 int i;
00260                 for (i = points_begin(); i != support_end; ++i, ++n_supp) {
00261                         double[] sp = (double[]) L.get(i);
00262                         if ((e = Math.abs(B.excess(sp))) > max_e) {
00263                                 max_e = e;
00264                         }
00265                 }
00266                 if (n_supp == nr_support_points()) {
00267                         System.out.println("Miniball.accuracy WARNING: STRANGE PROBLEM HERE!");
00268                 }
00269                 for (i = support_end; i != points_end(); ++i) {
00270                         double[] sp = (double[]) L.get(i);
00271                         if ((e = B.excess(sp)) > max_e) {
00272                                 max_e = e;
00273                         }
00274                 }
00275                 // slack = B.slack();
00276                 return (max_e / squared_radius());
00277         }
00278 
00283         public void build() {
00284                 B.reset();
00285                 support_end = 0;
00286                 pivot_mb(points_end());
00287         }
00288 
00294         public double[] center() {
00295                 return B.getCenter();
00296         }
00297 
00305         @SuppressWarnings("unchecked")
00306         public void check_in(double[] p) {
00307                 if (p != null) {
00308                         L.add(p);
00309                 } else {
00310                         System.out.println("Miniball.check_in WARNING: Skipping null point");
00311                 }
00312         }
00313 
00321         public void clear() {
00322                 L.clear();
00323         }
00324 
00325         boolean is_valid(double tolerance) {
00326                 double slack = 0.0;
00327                 return ((accuracy(slack) < tolerance) && (slack == 0));
00328         }
00329 
00330         double max_excess(int t, int i, pvt pivot) {
00331                 double[] c = B.getCenter();
00332                 double sqr_r = B.squared_radius();
00333                 double e, max_e = 0;
00334                 for (int k = t; k != i; ++k) {
00335                         double[] p = (double[]) L.get(k);
00336 
00337                         e = -sqr_r;
00338                         for (int j = 0; j < d; ++j) {
00339                                 e += mb_sqr(p[j] - c[j]);
00340                         }
00341                         if (e > max_e) {
00342                                 max_e = e;
00343                                 pivot.setVal(k);
00344                         }
00345                 }
00346                 return max_e;
00347         }
00348 
00349         @SuppressWarnings("unchecked")
00350         void move_to_front(int j) {
00351 
00352                 if (support_end <= j) {
00353                         support_end++;
00354                 }
00355                 // L.splice (L.begin(), L, j);
00356                 double[] sp = (double[]) L.get(j);
00357                 L.remove(j);
00358                 L.add(0, sp);
00359         }
00360 
00361         void mtf_mb(int i) {
00362                 int pj = 0;
00363                 support_end = points_begin();
00364                 if ((B.size()) == d + 1) {
00365                         return;
00366                 }
00367                 for (int k = points_begin(); k != i;) {
00368                         pj = pj + 1;
00369                         int j = k++;
00370                         double[] sp = (double[]) L.get(j);
00371                         if (B.excess(sp) > 0) {
00372                                 if (B.push(sp)) {
00373                                         mtf_mb(j);
00374                                         B.pop();
00375                                         move_to_front(j);
00376                                 }
00377                         }
00378                 }
00379         }
00380 
00386         public int nr_points() {
00387                 return L.size();
00388         }
00389 
00396         public int nr_support_points() {
00397                 return B.support_size();
00398         }
00399 
00400         void pivot_mb(int i) {
00401                 int t = 1;
00402                 mtf_mb(t);
00403                 double max_e = 0.0, old_sqr_r = -1;
00404                 pvt pivot = new pvt();
00405                 do {
00406                         max_e = max_excess(t, i, pivot);
00407                         if (max_e > 0) {
00408                                 t = support_end;
00409                                 if (t == pivot.getVal()) {
00410                                         ++t;
00411                                 }
00412                                 old_sqr_r = B.squared_radius();
00413                                 double[] sp = (double[]) L.get(pivot.getVal());
00414                                 B.push(sp);
00415                                 mtf_mb(support_end);
00416                                 B.pop();
00417                                 move_to_front(pivot.getVal());
00418                         }
00419                 } while ((max_e > 0) && (B.squared_radius() > old_sqr_r));
00420         }
00421 
00422         int points_end() {
00423                 return (L.size());
00424         }
00425 
00431         public double radius() {
00432                 return ((1 + 0.00001) * Math.sqrt(B.squared_radius()));
00433         }
00434 
00440         public double squared_radius() {
00441                 return B.squared_radius();
00442         }
00443 
00444         int support_points_end() {
00445                 return support_end;
00446         }
00447 }


knowrob_cad_parser
Author(s): Stefan Profanter
autogenerated on Mon Oct 6 2014 01:29:56