00001
00002 #ifdef VCL_NEEDS_PRAGMA_INTERFACE
00003 #pragma implementation
00004 #endif
00005
00006
00007 #include "vnl_rnpoly_solve.h"
00008
00009 #include <vnl/vnl_math.h>
00010 #include <vcl_cmath.h>
00011 #include <vcl_cassert.h>
00012 #ifdef DEBUG
00013 #include <vcl_iostream.h>
00014 #include <vcl_fstream.h>
00015 #endif
00016
00017 static unsigned int dim_ = 0;
00018 static unsigned int max_deg_ = 0;
00019 static unsigned int max_nterms_ = 0;
00020
00021
00022 class vnl_rnpoly_solve_cmplx
00023 {
00024 public:
00025 double R;
00026 double C;
00027 vnl_rnpoly_solve_cmplx(double a=0, double b=0) : R(a), C(b) {}
00028 inline double norm() const { return R*R+C*C; }
00029 inline vnl_rnpoly_solve_cmplx operator-() const
00030 { return vnl_rnpoly_solve_cmplx(-R, -C); }
00031 inline vnl_rnpoly_solve_cmplx operator+(vnl_rnpoly_solve_cmplx const& Y) const
00032 { return vnl_rnpoly_solve_cmplx(R+Y.R, C+Y.C); }
00033 inline vnl_rnpoly_solve_cmplx operator-(vnl_rnpoly_solve_cmplx const& Y) const
00034 { return vnl_rnpoly_solve_cmplx(R-Y.R, C-Y.C); }
00035 inline vnl_rnpoly_solve_cmplx& operator+=(vnl_rnpoly_solve_cmplx const& Y)
00036 { R+=Y.R; C+=Y.C; return *this; }
00037 inline vnl_rnpoly_solve_cmplx& operator-=(vnl_rnpoly_solve_cmplx const& Y)
00038 { R-=Y.R; C-=Y.C; return *this; }
00039 inline vnl_rnpoly_solve_cmplx operator*(vnl_rnpoly_solve_cmplx const& Y) const
00040 { return vnl_rnpoly_solve_cmplx(R*Y.R-C*Y.C, R*Y.C+C*Y.R); }
00041 inline vnl_rnpoly_solve_cmplx operator/(vnl_rnpoly_solve_cmplx const& Y) const
00042 { double N=1.0/Y.norm(); return vnl_rnpoly_solve_cmplx((R*Y.R+C*Y.C)*N, (C*Y.R-R*Y.C)*N); }
00043 inline vnl_rnpoly_solve_cmplx operator*(double T) const
00044 { return vnl_rnpoly_solve_cmplx(R*T, C*T); }
00045 inline vnl_rnpoly_solve_cmplx& operator*=(double T)
00046 { R*=T; C*=T; return *this; }
00047 inline vnl_rnpoly_solve_cmplx& operator*=(vnl_rnpoly_solve_cmplx const& Y)
00048 { double r=R*Y.R-C*Y.C; C=R*Y.C+C*Y.R; R=r; return *this; }
00049 inline vnl_rnpoly_solve_cmplx& operator/=(vnl_rnpoly_solve_cmplx const& Y)
00050 { return *this = operator/(Y); }
00051 };
00052
00053 static const double twopi = 2.0*vnl_math::pi;
00054
00055 static const double epsilonB = 2.e-03;
00056 static const vnl_rnpoly_solve_cmplx epsilonZ = vnl_rnpoly_solve_cmplx(1.e-04,1.e-04);
00057 static const double final_eps = 1.e-10;
00058 static const double stepinit = 1.e-02;
00059
00060
00061 vcl_vector<vnl_vector<double>*> vnl_rnpoly_solve::realroots(double tol)
00062 {
00063 tol *= tol;
00064 vcl_vector<vnl_vector<double>*> rr;
00065 vcl_vector<vnl_vector<double>*>::iterator rp = r_.begin(), ip = i_.begin();
00066 for (; rp != r_.end() && ip != i_.end(); ++rp, ++ip)
00067 if ((*ip)->squared_magnitude() < tol)
00068 rr.push_back(*rp);
00069
00070 return rr;
00071 }
00072
00073
00074
00075
00076
00077
00078
00079 static void inptbr(vcl_vector<vnl_rnpoly_solve_cmplx>& p, vcl_vector<vnl_rnpoly_solve_cmplx>& q)
00080 {
00081 vnl_rnpoly_solve_cmplx pp[10],qq[10];
00082
00083 pp[0] = vnl_rnpoly_solve_cmplx(.12324754231, .76253746298);
00084 pp[1] = vnl_rnpoly_solve_cmplx(.93857838950, -.99375892810);
00085 pp[2] = vnl_rnpoly_solve_cmplx(-.23467908356, .39383930009);
00086 pp[3] = vnl_rnpoly_solve_cmplx(.83542556622, -.10192888288);
00087 pp[4] = vnl_rnpoly_solve_cmplx(-.55763522521, -.83729899911);
00088 pp[5] = vnl_rnpoly_solve_cmplx(-.78348738738, -.10578234903);
00089 pp[6] = vnl_rnpoly_solve_cmplx(.03938347346, .04825184716);
00090 pp[7] = vnl_rnpoly_solve_cmplx(-.43428734331, .93836289418);
00091 pp[8] = vnl_rnpoly_solve_cmplx(-.99383729993, -.40947822291);
00092 pp[9] = vnl_rnpoly_solve_cmplx(.09383736736, .26459172298);
00093
00094 qq[0] = vnl_rnpoly_solve_cmplx(.58720452864, .01321964722);
00095 qq[1] = vnl_rnpoly_solve_cmplx(.97884134700, -.14433009712);
00096 qq[2] = vnl_rnpoly_solve_cmplx(.39383737289, .4154322311);
00097 qq[3] = vnl_rnpoly_solve_cmplx(-.03938376373, -.61253112318);
00098 qq[4] = vnl_rnpoly_solve_cmplx(.39383737388, -.26454678861);
00099 qq[5] = vnl_rnpoly_solve_cmplx(-.0093837766, .34447867861);
00100 qq[6] = vnl_rnpoly_solve_cmplx(-.04837366632, .48252736790);
00101 qq[7] = vnl_rnpoly_solve_cmplx(.93725237347, -.54356527623);
00102 qq[8] = vnl_rnpoly_solve_cmplx(.39373957747, .65573434564);
00103 qq[9] = vnl_rnpoly_solve_cmplx(-.39380038371, .98903450052);
00104
00105 p.resize(dim_); q.resize(dim_);
00106 for (unsigned int j=0; j<dim_; ++j) { int jj=j%10; p[j]=pp[jj]; q[j]=qq[jj]; }
00107 }
00108
00109
00110
00111 static inline vnl_rnpoly_solve_cmplx powr(int n,vnl_rnpoly_solve_cmplx const& y)
00112 {
00113 vnl_rnpoly_solve_cmplx x(1,0);
00114 if (n>0) while (n--) x *= y;
00115 else while (n++) x /= y;
00116 return x;
00117 }
00118
00119
00120 static void initr(vcl_vector<unsigned int> const& ideg,
00121 vcl_vector<vnl_rnpoly_solve_cmplx> const& p,
00122 vcl_vector<vnl_rnpoly_solve_cmplx> const& q,
00123 vcl_vector<vnl_rnpoly_solve_cmplx>& r,
00124 vcl_vector<vnl_rnpoly_solve_cmplx>& pdg,
00125 vcl_vector<vnl_rnpoly_solve_cmplx>& qdg)
00126 {
00127 assert(ideg.size()==dim_);
00128 assert(p.size()==dim_);
00129 assert(q.size()==dim_);
00130 pdg.resize(dim_); qdg.resize(dim_); r.resize(dim_);
00131 for (unsigned int j=0;j<dim_;j++)
00132 {
00133 pdg[j] = powr(ideg[j],p[j]);
00134 qdg[j] = powr(ideg[j],q[j]);
00135 r[j] = q[j] / p[j];
00136 }
00137 }
00138
00139
00140
00141
00142 static inline int degree(int index)
00143 {
00144 return (index<0) ? 0 : (index % max_deg_) + 1;
00145 }
00146
00147
00148
00149
00150
00151 static void ffunr(vcl_vector<double> const& coeff,
00152 vcl_vector<int> const& polyn,
00153 vcl_vector<unsigned int> const& terms,
00154 vcl_vector<vnl_rnpoly_solve_cmplx> const& x,
00155 vcl_vector<vnl_rnpoly_solve_cmplx>& pows,
00156 vcl_vector<vnl_rnpoly_solve_cmplx>& f,
00157 vcl_vector<vnl_rnpoly_solve_cmplx>& df)
00158 {
00159 assert(terms.size()==dim_);
00160 assert(x.size()==dim_);
00161
00162 pows.resize(max_deg_*dim_);
00163 for (unsigned int i=0;i<dim_;i++)
00164 {
00165 int index = max_deg_*i;
00166 pows[index]=x[i];
00167 for (unsigned int j=1;j<max_deg_;++j,++index)
00168 pows[index+1]= pows[index] * x[i];
00169 }
00170
00171
00172 for (unsigned int i=0; i<dim_; ++i)
00173 {
00174 f[i]=vnl_rnpoly_solve_cmplx(0,0);
00175 for (unsigned int j=0;j<dim_;j++)
00176 df[i*dim_+j]=vnl_rnpoly_solve_cmplx(0,0);
00177 }
00178
00179 for (unsigned int i=0; i<dim_; ++i)
00180 for (unsigned int j=0; j<terms[i]; ++j)
00181 {
00182 vnl_rnpoly_solve_cmplx tmp(1,0);
00183 for (unsigned int k=0; k<dim_; ++k)
00184 {
00185 int index=polyn[i*dim_*max_nterms_+j*dim_+k];
00186 if (index>=0)
00187 tmp *= pows[index];
00188 }
00189 f[i] += tmp * coeff[i*max_nterms_+j];
00190 }
00191
00192
00193 for (int i=dim_-1;i>=0;i--)
00194 for (int l=dim_-1;l>=0;l--)
00195 {
00196 vnl_rnpoly_solve_cmplx& df_il = df[i*dim_+l];
00197 for (int j=terms[i]-1;j>=0;j--)
00198 if (polyn[i*dim_*max_nterms_+j*dim_+l]>=0)
00199 {
00200 vnl_rnpoly_solve_cmplx tmp = vnl_rnpoly_solve_cmplx(1,0);
00201 for (int k=dim_-1;k>=0;k--)
00202 {
00203 int index=polyn[i*dim_*max_nterms_+j*dim_+k];
00204 if (index>=0)
00205 {
00206 if (k==l)
00207 {
00208 int deg = degree(index);
00209 if (deg > 1)
00210 tmp *= pows[index-1];
00211 tmp *= (double)deg;
00212 }
00213 else
00214 tmp *= pows[index];
00215 }
00216 }
00217 df_il += tmp * coeff[i*max_nterms_+j];
00218 }
00219 }
00220 }
00221
00222
00223
00224
00225
00226
00227 static void gfunr(vcl_vector<unsigned int> const& ideg,
00228 vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
00229 vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
00230 vcl_vector<vnl_rnpoly_solve_cmplx> const& pows,
00231 vcl_vector<vnl_rnpoly_solve_cmplx>& g,
00232 vcl_vector<vnl_rnpoly_solve_cmplx>& dg)
00233 {
00234 assert(ideg.size()==dim_);
00235 assert(g.size()==dim_);
00236 assert(dg.size()==dim_);
00237 vcl_vector<vnl_rnpoly_solve_cmplx> pxdgm1(dim_), pxdg(dim_);
00238
00239 for (unsigned int j=0; j<dim_; ++j)
00240 {
00241 vnl_rnpoly_solve_cmplx tmp;
00242 if (ideg[j] <= 1)
00243 tmp = vnl_rnpoly_solve_cmplx(1,0);
00244 else
00245 tmp = pows[j*max_deg_+ideg[j]-2];
00246
00247 pxdgm1[j] = pdg[j] * tmp;
00248 }
00249
00250 for (unsigned int j=0; j<dim_; ++j)
00251 {
00252 int index = j*max_deg_+ideg[j]-1;
00253 pxdg[j] = pdg[j] * pows[index];
00254 }
00255
00256 for (unsigned int j=0; j<dim_; ++j)
00257 {
00258 g[j] = pxdg[j] - qdg[j];
00259 dg[j] = pxdgm1[j] * ideg[j];
00260 }
00261 }
00262
00263
00264
00265
00266
00267 static void hfunr(vcl_vector<unsigned int> const& ideg,
00268 vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
00269 vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
00270 double t,
00271 vcl_vector<vnl_rnpoly_solve_cmplx> const& x,
00272 vcl_vector<vnl_rnpoly_solve_cmplx>& h,
00273 vcl_vector<vnl_rnpoly_solve_cmplx>& dhx,
00274 vcl_vector<vnl_rnpoly_solve_cmplx>& dht,
00275 vcl_vector<int> const& polyn,
00276 vcl_vector<double> const& coeff,
00277 vcl_vector<unsigned int> const& terms)
00278 {
00279 assert(ideg.size()==dim_);
00280 assert(terms.size()==dim_);
00281 assert(x.size()==dim_);
00282 assert(h.size()==dim_);
00283 assert(dht.size()==dim_);
00284 assert(dhx.size()==dim_*dim_);
00285 vcl_vector<vnl_rnpoly_solve_cmplx> df(dim_*dim_),dg(dim_),f(dim_),g(dim_);
00286 vcl_vector<vnl_rnpoly_solve_cmplx> pows;
00287
00288 ffunr(coeff,polyn,terms,x,pows,f,df);
00289 gfunr(ideg,pdg,qdg,pows,g,dg);
00290 assert(f.size()==dim_);
00291 assert(g.size()==dim_);
00292 assert(dg.size()==dim_);
00293 assert(df.size()==dim_*dim_);
00294
00295 double onemt=1.0 - t;
00296 for (unsigned int j=0; j<dim_; ++j)
00297 {
00298 for (unsigned int i=0; i<dim_; ++i)
00299 dhx[j*dim_+i] = df[j*dim_+i] * t;
00300
00301 dhx[j*dim_+j] += dg[j]*onemt;
00302 dht[j] = f[j] - g[j];
00303 h[j] = f[j] * t + g[j] * onemt;
00304 }
00305 }
00306
00307
00308
00309
00310 static int ludcmp(vcl_vector<vnl_rnpoly_solve_cmplx>& a, vcl_vector<int>& indx)
00311 {
00312 vcl_vector<double> vv(dim_);
00313
00314
00315 for (unsigned int i=0; i<dim_; ++i)
00316 {
00317 double big = 0.0;
00318 for (unsigned int j=0; j<dim_; ++j)
00319 {
00320 double temp = a[i*dim_+j].norm();
00321 if (temp > big) big = temp;
00322 }
00323 if (big == 0.0) return 1;
00324 vv[i]=1.0/vcl_sqrt(big);
00325 }
00326
00327
00328 for (unsigned int j=0; j<dim_; ++j)
00329 {
00330 for (unsigned int i=0; i<j; ++i)
00331 for (unsigned int k=0; k<i; ++k)
00332 a[i*dim_+j] -= a[i*dim_+k] * a[k*dim_+j];
00333
00334
00335 double big = 0.0;
00336 unsigned int imax = 0;
00337
00338 for (unsigned int i=j; i<dim_; ++i)
00339 {
00340 for (unsigned int k=0; k<j; ++k)
00341 a[i*dim_+j] -= a[i*dim_+k] * a[k*dim_+j];
00342
00343
00344 double rdum = vv[i]*a[i*dim_+j].norm();
00345 if (rdum >= big) { big = rdum; imax = i; }
00346 }
00347
00348
00349 if (j != imax)
00350 {
00351
00352 for (unsigned int k=0; k<dim_; ++k)
00353 {
00354 vnl_rnpoly_solve_cmplx dum = a[imax*dim_+k];
00355 a[imax*dim_+k] = a[j*dim_+k]; a[j*dim_+k] = dum;
00356 }
00357
00358
00359 vv[imax]=vv[j];
00360 }
00361 indx[j]=imax;
00362
00363 vnl_rnpoly_solve_cmplx& ajj = a[j*dim_+j];
00364 if (ajj.norm() == 0.0)
00365 ajj = epsilonZ;
00366
00367
00368 if (j+1 != dim_)
00369 {
00370 vnl_rnpoly_solve_cmplx dum = vnl_rnpoly_solve_cmplx(1,0) / ajj;
00371
00372
00373 for (unsigned int i=j+1; i<dim_; ++i)
00374 a[i*dim_+j] *= dum;
00375 }
00376 }
00377 return 0;
00378 }
00379
00380
00381
00382 static void lubksb(vcl_vector<vnl_rnpoly_solve_cmplx> const& a,
00383 vcl_vector<int> const& indx,
00384 vcl_vector<vnl_rnpoly_solve_cmplx> const& bb,
00385 vcl_vector<vnl_rnpoly_solve_cmplx>& b)
00386 {
00387 int ii=-1;
00388 for (unsigned int k=0; k<dim_; ++k)
00389 b[k] = bb[k];
00390
00391 for (unsigned int i=0; i<dim_; ++i)
00392 {
00393 int ip = indx[i];
00394 vnl_rnpoly_solve_cmplx sum = b[ip];
00395 b[ip] = b[i];
00396
00397 if (ii>=0)
00398 for (unsigned int j=ii;j<i;++j)
00399 sum -= a[i*dim_+j] * b[j];
00400 else
00401
00402
00403 if (sum.norm() > 0) ii = i;
00404
00405 b[i] = sum;
00406 }
00407
00408
00409 for (int i=dim_-1;i>=0;i--)
00410 {
00411 for (unsigned int j=i+1; j<dim_; ++j)
00412 b[i] -= a[i*dim_+j] * b[j];
00413
00414 b[i] /= a[i*dim_+i];
00415 }
00416 }
00417
00418
00419
00420
00421 static int linnr(vcl_vector<vnl_rnpoly_solve_cmplx>& dhx,
00422 vcl_vector<vnl_rnpoly_solve_cmplx> const& rhs,
00423 vcl_vector<vnl_rnpoly_solve_cmplx>& resid)
00424 {
00425 vcl_vector<int> irow(dim_);
00426 if (ludcmp(dhx,irow)==1) return 1;
00427 lubksb(dhx,irow,rhs,resid);
00428 return 0;
00429 }
00430
00431
00432
00433
00434 static double xnorm(vcl_vector<vnl_rnpoly_solve_cmplx> const& v)
00435 {
00436 assert(v.size()==dim_);
00437 double txnorm=0.0;
00438 for (unsigned int j=0; j<dim_; ++j)
00439 txnorm += vcl_fabs(v[j].R) + vcl_fabs(v[j].C);
00440 return txnorm;
00441 }
00442
00443
00444
00445 static void predict(vcl_vector<unsigned int> const& ideg,
00446 vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
00447 vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
00448 double step, double& t,
00449 vcl_vector<vnl_rnpoly_solve_cmplx>& x,
00450 vcl_vector<int> const& polyn,
00451 vcl_vector<double> const& coeff,
00452 vcl_vector<unsigned int> const& terms)
00453 {
00454 assert(ideg.size()==dim_);
00455 assert(terms.size()==dim_);
00456 assert(x.size()==dim_);
00457
00458 double maxdt =.2;
00459
00460
00461
00462 vcl_vector<vnl_rnpoly_solve_cmplx> dht(dim_),dhx(dim_*dim_),dz(dim_),h(dim_),rhs(dim_);
00463
00464 hfunr(ideg,pdg,qdg,t,x,h,dhx,dht,polyn,coeff,terms);
00465
00466 for (unsigned int j=0; j<dim_; ++j)
00467 rhs[j] = - dht[j];
00468
00469
00470 if (linnr(dhx,rhs,dz) == 1) return;
00471
00472
00473 double factor = step/(1+xnorm(dz));
00474 if (factor>maxdt) factor = maxdt;
00475
00476 bool tis1=true;
00477 if (t+factor>1) { tis1 = false; factor = 1.0 - t; }
00478
00479
00480 for (unsigned int j=0; j<dim_; ++j)
00481 x[j] += dz[j] * factor;
00482
00483 if (tis1) t += factor;
00484 else t = 1.0;
00485 }
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496 static int correct(vcl_vector<unsigned int> const& ideg, int loop, double eps,
00497 vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
00498 vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
00499 double t,
00500 vcl_vector<vnl_rnpoly_solve_cmplx>& x,
00501 vcl_vector<int> const& polyn,
00502 vcl_vector<double> const& coeff,
00503 vcl_vector<unsigned int> const& terms)
00504 {
00505 double maxroot= 1000;
00506 vcl_vector<vnl_rnpoly_solve_cmplx> dhx(dim_*dim_),dht(dim_),h(dim_),resid(dim_);
00507
00508 assert(ideg.size()==dim_);
00509 assert(terms.size()==dim_);
00510 assert(x.size()==dim_);
00511
00512 for (int i=0;i<loop;i++)
00513 {
00514 hfunr(ideg,pdg,qdg,t,x,h,dhx,dht,polyn,coeff,terms);
00515
00516
00517 if (linnr(dhx,h,resid)==1) return 1;
00518
00519 for (unsigned int j=0; j<dim_; ++j)
00520 x[j] -= resid[j];
00521
00522 double xresid = xnorm(resid);
00523 if (xresid < eps) return 0;
00524 if (xresid > maxroot) return 3;
00525 }
00526 return 2;
00527 }
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541 static int trace(vcl_vector<vnl_rnpoly_solve_cmplx>& x,
00542 vcl_vector<unsigned int> const& ideg,
00543 vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
00544 vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
00545 vcl_vector<int> const& polyn,
00546 vcl_vector<double> const& coeff,
00547 vcl_vector<unsigned int> const& terms)
00548 {
00549 assert(ideg.size()==dim_);
00550 assert(terms.size()==dim_);
00551 assert(x.size()==dim_);
00552
00553 int maxns=500;
00554 int maxit=5;
00555
00556
00557
00558
00559
00560 double eps=0;
00561 double epsilonS=1.0e-3 * epsilonB;
00562 double stepmin=1.0e-5 * stepinit;
00563 double step=stepinit;
00564 double t=0.0;
00565 double oldt=0.0;
00566 vcl_vector<vnl_rnpoly_solve_cmplx> oldx = x;
00567 int nadv=0;
00568
00569 for (int numstep=0;numstep<maxns;numstep++)
00570 {
00571
00572 predict(ideg,pdg,qdg,step,t,x,polyn,coeff,terms);
00573
00574
00575
00576 if (t > .95)
00577 {
00578 if (eps != epsilonS) step = step/4.0;
00579 eps = epsilonS;
00580 }
00581 else
00582 eps = epsilonB;
00583 #ifdef DEBUG
00584 vcl_cout << "t=" << t << vcl_endl;
00585 #endif
00586
00587 if (t>=.99999)
00588 {
00589 #ifdef DEBUG
00590 vcl_cout << "path converged\n" << vcl_flush;
00591 #endif
00592 double factor = (1.0-oldt)/(t-oldt);
00593 for (unsigned int j=0; j<dim_; ++j)
00594 x[j] = oldx[j] + (x[j]-oldx[j]) * factor;
00595 t = 1.0;
00596 int cflag=correct(ideg,10*maxit,final_eps,pdg,qdg,t,x, polyn, coeff,terms);
00597 if ((cflag==0) ||(cflag==2))
00598 return 1;
00599 else if (cflag==3)
00600 return 3;
00601 else return 4;
00602 }
00603
00604
00605 int cflag=correct(ideg,maxit,eps,pdg,qdg,t,x,polyn, coeff,terms);
00606 if (cflag==0)
00607 {
00608
00609 if ((++nadv)==maxit) { step *= 2; nadv=0; }
00610
00611 oldt = t;
00612 oldx = x;
00613 }
00614 else
00615 {
00616 nadv=0;
00617 step /= 2.0;
00618
00619 if (cflag==3) return 3;
00620 if (step<stepmin) return 2;
00621
00622
00623 t = oldt;
00624 x = oldx;
00625 }
00626 }
00627
00628 return 0;
00629 }
00630
00631
00632
00633
00634
00635 static void strptr(vcl_vector<unsigned int>& icount,
00636 vcl_vector<unsigned int> const& ideg,
00637 vcl_vector<vnl_rnpoly_solve_cmplx> const& r,
00638 vcl_vector<vnl_rnpoly_solve_cmplx>& x)
00639 {
00640 assert(ideg.size()==dim_);
00641 assert(r.size()==dim_);
00642 x.resize(dim_);
00643
00644 for (unsigned int i=0; i<dim_; ++i)
00645 if (icount[i] >= ideg[i]) icount[i] = 1;
00646 else { icount[i]++; break; }
00647
00648 for (unsigned int j=0; j<dim_; ++j)
00649 {
00650 double angle = twopi / ideg[j] * icount[j];
00651 x[j] = r[j] * vnl_rnpoly_solve_cmplx (vcl_cos(angle), vcl_sin(angle));
00652 }
00653 }
00654
00655
00656 static vcl_vector<vcl_vector<vnl_rnpoly_solve_cmplx> >
00657 Perform_Distributed_Task(vcl_vector<unsigned int> const& ideg,
00658 vcl_vector<unsigned int> const& terms,
00659 vcl_vector<int> const& polyn,
00660 vcl_vector<double> const& coeff)
00661 {
00662 assert(ideg.size()==dim_);
00663
00664 vcl_vector<vcl_vector<vnl_rnpoly_solve_cmplx> > sols;
00665 vcl_vector<vnl_rnpoly_solve_cmplx> pdg, qdg, p, q, r, x;
00666 vcl_vector<unsigned int> icount(dim_,1); icount[0]=0;
00667 bool solflag;
00668 #ifdef DEBUG
00669 char const* FILENAM = "/tmp/cont.results";
00670 vcl_ofstream F(FILENAM);
00671 if (!F)
00672 {
00673 vcl_cerr<<"could not open "<<FILENAM<<" for writing\nplease erase old file first\n";
00674 F = vcl_cerr;
00675 }
00676 else
00677 vcl_cerr << "Writing to " << FILENAM << '\n';
00678 #endif
00679
00680 inptbr(p,q);
00681 initr(ideg,p,q,r,pdg,qdg);
00682
00683
00684 int totdegree = 1;
00685 for (unsigned int j=0;j<dim_;j++) totdegree *= ideg[j];
00686
00687
00688
00689
00690 while ((totdegree--) > 0)
00691 {
00692
00693 strptr(icount,ideg,r,x);
00694
00695
00696 solflag = 1 == trace(x,ideg,pdg,qdg,polyn,coeff,terms);
00697
00698 if (solflag)
00699 {
00700 #ifdef DEBUG
00701 for (unsigned int i=0; i<dim_; ++i)
00702 F << '<' << x[dim_-i-1].R << ' ' << x[dim_-i-1].C << '>';
00703 F << vcl_endl;
00704 #endif
00705 sols.push_back(x);
00706 }
00707 #ifdef DEBUG
00708
00709 if (solflag) vcl_cout << '.';
00710 else vcl_cout << '*';
00711 vcl_cout.flush();
00712 #endif
00713 }
00714
00715 #ifdef DEBUG
00716 vcl_cout<< vcl_endl;
00717 #endif
00718
00719 return sols;
00720 }
00721
00722
00723
00724
00725 void vnl_rnpoly_solve::Read_Input(vcl_vector<unsigned int>& ideg,
00726 vcl_vector<unsigned int>& terms,
00727 vcl_vector<int>& polyn,
00728 vcl_vector<double>& coeff)
00729 {
00730
00731 dim_ = ps_.size();
00732
00733 ideg.resize(dim_); terms.resize(dim_);
00734
00735 max_deg_=0;
00736 max_nterms_=0;
00737 for (unsigned int i=0;i<dim_;i++)
00738 {
00739 ideg[i] = ps_[i]->ideg_;
00740 terms[i] = ps_[i]->nterms_;
00741 if (ideg[i] > max_deg_)
00742 max_deg_ = ideg[i];
00743 if (terms[i] > max_nterms_)
00744 max_nterms_ = terms[i];
00745 }
00746 coeff.resize(dim_*max_nterms_);
00747 polyn.resize(dim_*max_nterms_*dim_);
00748 for (unsigned int i=0;i<dim_;i++)
00749 {
00750 for (unsigned int k=0;k<terms[i];k++)
00751 {
00752 coeff[i*max_nterms_+k] = ps_[i]->coeffs_(k);
00753 for (unsigned int j=0;j<dim_;j++)
00754 {
00755 int deg = ps_[i]->polyn_(k,j);
00756 polyn[i*dim_*max_nterms_+k*dim_+j] = deg ? int(j*max_deg_)+deg-1 : -1;
00757 }
00758 }
00759 }
00760 }
00761
00762
00763 vnl_rnpoly_solve::~vnl_rnpoly_solve()
00764 {
00765 while (r_.size() > 0) { delete r_.back(); r_.pop_back(); }
00766 while (i_.size() > 0) { delete i_.back(); i_.pop_back(); }
00767 }
00768
00769 bool vnl_rnpoly_solve::compute()
00770 {
00771 vcl_vector<unsigned int> ideg, terms;
00772 vcl_vector<int> polyn;
00773 vcl_vector<double> coeff;
00774
00775 Read_Input(ideg,terms,polyn,coeff);
00776 assert(ideg.size()==dim_);
00777 assert(terms.size()==dim_);
00778 assert(polyn.size()==dim_*max_nterms_*dim_);
00779 assert(coeff.size()==dim_*max_nterms_);
00780
00781 int totdegree = 1;
00782 for (unsigned int j=0; j<dim_; ++j) totdegree *= ideg[j];
00783
00784 vcl_vector<vcl_vector<vnl_rnpoly_solve_cmplx> > ans = Perform_Distributed_Task(ideg,terms,polyn,coeff);
00785
00786
00787 vnl_vector<double> * rp, *ip;
00788 #ifdef DEBUG
00789 vcl_cout << "Total degree: " << totdegree << vcl_endl
00790 << "# solutions : " << ans.size() << vcl_endl;
00791 #endif
00792 for (unsigned int i=0; i<ans.size(); ++i)
00793 {
00794 assert(ans[i].size()==dim_);
00795 rp=new vnl_vector<double>(dim_); r_.push_back(rp);
00796 ip=new vnl_vector<double>(dim_); i_.push_back(ip);
00797 for (unsigned int j=0; j<dim_; ++j)
00798 {
00799 #ifdef DEBUG
00800 vcl_cout << ans[i][j].R << " + j " << ans[i][j].C << vcl_endl;
00801 #endif
00802 (*rp)[j]=ans[i][j].R; (*ip)[j]=ans[i][j].C;
00803 }
00804 #ifdef DEBUG
00805 vcl_cout<< vcl_endl;
00806 #endif
00807 }
00808 return true;
00809 }