00001
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifdef _WIN32
00037 #pragma warning(disable:4819)
00038 #endif //_WIN32
00039
00040 #include <cstring>
00041 #include <ctime>
00042 #include <iostream>
00043 #include <cstdio>
00044 #include <cstdlib>
00045 #include <boost/format.hpp>
00046 #include "emdata.h"
00047 #include "util.h"
00048 #include "fundamentals.h"
00049 #include "lapackblas.h"
00050 #include "lbfgsb.h"
00051 using namespace EMAN;
00052 #include "steepest.h"
00053 #include "emassert.h"
00054 #include "randnum.h"
00055
00056 #include <gsl/gsl_sf_bessel.h>
00057 #include <gsl/gsl_sf_bessel.h>
00058 #include <cmath>
00059 using namespace std;
00060 using std::complex;
00061
00062 vector<float> Util::infomask(EMData* Vol, EMData* mask, bool flip = false)
00063
00064
00065 {
00066 ENTERFUNC;
00067 vector<float> stats;
00068 float *Volptr, *maskptr,MAX,MIN;
00069 long double Sum1,Sum2;
00070 long count;
00071
00072 MAX = -FLT_MAX;
00073 MIN = FLT_MAX;
00074 count = 0L;
00075 Sum1 = 0.0L;
00076 Sum2 = 0.0L;
00077
00078 if (mask == NULL) {
00079
00080 stats.push_back(Vol->get_attr("mean"));
00081 stats.push_back(Vol->get_attr("sigma"));
00082 stats.push_back(Vol->get_attr("minimum"));
00083 stats.push_back(Vol->get_attr("maximum"));
00084 return stats;
00085 }
00086
00087
00088
00089 size_t nx = Vol->get_xsize();
00090 size_t ny = Vol->get_ysize();
00091 size_t nz = Vol->get_zsize();
00092
00093 size_t mask_nx = mask->get_xsize();
00094 size_t mask_ny = mask->get_ysize();
00095 size_t mask_nz = mask->get_zsize();
00096
00097 if (nx != mask_nx || ny != mask_ny || nz != mask_nz )
00098 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111 Volptr = Vol->get_data();
00112 maskptr = mask->get_data();
00113
00114 for (size_t i = 0; i < nx*ny*nz; i++) {
00115 if (maskptr[i]>0.5f == flip) {
00116 Sum1 += Volptr[i];
00117 Sum2 += Volptr[i]*Volptr[i];
00118 MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00119 MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00120 count++;
00121 }
00122 }
00123
00124 if (count == 0) {
00125 LOGERR("Invalid mask");
00126 throw ImageFormatException( "Invalid mask");
00127 }
00128
00129 float avg = static_cast<float>(Sum1/count);
00130 float sig2 = static_cast<float>(Sum2 - count*avg*avg)/(count-1);
00131 float sig = sqrt(sig2);
00132
00133 stats.push_back(avg);
00134 stats.push_back(sig);
00135 stats.push_back(MIN);
00136 stats.push_back(MAX);
00137
00138 return stats;
00139 }
00140
00141
00142
00143
00144 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00145 {
00146 ENTERFUNC;
00147
00148 if (!EMUtil::is_same_size(V1, V2)) {
00149 LOGERR("images not same size");
00150 throw ImageFormatException( "images not same size");
00151 }
00152
00153 size_t nx = V1->get_xsize();
00154 size_t ny = V1->get_ysize();
00155 size_t nz = V1->get_zsize();
00156 size_t size = nx*ny*nz;
00157
00158 EMData *BD = new EMData();
00159 BD->set_size(nx, ny, nz);
00160
00161 float *params = new float[2];
00162
00163 float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00164 long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00165 int nvox = 0L;
00166
00167 V1ptr = V1->get_data();
00168 V2ptr = V2->get_data();
00169 BDptr = BD->get_data();
00170
00171
00172 if(!mask){
00173 EMData * Mask = new EMData();
00174 Mask->set_size(nx,ny,nz);
00175 Mask->to_one();
00176 MASKptr = Mask->get_data();
00177 } else {
00178 if (!EMUtil::is_same_size(V1, mask)) {
00179 LOGERR("mask not same size");
00180 throw ImageFormatException( "mask not same size");
00181 }
00182
00183 MASKptr = mask->get_data();
00184 }
00185
00186
00187
00188
00189
00190 for (size_t i = 0L;i < size; i++) {
00191 if (MASKptr[i]>0.5f) {
00192 S1 += V1ptr[i]*V2ptr[i];
00193 S2 += V1ptr[i]*V1ptr[i];
00194 S3 += V2ptr[i];
00195 S4 += V1ptr[i];
00196 nvox ++;
00197 }
00198 }
00199
00200 if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00201 A =1.0f ;
00202 } else {
00203 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00204 }
00205 B = static_cast<float> (A*S4 - S3)/nvox;
00206
00207
00208
00209 for (size_t i = 0L;i < size; i++) {
00210 if (MASKptr[i]>0.5f) {
00211 BDptr[i] = A*V1ptr[i] - B - V2ptr[i];
00212 } else {
00213 BDptr[i] = 0.f;
00214 }
00215 }
00216
00217 BD->update();
00218
00219 params[0] = A;
00220 params[1] = B;
00221
00222 Dict BDnParams;
00223 BDnParams["imdiff"] = BD;
00224 BDnParams["A"] = params[0];
00225 BDnParams["B"] = params[1];
00226
00227 EXITFUNC;
00228 return BDnParams;
00229 }
00230
00231
00232
00233
00234
00235 EMData *Util::TwoDTestFunc(int Size, float p, float q, float a, float b, int flag, float alphaDeg)
00236 {
00237 ENTERFUNC;
00238 int Mid= (Size+1)/2;
00239
00240 if (flag==0) {
00241 EMData* ImBW = new EMData();
00242 ImBW->set_size(Size,Size,1);
00243 ImBW->to_zero();
00244
00245 float tempIm;
00246 float x,y;
00247
00248 for (int ix=(1-Mid); ix<Mid; ix++){
00249 for (int iy=(1-Mid); iy<Mid; iy++){
00250 x = (float)ix;
00251 y = (float)iy;
00252 tempIm= static_cast<float>( (1/(2*M_PI)) * cos(p*x)* cos(q*y) * exp(-.5*x*x/(a*a))* exp(-.5*y*y/(b*b)) );
00253 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00254 }
00255 }
00256 ImBW->update();
00257 ImBW->set_complex(false);
00258 ImBW->set_ri(true);
00259
00260 return ImBW;
00261 }
00262 else if (flag==1) {
00263 EMData* ImBWFFT = new EMData();
00264 ImBWFFT ->set_size(2*Size,Size,1);
00265 ImBWFFT ->to_zero();
00266
00267 float r,s;
00268
00269 for (int ir=(1-Mid); ir<Mid; ir++){
00270 for (int is=(1-Mid); is<Mid; is++){
00271 r = (float)ir;
00272 s = (float)is;
00273 (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00274 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00275 }
00276 }
00277 ImBWFFT->update();
00278 ImBWFFT->set_complex(true);
00279 ImBWFFT->set_ri(true);
00280 ImBWFFT->set_shuffled(true);
00281 ImBWFFT->set_fftodd(true);
00282
00283 return ImBWFFT;
00284 }
00285 else if (flag==2 || flag==3) {
00286 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00287 float C=cos(alpha);
00288 float S=sin(alpha);
00289 float D= sqrt(S*S*b*b + C*C*a*a);
00290
00291
00292 float P = p * C *a*a/D ;
00293 float Q = q * S *b*b/D ;
00294
00295 if (flag==2) {
00296 EMData* pofalpha = new EMData();
00297 pofalpha ->set_size(Size,1,1);
00298 pofalpha ->to_zero();
00299
00300 float Norm0 = D*(float)sqrt(2*pi);
00301 float Norm1 = exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00302 float Norm2 = exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00303 float sD;
00304
00305 for (int is=(1-Mid); is<Mid; is++){
00306 sD = is/D ;
00307 (*pofalpha)(is+Mid-1) = Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00308 + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00309 }
00310 pofalpha-> update();
00311 pofalpha-> set_complex(false);
00312 pofalpha-> set_ri(true);
00313
00314 return pofalpha;
00315 }
00316 if (flag==3) {
00317 float vD;
00318
00319 EMData* pofalphak = new EMData();
00320 pofalphak ->set_size(2*Size,1,1);
00321 pofalphak ->to_zero();
00322
00323 for (int iv=(1-Mid); iv<Mid; iv++){
00324 vD = iv*D ;
00325 (*pofalphak)(2*(iv+Mid-1)) = exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00326 }
00327 pofalphak-> update();
00328 pofalphak-> set_complex(false);
00329 pofalphak-> set_ri(true);
00330
00331 return pofalphak;
00332 }
00333 }
00334 else if (flag==4) {
00335 cout <<" FH under construction";
00336 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00337 EMData* TryFH= OutFT -> real2FH(4.0);
00338 return TryFH;
00339 } else {
00340 cout <<" flag must be 0,1,2,3, or 4";
00341 }
00342
00343 EXITFUNC;
00344 return 0;
00345 }
00346
00347
00348 void Util::spline_mat(float *x, float *y, int n, float *xq, float *yq, int m)
00349 {
00350
00351 float x0= x[0];
00352 float x1= x[1];
00353 float x2= x[2];
00354 float y0= y[0];
00355 float y1= y[1];
00356 float y2= y[2];
00357 float yp1 = (y1-y0)/(x1-x0) + (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1) ;
00358 float xn = x[n];
00359 float xnm1= x[n-1];
00360 float xnm2= x[n-2];
00361 float yn = y[n];
00362 float ynm1= y[n-1];
00363 float ynm2= y[n-2];
00364 float ypn= (yn-ynm1)/(xn-xnm1) + (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00365 float *y2d = new float[n];
00366 Util::spline(x,y,n,yp1,ypn,y2d);
00367 Util::splint(x,y,y2d,n,xq,yq,m);
00368 delete [] y2d;
00369 return;
00370 }
00371
00372
00373 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2)
00374 {
00375 int i,k;
00376 float p, qn, sig, un, *u;
00377 u = new float[n-1];
00378
00379 if (yp1 > .99e30){
00380 y2[0]=u[0]=0.0;
00381 } else {
00382 y2[0]=-.5f;
00383 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00384 }
00385
00386 for (i=1; i < n-1; i++) {
00387 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00388 p = sig*y2[i-1] + 2.0f;
00389 y2[i] = (sig-1.0f)/p;
00390 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) - (y[i] - y[i-1] )/(x[i] -x[i-1]);
00391 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00392 }
00393
00394 if (ypn>.99e30){
00395 qn=0; un=0;
00396 } else {
00397 qn= .5f;
00398 un= (3.0f/(x[n-1] -x[n-2])) * (ypn - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00399 }
00400 y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00401 for (k=n-2; k>=0; k--){
00402 y2[k]=y2[k]*y2[k+1]+u[k];
00403 }
00404 delete [] u;
00405 }
00406
00407
00408 void Util::splint( float *xa, float *ya, float *y2a, int n, float *xq, float *yq, int m)
00409 {
00410 int klo, khi, k;
00411 float h, b, a;
00412
00413
00414 for (int j=0; j<m;j++){
00415 klo=0;
00416 khi=n-1;
00417 while (khi-klo >1) {
00418 k=(khi+klo) >>1;
00419 if (xa[k]>xq[j]){ khi=k;}
00420 else { klo=k;}
00421 }
00422 h=xa[khi]- xa[klo];
00423 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00424 a =(xa[khi]-xq[j])/h;
00425 b=(xq[j]-xa[klo])/h;
00426 yq[j]=a*ya[klo] + b*ya[khi]
00427 + ((a*a*a-a)*y2a[klo]
00428 +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00429 }
00430
00431 }
00432
00433
00434 void Util::Radialize(int *PermMatTr, float *kValsSorted,
00435 float *weightofkValsSorted, int Size, int *SizeReturned)
00436 {
00437 int iMax = (int) floor( (Size-1.0)/2 +.01);
00438 int CountMax = (iMax+2)*(iMax+1)/2;
00439 int Count=-1;
00440 float *kVals = new float[CountMax];
00441 float *weightMat = new float[CountMax];
00442 int *PermMat = new int[CountMax];
00443 SizeReturned[0] = CountMax;
00444
00445
00446 for (int jkx=0; jkx< iMax+1; jkx++) {
00447 for (int jky=0; jky< jkx+1; jky++) {
00448 Count++;
00449 kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00450 weightMat[Count]= 1.0;
00451 if (jkx!=0) { weightMat[Count] *=2;}
00452 if (jky!=0) { weightMat[Count] *=2;}
00453 if (jkx!=jky){ weightMat[Count] *=2;}
00454 PermMat[Count]=Count+1;
00455 }
00456 }
00457
00458 int lkVals = Count+1;
00459
00460
00461 sort_mat(&kVals[0],&kVals[Count],
00462 &PermMat[0], &PermMat[Count]);
00463
00464 fflush(stdout);
00465
00466 int newInd;
00467
00468 for (int iP=0; iP < lkVals ; iP++ ) {
00469 newInd = PermMat[iP];
00470 PermMatTr[newInd-1] = iP+1;
00471 }
00472
00473
00474
00475 int CountA=-1;
00476 int CountB=-1;
00477
00478 while (CountB< (CountMax-1)) {
00479 CountA++;
00480 CountB++;
00481
00482 kValsSorted[CountA] = kVals[CountB] ;
00483 if (CountB<(CountMax-1) ) {
00484 while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001 ) {
00485 SizeReturned[0]--;
00486 for (int iP=0; iP < lkVals; iP++){
00487
00488 if (PermMatTr[iP]>CountA+1) {
00489 PermMatTr[iP]--;
00490 }
00491 }
00492 CountB++;
00493 }
00494 }
00495 }
00496
00497
00498 for (int CountD=0; CountD < CountMax; CountD++) {
00499 newInd = PermMatTr[CountD];
00500 weightofkValsSorted[newInd-1] += weightMat[CountD];
00501 }
00502
00503 }
00504
00505
00506 vector<float>
00507 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00508 {
00509 vector<float> angles;
00510 float psi = 0.0;
00511 if ((0.0 == t1)&&(0.0 == t2)||(t1 >= t2)) {
00512 t1 = 0.0f;
00513 t2 = 90.0f;
00514 }
00515 if ((0.0 == p1)&&(0.0 == p2)||(p1 >= p2)) {
00516 p1 = 0.0f;
00517 p2 = 359.9f;
00518 }
00519 bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00520 for (float theta = t1; theta <= t2; theta += delta) {
00521 float detphi;
00522 int lt;
00523 if ((0.0 == theta)||(180.0 == theta)) {
00524 detphi = 360.0f;
00525 lt = 1;
00526 } else {
00527 detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00528 lt = int((p2 - p1)/detphi)-1;
00529 if (lt < 1) lt = 1;
00530 detphi = (p2 - p1)/lt;
00531 }
00532 for (int i = 0; i < lt; i++) {
00533 float phi = p1 + i*detphi;
00534 if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00535 angles.push_back(phi);
00536 angles.push_back(theta);
00537 angles.push_back(psi);
00538 }
00539 }
00540 return angles;
00541 }
00542
00543
00544 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00647 {
00648
00649
00650 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00651 float quadri;
00652 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00653
00654 x = xx;
00655 y = yy;
00656
00657
00658 while ( x < 1.0 ) x += nxdata;
00659 while ( x >= (float)(nxdata+1) ) x -= nxdata;
00660 while ( y < 1.0 ) y += nydata;
00661 while ( y >= (float)(nydata+1) ) y -= nydata;
00662
00663 i = (int) x;
00664 j = (int) y;
00665
00666 dx0 = x - i;
00667 dy0 = y - j;
00668
00669 ip1 = i + 1;
00670 im1 = i - 1;
00671 jp1 = j + 1;
00672 jm1 = j - 1;
00673
00674 if (ip1 > nxdata) ip1 -= nxdata;
00675 if (im1 < 1) im1 += nxdata;
00676 if (jp1 > nydata) jp1 -= nydata;
00677 if (jm1 < 1) jm1 += nydata;
00678
00679 f0 = fdata(i,j);
00680 c1 = fdata(ip1,j) - f0;
00681 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00682 c3 = fdata(i,jp1) - f0;
00683 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00684
00685 dxb = dx0 - 1;
00686 dyb = dy0 - 1;
00687
00688
00689 if (dx0 >= 0) hxc = 1; else hxc = -1;
00690 if (dy0 >= 0) hyc = 1; else hyc = -1;
00691
00692 ic = i + hxc;
00693 jc = j + hyc;
00694
00695 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00696 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00697
00698 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00699 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00700
00701
00702 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00703
00704 return quadri;
00705 }
00706
00707 #undef fdata
00708
00709 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00710 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00711 {
00712
00713
00714 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00715 float quadri;
00716 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00717
00718 x = xx;
00719 y = yy;
00720
00721
00722 if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00723 x = (float)xnew;
00724 y = (float)ynew;
00725 }
00726
00727
00728 i = (int) x;
00729 j = (int) y;
00730
00731 dx0 = x - i;
00732 dy0 = y - j;
00733
00734 ip1 = i + 1;
00735 im1 = i - 1;
00736 jp1 = j + 1;
00737 jm1 = j - 1;
00738
00739 if (ip1 > nxdata) ip1 -= nxdata;
00740 if (im1 < 1) im1 += nxdata;
00741 if (jp1 > nydata) jp1 -= nydata;
00742 if (jm1 < 1) jm1 += nydata;
00743
00744 f0 = fdata(i,j);
00745 c1 = fdata(ip1,j) - f0;
00746 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00747 c3 = fdata(i,jp1) - f0;
00748 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00749
00750 dxb = dx0 - 1;
00751 dyb = dy0 - 1;
00752
00753
00754 if (dx0 >= 0) hxc = 1; else hxc = -1;
00755 if (dy0 >= 0) hyc = 1; else hyc = -1;
00756
00757 ic = i + hxc;
00758 jc = j + hyc;
00759
00760 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00761 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00762
00763 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00764 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00765
00766
00767 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00768
00769 return quadri;
00770 }
00771
00772 #undef fdata
00773
00774
00775 float Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00776 int K = kb.get_window_size();
00777 int kbmin = -K/2;
00778 int kbmax = -kbmin;
00779 int kbc = kbmax+1;
00780
00781 float pixel =0.0f;
00782 float w=0.0f;
00783
00784 delx = restrict1(delx, nx);
00785 int inxold = int(round(delx));
00786 if ( ny < 2 ) {
00787 float tablex1 = kb.i0win_tab(delx-inxold+3);
00788 float tablex2 = kb.i0win_tab(delx-inxold+2);
00789 float tablex3 = kb.i0win_tab(delx-inxold+1);
00790 float tablex4 = kb.i0win_tab(delx-inxold);
00791 float tablex5 = kb.i0win_tab(delx-inxold-1);
00792 float tablex6 = kb.i0win_tab(delx-inxold-2);
00793 float tablex7 = kb.i0win_tab(delx-inxold-3);
00794
00795 int x1, x2, x3, x4, x5, x6, x7;
00796
00797 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
00798 x1 = (inxold-3+nx)%nx;
00799 x2 = (inxold-2+nx)%nx;
00800 x3 = (inxold-1+nx)%nx;
00801 x4 = (inxold +nx)%nx;
00802 x5 = (inxold+1+nx)%nx;
00803 x6 = (inxold+2+nx)%nx;
00804 x7 = (inxold+3+nx)%nx;
00805 } else {
00806 x1 = inxold-3;
00807 x2 = inxold-2;
00808 x3 = inxold-1;
00809 x4 = inxold;
00810 x5 = inxold+1;
00811 x6 = inxold+2;
00812 x7 = inxold+3;
00813 }
00814
00815 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00816 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00817 data[x7]*tablex7 ;
00818
00819 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00820 } else if ( nz < 2 ) {
00821 dely = restrict1(dely, ny);
00822 int inyold = int(round(dely));
00823 float tablex1 = kb.i0win_tab(delx-inxold+3);
00824 float tablex2 = kb.i0win_tab(delx-inxold+2);
00825 float tablex3 = kb.i0win_tab(delx-inxold+1);
00826 float tablex4 = kb.i0win_tab(delx-inxold);
00827 float tablex5 = kb.i0win_tab(delx-inxold-1);
00828 float tablex6 = kb.i0win_tab(delx-inxold-2);
00829 float tablex7 = kb.i0win_tab(delx-inxold-3);
00830
00831 float tabley1 = kb.i0win_tab(dely-inyold+3);
00832 float tabley2 = kb.i0win_tab(dely-inyold+2);
00833 float tabley3 = kb.i0win_tab(dely-inyold+1);
00834 float tabley4 = kb.i0win_tab(dely-inyold);
00835 float tabley5 = kb.i0win_tab(dely-inyold-1);
00836 float tabley6 = kb.i0win_tab(dely-inyold-2);
00837 float tabley7 = kb.i0win_tab(dely-inyold-3);
00838
00839 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00840
00841 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
00842 x1 = (inxold-3+nx)%nx;
00843 x2 = (inxold-2+nx)%nx;
00844 x3 = (inxold-1+nx)%nx;
00845 x4 = (inxold +nx)%nx;
00846 x5 = (inxold+1+nx)%nx;
00847 x6 = (inxold+2+nx)%nx;
00848 x7 = (inxold+3+nx)%nx;
00849
00850 y1 = ((inyold-3+ny)%ny)*nx;
00851 y2 = ((inyold-2+ny)%ny)*nx;
00852 y3 = ((inyold-1+ny)%ny)*nx;
00853 y4 = ((inyold +ny)%ny)*nx;
00854 y5 = ((inyold+1+ny)%ny)*nx;
00855 y6 = ((inyold+2+ny)%ny)*nx;
00856 y7 = ((inyold+3+ny)%ny)*nx;
00857 } else {
00858 x1 = inxold-3;
00859 x2 = inxold-2;
00860 x3 = inxold-1;
00861 x4 = inxold;
00862 x5 = inxold+1;
00863 x6 = inxold+2;
00864 x7 = inxold+3;
00865
00866 y1 = (inyold-3)*nx;
00867 y2 = (inyold-2)*nx;
00868 y3 = (inyold-1)*nx;
00869 y4 = inyold*nx;
00870 y5 = (inyold+1)*nx;
00871 y6 = (inyold+2)*nx;
00872 y7 = (inyold+3)*nx;
00873 }
00874
00875 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00876 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00877 data[x7+y1]*tablex7 ) * tabley1 +
00878 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00879 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00880 data[x7+y2]*tablex7 ) * tabley2 +
00881 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00882 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00883 data[x7+y3]*tablex7 ) * tabley3 +
00884 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00885 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00886 data[x7+y4]*tablex7 ) * tabley4 +
00887 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00888 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00889 data[x7+y5]*tablex7 ) * tabley5 +
00890 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00891 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00892 data[x7+y6]*tablex7 ) * tabley6 +
00893 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00894 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00895 data[x7+y7]*tablex7 ) * tabley7;
00896
00897 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00898 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00899 } else {
00900 dely = restrict1(dely, ny);
00901 int inyold = int(Util::round(dely));
00902 delz = restrict1(delz, nz);
00903 int inzold = int(Util::round(delz));
00904
00905 float tablex1 = kb.i0win_tab(delx-inxold+3);
00906 float tablex2 = kb.i0win_tab(delx-inxold+2);
00907 float tablex3 = kb.i0win_tab(delx-inxold+1);
00908 float tablex4 = kb.i0win_tab(delx-inxold);
00909 float tablex5 = kb.i0win_tab(delx-inxold-1);
00910 float tablex6 = kb.i0win_tab(delx-inxold-2);
00911 float tablex7 = kb.i0win_tab(delx-inxold-3);
00912
00913 float tabley1 = kb.i0win_tab(dely-inyold+3);
00914 float tabley2 = kb.i0win_tab(dely-inyold+2);
00915 float tabley3 = kb.i0win_tab(dely-inyold+1);
00916 float tabley4 = kb.i0win_tab(dely-inyold);
00917 float tabley5 = kb.i0win_tab(dely-inyold-1);
00918 float tabley6 = kb.i0win_tab(dely-inyold-2);
00919 float tabley7 = kb.i0win_tab(dely-inyold-3);
00920
00921 float tablez1 = kb.i0win_tab(delz-inzold+3);
00922 float tablez2 = kb.i0win_tab(delz-inzold+2);
00923 float tablez3 = kb.i0win_tab(delz-inzold+1);
00924 float tablez4 = kb.i0win_tab(delz-inzold);
00925 float tablez5 = kb.i0win_tab(delz-inzold-1);
00926 float tablez6 = kb.i0win_tab(delz-inzold-2);
00927 float tablez7 = kb.i0win_tab(delz-inzold-3);
00928
00929 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00930
00931 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
00932 x1 = (inxold-3+nx)%nx;
00933 x2 = (inxold-2+nx)%nx;
00934 x3 = (inxold-1+nx)%nx;
00935 x4 = (inxold +nx)%nx;
00936 x5 = (inxold+1+nx)%nx;
00937 x6 = (inxold+2+nx)%nx;
00938 x7 = (inxold+3+nx)%nx;
00939
00940 y1 = ((inyold-3+ny)%ny)*nx;
00941 y2 = ((inyold-2+ny)%ny)*nx;
00942 y3 = ((inyold-1+ny)%ny)*nx;
00943 y4 = ((inyold +ny)%ny)*nx;
00944 y5 = ((inyold+1+ny)%ny)*nx;
00945 y6 = ((inyold+2+ny)%ny)*nx;
00946 y7 = ((inyold+3+ny)%ny)*nx;
00947
00948 z1 = ((inzold-3+nz)%nz)*nx*ny;
00949 z2 = ((inzold-2+nz)%nz)*nx*ny;
00950 z3 = ((inzold-1+nz)%nz)*nx*ny;
00951 z4 = ((inzold +nz)%nz)*nx*ny;
00952 z5 = ((inzold+1+nz)%nz)*nx*ny;
00953 z6 = ((inzold+2+nz)%nz)*nx*ny;
00954 z7 = ((inzold+3+nz)%nz)*nx*ny;
00955 } else {
00956 x1 = inxold-3;
00957 x2 = inxold-2;
00958 x3 = inxold-1;
00959 x4 = inxold;
00960 x5 = inxold+1;
00961 x6 = inxold+2;
00962 x7 = inxold+3;
00963
00964 y1 = (inyold-3)*nx;
00965 y2 = (inyold-2)*nx;
00966 y3 = (inyold-1)*nx;
00967 y4 = inyold*nx;
00968 y5 = (inyold+1)*nx;
00969 y6 = (inyold+2)*nx;
00970 y7 = (inyold+3)*nx;
00971
00972 z1 = (inzold-3)*nx*ny;
00973 z2 = (inzold-2)*nx*ny;
00974 z3 = (inzold-1)*nx*ny;
00975 z4 = inzold*nx*ny;
00976 z5 = (inzold+1)*nx*ny;
00977 z6 = (inzold+2)*nx*ny;
00978 z7 = (inzold+3)*nx*ny;
00979 }
00980
00981 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00982 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00983 data[x7+y1+z1]*tablex7 ) * tabley1 +
00984 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00985 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00986 data[x7+y2+z1]*tablex7 ) * tabley2 +
00987 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00988 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00989 data[x7+y3+z1]*tablex7 ) * tabley3 +
00990 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00991 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00992 data[x7+y4+z1]*tablex7 ) * tabley4 +
00993 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
00994 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
00995 data[x7+y5+z1]*tablex7 ) * tabley5 +
00996 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
00997 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
00998 data[x7+y6+z1]*tablex7 ) * tabley6 +
00999 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01000 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01001 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01002 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01003 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01004 data[x7+y1+z2]*tablex7 ) * tabley1 +
01005 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01006 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01007 data[x7+y2+z2]*tablex7 ) * tabley2 +
01008 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01009 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01010 data[x7+y3+z2]*tablex7 ) * tabley3 +
01011 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01012 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01013 data[x7+y4+z2]*tablex7 ) * tabley4 +
01014 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01015 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01016 data[x7+y5+z2]*tablex7 ) * tabley5 +
01017 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01018 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01019 data[x7+y6+z2]*tablex7 ) * tabley6 +
01020 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01021 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01022 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01023 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01024 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01025 data[x7+y1+z3]*tablex7 ) * tabley1 +
01026 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01027 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01028 data[x7+y2+z3]*tablex7 ) * tabley2 +
01029 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01030 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01031 data[x7+y3+z3]*tablex7 ) * tabley3 +
01032 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01033 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01034 data[x7+y4+z3]*tablex7 ) * tabley4 +
01035 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01036 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01037 data[x7+y5+z3]*tablex7 ) * tabley5 +
01038 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01039 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01040 data[x7+y6+z3]*tablex7 ) * tabley6 +
01041 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01042 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01043 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01044 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01045 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01046 data[x7+y1+z4]*tablex7 ) * tabley1 +
01047 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01048 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01049 data[x7+y2+z4]*tablex7 ) * tabley2 +
01050 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01051 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01052 data[x7+y3+z4]*tablex7 ) * tabley3 +
01053 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01054 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01055 data[x7+y4+z4]*tablex7 ) * tabley4 +
01056 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01057 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01058 data[x7+y5+z4]*tablex7 ) * tabley5 +
01059 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01060 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01061 data[x7+y6+z4]*tablex7 ) * tabley6 +
01062 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01063 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01064 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01065 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01066 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01067 data[x7+y1+z5]*tablex7 ) * tabley1 +
01068 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01069 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01070 data[x7+y2+z5]*tablex7 ) * tabley2 +
01071 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01072 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01073 data[x7+y3+z5]*tablex7 ) * tabley3 +
01074 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01075 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01076 data[x7+y4+z5]*tablex7 ) * tabley4 +
01077 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01078 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01079 data[x7+y5+z5]*tablex7 ) * tabley5 +
01080 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01081 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01082 data[x7+y6+z5]*tablex7 ) * tabley6 +
01083 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01084 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01085 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01086 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01087 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01088 data[x7+y1+z6]*tablex7 ) * tabley1 +
01089 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01090 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01091 data[x7+y2+z6]*tablex7 ) * tabley2 +
01092 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01093 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01094 data[x7+y3+z6]*tablex7 ) * tabley3 +
01095 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01096 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01097 data[x7+y4+z6]*tablex7 ) * tabley4 +
01098 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01099 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01100 data[x7+y5+z6]*tablex7 ) * tabley5 +
01101 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01102 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01103 data[x7+y6+z6]*tablex7 ) * tabley6 +
01104 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01105 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01106 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01107 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01108 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01109 data[x7+y1+z7]*tablex7 ) * tabley1 +
01110 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01111 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01112 data[x7+y2+z7]*tablex7 ) * tabley2 +
01113 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01114 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01115 data[x7+y3+z7]*tablex7 ) * tabley3 +
01116 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01117 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01118 data[x7+y4+z7]*tablex7 ) * tabley4 +
01119 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01120 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01121 data[x7+y5+z7]*tablex7 ) * tabley5 +
01122 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01123 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01124 data[x7+y6+z7]*tablex7 ) * tabley6 +
01125 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01126 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01127 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01128
01129 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01130 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01131 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01132 }
01133 return pixel/w;
01134 }
01135
01136 float Util::get_pixel_conv_new_background(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb, int xnew, int ynew) {
01137 int K = kb.get_window_size();
01138 int kbmin = -K/2;
01139 int kbmax = -kbmin;
01140 int kbc = kbmax+1;
01141
01142 float pixel =0.0f;
01143 float w=0.0f;
01144
01145 float argdelx = delx;
01146 delx = restrict1(delx, nx);
01147 int inxold = int(round(delx));
01148 if ( ny < 2 ) {
01149 float tablex1 = kb.i0win_tab(delx-inxold+3);
01150 float tablex2 = kb.i0win_tab(delx-inxold+2);
01151 float tablex3 = kb.i0win_tab(delx-inxold+1);
01152 float tablex4 = kb.i0win_tab(delx-inxold);
01153 float tablex5 = kb.i0win_tab(delx-inxold-1);
01154 float tablex6 = kb.i0win_tab(delx-inxold-2);
01155 float tablex7 = kb.i0win_tab(delx-inxold-3);
01156
01157 int x1, x2, x3, x4, x5, x6, x7;
01158
01159 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
01160 x1 = (inxold-3+nx)%nx;
01161 x2 = (inxold-2+nx)%nx;
01162 x3 = (inxold-1+nx)%nx;
01163 x4 = (inxold +nx)%nx;
01164 x5 = (inxold+1+nx)%nx;
01165 x6 = (inxold+2+nx)%nx;
01166 x7 = (inxold+3+nx)%nx;
01167 } else {
01168 x1 = inxold-3;
01169 x2 = inxold-2;
01170 x3 = inxold-1;
01171 x4 = inxold;
01172 x5 = inxold+1;
01173 x6 = inxold+2;
01174 x7 = inxold+3;
01175 }
01176
01177 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01178 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01179 data[x7]*tablex7 ;
01180
01181 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01182 } else if ( nz < 2 ) {
01183
01184 delx = argdelx;
01185
01186 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01187 delx = (float)xnew*2.0f;
01188 dely = (float)ynew*2.0f;
01189 }
01190
01191 int inxold = int(round(delx));
01192 int inyold = int(round(dely));
01193
01194 float tablex1 = kb.i0win_tab(delx-inxold+3);
01195 float tablex2 = kb.i0win_tab(delx-inxold+2);
01196 float tablex3 = kb.i0win_tab(delx-inxold+1);
01197 float tablex4 = kb.i0win_tab(delx-inxold);
01198 float tablex5 = kb.i0win_tab(delx-inxold-1);
01199 float tablex6 = kb.i0win_tab(delx-inxold-2);
01200 float tablex7 = kb.i0win_tab(delx-inxold-3);
01201
01202 float tabley1 = kb.i0win_tab(dely-inyold+3);
01203 float tabley2 = kb.i0win_tab(dely-inyold+2);
01204 float tabley3 = kb.i0win_tab(dely-inyold+1);
01205 float tabley4 = kb.i0win_tab(dely-inyold);
01206 float tabley5 = kb.i0win_tab(dely-inyold-1);
01207 float tabley6 = kb.i0win_tab(dely-inyold-2);
01208 float tabley7 = kb.i0win_tab(dely-inyold-3);
01209
01210 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01211
01212 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
01213 x1 = (inxold-3+nx)%nx;
01214 x2 = (inxold-2+nx)%nx;
01215 x3 = (inxold-1+nx)%nx;
01216 x4 = (inxold +nx)%nx;
01217 x5 = (inxold+1+nx)%nx;
01218 x6 = (inxold+2+nx)%nx;
01219 x7 = (inxold+3+nx)%nx;
01220
01221 y1 = ((inyold-3+ny)%ny)*nx;
01222 y2 = ((inyold-2+ny)%ny)*nx;
01223 y3 = ((inyold-1+ny)%ny)*nx;
01224 y4 = ((inyold +ny)%ny)*nx;
01225 y5 = ((inyold+1+ny)%ny)*nx;
01226 y6 = ((inyold+2+ny)%ny)*nx;
01227 y7 = ((inyold+3+ny)%ny)*nx;
01228 } else {
01229 x1 = inxold-3;
01230 x2 = inxold-2;
01231 x3 = inxold-1;
01232 x4 = inxold;
01233 x5 = inxold+1;
01234 x6 = inxold+2;
01235 x7 = inxold+3;
01236
01237 y1 = (inyold-3)*nx;
01238 y2 = (inyold-2)*nx;
01239 y3 = (inyold-1)*nx;
01240 y4 = inyold*nx;
01241 y5 = (inyold+1)*nx;
01242 y6 = (inyold+2)*nx;
01243 y7 = (inyold+3)*nx;
01244 }
01245
01246 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01247 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01248 data[x7+y1]*tablex7 ) * tabley1 +
01249 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01250 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01251 data[x7+y2]*tablex7 ) * tabley2 +
01252 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01253 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01254 data[x7+y3]*tablex7 ) * tabley3 +
01255 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01256 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01257 data[x7+y4]*tablex7 ) * tabley4 +
01258 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01259 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01260 data[x7+y5]*tablex7 ) * tabley5 +
01261 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01262 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01263 data[x7+y6]*tablex7 ) * tabley6 +
01264 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01265 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01266 data[x7+y7]*tablex7 ) * tabley7;
01267
01268 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01269 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01270 } else {
01271 dely = restrict1(dely, ny);
01272 int inyold = int(Util::round(dely));
01273 delz = restrict1(delz, nz);
01274 int inzold = int(Util::round(delz));
01275
01276 float tablex1 = kb.i0win_tab(delx-inxold+3);
01277 float tablex2 = kb.i0win_tab(delx-inxold+2);
01278 float tablex3 = kb.i0win_tab(delx-inxold+1);
01279 float tablex4 = kb.i0win_tab(delx-inxold);
01280 float tablex5 = kb.i0win_tab(delx-inxold-1);
01281 float tablex6 = kb.i0win_tab(delx-inxold-2);
01282 float tablex7 = kb.i0win_tab(delx-inxold-3);
01283
01284 float tabley1 = kb.i0win_tab(dely-inyold+3);
01285 float tabley2 = kb.i0win_tab(dely-inyold+2);
01286 float tabley3 = kb.i0win_tab(dely-inyold+1);
01287 float tabley4 = kb.i0win_tab(dely-inyold);
01288 float tabley5 = kb.i0win_tab(dely-inyold-1);
01289 float tabley6 = kb.i0win_tab(dely-inyold-2);
01290 float tabley7 = kb.i0win_tab(dely-inyold-3);
01291
01292 float tablez1 = kb.i0win_tab(delz-inzold+3);
01293 float tablez2 = kb.i0win_tab(delz-inzold+2);
01294 float tablez3 = kb.i0win_tab(delz-inzold+1);
01295 float tablez4 = kb.i0win_tab(delz-inzold);
01296 float tablez5 = kb.i0win_tab(delz-inzold-1);
01297 float tablez6 = kb.i0win_tab(delz-inzold-2);
01298 float tablez7 = kb.i0win_tab(delz-inzold-3);
01299
01300 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01301
01302 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
01303 x1 = (inxold-3+nx)%nx;
01304 x2 = (inxold-2+nx)%nx;
01305 x3 = (inxold-1+nx)%nx;
01306 x4 = (inxold +nx)%nx;
01307 x5 = (inxold+1+nx)%nx;
01308 x6 = (inxold+2+nx)%nx;
01309 x7 = (inxold+3+nx)%nx;
01310
01311 y1 = ((inyold-3+ny)%ny)*nx;
01312 y2 = ((inyold-2+ny)%ny)*nx;
01313 y3 = ((inyold-1+ny)%ny)*nx;
01314 y4 = ((inyold +ny)%ny)*nx;
01315 y5 = ((inyold+1+ny)%ny)*nx;
01316 y6 = ((inyold+2+ny)%ny)*nx;
01317 y7 = ((inyold+3+ny)%ny)*nx;
01318
01319 z1 = ((inzold-3+nz)%nz)*nx*ny;
01320 z2 = ((inzold-2+nz)%nz)*nx*ny;
01321 z3 = ((inzold-1+nz)%nz)*nx*ny;
01322 z4 = ((inzold +nz)%nz)*nx*ny;
01323 z5 = ((inzold+1+nz)%nz)*nx*ny;
01324 z6 = ((inzold+2+nz)%nz)*nx*ny;
01325 z7 = ((inzold+3+nz)%nz)*nx*ny;
01326 } else {
01327 x1 = inxold-3;
01328 x2 = inxold-2;
01329 x3 = inxold-1;
01330 x4 = inxold;
01331 x5 = inxold+1;
01332 x6 = inxold+2;
01333 x7 = inxold+3;
01334
01335 y1 = (inyold-3)*nx;
01336 y2 = (inyold-2)*nx;
01337 y3 = (inyold-1)*nx;
01338 y4 = inyold*nx;
01339 y5 = (inyold+1)*nx;
01340 y6 = (inyold+2)*nx;
01341 y7 = (inyold+3)*nx;
01342
01343 z1 = (inzold-3)*nx*ny;
01344 z2 = (inzold-2)*nx*ny;
01345 z3 = (inzold-1)*nx*ny;
01346 z4 = inzold*nx*ny;
01347 z5 = (inzold+1)*nx*ny;
01348 z6 = (inzold+2)*nx*ny;
01349 z7 = (inzold+3)*nx*ny;
01350 }
01351
01352 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01353 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01354 data[x7+y1+z1]*tablex7 ) * tabley1 +
01355 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01356 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01357 data[x7+y2+z1]*tablex7 ) * tabley2 +
01358 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01359 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01360 data[x7+y3+z1]*tablex7 ) * tabley3 +
01361 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01362 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01363 data[x7+y4+z1]*tablex7 ) * tabley4 +
01364 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01365 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01366 data[x7+y5+z1]*tablex7 ) * tabley5 +
01367 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01368 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01369 data[x7+y6+z1]*tablex7 ) * tabley6 +
01370 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01371 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01372 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01373 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01374 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01375 data[x7+y1+z2]*tablex7 ) * tabley1 +
01376 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01377 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01378 data[x7+y2+z2]*tablex7 ) * tabley2 +
01379 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01380 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01381 data[x7+y3+z2]*tablex7 ) * tabley3 +
01382 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01383 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01384 data[x7+y4+z2]*tablex7 ) * tabley4 +
01385 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01386 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01387 data[x7+y5+z2]*tablex7 ) * tabley5 +
01388 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01389 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01390 data[x7+y6+z2]*tablex7 ) * tabley6 +
01391 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01392 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01393 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01394 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01395 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01396 data[x7+y1+z3]*tablex7 ) * tabley1 +
01397 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01398 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01399 data[x7+y2+z3]*tablex7 ) * tabley2 +
01400 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01401 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01402 data[x7+y3+z3]*tablex7 ) * tabley3 +
01403 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01404 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01405 data[x7+y4+z3]*tablex7 ) * tabley4 +
01406 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01407 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01408 data[x7+y5+z3]*tablex7 ) * tabley5 +
01409 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01410 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01411 data[x7+y6+z3]*tablex7 ) * tabley6 +
01412 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01413 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01414 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01415 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01416 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01417 data[x7+y1+z4]*tablex7 ) * tabley1 +
01418 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01419 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01420 data[x7+y2+z4]*tablex7 ) * tabley2 +
01421 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01422 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01423 data[x7+y3+z4]*tablex7 ) * tabley3 +
01424 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01425 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01426 data[x7+y4+z4]*tablex7 ) * tabley4 +
01427 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01428 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01429 data[x7+y5+z4]*tablex7 ) * tabley5 +
01430 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01431 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01432 data[x7+y6+z4]*tablex7 ) * tabley6 +
01433 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01434 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01435 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01436 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01437 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01438 data[x7+y1+z5]*tablex7 ) * tabley1 +
01439 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01440 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01441 data[x7+y2+z5]*tablex7 ) * tabley2 +
01442 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01443 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01444 data[x7+y3+z5]*tablex7 ) * tabley3 +
01445 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01446 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01447 data[x7+y4+z5]*tablex7 ) * tabley4 +
01448 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01449 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01450 data[x7+y5+z5]*tablex7 ) * tabley5 +
01451 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01452 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01453 data[x7+y6+z5]*tablex7 ) * tabley6 +
01454 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01455 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01456 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01457 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01458 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01459 data[x7+y1+z6]*tablex7 ) * tabley1 +
01460 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01461 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01462 data[x7+y2+z6]*tablex7 ) * tabley2 +
01463 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01464 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01465 data[x7+y3+z6]*tablex7 ) * tabley3 +
01466 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01467 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01468 data[x7+y4+z6]*tablex7 ) * tabley4 +
01469 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01470 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01471 data[x7+y5+z6]*tablex7 ) * tabley5 +
01472 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01473 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01474 data[x7+y6+z6]*tablex7 ) * tabley6 +
01475 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01476 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01477 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01478 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01479 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01480 data[x7+y1+z7]*tablex7 ) * tabley1 +
01481 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01482 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01483 data[x7+y2+z7]*tablex7 ) * tabley2 +
01484 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01485 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01486 data[x7+y3+z7]*tablex7 ) * tabley3 +
01487 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01488 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01489 data[x7+y4+z7]*tablex7 ) * tabley4 +
01490 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01491 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01492 data[x7+y5+z7]*tablex7 ) * tabley5 +
01493 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01494 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01495 data[x7+y6+z7]*tablex7 ) * tabley6 +
01496 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01497 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01498 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01499
01500 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01501 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01502 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01503 }
01504 return pixel/w;
01505 }
01506
01507
01508
01509
01510
01511
01512
01513
01514
01515
01516
01517
01518
01519
01520
01521
01522
01523
01524
01525
01526
01527
01528
01529
01530
01531
01532
01533
01534
01535
01536
01537
01538
01539
01540
01541
01542
01543
01544
01545
01546
01547
01548
01549
01550
01551
01552
01553
01554
01555
01556
01557
01558
01559
01560
01561
01562
01563
01564
01565
01566
01567
01568
01569
01570
01571
01572
01573
01574
01575
01576
01577
01578
01579
01580
01581
01582
01583
01584
01585
01586
01587
01588
01589
01590
01591
01592
01593
01594
01595
01596
01597
01598
01599
01600
01601
01602
01603
01604
01605
01606
01607
01608
01609
01610
01611
01612
01613
01614
01615
01616
01617
01618
01619
01620
01621
01622
01623
01624
01625
01626
01627
01628
01629
01630
01631
01632
01633
01634
01635
01636
01637
01638
01639
01640
01641
01642
01643
01644
01645
01646
01647
01648
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672
01673
01674
01675
01676
01677
01678
01679
01680
01681
01682
01683
01684
01685 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01686
01687 int nxreal = nx - 2;
01688 if (nxreal != ny)
01689 throw ImageDimensionException("extractpoint requires ny == nx");
01690 int nhalf = nxreal/2;
01691 bool flip = (nuxnew < 0.f);
01692 if (flip) {
01693 nuxnew *= -1;
01694 nuynew *= -1;
01695 }
01696 if (nuynew >= nhalf-0.5) {
01697 nuynew -= nxreal;
01698 } else if (nuynew < -nhalf-0.5) {
01699 nuynew += nxreal;
01700 }
01701
01702
01703
01704
01705 int ixn = int(Util::round(nuxnew));
01706 int iyn = int(Util::round(nuynew));
01707
01708
01709 static float wy[7];
01710 static float wx[7];
01711
01712 float iynn = nuynew - iyn;
01713 wy[0] = kb.i0win_tab(iynn+3);
01714 wy[1] = kb.i0win_tab(iynn+2);
01715 wy[2] = kb.i0win_tab(iynn+1);
01716 wy[3] = kb.i0win_tab(iynn);
01717 wy[4] = kb.i0win_tab(iynn-1);
01718 wy[5] = kb.i0win_tab(iynn-2);
01719 wy[6] = kb.i0win_tab(iynn-3);
01720
01721 float ixnn = nuxnew - ixn;
01722 wx[0] = kb.i0win_tab(ixnn+3);
01723 wx[1] = kb.i0win_tab(ixnn+2);
01724 wx[2] = kb.i0win_tab(ixnn+1);
01725 wx[3] = kb.i0win_tab(ixnn);
01726 wx[4] = kb.i0win_tab(ixnn-1);
01727 wx[5] = kb.i0win_tab(ixnn-2);
01728 wx[6] = kb.i0win_tab(ixnn-3);
01729
01730 float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01731
01732 complex<float> result(0.f,0.f);
01733 if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01734
01735 for (int iy = 0; iy < 7; iy++) {
01736 int iyp = iyn + iy - 3 ;
01737 for (int ix = 0; ix < 7; ix++) {
01738 int ixp = ixn + ix - 3;
01739 float w = wx[ix]*wy[iy];
01740 complex<float> val = fimage->cmplx(ixp,iyp);
01741 result += val*w;
01742 }
01743 }
01744 } else {
01745
01746 for (int iy = 0; iy < 7; iy++) {
01747 int iyp = iyn + iy - 3;
01748 for (int ix = 0; ix < 7; ix++) {
01749 int ixp = ixn + ix - 3;
01750 bool mirror = false;
01751 int ixt = ixp, iyt = iyp;
01752 if (ixt < 0) {
01753 ixt = -ixt;
01754 iyt = -iyt;
01755 mirror = !mirror;
01756 }
01757 if (ixt > nhalf) {
01758 ixt = nxreal - ixt;
01759 iyt = -iyt;
01760 mirror = !mirror;
01761 }
01762 if (iyt > nhalf-1) iyt -= nxreal;
01763 if (iyt < -nhalf) iyt += nxreal;
01764 float w = wx[ix]*wy[iy];
01765 complex<float> val = fimage->cmplx(ixt,iyt);
01766 if (mirror) result += conj(val)*w;
01767 else result += val*w;
01768 }
01769 }
01770 }
01771 if (flip) result = conj(result)/wsum;
01772 else result /= wsum;
01773 return result;
01774 }
01775
01776
01777
01778
01779
01780
01781
01782
01783
01784
01785
01786
01787
01788
01789
01790
01791
01792
01793
01794
01795
01796
01797
01798
01799
01800
01801
01802
01803
01804
01805
01806
01807
01808
01809
01810
01811
01812
01813
01814
01815
01816
01817
01818
01819
01820
01821
01822
01823
01824
01825
01826
01827
01828
01829
01830
01831
01832
01833
01834
01835
01836
01837
01838
01839
01840
01841
01842
01843
01844
01845
01846
01847
01848
01849
01850
01851
01852
01853
01854
01855
01856
01857
01858
01859
01860
01861
01862
01863
01864
01865
01866
01867
01868
01869
01870
01871
01872
01873
01874
01875
01876
01877
01878
01879
01880
01881
01882
01883
01884
01885
01886
01887
01888
01889
01890
01891
01892
01893
01894
01895
01896
01897
01898
01899
01900
01901
01902
01903
01904 float Util::triquad(float R, float S, float T, float* fdata)
01905 {
01906
01907 const float C2 = 0.5f;
01908 const float C4 = 0.25f;
01909 const float C8 = 0.125f;
01910
01911 float RS = R * S;
01912 float ST = S * T;
01913 float RT = R * T;
01914 float RST = R * ST;
01915
01916 float RSQ = 1-R*R;
01917 float SSQ = 1-S*S;
01918 float TSQ = 1-T*T;
01919
01920 float RM1 = (1-R);
01921 float SM1 = (1-S);
01922 float TM1 = (1-T);
01923
01924 float RP1 = (1+R);
01925 float SP1 = (1+S);
01926 float TP1 = (1+T);
01927
01928 float triquad =
01929 (-C8) * RST * RM1 * SM1 * TM1 * fdata[0] +
01930 ( C4) * ST * RSQ * SM1 * TM1 * fdata[1] +
01931 ( C8) * RST * RP1 * SM1 * TM1 * fdata[2] +
01932 ( C4) * RT * RM1 * SSQ * TM1 * fdata[3] +
01933 (-C2) * T * RSQ * SSQ * TM1 * fdata[4] +
01934 (-C4) * RT * RP1 * SSQ * TM1 * fdata[5] +
01935 ( C8) * RST * RM1 * SP1 * TM1 * fdata[6] +
01936 (-C4) * ST * RSQ * SP1 * TM1 * fdata[7] +
01937 (-C8) * RST * RP1 * SP1 * TM1 * fdata[8] +
01938
01939 ( C4) * RS * RM1 * SM1 * TSQ * fdata[9] +
01940 (-C2) * S * RSQ * SM1 * TSQ * fdata[10] +
01941 (-C4) * RS * RP1 * SM1 * TSQ * fdata[11] +
01942 (-C2) * R * RM1 * SSQ * TSQ * fdata[12] +
01943 RSQ * SSQ * TSQ * fdata[13] +
01944 ( C2) * R * RP1 * SSQ * TSQ * fdata[14] +
01945 (-C4) * RS * RM1 * SP1 * TSQ * fdata[15] +
01946 ( C2) * S * RSQ * SP1 * TSQ * fdata[16] +
01947 ( C4) * RS * RP1 * SP1 * TSQ * fdata[17] +
01948
01949 ( C8) * RST * RM1 * SM1 * TP1 * fdata[18] +
01950 (-C4) * ST * RSQ * SM1 * TP1 * fdata[19] +
01951 (-C8) * RST * RP1 * SM1 * TP1 * fdata[20] +
01952 (-C4) * RT * RM1 * SSQ * TP1 * fdata[21] +
01953 ( C2) * T * RSQ * SSQ * TP1 * fdata[22] +
01954 ( C4) * RT * RP1 * SSQ * TP1 * fdata[23] +
01955 (-C8) * RST * RM1 * SP1 * TP1 * fdata[24] +
01956 ( C4) * ST * RSQ * SP1 * TP1 * fdata[25] +
01957 ( C8) * RST * RP1 * SP1 * TP1 * fdata[26] ;
01958 return triquad;
01959 }
01960
01961 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01962 : M(M_), fc(fc_), ntable(ntable_) {
01963
01964 build_sBtable();
01965 }
01966
01967 void Util::sincBlackman::build_sBtable() {
01968 sBtable.resize(ntable+1);
01969 int ltab = int(round(float(ntable)/1.25f));
01970 int M2 = M/2;
01971 fltb = float(ltab)/M2;
01972 for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01973 float x = 1.0e-7f;
01974 sBtable[0] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01975 for (int i=1; i <= ltab; i++) {
01976 x = float(i)/fltb;
01977 sBtable[i] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01978
01979 }
01980 }
01981
01982 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01983 int N_, float vtable_, int ntable_)
01984 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01985 ntable(ntable_) {
01986
01987 if (0.f == v) v = float(K)/2;
01988 if (0.f == vtable) vtable = v;
01989 alphar = alpha*r;
01990 fac = static_cast<float>(twopi)*alphar*v;
01991 vadjust = 1.0f*v;
01992 facadj = static_cast<float>(twopi)*alphar*vadjust;
01993 build_I0table();
01994 }
01995
01996 float Util::KaiserBessel::i0win(float x) const {
01997 float val0 = float(gsl_sf_bessel_I0(facadj));
01998 float absx = fabs(x);
01999 if (absx > vadjust) return 0.f;
02000 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02001 float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02002 return res;
02003 }
02004
02005 void Util::KaiserBessel::build_I0table() {
02006 i0table.resize(ntable+1);
02007 int ltab = int(round(float(ntable)/1.25f));
02008 fltb = float(ltab)/(K/2);
02009 float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02010 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02011 for (int i=0; i <= ltab; i++) {
02012 float s = float(i)/fltb/N;
02013 if (s < vadjust) {
02014 float rt = sqrt(1.f - pow(s/vadjust, 2));
02015 i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02016 } else {
02017 i0table[i] = 0.f;
02018 }
02019
02020 }
02021 }
02022
02023 float Util::KaiserBessel::I0table_maxerror() {
02024 float maxdiff = 0.f;
02025 for (int i = 1; i <= ntable; i++) {
02026 float diff = fabs(i0table[i] - i0table[i-1]);
02027 if (diff > maxdiff) maxdiff = diff;
02028 }
02029 return maxdiff;
02030 }
02031
02032 float Util::KaiserBessel::sinhwin(float x) const {
02033 float val0 = sinh(fac)/fac;
02034 float absx = fabs(x);
02035 if (0.0 == x) {
02036 float res = 1.0f;
02037 return res;
02038 } else if (absx == alphar) {
02039 return 1.0f/val0;
02040 } else if (absx < alphar) {
02041 float rt = sqrt(1.0f - pow((x/alphar), 2));
02042 float facrt = fac*rt;
02043 float res = (sinh(facrt)/facrt)/val0;
02044 return res;
02045 } else {
02046 float rt = sqrt(pow((x/alphar),2) - 1.f);
02047 float facrt = fac*rt;
02048 float res = (sin(facrt)/facrt)/val0;
02049 return res;
02050 }
02051 }
02052
02053 float Util::FakeKaiserBessel::i0win(float x) const {
02054 float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02055 float absx = fabs(x);
02056 if (absx > vadjust) return 0.f;
02057 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02058 float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02059 return res;
02060 }
02061
02062 void Util::FakeKaiserBessel::build_I0table() {
02063 i0table.resize(ntable+1);
02064 int ltab = int(round(float(ntable)/1.1f));
02065 fltb = float(ltab)/(K/2);
02066 float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02067 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02068 for (int i=0; i <= ltab; i++) {
02069 float s = float(i)/fltb/N;
02070 if (s < vadjust) {
02071 float rt = sqrt(1.f - pow(s/vadjust, 2));
02072 i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02073 } else {
02074 i0table[i] = 0.f;
02075 }
02076 }
02077 }
02078
02079 float Util::FakeKaiserBessel::sinhwin(float x) const {
02080 float val0 = sinh(fac)/fac;
02081 float absx = fabs(x);
02082 if (0.0 == x) {
02083 float res = 1.0f;
02084 return res;
02085 } else if (absx == alphar) {
02086 return 1.0f/val0;
02087 } else if (absx < alphar) {
02088 float rt = sqrt(1.0f - pow((x/alphar), 2));
02089 float facrt = fac*rt;
02090 float res = (sinh(facrt)/facrt)/val0;
02091 return res;
02092 } else {
02093 float rt = sqrt(pow((x/alphar),2) - 1.f);
02094 float facrt = fac*rt;
02095 float res = (sin(facrt)/facrt)/val0;
02096 return res;
02097 }
02098 }
02099
02100 #if 0 // 1-st order KB window
02101 float Util::FakeKaiserBessel::sinhwin(float x) const {
02102
02103 float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02104 float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02105 float absx = fabs(x);
02106 if (0.0 == x) {
02107
02108 float res = val0;
02109 return res;
02110 } else if (absx == alphar) {
02111
02112 return prefix;
02113 } else if (absx < alphar) {
02114 float rt = sqrt(1.0f - pow((x/alphar), 2));
02115
02116 float facrt = facadj*rt;
02117
02118 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02119 return res;
02120 } else {
02121 float rt = sqrt(pow((x/alphar),2) - 1.f);
02122
02123 float facrt = facadj*rt;
02124
02125 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02126 return res;
02127 }
02128 }
02129 #endif // 0
02130
02131
02132
02133 #define circ(i) circ[i-1]
02134 #define numr(i,j) numr[(j-1)*3 + i-1]
02135 #define xim(i,j) xim[(j-1)*nsam + i-1]
02136
02137 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02138 int nsam = image->get_xsize();
02139 int nrow = image->get_ysize();
02140 int nring = numr.size()/3;
02141 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02142 EMData* out = new EMData();
02143 out->set_size(lcirc,1,1);
02144 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02145 float *xim = image->get_data();
02146 float *circ = out->get_data();
02147
02148
02149
02150
02151
02152
02153
02154
02155
02156
02157
02158
02159
02160
02161
02162
02163 double dfi, dpi;
02164 int ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02165 float yq, xold, yold, fi, x, y;
02166
02167 ns2 = nsam/2+1;
02168 nr2 = nrow/2+1;
02169 dpi = 2.0*atan(1.0);
02170
02171 for (i=1;i<=nring;i++) {
02172
02173 inr = numr(1,i);
02174 yq = static_cast<float>(inr);
02175 l = numr(3,i);
02176 if (mode == 'h' || mode == 'H') lt = l/2;
02177 else lt = l/4;
02178
02179 nsim = lt-1;
02180 dfi = dpi/(nsim+1);
02181 kcirc = numr(2,i);
02182 xold = 0.0f;
02183 yold = static_cast<float>(inr);
02184 circ(kcirc) = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02185 xold = static_cast<float>(inr);
02186 yold = 0.0f;
02187 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02188
02189 if (mode == 'f' || mode == 'F') {
02190 xold = 0.0f;
02191 yold = static_cast<float>(-inr);
02192 circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02193 xold = static_cast<float>(-inr);
02194 yold = 0.0f;
02195 circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02196 }
02197
02198 for (j=1;j<=nsim;j++) {
02199 fi = static_cast<float>(dfi*j);
02200 x = sin(fi)*yq;
02201 y = cos(fi)*yq;
02202 xold = x;
02203 yold = y;
02204 circ(j+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02205 xold = y;
02206 yold = -x;
02207 circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02208
02209 if (mode == 'f' || mode == 'F') {
02210 xold = -x;
02211 yold = -y;
02212 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02213 xold = -y;
02214 yold = x;
02215 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02216 }
02217 }
02218 }
02219 return out;
02220 }
02221
02222 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02223 int nsam = image->get_xsize();
02224 int nrow = image->get_ysize();
02225 int nring = numr.size()/3;
02226 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02227 EMData* out = new EMData();
02228 out->set_size(lcirc,1,1);
02229 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02230 float *xim = image->get_data();
02231 float *circ = out->get_data();
02232 double dpi, dfi;
02233 int it, jt, inr, l, nsim, kcirc, lt;
02234 float xold, yold, fi, x, y;
02235
02236
02237
02238 dpi = 2*atan(1.0);
02239 for (it=1; it<=nring; it++) {
02240
02241 inr = numr(1,it);
02242
02243
02244
02245
02246 l = numr(3,it);
02247 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02248 else lt = l / 4;
02249
02250 nsim = lt - 1;
02251 dfi = dpi / (nsim+1);
02252 kcirc = numr(2,it);
02253 xold = 0.0f+cns2;
02254 yold = inr+cnr2;
02255
02256 Assert( kcirc <= lcirc );
02257 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02258
02259 xold = inr+cns2;
02260 yold = 0.0f+cnr2;
02261 Assert( lt+kcirc <= lcirc );
02262 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02263
02264 if ( mode == 'f' || mode == 'F' ) {
02265 xold = 0.0f+cns2;
02266 yold = -inr+cnr2;
02267 Assert( lt+lt+kcirc <= lcirc );
02268 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02269
02270 xold = -inr+cns2;
02271 yold = 0.0f+cnr2;
02272 Assert(lt+lt+lt+kcirc <= lcirc );
02273 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02274 }
02275
02276 for (jt=1; jt<=nsim; jt++) {
02277 fi = static_cast<float>(dfi * jt);
02278 x = sin(fi) * inr;
02279 y = cos(fi) * inr;
02280
02281 xold = x+cns2;
02282 yold = y+cnr2;
02283
02284 Assert( jt+kcirc <= lcirc );
02285 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02286
02287 xold = y+cns2;
02288 yold = -x+cnr2;
02289
02290 Assert( jt+lt+kcirc <= lcirc );
02291 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02292
02293 if ( mode == 'f' || mode == 'F' ) {
02294 xold = -x+cns2;
02295 yold = -y+cnr2;
02296
02297 Assert( jt+lt+lt+kcirc <= lcirc );
02298 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02299
02300 xold = -y+cns2;
02301 yold = x+cnr2;
02302
02303 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02304 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02305 }
02306 }
02307 }
02308 return out;
02309 }
02310
02311 float Util::bilinear(float xold, float yold, int nsam, int nrow, float* xim)
02312 {
02313
02314
02315
02316
02317 float bilinear;
02318 int ixold, iyold;
02319
02320
02321
02322
02323
02324
02325
02326
02327
02328
02329
02330
02331
02332
02333
02334
02335
02336
02337
02338
02339
02340
02341
02342 float xdif, ydif;
02343
02344 ixold = (int) xold;
02345 iyold = (int) yold;
02346 ydif = yold - iyold;
02347
02348
02349
02350
02351
02352 xdif = xold - ixold;
02353 bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02354 xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02355 ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02356
02357 return bilinear;
02358 }
02359
02360 void Util::alrl_ms(float *xim, int nsam, int nrow, float cns2, float cnr2,
02361 int *numr, float *circ, int , int nring, char mode) {
02362 double dpi, dfi;
02363 int it, jt, inr, l, nsim, kcirc, lt;
02364 float xold, yold, fi, x, y;
02365
02366
02367
02368
02369 dpi = 2*atan(1.0);
02370 for (it=1; it<=nring; it++) {
02371
02372 inr = numr(1,it);
02373
02374 l = numr(3,it);
02375 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02376 else lt = l / 4;
02377
02378 nsim = lt - 1;
02379 dfi = dpi / (nsim+1);
02380 kcirc = numr(2,it);
02381
02382
02383 xold = 0.0f+cns2;
02384 yold = inr+cnr2;
02385
02386 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02387
02388 xold = inr+cns2;
02389 yold = 0.0f+cnr2;
02390 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02391
02392 if ( mode == 'f' || mode == 'F' ) {
02393 xold = 0.0f+cns2;
02394 yold = -inr+cnr2;
02395 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02396
02397 xold = -inr+cns2;
02398 yold = 0.0f+cnr2;
02399 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02400 }
02401
02402 for (jt=1; jt<=nsim; jt++) {
02403 fi = static_cast<float>(dfi * jt);
02404 x = sin(fi) * inr;
02405 y = cos(fi) * inr;
02406
02407 xold = x+cns2;
02408 yold = y+cnr2;
02409 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02410
02411 xold = y+cns2;
02412 yold = -x+cnr2;
02413 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02414
02415 if ( mode == 'f' || mode == 'F' ) {
02416 xold = -x+cns2;
02417 yold = -y+cnr2;
02418 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02419
02420 xold = -y+cns2;
02421 yold = x+cnr2;
02422 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02423 }
02424 }
02425 }
02426 }
02427
02428
02429
02430
02431
02432
02433
02434
02435
02436
02437
02438
02439
02440
02441
02442
02443
02444
02445
02446
02447
02448
02449
02450
02451
02452
02453
02454
02455
02456
02457
02458
02459
02460
02461
02462
02463
02464
02465
02466
02467
02468
02469
02470
02471
02472
02473
02474
02475
02476
02477
02478
02479
02480
02481
02482
02483
02484
02485
02486
02487
02488
02489
02490
02491
02492
02493
02494
02495
02496
02497
02498
02499
02500
02501
02502
02503 #undef xim
02504
02505 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02506
02507 int nring = numr.size()/3;
02508 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02509 EMData* out = new EMData();
02510 out->set_size(lcirc,1,1);
02511 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02512 float *circ = out->get_data();
02513 float *fimage = image->get_data();
02514 int nx = image->get_xsize();
02515 int ny = image->get_ysize();
02516 int nz = image->get_zsize();
02517 double dpi, dfi;
02518 int it, jt, inr, l, nsim, kcirc, lt;
02519 float yq, xold, yold, fi, x, y;
02520
02521
02522
02523
02524 dpi = 2*atan(1.0);
02525 for (it=1;it<=nring;it++) {
02526
02527 inr = numr(1,it);
02528 yq = static_cast<float>(inr);
02529
02530 l = numr(3,it);
02531 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02532 else lt = l / 4;
02533
02534 nsim = lt - 1;
02535 dfi = dpi / (nsim+1);
02536 kcirc = numr(2,it);
02537 xold = 0.0f;
02538 yold = static_cast<float>(inr);
02539 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02540
02541
02542 xold = static_cast<float>(inr);
02543 yold = 0.0f;
02544 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02545
02546
02547 if ( mode == 'f' || mode == 'F' ) {
02548 xold = 0.0f;
02549 yold = static_cast<float>(-inr);
02550 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02551
02552
02553 xold = static_cast<float>(-inr);
02554 yold = 0.0f;
02555 circ(lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02556
02557 }
02558
02559 for (jt=1;jt<=nsim;jt++) {
02560 fi = static_cast<float>(dfi * jt);
02561 x = sin(fi) * yq;
02562 y = cos(fi) * yq;
02563
02564 xold = x;
02565 yold = y;
02566 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02567
02568
02569 xold = y;
02570 yold = -x;
02571 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02572
02573
02574 if ( mode == 'f' || mode == 'F' ) {
02575 xold = -x;
02576 yold = -y;
02577 circ(jt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02578
02579
02580 xold = -y;
02581 yold = x;
02582 circ(jt+lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02583
02584 }
02585 }
02586 }
02587 return out;
02588 }
02589
02590
02591
02592
02593
02594
02595
02596
02597
02598
02599
02600
02601
02602
02603
02604
02605
02606
02607
02608
02609
02610
02611
02612
02613 #define tab1(i) tab1[i-1]
02614 #define xcmplx(i,j) xcmplx [(j-1)*2 + i-1]
02615 #define br(i) br[i-1]
02616 #define bi(i) bi[i-1]
02617
02618 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02619 {
02620 double rni,sgn,tr1,tr2,ti1,ti2;
02621 double cc,c,ss,s,t,x2,x3,x4,x5;
02622 int b3,b4,b5,b6,b7,b56;
02623 int n, k, l, j, i, ix0, ix1, status=0;
02624
02625 const double tab1[] = {
02626 9.58737990959775e-5,
02627 1.91747597310703e-4,
02628 3.83495187571395e-4,
02629 7.66990318742704e-4,
02630 1.53398018628476e-3,
02631 3.06795676296598e-3,
02632 6.13588464915449e-3,
02633 1.22715382857199e-2,
02634 2.45412285229123e-2,
02635 4.90676743274181e-2,
02636 9.80171403295604e-2,
02637 1.95090322016128e-1,
02638 3.82683432365090e-1,
02639 7.07106781186546e-1,
02640 1.00000000000000,
02641 };
02642
02643 n=(int)pow(2.0f,ln);
02644
02645 k=abs(ks);
02646 l=16-ln;
02647 b3=n*k;
02648 b6=b3;
02649 b7=k;
02650 if (ks > 0) {
02651 sgn=1.0f;
02652 } else {
02653 sgn=-1.0f;
02654 rni=1.0f/(float)(n);
02655 j=1;
02656 for (i=1; i<=n; i++) {
02657 br(j)=br(j)*rni;
02658 bi(j)=bi(j)*rni;
02659 j=j+k;
02660 }
02661 }
02662
02663 L12:
02664 b6=b6/2;
02665 b5=b6;
02666 b4=2*b6;
02667 b56=b5-b6;
02668
02669 L14:
02670 tr1=br(b5+1);
02671 ti1=bi(b5+1);
02672 tr2=br(b56+1);
02673 ti2=bi(b56+1);
02674
02675 br(b5+1)=tr2-tr1;
02676 bi(b5+1)=ti2-ti1;
02677 br(b56+1)=tr1+tr2;
02678 bi(b56+1)=ti1+ti2;
02679
02680 b5=b5+b4;
02681 b56=b5-b6;
02682 if ( b5 <= b3 ) goto L14;
02683 if ( b6 == b7 ) goto L20;
02684
02685 b4=b7;
02686 cc=2.0f*pow(tab1(l),2);
02687 c=1.0f-cc;
02688 l++;
02689 ss=sgn*tab1(l);
02690 s=ss;
02691
02692 L16:
02693 b5=b6+b4;
02694 b4=2*b6;
02695 b56=b5-b6;
02696
02697 L18:
02698 tr1=br(b5+1);
02699 ti1=bi(b5+1);
02700 tr2=br(b56+1);
02701 ti2=bi(b56+1);
02702 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02703 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02704 br(b56+1)=tr1+tr2;
02705 bi(b56+1)=ti1+ti2;
02706
02707 b5=b5+b4;
02708 b56=b5-b6;
02709 if ( b5 <= b3 ) goto L18;
02710 b4=b5-b6;
02711 b5=b4-b3;
02712 c=-c;
02713 b4=b6-b5;
02714 if ( b5 < b4 ) goto L16;
02715 b4=b4+b7;
02716 if ( b4 >= b5 ) goto L12;
02717
02718 t=c-cc*c-ss*s;
02719 s=s+ss*c-cc*s;
02720 c=t;
02721 goto L16;
02722
02723 L20:
02724 ix0=b3/2;
02725 b3=b3-b7;
02726 b4=0;
02727 b5=0;
02728 b6=ix0;
02729 ix1=0;
02730 if (b6 == b7) goto EXIT;
02731
02732 L22:
02733 b4=b3-b4;
02734 b5=b3-b5;
02735 x2=br(b4+1);
02736 x3=br(b5+1);
02737 x4=bi(b4+1);
02738 x5=bi(b5+1);
02739 br(b4+1)=x3;
02740 br(b5+1)=x2;
02741 bi(b4+1)=x5;
02742 bi(b5+1)=x4;
02743 if(b6 < b4) goto L22;
02744
02745 L24:
02746 b4=b4+b7;
02747 b5=b6+b5;
02748 x2=br(b4+1);
02749 x3=br(b5+1);
02750 x4=bi(b4+1);
02751 x5=bi(b5+1);
02752 br(b4+1)=x3;
02753 br(b5+1)=x2;
02754 bi(b4+1)=x5;
02755 bi(b5+1)=x4;
02756 ix0=b6;
02757
02758 L26:
02759 ix0=ix0/2;
02760 ix1=ix1-ix0;
02761 if( ix1 >= 0) goto L26;
02762
02763 ix0=2*ix0;
02764 b4=b4+b7;
02765 ix1=ix1+ix0;
02766 b5=ix1;
02767 if ( b5 >= b4) goto L22;
02768 if ( b4 < b6) goto L24;
02769
02770 EXIT:
02771 status = 0;
02772 }
02773
02774
02775 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02776 {
02777
02778
02779 int b3,b4,b5,b6,b7,b56;
02780 int n, k, l, j, i, ix0, ix1;
02781 float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02782 int status=0;
02783
02784 const float tab1[] = {
02785 9.58737990959775e-5f,
02786 1.91747597310703e-4f,
02787 3.83495187571395e-4f,
02788 7.66990318742704e-4f,
02789 1.53398018628476e-3f,
02790 3.06795676296598e-3f,
02791 6.13588464915449e-3f,
02792 1.22715382857199e-2f,
02793 2.45412285229123e-2f,
02794 4.90676743274181e-2f,
02795 9.80171403295604e-2f,
02796 1.95090322016128e-1f,
02797 3.82683432365090e-1f,
02798 7.07106781186546e-1f,
02799 1.00000000000000f,
02800 };
02801
02802 n=(int)pow(2.0f,ln);
02803
02804 k=abs(ks);
02805 l=16-ln;
02806 b3=n*k;
02807 b6=b3;
02808 b7=k;
02809 if( ks > 0 ) {
02810 sgn=1.0f;
02811 } else {
02812 sgn=-1.0f;
02813 rni=1.0f/(float)n;
02814 j=1;
02815 for (i=1; i<=n; i++) {
02816 br(j)=br(j)*rni;
02817 bi(j)=bi(j)*rni;
02818 j=j+k;
02819 }
02820 }
02821 L12:
02822 b6=b6/2;
02823 b5=b6;
02824 b4=2*b6;
02825 b56=b5-b6;
02826 L14:
02827 tr1=br(b5+1);
02828 ti1=bi(b5+1);
02829
02830 tr2=br(b56+1);
02831 ti2=bi(b56+1);
02832
02833 br(b5+1)=tr2-tr1;
02834 bi(b5+1)=ti2-ti1;
02835 br(b56+1)=tr1+tr2;
02836 bi(b56+1)=ti1+ti2;
02837
02838 b5=b5+b4;
02839 b56=b5-b6;
02840 if ( b5 <= b3 ) goto L14;
02841 if ( b6 == b7 ) goto L20;
02842
02843 b4=b7;
02844 cc=2.0f*pow(tab1(l),2);
02845 c=1.0f-cc;
02846 l++;
02847 ss=sgn*tab1(l);
02848 s=ss;
02849 L16:
02850 b5=b6+b4;
02851 b4=2*b6;
02852 b56=b5-b6;
02853 L18:
02854 tr1=br(b5+1);
02855 ti1=bi(b5+1);
02856 tr2=br(b56+1);
02857 ti2=bi(b56+1);
02858 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02859 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02860 br(b56+1)=tr1+tr2;
02861 bi(b56+1)=ti1+ti2;
02862
02863 b5=b5+b4;
02864 b56=b5-b6;
02865 if(b5 <= b3) goto L18;
02866 b4=b5-b6;
02867 b5=b4-b3;
02868 c=-c;
02869 b4=b6-b5;
02870 if(b5 < b4) goto L16;
02871 b4=b4+b7;
02872 if(b4 >= b5) goto L12;
02873
02874 t=c-cc*c-ss*s;
02875 s=s+ss*c-cc*s;
02876 c=t;
02877 goto L16;
02878 L20:
02879 ix0=b3/2;
02880 b3=b3-b7;
02881 b4=0;
02882 b5=0;
02883 b6=ix0;
02884 ix1=0;
02885 if ( b6 == b7) goto EXIT;
02886 L22:
02887 b4=b3-b4;
02888 b5=b3-b5;
02889 x2=br(b4+1);
02890 x3=br(b5+1);
02891 x4=bi(b4+1);
02892 x5=bi(b5+1);
02893 br(b4+1)=x3;
02894 br(b5+1)=x2;
02895 bi(b4+1)=x5;
02896 bi(b5+1)=x4;
02897 if (b6 < b4) goto L22;
02898 L24:
02899 b4=b4+b7;
02900 b5=b6+b5;
02901 x2=br(b4+1);
02902 x3=br(b5+1);
02903 x4=bi(b4+1);
02904 x5=bi(b5+1);
02905 br(b4+1)=x3;
02906 br(b5+1)=x2;
02907 bi(b4+1)=x5;
02908 bi(b5+1)=x4;
02909 ix0=b6;
02910 L26:
02911 ix0=ix0/2;
02912 ix1=ix1-ix0;
02913 if(ix1 >= 0) goto L26;
02914
02915 ix0=2*ix0;
02916 b4=b4+b7;
02917 ix1=ix1+ix0;
02918 b5=ix1;
02919 if (b5 >= b4) goto L22;
02920 if (b4 < b6) goto L24;
02921 EXIT:
02922 status = 0;
02923 }
02924
02925 void Util::fftr_q(float *xcmplx, int nv)
02926 {
02927
02928
02929 int nu, inv, nu1, n, isub, n2, i1, i2, i;
02930 float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02931
02932 const float tab1[] = {
02933 9.58737990959775e-5f,
02934 1.91747597310703e-4f,
02935 3.83495187571395e-4f,
02936 7.66990318742704e-4f,
02937 1.53398018628476e-3f,
02938 3.06795676296598e-3f,
02939 6.13588464915449e-3f,
02940 1.22715382857199e-2f,
02941 2.45412285229123e-2f,
02942 4.90676743274181e-2f,
02943 9.80171403295604e-2f,
02944 1.95090322016128e-1f,
02945 3.82683432365090e-1f,
02946 7.07106781186546e-1f,
02947 1.00000000000000f,
02948 };
02949
02950 nu=abs(nv);
02951 inv=nv/nu;
02952 nu1=nu-1;
02953 n=(int)pow(2.f,nu1);
02954 isub=16-nu1;
02955
02956 ss=-tab1(isub);
02957 cc=-2.0f*pow(tab1(isub-1),2.f);
02958 c=1.0f;
02959 s=0.0f;
02960 n2=n/2;
02961 if ( inv > 0) {
02962 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02963 tr=xcmplx(1,1);
02964 ti=xcmplx(2,1);
02965 xcmplx(1,1)=tr+ti;
02966 xcmplx(2,1)=tr-ti;
02967 for (i=1;i<=n2;i++) {
02968 i1=i+1;
02969 i2=n-i+1;
02970 tr1=xcmplx(1,i1);
02971 tr2=xcmplx(1,i2);
02972 ti1=xcmplx(2,i1);
02973 ti2=xcmplx(2,i2);
02974 t=(cc*c-ss*s)+c;
02975 s=(cc*s+ss*c)+s;
02976 c=t;
02977 xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02978 xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02979 xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02980 xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02981 }
02982 } else {
02983 tr=xcmplx(1,1);
02984 ti=xcmplx(2,1);
02985 xcmplx(1,1)=0.5f*(tr+ti);
02986 xcmplx(2,1)=0.5f*(tr-ti);
02987 for (i=1; i<=n2; i++) {
02988 i1=i+1;
02989 i2=n-i+1;
02990 tr1=xcmplx(1,i1);
02991 tr2=xcmplx(1,i2);
02992 ti1=xcmplx(2,i1);
02993 ti2=xcmplx(2,i2);
02994 t=(cc*c-ss*s)+c;
02995 s=(cc*s+ss*c)+s;
02996 c=t;
02997 xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
02998 xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
02999 xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03000 xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03001 }
03002 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03003 }
03004 }
03005
03006
03007 void Util::fftr_d(double *xcmplx, int nv)
03008 {
03009
03010 int i1, i2, nu, inv, nu1, n, isub, n2, i;
03011 double tr1,tr2,ti1,ti2,tr,ti;
03012 double cc,c,ss,s,t;
03013 const double tab1[] = {
03014 9.58737990959775e-5,
03015 1.91747597310703e-4,
03016 3.83495187571395e-4,
03017 7.66990318742704e-4,
03018 1.53398018628476e-3,
03019 3.06795676296598e-3,
03020 6.13588464915449e-3,
03021 1.22715382857199e-2,
03022 2.45412285229123e-2,
03023 4.90676743274181e-2,
03024 9.80171403295604e-2,
03025 1.95090322016128e-1,
03026 3.82683432365090e-1,
03027 7.07106781186546e-1,
03028 1.00000000000000,
03029 };
03030
03031 nu=abs(nv);
03032 inv=nv/nu;
03033 nu1=nu-1;
03034 n=(int)pow(2.0f,nu1);
03035 isub=16-nu1;
03036 ss=-tab1(isub);
03037 cc=-2.0*pow(tab1(isub-1),2);
03038 c=1.0f;
03039 s=0.0f;
03040 n2=n/2;
03041
03042 if ( inv > 0 ) {
03043 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03044 tr=xcmplx(1,1);
03045 ti=xcmplx(2,1);
03046 xcmplx(1,1)=tr+ti;
03047 xcmplx(2,1)=tr-ti;
03048 for (i=1;i<=n2;i++) {
03049 i1=i+1;
03050 i2=n-i+1;
03051 tr1=xcmplx(1,i1);
03052 tr2=xcmplx(1,i2);
03053 ti1=xcmplx(2,i1);
03054 ti2=xcmplx(2,i2);
03055 t=(cc*c-ss*s)+c;
03056 s=(cc*s+ss*c)+s;
03057 c=t;
03058 xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03059 xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03060 xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03061 xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03062 }
03063 } else {
03064 tr=xcmplx(1,1);
03065 ti=xcmplx(2,1);
03066 xcmplx(1,1)=0.5*(tr+ti);
03067 xcmplx(2,1)=0.5*(tr-ti);
03068 for (i=1; i<=n2; i++) {
03069 i1=i+1;
03070 i2=n-i+1;
03071 tr1=xcmplx(1,i1);
03072 tr2=xcmplx(1,i2);
03073 ti1=xcmplx(2,i1);
03074 ti2=xcmplx(2,i2);
03075 t=(cc*c-ss*s)+c;
03076 s=(cc*s+ss*c)+s;
03077 c=t;
03078 xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03079 xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03080 xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03081 xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03082 }
03083 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03084 }
03085 }
03086 #undef tab1
03087 #undef xcmplx
03088 #undef br
03089 #undef bi
03090
03091
03092 void Util::Frngs(EMData* circp, vector<int> numr){
03093 int nring = numr.size()/3;
03094 float *circ = circp->get_data();
03095 int i, l;
03096 for (i=1; i<=nring;i++) {
03097
03098 #ifdef _WIN32
03099 l = (int)( log((float)numr(3,i))/log(2.0f) );
03100 #else
03101 l=(int)(log2(numr(3,i)));
03102 #endif //_WIN32
03103
03104 fftr_q(&circ(numr(2,i)),l);
03105 }
03106 }
03107
03108 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03109 int nring = numr.size()/3;
03110 float *circ = circp->get_data();
03111 int i, l;
03112 for (i=1; i<=nring;i++) {
03113
03114 #ifdef _WIN32
03115 l = (int)( log((float)numr(3,i))/log(2.0f) );
03116 #else
03117 l=(int)(log2(numr(3,i)));
03118 #endif //_WIN32
03119
03120 fftr_q(&circ(numr(2,i)),-l);
03121 }
03122 }
03123 #undef circ
03124
03125 #define b(i) b[i-1]
03126 void Util::prb1d(double *b, int npoint, float *pos) {
03127 double c2,c3;
03128 int nhalf;
03129
03130 nhalf = npoint/2 + 1;
03131 *pos = 0.0;
03132
03133 if (npoint == 7) {
03134 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03135 - 6.*b(6) + 31.*b(7);
03136 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03137 }
03138 else if (npoint == 5) {
03139 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03140 + 46.*b(5) ) / (-70.);
03141 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03142 }
03143 else if (npoint == 3) {
03144 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03145 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03146 }
03147
03148 else {
03149 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03150 - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03151 + 1092.*b(9) ) / (-4620.);
03152 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03153 - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03154 }
03155 if (c3 != 0.0) *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03156 }
03157 #undef b
03158
03159 #define circ1(i) circ1[i-1]
03160 #define circ2(i) circ2[i-1]
03161 #define t(i) t[i-1]
03162 #define q(i) q[i-1]
03163 #define b(i) b[i-1]
03164 #define t7(i) t7[i-1]
03165 Dict Util::Crosrng_e(EMData* circ1p, EMData* circ2p, vector<int> numr, int neg) {
03166
03167 int nring = numr.size()/3;
03168
03169 int maxrin = numr[numr.size()-1];
03170 double qn; float tot;
03171 float *circ1 = circ1p->get_data();
03172 float *circ2 = circ2p->get_data();
03173
03174
03175
03176
03177
03178
03179
03180
03181
03182
03183
03184 float *t;
03185 double t7[7], *q;
03186 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03187 float pos;
03188
03189 #ifdef _WIN32
03190 ip = -(int)(log((float)maxrin)/log(2.0f));
03191 #else
03192 ip = -(int) (log2(maxrin));
03193 #endif //_WIN32
03194
03195 q = (double*)calloc(maxrin, sizeof(double));
03196 t = (float*)calloc(maxrin, sizeof(float));
03197
03198
03199 for (i=1; i<=nring; i++) {
03200 numr3i = numr(3,i);
03201 numr2i = numr(2,i);
03202
03203 t(1) = (circ1(numr2i)) * circ2(numr2i);
03204
03205 if (numr3i != maxrin) {
03206
03207 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03208 t(2) = 0.0;
03209
03210 if (neg) {
03211
03212 for (j=3;j<=numr3i;j=j+2) {
03213 jc = j+numr2i-1;
03214 t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03215 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03216 }
03217 } else {
03218 for (j=3;j<=numr3i;j=j+2) {
03219 jc = j+numr2i-1;
03220 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03221 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03222 }
03223 }
03224 for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03225 } else {
03226 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03227 if (neg) {
03228
03229 for (j=3;j<=maxrin;j=j+2) {
03230 jc = j+numr2i-1;
03231 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03232 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03233 }
03234 } else {
03235 for (j=3;j<=maxrin;j=j+2) {
03236 jc = j+numr2i-1;
03237 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03238 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03239 }
03240 }
03241 for (j = 1; j <= maxrin; j++) q(j) += t(j);
03242 }
03243 }
03244
03245 fftr_d(q,ip);
03246
03247 qn = -1.0e20;
03248 for (j=1;j<=maxrin;j++) {
03249 if (q(j) >= qn) {
03250 qn = q(j); jtot = j;
03251 }
03252 }
03253
03254 for (k=-3; k<=3; k++) {
03255 j = (jtot+k+maxrin-1)%maxrin + 1;
03256 t7(k+4) = q(j);
03257 }
03258
03259 prb1d(t7,7,&pos);
03260
03261 tot = (float)jtot + pos;
03262
03263 if (q) free(q);
03264 if (t) free(t);
03265
03266 Dict retvals;
03267 retvals["qn"] = qn;
03268 retvals["tot"] = tot;
03269 return retvals;
03270 }
03271
03272 Dict Util::Crosrng_ew(EMData* circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03273
03274 int nring = numr.size()/3;
03275
03276 int maxrin = numr[numr.size()-1];
03277 double qn; float tot;
03278 float *circ1 = circ1p->get_data();
03279 float *circ2 = circ2p->get_data();
03280
03281
03282
03283
03284
03285
03286
03287
03288
03289
03290
03291 float *t;
03292 double t7[7], *q;
03293 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03294 float pos;
03295
03296 #ifdef _WIN32
03297 ip = -(int)(log((float)maxrin)/log(2.0f));
03298 #else
03299 ip = -(int) (log2(maxrin));
03300 #endif //_WIN32
03301
03302 q = (double*)calloc(maxrin, sizeof(double));
03303 t = (float*)calloc(maxrin, sizeof(float));
03304
03305
03306 for (i=1;i<=nring;i++) {
03307 numr3i = numr(3,i);
03308 numr2i = numr(2,i);
03309
03310 t(1) = circ1(numr2i) * circ2(numr2i);
03311
03312 if (numr3i != maxrin) {
03313
03314 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03315 t(2) = 0.0;
03316
03317 if (neg) {
03318
03319 for (j=3; j<=numr3i; j=j+2) {
03320 jc = j+numr2i-1;
03321 t(j) = (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03322 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03323 }
03324 } else {
03325 for (j=3; j<=numr3i; j=j+2) {
03326 jc = j+numr2i-1;
03327 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03328 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03329 }
03330 }
03331 for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03332 } else {
03333 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03334 if (neg) {
03335
03336 for (j=3; j<=maxrin; j=j+2) {
03337 jc = j+numr2i-1;
03338 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03339 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03340 }
03341 } else {
03342 for (j=3; j<=maxrin; j=j+2) {
03343 jc = j+numr2i-1;
03344 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03345 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03346 }
03347 }
03348 for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03349 }
03350 }
03351
03352 fftr_d(q,ip);
03353
03354 qn = -1.0e20;
03355 for (j=1;j<=maxrin;j++) {
03356
03357 if (q(j) >= qn) {
03358 qn = q(j);
03359 jtot = j;
03360 }
03361 }
03362
03363 for (k=-3; k<=3; k++) {
03364 j = (jtot+k+maxrin-1)%maxrin + 1;
03365 t7(k+4) = q(j);
03366 }
03367
03368 prb1d(t7,7,&pos);
03369
03370 tot = (float)jtot + pos;
03371
03372
03373 if (t) free(t);
03374
03375 Dict retvals;
03376
03377
03378 retvals["qn"] = qn;
03379 retvals["tot"] = tot;
03380
03381 if (q) free(q);
03382
03383 return retvals;
03384 }
03385
03386 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03387 int nring = numr.size()/3;
03388
03389 int maxrin = numr[numr.size()-1];
03390 double qn; float tot; double qm; float tmt;
03391 float *circ1 = circ1p->get_data();
03392 float *circ2 = circ2p->get_data();
03393
03394
03395
03396
03397
03398
03399
03400
03401
03402
03403
03404
03405 double *t, *q, t7[7];
03406
03407 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03408 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03409
03410 qn = 0.0f;
03411 qm = 0.0f;
03412 tot = 0.0f;
03413 tmt = 0.0f;
03414 #ifdef _WIN32
03415 ip = -(int)(log((float)maxrin)/log(2.0f));
03416 #else
03417 ip = -(int)(log2(maxrin));
03418 #endif //_WIN32
03419
03420
03421
03422
03423
03424 q = (double*)calloc(maxrin,sizeof(double));
03425
03426
03427
03428 t = (double*)calloc(maxrin,sizeof(double));
03429
03430
03431 for (i=1; i<=nring; i++) {
03432
03433 numr3i = numr(3,i);
03434 numr2i = numr(2,i);
03435
03436 t1 = circ1(numr2i) * circ2(numr2i);
03437 q(1) += t1;
03438 t(1) += t1;
03439
03440 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03441 if (numr3i == maxrin) {
03442 q(2) += t1;
03443 t(2) += t1;
03444 } else {
03445 q(numr3i+1) += t1;
03446 t(numr3i+1) += t1;
03447 }
03448
03449 for (j=3; j<=numr3i; j += 2) {
03450 jc = j+numr2i-1;
03451
03452
03453
03454
03455
03456
03457
03458
03459 c1 = circ1(jc);
03460 c2 = circ1(jc+1);
03461 d1 = circ2(jc);
03462 d2 = circ2(jc+1);
03463
03464 t1 = c1 * d1;
03465 t2 = c2 * d2;
03466 t3 = c1 * d2;
03467 t4 = c2 * d1;
03468
03469 q(j) += t1 + t2;
03470 q(j+1) += -t3 + t4;
03471 t(j) += t1 - t2;
03472 t(j+1) += -t3 - t4;
03473 }
03474 }
03475
03476 fftr_d(q,ip);
03477
03478 qn = -1.0e20;
03479 for (j=1; j<=maxrin; j++) {
03480 if (q(j) >= qn) {
03481 qn = q(j);
03482 jtot = j;
03483 }
03484 }
03485
03486 for (k=-3; k<=3; k++) {
03487 j = ((jtot+k+maxrin-1)%maxrin)+1;
03488 t7(k+4) = q(j);
03489 }
03490
03491
03492 prb1d(t7,7,&pos);
03493 tot = (float)(jtot)+pos;
03494
03495
03496
03497
03498 fftr_d(t,ip);
03499
03500
03501 qm = -1.0e20;
03502 for (j=1; j<=maxrin;j++) {
03503 if ( t(j) >= qm ) {
03504 qm = t(j);
03505 jtot = j;
03506 }
03507 }
03508
03509 for (k=-3; k<=3; k++) {
03510 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03511 t7(k+4) = t(j);
03512 }
03513
03514
03515
03516 prb1d(t7,7,&pos);
03517 tmt = float(jtot) + pos;
03518
03519
03520
03521 free(t);
03522 free(q);
03523
03524 Dict retvals;
03525 retvals["qn"] = qn;
03526 retvals["tot"] = tot;
03527 retvals["qm"] = qm;
03528 retvals["tmt"] = tmt;
03529 return retvals;
03530 }
03531
03532 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03533 int nring = numr.size()/3;
03534
03535 int maxrin = numr[numr.size()-1];
03536 double qn; float tot; double qm; float tmt;
03537 float *circ1 = circ1p->get_data();
03538 float *circ2 = circ2p->get_data();
03539
03540
03541
03542
03543
03544
03545
03546
03547
03548
03549
03550
03551 double *t, *q, t7[7];
03552
03553 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03554 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03555
03556 qn = 0.0f;
03557 qm = 0.0f;
03558 tot = 0.0f;
03559 tmt = 0.0f;
03560 #ifdef _WIN32
03561 ip = -(int)(log((float)maxrin)/log(2.0f));
03562 #else
03563 ip = -(int)(log2(maxrin));
03564 #endif //_WIN32
03565
03566
03567
03568
03569
03570 q = (double*)calloc(maxrin,sizeof(double));
03571
03572
03573
03574 t = (double*)calloc(maxrin,sizeof(double));
03575
03576
03577 for (i=1; i<=nring; i++) {
03578
03579 numr3i = numr(3,i);
03580 numr2i = numr(2,i);
03581
03582 t1 = circ1(numr2i) * circ2(numr2i);
03583 q(1) += t1;
03584 t(1) += t1;
03585
03586 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03587 if (numr3i == maxrin) {
03588 q(2) += t1;
03589 t(2) += t1;
03590 } else {
03591 q(numr3i+1) += t1;
03592 t(numr3i+1) += t1;
03593 }
03594
03595 for (j=3; j<=numr3i; j += 2) {
03596 jc = j+numr2i-1;
03597
03598
03599
03600
03601
03602
03603
03604
03605 c1 = circ1(jc);
03606 c2 = circ1(jc+1);
03607 d1 = circ2(jc);
03608 d2 = circ2(jc+1);
03609
03610 t1 = c1 * d1;
03611 t2 = c2 * d2;
03612 t3 = c1 * d2;
03613 t4 = c2 * d1;
03614
03615 q(j) += t1 + t2;
03616 q(j+1) += -t3 + t4;
03617 t(j) += t1 - t2;
03618 t(j+1) += -t3 - t4;
03619 }
03620 }
03621
03622 fftr_d(q,ip);
03623
03624 qn = -1.0e20;
03625
03626 int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03627 int jstep = static_cast<int>(delta/360.0*maxrin);
03628 if (jstep < 1) { jstep = 1; }
03629
03630 for (j=jstart; j<=maxrin; j+=jstep) {
03631 if (q(j) >= qn) {
03632 qn = q(j);
03633 jtot = j;
03634 }
03635 }
03636
03637
03638
03639
03640
03641
03642
03643
03644
03645
03646 tot = (float)(jtot);
03647
03648
03649 fftr_d(t,ip);
03650
03651
03652 qm = -1.0e20;
03653 for (j=jstart; j<=maxrin;j+=jstep) {
03654 if ( t(j) >= qm ) {
03655 qm = t(j);
03656 jtot = j;
03657 }
03658 }
03659
03660
03661
03662
03663
03664
03665
03666
03667
03668
03669
03670 tmt = float(jtot);
03671
03672 free(t);
03673 free(q);
03674
03675 Dict retvals;
03676 retvals["qn"] = qn;
03677 retvals["tot"] = tot;
03678 retvals["qm"] = qm;
03679 retvals["tmt"] = tmt;
03680 return retvals;
03681 }
03682
03683
03684 Dict Util::Crosrng_psi_0_180(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03685 int nring = numr.size()/3;
03686
03687 int maxrin = numr[numr.size()-1];
03688 double qn; float tot; double qm; float tmt;
03689 float *circ1 = circ1p->get_data();
03690 float *circ2 = circ2p->get_data();
03691
03692
03693
03694
03695 double *t, *q, t7[7];
03696
03697 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03698 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03699
03700 qn = 0.0f;
03701 qm = 0.0f;
03702 tot = 0.0f;
03703 tmt = 0.0f;
03704 #ifdef _WIN32
03705 ip = -(int)(log((float)maxrin)/log(2.0f));
03706 #else
03707 ip = -(int)(log2(maxrin));
03708 #endif //_WIN32
03709
03710
03711
03712
03713
03714 q = (double*)calloc(maxrin,sizeof(double));
03715
03716
03717
03718 t = (double*)calloc(maxrin,sizeof(double));
03719
03720
03721 for (i=1; i<=nring; i++) {
03722
03723 numr3i = numr(3,i);
03724 numr2i = numr(2,i);
03725
03726 t1 = circ1(numr2i) * circ2(numr2i);
03727 q(1) += t1;
03728 t(1) += t1;
03729
03730 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03731 if (numr3i == maxrin) {
03732 q(2) += t1;
03733 t(2) += t1;
03734 } else {
03735 q(numr3i+1) += t1;
03736 t(numr3i+1) += t1;
03737 }
03738
03739 for (j=3; j<=numr3i; j += 2) {
03740 jc = j+numr2i-1;
03741
03742
03743
03744
03745
03746
03747
03748
03749 c1 = circ1(jc);
03750 c2 = circ1(jc+1);
03751 d1 = circ2(jc);
03752 d2 = circ2(jc+1);
03753
03754 t1 = c1 * d1;
03755 t2 = c2 * d2;
03756 t3 = c1 * d2;
03757 t4 = c2 * d1;
03758
03759 q(j) += t1 + t2;
03760 q(j+1) += -t3 + t4;
03761 t(j) += t1 - t2;
03762 t(j+1) += -t3 - t4;
03763 }
03764 }
03765
03766 fftr_d(q,ip);
03767
03768 int psi_range = int(psi_max/360.0*maxrin+0.5);
03769 const int psi_0 = 0;
03770 int psi_180 = int( 180.0/360.0*maxrin+0.5);
03771
03772 qn = -1.0e20;
03773 for (k=-psi_range; k<=psi_range; k++) {
03774 j = (k+psi_0+maxrin-1)%maxrin+1;
03775 if (q(j) >= qn) {
03776 qn = q(j);
03777 jtot = j;
03778 }
03779 }
03780
03781 for (k=-psi_range; k<=psi_range; k++) {
03782 j = (k+psi_180+maxrin-1)%maxrin+1;
03783 if (q(j) >= qn) {
03784 qn = q(j);
03785 jtot = j;
03786 }
03787 }
03788
03789 for (k=-3; k<=3; k++) {
03790 j = ((jtot+k+maxrin-1)%maxrin)+1;
03791 t7(k+4) = q(j);
03792 }
03793
03794
03795 prb1d(t7,7,&pos);
03796 tot = (float)(jtot)+pos;
03797
03798
03799
03800
03801 fftr_d(t,ip);
03802
03803
03804 qm = -1.0e20;
03805 for (k=-psi_range; k<=psi_range; k++) {
03806 j = (k+psi_0+maxrin-1)%maxrin+1;
03807 if (t(j) >= qm) {
03808 qm = t(j);
03809 jtot = j;
03810 }
03811 }
03812
03813 for (k=-psi_range; k<=psi_range; k++) {
03814 j = (k+psi_180+maxrin-1)%maxrin+1;
03815 if (t(j) >= qm) {
03816 qm = t(j);
03817 jtot = j;
03818 }
03819 }
03820
03821 for (k=-3; k<=3; k++) {
03822 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03823 t7(k+4) = t(j);
03824 }
03825
03826
03827
03828 prb1d(t7,7,&pos);
03829 tmt = float(jtot) + pos;
03830
03831
03832
03833 free(t);
03834 free(q);
03835
03836 Dict retvals;
03837 retvals["qn"] = qn;
03838 retvals["tot"] = tot;
03839 retvals["qm"] = qm;
03840 retvals["tmt"] = tmt;
03841 return retvals;
03842 }
03843
03844
03845 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag) {
03846
03847
03848 int nring = numr.size()/3;
03849 int maxrin = numr[numr.size()-1];
03850 double qn; float tot; double qm; float tmt;
03851 float *circ1 = circ1p->get_data();
03852 float *circ2 = circ2p->get_data();
03853
03854 double *q, t7[7];
03855
03856 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03857 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03858
03859 qn = 0.0f;
03860 qm = 0.0f;
03861 tot = 0.0f;
03862 tmt = 0.0f;
03863 #ifdef _WIN32
03864 ip = -(int)(log((float)maxrin)/log(2.0f));
03865 #else
03866 ip = -(int)(log2(maxrin));
03867 #endif //_WIN32
03868
03869
03870
03871
03872 q = (double*)calloc(maxrin,sizeof(double));
03873
03874
03875 if (flag==0) {
03876 for (i=1; i<=nring; i++) {
03877
03878 numr3i = numr(3,i);
03879 numr2i = numr(2,i);
03880
03881 t1 = circ1(numr2i) * circ2(numr2i);
03882 q(1) += t1;
03883
03884 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03885 if (numr3i == maxrin) {
03886 q(2) += t1;
03887 } else {
03888 q(numr3i+1) += t1;
03889 }
03890
03891 for (j=3; j<=numr3i; j += 2) {
03892 jc = j+numr2i-1;
03893
03894
03895
03896
03897
03898 c1 = circ1(jc);
03899 c2 = circ1(jc+1);
03900 d1 = circ2(jc);
03901 d2 = circ2(jc+1);
03902
03903 t1 = c1 * d1;
03904 t3 = c1 * d2;
03905 t2 = c2 * d2;
03906 t4 = c2 * d1;
03907
03908 q(j) += t1 + t2;
03909 q(j+1) += -t3 + t4;
03910 }
03911 }
03912 } else {
03913 for (i=1; i<=nring; i++) {
03914
03915 numr3i = numr(3,i);
03916 numr2i = numr(2,i);
03917
03918 t1 = circ1(numr2i) * circ2(numr2i);
03919 q(1) += t1;
03920
03921 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03922 if (numr3i == maxrin) {
03923 q(2) += t1;
03924 } else {
03925 q(numr3i+1) += t1;
03926 }
03927
03928 for (j=3; j<=numr3i; j += 2) {
03929 jc = j+numr2i-1;
03930
03931
03932
03933
03934
03935 c1 = circ1(jc);
03936 c2 = circ1(jc+1);
03937 d1 = circ2(jc);
03938 d2 = circ2(jc+1);
03939
03940 t1 = c1 * d1;
03941 t3 = c1 * d2;
03942 t2 = c2 * d2;
03943 t4 = c2 * d1;
03944
03945 q(j) += t1 - t2;
03946 q(j+1) += -t3 - t4;
03947 }
03948 }
03949 }
03950 fftr_d(q,ip);
03951
03952 qn = -1.0e20;
03953 int psi_pos = int(psi/360.0*maxrin+0.5);
03954
03955 for (k=-5; k<=5; k++) {
03956 j = (psi_pos+maxrin-1)%maxrin+1;
03957 if (q(j) >= qn) {
03958 qn = q(j);
03959 jtot = j;
03960 }
03961 }
03962
03963 for (k=-3; k<=3; k++) {
03964 j = ((jtot+k+maxrin-1)%maxrin)+1;
03965 t7(k+4) = q(j);
03966 }
03967
03968
03969 prb1d(t7,7,&pos);
03970 tot = (float)(jtot)+pos;
03971 free(q);
03972
03973 Dict retvals;
03974 retvals["qn"] = qn;
03975 retvals["tot"] = tot;
03976 return retvals;
03977 }
03978
03979 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03980 int nring = numr.size()/3;
03981
03982 int maxrin = numr[numr.size()-1];
03983 double qn; float tot;
03984 float *circ1 = circ1p->get_data();
03985 float *circ2 = circ2p->get_data();
03986
03987
03988
03989
03990
03991
03992
03993
03994
03995
03996
03997
03998 double *q, t7[7];
03999
04000 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04001 float c1, c2, d1, d2, pos;
04002
04003 qn = 0.0;
04004 tot = 0.0;
04005 #ifdef _WIN32
04006 ip = -(int)(log((float)maxrin)/log(2.0f));
04007 #else
04008 ip = -(int)(log2(maxrin));
04009 #endif //_WIN32
04010
04011
04012
04013
04014
04015 q = (double*)calloc(maxrin,sizeof(double));
04016
04017
04018 for (i=1; i<=nring; i++) {
04019
04020 numr3i = numr(3,i);
04021 numr2i = numr(2,i);
04022
04023 q(1) += circ1(numr2i) * circ2(numr2i);
04024
04025 if (numr3i == maxrin) q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04026 else q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04027
04028 for (j=3; j<=numr3i; j += 2) {
04029 jc = j+numr2i-1;
04030
04031
04032
04033
04034
04035 c1 = circ1(jc);
04036 c2 = circ1(jc+1);
04037 d1 = circ2(jc);
04038 d2 = circ2(jc+1);
04039
04040 q(j) += c1 * d1 + c2 * d2;
04041 q(j+1) += -c1 * d2 + c2 * d1;
04042 }
04043 }
04044
04045 fftr_d(q,ip);
04046
04047 qn = -1.0e20;
04048 for (j=1; j<=maxrin; j++) {
04049 if (q(j) >= qn) {
04050 qn = q(j);
04051 jtot = j;
04052 }
04053 }
04054
04055 for (k=-3; k<=3; k++) {
04056 j = ((jtot+k+maxrin-1)%maxrin)+1;
04057 t7(k+4) = q(j);
04058 }
04059
04060
04061 prb1d(t7,7,&pos);
04062 tot = (float)(jtot)+pos;
04063
04064
04065
04066 free(q);
04067
04068 Dict retvals;
04069 retvals["qn"] = qn;
04070 retvals["tot"] = tot;
04071 return retvals;
04072 }
04073
04074 #define dout(i,j) dout[i+maxrin*j]
04075 #define circ1b(i) circ1b[i-1]
04076 #define circ2b(i) circ2b[i-1]
04077
04078 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04079
04080
04081
04082 int ip, jc, numr3i, numr2i, i, j;
04083 float t1, t2, t3, t4, c1, c2, d1, d2;
04084
04085 int nring = numr.size()/3;
04086
04087 int maxrin = numr[numr.size()-1];
04088
04089 float* circ1b = circ1->get_data();
04090 float* circ2b = circ2->get_data();
04091
04092
04093 double *t, *q;
04094
04095 q = (double*)calloc(maxrin,sizeof(double));
04096 t = (double*)calloc(maxrin,sizeof(double));
04097
04098 #ifdef _WIN32
04099 ip = -(int)(log((float)maxrin)/log(2.0f));
04100 #else
04101 ip = -(int)(log2(maxrin));
04102 #endif //_WIN32
04103
04104
04105
04106
04107
04108
04109
04110 for (i=1; i<=nring; i++) {
04111
04112 numr3i = numr(3,i);
04113 numr2i = numr(2,i);
04114
04115 t1 = circ1b(numr2i) * circ2b(numr2i);
04116 q(1) = q(1)+t1;
04117 t(1) = t(1)+t1;
04118
04119 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04120 if (numr3i == maxrin) {
04121 q(2) += t1;
04122 t(2) += t1;
04123 } else {
04124 q(numr3i+1) += t1;
04125 t(numr3i+1) += t1;
04126 }
04127
04128 for (j=3; j<=numr3i; j=j+2) {
04129 jc = j+numr2i-1;
04130
04131 c1 = circ1b(jc);
04132 c2 = circ1b(jc+1);
04133 d1 = circ2b(jc);
04134 d2 = circ2b(jc+1);
04135
04136 t1 = c1 * d1;
04137 t3 = c1 * d2;
04138 t2 = c2 * d2;
04139 t4 = c2 * d1;
04140
04141 q(j) += t1 + t2;
04142 q(j+1) += - t3 + t4;
04143 t(j) += t1 - t2;
04144 t(j+1) += - t3 - t4;
04145 }
04146 }
04147
04148
04149 fftr_d(q,ip);
04150
04151
04152 fftr_d(t,ip);
04153
04154 EMData* out = new EMData();
04155 out->set_size(maxrin,2,1);
04156 float *dout = out->get_data();
04157 for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04158
04159
04160
04161 free(t);
04162 free(q);
04163 return out;
04164 }
04165
04166
04167 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04168
04169 int maxrin = numr[numr.size()-1];
04170
04171 vector<float> r(2*maxrin);
04172
04173 Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04174
04175 return r;
04176 }
04177
04178 #define dout(i,j) dout[i+maxrin*j]
04179 #define circ1b(i) circ1b[i-1]
04180 #define circ2b(i) circ2b[i-1]
04181
04182 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04183
04184
04185
04186 int ip, jc, numr3i, numr2i, i, j;
04187 float t1, t2, t3, t4, c1, c2, d1, d2;
04188
04189 int nring = numr.size()/3;
04190
04191 int maxrin = numr[numr.size()-1];
04192
04193 float* circ1b = circ1->get_data();
04194 float* circ2b = circ2->get_data();
04195
04196 #ifdef _WIN32
04197 ip = -(int)(log((float)maxrin)/log(2.0f));
04198 #else
04199 ip = -(int)(log2(maxrin));
04200 #endif //_WIN32
04201 for (int i=1; i<=maxrin; i++) {q(i) = 0.0f; t(i) = 0.0f;}
04202
04203
04204
04205
04206
04207 for (i=1; i<=nring; i++) {
04208
04209 numr3i = numr(3,i);
04210 numr2i = numr(2,i);
04211
04212 t1 = circ1b(numr2i) * circ2b(numr2i);
04213 q(1) += t1;
04214 t(1) += t1;
04215
04216 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04217 if (numr3i == maxrin) {
04218 q(2) += t1;
04219 t(2) += t1;
04220 } else {
04221 q(numr3i+1) += t1;
04222 t(numr3i+1) += t1;
04223 }
04224
04225 for (j=3; j<=numr3i; j=j+2) {
04226 jc = j+numr2i-1;
04227
04228 c1 = circ1b(jc);
04229 c2 = circ1b(jc+1);
04230 d1 = circ2b(jc);
04231 d2 = circ2b(jc+1);
04232
04233 t1 = c1 * d1;
04234 t3 = c1 * d2;
04235 t2 = c2 * d2;
04236 t4 = c2 * d1;
04237
04238 q(j) += t1 + t2;
04239 q(j+1) += -t3 + t4;
04240 t(j) += t1 - t2;
04241 t(j+1) += -t3 - t4;
04242 }
04243 }
04244
04245 fftr_q(q,ip);
04246
04247
04248
04249 fftr_q(t,ip);
04250 }
04251
04252
04253
04254 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04255 {
04256
04257 int ip, jc, numr3i, numr2i, i, j;
04258 float t1, t2, t3, t4, c1, c2, d1, d2;
04259
04260 int nring = numr.size()/3;
04261 int maxrin = numr[numr.size()-1];
04262
04263 float* circ1b = circ1->get_data();
04264 float* circ2b = circ2->get_data();
04265
04266 double *q;
04267
04268 q = (double*)calloc(maxrin,sizeof(double));
04269
04270 #ifdef _WIN32
04271 ip = -(int)(log((float)maxrin)/log(2.0f));
04272 #else
04273 ip = -(int)(log2(maxrin));
04274 #endif //_WIN32
04275
04276
04277
04278 for (i=1;i<=nring;i++) {
04279
04280 numr3i = numr(3,i);
04281 numr2i = numr(2,i);
04282
04283 t1 = circ1b(numr2i) * circ2b(numr2i);
04284 q(1) = q(1)+t1;
04285
04286 if (numr3i == maxrin) {
04287 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04288 q(2) = q(2)+t1;
04289 } else {
04290 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04291 q(numr3i+1) = q(numr3i+1)+t1;
04292 }
04293
04294 for (j=3;j<=numr3i;j=j+2) {
04295 jc = j+numr2i-1;
04296
04297 c1 = circ1b(jc);
04298 c2 = circ1b(jc+1);
04299 d1 = circ2b(jc);
04300 d2 = circ2b(jc+1);
04301
04302 t1 = c1 * d1;
04303 t3 = c1 * d2;
04304 t2 = c2 * d2;
04305 t4 = c2 * d1;
04306
04307 q(j) = q(j) + t1 + t2;
04308 q(j+1) = q(j+1) - t3 + t4;
04309 }
04310 }
04311
04312
04313 fftr_d(q,ip);
04314
04315 EMData* out = new EMData();
04316 out->set_size(maxrin,1,1);
04317 float *dout = out->get_data();
04318 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04319 free(q);
04320 return out;
04321
04322 }
04323
04324
04325 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04326 {
04327
04328 int ip, jc, numr3i, numr2i, i, j;
04329 float t1, t2, t3, t4, c1, c2, d1, d2;
04330
04331 int nring = numr.size()/3;
04332 int maxrin = numr[numr.size()-1];
04333
04334 float* circ1b = circ1->get_data();
04335 float* circ2b = circ2->get_data();
04336
04337 double *t;
04338
04339 t = (double*)calloc(maxrin,sizeof(double));
04340
04341 #ifdef _WIN32
04342 ip = -(int)(log((float)maxrin)/log(2.0f));
04343 #else
04344 ip = -(int)(log2(maxrin));
04345 #endif //_WIN32
04346
04347
04348
04349 for (i=1;i<=nring;i++) {
04350
04351 numr3i = numr(3,i);
04352 numr2i = numr(2,i);
04353
04354 t1 = circ1b(numr2i) * circ2b(numr2i);
04355 t(1) = t(1)+t1;
04356
04357 if (numr3i == maxrin) {
04358 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04359 t(2) = t(2)+t1;
04360 }
04361
04362 for (j=3;j<=numr3i;j=j+2) {
04363 jc = j+numr2i-1;
04364
04365 c1 = circ1b(jc);
04366 c2 = circ1b(jc+1);
04367 d1 = circ2b(jc);
04368 d2 = circ2b(jc+1);
04369
04370 t1 = c1 * d1;
04371 t3 = c1 * d2;
04372 t2 = c2 * d2;
04373 t4 = c2 * d1;
04374
04375 t(j) = t(j) + t1 - t2;
04376 t(j+1) = t(j+1) - t3 - t4;
04377 }
04378 }
04379
04380
04381 fftr_d(t,ip);
04382
04383 EMData* out = new EMData();
04384 out->set_size(maxrin,1,1);
04385 float *dout = out->get_data();
04386 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04387 free(t);
04388 return out;
04389
04390 }
04391
04392 #undef circ1b
04393 #undef circ2b
04394 #undef dout
04395
04396 #undef circ1
04397 #undef circ2
04398 #undef t
04399 #undef q
04400 #undef b
04401 #undef t7
04402
04403
04404 #define QUADPI 3.141592653589793238462643383279502884197
04405 #define PI2 2*QUADPI
04406
04407 float Util::ener(EMData* ave, vector<int> numr) {
04408 ENTERFUNC;
04409 long double ener,en;
04410
04411 int nring = numr.size()/3;
04412 float *aveptr = ave->get_data();
04413
04414 ener = 0.0;
04415 for (int i=1; i<=nring; i++) {
04416 int numr3i = numr(3,i);
04417 int np = numr(2,i)-1;
04418 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04419 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04420 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04421 ener += en/numr3i;
04422 }
04423 EXITFUNC;
04424 return static_cast<float>(ener);
04425 }
04426
04427 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04428 ENTERFUNC;
04429 long double ener, en;
04430 float arg, cs, si;
04431
04432 int nima = data.size();
04433 int nring = numr.size()/3;
04434 int maxrin = numr(3,nring);
04435
04436 ener = 0.0;
04437 for (int i=1; i<=nring; i++) {
04438 int numr3i = numr(3,i);
04439 int np = numr(2,i)-1;
04440 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04441 float temp1 = 0.0, temp2 = 0.0;
04442 for (int kk=0; kk<nima; kk++) {
04443 float *ptr = data[kk]->get_data();
04444 temp1 += ptr[np];
04445 temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04446 }
04447 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04448 for (int j=2; j<numr3i; j+=2) {
04449 float tempr = 0.0, tempi = 0.0;
04450 for (int kk=0; kk<nima; kk++) {
04451 float *ptr = data[kk]->get_data();
04452 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04453 cs = cos(arg);
04454 si = sin(arg);
04455 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04456 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04457 }
04458 en += tq*(tempr*tempr+tempi*tempi);
04459 }
04460 ener += en/numr3i;
04461 }
04462 EXITFUNC;
04463 return static_cast<float>(ener);
04464 }
04465
04466 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04467 int nring = numr.size()/3;
04468 float *ave = avep->get_data();
04469 float *dat = datp->get_data();
04470 int i, j, numr3i, np;
04471 float arg, cs, si;
04472 int maxrin = numr(3,nring);
04473 if(mirror == 1) {
04474 for (i=1; i<=nring; i++) {
04475 numr3i = numr(3,i);
04476 np = numr(2,i)-1;
04477 ave[np] += dat[np];
04478 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04479 for (j=2; j<numr3i; j=j+2) {
04480 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04481 cs = cos(arg);
04482 si = sin(arg);
04483
04484 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04485 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04486 }
04487 }
04488 } else {
04489 for (i=1; i<=nring; i++) {
04490 numr3i = numr(3,i);
04491 np = numr(2,i)-1;
04492 ave[np] += dat[np];
04493 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04494 for (j=2; j<numr3i; j=j+2) {
04495 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04496 cs = cos(arg);
04497 si = sin(arg);
04498
04499 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04500 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04501 }
04502 }
04503 }
04504 avep->update();
04505 EXITFUNC;
04506 }
04507
04508 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04509 int nring = numr.size()/3;
04510 float *ave = avep->get_data();
04511 float *dat = datp->get_data();
04512 int i, j, numr3i, np;
04513 float arg, cs, si;
04514 int maxrin = numr(3,nring);
04515 if(mirror == 1) {
04516 for (i=1; i<=nring; i++) {
04517 numr3i = numr(3,i);
04518 np = numr(2,i)-1;
04519 ave[np] -= dat[np];
04520 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04521 for (j=2; j<numr3i; j=j+2) {
04522 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04523 cs = cos(arg);
04524 si = sin(arg);
04525
04526 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04527 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04528 }
04529 }
04530 } else {
04531 for (i=1; i<=nring; i++) {
04532 numr3i = numr(3,i);
04533 np = numr(2,i)-1;
04534 ave[np] -= dat[np];
04535 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04536 for (j=2; j<numr3i; j=j+2) {
04537 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04538 cs = cos(arg);
04539 si = sin(arg);
04540
04541 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04542 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04543 }
04544 }
04545 }
04546 avep->update();
04547 EXITFUNC;
04548 }
04549
04550
04551 #undef QUADPI
04552 #undef PI2
04553
04554 #undef numr
04555 #undef circ
04556
04557
04558 #define QUADPI 3.141592653589793238462643383279502884197
04559 #define PI2 QUADPI*2
04560 #define deg_rad QUADPI/180.0
04561 #define rad_deg 180.0/QUADPI
04562
04563 struct ori_t
04564 {
04565 int iphi;
04566 int itht;
04567 int id;
04568 };
04569
04570
04571 struct cmpang
04572 {
04573 bool operator()( const ori_t& a, const ori_t& b )
04574 {
04575 if( a.itht != b.itht )
04576 {
04577 return a.itht < b.itht;
04578 }
04579
04580 return a.iphi < b.iphi;
04581 }
04582 };
04583
04584
04585 vector<double> Util::cml_weights(const vector<float>& cml){
04586 static const int NBIN = 100;
04587 int nline=cml.size()/2;
04588 vector<double> weights(nline);
04589
04590 vector<ori_t> angs(nline);
04591 for( int i=0; i < nline; ++i ) {
04592 angs[i].iphi = int( NBIN*cml[2*i] );
04593 angs[i].itht = int( NBIN*cml[2*i+1] );
04594 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04595 angs[i].id = i;
04596 }
04597
04598
04599
04600 std::sort( angs.begin(), angs.end(), cmpang() );
04601
04602 vector<float> newphi;
04603 vector<float> newtht;
04604 vector< vector<int> > indices;
04605
04606 int curt_iphi = -1;
04607 int curt_itht = -1;
04608 for(unsigned int i=0 ;i < angs.size(); ++i ) {
04609 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04610 Assert( indices.size() > 0 );
04611 indices.back().push_back(angs[i].id);
04612 } else {
04613 curt_iphi = angs[i].iphi;
04614 curt_itht = angs[i].itht;
04615
04616 newphi.push_back( float(curt_iphi)/NBIN );
04617 newtht.push_back( float(curt_itht)/NBIN );
04618 indices.push_back( vector<int>(1,angs[i].id) );
04619 }
04620 }
04621
04622
04623
04624
04625 int num_agl = newphi.size();
04626
04627 if(num_agl>2) {
04628 vector<double> w=Util::vrdg(newphi, newtht);
04629
04630 Assert( w.size()==newphi.size() );
04631 Assert( indices.size()==newphi.size() );
04632
04633 for(unsigned int i=0; i < newphi.size(); ++i ) {
04634
04635
04636
04637
04638
04639
04640
04641
04642 for(unsigned int j=0; j < indices[i].size(); ++j ) {
04643 int id = indices[i][j];
04644 weights[id] = w[i]/indices[i].size();
04645
04646 }
04647
04648
04649
04650 }
04651 } else {
04652 cout<<"warning in Util.cml_weights"<<endl;
04653 double val = PI2/float(nline);
04654 for(int i=0; i<nline; i++) weights[i]=val;
04655 }
04656
04657 return weights;
04658
04659 }
04660
04661
04662
04663
04664
04665 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04666 {
04667 int i;
04668 int nx=img->get_xsize();
04669 float *img_ptr = img->get_data();
04670 float *line_ptr = line->get_data();
04671 for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04672 img->update();
04673 }
04674
04675 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04676 int j;
04677 int nx = sino->get_xsize();
04678 int i = nx * pos_line;
04679 float r1, r2;
04680 float *line_ptr = line->get_data();
04681 float *sino_ptr = sino->get_data();
04682 for (j=ilf;j<=ihf; j += 2) {
04683 r1 = line_ptr[j];
04684 r2 = line_ptr[j + 1];
04685 sino_ptr[i + j - ilf] = r1;
04686 sino_ptr[i + j - ilf + 1] = r2;
04687 sino_ptr[i + nx * nblines + j - ilf] = r1;
04688 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04689 }
04690 sino->update();
04691 }
04692
04693 vector<double> Util::cml_init_rot(vector<float> Ori){
04694 int nb_ori = Ori.size() / 4;
04695 int i, ind;
04696 float ph, th, ps;
04697 double cph, cth, cps, sph, sth, sps;
04698 vector<double> Rot(nb_ori*9);
04699 for (i=0; i<nb_ori; ++i){
04700 ind = i*4;
04701
04702 ph = Ori[ind+2]-90;
04703 th = Ori[ind+1];
04704 ps = Ori[ind]+90;
04705 ph *= deg_rad;
04706 th *= deg_rad;
04707 ps *= deg_rad;
04708
04709 cph = cos(ph);
04710 cth = cos(th);
04711 cps = cos(ps);
04712 sph = sin(ph);
04713 sth = sin(th);
04714 sps = sin(ps);
04715
04716 ind = i*9;
04717 Rot[ind] = cph*cps-cth*sps*sph;
04718 Rot[ind+1] = cph*sps+cth*cps*sph;
04719 Rot[ind+2] = sth*sph;
04720 Rot[ind+3] = -sph*cps-cth*sps*cph;
04721 Rot[ind+4] = -sph*sps+cth*cps*cph;
04722 Rot[ind+5] = sth*cph;
04723 Rot[ind+6] = sth*sps;
04724 Rot[ind+7] = -sth*cps;
04725 Rot[ind+8] = cth;
04726 }
04727
04728 return Rot;
04729 }
04730
04731 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04732 float ph, ps;
04733 double cph, cth, cps, sph, sth, sps;
04734 int ind = iprj*9;
04735
04736 ph = nps-90;
04737 ps = nph+90;
04738 ph *= deg_rad;
04739 th *= deg_rad;
04740 ps *= deg_rad;
04741
04742 cph = cos(ph);
04743 cth = cos(th);
04744 cps = cos(ps);
04745 sph = sin(ph);
04746 sth = sin(th);
04747 sps = sin(ps);
04748
04749 Rot[ind] = (float)(cph*cps-cth*sps*sph);
04750 Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04751 Rot[ind+2] = (float)(sth*sph);
04752 Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04753 Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04754 Rot[ind+5] = (float)(sth*cph);
04755 Rot[ind+6] = (float)(sth*sps);
04756 Rot[ind+7] = (float)(-sth*cps);
04757 Rot[ind+8] = (float)(cth);
04758
04759 return Rot;
04760 }
04761
04762 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04763 vector<int> com(2*(n_prj - 1));
04764 int a = i_prj*9;
04765 int i, b, c;
04766 int n1=0, n2=0;
04767 float vmax = 1 - 1.0e-6f;
04768 double r11, r12, r13, r23, r31, r32, r33;
04769
04770 c = 0;
04771 for (i=0; i<n_prj; ++i){
04772 if (i!=i_prj){
04773 b = i*9;
04774
04775 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04776 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04777 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04778 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04779 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04780 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04781 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04782 if (r33 > vmax) {
04783 n2 = 270;
04784 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04785 }
04786 else if (r33 < -vmax) {
04787 n2 = 270;
04788 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04789 } else {
04790 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04791 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04792 if (n1 < 0) {n1 += 360;}
04793 if (n2 <= 0) {n2 = abs(n2);}
04794 else {n2 = 360 - n2;}
04795 }
04796
04797 if (n1 >= 360){n1 = n1 % 360;}
04798 if (n2 >= 360){n2 = n2 % 360;}
04799
04800
04801 b = c*2;
04802 com[b] = n1;
04803 com[b+1] = n2;
04804 ++c;
04805 }
04806 }
04807
04808 return com;
04809
04810 }
04811
04812 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int n_prj, int n_lines) {
04813 vector<int> com(2*n_lines);
04814 int a=0, b, c, l;
04815 int n1=0, n2=0, mem=-1;
04816 float vmax = 1 - 1.0e-6f;
04817 double r11, r12, r13, r23, r31, r32, r33;
04818 c = 0;
04819 for (l=0; l<n_lines; ++l){
04820 c = 2*l;
04821 if (seq[c]!=mem){
04822 mem = seq[c];
04823 a = seq[c]*9;
04824 }
04825 b = seq[c+1]*9;
04826
04827
04828 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04829 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04830 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04831 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04832 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04833 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04834 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04835 if (r33 > vmax) {
04836 n2 = 270;
04837 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04838 }
04839 else if (r33 < -vmax) {
04840 n2 = 270;
04841 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04842 } else {
04843 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04844 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04845 if (n1 < 0) {n1 += 360;}
04846 if (n2 <= 0) {n2 = abs(n2);}
04847 else {n2 = 360 - n2;}
04848 }
04849 if (n1 >= 360){n1 = n1 % 360;}
04850 if (n2 >= 360){n2 = n2 % 360;}
04851
04852
04853 com[c] = n1;
04854 com[c+1] = n2;
04855 }
04856
04857 return com;
04858
04859 }
04860
04861 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int nprj, int nlines){
04862
04863 vector<double> cml(2*nlines);
04864 float ph1, th1;
04865 float ph2, th2;
04866 double nx, ny, nz;
04867 double norm;
04868 double sth1=0, sph1=0, cth1=0, cph1=0;
04869 double sth2, sph2, cth2, cph2;
04870 int l, ind, c;
04871 int mem = -1;
04872 for (l=0; l<nlines; ++l){
04873 c = 2*l;
04874 if (seq[c]!=mem){
04875 mem = seq[c];
04876 ind = 4*seq[c];
04877 ph1 = Ori[ind]*deg_rad;
04878 th1 = Ori[ind+1]*deg_rad;
04879 sth1 = sin(th1);
04880 sph1 = sin(ph1);
04881 cth1 = cos(th1);
04882 cph1 = cos(ph1);
04883 }
04884 ind = 4*seq[c+1];
04885 ph2 = Ori[ind]*deg_rad;
04886 th2 = Ori[ind+1]*deg_rad;
04887 sth2 = sin(th2);
04888 cth2 = cos(th2);
04889 sph2 = sin(ph2);
04890 cph2 = cos(ph2);
04891
04892 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04893 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04894 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04895 norm = sqrt(nx*nx+ny*ny+nz*nz);
04896 nx /= norm;
04897 ny /= norm;
04898 nz /= norm;
04899
04900 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04901
04902 cml[c+1] = acos(nz);
04903 if (cml[c+1] == 0) {cml[c] = 0;}
04904 else {
04905 cml[c+1] *= rad_deg;
04906 if (cml[c+1] > 89.99) {cml[c+1] = 89.99;}
04907 cml[c] = rad_deg * atan2(nx, ny);
04908 cml[c] = fmod(360 + cml[c], 360);
04909
04910 }
04911 }
04912
04913 return cml;
04914 }
04915
04916 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04917 double res = 0;
04918 double buf = 0;
04919 float* line_1;
04920 float* line_2;
04921 int i, n, ind;
04922 int lnlen = data[0]->get_xsize();
04923 for (n=0; n<n_lines; ++n) {
04924 ind = n*2;
04925 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04926 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04927 buf = 0;
04928 for (i=0; i<lnlen; ++i) {
04929 buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04930 }
04931 res += buf * weights[n];
04932 }
04933
04934 return res;
04935
04936 }
04937
04938 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04939 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04940
04941
04942
04943 vector<double> res(2);
04944 int lnlen = data[0]->get_xsize();
04945 int end = 2*(n_prj-1);
04946 double disc, buf, bdisc, tmp;
04947 int n, i, ipsi, ind, bipsi, c;
04948 float* line_1;
04949 float* line_2;
04950 bdisc = 1.0e6;
04951 bipsi = -1;
04952
04953 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04954
04955 disc = 0;
04956 c = 0;
04957 for (n=0; n<n_prj; ++n) {
04958 if(n!=iprj) {
04959 ind = 2*c;
04960 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04961 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04962 buf = 0;
04963 for (i=0; i<lnlen; ++i) {
04964 tmp = line_1[i]-line_2[i];
04965 buf += tmp*tmp;
04966 }
04967 disc += buf * weights[iw[c]];
04968 ++c;
04969 }
04970 }
04971
04972 if (disc <= bdisc) {
04973 bdisc = disc;
04974 bipsi = ipsi;
04975 }
04976
04977 for (i=0; i<end; i+=2){
04978 com[i] += d_psi;
04979 if (com[i] >= n_psi) {com[i] = com[i] % n_psi;}
04980 }
04981 }
04982 res[0] = bdisc;
04983 res[1] = float(bipsi);
04984
04985 return res;
04986 }
04987
04988 #undef QUADPI
04989 #undef PI2
04990 #undef deg_rad
04991 #undef rad_deg
04992
04993
04994
04995
04996
04997
04998 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
04999 ENTERFUNC;
05000
05001 int nima = data.size();
05002 vector<float> res(nima);
05003 double result = 0.;
05004 double valmin = 1.0e20;
05005 int valpos = -1;
05006
05007 for (int kk=0; kk<nima; kk++){
05008 result = 0;
05009
05010 float *y_data = data[kk]->get_data();
05011 float *x_data = image->get_data();
05012 long totsize = image->get_xsize()*image->get_ysize();
05013 for (long i = 0; i < totsize; i++) {
05014 double temp = x_data[i]- y_data[i];
05015 result += temp*temp;
05016 }
05017 result /= totsize;
05018 res[kk] = (float)result;
05019
05020 if(result<valmin) {valmin = result; valpos = kk;}
05021
05022 }
05023
05024 Dict retvals;
05025 retvals["dist"] = res;
05026 retvals["pos"] = valpos;
05027
05028 EXITFUNC;
05029 return retvals;
05030
05031 }
05032
05033 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05034 ENTERFUNC;
05035
05036 int nima = data.size();
05037 vector<float> res(nima);
05038 double result = 0.;
05039 double valmin = 1.0e20;
05040 int valpos = -1;
05041
05042 for (int kk=0; kk<nima; kk++){
05043 result = 0;
05044
05045
05046 float *y_data = data[kk]->get_data();
05047 float *x_data = image->get_data();
05048
05049
05050 int nx = data[kk]->get_xsize();
05051 int ny = data[kk]->get_ysize();
05052 nx = (nx - 2 + data[kk]->is_fftodd());
05053 int lsd2 = (nx + 2 - nx%2) ;
05054
05055 int ixb = 2*((nx+1)%2);
05056 int iyb = ny%2;
05057 int iz = 0;
05058
05059 for ( int iy = 0; iy <= ny-1; iy++) {
05060 for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05061 int ii = ix + (iy + iz * ny)* lsd2;
05062 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05063 }
05064 }
05065 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05066 int ii = (iy + iz * ny)* lsd2;
05067 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05068 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05069 }
05070 if(nx%2 == 0) {
05071 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05072 int ii = lsd2 - 2 + (iy + iz * ny)* lsd2;
05073 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05074 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05075 }
05076
05077 }
05078 result *= 2;
05079 result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05080 if(ny%2 == 0) {
05081 int ii = (ny/2 + iz * ny)* lsd2;
05082 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05083 }
05084 if(nx%2 == 0) {
05085 int ii = lsd2 - 2 + (0 + iz * ny)* lsd2;
05086 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05087 if(ny%2 == 0) {
05088 int ii = lsd2 - 2 +(ny/2 + iz * ny)* lsd2;
05089 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05090 }
05091 }
05092
05093 result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05094 res[kk] = (float)result;
05095
05096 if(result<valmin) {valmin = result; valpos = kk;}
05097
05098 }
05099
05100 Dict retvals;
05101 retvals["dist"] = res;
05102 retvals["pos"] = valpos;
05103
05104 EXITFUNC;
05105 return retvals;
05106 }
05107
05108 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05109 long int d2 = group2[s2 - 1] - group2[0];
05110 long int p2 = 0;
05111 long int i1 = 0;
05112 long int i2 = 0;
05113 long int max = 0;
05114 long int cont = 0;
05115 long int i = 0;
05116 int stop1 = 0;
05117 int stop2 = 0;
05118
05119 for (i=0; i<s1; i++) {
05120 p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05121 if (p2 >= s2) {p2 = s2 - 1;}
05122 i1 = p2;
05123 i2 = p2;
05124 max = s2;
05125 if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05126
05127 stop1 = 0;
05128 stop2 = 0;
05129 while (max--) {
05130 if (group1[i] == group2[i1]) {
05131 if (flag) {stb[cont] = group1[i];}
05132 cont++;
05133 break;
05134 }
05135 if (group2[i1] < group1[i]) {stop1=1;}
05136 if (group1[i] == group2[i2]) {
05137 if (flag) {stb[cont] = group1[i];}
05138 cont++;
05139 break;
05140 }
05141 if (group2[i2] > group1[i]) {stop2=1;}
05142
05143
05144 if (stop1 & stop2) {break;}
05145 i1--;
05146 i2++;
05147 if (i1 < 0) {i1 = 0;}
05148 if (i2 >= s2) {i2 = s2 - 1;}
05149 }
05150
05151 }
05152
05153 return cont;
05154 }
05155
05156
05157
05158 #define old_ptr(i,j,k) old_ptr[i+(j+(k*ny))*nx]
05159 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*new_nx]
05160 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05161 {
05162
05163 if (!img) {
05164 throw NullPointerException("NULL input image");
05165 }
05166
05167
05168
05169 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05170
05171
05172
05173
05174 if ((x_step-1 > nx/2 || y_step-1 > ny/2 || z_step-1 > nz/2) || (x_step-1)<0 || (y_step-1)<0 || (z_step-1)<0)
05175 {
05176 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05177 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05178 }
05179
05180
05181
05182
05183 int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05184
05185
05186
05187
05188 int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05189 int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05190 int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05191 if(r1>1){r1=1;}
05192 if(r2>1){r2=1;}
05193 if(r3>1){r3=1;}
05194 int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05195
05196
05197
05198 EMData* img2 = new EMData();
05199 img2->set_size(new_nx,new_ny,new_nz);
05200 float *new_ptr = img2->get_data();
05201 float *old_ptr = img->get_data();
05202 int iptr, jptr, kptr = 0;
05203 for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05204 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05205 for (int i=new_st_x; i<nx; i+=x_step) {
05206 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05207 iptr++;}
05208 jptr++;}
05209 kptr++;}
05210 img2->update();
05211 return img2;
05212 }
05213 #undef old_ptr
05214 #undef new_ptr
05215
05216 #define inp(i,j,k) inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*nx]
05217 #define outp(i,j,k) outp[i+(j+(k*new_ny))*new_nx]
05218 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05219 {
05220
05221 if (!img) throw NullPointerException("NULL input image");
05222
05223
05224
05225 int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05226
05227
05228
05229 if(new_nx>nx || new_ny>ny || new_nz>nz)
05230 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size.");
05231 if((nx/2)-(new_nx/2)+x_offset<0 || (ny/2)-(new_ny/2)+y_offset<0 || (nz/2)-(new_nz/2)+z_offset<0)
05232 throw ImageDimensionException("The offset inconsistent with the input image size.");
05233 if(x_offset>((nx-(nx/2))-(new_nx-(new_nx/2))) || y_offset>((ny-(ny/2))-(new_ny-(new_ny/2))) || z_offset>((nz-(nz/2))-(new_nz-(new_nz/2))))
05234 throw ImageDimensionException("The offset inconsistent with the input image size.");
05235
05236
05237
05238 int new_st_x = nx/2-new_nx/2 + x_offset,
05239 new_st_y = ny/2-new_ny/2 + y_offset,
05240 new_st_z = nz/2-new_nz/2 + z_offset;
05241
05242
05243
05244 if (new_st_x<0 || new_st_y<0 || new_st_z<0)
05245 throw ImageDimensionException("The offset inconsistent with the input image size.");
05246
05247
05248 EMData* wind = img->copy_head();
05249 wind->set_size(new_nx, new_ny, new_nz);
05250 float *outp=wind->get_data();
05251 float *inp=img->get_data();
05252
05253 for (int k=0; k<new_nz; k++)
05254 for(int j=0; j<new_ny; j++)
05255 for(int i=0; i<new_nx; i++)
05256 outp(i,j,k) = inp(i,j,k);
05257 wind->update();
05258 return wind;
05259 }
05260 #undef inp
05261 #undef outp
05262
05263 #define inp(i,j,k) inp[i+(j+(k*ny))*nx]
05264 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*new_nx]
05265 EMData *Util::pad(EMData* img,int new_nx, int new_ny, int new_nz, int x_offset, int y_offset, int z_offset,char *params)
05266 {
05267
05268 if (!img) throw NullPointerException("NULL input image");
05269
05270
05271
05272 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05273
05274
05275
05276 if(new_nx<nx || new_ny<ny || new_nz<nz)
05277 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05278 if((new_nx/2)-(nx/2)+x_offset<0 || (new_ny/2)-(ny/2)+y_offset<0 || (new_nz/2)-(nz/2)+z_offset<0)
05279 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05280 if(x_offset>((new_nx-(new_nx/2))-(nx-(nx/2))) || y_offset>((new_ny-(new_ny/2))-(ny-(ny/2))) || z_offset>((new_nz-(new_nz/2))-(nz-(nz/2))))
05281 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05282
05283
05284 EMData* pading = img->copy_head();
05285 pading->set_size(new_nx, new_ny, new_nz);
05286 float *inp = img->get_data();
05287 float *outp = pading->get_data();
05288
05289
05290
05291
05292 float background;
05293
05294 if (strcmp(params,"average")==0) background = img->get_attr("mean");
05295 else if (strcmp(params,"circumference")==0) {
05296 float sum1=0.0f;
05297 int cnt=0;
05298 for(int i=0;i<nx;i++) {
05299 sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05300 cnt+=2;
05301 }
05302 if(nz-1 == 0) {
05303 for (int j=1;j<ny-1;j++) {
05304 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05305 cnt+=2;
05306 }
05307 } else {
05308 for (int k=1;k<nz-1;k++) {
05309 for (int j=1;j<ny-1;j++) {
05310 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05311 cnt+=2;
05312 }
05313 }
05314 }
05315 background = sum1/cnt;
05316 } else {
05317 background = static_cast<float>( atof( params ) );
05318 }
05319
05320
05321
05322 int new_st_x=0,new_st_y=0,new_st_z=0;
05323 for (int k=0;k<new_nz;k++)
05324 for(int j=0;j<new_ny;j++)
05325 for (int i=0;i<new_nx;i++)
05326 outp(i,j,k)=background;
05327
05328
05329
05330 new_st_x=int((new_nx/2-nx/2) + x_offset);
05331 new_st_y=int((new_ny/2-ny/2) + y_offset);
05332 new_st_z=int((new_nz/2-nz/2) + z_offset);
05333
05334
05335 for (int k=0;k<nz;k++)
05336 for(int j=0;j<ny;j++)
05337 for(int i=0;i<nx;i++)
05338 outp(i,j,k)=inp(i,j,k);
05339 pading->update();
05340 return pading;
05341 }
05342 #undef inp
05343 #undef outp
05344
05345
05346 void Util::colreverse(float* beg, float* end, int nx) {
05347 float* tmp = new float[nx];
05348 int n = (end - beg)/nx;
05349 int nhalf = n/2;
05350 for (int i = 0; i < nhalf; i++) {
05351
05352 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05353 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05354 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05355 }
05356 delete[] tmp;
05357 }
05358
05359 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05360 {
05361 int nxy = nx*ny;
05362 colreverse(beg, end, nxy);
05363 }
05364
05365
05366 void Util::cyclicshift(EMData *image, Dict params) {
05367
05368 if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05369
05370 int dx = params["dx"];
05371 int dy = params["dy"];
05372 int dz = params["dz"];
05373
05374
05375 int nx = image->get_xsize();
05376 dx %= nx;
05377 if (dx < 0) dx += nx;
05378 int ny = image->get_ysize();
05379 dy %= ny;
05380 if (dy < 0) dy += ny;
05381 int nz = image->get_zsize();
05382 dz %= nz;
05383 if (dz < 0) dz += nz;
05384
05385 int mx = -(dx - nx);
05386 int my = -(dy - ny);
05387 int mz = -(dz - nz);
05388
05389 float* data = image->get_data();
05390
05391 if (mx != 0) {
05392 for (int iz = 0; iz < nz; iz++)
05393 for (int iy = 0; iy < ny; iy++) {
05394
05395 int offset = nx*iy + nx*ny*iz;
05396 reverse(&data[offset],&data[offset+mx]);
05397 reverse(&data[offset+mx],&data[offset+nx]);
05398 reverse(&data[offset],&data[offset+nx]);
05399 }
05400 }
05401
05402 if (my != 0) {
05403 for (int iz = 0; iz < nz; iz++) {
05404 int offset = nx*ny*iz;
05405 colreverse(&data[offset], &data[offset + my*nx], nx);
05406 colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05407 colreverse(&data[offset], &data[offset + ny*nx], nx);
05408 }
05409 }
05410 if (mz != 0) {
05411 slicereverse(&data[0], &data[mz*ny*nx], nx, ny);
05412 slicereverse(&data[mz*ny*nx], &data[nz*ny*nx], nx, ny);
05413 slicereverse(&data[0], &data[nz*ny*nx], nx ,ny);
05414 }
05415 image->update();
05416 }
05417
05418
05419
05420
05421 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05422 {
05423 if (image->is_complex())
05424 throw ImageFormatException("Cannot do histogram on Fourier image");
05425
05426 float *imageptr=0, *maskptr=0;
05427 int nx=image->get_xsize();
05428 int ny=image->get_ysize();
05429 int nz=image->get_zsize();
05430
05431 if(mask != NULL){
05432 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05433 throw ImageDimensionException("The size of mask image should be of same size as the input image");
05434 maskptr =mask->get_data();
05435 }
05436 if( nbins == 0) nbins = nx;
05437 vector <float> freq(2*nbins, 0.0);
05438
05439 imageptr=image->get_data();
05440 if( hmin == hmax ) {
05441 if(mask == NULL) {
05442 hmax = image->get_attr("maximum");
05443 hmin = image->get_attr("minimum");
05444 } else {
05445 bool First = true;
05446 for (int i = 0;i < nx*ny*nz; i++) {
05447 if (maskptr[i]>=0.5f) {
05448 if(First) {
05449 hmax = imageptr[i];
05450 hmin = imageptr[i];
05451 First = false;
05452 } else {
05453 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05454 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05455 }
05456 }
05457 }
05458 }
05459 }
05460 float hdiff = hmax - hmin;
05461 float ff = (nbins-1)/hdiff;
05462 for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05463 if(mask == NULL) {
05464 for(int i = 0; i < nx*ny*nz; i++) {
05465 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05466 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05467 }
05468 } else {
05469 for(int i = 0; i < nx*ny*nz; i++) {
05470 if(maskptr[i] >= 0.5) {
05471 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05472 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05473 }
05474 }
05475 }
05476 return freq;
05477 }
05478
05479 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05480 {
05481
05482 if (img->is_complex() || ref->is_complex())
05483 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05484
05485 if(mask != NULL){
05486 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05487 throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05488
05489
05490
05491 int size_ref = ((ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05492 int size_img = ((img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05493
05494
05495
05496 float *ref_ptr = ref->get_data();
05497 float ref_h_min = ref->get_attr("minimum");
05498 float ref_h_max = ref->get_attr("maximum");
05499 float ref_h_avg = ref->get_attr("mean");
05500 float ref_h_sig = ref->get_attr("sigma");
05501
05502
05503
05504 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05505
05506 vector<float> img_data = Util::infomask(img, mask);
05507 float img_avg = img_data[0];
05508 float img_sig = img_data[1];
05509
05510
05511 int cnt=0;
05512 for(int i=0;i<size_img;i++)
05513 if (mask_ptr[i]>0.5f)
05514 cnt++;
05515
05516
05517
05518 float ref_h_diff = ref_h_max - ref_h_min;
05519
05520 #ifdef _WIN32
05521 int hist_len = _MIN((int)size_ref/16,_MIN((int)size_img/16,256));
05522 #else
05523 int hist_len = std::min((int)size_ref/16,std::min((int)size_img/16,256));
05524 #endif //_WIN32
05525
05526 float *ref_freq_bin = new float[3*hist_len];
05527
05528
05529 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05530
05531 for (int i = 0;i < size_ref;i++) {
05532 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05533 ref_freq_bin[L]++;
05534 }
05535 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05536
05537
05538 float A = ref_h_sig/img_sig;
05539 float B = ref_h_avg - (A*img_avg);
05540
05541 vector<float> args;
05542 args.push_back(A);
05543 args.push_back(B);
05544
05545 vector<float> scale;
05546 scale.push_back(1.e-7f*A);
05547 scale.push_back(-1.e-7f*B);
05548
05549 vector<float> ref_freq_hist;
05550 for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05551
05552 vector<float> data;
05553 data.push_back(ref_h_diff);
05554 data.push_back(ref_h_min);
05555
05556 Dict parameter;
05557
05558
05559 parameter["args"] = args;
05560 parameter["scale"]= scale;
05561 parameter["data"] = data;
05562 parameter["ref_freq_bin"] = ref_freq_hist;
05563 parameter["size_img"]=size_img;
05564 parameter["hist_len"]=hist_len;
05565
05566
05567 return parameter;
05568 }
05569
05570
05571 float Util::hist_comp_freq(float PA,float PB,int size_img, int hist_len, EMData *img, vector<float> ref_freq_hist, EMData *mask, float ref_h_diff, float ref_h_min)
05572 {
05573 float *img_ptr = img->get_data();
05574 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05575
05576 int *img_freq_bin = new int[3*hist_len];
05577 for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05578 for(int i = 0;i < size_img;i++) {
05579 if(mask_ptr[i] > 0.5f) {
05580 float img_xn = img_ptr[i]*PA + PB;
05581 int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05582 if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05583 }
05584 }
05585 int freq_hist = 0;
05586
05587 for(int i = 0;i < (3*hist_len);i++) freq_hist += (int)pow((float)((int)ref_freq_hist[i] - (int)img_freq_bin[i]),2.f);
05588 freq_hist = (-freq_hist);
05589 return static_cast<float>(freq_hist);
05590 }
05591
05592 #define QUADPI 3.141592653589793238462643383279502884197
05593 #define DGR_TO_RAD QUADPI/180
05594 #define DM(I) DM [I-1]
05595 #define SS(I) SS [I-1]
05596 Dict Util::CANG(float PHI,float THETA,float PSI)
05597 {
05598 double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05599 vector<float> DM,SS;
05600
05601 for(int i =0;i<9;i++) DM.push_back(0);
05602
05603 for(int i =0;i<6;i++) SS.push_back(0);
05604
05605 CPHI = cos(double(PHI)*DGR_TO_RAD);
05606 SPHI = sin(double(PHI)*DGR_TO_RAD);
05607 CTHE = cos(double(THETA)*DGR_TO_RAD);
05608 STHE = sin(double(THETA)*DGR_TO_RAD);
05609 CPSI = cos(double(PSI)*DGR_TO_RAD);
05610 SPSI = sin(double(PSI)*DGR_TO_RAD);
05611
05612 SS(1) = float(CPHI);
05613 SS(2) = float(SPHI);
05614 SS(3) = float(CTHE);
05615 SS(4) = float(STHE);
05616 SS(5) = float(CPSI);
05617 SS(6) = float(SPSI);
05618
05619 DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05620 DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05621 DM(3) = float(-STHE*CPSI);
05622 DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05623 DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05624 DM(6) = float(STHE*SPSI);
05625 DM(7) = float(STHE*CPHI);
05626 DM(8) = float(STHE*SPHI);
05627 DM(9) = float(CTHE);
05628
05629 Dict DMnSS;
05630 DMnSS["DM"] = DM;
05631 DMnSS["SS"] = SS;
05632
05633 return(DMnSS);
05634 }
05635 #undef SS
05636 #undef DM
05637 #undef QUADPI
05638 #undef DGR_TO_RAD
05639
05640 #define DM(I) DM[I-1]
05641 #define B(i,j) Bptr[i-1+((j-1)*NSAM)]
05642 #define CUBE(i,j,k) CUBEptr[(i-1)+((j-1)+((k-1)*NY3D))*NX3D]
05643
05644 void Util::BPCQ(EMData *B,EMData *CUBE, vector<float> DM)
05645 {
05646
05647 float *Bptr = B->get_data();
05648 float *CUBEptr = CUBE->get_data();
05649
05650 int NSAM,NROW,NX3D,NY3D,NZC,KZ,IQX,IQY,LDPX,LDPY,LDPZ,LDPNMX,LDPNMY,NZ1;
05651 float DIPX,DIPY,XB,YB,XBB,YBB;
05652
05653 Transform * t = B->get_attr("xform.projection");
05654 Dict d = t->get_params("spider");
05655 if(t) {delete t; t=0;}
05656
05657 float x_shift = d[ "tx" ];
05658 float y_shift = d[ "ty" ];
05659 x_shift = -x_shift;
05660 y_shift = -y_shift;
05661
05662 NSAM = B->get_xsize();
05663 NROW = B->get_ysize();
05664 NX3D = CUBE->get_xsize();
05665 NY3D = CUBE->get_ysize();
05666 NZC = CUBE->get_zsize();
05667
05668
05669 LDPX = NX3D/2 +1;
05670 LDPY = NY3D/2 +1;
05671 LDPZ = NZC/2 +1;
05672 LDPNMX = NSAM/2 +1;
05673 LDPNMY = NROW/2 +1;
05674 NZ1 = 1;
05675
05676 for(int K=1;K<=NZC;K++) {
05677 KZ=K-1+NZ1;
05678 for(int J=1;J<=NY3D;J++) {
05679 XBB = (1-LDPX)*DM(1)+(J-LDPY)*DM(2)+(KZ-LDPZ)*DM(3);
05680 YBB = (1-LDPX)*DM(4)+(J-LDPY)*DM(5)+(KZ-LDPZ)*DM(6);
05681 for(int I=1;I<=NX3D;I++) {
05682 XB = (I-1)*DM(1)+XBB-x_shift;
05683 IQX = int(XB+float(LDPNMX));
05684 if (IQX <1 || IQX >= NSAM) continue;
05685 YB = (I-1)*DM(4)+YBB-y_shift;
05686 IQY = int(YB+float(LDPNMY));
05687 if (IQY<1 || IQY>=NROW) continue;
05688 DIPX = XB+LDPNMX-IQX;
05689 DIPY = YB+LDPNMY-IQY;
05690
05691 CUBE(I,J,K) = CUBE(I,J,K)+B(IQX,IQY)+DIPY*(B(IQX,IQY+1)-B(IQX,IQY))+DIPX*(B(IQX+1,IQY)-B(IQX,IQY)+DIPY*(B(IQX+1,IQY+1)-B(IQX+1,IQY)-B(IQX,IQY+1)+B(IQX,IQY)));
05692 }
05693 }
05694 }
05695 }
05696
05697 #undef DM
05698 #undef B
05699 #undef CUBE
05700
05701
05702 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
05703 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
05704 #define SS(I,J) SS [I-1 + (J-1)*6]
05705
05706 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K,vector<float> exptable)
05707 {
05708 int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05709 float WW,OX,OY,Y;
05710
05711 NSAM = PROJ->get_xsize();
05712 NROW = PROJ->get_ysize();
05713 NNNN = NSAM+2-(NSAM%2);
05714 NR2 = NROW/2;
05715
05716 NANG = int(SS.size())/6;
05717
05718 EMData* W = new EMData();
05719 int Wnx = NNNN/2;
05720 W->set_size(Wnx,NROW,1);
05721 W->to_zero();
05722 float *Wptr = W->get_data();
05723 float *PROJptr = PROJ->get_data();
05724 float indcnst = 1000/2.0;
05725
05726
05727 for (L=1; L<=NANG; L++) {
05728 OX = SS(6,K)*SS(4,L)*(-SS(1,L)*SS(2,K)+ SS(1,K)*SS(2,L)) + SS(5,K)*(-SS(3,L)*SS(4,K)+SS(3,K)*SS(4,L)*(SS(1,K)*SS(1,L) + SS(2,K)*SS(2,L)));
05729 OY = SS(5,K)*SS(4,L)*(-SS(1,L)*SS(2,K)+ SS(1,K)*SS(2,L)) - SS(6,K)*(-SS(3,L)*SS(4,K)+SS(3,K)*SS(4,L)*(SS(1,K)*SS(1,L) + SS(2,K)*SS(2,L)));
05730
05731 if(OX != 0.0f || OY!=0.0f) {
05732
05733 for(int J=1;J<=NROW;J++) {
05734 JY = (J-1);
05735 if(JY > NR2) JY=JY-NROW;
05736 for(int I=1;I<=NNNN/2;I++) {
05737 Y = fabs(OX * (I-1) + OY * JY);
05738 if(Y < 2.0f) W(I,J) += exptable[int(Y*indcnst)];
05739
05740 }
05741 }
05742 } else {
05743 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J) += 1.0f;
05744 }
05745 }
05746
05747 EMData* proj_in = PROJ;
05748
05749 PROJ = PROJ->norm_pad( false, 2);
05750 PROJ->do_fft_inplace();
05751 PROJ->update();
05752 PROJptr = PROJ->get_data();
05753
05754 float WNRMinv,temp;
05755 float osnr = 1.0f/SNR;
05756 WNRMinv = 1/W(1,1);
05757 for(int J=1;J<=NROW;J++)
05758 for(int I=1;I<=NNNN;I+=2) {
05759 KX = (I+1)/2;
05760 temp = W(KX,J)*WNRMinv;
05761 WW = temp/(temp*temp + osnr);
05762 PROJ(I,J) *= WW;
05763 PROJ(I+1,J) *= WW;
05764 }
05765 delete W; W = 0;
05766 PROJ->do_ift_inplace();
05767 PROJ->depad();
05768
05769 float* data_src = PROJ->get_data();
05770 float* data_dst = proj_in->get_data();
05771
05772 int ntotal = NSAM*NROW;
05773 for( int i=0; i < ntotal; ++i )
05774 {
05775 data_dst[i] = data_src[i];
05776 }
05777
05778 proj_in->update();
05779
05780 delete PROJ;
05781 }
05782
05783 #undef PROJ
05784 #undef W
05785 #undef SS
05786
05787 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
05788 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
05789 #define SS(I,J) SS [I-1 + (J-1)*6]
05790 #define RI(i,j) RI [(i-1) + ((j-1)*3)]
05791 #define CC(i) CC [i-1]
05792 #define CP(i) CP [i-1]
05793 #define VP(i) VP [i-1]
05794 #define VV(i) VV [i-1]
05795 #define AMAX1(i,j) i>j?i:j
05796 #define AMIN1(i,j) i<j?i:j
05797
05798 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
05799 {
05800 float rad2deg =(180.0f/3.1415926f);
05801 float deg2rad = (3.1415926f/180.0f);
05802
05803 int NSAM,NROW,NNNN,NR2,NANG,L,JY;
05804
05805 NSAM = PROJ->get_xsize();
05806 NROW = PROJ->get_ysize();
05807 NNNN = NSAM+2-(NSAM%2);
05808 NR2 = NROW/2;
05809 NANG = int(SS.size())/6;
05810
05811 float RI[9];
05812 RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
05813 RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
05814 RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
05815 RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
05816 RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
05817 RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
05818 RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
05819 RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
05820 RI(3,3)=SS(3,NUMP);
05821
05822 float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
05823
05824 EMData* W = new EMData();
05825 int Wnx = NNNN/2;
05826 W->set_size(NNNN/2,NROW,1);
05827 W->to_one();
05828 float *Wptr = W->get_data();
05829
05830 float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
05831
05832 for (L=1; L<=NANG; L++) {
05833 if (L != NUMP) {
05834 CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
05835 CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
05836 CC(3)=SS(1,L)*SS(4,L)*SS(2,NUMP)*SS(4,NUMP)-SS(2,L)*SS(4,L)*SS(1,NUMP)*SS(4,NUMP);
05837
05838 TMP = sqrt(CC(1)*CC(1) + CC(2)*CC(2) + CC(3)*CC(3));
05839 CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
05840 ALPHA=rad2deg*float(asin(CCN));
05841 if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
05842 if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
05843 if(ALPHA<1.0E-6) {
05844 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
05845 } else {
05846 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
05847 CC(1) = CC(1)/CCN;CC(2) = CC(2)/CCN;CC(3) = CC(3)/CCN;
05848 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
05849 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
05850 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
05851 CP(1) = 0.0;CP(2) = 0.0;
05852 VP(1) = 0.0;VP(2) = 0.0;
05853
05854 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
05855 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
05856 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
05857 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
05858
05859 TMP = CP(1)*VP(2)-CP(2)*VP(1);
05860
05861
05862 TMP = AMAX1(1.0E-4f,fabs(TMP));
05863 float tmpinv = 1.0f/TMP;
05864 for(int J=1;J<=NROW;J++) {
05865 JY = (J-1);
05866 if (JY>NR2) JY=JY-NROW;
05867 for(int I=1;I<=NNNN/2;I++) {
05868 FV = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
05869 RT = 1.0f-FV/FM;
05870 W(I,J) += ((RT>0.0f)*RT);
05871 }
05872 }
05873 }
05874 }
05875 }
05876
05877 EMData* proj_in = PROJ;
05878
05879 PROJ = PROJ->norm_pad( false, 2);
05880 PROJ->do_fft_inplace();
05881 PROJ->update();
05882 float *PROJptr = PROJ->get_data();
05883
05884 int KX;
05885 float WW;
05886 for(int J=1; J<=NROW; J++)
05887 for(int I=1; I<=NNNN; I+=2) {
05888 KX = (I+1)/2;
05889 WW = 1.0f/W(KX,J);
05890 PROJ(I,J) = PROJ(I,J)*WW;
05891 PROJ(I+1,J) = PROJ(I+1,J)*WW;
05892 }
05893 delete W; W = 0;
05894 PROJ->do_ift_inplace();
05895 PROJ->depad();
05896
05897 float* data_src = PROJ->get_data();
05898 float* data_dst = proj_in->get_data();
05899
05900 int ntotal = NSAM*NROW;
05901 for( int i=0; i < ntotal; ++i )
05902 {
05903 data_dst[i] = data_src[i];
05904 }
05905
05906 proj_in->update();
05907 delete PROJ;
05908 }
05909
05910 #undef AMAX1
05911 #undef AMIN1
05912 #undef RI
05913 #undef CC
05914 #undef CP
05915 #undef VV
05916 #undef VP
05917
05918
05919 #undef W
05920 #undef SS
05921 #undef PROJ
05922
05923
05924 Dict Util::ExpMinus4YSqr(float ymax,int nsamples)
05925 {
05926
05927 vector<float> expvect;
05928
05929 double inc = double(ymax)/nsamples;
05930 double temp;
05931 for(int i =0;i<nsamples;i++) {
05932 temp = exp((-4*(i*inc)*(i*inc)));
05933 expvect.push_back(float(temp));
05934 }
05935 expvect.push_back(0.0);
05936 Dict lookupdict;
05937 lookupdict["table"] = expvect;
05938 lookupdict["ymax"] = ymax;
05939 lookupdict["nsamples"] = nsamples;
05940
05941 return lookupdict;
05942 }
05943
05944
05945
05946 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
05947 {
05948 float cst = cs*1.0e7f;
05949
05950 wgh /= 100.0;
05951 float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
05952 float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
05953 float ak2 = ak*ak;
05954 float g1 = dzz*1.0e4f*lambda*ak2;
05955 float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
05956
05957 float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
05958 if(b_factor != 0.0f) ctfv *= exp(-b_factor*ak2/4.0f);
05959
05960 return ctfv;
05961 }
05962
05963 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
05964 {
05965
05966
05967
05968 int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();
05969
05970
05971
05972 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05973 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
05974
05975 int i, size = nx*ny*nz;
05976
05977 float* img_ptr = image->get_data();
05978 float* mask_ptr = mask->get_data();
05979
05980 int ln=0;
05981 for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
05982
05983 EMData* new_image = new EMData();
05984 new_image->set_size(ln,1,1);
05985 float *new_ptr = new_image->get_data();
05986
05987 ln=-1;
05988 for(i = 0;i < size;i++){
05989 if(mask_ptr[i] > 0.5f) {
05990 ln++;
05991 new_ptr[ln]=img_ptr[i];
05992 }
05993 }
05994
05995 return new_image;
05996 }
05997
05998 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
05999 {
06000
06001
06002
06003 if(mask == NULL)
06004 throw ImageDimensionException("The mask cannot be an null image");
06005
06006
06007
06008
06009 int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06010
06011 int i,size = nx*ny*nz;
06012
06013 EMData *new_image = new EMData();
06014 new_image->set_size(nx,ny,nz);
06015 float *new_ptr = new_image->get_data();
06016 float *mask_ptr = mask->get_data();
06017 float *img_ptr = image->get_data();
06018 int count = 0;
06019 float sum_under_mask = 0.0 ;
06020 for(i = 0;i < size;i++){
06021 if(mask_ptr[i] > 0.5f){
06022 new_ptr[i] = img_ptr[count];
06023 sum_under_mask += img_ptr[count];
06024 count++;
06025 if( count > image->get_xsize() ) {
06026 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06027 }
06028 }
06029 }
06030
06031 if( count > image->get_xsize() ) {
06032 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06033 }
06034
06035 float avg_under_mask = sum_under_mask / count;
06036 for(i = 0;i < size;i++) {
06037 if(mask_ptr[i] <= 0.5f) new_ptr[i] = avg_under_mask;
06038 }
06039 new_image->update();
06040 return new_image;
06041 }
06042
06043
06044
06045 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06046 {
06047 vector<float>new_peak;
06048 int n1=peak1.size()/3;
06049 float p_size2=p_size*p_size;
06050 for (int i=0;i<n1;++i) {
06051 vector<float>::iterator it2= peak1.begin()+3*i;
06052 bool push_back1=true;
06053 int n2=peak2.size()/3;
06054
06055
06056 if(n2 ==0) {
06057 new_peak.push_back(*it2);
06058 new_peak.push_back(*(it2+1));
06059 new_peak.push_back(*(it2+2));
06060 } else {
06061 int j=0;
06062 while (j< n2-1 ) {
06063 vector<float>::iterator it3= peak2.begin()+3*j;
06064 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06065 if(d2< p_size2 ) {
06066 if( (*it2)<(*it3) ) {
06067 new_peak.push_back(*it3);
06068 new_peak.push_back(*(it3+1));
06069 new_peak.push_back(*(it3+2));
06070 peak2.erase(it3);
06071 peak2.erase(it3);
06072 peak2.erase(it3);
06073 push_back1=false;
06074 } else {
06075 peak2.erase(it3);
06076 peak2.erase(it3);
06077 peak2.erase(it3);
06078 }
06079 } else j=j+1;
06080 n2=peak2.size()/3;
06081 }
06082 if(push_back1) {
06083 new_peak.push_back(*it2);
06084 new_peak.push_back(*(it2+1));
06085 new_peak.push_back(*(it2+2));
06086 }
06087 }
06088 }
06089 return new_peak;
06090 }
06091
06092 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06093 {
06094
06095
06096
06097
06098
06099 ENTERFUNC;
06100
06101 int i;
06102
06103
06104 for ( i = 0 ; i < n * n ; i++ ) eigvec[i] = covmat[i];
06105
06106 char NEEDV = 'V';
06107 char UPLO = 'U';
06108 int lwork = -1;
06109 int info = 0;
06110 float *work, wsize;
06111
06112
06113 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06114 lwork = (int)wsize;
06115
06116 work = (float *)calloc(lwork, sizeof(float));
06117
06118 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06119 free(work);
06120 EXITFUNC;
06121 return info;
06122 }
06123
06124 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06125 {
06126
06127 ENTERFUNC;
06128 int len = covmatpy.size();
06129 float *eigvec;
06130 float *eigval;
06131 float *covmat;
06132 int status = 0;
06133 eigval = (float*)calloc(ncov,sizeof(float));
06134 eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06135 covmat = (float*)calloc(ncov*ncov, sizeof(float));
06136
06137 const float *covmat_ptr;
06138 covmat_ptr = &covmatpy[0];
06139 for(int i=0;i<len;i++){
06140 covmat[i] = covmat_ptr[i];
06141 }
06142
06143 status = Util::coveig(ncov, covmat, eigval, eigvec);
06144
06145 vector<float> eigval_py(ncov);
06146 const float *eigval_ptr;
06147 eigval_ptr = &eigval[0];
06148 for(int i=0;i<ncov;i++){
06149 eigval_py[i] = eigval_ptr[i];
06150 }
06151
06152 vector<float> eigvec_py(ncov*ncov);
06153 const float *eigvec_ptr;
06154 eigvec_ptr = &eigvec[0];
06155 for(int i=0;i<ncov*ncov;i++){
06156 eigvec_py[i] = eigvec_ptr[i];
06157 }
06158
06159 Dict res;
06160 res["eigval"] = eigval_py;
06161 res["eigvec"] = eigvec_py;
06162
06163 EXITFUNC;
06164 return res;
06165 }
06166
06167 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06168 {
06169 int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06170
06171 k=(int)pw.size();
06172 l=0;
06173 m=k;
06174 n2=n+2;
06175 n1=n+1;
06176 klmd=k+l+m;
06177 klm2d= k+l+m+2;
06178 nklmd=k+l+m+n;
06179 n2d=n+2;
06180
06181 n_larg=klmd*2;
06182 klm2d=n_larg+klm2d;
06183 klmd=n_larg+klmd;
06184 nklmd=n_larg+nklmd;
06185 int size_q=klm2d*n2d;
06186 int size_cu=nklmd*2;
06187 static int i__;
06188
06189 double *q ;
06190 double *x ;
06191 double *res;
06192 double *cu;
06193 float *q2;
06194 float *pw_;
06195 long int *iu;
06196 double *s;
06197 q = (double*)calloc(size_q,sizeof(double));
06198 x = (double*)calloc(n2d,sizeof(double));
06199 res = (double*)calloc(klmd,sizeof(double));
06200 cu =(double*)calloc(size_cu,sizeof(double));
06201 s = (double*)calloc(klmd,sizeof(double));
06202 q2 = (float*)calloc(size_q,sizeof(float));
06203 iu = (long int*)calloc(size_cu,sizeof(long int));
06204 pw_ = (float*)calloc(k,sizeof(float));
06205
06206 for( i__ =0;i__<k;++i__)
06207 {
06208 pw_[i__]=log(pw[i__]); }
06209 long int l_k=k;
06210 long int l_n=n;
06211 long int l_iswi=iswi;
06212 vector<float> cl1_res;
06213 cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06214 free(q);
06215 free(x);
06216 free(res);
06217 free(s);
06218 free(cu);
06219 free(q2);
06220 free(iu);
06221 free(pw_);
06222 return cl1_res;
06223 }
06224 vector<float> Util::call_cl1(long int *k, long int *n, float *ps, long int *iswi, float *pw, float *q2,double *q, double *x, double *res, double *cu, double *s, long int *iu)
06225 {
06226 long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06227 float r__1;
06228 int tmp__i;
06229 long int i__, j;
06230 --s;
06231 --res;
06232 iu -= 3;
06233 cu -= 3;
06234 --x;
06235 long int klm2d;
06236 klm2d= *k+*k+2;
06237 klm2d=klm2d+klm2d;
06238 q_dim1 = klm2d;
06239 q_offset = 1 + q_dim1;
06240 q -= q_offset;
06241 q2_dim1 = klm2d;
06242 q2_offset = 1 + q2_dim1;
06243 q2 -= q2_offset;
06244 i__2=0;
06245 i__1 = *n - 1;
06246 tmp__i=0;
06247 for (j = 1; j <= i__1; ++j) {
06248 i__2 = *k;
06249 tmp__i+=1;
06250 for (i__ = 1; i__ <= i__2; ++i__) {
06251 r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06252 q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06253 }
06254 }
06255 for (i__ = 1; i__ <= i__2; ++i__)
06256 { q2[i__ + *n * q2_dim1] = 1.f;
06257 q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06258 }
06259 vector<float> fit_res;
06260 fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06261 return fit_res;
06262 }
06263 vector<float> Util::lsfit(long int *ks,long int *n, long int *klm2d, long int *iswi,float *q1,double *q, double *x, double *res, double *cu, double *s, long int *iu)
06264 {
06265
06266 long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06267
06268
06269 long int i__, j, m, n1, ii, jj;
06270 double tmp;
06271 vector<float> p;
06272 --x;
06273 q_dim1 = *klm2d;
06274 q_offset = 1 + q_dim1;
06275 q -= q_offset;
06276 q1_dim1 = *klm2d;
06277 q1_offset = 1 + q1_dim1;
06278 q1 -= q1_offset;
06279 --s;
06280 --res;
06281 iu -= 3;
06282 cu -= 3;
06283
06284
06285 long int l = 0;
06286
06287
06288 m = *ks;
06289 n1 = *n + 1;
06290 if (*iswi == 1) {
06291 i__1 = n1;
06292 for (jj = 1; jj <= i__1; ++jj) {
06293 i__2 = *ks;
06294 for (ii = 1; ii <= i__2; ++ii) {
06295
06296
06297 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06298 ;
06299 }
06300 }
06301 } else if (*iswi == 2) {
06302 i__1 = *ks;
06303 for (ii = 1; ii <= i__1; ++ii) {
06304 i__2 = n1;
06305 for (jj = 1; jj <= i__2; ++jj) {
06306 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06307 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06308 }
06309 }
06310 } else if (*iswi == 3) {
06311 l = 2;
06312 i__1 = n1;
06313 for (jj = 1; jj <= i__1; ++jj) {
06314 i__2 = *ks + 2;
06315 for (ii = 1; ii <= i__2; ++ii) {
06316 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06317 }
06318 i__2 = *ks;
06319 for (ii = 1; ii <= i__2; ++ii) {
06320 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06321 }
06322 }
06323 } else if (*iswi == 4) {
06324 l = 2;
06325 i__1 = n1;
06326 for (jj = 1; jj <= i__1; ++jj) {
06327 i__2 = *ks + 2;
06328 for (ii = 1; ii <= i__2; ++ii) {
06329 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06330 }
06331 i__2 = *ks;
06332 for (ii = 1; ii <= i__2; ++ii) {
06333 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06334 }
06335 }
06336 } else if (*iswi == 5) {
06337 l = 1;
06338 i__1 = n1;
06339 for (jj = 1; jj <= i__1; ++jj) {
06340 i__2 = *ks + 1;
06341 for (ii = 1; ii <= i__2; ++ii) {
06342 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06343 }
06344 i__2 = *ks;
06345 for (ii = 1; ii <= i__2; ++ii) {
06346 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06347 }
06348 }
06349 } else if (*iswi == 6) {
06350 l = 1;
06351 i__1 = n1;
06352 for (jj = 1; jj <= i__1; ++jj) {
06353 i__2 = *ks + 1;
06354 for (ii = 1; ii <= i__2; ++ii) {
06355 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06356 }
06357 i__2 = *ks;
06358 for (ii = 1; ii <= i__2; ++ii) {
06359 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06360 }
06361 }
06362 } else if (*iswi == 7) {
06363 l = 3;
06364 i__1 = n1;
06365 for (jj = 1; jj <= i__1; ++jj) {
06366 i__2 = *ks + 3;
06367 for (ii = 1; ii <= i__2; ++ii) {
06368 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06369 }
06370 i__2 = *ks;
06371 for (ii = 1; ii <= i__2; ++ii) {
06372 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06373 }
06374 }
06375 } else if (*iswi == 8) {
06376 l = 4;
06377 i__1 = n1;
06378 for (jj = 1; jj <= i__1; ++jj) {
06379 i__2 = *ks + 4;
06380 for (ii = 1; ii <= i__2; ++ii) {
06381 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06382 }
06383 i__2 = *ks;
06384 for (ii = 1; ii <= i__2; ++ii) {
06385 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06386 }
06387 }
06388 }
06389
06390 Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06391 i__1 = *ks;
06392 int tmp__j=0;
06393 for (i__ = 1; i__ <= i__1; ++i__) {
06394 tmp = 0.f;
06395 i__2 = *n - 1;
06396 for (j = 1; j <= i__2; ++j) {
06397 tmp__j=j;
06398 tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06399 }
06400 tmp += x[*n];
06401 p.push_back(static_cast<float>(exp(tmp)));
06402 p.push_back(q1[i__ + q1_dim1]);
06403 }
06404 i__2=*n;
06405 for (i__=1;i__<=i__2;++i__)
06406 { p.push_back(static_cast<float>(x[i__]));}
06407 return p;
06408 }
06409 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06410 double *q, double *x, double *res, double *cu, long int *iu, double *s)
06411 {
06412
06413 long int q_dim1, q_offset, i__1, i__2;
06414 double d__1;
06415
06416 static long int i__, j;
06417 static double z__;
06418 static long int n1, n2, ia, ii, kk, in, nk, js;
06419 static double sn, zu, zv;
06420 static long int nk1, klm, nkl, jmn, jpn;
06421 static double cuv;
06422 static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06423 static float xmin;
06424 static double xmax;
06425 static long int iout;
06426 static double xsum;
06427 static long int iineg, maxit;
06428 static double toler;
06429 static float error;
06430 static double pivot;
06431 static long int kforce, iphase;
06432 static double tpivot;
06433
06434 --s;
06435 --res;
06436 iu -= 3;
06437 cu -= 3;
06438 --x;
06439 q_dim1 = *klm2d;
06440 q_offset = 1 + q_dim1;
06441 q -= q_offset;
06442
06443
06444 maxit = 500;
06445 kode = 0;
06446 toler = 1e-4f;
06447 iter = 0;
06448 n1 = *n + 1;
06449 n2 = *n + 2;
06450 nk = *n + *k;
06451 nk1 = nk + 1;
06452 nkl = nk + *l;
06453 nkl1 = nkl + 1;
06454 klm = *k + *l + *m;
06455 klm1 = klm + 1;
06456 klm2 = klm + 2;
06457 nklm = *n + klm;
06458 kforce = 1;
06459 iter = 0;
06460 js = 1;
06461 ia = 0;
06462
06463 i__1 = *n;
06464 for (j = 1; j <= i__1; ++j) {
06465 q[klm2 + j * q_dim1] = (double) j;
06466
06467 }
06468 i__1 = klm;
06469 for (i__ = 1; i__ <= i__1; ++i__) {
06470 q[i__ + n2 * q_dim1] = (double) (*n + i__);
06471 if (q[i__ + n1 * q_dim1] >= 0.f) {
06472 goto L30;
06473 }
06474 i__2 = n2;
06475 for (j = 1; j <= i__2; ++j) {
06476 q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06477
06478 }
06479 L30:
06480 ;
06481 }
06482
06483 iphase = 2;
06484 i__1 = nklm;
06485 for (j = 1; j <= i__1; ++j) {
06486 cu[(j << 1) + 1] = 0.f;
06487 cu[(j << 1) + 2] = 0.f;
06488 iu[(j << 1) + 1] = 0;
06489 iu[(j << 1) + 2] = 0;
06490
06491 }
06492 if (*l == 0) {
06493 goto L60;
06494 }
06495 i__1 = nkl;
06496 for (j = nk1; j <= i__1; ++j) {
06497 cu[(j << 1) + 1] = 1.f;
06498 cu[(j << 1) + 2] = 1.f;
06499 iu[(j << 1) + 1] = 1;
06500 iu[(j << 1) + 2] = 1;
06501
06502 }
06503 iphase = 1;
06504 L60:
06505 if (*m == 0) {
06506 goto L80;
06507 }
06508 i__1 = nklm;
06509 for (j = nkl1; j <= i__1; ++j) {
06510 cu[(j << 1) + 2] = 1.f;
06511 iu[(j << 1) + 2] = 1;
06512 jmn = j - *n;
06513 if (q[jmn + n2 * q_dim1] < 0.f) {
06514 iphase = 1;
06515 }
06516
06517 }
06518 L80:
06519 if (kode == 0) {
06520 goto L150;
06521 }
06522 i__1 = *n;
06523 for (j = 1; j <= i__1; ++j) {
06524 if ((d__1 = x[j]) < 0.) {
06525 goto L90;
06526 } else if (d__1 == 0) {
06527 goto L110;
06528 } else {
06529 goto L100;
06530 }
06531 L90:
06532 cu[(j << 1) + 1] = 1.f;
06533 iu[(j << 1) + 1] = 1;
06534 goto L110;
06535 L100:
06536 cu[(j << 1) + 2] = 1.f;
06537 iu[(j << 1) + 2] = 1;
06538 L110:
06539 ;
06540 }
06541 i__1 = *k;
06542 for (j = 1; j <= i__1; ++j) {
06543 jpn = j + *n;
06544 if ((d__1 = res[j]) < 0.) {
06545 goto L120;
06546 } else if (d__1 == 0) {
06547 goto L140;
06548 } else {
06549 goto L130;
06550 }
06551 L120:
06552 cu[(jpn << 1) + 1] = 1.f;
06553 iu[(jpn << 1) + 1] = 1;
06554 if (q[j + n2 * q_dim1] > 0.f) {
06555 iphase = 1;
06556 }
06557 goto L140;
06558 L130:
06559 cu[(jpn << 1) + 2] = 1.f;
06560 iu[(jpn << 1) + 2] = 1;
06561 if (q[j + n2 * q_dim1] < 0.f) {
06562 iphase = 1;
06563 }
06564 L140:
06565 ;
06566 }
06567 L150:
06568 if (iphase == 2) {
06569 goto L500;
06570 }
06571
06572 L160:
06573 i__1 = n1;
06574 for (j = js; j <= i__1; ++j) {
06575 xsum = 0.;
06576 i__2 = klm;
06577 for (i__ = 1; i__ <= i__2; ++i__) {
06578 ii = (long int) q[i__ + n2 * q_dim1];
06579 if (ii < 0) {
06580 goto L170;
06581 }
06582 z__ = cu[(ii << 1) + 1];
06583 goto L180;
06584 L170:
06585 iineg = -ii;
06586 z__ = cu[(iineg << 1) + 2];
06587 L180:
06588 xsum += q[i__ + j * q_dim1] * z__;
06589
06590
06591 }
06592 q[klm1 + j * q_dim1] = xsum;
06593
06594 }
06595 i__1 = *n;
06596 for (j = js; j <= i__1; ++j) {
06597 ii = (long int) q[klm2 + j * q_dim1];
06598 if (ii < 0) {
06599 goto L210;
06600 }
06601 z__ = cu[(ii << 1) + 1];
06602 goto L220;
06603 L210:
06604 iineg = -ii;
06605 z__ = cu[(iineg << 1) + 2];
06606 L220:
06607 q[klm1 + j * q_dim1] -= z__;
06608
06609 }
06610
06611 L240:
06612 xmax = 0.f;
06613 if (js > *n) {
06614 goto L490;
06615 }
06616 i__1 = *n;
06617 for (j = js; j <= i__1; ++j) {
06618 zu = q[klm1 + j * q_dim1];
06619 ii = (long int) q[klm2 + j * q_dim1];
06620 if (ii > 0) {
06621 goto L250;
06622 }
06623 ii = -ii;
06624 zv = zu;
06625 zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06626 goto L260;
06627 L250:
06628 zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06629 L260:
06630 if (kforce == 1 && ii > *n) {
06631 goto L280;
06632 }
06633 if (iu[(ii << 1) + 1] == 1) {
06634 goto L270;
06635 }
06636 if (zu <= xmax) {
06637 goto L270;
06638 }
06639 xmax = zu;
06640 in = j;
06641 L270:
06642 if (iu[(ii << 1) + 2] == 1) {
06643 goto L280;
06644 }
06645 if (zv <= xmax) {
06646 goto L280;
06647 }
06648 xmax = zv;
06649 in = j;
06650 L280:
06651 ;
06652 }
06653 if (xmax <= toler) {
06654 goto L490;
06655 }
06656 if (q[klm1 + in * q_dim1] == xmax) {
06657 goto L300;
06658 }
06659 i__1 = klm2;
06660 for (i__ = 1; i__ <= i__1; ++i__) {
06661 q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06662
06663 }
06664 q[klm1 + in * q_dim1] = xmax;
06665
06666 L300:
06667 if (iphase == 1 || ia == 0) {
06668 goto L330;
06669 }
06670 xmax = 0.f;
06671 i__1 = ia;
06672 for (i__ = 1; i__ <= i__1; ++i__) {
06673 z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06674 if (z__ <= xmax) {
06675 goto L310;
06676 }
06677 xmax = z__;
06678 iout = i__;
06679 L310:
06680 ;
06681 }
06682 if (xmax <= toler) {
06683 goto L330;
06684 }
06685 i__1 = n2;
06686 for (j = 1; j <= i__1; ++j) {
06687 z__ = q[ia + j * q_dim1];
06688 q[ia + j * q_dim1] = q[iout + j * q_dim1];
06689 q[iout + j * q_dim1] = z__;
06690
06691 }
06692 iout = ia;
06693 --ia;
06694 pivot = q[iout + in * q_dim1];
06695 goto L420;
06696 L330:
06697 kk = 0;
06698 i__1 = klm;
06699 for (i__ = 1; i__ <= i__1; ++i__) {
06700 z__ = q[i__ + in * q_dim1];
06701 if (z__ <= toler) {
06702 goto L340;
06703 }
06704 ++kk;
06705 res[kk] = q[i__ + n1 * q_dim1] / z__;
06706 s[kk] = (double) i__;
06707 L340:
06708 ;
06709 }
06710 L350:
06711 if (kk > 0) {
06712 goto L360;
06713 }
06714 kode = 2;
06715 goto L590;
06716 L360:
06717 xmin = static_cast<float>( res[1] );
06718 iout = (long int) s[1];
06719 j = 1;
06720 if (kk == 1) {
06721 goto L380;
06722 }
06723 i__1 = kk;
06724 for (i__ = 2; i__ <= i__1; ++i__) {
06725 if (res[i__] >= xmin) {
06726 goto L370;
06727 }
06728 j = i__;
06729 xmin = static_cast<float>( res[i__] );
06730 iout = (long int) s[i__];
06731 L370:
06732 ;
06733 }
06734 res[j] = res[kk];
06735 s[j] = s[kk];
06736 L380:
06737 --kk;
06738 pivot = q[iout + in * q_dim1];
06739 ii = (long int) q[iout + n2 * q_dim1];
06740 if (iphase == 1) {
06741 goto L400;
06742 }
06743 if (ii < 0) {
06744 goto L390;
06745 }
06746 if (iu[(ii << 1) + 2] == 1) {
06747 goto L420;
06748 }
06749 goto L400;
06750 L390:
06751 iineg = -ii;
06752 if (iu[(iineg << 1) + 1] == 1) {
06753 goto L420;
06754 }
06755
06756 L400:
06757 ii = abs(ii);
06758 cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06759 if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06760 goto L420;
06761 }
06762
06763 i__1 = n1;
06764 for (j = js; j <= i__1; ++j) {
06765 z__ = q[iout + j * q_dim1];
06766 q[klm1 + j * q_dim1] -= z__ * cuv;
06767 q[iout + j * q_dim1] = -z__;
06768
06769 }
06770 q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06771 goto L350;
06772
06773 L420:
06774 if (iter < maxit) {
06775 goto L430;
06776 }
06777 kode = 3;
06778 goto L590;
06779 L430:
06780 ++iter;
06781 i__1 = n1;
06782 for (j = js; j <= i__1; ++j) {
06783 if (j != in) {
06784 q[iout + j * q_dim1] /= pivot;
06785 }
06786
06787 }
06788
06789
06790
06791
06792
06793
06794
06795
06796 i__1 = n1;
06797 for (j = js; j <= i__1; ++j) {
06798 if (j == in) {
06799 goto L460;
06800 }
06801 z__ = -q[iout + j * q_dim1];
06802 i__2 = klm1;
06803 for (i__ = 1; i__ <= i__2; ++i__) {
06804 if (i__ != iout) {
06805 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06806 }
06807
06808 }
06809 L460:
06810 ;
06811 }
06812 tpivot = -pivot;
06813 i__1 = klm1;
06814 for (i__ = 1; i__ <= i__1; ++i__) {
06815 if (i__ != iout) {
06816 q[i__ + in * q_dim1] /= tpivot;
06817 }
06818
06819 }
06820 q[iout + in * q_dim1] = 1.f / pivot;
06821 z__ = q[iout + n2 * q_dim1];
06822 q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
06823 q[klm2 + in * q_dim1] = z__;
06824 ii = (long int) abs(z__);
06825 if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
06826 goto L240;
06827 }
06828 i__1 = klm2;
06829 for (i__ = 1; i__ <= i__1; ++i__) {
06830 z__ = q[i__ + in * q_dim1];
06831 q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
06832 q[i__ + js * q_dim1] = z__;
06833
06834 }
06835 ++js;
06836 goto L240;
06837
06838 L490:
06839 if (kforce == 0) {
06840 goto L580;
06841 }
06842 if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
06843 goto L500;
06844 }
06845 kforce = 0;
06846 goto L240;
06847
06848 L500:
06849 iphase = 2;
06850 i__1 = nklm;
06851 for (j = 1; j <= i__1; ++j) {
06852 cu[(j << 1) + 1] = 0.f;
06853 cu[(j << 1) + 2] = 0.f;
06854
06855 }
06856 i__1 = nk;
06857 for (j = n1; j <= i__1; ++j) {
06858 cu[(j << 1) + 1] = 1.f;
06859 cu[(j << 1) + 2] = 1.f;
06860
06861 }
06862 i__1 = klm;
06863 for (i__ = 1; i__ <= i__1; ++i__) {
06864 ii = (long int) q[i__ + n2 * q_dim1];
06865 if (ii > 0) {
06866 goto L530;
06867 }
06868 ii = -ii;
06869 if (iu[(ii << 1) + 2] == 0) {
06870 goto L560;
06871 }
06872 cu[(ii << 1) + 2] = 0.f;
06873 goto L540;
06874 L530:
06875 if (iu[(ii << 1) + 1] == 0) {
06876 goto L560;
06877 }
06878 cu[(ii << 1) + 1] = 0.f;
06879 L540:
06880 ++ia;
06881 i__2 = n2;
06882 for (j = 1; j <= i__2; ++j) {
06883 z__ = q[ia + j * q_dim1];
06884 q[ia + j * q_dim1] = q[i__ + j * q_dim1];
06885 q[i__ + j * q_dim1] = z__;
06886
06887 }
06888 L560:
06889 ;
06890 }
06891 goto L160;
06892 L570:
06893 if (q[klm1 + n1 * q_dim1] <= toler) {
06894 goto L500;
06895 }
06896 kode = 1;
06897 goto L590;
06898 L580:
06899 if (iphase == 1) {
06900 goto L570;
06901 }
06902
06903 kode = 0;
06904 L590:
06905 xsum = 0.;
06906 i__1 = *n;
06907 for (j = 1; j <= i__1; ++j) {
06908 x[j] = 0.f;
06909
06910 }
06911 i__1 = klm;
06912 for (i__ = 1; i__ <= i__1; ++i__) {
06913 res[i__] = 0.f;
06914
06915 }
06916 i__1 = klm;
06917 for (i__ = 1; i__ <= i__1; ++i__) {
06918 ii = (long int) q[i__ + n2 * q_dim1];
06919 sn = 1.f;
06920 if (ii > 0) {
06921 goto L620;
06922 }
06923 ii = -ii;
06924 sn = -1.f;
06925 L620:
06926 if (ii > *n) {
06927 goto L630;
06928 }
06929 x[ii] = sn * q[i__ + n1 * q_dim1];
06930 goto L640;
06931 L630:
06932 iimn = ii - *n;
06933 res[iimn] = sn * q[i__ + n1 * q_dim1];
06934 if (ii >= n1 && ii <= nk) {
06935 xsum += q[i__ + n1 * q_dim1];
06936 }
06937 L640:
06938 ;
06939 }
06940 error = (float)xsum;
06941 return;
06942 }
06943
06944 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
06945 {
06946 int j,d;
06947 EMData * e = new EMData();
06948 float *eptr, *imgptr;
06949 imgptr = img->get_data();
06950 float SSE = 0.f;
06951 for (j = 0 ; j < N ; j++) {
06952 e->read_image(images,S[j]);
06953 eptr = e->get_data();
06954 for (d = 0; d < size; d++) {
06955 SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
06956 }
06957 delete e;
06958 return SSE;
06959 }
06960
06961
06962 #define mymax(x,y) (((x)>(y))?(x):(y))
06963 #define mymin(x,y) (((x)<(y))?(x):(y))
06964 #define sign(x,y) (((((y)>0)?(1):(-1))*(y!=0))*(x))
06965
06966
06967 #define quadpi 3.141592653589793238462643383279502884197
06968 #define dgr_to_rad quadpi/180
06969 #define deg_to_rad quadpi/180
06970 #define rad_to_deg 180/quadpi
06971 #define rad_to_dgr 180/quadpi
06972 #define TRUE 1
06973 #define FALSE 0
06974
06975
06976 #define theta(i) theta [i-1]
06977 #define phi(i) phi [i-1]
06978 #define weight(i) weight [i-1]
06979 #define lband(i) lband [i-1]
06980 #define ts(i) ts [i-1]
06981 #define thetast(i) thetast [i-1]
06982 #define key(i) key [i-1]
06983
06984
06985 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
06986 {
06987
06988 ENTERFUNC;
06989
06990 if ( th.size() != ph.size() ) {
06991 LOGERR("images not same size");
06992 throw ImageFormatException( "images not same size");
06993 }
06994
06995
06996 srand(10);
06997
06998 int i,*key;
06999 int len = th.size();
07000 double *theta,*phi,*weight;
07001 theta = (double*) calloc(len,sizeof(double));
07002 phi = (double*) calloc(len,sizeof(double));
07003 weight = (double*) calloc(len,sizeof(double));
07004 key = (int*) calloc(len,sizeof(int));
07005 const float *thptr, *phptr;
07006
07007 thptr = &th[0];
07008 phptr = &ph[0];
07009 for(i=1;i<=len;i++){
07010 key(i) = i;
07011 weight(i) = 0.0;
07012 }
07013
07014 for(i = 0;i<len;i++){
07015 theta[i] = thptr[i];
07016 phi[i] = phptr[i];
07017 }
07018
07019
07020 Util::hsortd(theta, phi, key, len, 1);
07021
07022
07023 Util::voronoi(phi, theta, weight, len);
07024
07025
07026 Util::hsortd(weight, weight, key, len, 2);
07027
07028 free(theta);
07029 free(phi);
07030 free(key);
07031 vector<double> wt;
07032 double count = 0;
07033 for(i=1; i<= len; i++)
07034 {
07035 wt.push_back(weight(i));
07036 count += weight(i);
07037 }
07038
07039
07040
07041
07042
07043
07044 free(weight);
07045
07046 EXITFUNC;
07047 return wt;
07048
07049 }
07050
07051 struct tmpstruct{
07052 double theta1,phi1;
07053 int key1;
07054 };
07055
07056 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07057 {
07058 ENTERFUNC;
07059 vector<tmpstruct> tmp(len);
07060 int i;
07061 for(i = 1;i<=len;i++)
07062 {
07063 tmp[i-1].theta1 = theta(i);
07064 tmp[i-1].phi1 = phi(i);
07065 tmp[i-1].key1 = key(i);
07066 }
07067
07068 if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07069 if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07070
07071 for(i = 1;i<=len;i++)
07072 {
07073 theta(i) = tmp[i-1].theta1;
07074 phi(i) = tmp[i-1].phi1;
07075 key(i) = tmp[i-1].key1;
07076 }
07077 EXITFUNC;
07078 }
07079
07080 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07081 {
07082 return(tmp1.theta1 < tmp2.theta1);
07083 }
07084
07085 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07086 {
07087 return(tmp1.key1 < tmp2.key1);
07088 }
07089
07090
07091
07092
07093
07094
07095
07096
07097
07098
07099
07100
07101
07102
07103
07104
07105
07106
07107
07108
07109
07110
07111
07112
07113
07114
07115
07116
07117
07118
07119
07120
07121
07122
07123
07124
07125
07126
07127
07128
07129
07130
07131
07132
07133
07134
07135
07136
07137
07138
07139
07140
07141
07142
07143
07144
07145
07146
07147
07148
07149
07150
07151
07152
07153
07154
07155
07156
07157
07158
07159
07160
07161
07162
07163
07164
07165
07166
07167
07168
07169
07170
07171
07172
07173
07174
07175
07176
07177
07178
07179
07180
07181
07182
07183
07184
07185
07186
07187
07188
07189
07190
07191
07192
07193
07194
07195
07196
07197
07198
07199
07200
07201
07202
07203
07204
07205
07206
07207
07208
07209
07210
07211
07212
07213
07214
07215
07216
07217
07218
07219
07220
07221
07222
07223
07224
07225
07226
07227
07228
07229
07230
07231
07232
07233
07234
07235
07236
07237
07238
07239
07240
07241
07242
07243
07244
07245
07246
07247
07248
07249
07250
07251
07252
07253
07254
07255
07256
07257
07258
07259
07260
07261
07262
07263
07264
07265
07266
07267
07268
07269
07270
07271
07272
07273
07274
07275
07276
07277
07278
07279
07280
07281
07282
07283
07284
07285
07286
07287
07288
07289
07290
07291
07292
07293
07294
07295
07296
07297
07298
07299
07300
07301
07302
07303
07304
07305
07306
07307
07308
07309
07310
07311
07312
07313
07314
07315
07316
07317
07318
07319
07320
07321
07322
07323
07324
07325
07326
07327
07328
07329
07330
07331
07332
07333
07334
07335
07336
07337
07338
07339
07340
07341
07342
07343
07344
07345
07346
07347
07348
07349
07350
07351
07352
07353
07354
07355
07356
07357
07358
07359
07360
07361
07362
07363
07364
07365
07366
07367
07368
07369
07370
07371
07372
07373
07374
07375
07376
07377
07378
07379
07380
07381
07382
07383
07384
07385
07386
07387
07388
07389
07390
07391
07392
07393
07394
07395
07396
07397
07398
07399
07400
07401
07402
07403
07404
07405
07406
07407
07408 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07409 {
07410
07411 ENTERFUNC;
07412
07413 int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07414 int nt6, n, ier, nout, lnew, mdup, nd;
07415 int i,k,mt,status;
07416
07417
07418 double *ds, *x, *y, *z;
07419 double tol = 1.0e-8;
07420 double dtol = 15;
07421 double a;
07422
07423
07424
07425
07426
07427
07428
07429
07430
07431 n = nt + nt;
07432
07433 nt6 = n*6;
07434
07435 list = (int*)calloc(nt6,sizeof(int));
07436 lptr = (int*)calloc(nt6,sizeof(int));
07437 lend = (int*)calloc(n ,sizeof(int));
07438 iwk = (int*)calloc(n ,sizeof(int));
07439 good = (int*)calloc(n ,sizeof(int));
07440 key = (int*)calloc(n ,sizeof(int));
07441 indx = (int*)calloc(n ,sizeof(int));
07442 lcnt = (int*)calloc(n ,sizeof(int));
07443
07444 ds = (double*) calloc(n,sizeof(double));
07445 x = (double*) calloc(n,sizeof(double));
07446 y = (double*) calloc(n,sizeof(double));
07447 z = (double*) calloc(n,sizeof(double));
07448
07449 if (list == NULL ||
07450 lptr == NULL ||
07451 lend == NULL ||
07452 iwk == NULL ||
07453 good == NULL ||
07454 key == NULL ||
07455 indx == NULL ||
07456 lcnt == NULL ||
07457 x == NULL ||
07458 y == NULL ||
07459 z == NULL ||
07460 ds == NULL) {
07461 printf("memory allocation failure!\n");
07462 exit(1);
07463 }
07464
07465 bool colinear=true;
07466 while(colinear)
07467 {
07468
07469 L1:
07470 for(i = 0; i<nt; i++){
07471 x[i] = theta[i];
07472 y[i] = phi[i];
07473 x[nt+i] = 180.0 - x[i];
07474 y[nt+i] = 180.0 + y[i];
07475 }
07476
07477 Util::disorder2(x, y, key, n);
07478
07479
07480 double val;
07481 for(k=0; k<2; k++){
07482 for(i=k+1; i<3; i++){
07483 val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07484 if( val < dtol) {
07485 goto L1;
07486 }
07487 }
07488 }
07489
07490 Util::ang_to_xyz(x, y, z, n);
07491
07492
07493 bool dupnode=true;
07494 dupnode=true;
07495 while(dupnode)
07496 {
07497 for(k=0; k<2; k++){
07498 for(i=k+1; i<3; i++){
07499 if( x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07500 Util::flip23(x, y, z, key, k, n);
07501 continue;
07502 }
07503 }
07504 }
07505 dupnode = false;
07506 }
07507
07508
07509 ier = 0;
07510
07511 status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07512
07513 if (status != 0) {
07514 printf(" error in trmsh3 \n");
07515 exit(1);
07516 }
07517
07518 if (ier > 0) {
07519 printf("*** Error in TRMESH: duplicate nodes encountered ***\n");
07520 exit(1);
07521 }
07522
07523 mdup=n-nout;
07524 if (ier == -2) {
07525
07526 }
07527 else
07528 {
07529 colinear=false;
07530 }
07531 }
07532
07533
07534 Assert( ier != -2 );
07535
07536
07537 nd=0;
07538 for (k=1; k<=n; k++){
07539 if (indx[k-1]>0) {
07540 nd++;
07541 good[nd-1]=k;
07542 }
07543 }
07544
07545
07546
07547
07548 for(i = 1; i<=nout; i++) {
07549 k=good[i-1];
07550
07551 if (key[k-1] <= nt) {
07552
07553 a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07554 if (ier != 0){
07555
07556
07557 printf(" *** error in areav: ier = %d ***\n", ier);
07558 weight[key[k-1]-1] =-1.0;
07559 } else {
07560
07561 weight[key[k-1]-1]=a/lcnt[i-1];
07562 }
07563 }
07564 }
07565
07566
07567
07568 for(i = 1; i<=n; i++){
07569 mt =- indx[i-1];
07570 if (mt>0){
07571 k = good[mt-1];
07572
07573
07574
07575 if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07576 }
07577 }
07578
07579 free(list);
07580 free(lend);
07581 free(iwk);
07582 free(good);
07583 free(key);
07584 free(lptr);
07585 free(indx);
07586 free(lcnt);
07587 free(ds);
07588 free(x);
07589 free(y);
07590 free(z);
07591
07592
07593 EXITFUNC;
07594 }
07595
07596 void Util::disorder2(double *x,double *y, int *key, int len)
07597 {
07598 ENTERFUNC;
07599 int k, i;
07600 for(i=0; i<len; i++) key[i]=i+1;
07601
07602 for(i = 0; i<len;i++){
07603 k = rand()%len;
07604 std::swap(key[k], key[i]);
07605 std::swap(x[k], x[i]);
07606 std::swap(y[k], y[i]);
07607 }
07608 EXITFUNC;
07609 }
07610
07611 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07612 {
07613 ENTERFUNC;
07614 double costheta,sintheta,cosphi,sinphi;
07615 for(int i = 0; i<len; i++)
07616 {
07617 cosphi = cos(y[i]*dgr_to_rad);
07618 sinphi = sin(y[i]*dgr_to_rad);
07619 if(fabs(x[i]-90.0)< 1.0e-5){
07620 x[i] = cosphi;
07621 y[i] = sinphi;
07622 z[i] = 0.0;
07623 }
07624 else{
07625 costheta = cos(x[i]*dgr_to_rad);
07626 sintheta = sin(x[i]*dgr_to_rad);
07627 x[i] = cosphi*sintheta;
07628 y[i] = sinphi*sintheta;
07629 z[i] = costheta;
07630 }
07631 }
07632 EXITFUNC;
07633 }
07634
07635 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07636 {
07637 ENTERFUNC;
07638 int i = k;
07639 while( i == k ) i = rand()%len;
07640 std::swap(key[i], key[k]);
07641 std::swap(x[i], x[k]);
07642 std::swap(y[i], y[k]);
07643 std::swap(z[i], z[k]);
07644 EXITFUNC;
07645 }
07646
07647
07648 #undef mymax
07649 #undef mymin
07650 #undef sign
07651 #undef quadpi
07652 #undef dgr_to_rad
07653 #undef deg_to_rad
07654 #undef rad_to_deg
07655 #undef rad_to_dgr
07656 #undef TRUE
07657 #undef FALSE
07658 #undef theta
07659 #undef phi
07660 #undef weight
07661 #undef lband
07662 #undef ts
07663 #undef thetast
07664 #undef key
07665
07666
07667
07668
07669
07670
07671
07672
07673
07674
07675
07676 #define TRUE_ (1)
07677 #define FALSE_ (0)
07678 #define abs(x) ((x) >= 0 ? (x) : -(x))
07679
07680 struct stcom_{
07681 double y;
07682 };
07683 stcom_ stcom_1;
07684 #ifdef KR_headers
07685 double floor();
07686 int i_dnnt(x) double *x;
07687 #else
07688 int i_dnnt(double *x)
07689 #endif
07690 {
07691 return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07692 }
07693
07694
07695
07696
07697
07698 int Util::trmsh3_(int *n0, double *tol, double *x,
07699 double *y, double *z__, int *n, int *list, int *
07700 lptr, int *lend, int *lnew, int *indx, int *lcnt,
07701 int *near__, int *next, double *dist, int *ier)
07702 {
07703
07704 int i__1, i__2;
07705
07706
07707 static double d__;
07708 static int i__, j;
07709 static double d1, d2, d3;
07710 static int i0, lp, kt, ku, lpl, nku;
07711 extern long int left_(double *, double *, double *, double
07712 *, double *, double *, double *, double *,
07713 double *);
07714 static int nexti;
07715 extern int addnod_(int *, int *, double *,
07716 double *, double *, int *, int *, int *,
07717 int *, int *);
07718
07719
07720
07721
07722
07723
07724
07725
07726
07727
07728
07729
07730
07731
07732
07733
07734
07735
07736
07737
07738
07739
07740
07741
07742
07743
07744
07745
07746
07747
07748
07749
07750
07751
07752
07753
07754
07755
07756
07757
07758
07759
07760
07761
07762
07763
07764
07765
07766
07767
07768
07769
07770
07771
07772
07773
07774
07775
07776
07777
07778
07779
07780
07781
07782
07783
07784
07785
07786
07787
07788
07789
07790
07791
07792
07793
07794
07795
07796
07797
07798
07799
07800
07801
07802
07803
07804
07805
07806
07807
07808
07809
07810
07811
07812
07813
07814
07815
07816
07817
07818
07819
07820
07821
07822
07823
07824
07825
07826
07827
07828
07829
07830
07831
07832
07833
07834
07835
07836
07837
07838
07839
07840
07841
07842
07843
07844
07845
07846
07847
07848
07849
07850
07851
07852
07853
07854
07855
07856
07857
07858
07859
07860
07861
07862
07863
07864
07865
07866
07867
07868
07869 --dist;
07870 --next;
07871 --near__;
07872 --indx;
07873 --lend;
07874 --z__;
07875 --y;
07876 --x;
07877 --list;
07878 --lptr;
07879 --lcnt;
07880
07881
07882 if (*n0 < 3) {
07883 *n = 0;
07884 *ier = -1;
07885 return 0;
07886 }
07887
07888
07889
07890 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
07891 z__[3])) {
07892
07893
07894
07895 list[1] = 3;
07896 lptr[1] = 2;
07897 list[2] = -2;
07898 lptr[2] = 1;
07899 lend[1] = 2;
07900
07901 list[3] = 1;
07902 lptr[3] = 4;
07903 list[4] = -3;
07904 lptr[4] = 3;
07905 lend[2] = 4;
07906
07907 list[5] = 2;
07908 lptr[5] = 6;
07909 list[6] = -1;
07910 lptr[6] = 5;
07911 lend[3] = 6;
07912
07913 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
07914 y[3], &z__[3])) {
07915
07916
07917
07918
07919
07920 list[1] = 2;
07921 lptr[1] = 2;
07922 list[2] = -3;
07923 lptr[2] = 1;
07924 lend[1] = 2;
07925
07926 list[3] = 3;
07927 lptr[3] = 4;
07928 list[4] = -1;
07929 lptr[4] = 3;
07930 lend[2] = 4;
07931
07932 list[5] = 1;
07933 lptr[5] = 6;
07934 list[6] = -2;
07935 lptr[6] = 5;
07936 lend[3] = 6;
07937
07938
07939 } else {
07940
07941
07942
07943 *n = 0;
07944 *ier = -2;
07945 return 0;
07946 }
07947
07948
07949
07950
07951
07952 *lnew = 7;
07953 indx[1] = 1;
07954 indx[2] = 2;
07955 indx[3] = 3;
07956 lcnt[1] = 1;
07957 lcnt[2] = 1;
07958 lcnt[3] = 1;
07959 if (*n0 == 3) {
07960 *n = 3;
07961 *ier = 0;
07962 return 0;
07963 }
07964
07965
07966
07967
07968
07969
07970
07971
07972
07973
07974
07975
07976
07977
07978
07979
07980
07981
07982
07983
07984
07985
07986
07987
07988
07989
07990
07991 near__[1] = 0;
07992 near__[2] = 0;
07993 near__[3] = 0;
07994 for (ku = *n0; ku >= 4; --ku) {
07995 d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
07996 d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
07997 d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
07998 if (d1 <= d2 && d1 <= d3) {
07999 near__[ku] = 1;
08000 dist[ku] = d1;
08001 next[ku] = near__[1];
08002 near__[1] = ku;
08003 } else if (d2 <= d1 && d2 <= d3) {
08004 near__[ku] = 2;
08005 dist[ku] = d2;
08006 next[ku] = near__[2];
08007 near__[2] = ku;
08008 } else {
08009 near__[ku] = 3;
08010 dist[ku] = d3;
08011 next[ku] = near__[3];
08012 near__[3] = ku;
08013 }
08014
08015 }
08016
08017
08018
08019
08020 kt = 3;
08021 i__1 = *n0;
08022 for (ku = 4; ku <= i__1; ++ku) {
08023 nku = near__[ku];
08024
08025
08026
08027 i__ = nku;
08028 if (near__[i__] == ku) {
08029 near__[i__] = next[ku];
08030 } else {
08031 i__ = near__[i__];
08032 L2:
08033 i0 = i__;
08034 i__ = next[i0];
08035 if (i__ != ku) {
08036 goto L2;
08037 }
08038 next[i0] = next[ku];
08039 }
08040 near__[ku] = 0;
08041
08042
08043
08044 if (dist[ku] <= *tol - 1.) {
08045 indx[ku] = -nku;
08046 ++lcnt[nku];
08047 goto L6;
08048 }
08049
08050
08051
08052 ++kt;
08053 x[kt] = x[ku];
08054 y[kt] = y[ku];
08055 z__[kt] = z__[ku];
08056 indx[ku] = kt;
08057 lcnt[kt] = 1;
08058 addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08059 , lnew, ier);
08060 if (*ier != 0) {
08061 *n = 0;
08062 *ier = -3;
08063 return 0;
08064 }
08065
08066
08067
08068 lpl = lend[kt];
08069 lp = lpl;
08070 L3:
08071 lp = lptr[lp];
08072 j = (i__2 = list[lp], abs(i__2));
08073
08074
08075
08076
08077
08078
08079
08080 i__ = near__[j];
08081 L4:
08082 if (i__ == 0) {
08083 goto L5;
08084 }
08085 nexti = next[i__];
08086
08087
08088
08089
08090 d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08091 if (d__ < dist[i__]) {
08092
08093
08094
08095
08096
08097 near__[i__] = kt;
08098 dist[i__] = d__;
08099 if (i__ == near__[j]) {
08100 near__[j] = nexti;
08101 } else {
08102 next[i0] = nexti;
08103 }
08104 next[i__] = near__[kt];
08105 near__[kt] = i__;
08106 } else {
08107 i0 = i__;
08108 }
08109
08110
08111
08112 i__ = nexti;
08113 goto L4;
08114
08115
08116
08117 L5:
08118 if (lp != lpl) {
08119 goto L3;
08120 }
08121 L6:
08122 ;
08123 }
08124 *n = kt;
08125 *ier = 0;
08126 return 0;
08127 }
08128
08129
08130 int addnod_(int *nst, int *k, double *x,
08131 double *y, double *z__, int *list, int *lptr, int
08132 *lend, int *lnew, int *ier)
08133 {
08134
08135
08136 static double tol = 0.;
08137
08138
08139 int i__1;
08140
08141
08142 static int l;
08143 static double p[3], b1, b2, b3;
08144 static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08145 extern int swap_(int *, int *, int *,
08146 int *, int *, int *, int *, int *);
08147 static int lpo1s;
08148 extern int bdyadd_(int *, int *, int *,
08149 int *, int *, int *, int *), intadd_(int *,
08150 int *, int *, int *, int *, int *, int *,
08151 int *), trfind_(int *, double *, int *,
08152 double *, double *, double *, int *, int *,
08153 int *, double *, double *, double *, int *,
08154 int *, int *), covsph_(int *, int *, int *,
08155 int *, int *, int *);
08156 extern int lstptr_(int *, int *, int *, int *);
08157 extern long int swptst_(int *, int *, int *, int *,
08158 double *, double *, double *);
08159
08160
08161
08162
08163
08164
08165
08166
08167
08168
08169
08170
08171
08172
08173
08174
08175
08176
08177
08178
08179
08180
08181
08182
08183
08184
08185
08186
08187
08188
08189
08190
08191
08192
08193
08194
08195
08196
08197
08198
08199
08200
08201
08202
08203
08204
08205
08206
08207
08208
08209
08210
08211
08212
08213
08214
08215
08216
08217
08218
08219
08220
08221
08222
08223
08224
08225
08226
08227
08228
08229
08230
08231
08232
08233
08234
08235
08236
08237
08238
08239
08240
08241
08242
08243
08244
08245
08246
08247
08248
08249
08250
08251
08252
08253
08254
08255
08256
08257
08258
08259
08260
08261 --lend;
08262 --z__;
08263 --y;
08264 --x;
08265 --list;
08266 --lptr;
08267
08268
08269
08270 kk = *k;
08271 if (kk < 4) {
08272 goto L3;
08273 }
08274
08275
08276 km1 = kk - 1;
08277 ist = *nst;
08278 if (ist < 1) {
08279 ist = km1;
08280 }
08281 p[0] = x[kk];
08282 p[1] = y[kk];
08283 p[2] = z__[kk];
08284
08285
08286
08287
08288 trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08289 , &b1, &b2, &b3, &i1, &i2, &i3);
08290
08291
08292
08293 if (i1 == 0) {
08294 goto L4;
08295 }
08296 l = i1;
08297 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08298 goto L5;
08299 }
08300 l = i2;
08301 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08302 goto L5;
08303 }
08304 if (i3 != 0) {
08305 l = i3;
08306 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08307 goto L5;
08308 }
08309 intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08310 } else {
08311 if (i1 != i2) {
08312 bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08313 } else {
08314 covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08315 }
08316 }
08317 *ier = 0;
08318
08319
08320
08321 lp = lend[kk];
08322 lpf = lptr[lp];
08323 io2 = list[lpf];
08324 lpo1 = lptr[lpf];
08325 io1 = (i__1 = list[lpo1], abs(i__1));
08326
08327
08328
08329 L1:
08330 lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08331 if (list[lp] < 0) {
08332 goto L2;
08333 }
08334 lp = lptr[lp];
08335 in1 = (i__1 = list[lp], abs(i__1));
08336
08337
08338
08339
08340 lpo1s = lpo1;
08341 if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08342 goto L2;
08343 }
08344 swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08345 if (lpo1 == 0) {
08346
08347
08348
08349
08350
08351
08352 lpo1 = lpo1s;
08353 goto L2;
08354 }
08355 io1 = in1;
08356 goto L1;
08357
08358
08359
08360
08361 L2:
08362 if (lpo1 == lpf || list[lpo1] < 0) {
08363 return 0;
08364 }
08365 io2 = io1;
08366 lpo1 = lptr[lpo1];
08367 io1 = (i__1 = list[lpo1], abs(i__1));
08368 goto L1;
08369
08370
08371
08372 L3:
08373 *ier = -1;
08374 return 0;
08375
08376
08377
08378 L4:
08379 *ier = -2;
08380 return 0;
08381
08382
08383
08384 L5:
08385 *ier = l;
08386 return 0;
08387 }
08388
08389 double angle_(double *v1, double *v2, double *v3)
08390 {
08391
08392 double ret_val;
08393
08394
08395
08396
08397
08398 static double a;
08399 static int i__;
08400 static double ca, s21, s23, u21[3], u23[3];
08401 extern long int left_(double *, double *, double *, double
08402 *, double *, double *, double *, double *,
08403 double *);
08404
08405
08406
08407
08408
08409
08410
08411
08412
08413
08414
08415
08416
08417
08418
08419
08420
08421
08422
08423
08424
08425
08426
08427
08428
08429
08430
08431
08432
08433
08434
08435
08436
08437
08438
08439
08440
08441
08442
08443
08444
08445
08446
08447
08448
08449
08450
08451
08452
08453
08454
08455
08456
08457
08458
08459
08460
08461
08462
08463 --v3;
08464 --v2;
08465 --v1;
08466
08467
08468 u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08469 u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08470 u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08471
08472 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08473 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08474 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08475
08476
08477
08478 s21 = 0.;
08479 s23 = 0.;
08480 for (i__ = 1; i__ <= 3; ++i__) {
08481 s21 += u21[i__ - 1] * u21[i__ - 1];
08482 s23 += u23[i__ - 1] * u23[i__ - 1];
08483
08484 }
08485
08486
08487
08488
08489 if (s21 == 0. || s23 == 0.) {
08490 ret_val = 0.;
08491 return ret_val;
08492 }
08493 s21 = sqrt(s21);
08494 s23 = sqrt(s23);
08495 for (i__ = 1; i__ <= 3; ++i__) {
08496 u21[i__ - 1] /= s21;
08497 u23[i__ - 1] /= s23;
08498
08499 }
08500
08501
08502
08503
08504
08505 ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08506 if (ca < -1.) {
08507 ca = -1.;
08508 }
08509 if (ca > 1.) {
08510 ca = 1.;
08511 }
08512 a = acos(ca);
08513
08514
08515
08516
08517 if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08518 , &v3[3])) {
08519 a = acos(-1.) * 2. - a;
08520 }
08521 ret_val = a;
08522 return ret_val;
08523 }
08524
08525 double areas_(double *v1, double *v2, double *v3)
08526 {
08527
08528 double ret_val;
08529
08530
08531
08532
08533
08534 static int i__;
08535 static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08536 ca2, ca3;
08537
08538
08539
08540
08541
08542
08543
08544
08545
08546
08547
08548
08549
08550
08551
08552
08553
08554
08555
08556
08557
08558
08559
08560
08561
08562
08563
08564
08565
08566
08567
08568
08569
08570
08571
08572
08573
08574
08575
08576
08577
08578
08579
08580
08581
08582
08583
08584
08585
08586
08587
08588
08589
08590 --v3;
08591 --v2;
08592 --v1;
08593
08594
08595 u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08596 u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08597 u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08598
08599 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08600 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08601 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08602
08603 u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08604 u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08605 u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08606
08607
08608
08609 s12 = 0.;
08610 s23 = 0.;
08611 s31 = 0.;
08612 for (i__ = 1; i__ <= 3; ++i__) {
08613 s12 += u12[i__ - 1] * u12[i__ - 1];
08614 s23 += u23[i__ - 1] * u23[i__ - 1];
08615 s31 += u31[i__ - 1] * u31[i__ - 1];
08616
08617 }
08618
08619
08620
08621
08622 if (s12 == 0. || s23 == 0. || s31 == 0.) {
08623 ret_val = 0.;
08624 return ret_val;
08625 }
08626 s12 = sqrt(s12);
08627 s23 = sqrt(s23);
08628 s31 = sqrt(s31);
08629 for (i__ = 1; i__ <= 3; ++i__) {
08630 u12[i__ - 1] /= s12;
08631 u23[i__ - 1] /= s23;
08632 u31[i__ - 1] /= s31;
08633
08634 }
08635
08636
08637
08638
08639
08640
08641
08642 ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08643 ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08644 ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08645 if (ca1 < -1.) {
08646 ca1 = -1.;
08647 }
08648 if (ca1 > 1.) {
08649 ca1 = 1.;
08650 }
08651 if (ca2 < -1.) {
08652 ca2 = -1.;
08653 }
08654 if (ca2 > 1.) {
08655 ca2 = 1.;
08656 }
08657 if (ca3 < -1.) {
08658 ca3 = -1.;
08659 }
08660 if (ca3 > 1.) {
08661 ca3 = 1.;
08662 }
08663 a1 = acos(ca1);
08664 a2 = acos(ca2);
08665 a3 = acos(ca3);
08666
08667
08668
08669 ret_val = a1 + a2 + a3 - acos(-1.);
08670 if (ret_val < 0.) {
08671 ret_val = 0.;
08672 }
08673 return ret_val;
08674 }
08675
08676 double Util::areav_(int *k, int *n, double *x, double *y,
08677 double *z__, int *list, int *lptr, int *lend, int
08678 *ier)
08679 {
08680
08681
08682 static double amax = 6.28;
08683
08684
08685 double ret_val;
08686
08687
08688 static double a, c0[3], c2[3], c3[3];
08689 static int n1, n2, n3;
08690 static double v1[3], v2[3], v3[3];
08691 static int lp, lpl, ierr;
08692 static double asum;
08693 extern double areas_(double *, double *, double *);
08694 static long int first;
08695 extern int circum_(double *, double *,
08696 double *, double *, int *);
08697
08698
08699
08700
08701
08702
08703
08704
08705
08706
08707
08708
08709
08710
08711
08712
08713
08714
08715
08716
08717
08718
08719
08720
08721
08722
08723
08724
08725
08726
08727
08728
08729
08730
08731
08732
08733
08734
08735
08736
08737
08738
08739
08740
08741
08742
08743
08744
08745
08746
08747
08748
08749
08750
08751
08752
08753
08754
08755 --lend;
08756 --z__;
08757 --y;
08758 --x;
08759 --list;
08760 --lptr;
08761
08762
08763
08764
08765
08766 if (*k < 1 || *k > *n || *n <= 3) {
08767 goto L11;
08768 }
08769
08770
08771
08772
08773
08774 n1 = *k;
08775 v1[0] = x[n1];
08776 v1[1] = y[n1];
08777 v1[2] = z__[n1];
08778 lpl = lend[n1];
08779 n3 = list[lpl];
08780 if (n3 < 0) {
08781 goto L12;
08782 }
08783 lp = lpl;
08784 first = TRUE_;
08785 asum = 0.;
08786
08787
08788
08789 L1:
08790 n2 = n3;
08791 lp = lptr[lp];
08792 n3 = list[lp];
08793 v2[0] = x[n2];
08794 v2[1] = y[n2];
08795 v2[2] = z__[n2];
08796 v3[0] = x[n3];
08797 v3[1] = y[n3];
08798 v3[2] = z__[n3];
08799 if (first) {
08800
08801
08802
08803
08804 circum_(v1, v2, v3, c3, &ierr);
08805 if (ierr != 0) {
08806 goto L13;
08807 }
08808 c0[0] = c3[0];
08809 c0[1] = c3[1];
08810 c0[2] = c3[2];
08811 first = FALSE_;
08812 } else {
08813
08814
08815
08816
08817 c2[0] = c3[0];
08818 c2[1] = c3[1];
08819 c2[2] = c3[2];
08820 circum_(v1, v2, v3, c3, &ierr);
08821 if (ierr != 0) {
08822 goto L13;
08823 }
08824 a = areas_(v1, c2, c3);
08825 if (a > amax) {
08826 goto L14;
08827 }
08828 asum += a;
08829 }
08830
08831
08832
08833 if (lp != lpl) {
08834 goto L1;
08835 }
08836
08837
08838
08839 a = areas_(v1, c3, c0);
08840 if (a > amax) {
08841 goto L14;
08842 }
08843 asum += a;
08844
08845
08846
08847 *ier = 0;
08848 ret_val = asum;
08849 return ret_val;
08850
08851
08852
08853 L11:
08854 *ier = 1;
08855 ret_val = 0.;
08856 return ret_val;
08857
08858
08859
08860 L12:
08861 *ier = 2;
08862 ret_val = 0.;
08863 return ret_val;
08864
08865
08866
08867 L13:
08868 *ier = 3;
08869 ret_val = 0.;
08870 return ret_val;
08871
08872
08873
08874 L14:
08875 *ier = 4;
08876 ret_val = 0.;
08877 return ret_val;
08878 }
08879
08880 double areav_new__(int *k, int *n, double *x, double *y,
08881 double *z__, int *list, int *lptr, int *lend, int
08882 *ier)
08883 {
08884
08885 double ret_val = 0;
08886
08887
08888
08889
08890
08891 static int m;
08892 static double c1[3], c2[3], c3[3];
08893 static int n1, n2, n3;
08894 static double v1[3], v2[3], v3[3];
08895 static int lp;
08896 static double c1s[3], c2s[3];
08897 static int lpl, ierr;
08898 static double asum;
08899 extern double angle_(double *, double *, double *);
08900 static float areav;
08901 extern int circum_(double *, double *,
08902 double *, double *, int *);
08903
08904
08905
08906
08907
08908
08909
08910
08911
08912
08913
08914
08915
08916
08917
08918
08919
08920
08921
08922
08923
08924
08925
08926
08927
08928
08929
08930
08931
08932
08933
08934
08935
08936
08937
08938
08939
08940
08941
08942
08943
08944
08945
08946
08947
08948
08949
08950
08951
08952
08953
08954
08955
08956
08957
08958
08959
08960
08961
08962
08963
08964 --lend;
08965 --z__;
08966 --y;
08967 --x;
08968 --list;
08969 --lptr;
08970
08971
08972 if (*k < 1 || *k > *n || *n <= 3) {
08973 goto L11;
08974 }
08975
08976
08977
08978
08979
08980 n1 = *k;
08981 v1[0] = x[n1];
08982 v1[1] = y[n1];
08983 v1[2] = z__[n1];
08984 lpl = lend[n1];
08985 n3 = list[lpl];
08986 if (n3 < 0) {
08987 goto L12;
08988 }
08989 lp = lpl;
08990 m = 0;
08991 asum = 0.;
08992
08993
08994
08995 L1:
08996 ++m;
08997 n2 = n3;
08998 lp = lptr[lp];
08999 n3 = list[lp];
09000 v2[0] = x[n2];
09001 v2[1] = y[n2];
09002 v2[2] = z__[n2];
09003 v3[0] = x[n3];
09004 v3[1] = y[n3];
09005 v3[2] = z__[n3];
09006 if (m == 1) {
09007
09008
09009
09010
09011 circum_(v1, v2, v3, c2, &ierr);
09012 if (ierr != 0) {
09013 goto L13;
09014 }
09015 c1s[0] = c2[0];
09016 c1s[1] = c2[1];
09017 c1s[2] = c2[2];
09018 } else if (m == 2) {
09019
09020
09021
09022
09023 circum_(v1, v2, v3, c3, &ierr);
09024 if (ierr != 0) {
09025 goto L13;
09026 }
09027 c2s[0] = c3[0];
09028 c2s[1] = c3[1];
09029 c2s[2] = c3[2];
09030 } else {
09031
09032
09033
09034
09035
09036 c1[0] = c2[0];
09037 c1[1] = c2[1];
09038 c1[2] = c2[2];
09039 c2[0] = c3[0];
09040 c2[1] = c3[1];
09041 c2[2] = c3[2];
09042 circum_(v1, v2, v3, c3, &ierr);
09043 if (ierr != 0) {
09044 goto L13;
09045 }
09046 asum += angle_(c1, c2, c3);
09047 }
09048
09049
09050
09051 if (lp != lpl) {
09052 goto L1;
09053 }
09054
09055
09056
09057
09058 asum += angle_(c2, c3, c1s);
09059
09060
09061
09062
09063 asum += angle_(c3, c1s, c2s);
09064
09065
09066
09067 *ier = 0;
09068 ret_val = asum - (double) (m - 2) * acos(-1.);
09069 return ret_val;
09070
09071
09072
09073 L11:
09074 *ier = 1;
09075 areav = 0.f;
09076 return ret_val;
09077
09078
09079
09080 L12:
09081 *ier = 2;
09082 areav = 0.f;
09083 return ret_val;
09084
09085
09086
09087 L13:
09088 *ier = 3;
09089 areav = 0.f;
09090 return ret_val;
09091 }
09092
09093 int bdyadd_(int *kk, int *i1, int *i2, int *
09094 list, int *lptr, int *lend, int *lnew)
09095 {
09096 static int k, n1, n2, lp, lsav, nsav, next;
09097 extern int insert_(int *, int *, int *,
09098 int *, int *);
09099
09100
09101
09102
09103
09104
09105
09106
09107
09108
09109
09110
09111
09112
09113
09114
09115
09116
09117
09118
09119
09120
09121
09122
09123
09124
09125
09126
09127
09128
09129
09130
09131
09132
09133
09134
09135
09136
09137
09138
09139
09140
09141
09142
09143
09144
09145
09146
09147
09148
09149
09150
09151
09152
09153
09154
09155
09156
09157
09158
09159
09160
09161
09162
09163 --lend;
09164 --lptr;
09165 --list;
09166
09167
09168 k = *kk;
09169 n1 = *i1;
09170 n2 = *i2;
09171
09172
09173
09174 lp = lend[n1];
09175 lsav = lptr[lp];
09176 lptr[lp] = *lnew;
09177 list[*lnew] = -k;
09178 lptr[*lnew] = lsav;
09179 lend[n1] = *lnew;
09180 ++(*lnew);
09181 next = -list[lp];
09182 list[lp] = next;
09183 nsav = next;
09184
09185
09186
09187
09188 L1:
09189 lp = lend[next];
09190 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09191 if (next == n2) {
09192 goto L2;
09193 }
09194 next = -list[lp];
09195 list[lp] = next;
09196 goto L1;
09197
09198
09199
09200
09201 L2:
09202 lsav = *lnew;
09203 list[*lnew] = n1;
09204 lptr[*lnew] = *lnew + 1;
09205 ++(*lnew);
09206 next = nsav;
09207
09208 L3:
09209 if (next == n2) {
09210 goto L4;
09211 }
09212 list[*lnew] = next;
09213 lptr[*lnew] = *lnew + 1;
09214 ++(*lnew);
09215 lp = lend[next];
09216 next = list[lp];
09217 goto L3;
09218
09219 L4:
09220 list[*lnew] = -n2;
09221 lptr[*lnew] = lsav;
09222 lend[k] = *lnew;
09223 ++(*lnew);
09224 return 0;
09225 }
09226
09227 int bnodes_(int *n, int *list, int *lptr,
09228 int *lend, int *nodes, int *nb, int *na, int *nt)
09229 {
09230
09231 int i__1;
09232
09233
09234 static int k, n0, lp, nn, nst;
09235
09236
09237
09238
09239
09240
09241
09242
09243
09244
09245
09246
09247
09248
09249
09250
09251
09252
09253
09254
09255
09256
09257
09258
09259
09260
09261
09262
09263
09264
09265
09266
09267
09268
09269
09270
09271
09272
09273
09274
09275
09276
09277
09278
09279
09280
09281
09282
09283
09284
09285
09286
09287
09288
09289
09290
09291
09292
09293
09294
09295 --lend;
09296 --list;
09297 --lptr;
09298 --nodes;
09299
09300
09301 nn = *n;
09302
09303
09304
09305 i__1 = nn;
09306 for (nst = 1; nst <= i__1; ++nst) {
09307 lp = lend[nst];
09308 if (list[lp] < 0) {
09309 goto L2;
09310 }
09311
09312 }
09313
09314
09315
09316 *nb = 0;
09317 *na = (nn - 2) * 3;
09318 *nt = nn - (2<<1);
09319 return 0;
09320
09321
09322
09323
09324 L2:
09325 nodes[1] = nst;
09326 k = 1;
09327 n0 = nst;
09328
09329
09330
09331 L3:
09332 lp = lend[n0];
09333 lp = lptr[lp];
09334 n0 = list[lp];
09335 if (n0 == nst) {
09336 goto L4;
09337 }
09338 ++k;
09339 nodes[k] = n0;
09340 goto L3;
09341
09342
09343
09344 L4:
09345 *nb = k;
09346 *nt = (*n << 1) - *nb - 2;
09347 *na = *nt + *n - 1;
09348 return 0;
09349 }
09350
09351 int circle_(int *k, double *xc, double *yc,
09352 int *ier)
09353 {
09354
09355 int i__1;
09356
09357
09358
09359
09360
09361 static double a, c__;
09362 static int i__;
09363 static double s;
09364 static int k2, k3;
09365 static double x0, y0;
09366 static int kk, np1;
09367
09368
09369
09370
09371
09372
09373
09374
09375
09376
09377
09378
09379
09380
09381
09382
09383
09384
09385
09386
09387
09388
09389
09390
09391
09392
09393
09394
09395
09396
09397
09398
09399
09400
09401
09402
09403
09404
09405
09406
09407
09408
09409
09410
09411
09412
09413
09414
09415
09416
09417
09418
09419
09420
09421
09422
09423
09424
09425
09426
09427
09428
09429
09430
09431 --yc;
09432 --xc;
09433
09434
09435 kk = *k;
09436 k2 = kk << 1;
09437 k3 = kk * 3;
09438 np1 = (kk << 2) + 1;
09439
09440
09441
09442
09443 if (kk < 1) {
09444 goto L2;
09445 }
09446 a = atan(1.) * 2. / (double) kk;
09447 c__ = cos(a);
09448 s = sin(a);
09449 x0 = 1.;
09450 y0 = 0.;
09451
09452
09453
09454
09455
09456 i__1 = kk;
09457 for (i__ = 1; i__ <= i__1; ++i__) {
09458 xc[i__] = x0;
09459 yc[i__] = y0;
09460 xc[i__ + kk] = -y0;
09461 yc[i__ + kk] = x0;
09462 xc[i__ + k2] = -x0;
09463 yc[i__ + k2] = -y0;
09464 xc[i__ + k3] = y0;
09465 yc[i__ + k3] = -x0;
09466
09467
09468
09469 x0 = c__ * x0 - s * y0;
09470 y0 = s * x0 + c__ * y0;
09471
09472 }
09473
09474
09475
09476
09477 xc[np1] = xc[1];
09478 yc[np1] = yc[1];
09479 *ier = 0;
09480 return 0;
09481
09482
09483
09484 L2:
09485 *ier = 1;
09486 return 0;
09487 }
09488
09489 int circum_(double *v1, double *v2, double *v3,
09490 double *c__, int *ier)
09491 {
09492
09493
09494
09495
09496 static int i__;
09497 static double e1[3], e2[3], cu[3], cnorm;
09498
09499
09500
09501
09502
09503
09504
09505
09506
09507
09508
09509
09510
09511
09512
09513
09514
09515
09516
09517
09518
09519
09520
09521
09522
09523
09524
09525
09526
09527
09528
09529
09530
09531
09532
09533
09534
09535
09536
09537
09538
09539
09540
09541
09542
09543
09544
09545
09546
09547
09548
09549
09550
09551
09552
09553
09554 --c__;
09555 --v3;
09556 --v2;
09557 --v1;
09558
09559
09560 for (i__ = 1; i__ <= 3; ++i__) {
09561 e1[i__ - 1] = v2[i__] - v1[i__];
09562 e2[i__ - 1] = v3[i__] - v1[i__];
09563
09564 }
09565
09566
09567
09568 cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09569 cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09570 cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09571 cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09572
09573
09574
09575
09576 if (cnorm != 0.) {
09577
09578
09579
09580 cnorm = sqrt(cnorm);
09581 for (i__ = 1; i__ <= 3; ++i__) {
09582 c__[i__] = cu[i__ - 1] / cnorm;
09583
09584 }
09585
09586
09587
09588
09589
09590
09591 if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09592 c__[1] = -c__[1];
09593 c__[2] = -c__[2];
09594 c__[3] = -c__[3];
09595 }
09596 *ier = 0;
09597 } else {
09598
09599
09600
09601 *ier = 1;
09602 }
09603 return 0;
09604 }
09605
09606 int covsph_(int *kk, int *n0, int *list, int
09607 *lptr, int *lend, int *lnew)
09608 {
09609 static int k, lp, nst, lsav, next;
09610 extern int insert_(int *, int *, int *,
09611 int *, int *);
09612
09613
09614
09615
09616
09617
09618
09619
09620
09621
09622
09623
09624
09625
09626
09627
09628
09629
09630
09631
09632
09633
09634
09635
09636
09637
09638
09639
09640
09641
09642
09643
09644
09645
09646
09647
09648
09649
09650
09651
09652
09653
09654
09655
09656
09657
09658
09659
09660
09661
09662
09663
09664
09665
09666
09667
09668
09669 --lend;
09670 --lptr;
09671 --list;
09672
09673
09674 k = *kk;
09675 nst = *n0;
09676
09677
09678
09679
09680
09681 next = nst;
09682 L1:
09683 lp = lend[next];
09684 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09685 next = -list[lp];
09686 list[lp] = next;
09687 if (next != nst) {
09688 goto L1;
09689 }
09690
09691
09692
09693
09694 lsav = *lnew;
09695 L2:
09696 lp = lend[next];
09697 list[*lnew] = next;
09698 lptr[*lnew] = *lnew + 1;
09699 ++(*lnew);
09700 next = list[lp];
09701 if (next != nst) {
09702 goto L2;
09703 }
09704
09705 lptr[*lnew - 1] = lsav;
09706 lend[k] = *lnew - 1;
09707 return 0;
09708 }
09709
09710 int crlist_(int *n, int *ncol, double *x,
09711 double *y, double *z__, int *list, int *lend, int
09712 *lptr, int *lnew, int *ltri, int *listc, int *nb,
09713 double *xc, double *yc, double *zc, double *rc,
09714 int *ier)
09715 {
09716
09717 int i__1, i__2;
09718
09719
09720
09721
09722
09723 static double c__[3], t;
09724 static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09725 static double v1[3], v2[3], v3[3];
09726 static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09727 lpn;
09728 static long int swp;
09729 static int ierr;
09730 extern int circum_(double *, double *,
09731 double *, double *, int *);
09732 extern int lstptr_(int *, int *, int *, int *);
09733 extern long int swptst_(int *, int *, int *, int *,
09734 double *, double *, double *);
09735
09736
09737
09738
09739
09740
09741
09742
09743
09744
09745
09746
09747
09748
09749
09750
09751
09752
09753
09754
09755
09756
09757
09758
09759
09760
09761
09762
09763
09764
09765
09766
09767
09768
09769
09770
09771
09772
09773
09774
09775
09776
09777
09778
09779
09780
09781
09782
09783
09784
09785
09786
09787
09788
09789
09790
09791
09792
09793
09794
09795
09796
09797
09798
09799
09800
09801
09802
09803
09804
09805
09806
09807
09808
09809
09810
09811
09812
09813
09814
09815
09816
09817
09818
09819
09820
09821
09822
09823
09824
09825
09826
09827
09828
09829
09830
09831
09832
09833
09834
09835
09836
09837
09838
09839
09840
09841
09842
09843
09844
09845
09846
09847
09848
09849
09850
09851
09852
09853
09854
09855
09856
09857
09858
09859
09860
09861
09862
09863
09864
09865
09866
09867
09868
09869
09870
09871
09872
09873
09874
09875
09876
09877
09878
09879
09880
09881
09882
09883
09884
09885
09886
09887
09888
09889
09890
09891
09892
09893
09894
09895
09896
09897
09898
09899
09900
09901
09902
09903
09904
09905
09906
09907
09908
09909
09910
09911 --lend;
09912 --z__;
09913 --y;
09914 --x;
09915 ltri -= 7;
09916 --list;
09917 --lptr;
09918 --listc;
09919 --xc;
09920 --yc;
09921 --zc;
09922 --rc;
09923
09924
09925 nn = *n;
09926 *nb = 0;
09927 nt = 0;
09928 if (nn < 3) {
09929 goto L21;
09930 }
09931
09932
09933
09934 i__1 = nn;
09935 for (n1 = 1; n1 <= i__1; ++n1) {
09936 lp = lend[n1];
09937 if (list[lp] < 0) {
09938 goto L2;
09939 }
09940
09941 }
09942
09943
09944
09945 goto L9;
09946
09947
09948
09949
09950
09951
09952
09953
09954 L2:
09955 n2 = -list[lp];
09956 lp = lptr[lp];
09957 n3 = list[lp];
09958
09959
09960
09961
09962
09963
09964 L3:
09965 ++nt;
09966 if (nt <= *ncol) {
09967 ltri[nt * 6 + 1] = n1;
09968 ltri[nt * 6 + 2] = n2;
09969 ltri[nt * 6 + 3] = n3;
09970 ltri[nt * 6 + 4] = nt + 1;
09971 ltri[nt * 6 + 5] = nt - 1;
09972 ltri[nt * 6 + 6] = 0;
09973 }
09974 n1 = n2;
09975 lp = lend[n1];
09976 n2 = -list[lp];
09977 if (n2 != n3) {
09978 goto L3;
09979 }
09980
09981 *nb = nt + 2;
09982 if (*ncol < nt) {
09983 goto L22;
09984 }
09985 ltri[nt * 6 + 4] = 0;
09986 if (nt == 1) {
09987 goto L7;
09988 }
09989
09990
09991
09992
09993
09994
09995
09996 L4:
09997 swp = FALSE_;
09998 i__1 = nt - 1;
09999 for (kt1 = 1; kt1 <= i__1; ++kt1) {
10000 for (i3 = 1; i3 <= 3; ++i3) {
10001 kt2 = ltri[i3 + 3 + kt1 * 6];
10002 if (kt2 <= kt1) {
10003 goto L5;
10004 }
10005
10006
10007
10008
10009 if (i3 == 1) {
10010 i1 = 2;
10011 i2 = 3;
10012 } else if (i3 == 2) {
10013 i1 = 3;
10014 i2 = 1;
10015 } else {
10016 i1 = 1;
10017 i2 = 2;
10018 }
10019 n1 = ltri[i1 + kt1 * 6];
10020 n2 = ltri[i2 + kt1 * 6];
10021 n3 = ltri[i3 + kt1 * 6];
10022
10023
10024
10025
10026 if (ltri[kt2 * 6 + 4] == kt1) {
10027 i4 = 1;
10028 } else if (ltri[kt2 * 6 + 5] == kt1) {
10029 i4 = 2;
10030 } else {
10031 i4 = 3;
10032 }
10033 n4 = ltri[i4 + kt2 * 6];
10034
10035
10036
10037
10038
10039 if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10040 goto L5;
10041 }
10042
10043
10044
10045
10046 swp = TRUE_;
10047 kt11 = ltri[i1 + 3 + kt1 * 6];
10048 kt12 = ltri[i2 + 3 + kt1 * 6];
10049 if (i4 == 1) {
10050 i2 = 2;
10051 i1 = 3;
10052 } else if (i4 == 2) {
10053 i2 = 3;
10054 i1 = 1;
10055 } else {
10056 i2 = 1;
10057 i1 = 2;
10058 }
10059 kt21 = ltri[i1 + 3 + kt2 * 6];
10060 kt22 = ltri[i2 + 3 + kt2 * 6];
10061 ltri[kt1 * 6 + 1] = n4;
10062 ltri[kt1 * 6 + 2] = n3;
10063 ltri[kt1 * 6 + 3] = n1;
10064 ltri[kt1 * 6 + 4] = kt12;
10065 ltri[kt1 * 6 + 5] = kt22;
10066 ltri[kt1 * 6 + 6] = kt2;
10067 ltri[kt2 * 6 + 1] = n3;
10068 ltri[kt2 * 6 + 2] = n4;
10069 ltri[kt2 * 6 + 3] = n2;
10070 ltri[kt2 * 6 + 4] = kt21;
10071 ltri[kt2 * 6 + 5] = kt11;
10072 ltri[kt2 * 6 + 6] = kt1;
10073
10074
10075
10076 if (kt11 != 0) {
10077 i4 = 4;
10078 if (ltri[kt11 * 6 + 4] != kt1) {
10079 i4 = 5;
10080 if (ltri[kt11 * 6 + 5] != kt1) {
10081 i4 = 6;
10082 }
10083 }
10084 ltri[i4 + kt11 * 6] = kt2;
10085 }
10086 if (kt22 != 0) {
10087 i4 = 4;
10088 if (ltri[kt22 * 6 + 4] != kt2) {
10089 i4 = 5;
10090 if (ltri[kt22 * 6 + 5] != kt2) {
10091 i4 = 6;
10092 }
10093 }
10094 ltri[i4 + kt22 * 6] = kt1;
10095 }
10096 L5:
10097 ;
10098 }
10099
10100 }
10101 if (swp) {
10102 goto L4;
10103 }
10104
10105
10106
10107
10108 L7:
10109 i__1 = nt;
10110 for (kt = 1; kt <= i__1; ++kt) {
10111 n1 = ltri[kt * 6 + 1];
10112 n2 = ltri[kt * 6 + 2];
10113 n3 = ltri[kt * 6 + 3];
10114 v1[0] = x[n1];
10115 v1[1] = y[n1];
10116 v1[2] = z__[n1];
10117 v2[0] = x[n2];
10118 v2[1] = y[n2];
10119 v2[2] = z__[n2];
10120 v3[0] = x[n3];
10121 v3[1] = y[n3];
10122 v3[2] = z__[n3];
10123 circum_(v2, v1, v3, c__, &ierr);
10124 if (ierr != 0) {
10125 goto L23;
10126 }
10127
10128
10129
10130
10131 xc[kt] = -c__[0];
10132 yc[kt] = -c__[1];
10133 zc[kt] = -c__[2];
10134 t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10135 if (t < -1.) {
10136 t = -1.;
10137 }
10138 if (t > 1.) {
10139 t = 1.;
10140 }
10141 rc[kt] = acos(t);
10142
10143 }
10144
10145
10146
10147
10148
10149
10150 L9:
10151 kt = nt;
10152
10153
10154
10155 nm2 = nn - 2;
10156 i__1 = nm2;
10157 for (n1 = 1; n1 <= i__1; ++n1) {
10158 lpl = lend[n1];
10159 lp = lpl;
10160 n3 = list[lp];
10161
10162
10163
10164
10165 L10:
10166 lp = lptr[lp];
10167 n2 = n3;
10168 n3 = (i__2 = list[lp], abs(i__2));
10169 if (n2 <= n1 || n3 <= n1) {
10170 goto L11;
10171 }
10172 ++kt;
10173
10174
10175
10176 v1[0] = x[n1];
10177 v1[1] = y[n1];
10178 v1[2] = z__[n1];
10179 v2[0] = x[n2];
10180 v2[1] = y[n2];
10181 v2[2] = z__[n2];
10182 v3[0] = x[n3];
10183 v3[1] = y[n3];
10184 v3[2] = z__[n3];
10185 circum_(v1, v2, v3, c__, &ierr);
10186 if (ierr != 0) {
10187 goto L23;
10188 }
10189
10190
10191
10192 xc[kt] = c__[0];
10193 yc[kt] = c__[1];
10194 zc[kt] = c__[2];
10195 t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10196 if (t < -1.) {
10197 t = -1.;
10198 }
10199 if (t > 1.) {
10200 t = 1.;
10201 }
10202 rc[kt] = acos(t);
10203
10204
10205
10206
10207
10208 lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10209 listc[lpn] = kt;
10210 lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10211 listc[lpn] = kt;
10212 lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10213 listc[lpn] = kt;
10214 L11:
10215 if (lp != lpl) {
10216 goto L10;
10217 }
10218
10219 }
10220 if (nt == 0) {
10221 goto L20;
10222 }
10223
10224
10225
10226
10227
10228
10229 kt1 = 0;
10230 L13:
10231 ++kt1;
10232 if (ltri[kt1 * 6 + 4] == 0) {
10233 i1 = 2;
10234 i2 = 3;
10235 i3 = 1;
10236 goto L14;
10237 } else if (ltri[kt1 * 6 + 5] == 0) {
10238 i1 = 3;
10239 i2 = 1;
10240 i3 = 2;
10241 goto L14;
10242 } else if (ltri[kt1 * 6 + 6] == 0) {
10243 i1 = 1;
10244 i2 = 2;
10245 i3 = 3;
10246 goto L14;
10247 }
10248 goto L13;
10249 L14:
10250 n1 = ltri[i1 + kt1 * 6];
10251 n0 = n1;
10252
10253
10254
10255
10256
10257
10258
10259
10260
10261 L15:
10262 lp = lend[n1];
10263 lpn = lptr[lp];
10264 listc[lp] = kt1;
10265
10266
10267
10268 L16:
10269 kt2 = ltri[i2 + 3 + kt1 * 6];
10270 if (kt2 != 0) {
10271
10272
10273
10274 lptr[lp] = *lnew;
10275 lp = *lnew;
10276 listc[lp] = kt2;
10277 ++(*lnew);
10278
10279
10280
10281
10282 kt1 = kt2;
10283 if (ltri[kt1 * 6 + 1] == n1) {
10284 i1 = 1;
10285 i2 = 2;
10286 i3 = 3;
10287 } else if (ltri[kt1 * 6 + 2] == n1) {
10288 i1 = 2;
10289 i2 = 3;
10290 i3 = 1;
10291 } else {
10292 i1 = 3;
10293 i2 = 1;
10294 i3 = 2;
10295 }
10296 goto L16;
10297 }
10298
10299
10300
10301
10302
10303
10304
10305 lptr[lp] = lpn;
10306 n1 = ltri[i3 + kt1 * 6];
10307 if (n1 != n0) {
10308 i4 = i3;
10309 i3 = i2;
10310 i2 = i1;
10311 i1 = i4;
10312 goto L15;
10313 }
10314
10315
10316
10317 L20:
10318 *ier = 0;
10319 return 0;
10320
10321
10322
10323 L21:
10324 *ier = 1;
10325 return 0;
10326
10327
10328
10329 L22:
10330 *ier = 2;
10331 return 0;
10332
10333
10334
10335 L23:
10336 *ier = 3;
10337 return 0;
10338 }
10339
10340 int delarc_(int *n, int *io1, int *io2, int *
10341 list, int *lptr, int *lend, int *lnew, int *ier)
10342 {
10343
10344 int i__1;
10345
10346
10347 static int n1, n2, n3, lp, lph, lpl;
10348 extern int delnb_(int *, int *, int *,
10349 int *, int *, int *, int *, int *);
10350 extern int lstptr_(int *, int *, int *, int *);
10351
10352
10353
10354
10355
10356
10357
10358
10359
10360
10361
10362
10363
10364
10365
10366
10367
10368
10369
10370
10371
10372
10373
10374
10375
10376
10377
10378
10379
10380
10381
10382
10383
10384
10385
10386
10387
10388
10389
10390
10391
10392
10393
10394
10395
10396
10397
10398
10399
10400
10401
10402
10403
10404
10405
10406
10407
10408
10409
10410
10411
10412
10413
10414
10415
10416
10417
10418
10419
10420
10421
10422
10423
10424 --lend;
10425 --list;
10426 --lptr;
10427
10428
10429 n1 = *io1;
10430 n2 = *io2;
10431
10432
10433
10434
10435
10436 if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10437 *ier = 1;
10438 return 0;
10439 }
10440
10441 lpl = lend[n2];
10442 if (-list[lpl] != n1) {
10443 n1 = n2;
10444 n2 = *io1;
10445 lpl = lend[n2];
10446 if (-list[lpl] != n1) {
10447 *ier = 2;
10448 return 0;
10449 }
10450 }
10451
10452
10453
10454
10455
10456 lpl = lend[n1];
10457 lp = lptr[lpl];
10458 lp = lptr[lp];
10459 n3 = (i__1 = list[lp], abs(i__1));
10460 lpl = lend[n3];
10461 if (list[lpl] <= 0) {
10462 *ier = 3;
10463 return 0;
10464 }
10465
10466
10467
10468
10469
10470
10471 delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10472 if (lph < 0) {
10473 *ier = 4;
10474 return 0;
10475 }
10476
10477
10478
10479
10480 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10481
10482
10483
10484
10485 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10486 lend[n3] = lp;
10487 list[lp] = -n1;
10488
10489
10490
10491 *ier = 0;
10492 return 0;
10493 }
10494
10495 int delnb_(int *n0, int *nb, int *n, int *
10496 list, int *lptr, int *lend, int *lnew, int *lph)
10497 {
10498
10499 int i__1;
10500
10501
10502 static int i__, lp, nn, lpb, lpl, lpp, lnw;
10503
10504
10505
10506
10507
10508
10509
10510
10511
10512
10513
10514
10515
10516
10517
10518
10519
10520
10521
10522
10523
10524
10525
10526
10527
10528
10529
10530
10531
10532
10533
10534
10535
10536
10537
10538
10539
10540
10541
10542
10543
10544
10545
10546
10547
10548
10549
10550
10551
10552
10553
10554
10555
10556
10557
10558
10559
10560
10561
10562
10563
10564
10565
10566
10567
10568
10569
10570
10571
10572
10573
10574 --lend;
10575 --list;
10576 --lptr;
10577
10578
10579 nn = *n;
10580
10581
10582
10583 if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10584 *lph = -1;
10585 return 0;
10586 }
10587
10588
10589
10590
10591
10592
10593
10594 lpl = lend[*n0];
10595 lpp = lpl;
10596 lpb = lptr[lpp];
10597 L1:
10598 if (list[lpb] == *nb) {
10599 goto L2;
10600 }
10601 lpp = lpb;
10602 lpb = lptr[lpp];
10603 if (lpb != lpl) {
10604 goto L1;
10605 }
10606
10607
10608
10609 if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10610 *lph = -2;
10611 return 0;
10612 }
10613
10614
10615
10616
10617
10618 lend[*n0] = lpp;
10619 lp = lend[*nb];
10620 if (list[lp] < 0) {
10621 list[lpp] = -list[lpp];
10622 }
10623 goto L3;
10624
10625
10626
10627
10628
10629 L2:
10630 lp = lend[*nb];
10631 if (list[lp] < 0 && list[lpl] > 0) {
10632 lend[*n0] = lpp;
10633 list[lpp] = -list[lpp];
10634 }
10635
10636
10637
10638
10639 L3:
10640 lptr[lpp] = lptr[lpb];
10641 lnw = *lnew - 1;
10642 list[lpb] = list[lnw];
10643 lptr[lpb] = lptr[lnw];
10644 for (i__ = nn; i__ >= 1; --i__) {
10645 if (lend[i__] == lnw) {
10646 lend[i__] = lpb;
10647 goto L5;
10648 }
10649
10650 }
10651
10652 L5:
10653 i__1 = lnw - 1;
10654 for (i__ = 1; i__ <= i__1; ++i__) {
10655 if (lptr[i__] == lnw) {
10656 lptr[i__] = lpb;
10657 }
10658
10659 }
10660
10661
10662
10663 *lnew = lnw;
10664 *lph = lpb;
10665 return 0;
10666 }
10667
10668 int delnod_(int *k, int *n, double *x,
10669 double *y, double *z__, int *list, int *lptr, int
10670 *lend, int *lnew, int *lwk, int *iwk, int *ier)
10671 {
10672
10673 int i__1;
10674
10675
10676 static int i__, j, n1, n2;
10677 static double x1, x2, y1, y2, z1, z2;
10678 static int nl, lp, nn, nr;
10679 static double xl, yl, zl, xr, yr, zr;
10680 static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10681 extern long int left_(double *, double *, double *, double
10682 *, double *, double *, double *, double *,
10683 double *);
10684 static long int bdry;
10685 static int ierr, lwkl;
10686 extern int swap_(int *, int *, int *,
10687 int *, int *, int *, int *, int *), delnb_(
10688 int *, int *, int *, int *, int *, int *,
10689 int *, int *);
10690 extern int nbcnt_(int *, int *);
10691 extern int optim_(double *, double *, double
10692 *, int *, int *, int *, int *, int *, int
10693 *, int *);
10694 static int nfrst;
10695 extern int lstptr_(int *, int *, int *, int *);
10696
10697
10698
10699
10700
10701
10702
10703
10704
10705
10706
10707
10708
10709
10710
10711
10712
10713
10714
10715
10716
10717
10718
10719
10720
10721
10722
10723
10724
10725
10726
10727
10728
10729
10730
10731
10732
10733
10734
10735
10736
10737
10738
10739
10740
10741
10742
10743
10744
10745
10746
10747
10748
10749
10750
10751
10752
10753
10754
10755
10756
10757
10758
10759
10760
10761
10762
10763
10764
10765
10766
10767
10768
10769
10770
10771
10772
10773
10774
10775
10776
10777
10778
10779
10780
10781
10782
10783
10784
10785
10786
10787
10788
10789
10790
10791
10792
10793
10794
10795
10796
10797
10798
10799
10800
10801
10802
10803
10804
10805
10806
10807
10808
10809
10810
10811
10812
10813
10814
10815
10816
10817
10818
10819
10820
10821
10822
10823
10824
10825
10826
10827
10828
10829
10830
10831
10832
10833
10834
10835
10836
10837
10838
10839 iwk -= 3;
10840 --lend;
10841 --lptr;
10842 --list;
10843 --z__;
10844 --y;
10845 --x;
10846
10847
10848 n1 = *k;
10849 nn = *n;
10850 if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
10851 goto L21;
10852 }
10853 lpl = lend[n1];
10854 lpf = lptr[lpl];
10855 nnb = nbcnt_(&lpl, &lptr[1]);
10856 bdry = list[lpl] < 0;
10857 if (bdry) {
10858 ++nnb;
10859 }
10860 if (nnb < 3) {
10861 goto L23;
10862 }
10863 lwkl = *lwk;
10864 *lwk = nnb - 3;
10865 if (lwkl < *lwk) {
10866 goto L22;
10867 }
10868 iwl = 0;
10869 if (nnb == 3) {
10870 goto L3;
10871 }
10872
10873
10874
10875
10876
10877
10878
10879 x1 = x[n1];
10880 y1 = y[n1];
10881 z1 = z__[n1];
10882 nfrst = list[lpf];
10883 nr = nfrst;
10884 xr = x[nr];
10885 yr = y[nr];
10886 zr = z__[nr];
10887 lp = lptr[lpf];
10888 n2 = list[lp];
10889 x2 = x[n2];
10890 y2 = y[n2];
10891 z2 = z__[n2];
10892 lp = lptr[lp];
10893
10894
10895
10896 L1:
10897 nl = (i__1 = list[lp], abs(i__1));
10898 if (nl == nfrst && bdry) {
10899 goto L3;
10900 }
10901 xl = x[nl];
10902 yl = y[nl];
10903 zl = z__[nl];
10904
10905
10906
10907
10908
10909
10910 lpl2 = lend[n2];
10911 if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
10912 list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
10913 z2)))) {
10914
10915
10916
10917 nr = n2;
10918 xr = x2;
10919 yr = y2;
10920 zr = z2;
10921 goto L2;
10922 }
10923
10924
10925
10926
10927
10928
10929
10930
10931 swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
10932 if (lp21 == 0) {
10933 nr = n2;
10934 xr = x2;
10935 yr = y2;
10936 zr = z2;
10937 goto L2;
10938 }
10939 ++iwl;
10940 if (nl <= n1) {
10941 iwk[(iwl << 1) + 1] = nl;
10942 } else {
10943 iwk[(iwl << 1) + 1] = nl - 1;
10944 }
10945 if (nr <= n1) {
10946 iwk[(iwl << 1) + 2] = nr;
10947 } else {
10948 iwk[(iwl << 1) + 2] = nr - 1;
10949 }
10950
10951
10952
10953 lpl = lend[n1];
10954 --nnb;
10955 if (nnb == 3) {
10956 goto L3;
10957 }
10958 lpf = lptr[lpl];
10959 nfrst = list[lpf];
10960 lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
10961 if (nr == nfrst) {
10962 goto L2;
10963 }
10964
10965
10966
10967
10968
10969
10970
10971 n2 = nr;
10972 x2 = xr;
10973 y2 = yr;
10974 z2 = zr;
10975 lp21 = lptr[lp21];
10976 lp21 = lptr[lp21];
10977 nr = (i__1 = list[lp21], abs(i__1));
10978 xr = x[nr];
10979 yr = y[nr];
10980 zr = z__[nr];
10981 goto L1;
10982
10983
10984
10985 L2:
10986 if (n2 == nfrst) {
10987 goto L3;
10988 }
10989 n2 = nl;
10990 x2 = xl;
10991 y2 = yl;
10992 z2 = zl;
10993 lp = lptr[lp];
10994 goto L1;
10995
10996
10997
10998
10999
11000
11001
11002
11003
11004
11005 L3:
11006 if (! bdry) {
11007 if (nnb > 3) {
11008 bdry = TRUE_;
11009 } else {
11010 lpf = lptr[lpl];
11011 nr = list[lpf];
11012 lp = lptr[lpf];
11013 n2 = list[lp];
11014 nl = list[lpl];
11015 bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11016 x[n2], &y[n2], &z__[n2]);
11017 }
11018 if (bdry) {
11019
11020
11021
11022
11023
11024
11025 i__1 = nn;
11026 for (i__ = 1; i__ <= i__1; ++i__) {
11027 if (list[lend[i__]] < 0) {
11028 bdry = FALSE_;
11029 goto L5;
11030 }
11031
11032 }
11033 list[lpl] = -list[lpl];
11034 ++nnb;
11035 }
11036 }
11037 L5:
11038 if (! bdry && nnb > 3) {
11039 goto L24;
11040 }
11041
11042
11043
11044
11045 lp = lpl;
11046 lnw = *lnew;
11047
11048
11049
11050 L6:
11051 lp = lptr[lp];
11052 n2 = (i__1 = list[lp], abs(i__1));
11053 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11054 if (lph < 0) {
11055 goto L23;
11056 }
11057
11058
11059
11060 if (lpl == lnw) {
11061 lpl = lph;
11062 }
11063 if (lp == lnw) {
11064 lp = lph;
11065 }
11066 if (lp != lpl) {
11067 goto L6;
11068 }
11069
11070
11071
11072
11073
11074 --nn;
11075 if (n1 > nn) {
11076 goto L9;
11077 }
11078 i__1 = nn;
11079 for (i__ = n1; i__ <= i__1; ++i__) {
11080 x[i__] = x[i__ + 1];
11081 y[i__] = y[i__ + 1];
11082 z__[i__] = z__[i__ + 1];
11083 lend[i__] = lend[i__ + 1];
11084
11085 }
11086
11087 i__1 = lnw - 1;
11088 for (i__ = 1; i__ <= i__1; ++i__) {
11089 if (list[i__] > n1) {
11090 --list[i__];
11091 }
11092 if (list[i__] < -n1) {
11093 ++list[i__];
11094 }
11095
11096 }
11097
11098
11099
11100
11101
11102
11103
11104
11105
11106
11107
11108 L9:
11109 if (bdry) {
11110 --nnb;
11111 }
11112 lpn = lpl;
11113 i__1 = nnb;
11114 for (j = 1; j <= i__1; ++j) {
11115 --lnw;
11116 lp = lpn;
11117 lpn = lptr[lp];
11118 list[lp] = list[lnw];
11119 lptr[lp] = lptr[lnw];
11120 if (lptr[lpn] == lnw) {
11121 lptr[lpn] = lp;
11122 }
11123 if (lpn == lnw) {
11124 lpn = lp;
11125 }
11126 for (i__ = nn; i__ >= 1; --i__) {
11127 if (lend[i__] == lnw) {
11128 lend[i__] = lp;
11129 goto L11;
11130 }
11131
11132 }
11133
11134 L11:
11135 for (i__ = lnw - 1; i__ >= 1; --i__) {
11136 if (lptr[i__] == lnw) {
11137 lptr[i__] = lp;
11138 }
11139
11140 }
11141
11142 }
11143
11144
11145
11146
11147
11148 *n = nn;
11149 *lnew = lnw;
11150 if (iwl > 0) {
11151 nit = iwl << 2;
11152 optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11153 nit, &iwk[3], &ierr);
11154 if (ierr != 0 && ierr != 1) {
11155 goto L25;
11156 }
11157 if (ierr == 1) {
11158 goto L26;
11159 }
11160 }
11161
11162
11163
11164 *ier = 0;
11165 return 0;
11166
11167
11168
11169 L21:
11170 *ier = 1;
11171 return 0;
11172
11173
11174
11175 L22:
11176 *ier = 2;
11177 return 0;
11178
11179
11180
11181
11182 L23:
11183 *ier = 3;
11184 return 0;
11185
11186
11187
11188 L24:
11189 *ier = 4;
11190 return 0;
11191
11192
11193
11194 L25:
11195 *ier = 5;
11196
11197
11198
11199 return 0;
11200
11201
11202
11203 L26:
11204 *ier = 6;
11205 return 0;
11206 }
11207
11208 int drwarc_(int *, double *p, double *q,
11209 double *tol, int *nseg)
11210 {
11211
11212 int i__1;
11213 double d__1;
11214
11215
11216
11217
11218
11219 static int i__, k;
11220 static double s, p1[3], p2[3], u1, u2, v1, v2;
11221 static int na;
11222 static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11223
11224
11225
11226
11227
11228
11229
11230
11231
11232
11233
11234
11235
11236
11237
11238
11239
11240
11241
11242
11243
11244
11245
11246
11247
11248
11249
11250
11251
11252
11253
11254
11255
11256
11257
11258
11259
11260
11261
11262
11263
11264
11265
11266
11267
11268
11269
11270
11271
11272
11273
11274
11275
11276
11277
11278
11279
11280
11281
11282
11283
11284
11285
11286
11287
11288
11289
11290
11291
11292
11293
11294
11295
11296
11297
11298
11299
11300 --q;
11301 --p;
11302
11303
11304 enrm = 0.;
11305 for (i__ = 1; i__ <= 3; ++i__) {
11306 pm[i__ - 1] = p[i__] + q[i__];
11307 enrm += pm[i__ - 1] * pm[i__ - 1];
11308
11309 }
11310 if (enrm == 0.) {
11311 goto L5;
11312 }
11313 enrm = sqrt(enrm);
11314 pm[0] /= enrm;
11315 pm[1] /= enrm;
11316 pm[2] /= enrm;
11317
11318
11319
11320
11321 u1 = p[1];
11322 v1 = p[2];
11323 u2 = q[1];
11324 v2 = q[2];
11325 um = pm[0];
11326 vm = pm[1];
11327
11328
11329
11330
11331
11332
11333 du = u2 - u1;
11334 dv = v2 - v1;
11335 enrm = du * du + dv * dv;
11336 if (enrm == 0.) {
11337 goto L5;
11338 }
11339 err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11340
11341
11342
11343
11344
11345 na = (int) (err / *tol + 1.);
11346
11347
11348
11349
11350
11351 s = 1. / (double) na;
11352 for (i__ = 1; i__ <= 3; ++i__) {
11353 dp[i__ - 1] = s * (q[i__] - p[i__]);
11354 pm[i__ - 1] = p[i__];
11355 p1[i__ - 1] = p[i__];
11356
11357 }
11358
11359
11360
11361
11362 i__1 = na - 1;
11363 for (k = 1; k <= i__1; ++k) {
11364 enrm = 0.;
11365 for (i__ = 1; i__ <= 3; ++i__) {
11366 pm[i__ - 1] += dp[i__ - 1];
11367 enrm += pm[i__ - 1] * pm[i__ - 1];
11368
11369 }
11370 if (enrm == 0.) {
11371 goto L5;
11372 }
11373 enrm = sqrt(enrm);
11374 p2[0] = pm[0] / enrm;
11375 p2[1] = pm[1] / enrm;
11376 p2[2] = pm[2] / enrm;
11377
11378
11379 p1[0] = p2[0];
11380 p1[1] = p2[1];
11381 p1[2] = p2[2];
11382
11383 }
11384
11385
11386
11387
11388 *nseg = na;
11389 return 0;
11390
11391
11392
11393 L5:
11394 *nseg = 0;
11395 return 0;
11396 }
11397
11398 int edge_(int *in1, int *in2, double *x,
11399 double *y, double *z__, int *lwk, int *iwk, int *
11400 list, int *lptr, int *lend, int *ier)
11401 {
11402
11403 int i__1;
11404
11405
11406 static int i__, n0, n1, n2;
11407 static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11408 static int nl, lp, nr;
11409 static double dp12;
11410 static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11411 static double dp1l, dp2l, dp1r, dp2r;
11412 extern long int left_(double *, double *, double *, double
11413 *, double *, double *, double *, double *,
11414 double *);
11415 static int ierr;
11416 extern int swap_(int *, int *, int *,
11417 int *, int *, int *, int *, int *);
11418 static int next, iwcp1, n1lst, iwend;
11419 extern int optim_(double *, double *, double
11420 *, int *, int *, int *, int *, int *, int
11421 *, int *);
11422 static int n1frst;
11423
11424
11425
11426
11427
11428
11429
11430
11431
11432
11433
11434
11435
11436
11437
11438
11439
11440
11441
11442
11443
11444
11445
11446
11447
11448
11449
11450
11451
11452
11453
11454
11455
11456
11457
11458
11459
11460
11461
11462
11463
11464
11465
11466
11467
11468
11469
11470
11471
11472
11473
11474
11475
11476
11477
11478
11479
11480
11481
11482
11483
11484
11485
11486
11487
11488
11489
11490
11491
11492
11493
11494
11495
11496
11497
11498
11499
11500
11501
11502
11503
11504
11505
11506
11507
11508
11509
11510
11511
11512
11513
11514
11515
11516
11517
11518
11519
11520
11521
11522
11523
11524
11525
11526
11527
11528
11529
11530
11531
11532
11533
11534
11535
11536
11537
11538
11539
11540
11541
11542
11543
11544
11545
11546
11547
11548
11549
11550
11551
11552
11553
11554
11555
11556
11557
11558
11559
11560 --lend;
11561 --lptr;
11562 --list;
11563 iwk -= 3;
11564 --z__;
11565 --y;
11566 --x;
11567
11568
11569 n1 = *in1;
11570 n2 = *in2;
11571 iwend = *lwk;
11572 if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11573 goto L31;
11574 }
11575
11576
11577
11578
11579 lpl = lend[n1];
11580 n0 = (i__1 = list[lpl], abs(i__1));
11581 lp = lpl;
11582 L1:
11583 if (n0 == n2) {
11584 goto L30;
11585 }
11586 lp = lptr[lp];
11587 n0 = list[lp];
11588 if (lp != lpl) {
11589 goto L1;
11590 }
11591
11592
11593
11594 iwl = 0;
11595 nit = 0;
11596
11597
11598
11599 L2:
11600 x1 = x[n1];
11601 y1 = y[n1];
11602 z1 = z__[n1];
11603 x2 = x[n2];
11604 y2 = y[n2];
11605 z2 = z__[n2];
11606
11607
11608
11609
11610
11611
11612
11613
11614
11615
11616 lpl = lend[n1];
11617 n1lst = list[lpl];
11618 lp = lptr[lpl];
11619 n1frst = list[lp];
11620 nl = n1frst;
11621 if (n1lst < 0) {
11622 goto L4;
11623 }
11624
11625
11626
11627
11628 L3:
11629 if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11630 goto L4;
11631 }
11632 lp = lptr[lp];
11633 nl = list[lp];
11634 if (nl != n1frst) {
11635 goto L3;
11636 }
11637
11638
11639
11640 goto L5;
11641
11642
11643
11644
11645 L4:
11646 nr = nl;
11647 lp = lptr[lp];
11648 nl = (i__1 = list[lp], abs(i__1));
11649 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11650
11651
11652
11653
11654
11655 dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11656 dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11657 dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11658 dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11659 dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11660 if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11661 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11662 goto L6;
11663 }
11664
11665
11666
11667
11668
11669 if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11670 goto L5;
11671 }
11672 }
11673
11674
11675
11676 if (nl != n1frst) {
11677 goto L4;
11678 }
11679
11680
11681
11682
11683
11684
11685
11686 L5:
11687 if (nit > 0) {
11688 goto L33;
11689 }
11690 nit = 1;
11691 n1 = n2;
11692 n2 = *in1;
11693 goto L2;
11694
11695
11696
11697
11698 L6:
11699 ++iwl;
11700 if (iwl > iwend) {
11701 goto L32;
11702 }
11703 iwk[(iwl << 1) + 1] = nl;
11704 iwk[(iwl << 1) + 2] = nr;
11705
11706
11707
11708 lpl = lend[nl];
11709 lp = lptr[lpl];
11710
11711
11712
11713
11714 L7:
11715 if (list[lp] == nr) {
11716 goto L8;
11717 }
11718 lp = lptr[lp];
11719 if (lp != lpl) {
11720 goto L7;
11721 }
11722
11723
11724
11725
11726 if (list[lp] != nr) {
11727 goto L33;
11728 }
11729
11730
11731
11732
11733 L8:
11734 lp = lptr[lp];
11735 next = (i__1 = list[lp], abs(i__1));
11736 if (next == n2) {
11737 goto L9;
11738 }
11739
11740
11741
11742 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11743 nl = next;
11744 } else {
11745 nr = next;
11746 }
11747 goto L6;
11748
11749
11750
11751
11752 L9:
11753 *lwk = iwl;
11754 iwend = iwl;
11755
11756
11757
11758
11759
11760
11761
11762
11763 iwf = 1;
11764
11765
11766
11767
11768
11769 L10:
11770 lft = 0;
11771 n0 = n1;
11772 x0 = x1;
11773 y0 = y1;
11774 z0 = z1;
11775 nl = iwk[(iwf << 1) + 1];
11776 nr = iwk[(iwf << 1) + 2];
11777 iwc = iwf;
11778
11779
11780
11781
11782 L11:
11783 if (iwc == iwl) {
11784 goto L21;
11785 }
11786 iwcp1 = iwc + 1;
11787 next = iwk[(iwcp1 << 1) + 1];
11788 if (next != nl) {
11789 goto L16;
11790 }
11791 next = iwk[(iwcp1 << 1) + 2];
11792
11793
11794
11795
11796 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11797 z__[next])) {
11798 goto L14;
11799 }
11800 if (lft >= 0) {
11801 goto L12;
11802 }
11803 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11804 z__[next])) {
11805 goto L14;
11806 }
11807
11808
11809
11810 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11811 iwk[(iwc << 1) + 1] = n0;
11812 iwk[(iwc << 1) + 2] = next;
11813 goto L15;
11814
11815
11816
11817
11818
11819 L12:
11820 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11821 i__1 = iwl;
11822 for (i__ = iwcp1; i__ <= i__1; ++i__) {
11823 iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11824 iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11825
11826 }
11827 iwk[(iwl << 1) + 1] = n0;
11828 iwk[(iwl << 1) + 2] = next;
11829 --iwl;
11830 nr = next;
11831 goto L11;
11832
11833
11834
11835 L14:
11836 n0 = nr;
11837 x0 = x[n0];
11838 y0 = y[n0];
11839 z0 = z__[n0];
11840 lft = 1;
11841
11842
11843
11844 L15:
11845 nr = next;
11846 ++iwc;
11847 goto L11;
11848
11849
11850
11851
11852 L16:
11853 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11854 z__[next])) {
11855 goto L19;
11856 }
11857 if (lft <= 0) {
11858 goto L17;
11859 }
11860 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11861 z__[next])) {
11862 goto L19;
11863 }
11864
11865
11866
11867 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11868 iwk[(iwc << 1) + 1] = next;
11869 iwk[(iwc << 1) + 2] = n0;
11870 goto L20;
11871
11872
11873
11874
11875
11876 L17:
11877 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11878 i__1 = iwf;
11879 for (i__ = iwc - 1; i__ >= i__1; --i__) {
11880 iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11881 iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11882
11883 }
11884 iwk[(iwf << 1) + 1] = n0;
11885 iwk[(iwf << 1) + 2] = next;
11886 ++iwf;
11887 goto L20;
11888
11889
11890
11891 L19:
11892 n0 = nl;
11893 x0 = x[n0];
11894 y0 = y[n0];
11895 z0 = z__[n0];
11896 lft = -1;
11897
11898
11899
11900 L20:
11901 nl = next;
11902 ++iwc;
11903 goto L11;
11904
11905
11906
11907 L21:
11908 if (n0 == n1) {
11909 goto L24;
11910 }
11911 if (lft < 0) {
11912 goto L22;
11913 }
11914
11915
11916
11917 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
11918 goto L10;
11919 }
11920
11921
11922
11923
11924 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11925 iwk[(iwl << 1) + 1] = n0;
11926 iwk[(iwl << 1) + 2] = n2;
11927 --iwl;
11928 goto L10;
11929
11930
11931
11932 L22:
11933 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
11934 goto L10;
11935 }
11936
11937
11938
11939
11940 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11941 i__ = iwl;
11942 L23:
11943 iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
11944 iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
11945 --i__;
11946 if (i__ > iwf) {
11947 goto L23;
11948 }
11949 iwk[(iwf << 1) + 1] = n0;
11950 iwk[(iwf << 1) + 2] = n2;
11951 ++iwf;
11952 goto L10;
11953
11954
11955
11956
11957 L24:
11958 swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11959 iwk[(iwc << 1) + 1] = 0;
11960 iwk[(iwc << 1) + 2] = 0;
11961
11962
11963
11964 *ier = 0;
11965 if (iwc > 1) {
11966
11967
11968
11969 nit = iwc - (1<<2);
11970 i__1 = iwc - 1;
11971 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
11972 nit, &iwk[3], &ierr);
11973 if (ierr != 0 && ierr != 1) {
11974 goto L34;
11975 }
11976 if (ierr == 1) {
11977 *ier = 5;
11978 }
11979 }
11980 if (iwc < iwend) {
11981
11982
11983
11984 nit = iwend - (iwc<<2);
11985 i__1 = iwend - iwc;
11986 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
11987 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
11988 if (ierr != 0 && ierr != 1) {
11989 goto L34;
11990 }
11991 if (ierr == 1) {
11992 goto L35;
11993 }
11994 }
11995 if (*ier == 5) {
11996 goto L35;
11997 }
11998
11999
12000
12001 return 0;
12002
12003
12004
12005 L30:
12006 *ier = 0;
12007 return 0;
12008
12009
12010
12011 L31:
12012 *ier = 1;
12013 return 0;
12014
12015
12016
12017 L32:
12018 *ier = 2;
12019 return 0;
12020
12021
12022
12023
12024 L33:
12025 *ier = 3;
12026
12027
12028
12029
12030 return 0;
12031
12032
12033
12034 L34:
12035 *ier = 4;
12036
12037
12038
12039 return 0;
12040
12041
12042
12043 L35:
12044 *ier = 5;
12045 return 0;
12046 }
12047
12048 int getnp_(double *x, double *y, double *z__,
12049 int *list, int *lptr, int *lend, int *l, int *
12050 npts, double *df, int *ier)
12051 {
12052
12053 int i__1, i__2;
12054
12055
12056 static int i__, n1;
12057 static double x1, y1, z1;
12058 static int nb, ni, lp, np, lm1;
12059 static double dnb, dnp;
12060 static int lpl;
12061
12062
12063
12064
12065
12066
12067
12068
12069
12070
12071
12072
12073
12074
12075
12076
12077
12078
12079
12080
12081
12082
12083
12084
12085
12086
12087
12088
12089
12090
12091
12092
12093
12094
12095
12096
12097
12098
12099
12100
12101
12102
12103
12104
12105
12106
12107
12108
12109
12110
12111
12112
12113
12114
12115
12116
12117
12118
12119
12120
12121
12122
12123
12124
12125
12126
12127
12128
12129
12130
12131
12132
12133
12134
12135
12136
12137
12138
12139
12140 --x;
12141 --y;
12142 --z__;
12143 --list;
12144 --lptr;
12145 --lend;
12146 --npts;
12147
12148
12149 lm1 = *l - 1;
12150 if (lm1 < 1) {
12151 goto L6;
12152 }
12153 *ier = 0;
12154
12155
12156
12157 n1 = npts[1];
12158 x1 = x[n1];
12159 y1 = y[n1];
12160 z1 = z__[n1];
12161 i__1 = lm1;
12162 for (i__ = 1; i__ <= i__1; ++i__) {
12163 ni = npts[i__];
12164 lend[ni] = -lend[ni];
12165
12166 }
12167
12168
12169
12170
12171
12172 dnp = 2.;
12173
12174
12175
12176 i__1 = lm1;
12177 for (i__ = 1; i__ <= i__1; ++i__) {
12178 ni = npts[i__];
12179 lpl = -lend[ni];
12180 lp = lpl;
12181
12182
12183
12184 L2:
12185 nb = (i__2 = list[lp], abs(i__2));
12186 if (lend[nb] < 0) {
12187 goto L3;
12188 }
12189
12190
12191
12192
12193 dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12194 if (dnb >= dnp) {
12195 goto L3;
12196 }
12197 np = nb;
12198 dnp = dnb;
12199 L3:
12200 lp = lptr[lp];
12201 if (lp != lpl) {
12202 goto L2;
12203 }
12204
12205 }
12206 npts[*l] = np;
12207 *df = dnp;
12208
12209
12210
12211 i__1 = lm1;
12212 for (i__ = 1; i__ <= i__1; ++i__) {
12213 ni = npts[i__];
12214 lend[ni] = -lend[ni];
12215
12216 }
12217 return 0;
12218
12219
12220
12221 L6:
12222 *ier = 1;
12223 return 0;
12224 }
12225
12226 int insert_(int *k, int *lp, int *list, int *
12227 lptr, int *lnew)
12228 {
12229 static int lsav;
12230
12231
12232
12233
12234
12235
12236
12237
12238
12239
12240
12241
12242
12243
12244
12245
12246
12247
12248
12249
12250
12251
12252
12253
12254
12255
12256
12257
12258
12259
12260
12261
12262
12263
12264
12265
12266
12267
12268
12269
12270
12271
12272
12273 --lptr;
12274 --list;
12275
12276
12277 lsav = lptr[*lp];
12278 lptr[*lp] = *lnew;
12279 list[*lnew] = *k;
12280 lptr[*lnew] = lsav;
12281 ++(*lnew);
12282 return 0;
12283 }
12284
12285 long int inside_(double *p, int *lv, double *xv, double *yv,
12286 double *zv, int *nv, int *listv, int *ier)
12287 {
12288
12289
12290 static double eps = .001;
12291
12292
12293 int i__1;
12294 long int ret_val = 0;
12295
12296
12297
12298
12299
12300 static double b[3], d__;
12301 static int k, n;
12302 static double q[3];
12303 static int i1, i2, k0;
12304 static double v1[3], v2[3], cn[3], bp, bq;
12305 static int ni;
12306 static double pn[3], qn[3], vn[3];
12307 static int imx;
12308 static long int lft1, lft2, even;
12309 static int ierr;
12310 static long int pinr, qinr;
12311 static double qnrm, vnrm;
12312 extern int intrsc_(double *, double *,
12313 double *, double *, int *);
12314
12315
12316
12317
12318
12319
12320
12321
12322
12323
12324
12325
12326
12327
12328
12329
12330
12331
12332
12333
12334
12335
12336
12337
12338
12339
12340
12341
12342
12343
12344
12345
12346
12347
12348
12349
12350
12351
12352
12353
12354
12355
12356
12357
12358
12359
12360
12361
12362
12363
12364
12365
12366
12367
12368
12369
12370
12371
12372
12373
12374
12375
12376
12377
12378
12379
12380
12381
12382
12383
12384
12385
12386
12387
12388
12389
12390
12391
12392
12393
12394
12395
12396
12397
12398
12399
12400
12401
12402
12403
12404
12405
12406
12407
12408
12409
12410
12411
12412
12413
12414
12415
12416
12417
12418
12419
12420
12421
12422
12423
12424
12425
12426
12427
12428
12429
12430
12431
12432
12433
12434
12435
12436
12437
12438
12439
12440
12441
12442
12443
12444
12445
12446
12447
12448
12449
12450
12451
12452
12453
12454
12455
12456
12457
12458
12459
12460
12461
12462
12463
12464
12465
12466 --p;
12467 --zv;
12468 --yv;
12469 --xv;
12470 --listv;
12471
12472
12473
12474
12475
12476
12477 imx = *lv;
12478 n = *nv;
12479 if (n < 3 || n > imx) {
12480 goto L11;
12481 }
12482 k0 = 0;
12483 i1 = listv[1];
12484 if (i1 < 1 || i1 > imx) {
12485 goto L12;
12486 }
12487
12488
12489
12490
12491
12492 L1:
12493 ++k0;
12494 if (k0 > n) {
12495 goto L14;
12496 }
12497 i1 = listv[k0];
12498 if (k0 < n) {
12499 i2 = listv[k0 + 1];
12500 } else {
12501 i2 = listv[1];
12502 }
12503 if (i2 < 1 || i2 > imx) {
12504 goto L12;
12505 }
12506 vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12507 vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12508 vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12509 vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12510 if (vnrm == 0.) {
12511 goto L1;
12512 }
12513 q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12514 q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12515 q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12516 qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12517 q[0] /= qnrm;
12518 q[1] /= qnrm;
12519 q[2] /= qnrm;
12520
12521
12522
12523 cn[0] = q[1] * p[3] - q[2] * p[2];
12524 cn[1] = q[2] * p[1] - q[0] * p[3];
12525 cn[2] = q[0] * p[2] - q[1] * p[1];
12526 if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12527 goto L1;
12528 }
12529 pn[0] = p[2] * cn[2] - p[3] * cn[1];
12530 pn[1] = p[3] * cn[0] - p[1] * cn[2];
12531 pn[2] = p[1] * cn[1] - p[2] * cn[0];
12532 qn[0] = cn[1] * q[2] - cn[2] * q[1];
12533 qn[1] = cn[2] * q[0] - cn[0] * q[2];
12534 qn[2] = cn[0] * q[1] - cn[1] * q[0];
12535
12536
12537
12538 ni = 0;
12539 even = TRUE_;
12540 bp = -2.;
12541 bq = -2.;
12542 pinr = TRUE_;
12543 qinr = TRUE_;
12544 i2 = listv[n];
12545 if (i2 < 1 || i2 > imx) {
12546 goto L12;
12547 }
12548 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12549
12550
12551
12552 i__1 = n;
12553 for (k = 1; k <= i__1; ++k) {
12554 i1 = i2;
12555 lft1 = lft2;
12556 i2 = listv[k];
12557 if (i2 < 1 || i2 > imx) {
12558 goto L12;
12559 }
12560 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12561 if (lft1 == lft2) {
12562 goto L2;
12563 }
12564
12565
12566
12567
12568 ++ni;
12569 v1[0] = xv[i1];
12570 v1[1] = yv[i1];
12571 v1[2] = zv[i1];
12572 v2[0] = xv[i2];
12573 v2[1] = yv[i2];
12574 v2[2] = zv[i2];
12575 intrsc_(v1, v2, cn, b, &ierr);
12576
12577
12578
12579
12580
12581 if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12582 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12583
12584
12585
12586 even = ! even;
12587 d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12588 if (d__ > bq) {
12589 bq = d__;
12590 qinr = lft2;
12591 }
12592 d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12593 if (d__ > bp) {
12594 bp = d__;
12595 pinr = lft1;
12596 }
12597 }
12598 L2:
12599 ;
12600 }
12601
12602
12603
12604
12605 if (ni != ni / 2 << 1 || ! qinr) {
12606 goto L1;
12607 }
12608
12609
12610
12611 if (pinr != even) {
12612 goto L13;
12613 }
12614
12615
12616
12617 *ier = 0;
12618 ret_val = even;
12619 return ret_val;
12620
12621
12622
12623 L11:
12624 *ier = 1;
12625 return ret_val;
12626
12627
12628
12629 L12:
12630 *ier = 2;
12631 return ret_val;
12632
12633
12634
12635 L13:
12636 *ier = 3;
12637 return ret_val;
12638
12639
12640
12641 L14:
12642 *ier = 4;
12643 return ret_val;
12644 }
12645
12646 int intadd_(int *kk, int *i1, int *i2, int *
12647 i3, int *list, int *lptr, int *lend, int *lnew)
12648 {
12649 static int k, n1, n2, n3, lp;
12650 extern int insert_(int *, int *, int *,
12651 int *, int *);
12652 extern int lstptr_(int *, int *, int *, int *);
12653
12654
12655
12656
12657
12658
12659
12660
12661
12662
12663
12664
12665
12666
12667
12668
12669
12670
12671
12672
12673
12674
12675
12676
12677
12678
12679
12680
12681
12682
12683
12684
12685
12686
12687
12688
12689
12690
12691
12692
12693
12694
12695
12696
12697
12698
12699
12700
12701
12702
12703
12704
12705
12706
12707
12708
12709
12710 --lend;
12711 --lptr;
12712 --list;
12713
12714
12715 k = *kk;
12716
12717
12718
12719 n1 = *i1;
12720 n2 = *i2;
12721 n3 = *i3;
12722
12723
12724
12725 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12726 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12727 lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12728 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12729 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12730 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12731
12732
12733
12734 list[*lnew] = n1;
12735 list[*lnew + 1] = n2;
12736 list[*lnew + 2] = n3;
12737 lptr[*lnew] = *lnew + 1;
12738 lptr[*lnew + 1] = *lnew + 2;
12739 lptr[*lnew + 2] = *lnew;
12740 lend[k] = *lnew + 2;
12741 *lnew += 3;
12742 return 0;
12743 }
12744
12745 int intrsc_(double *p1, double *p2, double *cn,
12746 double *p, int *ier)
12747 {
12748
12749
12750
12751
12752 static int i__;
12753 static double t, d1, d2, pp[3], ppn;
12754
12755
12756
12757
12758
12759
12760
12761
12762
12763
12764
12765
12766
12767
12768
12769
12770
12771
12772
12773
12774
12775
12776
12777
12778
12779
12780
12781
12782
12783
12784
12785
12786
12787
12788
12789
12790
12791
12792
12793
12794
12795
12796
12797
12798
12799
12800
12801
12802
12803
12804
12805
12806
12807
12808
12809
12810
12811
12812
12813
12814
12815
12816
12817
12818
12819
12820
12821
12822
12823 --p;
12824 --cn;
12825 --p2;
12826 --p1;
12827
12828
12829 d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
12830 d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
12831
12832 if (d1 == d2) {
12833 *ier = 1;
12834 return 0;
12835 }
12836
12837
12838
12839 t = d1 / (d1 - d2);
12840 ppn = 0.;
12841 for (i__ = 1; i__ <= 3; ++i__) {
12842 pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
12843 ppn += pp[i__ - 1] * pp[i__ - 1];
12844
12845 }
12846
12847
12848
12849 if (ppn == 0.) {
12850 *ier = 2;
12851 return 0;
12852 }
12853 ppn = sqrt(ppn);
12854
12855
12856
12857 for (i__ = 1; i__ <= 3; ++i__) {
12858 p[i__] = pp[i__ - 1] / ppn;
12859
12860 }
12861 *ier = 0;
12862 return 0;
12863 }
12864
12865 int jrand_(int *n, int *ix, int *iy, int *iz)
12866 {
12867
12868 int ret_val;
12869
12870
12871 static float u, x;
12872
12873
12874
12875
12876
12877
12878
12879
12880
12881
12882
12883
12884
12885
12886
12887
12888
12889
12890
12891
12892
12893
12894
12895
12896
12897
12898
12899
12900
12901
12902
12903
12904
12905
12906
12907
12908
12909
12910
12911
12912
12913
12914
12915
12916
12917
12918
12919
12920
12921
12922
12923
12924
12925 *ix = *ix * 171 % 30269;
12926 *iy = *iy * 172 % 30307;
12927 *iz = *iz * 170 % 30323;
12928 x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
12929 30323.f;
12930 u = x - (int) x;
12931 ret_val = (int) ((float) (*n) * u + 1.f);
12932 return ret_val;
12933 }
12934
12935 long int left_(double *x1, double *y1, double *z1, double *x2,
12936 double *y2, double *z2, double *x0, double *y0,
12937 double *z0)
12938 {
12939
12940 long int ret_val;
12941
12942
12943
12944
12945
12946
12947
12948
12949
12950
12951
12952
12953
12954
12955
12956
12957
12958
12959
12960
12961
12962
12963
12964
12965
12966
12967
12968
12969
12970
12971
12972
12973
12974
12975
12976
12977
12978
12979 ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
12980 *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
12981
12982
12983 return ret_val;
12984 }
12985
12986 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
12987 {
12988
12989 int ret_val;
12990
12991
12992 static int nd, lp;
12993
12994
12995
12996
12997
12998
12999
13000
13001
13002
13003
13004
13005
13006
13007
13008
13009
13010
13011
13012
13013
13014
13015
13016
13017
13018
13019
13020
13021
13022
13023
13024
13025
13026
13027
13028
13029
13030
13031
13032
13033
13034
13035
13036
13037
13038
13039
13040 --lptr;
13041 --list;
13042
13043
13044 lp = lptr[*lpl];
13045 L1:
13046 nd = list[lp];
13047 if (nd == *nb) {
13048 goto L2;
13049 }
13050 lp = lptr[lp];
13051 if (lp != *lpl) {
13052 goto L1;
13053 }
13054
13055 L2:
13056 ret_val = lp;
13057 return ret_val;
13058 }
13059
13060 int nbcnt_(int *lpl, int *lptr)
13061 {
13062
13063 int ret_val;
13064
13065
13066 static int k, lp;
13067
13068
13069
13070
13071
13072
13073
13074
13075
13076
13077
13078
13079
13080
13081
13082
13083
13084
13085
13086
13087
13088
13089
13090
13091
13092
13093
13094
13095
13096
13097
13098
13099
13100
13101
13102
13103
13104
13105
13106
13107
13108
13109 --lptr;
13110
13111
13112 lp = *lpl;
13113 k = 1;
13114
13115 L1:
13116 lp = lptr[lp];
13117 if (lp == *lpl) {
13118 goto L2;
13119 }
13120 ++k;
13121 goto L1;
13122
13123 L2:
13124 ret_val = k;
13125 return ret_val;
13126 }
13127
13128 int nearnd_(double *p, int *ist, int *n, double *x,
13129 double *y, double *z__, int *list, int *lptr, int
13130 *lend, double *al)
13131 {
13132
13133 int ret_val, i__1;
13134
13135
13136
13137
13138
13139 static int l;
13140 static double b1, b2, b3;
13141 static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13142 static double ds1;
13143 static int lp1, lp2;
13144 static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13145 static int lpl;
13146 static double dsr;
13147 static int nst, listp[25], lptrp[25];
13148 extern int trfind_(int *, double *, int *,
13149 double *, double *, double *, int *, int *,
13150 int *, double *, double *, double *, int *,
13151 int *, int *);
13152 extern int lstptr_(int *, int *, int *, int *);
13153
13154
13155
13156
13157
13158
13159
13160
13161
13162
13163
13164
13165
13166
13167
13168
13169
13170
13171
13172
13173
13174
13175
13176
13177
13178
13179
13180
13181
13182
13183
13184
13185
13186
13187
13188
13189
13190
13191
13192
13193
13194
13195
13196
13197
13198
13199
13200
13201
13202
13203
13204
13205
13206
13207
13208
13209
13210
13211
13212
13213
13214
13215
13216
13217
13218
13219
13220
13221
13222
13223
13224
13225
13226
13227
13228
13229
13230
13231
13232
13233
13234
13235
13236
13237
13238
13239
13240
13241
13242
13243
13244
13245
13246
13247
13248
13249 --p;
13250 --lend;
13251 --z__;
13252 --y;
13253 --x;
13254 --list;
13255 --lptr;
13256
13257
13258 nn = *n;
13259 if (nn < 3) {
13260 goto L6;
13261 }
13262 nst = *ist;
13263 if (nst < 1 || nst > nn) {
13264 nst = 1;
13265 }
13266
13267
13268
13269
13270
13271 trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13272 1], &b1, &b2, &b3, &i1, &i2, &i3);
13273
13274
13275
13276 if (i1 == 0) {
13277 goto L6;
13278 }
13279
13280
13281
13282
13283
13284
13285
13286 if (i3 != 0) {
13287 listp[0] = i1;
13288 lptrp[0] = 2;
13289 listp[1] = i2;
13290 lptrp[1] = 3;
13291 listp[2] = i3;
13292 lptrp[2] = 1;
13293 l = 3;
13294 } else {
13295 n1 = i1;
13296 l = 1;
13297 lp1 = 2;
13298 listp[l - 1] = n1;
13299 lptrp[l - 1] = lp1;
13300
13301
13302
13303
13304 L1:
13305 lpl = lend[n1];
13306 n1 = -list[lpl];
13307 l = lp1;
13308 lp1 = l + 1;
13309 listp[l - 1] = n1;
13310 lptrp[l - 1] = lp1;
13311 if (n1 != i2 && lp1 < 25) {
13312 goto L1;
13313 }
13314 l = lp1;
13315 listp[l - 1] = 0;
13316 lptrp[l - 1] = 1;
13317 }
13318
13319
13320
13321
13322
13323
13324 lp2 = 1;
13325 n2 = i1;
13326 lp1 = lptrp[0];
13327 n1 = listp[lp1 - 1];
13328
13329
13330
13331 L2:
13332 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13333 if (list[lp] < 0) {
13334 goto L3;
13335 }
13336 lp = lptr[lp];
13337 n3 = (i__1 = list[lp], abs(i__1));
13338
13339
13340
13341 if (l == 25) {
13342 goto L4;
13343 }
13344 dx1 = x[n1] - p[1];
13345 dy1 = y[n1] - p[2];
13346 dz1 = z__[n1] - p[3];
13347
13348 dx2 = x[n2] - p[1];
13349 dy2 = y[n2] - p[2];
13350 dz2 = z__[n2] - p[3];
13351
13352 dx3 = x[n3] - p[1];
13353 dy3 = y[n3] - p[2];
13354 dz3 = z__[n3] - p[3];
13355 if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13356 (dx2 * dy1 - dx1 * dy2) <= 0.) {
13357 goto L3;
13358 }
13359
13360
13361
13362
13363 ++l;
13364 lptrp[lp2 - 1] = l;
13365 listp[l - 1] = n3;
13366 lptrp[l - 1] = lp1;
13367 lp1 = l;
13368 n1 = n3;
13369 goto L2;
13370
13371
13372
13373
13374 L3:
13375 if (lp1 == 1) {
13376 goto L4;
13377 }
13378 lp2 = lp1;
13379 n2 = n1;
13380 lp1 = lptrp[lp1 - 1];
13381 n1 = listp[lp1 - 1];
13382 if (n1 == 0) {
13383 goto L4;
13384 }
13385 goto L2;
13386
13387
13388
13389
13390
13391 L4:
13392 nr = i1;
13393 dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13394 i__1 = l;
13395 for (lp = 2; lp <= i__1; ++lp) {
13396 n1 = listp[lp - 1];
13397 if (n1 == 0) {
13398 goto L5;
13399 }
13400 ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13401 if (ds1 < dsr) {
13402 nr = n1;
13403 dsr = ds1;
13404 }
13405 L5:
13406 ;
13407 }
13408 dsr = -dsr;
13409 if (dsr > 1.) {
13410 dsr = 1.;
13411 }
13412 *al = acos(dsr);
13413 ret_val = nr;
13414 return ret_val;
13415
13416
13417
13418 L6:
13419 ret_val = 0;
13420 return ret_val;
13421 }
13422
13423 int optim_(double *x, double *y, double *z__,
13424 int *na, int *list, int *lptr, int *lend, int *
13425 nit, int *iwk, int *ier)
13426 {
13427
13428 int i__1, i__2;
13429
13430
13431 static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13432 static long int swp;
13433 static int iter;
13434 extern int swap_(int *, int *, int *,
13435 int *, int *, int *, int *, int *);
13436 static int maxit;
13437 extern long int swptst_(int *, int *, int *, int *,
13438 double *, double *, double *);
13439
13440
13441
13442
13443
13444
13445
13446
13447
13448
13449
13450
13451
13452
13453
13454
13455
13456
13457
13458
13459
13460
13461
13462
13463
13464
13465
13466
13467
13468
13469
13470
13471
13472
13473
13474
13475
13476
13477
13478
13479
13480
13481
13482
13483
13484
13485
13486
13487
13488
13489
13490
13491
13492
13493
13494
13495
13496
13497
13498
13499
13500
13501
13502
13503
13504
13505
13506
13507
13508
13509
13510
13511
13512
13513
13514
13515
13516
13517
13518
13519
13520
13521
13522
13523
13524
13525
13526
13527
13528
13529
13530
13531
13532
13533
13534
13535 --x;
13536 --y;
13537 --z__;
13538 iwk -= 3;
13539 --list;
13540 --lptr;
13541 --lend;
13542
13543
13544 nna = *na;
13545 maxit = *nit;
13546 if (nna < 0 || maxit < 1) {
13547 goto L7;
13548 }
13549
13550
13551
13552 iter = 0;
13553 if (nna == 0) {
13554 goto L5;
13555 }
13556
13557
13558
13559
13560 L1:
13561 if (iter == maxit) {
13562 goto L6;
13563 }
13564 ++iter;
13565 swp = FALSE_;
13566
13567
13568
13569 i__1 = nna;
13570 for (i__ = 1; i__ <= i__1; ++i__) {
13571 io1 = iwk[(i__ << 1) + 1];
13572 io2 = iwk[(i__ << 1) + 2];
13573
13574
13575
13576
13577
13578
13579
13580
13581 lpl = lend[io1];
13582 lpp = lpl;
13583 lp = lptr[lpp];
13584 L2:
13585 if (list[lp] == io2) {
13586 goto L3;
13587 }
13588 lpp = lp;
13589 lp = lptr[lpp];
13590 if (lp != lpl) {
13591 goto L2;
13592 }
13593
13594
13595
13596
13597
13598 if ((i__2 = list[lp], abs(i__2)) != io2) {
13599 goto L8;
13600 }
13601 if (list[lp] < 0) {
13602 goto L4;
13603 }
13604
13605
13606
13607
13608 L3:
13609 n2 = list[lpp];
13610 if (n2 < 0) {
13611 goto L4;
13612 }
13613 lp = lptr[lp];
13614 n1 = (i__2 = list[lp], abs(i__2));
13615
13616
13617
13618 if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13619 goto L4;
13620 }
13621 swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13622 if (lp21 == 0) {
13623 goto L9;
13624 }
13625 swp = TRUE_;
13626 iwk[(i__ << 1) + 1] = n1;
13627 iwk[(i__ << 1) + 2] = n2;
13628 L4:
13629 ;
13630 }
13631 if (swp) {
13632 goto L1;
13633 }
13634
13635
13636
13637 L5:
13638 *nit = iter;
13639 *ier = 0;
13640 return 0;
13641
13642
13643
13644 L6:
13645 *nit = maxit;
13646 *ier = 1;
13647 return 0;
13648
13649
13650
13651 L7:
13652 *nit = 0;
13653 *ier = 2;
13654 return 0;
13655
13656
13657
13658 L8:
13659 *nit = iter;
13660 *ier = 3;
13661 return 0;
13662
13663
13664
13665 L9:
13666 *nit = iter;
13667 *ier = 4;
13668 return 0;
13669 }
13670
13671 int projct_(double *px, double *py, double *pz,
13672 double *ox, double *oy, double *oz, double *ex,
13673 double *ey, double *ez, double *vx, double *vy,
13674 double *vz, long int *init, double *x, double *y,
13675 double *z__, int *ier)
13676 {
13677
13678
13679
13680
13681 static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13682 oes, xoe, yoe, zoe, xep, yep, zep;
13683
13684
13685
13686
13687
13688
13689
13690
13691
13692
13693
13694
13695
13696
13697
13698
13699
13700
13701
13702
13703
13704
13705
13706
13707
13708
13709
13710
13711
13712
13713
13714
13715
13716
13717
13718
13719
13720
13721
13722
13723
13724
13725
13726
13727
13728
13729
13730
13731
13732
13733
13734
13735
13736
13737
13738
13739
13740
13741
13742
13743
13744
13745
13746
13747
13748
13749
13750
13751
13752
13753
13754
13755
13756
13757
13758
13759
13760
13761
13762
13763
13764
13765
13766
13767
13768
13769
13770
13771
13772
13773
13774
13775
13776
13777
13778
13779
13780
13781
13782
13783
13784
13785
13786
13787
13788
13789
13790
13791
13792
13793
13794
13795
13796
13797 if (*init) {
13798
13799
13800
13801
13802
13803
13804
13805
13806 xe = *ex;
13807 ye = *ey;
13808 ze = *ez;
13809 xoe = xe - *ox;
13810 yoe = ye - *oy;
13811 zoe = ze - *oz;
13812 oes = xoe * xoe + yoe * yoe + zoe * zoe;
13813 if (oes == 0.) {
13814 goto L1;
13815 }
13816
13817
13818
13819 s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
13820 xv = *vx - s * xoe;
13821 yv = *vy - s * yoe;
13822 zv = *vz - s * zoe;
13823
13824
13825
13826 sc = xv * xv + yv * yv + zv * zv;
13827 if (sc == 0.) {
13828 goto L2;
13829 }
13830 sc = 1. / sqrt(sc);
13831 xv = sc * xv;
13832 yv = sc * yv;
13833 zv = sc * zv;
13834
13835
13836
13837 xh = yv * zoe - yoe * zv;
13838 yh = xoe * zv - xv * zoe;
13839 zh = xv * yoe - xoe * yv;
13840 sc = sqrt(xh * xh + yh * yh + zh * zh);
13841 if (sc == 0.) {
13842 goto L2;
13843 }
13844 sc = 1. / sc;
13845 xh = sc * xh;
13846 yh = sc * yh;
13847 zh = sc * zh;
13848 }
13849
13850
13851
13852
13853
13854
13855 xep = *px - xe;
13856 yep = *py - ye;
13857 zep = *pz - ze;
13858 s = xoe * xep + yoe * yep + zoe * zep;
13859 if (s >= 0.) {
13860 goto L1;
13861 }
13862 s = oes / s;
13863 xw = xoe - s * xep;
13864 yw = yoe - s * yep;
13865 zw = zoe - s * zep;
13866
13867
13868
13869
13870 *x = xw * xh + yw * yh + zw * zh;
13871 *y = xw * xv + yw * yv + zw * zv;
13872 *z__ = s + 1.;
13873 *init = FALSE_;
13874 *ier = 0;
13875 return 0;
13876
13877
13878
13879 L1:
13880 *ier = 1;
13881 return 0;
13882
13883
13884
13885 L2:
13886 *ier = 2;
13887 return 0;
13888 }
13889
13890 int scoord_(double *px, double *py, double *pz,
13891 double *plat, double *plon, double *pnrm)
13892 {
13893
13894
13895
13896
13897
13898
13899
13900
13901
13902
13903
13904
13905
13906
13907
13908
13909
13910
13911
13912
13913
13914
13915
13916
13917
13918
13919
13920
13921
13922
13923
13924
13925
13926
13927
13928
13929
13930
13931
13932
13933
13934
13935 *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
13936 if (*px != 0. || *py != 0.) {
13937 *plon = atan2(*py, *px);
13938 } else {
13939 *plon = 0.;
13940 }
13941 if (*pnrm != 0.) {
13942 *plat = asin(*pz / *pnrm);
13943 } else {
13944 *plat = 0.;
13945 }
13946 return 0;
13947 }
13948
13949 double store_(double *x)
13950 {
13951
13952 double ret_val;
13953
13954
13955
13956
13957
13958
13959
13960
13961
13962
13963
13964
13965
13966
13967
13968
13969
13970
13971
13972
13973
13974
13975
13976
13977
13978
13979
13980
13981
13982
13983
13984
13985
13986
13987 stcom_1.y = *x;
13988 ret_val = stcom_1.y;
13989 return ret_val;
13990 }
13991
13992 int swap_(int *in1, int *in2, int *io1, int *
13993 io2, int *list, int *lptr, int *lend, int *lp21)
13994 {
13995
13996 int i__1;
13997
13998
13999 static int lp, lph, lpsav;
14000 extern int lstptr_(int *, int *, int *, int *);
14001
14002
14003
14004
14005
14006
14007
14008
14009
14010
14011
14012
14013
14014
14015
14016
14017
14018
14019
14020
14021
14022
14023
14024
14025
14026
14027
14028
14029
14030
14031
14032
14033
14034
14035
14036
14037
14038
14039
14040
14041
14042
14043
14044
14045
14046
14047
14048
14049
14050
14051
14052
14053
14054
14055
14056
14057
14058
14059
14060
14061 --lend;
14062 --lptr;
14063 --list;
14064
14065
14066 lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14067 if ((i__1 = list[lp], abs(i__1)) == *in2) {
14068 *lp21 = 0;
14069 return 0;
14070 }
14071
14072
14073
14074 lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14075 lph = lptr[lp];
14076 lptr[lp] = lptr[lph];
14077
14078
14079
14080
14081 if (lend[*io1] == lph) {
14082 lend[*io1] = lp;
14083 }
14084
14085
14086
14087
14088 lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14089 lpsav = lptr[lp];
14090 lptr[lp] = lph;
14091 list[lph] = *in2;
14092 lptr[lph] = lpsav;
14093
14094
14095
14096 lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14097 lph = lptr[lp];
14098 lptr[lp] = lptr[lph];
14099
14100
14101
14102
14103 if (lend[*io2] == lph) {
14104 lend[*io2] = lp;
14105 }
14106
14107
14108
14109 lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14110 lpsav = lptr[lp];
14111 lptr[lp] = lph;
14112 list[lph] = *in1;
14113 lptr[lph] = lpsav;
14114 *lp21 = lph;
14115 return 0;
14116 }
14117
14118 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14119 double *x, double *y, double *z__)
14120 {
14121
14122 long int ret_val;
14123
14124
14125 static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14126
14127
14128
14129
14130
14131
14132
14133
14134
14135
14136
14137
14138
14139
14140
14141
14142
14143
14144
14145
14146
14147
14148
14149
14150
14151
14152
14153
14154
14155
14156
14157
14158
14159
14160
14161
14162
14163
14164
14165
14166
14167
14168
14169
14170
14171
14172
14173
14174
14175
14176
14177
14178
14179
14180
14181
14182 --z__;
14183 --y;
14184 --x;
14185
14186
14187 x4 = x[*n4];
14188 y4 = y[*n4];
14189 z4 = z__[*n4];
14190 dx1 = x[*n1] - x4;
14191 dx2 = x[*n2] - x4;
14192 dx3 = x[*n3] - x4;
14193 dy1 = y[*n1] - y4;
14194 dy2 = y[*n2] - y4;
14195 dy3 = y[*n3] - y4;
14196 dz1 = z__[*n1] - z4;
14197 dz2 = z__[*n2] - z4;
14198 dz3 = z__[*n3] - z4;
14199
14200
14201
14202
14203
14204 ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14205 dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14206 return ret_val;
14207 }
14208
14209 int trans_(int *n, double *rlat, double *rlon,
14210 double *x, double *y, double *z__)
14211 {
14212
14213 int i__1;
14214
14215
14216
14217
14218
14219 static int i__, nn;
14220 static double phi, theta, cosphi;
14221
14222
14223
14224
14225
14226
14227
14228
14229
14230
14231
14232
14233
14234
14235
14236
14237
14238
14239
14240
14241
14242
14243
14244
14245
14246
14247
14248
14249
14250
14251
14252
14253
14254
14255
14256
14257
14258
14259
14260
14261
14262
14263
14264
14265
14266
14267
14268
14269
14270
14271
14272
14273
14274
14275 --z__;
14276 --y;
14277 --x;
14278 --rlon;
14279 --rlat;
14280
14281
14282 nn = *n;
14283 i__1 = nn;
14284 for (i__ = 1; i__ <= i__1; ++i__) {
14285 phi = rlat[i__];
14286 theta = rlon[i__];
14287 cosphi = cos(phi);
14288 x[i__] = cosphi * cos(theta);
14289 y[i__] = cosphi * sin(theta);
14290 z__[i__] = sin(phi);
14291
14292 }
14293 return 0;
14294 }
14295
14296 int trfind_(int *nst, double *p, int *n,
14297 double *x, double *y, double *z__, int *list, int
14298 *lptr, int *lend, double *b1, double *b2, double *b3,
14299 int *i1, int *i2, int *i3)
14300 {
14301
14302
14303 static int ix = 1;
14304 static int iy = 2;
14305 static int iz = 3;
14306
14307
14308 int i__1;
14309 double d__1, d__2;
14310
14311
14312 static double q[3];
14313 static int n0, n1, n2, n3, n4, nf;
14314 static double s12;
14315 static int nl, lp;
14316 static double xp, yp, zp;
14317 static int n1s, n2s;
14318 static double eps, tol, ptn1, ptn2;
14319 static int next;
14320 extern int jrand_(int *, int *, int *, int *);
14321 extern double store_(double *);
14322 extern int lstptr_(int *, int *, int *, int *);
14323
14324
14325
14326
14327
14328
14329
14330
14331
14332
14333
14334
14335
14336
14337
14338
14339
14340
14341
14342
14343
14344
14345
14346
14347
14348
14349
14350
14351
14352
14353
14354
14355
14356
14357
14358
14359
14360
14361
14362
14363
14364
14365
14366
14367
14368
14369
14370
14371
14372
14373
14374
14375
14376
14377
14378
14379
14380
14381
14382
14383
14384
14385
14386
14387
14388
14389
14390
14391
14392 --p;
14393 --lend;
14394 --z__;
14395 --y;
14396 --x;
14397 --list;
14398 --lptr;
14399
14400
14401
14402
14403
14404
14405
14406
14407
14408
14409
14410
14411
14412
14413
14414
14415
14416
14417
14418
14419
14420
14421
14422
14423
14424
14425
14426
14427
14428
14429
14430
14431
14432
14433
14434
14435
14436
14437
14438
14439
14440
14441
14442
14443
14444
14445
14446
14447 xp = p[1];
14448 yp = p[2];
14449 zp = p[3];
14450 n0 = *nst;
14451 if (n0 < 1 || n0 > *n) {
14452 n0 = jrand_(n, &ix, &iy, &iz);
14453 }
14454
14455
14456
14457 eps = 1.;
14458 L1:
14459 eps /= 2.;
14460 d__1 = eps + 1.;
14461 if (store_(&d__1) > 1.) {
14462 goto L1;
14463 }
14464 eps *= 2.;
14465 tol = eps * 4.;
14466
14467
14468
14469
14470 L2:
14471 lp = lend[n0];
14472 nl = list[lp];
14473 lp = lptr[lp];
14474 nf = list[lp];
14475 n1 = nf;
14476
14477
14478
14479
14480 if (nl > 0) {
14481
14482
14483
14484 L3:
14485 if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14486 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14487 -1e-10) {
14488 lp = lptr[lp];
14489 n1 = list[lp];
14490 if (n1 == nl) {
14491 goto L6;
14492 }
14493 goto L3;
14494 }
14495 } else {
14496
14497
14498
14499 nl = -nl;
14500 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14501 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14502 -1e-10) {
14503
14504
14505
14506 n1 = n0;
14507 n2 = nf;
14508 goto L9;
14509 }
14510 if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14511 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14512 -1e-10) {
14513
14514
14515
14516 n1 = nl;
14517 n2 = n0;
14518 goto L9;
14519 }
14520 }
14521
14522
14523
14524
14525 L4:
14526 lp = lptr[lp];
14527 n2 = (i__1 = list[lp], abs(i__1));
14528 if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14529 n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14530 goto L7;
14531 }
14532 n1 = n2;
14533 if (n1 != nl) {
14534 goto L4;
14535 }
14536 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14537 nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14538 goto L6;
14539 }
14540
14541
14542
14543
14544 d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14545 if (store_(&d__2) < 1. - eps * 4.) {
14546
14547
14548
14549
14550
14551 L5:
14552 if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14553 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14554 -1e-10) {
14555 lp = lptr[lp];
14556 n1 = (i__1 = list[lp], abs(i__1));
14557 if (n1 == nl) {
14558 goto L14;
14559 }
14560 goto L5;
14561 }
14562 }
14563
14564
14565
14566
14567 n0 = n1;
14568 goto L2;
14569
14570
14571
14572 L6:
14573 n2 = nf;
14574
14575
14576
14577
14578
14579 L7:
14580 n3 = n0;
14581 n1s = n1;
14582 n2s = n2;
14583
14584
14585
14586 L8:
14587
14588 *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14589 x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14590 if (*b3 < -1e-10) {
14591
14592
14593
14594
14595 lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14596 if (list[lp] < 0) {
14597 goto L9;
14598 }
14599 lp = lptr[lp];
14600 n4 = (i__1 = list[lp], abs(i__1));
14601
14602
14603
14604 if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14605 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14606 -1e-10) {
14607 n3 = n2;
14608 n2 = n4;
14609 n1s = n1;
14610 if (n2 != n2s && n2 != n0) {
14611 goto L8;
14612 }
14613 } else {
14614 n3 = n1;
14615 n1 = n4;
14616 n2s = n2;
14617 if (n1 != n1s && n1 != n0) {
14618 goto L8;
14619 }
14620 }
14621
14622
14623
14624
14625
14626 n0 = jrand_(n, &ix, &iy, &iz);
14627 goto L2;
14628 }
14629
14630
14631
14632
14633 if (*b3 >= eps) {
14634
14635
14636
14637 *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14638 - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14639 *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14640 - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14641 if (*b1 < -tol || *b2 < -tol) {
14642
14643
14644
14645 n0 = jrand_(n, &ix, &iy, &iz);
14646 goto L2;
14647 }
14648 } else {
14649
14650
14651
14652
14653 *b3 = 0.;
14654 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14655 ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14656 ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14657 *b1 = ptn1 - s12 * ptn2;
14658 *b2 = ptn2 - s12 * ptn1;
14659 if (*b1 < -tol || *b2 < -tol) {
14660
14661
14662
14663 n0 = jrand_(n, &ix, &iy, &iz);
14664 goto L2;
14665 }
14666 }
14667
14668
14669
14670 *i1 = n1;
14671 *i2 = n2;
14672 *i3 = n3;
14673 if (*b1 < 0.f) {
14674 *b1 = 0.f;
14675 }
14676 if (*b2 < 0.f) {
14677 *b2 = 0.f;
14678 }
14679 return 0;
14680
14681
14682
14683
14684
14685 L9:
14686 n1s = n1;
14687 n2s = n2;
14688 nl = 0;
14689
14690
14691
14692 L10:
14693
14694 lp = lend[n2];
14695 lp = lptr[lp];
14696 next = list[lp];
14697 if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14698 - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14699 >= -1e-10) {
14700
14701
14702
14703
14704 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14705 q[0] = x[n1] - s12 * x[n2];
14706 q[1] = y[n1] - s12 * y[n2];
14707 q[2] = z__[n1] - s12 * z__[n2];
14708 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14709 goto L11;
14710 }
14711 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14712 goto L11;
14713 }
14714
14715
14716
14717
14718 nl = n2;
14719 }
14720
14721
14722
14723 n1 = n2;
14724 n2 = next;
14725 if (n2 != n1s) {
14726 goto L10;
14727 }
14728
14729
14730
14731 *i1 = n1s;
14732 *i2 = n1s;
14733 *i3 = 0;
14734 return 0;
14735
14736
14737
14738 L11:
14739 nf = n2;
14740 if (nl == 0) {
14741
14742
14743
14744
14745 n2 = n2s;
14746 n1 = n1s;
14747
14748
14749
14750 L12:
14751 lp = lend[n1];
14752 next = -list[lp];
14753 if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14754 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14755 y[next]) >= -1e-10) {
14756
14757
14758
14759
14760 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14761 q[0] = x[n2] - s12 * x[n1];
14762 q[1] = y[n2] - s12 * y[n1];
14763 q[2] = z__[n2] - s12 * z__[n1];
14764 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14765 goto L13;
14766 }
14767 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14768 goto L13;
14769 }
14770
14771
14772
14773
14774 nf = n1;
14775 }
14776
14777
14778
14779 n2 = n1;
14780 n1 = next;
14781 if (n1 != n1s) {
14782 goto L12;
14783 }
14784
14785
14786
14787 *i1 = n1;
14788 *i2 = n1;
14789 *i3 = 0;
14790 return 0;
14791
14792
14793
14794 L13:
14795 nl = n1;
14796 }
14797
14798
14799
14800 *i1 = nf;
14801 *i2 = nl;
14802 *i3 = 0;
14803 return 0;
14804
14805
14806
14807 L14:
14808 *i1 = 0;
14809 *i2 = 0;
14810 *i3 = 0;
14811 return 0;
14812 }
14813
14814 int trlist_(int *n, int *list, int *lptr,
14815 int *lend, int *nrow, int *nt, int *ltri, int *
14816 ier)
14817 {
14818
14819 int ltri_dim1, ltri_offset, i__1, i__2;
14820
14821
14822 static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
14823 lpl, isv;
14824 static long int arcs;
14825 static int lpln1;
14826
14827
14828
14829
14830
14831
14832
14833
14834
14835
14836
14837
14838
14839
14840
14841
14842
14843
14844
14845
14846
14847
14848
14849
14850
14851
14852
14853
14854
14855
14856
14857
14858
14859
14860
14861
14862
14863
14864
14865
14866
14867
14868
14869
14870
14871
14872
14873
14874
14875
14876
14877
14878
14879
14880
14881
14882
14883
14884
14885
14886
14887
14888
14889
14890
14891
14892
14893
14894
14895
14896
14897
14898
14899
14900
14901
14902
14903
14904
14905
14906
14907
14908
14909
14910
14911
14912
14913
14914
14915
14916
14917
14918
14919
14920
14921
14922
14923
14924
14925
14926
14927
14928
14929
14930 --lend;
14931 --list;
14932 --lptr;
14933 ltri_dim1 = *nrow;
14934 ltri_offset = 1 + ltri_dim1;
14935 ltri -= ltri_offset;
14936
14937
14938 if (*n < 3 || *nrow != 6 && *nrow != 9) {
14939 goto L11;
14940 }
14941
14942
14943
14944
14945
14946
14947
14948
14949 arcs = *nrow == 9;
14950 ka = 0;
14951 kt = 0;
14952 nm2 = *n - 2;
14953
14954
14955
14956 i__1 = nm2;
14957 for (n1 = 1; n1 <= i__1; ++n1) {
14958
14959
14960
14961
14962 lpln1 = lend[n1];
14963 lp2 = lpln1;
14964 L1:
14965 lp2 = lptr[lp2];
14966 n2 = list[lp2];
14967 lp = lptr[lp2];
14968 n3 = (i__2 = list[lp], abs(i__2));
14969 if (n2 < n1 || n3 < n1) {
14970 goto L8;
14971 }
14972
14973
14974
14975 ++kt;
14976 ltri[kt * ltri_dim1 + 1] = n1;
14977 ltri[kt * ltri_dim1 + 2] = n2;
14978 ltri[kt * ltri_dim1 + 3] = n3;
14979
14980
14981
14982
14983 for (i__ = 1; i__ <= 3; ++i__) {
14984 if (i__ == 1) {
14985 i1 = n3;
14986 i2 = n2;
14987 } else if (i__ == 2) {
14988 i1 = n1;
14989 i2 = n3;
14990 } else {
14991 i1 = n2;
14992 i2 = n1;
14993 }
14994
14995
14996
14997
14998 lpl = lend[i1];
14999 lp = lptr[lpl];
15000 L2:
15001 if (list[lp] == i2) {
15002 goto L3;
15003 }
15004 lp = lptr[lp];
15005 if (lp != lpl) {
15006 goto L2;
15007 }
15008
15009
15010
15011
15012
15013 if ((i__2 = list[lp], abs(i__2)) != i2) {
15014 goto L12;
15015 }
15016 kn = 0;
15017 if (list[lp] < 0) {
15018 goto L6;
15019 }
15020
15021
15022
15023
15024 L3:
15025 lp = lptr[lp];
15026 i3 = (i__2 = list[lp], abs(i__2));
15027
15028
15029
15030
15031
15032 if (i1 < i2 && i1 < i3) {
15033 j = 3;
15034 } else if (i2 < i3) {
15035 j = 2;
15036 isv = i1;
15037 i1 = i2;
15038 i2 = i3;
15039 i3 = isv;
15040 } else {
15041 j = 1;
15042 isv = i1;
15043 i1 = i3;
15044 i3 = i2;
15045 i2 = isv;
15046 }
15047
15048
15049
15050 if (i1 > n1) {
15051 goto L7;
15052 }
15053
15054
15055
15056
15057 for (kn = kt - 1; kn >= 1; --kn) {
15058 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15059 == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15060 goto L5;
15061 }
15062
15063 }
15064 goto L7;
15065
15066
15067
15068 L5:
15069 ltri[j + 3 + kn * ltri_dim1] = kt;
15070
15071
15072
15073 L6:
15074 ltri[i__ + 3 + kt * ltri_dim1] = kn;
15075 if (arcs) {
15076 ++ka;
15077 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15078 if (kn != 0) {
15079 ltri[j + 6 + kn * ltri_dim1] = ka;
15080 }
15081 }
15082 L7:
15083 ;
15084 }
15085
15086
15087
15088 L8:
15089 if (lp2 != lpln1) {
15090 goto L1;
15091 }
15092
15093 }
15094
15095
15096
15097 *nt = kt;
15098 *ier = 0;
15099 return 0;
15100
15101
15102
15103 L11:
15104 *nt = 0;
15105 *ier = 1;
15106 return 0;
15107
15108
15109
15110
15111 L12:
15112 *nt = 0;
15113 *ier = 2;
15114 return 0;
15115 }
15116
15117 int trlprt_(int *n, double *x, double *y,
15118 double *z__, int *iflag, int *nrow, int *nt, int *
15119 ltri, int *lout)
15120 {
15121
15122
15123 static int nmax = 9999;
15124 static int nlmax = 58;
15125
15126
15127 int ltri_dim1, ltri_offset, i__1;
15128
15129
15130 static int i__, k, na, nb, nl, lun;
15131
15132
15133
15134
15135
15136
15137
15138
15139
15140
15141
15142
15143
15144
15145
15146
15147
15148
15149
15150
15151
15152
15153
15154
15155
15156
15157
15158
15159
15160
15161
15162
15163
15164
15165
15166
15167
15168
15169
15170
15171
15172
15173
15174
15175
15176
15177
15178
15179
15180
15181
15182
15183
15184
15185
15186
15187
15188
15189
15190
15191
15192
15193
15194
15195
15196
15197
15198
15199
15200
15201
15202
15203
15204 --z__;
15205 --y;
15206 --x;
15207 ltri_dim1 = *nrow;
15208 ltri_offset = 1 + ltri_dim1;
15209 ltri -= ltri_offset;
15210
15211
15212
15213
15214
15215
15216
15217
15218
15219
15220
15221
15222
15223
15224
15225
15226 lun = *lout;
15227 if (lun < 0 || lun > 99) {
15228 lun = 6;
15229 }
15230
15231
15232
15233
15234 nl = 3;
15235 if (*n < 3 || *n > nmax || *nrow != 6 && *nrow != 9 || *nt < 1 || *nt >
15236 nmax) {
15237
15238
15239
15240
15241 return 0;
15242 }
15243 if (*iflag == 0) {
15244
15245
15246
15247
15248 nl = 6;
15249 i__1 = *n;
15250 for (i__ = 1; i__ <= i__1; ++i__) {
15251 if (nl >= nlmax) {
15252
15253 nl = 0;
15254 }
15255
15256 ++nl;
15257
15258 }
15259 } else if (*iflag > 0) {
15260
15261
15262
15263
15264 nl = 6;
15265 i__1 = *n;
15266 for (i__ = 1; i__ <= i__1; ++i__) {
15267 if (nl >= nlmax) {
15268
15269 nl = 0;
15270 }
15271
15272 ++nl;
15273
15274 }
15275 }
15276
15277
15278
15279 if (nl > nlmax / 2) {
15280
15281 nl = 0;
15282 }
15283 if (*nrow == 6) {
15284
15285 } else {
15286
15287 }
15288 nl += 5;
15289 i__1 = *nt;
15290 for (k = 1; k <= i__1; ++k) {
15291 if (nl >= nlmax) {
15292
15293 nl = 0;
15294 }
15295
15296 ++nl;
15297
15298 }
15299
15300
15301
15302
15303 nb = (*n << 1) - *nt - 2;
15304 if (nb < 3) {
15305 nb = 0;
15306 na = *n * 3 - 6;
15307 } else {
15308 na = *nt + *n - 1;
15309 }
15310
15311 return 0;
15312
15313
15314
15315
15316
15317
15318
15319
15320
15321
15322
15323
15324
15325
15326
15327
15328
15329
15330
15331
15332
15333
15334
15335 }
15336
15337 int trmesh_(int *n, double *x, double *y,
15338 double *z__, int *list, int *lptr, int *lend, int
15339 *lnew, int *near__, int *next, double *dist, int *ier)
15340 {
15341
15342 int i__1, i__2;
15343
15344
15345 static double d__;
15346 static int i__, j, k;
15347 static double d1, d2, d3;
15348 static int i0, lp, nn, lpl;
15349 extern long int left_(double *, double *, double *, double
15350 *, double *, double *, double *, double *,
15351 double *);
15352 static int nexti;
15353 extern int addnod_(int *, int *, double *,
15354 double *, double *, int *, int *, int *,
15355 int *, int *);
15356
15357
15358
15359
15360
15361
15362
15363
15364
15365
15366
15367
15368
15369
15370
15371
15372
15373
15374
15375
15376
15377
15378
15379
15380
15381
15382
15383
15384
15385
15386
15387
15388
15389
15390
15391
15392
15393
15394
15395
15396
15397
15398
15399
15400
15401
15402
15403
15404
15405
15406
15407
15408
15409
15410
15411
15412
15413
15414
15415
15416
15417
15418
15419
15420
15421
15422
15423
15424
15425
15426
15427
15428
15429
15430
15431
15432
15433
15434
15435
15436
15437
15438
15439
15440
15441
15442
15443
15444
15445
15446
15447
15448
15449
15450
15451
15452
15453
15454
15455
15456
15457
15458
15459
15460
15461
15462
15463
15464
15465
15466
15467
15468
15469
15470
15471
15472
15473
15474
15475
15476
15477
15478
15479
15480
15481
15482
15483
15484
15485
15486
15487
15488
15489
15490
15491
15492
15493
15494
15495
15496
15497
15498
15499
15500
15501
15502
15503
15504
15505
15506
15507
15508
15509
15510
15511
15512
15513
15514
15515
15516
15517
15518
15519
15520
15521
15522
15523
15524
15525
15526
15527
15528
15529
15530
15531
15532
15533
15534
15535
15536
15537
15538
15539
15540
15541
15542
15543
15544
15545
15546
15547
15548
15549
15550
15551
15552
15553
15554
15555
15556
15557
15558
15559
15560
15561
15562
15563
15564
15565
15566
15567
15568
15569
15570
15571
15572
15573
15574
15575
15576
15577
15578
15579
15580
15581
15582 --dist;
15583 --next;
15584 --near__;
15585 --lend;
15586 --z__;
15587 --y;
15588 --x;
15589 --list;
15590 --lptr;
15591
15592
15593 nn = *n;
15594 if (nn < 3) {
15595 *ier = -1;
15596 return 0;
15597 }
15598
15599
15600
15601 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15602 z__[3])) {
15603
15604
15605
15606 list[1] = 3;
15607 lptr[1] = 2;
15608 list[2] = -2;
15609 lptr[2] = 1;
15610 lend[1] = 2;
15611
15612 list[3] = 1;
15613 lptr[3] = 4;
15614 list[4] = -3;
15615 lptr[4] = 3;
15616 lend[2] = 4;
15617
15618 list[5] = 2;
15619 lptr[5] = 6;
15620 list[6] = -1;
15621 lptr[6] = 5;
15622 lend[3] = 6;
15623
15624 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15625 y[3], &z__[3])) {
15626
15627
15628
15629
15630
15631 list[1] = 2;
15632 lptr[1] = 2;
15633 list[2] = -3;
15634 lptr[2] = 1;
15635 lend[1] = 2;
15636
15637 list[3] = 3;
15638 lptr[3] = 4;
15639 list[4] = -1;
15640 lptr[4] = 3;
15641 lend[2] = 4;
15642
15643 list[5] = 1;
15644 lptr[5] = 6;
15645 list[6] = -2;
15646 lptr[6] = 5;
15647 lend[3] = 6;
15648
15649 } else {
15650
15651
15652
15653 *ier = -2;
15654 return 0;
15655 }
15656
15657
15658
15659 *lnew = 7;
15660 if (nn == 3) {
15661 *ier = 0;
15662 return 0;
15663 }
15664
15665
15666
15667
15668
15669
15670
15671
15672
15673
15674
15675
15676
15677
15678
15679
15680
15681
15682
15683
15684
15685
15686
15687
15688
15689
15690
15691 near__[1] = 0;
15692 near__[2] = 0;
15693 near__[3] = 0;
15694 for (k = nn; k >= 4; --k) {
15695 d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15696 d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15697 d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15698 if (d1 <= d2 && d1 <= d3) {
15699 near__[k] = 1;
15700 dist[k] = d1;
15701 next[k] = near__[1];
15702 near__[1] = k;
15703 } else if (d2 <= d1 && d2 <= d3) {
15704 near__[k] = 2;
15705 dist[k] = d2;
15706 next[k] = near__[2];
15707 near__[2] = k;
15708 } else {
15709 near__[k] = 3;
15710 dist[k] = d3;
15711 next[k] = near__[3];
15712 near__[3] = k;
15713 }
15714
15715 }
15716
15717
15718
15719 i__1 = nn;
15720 for (k = 4; k <= i__1; ++k) {
15721 addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15722 lend[1], lnew, ier);
15723 if (*ier != 0) {
15724 return 0;
15725 }
15726
15727
15728
15729
15730 i__ = near__[k];
15731 if (near__[i__] == k) {
15732 near__[i__] = next[k];
15733 } else {
15734 i__ = near__[i__];
15735 L2:
15736 i0 = i__;
15737 i__ = next[i0];
15738 if (i__ != k) {
15739 goto L2;
15740 }
15741 next[i0] = next[k];
15742 }
15743 near__[k] = 0;
15744
15745
15746
15747 lpl = lend[k];
15748 lp = lpl;
15749 L3:
15750 lp = lptr[lp];
15751 j = (i__2 = list[lp], abs(i__2));
15752
15753
15754
15755
15756
15757
15758
15759 i__ = near__[j];
15760 L4:
15761 if (i__ == 0) {
15762 goto L5;
15763 }
15764 nexti = next[i__];
15765
15766
15767
15768
15769 d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15770 if (d__ < dist[i__]) {
15771
15772
15773
15774
15775
15776 near__[i__] = k;
15777 dist[i__] = d__;
15778 if (i__ == near__[j]) {
15779 near__[j] = nexti;
15780 } else {
15781 next[i0] = nexti;
15782 }
15783 next[i__] = near__[k];
15784 near__[k] = i__;
15785 } else {
15786 i0 = i__;
15787 }
15788
15789
15790
15791 i__ = nexti;
15792 goto L4;
15793
15794
15795
15796 L5:
15797 if (lp != lpl) {
15798 goto L3;
15799 }
15800
15801 }
15802 return 0;
15803 }
15804
15805 int trplot_(int *lun, double *pltsiz, double *
15806 elat, double *elon, double *a, int *n, double *x,
15807 double *y, double *z__, int *list, int *lptr, int
15808 *lend, char *, long int *numbr, int *ier, short )
15809 {
15810
15811
15812 static long int annot = TRUE_;
15813 static double fsizn = 10.;
15814 static double fsizt = 16.;
15815 static double tol = .5;
15816
15817
15818 int i__1, i__2;
15819 double d__1;
15820
15821
15822
15823
15824
15825
15826
15827 static double t;
15828 static int n0, n1;
15829 static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
15830 static int ir, lp;
15831 static double ex, ey, ez, wr, tx, ty;
15832 static int lpl;
15833 static double wrs;
15834 static int ipx1, ipx2, ipy1, ipy2, nseg;
15835 extern int drwarc_(int *, double *, double *,
15836 double *, int *);
15837
15838
15839
15840
15841
15842
15843
15844
15845
15846
15847
15848
15849
15850
15851
15852
15853
15854
15855
15856
15857
15858
15859
15860
15861
15862
15863
15864
15865
15866
15867
15868
15869
15870
15871
15872
15873
15874
15875
15876
15877
15878
15879
15880
15881
15882
15883
15884
15885
15886
15887
15888
15889
15890
15891
15892
15893
15894
15895
15896
15897
15898
15899
15900
15901
15902
15903
15904
15905
15906
15907
15908
15909
15910
15911
15912
15913
15914
15915
15916
15917
15918
15919
15920
15921
15922
15923
15924
15925
15926
15927
15928
15929
15930
15931
15932
15933
15934
15935
15936
15937
15938
15939
15940
15941
15942
15943 --lend;
15944 --z__;
15945 --y;
15946 --x;
15947 --list;
15948 --lptr;
15949
15950
15951
15952
15953
15954
15955
15956
15957
15958
15959
15960
15961
15962
15963
15964
15965
15966
15967
15968
15969
15970
15971
15972
15973
15974
15975
15976
15977
15978
15979
15980
15981
15982
15983
15984
15985
15986
15987
15988
15989
15990
15991
15992
15993
15994
15995
15996
15997
15998
15999
16000
16001
16002
16003
16004 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16005 goto L11;
16006 }
16007 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16008 goto L12;
16009 }
16010
16011
16012
16013
16014 cf = atan(1.) / 45.;
16015 wr = sin(cf * *a);
16016 wrs = wr * wr;
16017
16018
16019
16020
16021
16022
16023
16024
16025
16026
16027 d__1 = *pltsiz * 36.;
16028 ir = i_dnnt(&d__1);
16029 ipx1 = 306 - ir;
16030 ipx2 = ir + 306;
16031 ipy1 = 396 - ir;
16032 ipy2 = ir + 396;
16033
16034
16035
16036
16037
16038
16039
16040
16041
16042
16043
16044
16045
16046
16047 d__1 = (double) ir * .88;
16048 ir = i_dnnt(&d__1);
16049 ipx1 = 306 - ir;
16050 ipx2 = ir + 306;
16051 ipy1 = 396 - ir;
16052 ipy2 = ir + 396;
16053
16054
16055
16056
16057 t = 2.;
16058
16059
16060
16061
16062
16063
16064
16065
16066
16067
16068 sf = (double) ir / wr;
16069 tx = ipx1 + sf * wr;
16070 ty = ipy1 + sf * wr;
16071
16072
16073
16074
16075
16076
16077
16078
16079 t = 1. / sf;
16080
16081
16082
16083
16084
16085
16086
16087
16088
16089
16090
16091
16092
16093
16094
16095
16096
16097
16098
16099
16100
16101
16102
16103 t = cf * *elon;
16104 ct = cos(cf * *elat);
16105 ex = ct * cos(t);
16106 ey = ct * sin(t);
16107 ez = sin(cf * *elat);
16108 if (ct != 0.) {
16109 r11 = -ey / ct;
16110 r12 = ex / ct;
16111 } else {
16112 r11 = 0.;
16113 r12 = 1.;
16114 }
16115 r21 = -ez * r12;
16116 r22 = ez * r11;
16117 r23 = ct;
16118
16119
16120
16121
16122 i__1 = *n;
16123 for (n0 = 1; n0 <= i__1; ++n0) {
16124 p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16125 if (p0[2] < 0.) {
16126 goto L3;
16127 }
16128 p0[0] = r11 * x[n0] + r12 * y[n0];
16129 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16130 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16131 goto L3;
16132 }
16133 lpl = lend[n0];
16134 lp = lpl;
16135
16136
16137
16138
16139 L1:
16140 lp = lptr[lp];
16141 n1 = (i__2 = list[lp], abs(i__2));
16142 p1[0] = r11 * x[n1] + r12 * y[n1];
16143 p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16144 p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16145 if (p1[2] < 0.) {
16146
16147
16148
16149
16150
16151 p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16152 p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16153 t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16154 p1[0] /= t;
16155 p1[1] /= t;
16156 }
16157
16158
16159
16160
16161 if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16162 goto L2;
16163 }
16164
16165
16166
16167
16168 if (p1[2] < 0.) {
16169 p1[2] = 0.;
16170 }
16171 d__1 = tol / sf;
16172 drwarc_(lun, p0, p1, &d__1, &nseg);
16173
16174
16175
16176 L2:
16177 if (lp != lpl) {
16178 goto L1;
16179 }
16180 L3:
16181 ;
16182 }
16183
16184
16185
16186
16187
16188
16189
16190 if (*numbr) {
16191
16192
16193
16194
16195
16196 t = fsizn / sf;
16197
16198
16199
16200
16201
16202
16203
16204 i__1 = *n;
16205 for (n0 = 1; n0 <= i__1; ++n0) {
16206 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16207 goto L4;
16208 }
16209 p0[0] = r11 * x[n0] + r12 * y[n0];
16210 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16211 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16212 goto L4;
16213 }
16214
16215
16216
16217
16218
16219
16220
16221
16222
16223 L4:
16224 ;
16225 }
16226 }
16227
16228
16229
16230
16231 t = fsizt / sf;
16232
16233
16234
16235
16236 p0[1] = wr + t * 3.;
16237
16238
16239
16240
16241
16242 if (annot) {
16243
16244
16245
16246 p0[0] = -wr;
16247 p0[1] = -wr - 50. / sf;
16248
16249
16250 p0[1] -= t * 2.;
16251
16252
16253
16254
16255
16256 }
16257
16258
16259
16260
16261
16262
16263
16264
16265
16266
16267
16268
16269
16270
16271
16272
16273
16274
16275 *ier = 0;
16276 return 0;
16277
16278
16279
16280 L11:
16281 *ier = 1;
16282 return 0;
16283
16284
16285
16286 L12:
16287 *ier = 2;
16288 return 0;
16289
16290
16291
16292
16293 *ier = 3;
16294 return 0;
16295 }
16296
16297 int trprnt_(int *n, double *x, double *y,
16298 double *z__, int *iflag, int *list, int *lptr,
16299 int *lend, int *lout)
16300 {
16301
16302
16303 static int nmax = 9999;
16304 static int nlmax = 58;
16305
16306
16307 int i__1;
16308
16309
16310 static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16311 400];
16312
16313
16314
16315
16316
16317
16318
16319
16320
16321
16322
16323
16324
16325
16326
16327
16328
16329
16330
16331
16332
16333
16334
16335
16336
16337
16338
16339
16340
16341
16342
16343
16344
16345
16346
16347
16348
16349
16350
16351
16352
16353
16354
16355
16356
16357
16358
16359
16360
16361
16362
16363
16364
16365
16366
16367
16368
16369
16370
16371
16372
16373
16374 --lend;
16375 --z__;
16376 --y;
16377 --x;
16378 --list;
16379 --lptr;
16380
16381
16382
16383
16384
16385
16386
16387
16388
16389
16390
16391
16392
16393
16394
16395
16396
16397
16398
16399
16400
16401
16402
16403
16404
16405
16406
16407 nn = *n;
16408 lun = *lout;
16409 if (lun < 0 || lun > 99) {
16410 lun = 6;
16411 }
16412
16413
16414
16415
16416 if (nn < 3 || nn > nmax) {
16417
16418
16419
16420
16421 return 0;
16422 }
16423
16424
16425
16426
16427 nl = 6;
16428 nb = 0;
16429 if (*iflag < 0) {
16430
16431
16432
16433
16434
16435 i__1 = nn;
16436 for (node = 1; node <= i__1; ++node) {
16437 lpl = lend[node];
16438 lp = lpl;
16439 k = 0;
16440
16441 L1:
16442 ++k;
16443 lp = lptr[lp];
16444 nd = list[lp];
16445 nabor[k - 1] = nd;
16446 if (lp != lpl) {
16447 goto L1;
16448 }
16449 if (nd <= 0) {
16450
16451
16452
16453
16454
16455 nabor[k - 1] = -nd;
16456 ++k;
16457 nabor[k - 1] = 0;
16458 ++nb;
16459 }
16460
16461
16462
16463 inc = (k - 1) / 14 + 2;
16464 nl += inc;
16465 if (nl > nlmax) {
16466
16467 nl = inc;
16468 }
16469
16470
16471
16472
16473 }
16474 } else if (*iflag > 0) {
16475
16476
16477
16478
16479 i__1 = nn;
16480 for (node = 1; node <= i__1; ++node) {
16481 lpl = lend[node];
16482 lp = lpl;
16483 k = 0;
16484
16485 L3:
16486 ++k;
16487 lp = lptr[lp];
16488 nd = list[lp];
16489 nabor[k - 1] = nd;
16490 if (lp != lpl) {
16491 goto L3;
16492 }
16493 if (nd <= 0) {
16494
16495
16496
16497 nabor[k - 1] = -nd;
16498 ++k;
16499 nabor[k - 1] = 0;
16500 ++nb;
16501 }
16502
16503
16504
16505 inc = (k - 1) / 8 + 2;
16506 nl += inc;
16507 if (nl > nlmax) {
16508
16509 nl = inc;
16510 }
16511
16512
16513
16514
16515
16516 }
16517 } else {
16518
16519
16520
16521
16522 i__1 = nn;
16523 for (node = 1; node <= i__1; ++node) {
16524 lpl = lend[node];
16525 lp = lpl;
16526 k = 0;
16527
16528 L5:
16529 ++k;
16530 lp = lptr[lp];
16531 nd = list[lp];
16532 nabor[k - 1] = nd;
16533 if (lp != lpl) {
16534 goto L5;
16535 }
16536 if (nd <= 0) {
16537
16538
16539
16540 nabor[k - 1] = -nd;
16541 ++k;
16542 nabor[k - 1] = 0;
16543 ++nb;
16544 }
16545
16546
16547
16548 inc = (k - 1) / 5 + 2;
16549 nl += inc;
16550 if (nl > nlmax) {
16551
16552 nl = inc;
16553 }
16554
16555
16556
16557
16558
16559 }
16560 }
16561
16562
16563
16564
16565 if (nb != 0) {
16566 na = nn * 3 - nb - 3;
16567 nt = (nn << 1) - nb - 2;
16568 } else {
16569 na = nn * 3 - 6;
16570 nt = (nn << 1) - 4;
16571 }
16572
16573 return 0;
16574
16575
16576
16577
16578
16579
16580
16581
16582
16583
16584
16585
16586
16587
16588
16589
16590
16591
16592
16593
16594 }
16595
16596 int vrplot_(int *lun, double *pltsiz, double *
16597 elat, double *elon, double *a, int *n, double *x,
16598 double *y, double *z__, int *nt, int *listc, int *
16599 lptr, int *lend, double *xc, double *yc, double *zc,
16600 char *, long int *numbr, int *ier, short)
16601 {
16602
16603
16604 static long int annot = TRUE_;
16605 static double fsizn = 10.;
16606 static double fsizt = 16.;
16607 static double tol = .5;
16608
16609
16610 int i__1;
16611 double d__1;
16612
16613
16614
16615
16616
16617
16618
16619 static double t;
16620 static int n0;
16621 static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16622 sf;
16623 static int ir, lp;
16624 static double ex, ey, ez, wr, tx, ty;
16625 static long int in1, in2;
16626 static int kv1, kv2, lpl;
16627 static double wrs;
16628 static int ipx1, ipx2, ipy1, ipy2, nseg;
16629 extern int drwarc_(int *, double *, double *,
16630 double *, int *);
16631
16632
16633
16634
16635
16636
16637
16638
16639
16640
16641
16642
16643
16644
16645
16646
16647
16648
16649
16650
16651
16652
16653
16654
16655
16656
16657
16658
16659
16660
16661
16662
16663
16664
16665
16666
16667
16668
16669
16670
16671
16672
16673
16674
16675
16676
16677
16678
16679
16680
16681
16682
16683
16684
16685
16686
16687
16688
16689
16690
16691
16692
16693
16694
16695
16696
16697
16698
16699
16700
16701
16702
16703
16704
16705
16706
16707
16708
16709
16710
16711
16712
16713
16714
16715
16716
16717
16718
16719
16720
16721
16722
16723
16724
16725
16726
16727
16728
16729
16730
16731
16732
16733
16734
16735
16736
16737
16738
16739
16740
16741
16742
16743
16744
16745
16746
16747
16748
16749
16750
16751
16752
16753
16754
16755
16756
16757
16758
16759
16760
16761
16762
16763
16764
16765
16766
16767
16768
16769
16770
16771
16772
16773 --lend;
16774 --z__;
16775 --y;
16776 --x;
16777 --zc;
16778 --yc;
16779 --xc;
16780 --listc;
16781 --lptr;
16782
16783
16784
16785
16786
16787
16788
16789
16790
16791
16792
16793
16794
16795
16796
16797
16798
16799
16800
16801
16802
16803
16804
16805
16806
16807
16808
16809
16810
16811
16812
16813
16814
16815
16816
16817
16818
16819
16820
16821
16822
16823
16824
16825
16826
16827
16828
16829
16830
16831
16832
16833
16834
16835
16836
16837
16838
16839
16840 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
16841 nt != 2 * *n - 4) {
16842 goto L11;
16843 }
16844 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16845 goto L12;
16846 }
16847
16848
16849
16850
16851 cf = atan(1.) / 45.;
16852 wr = sin(cf * *a);
16853 wrs = wr * wr;
16854
16855
16856
16857
16858
16859
16860
16861
16862
16863
16864 d__1 = *pltsiz * 36.;
16865 ir = i_dnnt(&d__1);
16866 ipx1 = 306 - ir;
16867 ipx2 = ir + 306;
16868 ipy1 = 396 - ir;
16869 ipy2 = ir + 396;
16870
16871
16872
16873
16874
16875
16876
16877
16878
16879
16880
16881
16882
16883 d__1 = (double) ir * .88;
16884 ir = i_dnnt(&d__1);
16885 ipx1 = 306 - ir;
16886 ipx2 = ir + 306;
16887 ipy1 = 396 - ir;
16888 ipy2 = ir + 396;
16889
16890
16891
16892
16893 t = 2.;
16894
16895
16896
16897
16898
16899
16900
16901
16902
16903
16904 sf = (double) ir / wr;
16905 tx = ipx1 + sf * wr;
16906 ty = ipy1 + sf * wr;
16907
16908
16909
16910
16911
16912
16913
16914
16915 t = 1. / sf;
16916
16917
16918
16919
16920
16921
16922
16923
16924
16925
16926
16927
16928
16929
16930
16931
16932
16933
16934
16935
16936
16937
16938
16939 t = cf * *elon;
16940 ct = cos(cf * *elat);
16941 ex = ct * cos(t);
16942 ey = ct * sin(t);
16943 ez = sin(cf * *elat);
16944 if (ct != 0.) {
16945 r11 = -ey / ct;
16946 r12 = ex / ct;
16947 } else {
16948 r11 = 0.;
16949 r12 = 1.;
16950 }
16951 r21 = -ez * r12;
16952 r22 = ez * r11;
16953 r23 = ct;
16954
16955
16956
16957
16958 i__1 = *n;
16959 for (n0 = 1; n0 <= i__1; ++n0) {
16960 lpl = lend[n0];
16961
16962
16963
16964
16965 kv2 = listc[lpl];
16966 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
16967 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
16968 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
16969
16970
16971
16972 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
16973
16974
16975
16976
16977 lp = lpl;
16978 L1:
16979 lp = lptr[lp];
16980 kv1 = kv2;
16981 p1[0] = p2[0];
16982 p1[1] = p2[1];
16983 p1[2] = p2[2];
16984 in1 = in2;
16985 kv2 = listc[lp];
16986
16987
16988
16989 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
16990 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
16991 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
16992 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
16993
16994
16995
16996
16997
16998 if (! in1 || in2 && kv2 <= kv1) {
16999 goto L2;
17000 }
17001 if (p2[2] < 0.) {
17002
17003
17004
17005
17006
17007 p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17008 p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17009 t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17010 p2[0] /= t;
17011 p2[1] /= t;
17012 }
17013
17014
17015
17016
17017 if (p2[2] < 0.) {
17018 p2[2] = 0.f;
17019 }
17020 d__1 = tol / sf;
17021 drwarc_(lun, p1, p2, &d__1, &nseg);
17022
17023
17024
17025 L2:
17026 if (lp != lpl) {
17027 goto L1;
17028 }
17029
17030 }
17031
17032
17033
17034
17035
17036
17037
17038 if (*numbr) {
17039
17040
17041
17042
17043
17044 t = fsizn / sf;
17045
17046
17047
17048
17049
17050
17051
17052 i__1 = *n;
17053 for (n0 = 1; n0 <= i__1; ++n0) {
17054 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17055 goto L4;
17056 }
17057 x0 = r11 * x[n0] + r12 * y[n0];
17058 y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17059 if (x0 * x0 + y0 * y0 > wrs) {
17060 goto L4;
17061 }
17062
17063
17064
17065
17066
17067
17068
17069
17070 L4:
17071 ;
17072 }
17073 }
17074
17075
17076
17077
17078 t = fsizt / sf;
17079
17080
17081
17082
17083 y0 = wr + t * 3.;
17084
17085
17086
17087
17088
17089 if (annot) {
17090
17091
17092
17093 x0 = -wr;
17094 y0 = -wr - 50. / sf;
17095
17096
17097 y0 -= t * 2.;
17098
17099
17100
17101
17102
17103 }
17104
17105
17106
17107
17108
17109
17110
17111
17112
17113
17114
17115
17116
17117
17118
17119
17120
17121
17122 *ier = 0;
17123 return 0;
17124
17125
17126
17127 L11:
17128 *ier = 1;
17129 return 0;
17130
17131
17132
17133 L12:
17134 *ier = 2;
17135 return 0;
17136
17137
17138
17139
17140 *ier = 3;
17141 return 0;
17142 }
17143
17144 int random_(int *ix, int *iy, int *iz,
17145 double *rannum)
17146 {
17147 static double x;
17148
17149
17150
17151
17152
17153
17154
17155
17156
17157
17158
17159
17160
17161
17162
17163 *ix = *ix * 171 % 30269;
17164 *iy = *iy * 172 % 30307;
17165 *iz = *iz * 170 % 30323;
17166 x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17167 double) (*iz) / 30323.;
17168 *rannum = x - (int) x;
17169 return 0;
17170 }
17171
17172 #undef TRUE_
17173 #undef FALSE_
17174 #undef abs
17175
17176
17177
17178
17179
17180
17181
17182
17183
17184 EMData* Util::mult_scalar(EMData* img, float scalar)
17185 {
17186 ENTERFUNC;
17187
17188 if (!img) {
17189 throw NullPointerException("NULL input image");
17190 }
17191
17192
17193 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17194 int size = nx*ny*nz;
17195 EMData * img2 = img->copy_head();
17196 float *img_ptr =img->get_data();
17197 float *img2_ptr = img2->get_data();
17198 for (int i=0;i<size;i++)img2_ptr[i] = img_ptr[i]*scalar;
17199 img2->update();
17200
17201 if(img->is_complex()) {
17202 img2->set_complex(true);
17203 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17204 }
17205 EXITFUNC;
17206 return img2;
17207 }
17208
17209 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17210 {
17211 ENTERFUNC;
17212
17213 if (!img) {
17214 throw NullPointerException("NULL input image");
17215 }
17216
17217
17218 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17219 int size = nx*ny*nz;
17220 EMData * img2 = img->copy_head();
17221 float *img_ptr =img->get_data();
17222 float *img2_ptr = img2->get_data();
17223 float *img1_ptr = img1->get_data();
17224 for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17225 img2->update();
17226 if(img->is_complex()) {
17227 img2->set_complex(true);
17228 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17229 }
17230
17231 EXITFUNC;
17232 return img2;
17233 }
17234
17235 EMData* Util::addn_img(EMData* img, EMData* img1)
17236 {
17237 ENTERFUNC;
17238
17239 if (!img) {
17240 throw NullPointerException("NULL input image");
17241 }
17242
17243
17244 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17245 int size = nx*ny*nz;
17246 EMData * img2 = img->copy_head();
17247 float *img_ptr =img->get_data();
17248 float *img2_ptr = img2->get_data();
17249 float *img1_ptr = img1->get_data();
17250 for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17251 img2->update();
17252 if(img->is_complex()) {
17253 img2->set_complex(true);
17254 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17255 }
17256
17257 EXITFUNC;
17258 return img2;
17259 }
17260
17261 EMData* Util::subn_img(EMData* img, EMData* img1)
17262 {
17263 ENTERFUNC;
17264
17265 if (!img) {
17266 throw NullPointerException("NULL input image");
17267 }
17268
17269
17270 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17271 int size = nx*ny*nz;
17272 EMData * img2 = img->copy_head();
17273 float *img_ptr =img->get_data();
17274 float *img2_ptr = img2->get_data();
17275 float *img1_ptr = img1->get_data();
17276 for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17277 img2->update();
17278 if(img->is_complex()) {
17279 img2->set_complex(true);
17280 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17281 }
17282
17283 EXITFUNC;
17284 return img2;
17285 }
17286
17287 EMData* Util::muln_img(EMData* img, EMData* img1)
17288 {
17289 ENTERFUNC;
17290
17291 if (!img) {
17292 throw NullPointerException("NULL input image");
17293 }
17294
17295
17296 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17297 int size = nx*ny*nz;
17298 EMData * img2 = img->copy_head();
17299 float *img_ptr =img->get_data();
17300 float *img2_ptr = img2->get_data();
17301 float *img1_ptr = img1->get_data();
17302 if(img->is_complex()) {
17303 for (int i=0; i<size; i+=2) {
17304 img2_ptr[i] = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17305 img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17306 }
17307 img2->set_complex(true);
17308 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17309 } else {
17310 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17311 img2->update();
17312 }
17313
17314 EXITFUNC;
17315 return img2;
17316 }
17317
17318 EMData* Util::divn_img(EMData* img, EMData* img1)
17319 {
17320 ENTERFUNC;
17321
17322 if (!img) {
17323 throw NullPointerException("NULL input image");
17324 }
17325
17326
17327 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17328 int size = nx*ny*nz;
17329 EMData * img2 = img->copy_head();
17330 float *img_ptr =img->get_data();
17331 float *img2_ptr = img2->get_data();
17332 float *img1_ptr = img1->get_data();
17333 if(img->is_complex()) {
17334 float sq2;
17335 for (int i=0; i<size; i+=2) {
17336 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17337 img2_ptr[i] = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17338 img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17339 }
17340 img2->set_complex(true);
17341 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17342 } else {
17343 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17344 img2->update();
17345 }
17346
17347 EXITFUNC;
17348 return img2;
17349 }
17350
17351 EMData* Util::divn_filter(EMData* img, EMData* img1)
17352 {
17353 ENTERFUNC;
17354
17355 if (!img) {
17356 throw NullPointerException("NULL input image");
17357 }
17358
17359
17360 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17361 int size = nx*ny*nz;
17362 EMData * img2 = img->copy_head();
17363 float *img_ptr =img->get_data();
17364 float *img1_ptr = img1->get_data();
17365 float *img2_ptr = img2->get_data();
17366 if(img->is_complex()) {
17367 for (int i=0; i<size; i+=2) {
17368 if(img1_ptr[i] > 1.e-10f) {
17369 img2_ptr[i] = img_ptr[i] /img1_ptr[i];
17370 img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17371 } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17372 }
17373 } else throw ImageFormatException("Only Fourier image allowed");
17374
17375 img->update();
17376
17377 EXITFUNC;
17378 return img2;
17379 }
17380
17381 void Util::mul_scalar(EMData* img, float scalar)
17382 {
17383 ENTERFUNC;
17384
17385 if (!img) {
17386 throw NullPointerException("NULL input image");
17387 }
17388
17389
17390 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17391 int size = nx*ny*nz;
17392 float *img_ptr =img->get_data();
17393 for (int i=0;i<size;i++) img_ptr[i] *= scalar;
17394 img->update();
17395
17396 EXITFUNC;
17397 }
17398
17399 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17400 {
17401 ENTERFUNC;
17402
17403 if (!img) {
17404 throw NullPointerException("NULL input image");
17405 }
17406
17407
17408 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17409 int size = nx*ny*nz;
17410 float *img_ptr =img->get_data();
17411 float *img1_ptr = img1->get_data();
17412 for (int i=0;i<size;i++)img_ptr[i] += img1_ptr[i]*scalar;
17413 img1->update();
17414
17415 EXITFUNC;
17416 }
17417
17418 void Util::add_img(EMData* img, EMData* img1)
17419 {
17420 ENTERFUNC;
17421
17422 if (!img) {
17423 throw NullPointerException("NULL input image");
17424 }
17425
17426
17427 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17428 int size = nx*ny*nz;
17429 float *img_ptr = img->get_data();
17430 float *img1_ptr = img1->get_data();
17431 for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i];
17432 img->update();
17433
17434 EXITFUNC;
17435 }
17436
17437 void Util::add_img_abs(EMData* img, EMData* img1)
17438 {
17439 ENTERFUNC;
17440
17441 if (!img) {
17442 throw NullPointerException("NULL input image");
17443 }
17444
17445
17446 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17447 int size = nx*ny*nz;
17448 float *img_ptr = img->get_data();
17449 float *img1_ptr = img1->get_data();
17450 for (int i=0;i<size;i++) img_ptr[i] += abs(img1_ptr[i]);
17451 img->update();
17452
17453 EXITFUNC;
17454 }
17455
17456 void Util::add_img2(EMData* img, EMData* img1)
17457 {
17458 ENTERFUNC;
17459
17460 if (!img) {
17461 throw NullPointerException("NULL input image");
17462 }
17463
17464
17465 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17466 int size = nx*ny*nz;
17467 float *img_ptr = img->get_data();
17468 float *img1_ptr = img1->get_data();
17469 if(img->is_complex()) {
17470 for (int i=0; i<size; i+=2) img_ptr[i] += img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1] ;
17471 } else {
17472 for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17473 }
17474 img->update();
17475
17476 EXITFUNC;
17477 }
17478
17479 void Util::sub_img(EMData* img, EMData* img1)
17480 {
17481 ENTERFUNC;
17482
17483 if (!img) {
17484 throw NullPointerException("NULL input image");
17485 }
17486
17487
17488 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17489 int size = nx*ny*nz;
17490 float *img_ptr = img->get_data();
17491 float *img1_ptr = img1->get_data();
17492 for (int i=0;i<size;i++) img_ptr[i] -= img1_ptr[i];
17493 img->update();
17494
17495 EXITFUNC;
17496 }
17497
17498 void Util::mul_img(EMData* img, EMData* img1)
17499 {
17500 ENTERFUNC;
17501
17502 if (!img) {
17503 throw NullPointerException("NULL input image");
17504 }
17505
17506
17507 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17508 int size = nx*ny*nz;
17509 float *img_ptr = img->get_data();
17510 float *img1_ptr = img1->get_data();
17511 if(img->is_complex()) {
17512 for (int i=0; i<size; i+=2) {
17513 float tmp = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17514 img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17515 img_ptr[i] = tmp;
17516
17517 }
17518 } else {
17519 for (int i=0;i<size;i++) img_ptr[i] *= img1_ptr[i];
17520 }
17521 img->update();
17522
17523 EXITFUNC;
17524 }
17525
17526 void Util::div_img(EMData* img, EMData* img1)
17527 {
17528 ENTERFUNC;
17529
17530 if (!img) {
17531 throw NullPointerException("NULL input image");
17532 }
17533
17534
17535 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17536 int size = nx*ny*nz;
17537 float *img_ptr = img->get_data();
17538 float *img1_ptr = img1->get_data();
17539 if(img->is_complex()) {
17540 float sq2;
17541 for (int i=0; i<size; i+=2) {
17542 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17543 float tmp = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17544 img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17545 img_ptr[i] = tmp;
17546 }
17547 } else {
17548 for (int i=0; i<size; i++) img_ptr[i] /= img1_ptr[i];
17549 }
17550 img->update();
17551
17552 EXITFUNC;
17553 }
17554
17555 void Util::div_filter(EMData* img, EMData* img1)
17556 {
17557 ENTERFUNC;
17558
17559 if (!img) {
17560 throw NullPointerException("NULL input image");
17561 }
17562
17563
17564 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17565 int size = nx*ny*nz;
17566 float *img_ptr = img->get_data();
17567 float *img1_ptr = img1->get_data();
17568 if(img->is_complex()) {
17569 for (int i=0; i<size; i+=2) {
17570 if(img1_ptr[i] > 1.e-10f) {
17571 img_ptr[i] /= img1_ptr[i];
17572 img_ptr[i+1] /= img1_ptr[i];
17573 } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17574 }
17575 } else throw ImageFormatException("Only Fourier image allowed");
17576
17577 img->update();
17578
17579 EXITFUNC;
17580 }
17581
17582 #define img_ptr(i,j,k) img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*nxo]
17583
17584 EMData* Util::pack_complex_to_real(EMData* img)
17585 {
17586 ENTERFUNC;
17587
17588 if (!img) {
17589 throw NullPointerException("NULL input image");
17590 }
17591
17592
17593
17594 int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17595 int nx = nxo - 2 + img->is_fftodd();
17596 int lsd2 = (nx + 2 - nx%2) / 2;
17597 int nyt, nzt;
17598 int nx2 = nx/2;
17599 int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17600 int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17601 int nx2p = nx2+nx%2;
17602 int ny2p = ny2+ny%2;
17603 int nz2p = nz2+nz%2;
17604 EMData& power = *(new EMData());
17605 power.set_size(nx, ny, nz);
17606 power.set_array_offsets(-nx2,-ny2,-nz2);
17607
17608 float *img_ptr = img->get_data();
17609 for (int iz = 1; iz <= nz; iz++) {
17610 int jz=iz-1;
17611 if(jz>=nz2p) jz=jz-nzt;
17612 for (int iy = 1; iy <= ny; iy++) {
17613 int jy=iy-1;
17614 if(jy>=ny2p) jy=jy-nyt;
17615 for (int ix = 1; ix <= lsd2; ix++) {
17616 int jx=ix-1;
17617 if(jx>=nx2p) jx=jx-nx;
17618 power(jx,jy,jz) = img_ptr(ix,iy,iz);
17619 }
17620 }
17621 }
17622
17623 int nzb, nze, nyb, nye, nxb, nxe;
17624 nxb =-nx2+(nx+1)%2;
17625 nxe = nx2-(nx+1)%2;
17626 if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17627 if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17628 for (int iz = nzb; iz <= nze; iz++) {
17629 for (int iy = nyb; iy <= nye; iy++) {
17630 for (int ix = 1; ix <= nxe; ix++) {
17631 power(-ix,-iy,-iz) = power(ix,iy,iz);
17632 }
17633 }
17634 }
17635 if(ny2 != 0) {
17636 if(nz2 != 0) {
17637 if(nz%2 == 0) {
17638 for (int iy = nyb; iy <= nye; iy++) {
17639 for (int ix = nxb; ix <= -1; ix++) {
17640 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17641 }
17642 }
17643 if(ny%2 == 0) {
17644 for (int ix = nxb; ix <= -1; ix++) {
17645 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17646 }
17647 }
17648 }
17649 }
17650 if(ny%2 == 0) {
17651 for (int iz = nzb; iz <= nze; iz++) {
17652 for (int ix = nxb; ix <= -1; ix++) {
17653 power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17654 }
17655 }
17656 }
17657
17658 }
17659 power.update();
17660 power.set_array_offsets(0,0,0);
17661 return &power;
17662 }
17663 #undef img_ptr
17664
17665 float Util::ang_n(float peakp, string mode, int maxrin)
17666 {
17667 if (mode == "f" || mode == "F")
17668 return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17669 else
17670 return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17671 }
17672
17673
17674 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17675 {
17676 float* data = ring->get_data();
17677 float av=0.0;
17678 float sq=0.0;
17679 float nn=0.0;
17680 int nring = numr.size()/3;
17681 for( int i=0; i < nring; ++i )
17682 {
17683 int numr3i = numr[3*i+2];
17684 int numr2i = numr[3*i+1]-1;
17685 float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17686 for( int j=0; j < numr3i; ++j )
17687 {
17688 int jc = numr2i+j;
17689 av += data[jc] * w;
17690 sq += data[jc] * data[jc] * w;
17691 nn += w;
17692 }
17693 }
17694
17695 float avg = av/nn;
17696 float sgm = sqrt( (sq-av*av/nn)/nn );
17697 int n = ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17698 for( int i=0; i < n; ++i )
17699 {
17700 data[i] -= avg;
17701 data[i] /= sgm;
17702 }
17703
17704 ring->update();
17705 }
17706
17707 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17708 float xrng, float yrng, float step, string mode,
17709 vector<int>numr, float cnx, float cny) {
17710
17711
17712
17713
17714
17715
17716
17717
17718
17719
17720
17721
17722 size_t crefim_len = crefim.size();
17723
17724 int ky = int(2*yrng/step+0.5)/2;
17725 int kx = int(2*xrng/step+0.5)/2;
17726 int iref, nref=0, mirror=0;
17727 float iy, ix, sx=0, sy=0;
17728 float peak = -1.0E23f;
17729 float ang=0.0f;
17730 for (int i = -ky; i <= ky; i++) {
17731 iy = i * step ;
17732 for (int j = -kx; j <= kx; j++) {
17733 ix = j*step ;
17734 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17735
17736 Normalize_ring( cimage, numr );
17737
17738 Frngs(cimage, numr);
17739
17740
17741 for ( iref = 0; iref < (int)crefim_len; iref++) {
17742 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17743 double qn = retvals["qn"];
17744 double qm = retvals["qm"];
17745 if(qn >= peak || qm >= peak) {
17746 sx = -ix;
17747 sy = -iy;
17748 nref = iref;
17749 if (qn >= qm) {
17750 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17751 peak = static_cast<float>(qn);
17752 mirror = 0;
17753 } else {
17754 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17755 peak = static_cast<float>(qm);
17756 mirror = 1;
17757 }
17758 }
17759 } delete cimage; cimage = 0;
17760 }
17761 }
17762 float co, so, sxs, sys;
17763 co = static_cast<float>( cos(ang*pi/180.0) );
17764 so = static_cast<float>( -sin(ang*pi/180.0) );
17765 sxs = sx*co - sy*so;
17766 sys = sx*so + sy*co;
17767 vector<float> res;
17768 res.push_back(ang);
17769 res.push_back(sxs);
17770 res.push_back(sys);
17771 res.push_back(static_cast<float>(mirror));
17772 res.push_back(static_cast<float>(nref));
17773 res.push_back(peak);
17774 return res;
17775 }
17776
17777 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
17778 float xrng, float yrng, float step, string mode,
17779 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
17780
17781
17782
17783
17784
17785
17786
17787
17788
17789
17790
17791
17792 size_t crefim_len = crefim.size();
17793
17794 int ky = int(2*yrng/step+0.5)/2;
17795 int kx = int(2*xrng/step+0.5)/2;
17796 int iref, nref=0, mirror=0;
17797 float iy, ix, sx=0, sy=0;
17798 float peak = -1.0E23f;
17799 float ang=0.0f;
17800 for (int i = -ky; i <= ky; i++) {
17801 iy = i * step ;
17802 for (int j = -kx; j <= kx; j++) {
17803 ix = j*step ;
17804 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17805
17806 Normalize_ring( cimage, numr );
17807
17808 Frngs(cimage, numr);
17809
17810
17811 for ( iref = 0; iref < (int)crefim_len; iref++) {
17812 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
17813 double qn = retvals["qn"];
17814 double qm = retvals["qm"];
17815 if(qn >= peak || qm >= peak) {
17816 sx = -ix;
17817 sy = -iy;
17818 nref = iref;
17819 if (qn >= qm) {
17820 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17821 peak = static_cast<float>(qn);
17822 mirror = 0;
17823 } else {
17824 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17825 peak = static_cast<float>(qm);
17826 mirror = 1;
17827 }
17828 }
17829 } delete cimage; cimage = 0;
17830 }
17831 }
17832 float co, so, sxs, sys;
17833 co = static_cast<float>( cos(ang*pi/180.0) );
17834 so = static_cast<float>( -sin(ang*pi/180.0) );
17835 sxs = sx*co - sy*so;
17836 sys = sx*so + sy*co;
17837 vector<float> res;
17838 res.push_back(ang);
17839 res.push_back(sxs);
17840 res.push_back(sys);
17841 res.push_back(static_cast<float>(mirror));
17842 res.push_back(static_cast<float>(nref));
17843 res.push_back(peak);
17844 return res;
17845 }
17846
17847 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
17848 float xrng, float yrng, float step, string mode,
17849 vector< int >numr, float cnx, float cny) {
17850
17851
17852
17853
17854
17855
17856
17857
17858
17859
17860
17861 size_t crefim_len = crefim.size();
17862
17863 int ky = int(2*yrng/step+0.5)/2;
17864 int kx = int(2*xrng/step+0.5)/2;
17865 int iref, nref=0;
17866 float iy, ix, sx=0, sy=0;
17867 float peak = -1.0E23f;
17868 float ang=0.0f;
17869 for (int i = -ky; i <= ky; i++) {
17870 iy = i * step ;
17871 for (int j = -kx; j <= kx; j++) {
17872 ix = j*step ;
17873 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17874 Frngs(cimage, numr);
17875
17876
17877 for ( iref = 0; iref < (int)crefim_len; iref++) {
17878 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
17879 double qn = retvals["qn"];
17880 if(qn >= peak) {
17881 sx = -ix;
17882 sy = -iy;
17883 nref = iref;
17884 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17885 peak = static_cast<float>(qn);
17886 }
17887 } delete cimage; cimage = 0;
17888 }
17889 }
17890 float co, so, sxs, sys;
17891 co = static_cast<float>( cos(ang*pi/180.0) );
17892 so = static_cast<float>( -sin(ang*pi/180.0) );
17893 sxs = sx*co - sy*so;
17894 sys = sx*so + sy*co;
17895 vector<float> res;
17896 res.push_back(ang);
17897 res.push_back(sxs);
17898 res.push_back(sys);
17899 res.push_back(static_cast<float>(nref));
17900 res.push_back(peak);
17901 return res;
17902 }
17903
17904 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
17905 float xrng, float yrng, float step, float ant, string mode,
17906 vector<int>numr, float cnx, float cny) {
17907
17908
17909
17910
17911
17912
17913
17914
17915
17916
17917
17918 size_t crefim_len = crefim.size();
17919 const float qv = static_cast<float>( pi/180.0 );
17920
17921 Transform * t = image->get_attr("xform.projection");
17922 Dict d = t->get_params("spider");
17923 if(t) {delete t; t=0;}
17924 float phi = d["phi"];
17925 float theta = d["theta"];
17926 int ky = int(2*yrng/step+0.5)/2;
17927 int kx = int(2*xrng/step+0.5)/2;
17928 int iref, nref=0, mirror=0;
17929 float iy, ix, sx=0, sy=0;
17930 float peak = -1.0E23f;
17931 float ang=0.0f;
17932 float imn1 = sin(theta*qv)*cos(phi*qv);
17933 float imn2 = sin(theta*qv)*sin(phi*qv);
17934 float imn3 = cos(theta*qv);
17935 vector<float> n1(crefim_len);
17936 vector<float> n2(crefim_len);
17937 vector<float> n3(crefim_len);
17938 for ( iref = 0; iref < (int)crefim_len; iref++) {
17939 n1[iref] = crefim[iref]->get_attr("n1");
17940 n2[iref] = crefim[iref]->get_attr("n2");
17941 n3[iref] = crefim[iref]->get_attr("n3");
17942 }
17943 for (int i = -ky; i <= ky; i++) {
17944 iy = i * step ;
17945 for (int j = -kx; j <= kx; j++) {
17946 ix = j*step;
17947 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17948
17949 Normalize_ring( cimage, numr );
17950
17951 Frngs(cimage, numr);
17952
17953
17954 for ( iref = 0; iref < (int)crefim_len; iref++) {
17955 if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
17956 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17957 double qn = retvals["qn"];
17958 double qm = retvals["qm"];
17959 if(qn >= peak || qm >= peak) {
17960 sx = -ix;
17961 sy = -iy;
17962 nref = iref;
17963 if (qn >= qm) {
17964 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17965 peak = static_cast<float>( qn );
17966 mirror = 0;
17967 } else {
17968 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17969 peak = static_cast<float>( qm );
17970 mirror = 1;
17971 }
17972 }
17973 }
17974 } delete cimage; cimage = 0;
17975 }
17976 }
17977 float co, so, sxs, sys;
17978 if(peak == -1.0E23) {
17979 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
17980 nref = -1;
17981 } else {
17982 co = cos(ang*qv);
17983 so = -sin(ang*qv);
17984 sxs = sx*co - sy*so;
17985 sys = sx*so + sy*co;
17986 }
17987 vector<float> res;
17988 res.push_back(ang);
17989 res.push_back(sxs);
17990 res.push_back(sys);
17991 res.push_back(static_cast<float>(mirror));
17992 res.push_back(static_cast<float>(nref));
17993 res.push_back(peak);
17994 return res;
17995 }
17996
17997 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
17998 float xrng, float yrng, float step, float ant, float psi_max, string mode,
17999 vector<int>numr, float cnx, float cny) {
18000
18001
18002
18003
18004
18005
18006
18007
18008
18009
18010
18011 size_t crefim_len = crefim.size();
18012 const float qv = static_cast<float>(pi/180.0);
18013
18014 Transform* t = image->get_attr("xform.projection");
18015 Dict d = t->get_params("spider");
18016 if(t) {delete t; t=0;}
18017 float phi = d["phi"];
18018 float theta = d["theta"];
18019 float psi = d["psi"];
18020 int ky = int(2*yrng/step+0.5)/2;
18021 int kx = int(2*xrng/step+0.5)/2;
18022 int iref, nref = 0, mirror = 0;
18023 float iy, ix, sx = 0, sy = 0;
18024 float peak = -1.0E23f;
18025 float ang = 0.0f;
18026 float imn1 = sin(theta*qv)*cos(phi*qv);
18027 float imn2 = sin(theta*qv)*sin(phi*qv);
18028 float imn3 = cos(theta*qv);
18029 vector<float> n1(crefim_len);
18030 vector<float> n2(crefim_len);
18031 vector<float> n3(crefim_len);
18032 for (iref = 0; iref < (int)crefim_len; iref++) {
18033 n1[iref] = crefim[iref]->get_attr("n1");
18034 n2[iref] = crefim[iref]->get_attr("n2");
18035 n3[iref] = crefim[iref]->get_attr("n3");
18036 }
18037 bool nomirror = (theta<90.0) || (theta==90.0) && (psi<psi_max);
18038 if (!nomirror) {
18039 phi = fmod(phi+540.0f, 360.0f);
18040 theta = 180-theta;
18041 psi = fmod(540.0f-psi, 360.0f);
18042 }
18043 for (int i = -ky; i <= ky; i++) {
18044 iy = i * step ;
18045 for (int j = -kx; j <= kx; j++) {
18046 ix = j*step;
18047 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18048
18049 Normalize_ring(cimage, numr);
18050
18051 Frngs(cimage, numr);
18052
18053
18054 for (iref = 0; iref < (int)crefim_len; iref++) {
18055 if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18056 if (nomirror) {
18057 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 0);
18058 double qn = retvals["qn"];
18059 if (qn >= peak) {
18060 sx = -ix;
18061 sy = -iy;
18062 nref = iref;
18063 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18064 peak = static_cast<float>(qn);
18065 mirror = 0;
18066 }
18067 } else {
18068 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 1);
18069 double qn = retvals["qn"];
18070 if (qn >= peak) {
18071 sx = -ix;
18072 sy = -iy;
18073 nref = iref;
18074 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18075 peak = static_cast<float>(qn);
18076 mirror = 1;
18077 }
18078 }
18079 }
18080 } delete cimage; cimage = 0;
18081 }
18082 }
18083 float co, so, sxs, sys;
18084 if(peak == -1.0E23) {
18085 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18086 nref = -1;
18087 } else {
18088 co = cos(ang*qv);
18089 so = -sin(ang*qv);
18090 sxs = sx*co - sy*so;
18091 sys = sx*so + sy*co;
18092 }
18093 vector<float> res;
18094 res.push_back(ang);
18095 res.push_back(sxs);
18096 res.push_back(sys);
18097 res.push_back(static_cast<float>(mirror));
18098 res.push_back(static_cast<float>(nref));
18099 res.push_back(peak);
18100 return res;
18101 }
18102
18103
18104 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18105 float xrng, float yrng, float step, float psi_max, string mode,
18106 vector<int>numr, float cnx, float cny) {
18107
18108 size_t crefim_len = crefim.size();
18109
18110 int ky = int(2*yrng/step+0.5)/2;
18111 int kx = int(2*xrng/step+0.5)/2;
18112 int iref, nref=0, mirror=0;
18113 float iy, ix, sx=0, sy=0;
18114 float peak = -1.0E23f;
18115 float ang=0.0f;
18116 for (int i = -ky; i <= ky; i++) {
18117 iy = i * step ;
18118 for (int j = -kx; j <= kx; j++) {
18119 ix = j*step ;
18120 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18121
18122 Normalize_ring( cimage, numr );
18123
18124 Frngs(cimage, numr);
18125
18126
18127 for ( iref = 0; iref < (int)crefim_len; iref++) {
18128 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18129 double qn = retvals["qn"];
18130 double qm = retvals["qm"];
18131 if(qn >= peak || qm >= peak) {
18132 sx = -ix;
18133 sy = -iy;
18134 nref = iref;
18135 if (qn >= qm) {
18136 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18137 peak = static_cast<float>(qn);
18138 mirror = 0;
18139 } else {
18140 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18141 peak = static_cast<float>(qm);
18142 mirror = 1;
18143 }
18144 }
18145 } delete cimage; cimage = 0;
18146 }
18147 }
18148 float co, so, sxs, sys;
18149 co = static_cast<float>( cos(ang*pi/180.0) );
18150 so = static_cast<float>( -sin(ang*pi/180.0) );
18151 sxs = sx*co - sy*so;
18152 sys = sx*so + sy*co;
18153 vector<float> res;
18154 res.push_back(ang);
18155 res.push_back(sxs);
18156 res.push_back(sys);
18157 res.push_back(static_cast<float>(mirror));
18158 res.push_back(static_cast<float>(nref));
18159 res.push_back(peak);
18160 return res;
18161 }
18162
18163 void Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
18164 float xrng, float yrng, float step, string mode,
18165 vector< int >numr, float cnx, float cny,
18166 EMData *peaks, EMData *peakm) {
18167
18168 int maxrin = numr[numr.size()-1];
18169
18170 int ky = int(2*yrng/step+0.5)/2;
18171 int kx = int(2*xrng/step+0.5)/2;
18172
18173 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18174 float *p_ccf1ds = peaks->get_data();
18175
18176 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18177 float *p_ccf1dm = peakm->get_data();
18178
18179 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18180 p_ccf1ds[i] = -1.e20f;
18181 p_ccf1dm[i] = -1.e20f;
18182 }
18183
18184 for (int i = -ky; i <= ky; i++) {
18185 float iy = i * step;
18186 for (int j = -kx; j <= kx; j++) {
18187 float ix = j*step;
18188 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18189 Frngs(cimage, numr);
18190 Crosrng_msg_vec(crefim, cimage, numr,
18191 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18192 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18193 delete cimage; cimage = 0;
18194 }
18195 }
18196 return;
18197 }
18198
18199 void Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
18200 float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
18201 EMData *peaks_compress, EMData *peakm_compress) {
18202
18203 int maxrin = numr[numr.size()-1];
18204
18205 int ky = int(2*yrng/step+0.5)/2;
18206 int kx = int(2*xrng/step+0.5)/2;
18207
18208 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18209 float *p_ccf1ds = peaks->get_data();
18210
18211 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18212 float *p_ccf1dm = peakm->get_data();
18213
18214 peaks_compress->set_size(maxrin, 1, 1);
18215 float *p_ccf1ds_compress = peaks_compress->get_data();
18216
18217 peakm_compress->set_size(maxrin, 1, 1);
18218 float *p_ccf1dm_compress = peakm_compress->get_data();
18219
18220 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18221 p_ccf1ds[i] = -1.e20f;
18222 p_ccf1dm[i] = -1.e20f;
18223 }
18224
18225 for (int i = -ky; i <= ky; i++) {
18226 float iy = i * step;
18227 for (int j = -kx; j <= kx; j++) {
18228 float ix = j*step;
18229 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18230 Frngs(cimage, numr);
18231 Crosrng_msg_vec(crefim, cimage, numr,
18232 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18233 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18234 delete cimage; cimage = 0;
18235 }
18236 }
18237 for (int x=0; x<maxrin; x++) {
18238 float maxs = -1.0e22f;
18239 float maxm = -1.0e22f;
18240 for (int i=1; i<=2*ky+1; i++) {
18241 for (int j=1; j<=2*kx+1; j++) {
18242 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
18243 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
18244 }
18245 }
18246 p_ccf1ds_compress[x] = maxs;
18247 p_ccf1dm_compress[x] = maxm;
18248 }
18249 return;
18250 }
18251
18252 struct ccf_point
18253 {
18254 float value;
18255 int i;
18256 int j;
18257 int k;
18258 int mirror;
18259 };
18260
18261
18262 struct ccf_value
18263 {
18264 bool operator()( const ccf_point& a, const ccf_point& b )
18265 {
18266 return a.value > b.value;
18267 }
18268 };
18269
18270
18271 vector<float> Util::ali2d_ccf_list(EMData* image, EMData* crefim,
18272 float xrng, float yrng, float step, string mode,
18273 vector< int >numr, float cnx, float cny, double T) {
18274
18275 int maxrin = numr[numr.size()-1];
18276
18277 int ky = int(2*yrng/step+0.5)/2;
18278 int kx = int(2*xrng/step+0.5)/2;
18279
18280 float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
18281 float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
18282 int vol = maxrin*(2*kx+1)*(2*ky+1);
18283 vector<ccf_point> ccf(2*vol);
18284 ccf_point temp;
18285
18286 int index = 0;
18287 for (int i = -ky; i <= ky; i++) {
18288 float iy = i * step;
18289 for (int j = -kx; j <= kx; j++) {
18290 float ix = j*step;
18291 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18292 Frngs(cimage, numr);
18293 Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
18294 for (int k=0; k<maxrin; k++) {
18295 temp.value = p_ccf1ds[k];
18296 temp.i = k;
18297 temp.j = j;
18298 temp.k = i;
18299 temp.mirror = 0;
18300 ccf[index] = temp;
18301 index++;
18302 temp.value = p_ccf1dm[k];
18303 temp.mirror = 1;
18304 ccf[index] = temp;
18305 index++;
18306 }
18307 delete cimage; cimage = 0;
18308 }
18309 }
18310
18311 delete p_ccf1ds;
18312 delete p_ccf1dm;
18313 std::sort(ccf.begin(), ccf.end(), ccf_value());
18314
18315 double qt = (double)ccf[0].value;
18316 vector <double> p(2*vol), cp(2*vol);
18317
18318 double sump = 0.0;
18319 for (int i=0; i<2*vol; i++) {
18320 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
18321 sump += p[i];
18322 }
18323 for (int i=0; i<2*vol; i++) {
18324 p[i] /= sump;
18325 }
18326 for (int i=1; i<2*vol; i++) {
18327 p[i] += p[i-1];
18328 }
18329 p[2*vol-1] = 2.0;
18330
18331 float t = get_frand(0.0f, 1.0f);
18332 int select = 0;
18333 while (p[select] < t) select += 1;
18334
18335 vector<float> a(6);
18336 a[0] = ccf[select].value;
18337 a[1] = (float)ccf[select].i;
18338 a[2] = (float)ccf[select].j;
18339 a[3] = (float)ccf[select].k;
18340 a[4] = (float)ccf[select].mirror;
18341 a[5] = (float)select;
18342 return a;
18343 }
18344
18345
18346
18347
18348
18349
18350
18351
18352
18353
18354
18355
18356
18357
18358
18359
18360
18361
18362
18363
18364
18365
18366
18367
18368
18369
18370
18371
18372
18373
18374
18375
18376
18377
18378
18379
18380
18381
18382
18383
18384
18385
18386
18387
18388
18389
18390
18391
18392
18393
18394
18395
18396
18397
18398
18399
18400
18401
18402
18403
18404
18405
18406
18407
18408
18409
18410
18411
18412 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18413
18414 EMData *rot;
18415
18416 const int nmax=3, mmax=3;
18417 char task[60], csave[60];
18418 long int lsave[4];
18419 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18420 double f, f1, f2, f3, factr, pgtol, x[nmax], l[nmax], u[nmax], g[nmax], dsave[29], wa[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
18421 long int SIXTY=60;
18422
18423
18424 iprint = -1;
18425
18426
18427 factr=1.0e1;
18428 pgtol=1.0e-5;
18429
18430
18431
18432
18433 n=3;
18434 m=3;
18435
18436
18437
18438
18439
18440 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18441 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18442 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18443
18444
18445
18446
18447 strcpy(task,"START");
18448 for (int i=5;i<60;i++) task[i]=' ';
18449
18450
18451
18452 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18453
18454
18455
18456 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18457
18458 if (strncmp(task,"FG",2)==0) {
18459
18460
18461
18462
18463 rot = new EMData();
18464 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
18465 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18466
18467 delete rot;
18468
18469
18470 float dt = 1.0e-3f;
18471 rot = new EMData();
18472 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
18473 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18474
18475 g[0] = (f1-f)/dt;
18476 delete rot;
18477
18478 dt = 1.0e-2f;
18479 rot = new EMData();
18480 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
18481 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18482
18483 g[1] = (f2-f)/dt;
18484 delete rot;
18485
18486 rot = new EMData();
18487 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
18488 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18489
18490 g[2] = (f3-f)/dt;
18491 delete rot;
18492 }
18493
18494
18495 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18496
18497 }
18498
18499
18500 vector<float> res;
18501 res.push_back(static_cast<float>(x[0]));
18502 res.push_back(static_cast<float>(x[1]));
18503 res.push_back(static_cast<float>(x[2]));
18504
18505 return res;
18506 }
18507
18508 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
18509
18510 EMData *rot;
18511
18512 const int nmax=3, mmax=3;
18513 char task[60], csave[60];
18514 long int lsave[4];
18515 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18516 double f, f1, f2, f3, factr, pgtol, x[nmax], l[nmax], u[nmax], g[nmax], dsave[29], wa[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
18517 long int SIXTY=60;
18518
18519
18520 iprint = -1;
18521
18522
18523 factr=1.0e1;
18524 pgtol=1.0e-5;
18525
18526
18527
18528
18529 n=3;
18530 m=3;
18531
18532
18533
18534
18535
18536 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18537 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18538 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18539
18540
18541
18542
18543 strcpy(task,"START");
18544 for (int i=5;i<60;i++) task[i]=' ';
18545
18546
18547
18548 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18549
18550
18551
18552 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18553
18554 if (strncmp(task,"FG",2)==0) {
18555
18556
18557
18558
18559 rot = new EMData();
18560 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18561 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18562
18563 delete rot;
18564
18565
18566 float dt = 1.0e-3f;
18567 rot = new EMData();
18568 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18569 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18570
18571 g[0] = (f1-f)/dt;
18572 delete rot;
18573
18574 rot = new EMData();
18575 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
18576 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18577
18578 g[1] = (f2-f)/dt;
18579 delete rot;
18580
18581 rot = new EMData();
18582 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
18583 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18584
18585 g[2] = (f3-f)/dt;
18586 delete rot;
18587 }
18588
18589
18590 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18591
18592 }
18593
18594
18595 vector<float> res;
18596 res.push_back(static_cast<float>(x[0]));
18597 res.push_back(static_cast<float>(x[1]));
18598 res.push_back(static_cast<float>(x[2]));
18599
18600 return res;
18601 }
18602
18603 vector<float> Util::twoD_to_3D_ali(EMData* volft, Util::KaiserBessel& kb, EMData *refim, EMData* mask, float phi, float theta, float psi, float sxs, float sys) {
18604
18605 EMData *proj, *proj2;
18606
18607 const int nmax=5, mmax=5;
18608 char task[60], csave[60];
18609 long int lsave[4];
18610 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18611 double f, ft, factr, pgtol, x[nmax], l[nmax], u[nmax], g[nmax], dsave[29], wa[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
18612 long int SIXTY=60;
18613
18614
18615 iprint = -1;
18616
18617
18618 factr=1.0e1;
18619 pgtol=1.0e-5;
18620
18621
18622
18623
18624 n=5;
18625 m=5;
18626
18627
18628
18629
18630
18631 x[0] = phi; nbd[0] = 2; l[0] = phi-2.0; u[0] = phi+2.0;
18632 x[1] = theta; nbd[1] = 2; l[1] = theta-2.0; u[1] = theta+2.0;
18633 x[2] = psi; nbd[2] = 2; l[2] = psi-2.0; u[2] = psi+2.0;
18634 x[3] = sxs; nbd[3] = 2; l[3] = sxs-2.0; u[3] = sxs+2.0;
18635 x[4] = sys; nbd[4] = 2; l[4] = sys-2.0; u[4] = sys+2.0;
18636
18637
18638
18639
18640 strcpy(task,"START");
18641 for (int i=5;i<60;i++) task[i]=' ';
18642
18643
18644
18645 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18646 int step = 1;
18647
18648
18649 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18650
18651 if (strncmp(task,"FG",2)==0) {
18652
18653
18654
18655
18656 proj = new EMData();
18657 proj2 = new EMData();
18658 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18659 proj->fft_shuffle();
18660 proj->center_origin_fft();
18661 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18662 proj->do_ift_inplace();
18663 int M = proj->get_ysize()/2;
18664 proj2 = proj->window_center(M);
18665 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18666
18667 delete proj;
18668 delete proj2;
18669
18670
18671 float dt = 1.0e-3f;
18672 proj = new EMData();
18673 proj2 = new EMData();
18674 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
18675 proj->fft_shuffle();
18676 proj->center_origin_fft();
18677 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18678 proj->do_ift_inplace();
18679 proj2 = proj->window_center(M);
18680 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18681
18682 delete proj;
18683 delete proj2;
18684 g[0] = (ft-f)/dt;
18685
18686 proj = new EMData();
18687 proj2 = new EMData();
18688 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
18689 proj->fft_shuffle();
18690 proj->center_origin_fft();
18691 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18692 proj->do_ift_inplace();
18693 proj2 = proj->window_center(M);
18694 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18695
18696 delete proj;
18697 delete proj2;
18698 g[1] = (ft-f)/dt;
18699
18700 proj = new EMData();
18701 proj2 = new EMData();
18702 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
18703 proj->fft_shuffle();
18704 proj->center_origin_fft();
18705 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18706 proj->do_ift_inplace();
18707 proj2 = proj->window_center(M);
18708 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18709
18710 delete proj;
18711 delete proj2;
18712 g[2] = (ft-f)/dt;
18713
18714 proj = new EMData();
18715 proj2 = new EMData();
18716 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18717 proj->fft_shuffle();
18718 proj->center_origin_fft();
18719 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
18720 proj->do_ift_inplace();
18721 proj2 = proj->window_center(M);
18722 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18723
18724 delete proj;
18725 delete proj2;
18726 g[3] = (ft-f)/dt;
18727
18728 proj = new EMData();
18729 proj2 = new EMData();
18730 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18731 proj->fft_shuffle();
18732 proj->center_origin_fft();
18733 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
18734 proj->do_ift_inplace();
18735 proj2 = proj->window_center(M);
18736 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18737
18738 delete proj;
18739 delete proj2;
18740 g[4] = (ft-f)/dt;
18741 }
18742
18743
18744 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18745 step++;
18746 }
18747
18748
18749 vector<float> res;
18750 res.push_back(static_cast<float>(x[0]));
18751 res.push_back(static_cast<float>(x[1]));
18752 res.push_back(static_cast<float>(x[2]));
18753 res.push_back(static_cast<float>(x[3]));
18754 res.push_back(static_cast<float>(x[4]));
18755
18756 return res;
18757 }
18758
18759
18760 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18761
18762 double x[4];
18763 int n;
18764 int l = 3;
18765 int m = 200;
18766 double e = 1e-9;
18767 double step = 0.01;
18768 float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
18769
18770 x[1] = ang;
18771 x[2] = sxs;
18772 x[3] = sys;
18773
18774 Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);
18775
18776
18777 vector<float> res;
18778 res.push_back(static_cast<float>(x[1]));
18779 res.push_back(static_cast<float>(x[2]));
18780 res.push_back(static_cast<float>(x[3]));
18781 res.push_back(static_cast<float>(n));
18782 return res;
18783 }
18784
18785
18786 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
18787
18788 EMData *rot= new EMData();
18789 float ccc;
18790
18791 rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
18792 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18793 delete rot;
18794 return ccc;
18795 }
18796
18797 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
18798
18799 double x[4];
18800 int n;
18801 int l = 3;
18802 int m = 200;
18803 double e = 1e-9;
18804 double step = 0.001;
18805 float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
18806
18807 x[1] = ang;
18808 x[2] = sxs;
18809 x[3] = sys;
18810
18811 Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);
18812
18813
18814 vector<float> res;
18815 res.push_back(static_cast<float>(x[1]));
18816 res.push_back(static_cast<float>(x[2]));
18817 res.push_back(static_cast<float>(x[3]));
18818 res.push_back(static_cast<float>(n));
18819 return res;
18820 }
18821
18822
18823 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
18824
18825 EMData *rot= new EMData();
18826 float ccc;
18827
18828 rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
18829 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18830 delete rot;
18831 return ccc;
18832 }
18833
18834 #define img_ptr(i,j,k) img_ptr[i+(j+(k*ny))*nx]
18835 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*nx]
18836 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
18837 {
18838 ENTERFUNC;
18839
18840 if (!img) {
18841 throw NullPointerException("NULL input image");
18842 }
18843
18844 int newx, newy, newz;
18845 bool keep_going;
18846 cout << " entered " <<endl;
18847 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
18848
18849 EMData * img2 = new EMData();
18850 img2->set_size(nx,ny,nz);
18851 img2->to_zero();
18852 float *img_ptr =img->get_data();
18853 float *img2_ptr = img2->get_data();
18854 int r2 = ro*ro;
18855 int r3 = r2*ro;
18856 int ri2 = ri*ri;
18857 int ri3 = ri2*ri;
18858
18859 int n2 = nx/2;
18860
18861 for (int k=-n2; k<=n2; k++) {
18862 float z2 = static_cast<float>(k*k);
18863 for (int j=-n2; j<=n2; j++) {
18864 float y2 = z2 + j*j;
18865 if(y2 <= r2) {
18866
18867
18868 for (int i=-n2; i<=n2; i++) {
18869 float x2 = y2 + i*i;
18870 if(x2 <= r3) {
18871
18872 int ib = i+n2; int jb = j+n2; int kb = k+n2;
18873 if(x2 >= ri3) {
18874
18875 if(img_ptr(ib,jb,kb) == 1.0f) {
18876
18877 if(Util::get_frand(0.0f, 1.0f) > qprob){
18878 img2_ptr(ib,jb,kb) = 0.0f;
18879 keep_going = true;
18880
18881 while(keep_going) {
18882 newx = Util::get_irand(-ro,ro);
18883 newy = Util::get_irand(-ro,ro);
18884 newz = Util::get_irand(-ro,ro);
18885 if(newx*newx+newy*newy+newz*newz <= r3) {
18886 newx += n2; newy += n2; newz += n2;
18887 if( img_ptr(newx,newy,newz) == 0.0f) {
18888 img2_ptr(newx,newy,newz) = 1.0f;
18889 keep_going = false; }
18890 }
18891 }
18892 } else img2_ptr(ib,jb,kb) = 1.0f;
18893 }
18894 } else {
18895
18896 if(img_ptr(ib,jb,kb) == 1.0) {
18897 if(Util::get_frand(0.0f,1.0f) > qprob) {
18898
18899 float numn = -1.0f;
18900 for (newz = -1; newz <= 1; newz++)
18901 for (newy = -1; newy <= 1; newy++)
18902 for (newx = -1; newx <= 1; newx++)
18903 numn += img_ptr(ib+newx,jb+newy,kb+newz);
18904 img2_ptr(ib,jb,kb) = 0.0;
18905 if(numn == 26.0f) {
18906
18907 keep_going = true;
18908 while(keep_going) {
18909 newx = Util::get_irand(-ro,ro);
18910 newy = Util::get_irand(-ro,ro);
18911 newz = Util::get_irand(-ro,ro);
18912 if(newx*newx+newy*newy+newz*newz < r3) {
18913 newx += n2; newy += n2; newz += n2;
18914 if( img_ptr(newx,newy,newz) == 0.0f) {
18915 if(newx*newx+newy*newy+newz*newz < r3) {
18916 if(newx*newx+newy*newy+newz*newz < r3) {
18917 newx += n2; newy += n2; newz += n2;
18918 if( img_ptr(newx,newy,newz) == 0.0f) {
18919 img2_ptr(newx,newy,newz) = 1.0f;
18920 keep_going = false; }
18921 }
18922 }
18923 }
18924 }
18925 }
18926 } else if(numn == 25.0f) {
18927
18928 for (newz = -1; newz <= 1; newz++) {
18929 for (newy = -1; newy <= 1; newy++) {
18930 for (newx = -1; newx <= 1; newx++) {
18931 if( newx != 0 && newy != 0 && newz != 0) {
18932 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
18933 img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
18934 }
18935 }
18936 }
18937 }
18938 }
18939 } else {
18940
18941 keep_going = true;
18942 while(keep_going) {
18943 newx = Util::get_irand(-1,1);
18944 newy = Util::get_irand(-1,1);
18945 newz = Util::get_irand(-1,1);
18946 if(newx != 0 && newy != 0 && newz != 0) {
18947 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
18948 img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;
18949 keep_going = false;
18950 }
18951 }
18952 }
18953 }
18954 } else img2_ptr(ib,jb,kb) = 1.0f;
18955 }
18956 }
18957 }
18958 }
18959 }
18960 }
18961 }
18962
18963 img2->update();
18964
18965 EXITFUNC;
18966 return img2;
18967 }
18968 #undef img_ptr
18969 #undef img2_ptr
18970
18971 struct point3d_t
18972 {
18973 point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
18974
18975 int x;
18976 int y;
18977 int z;
18978 };
18979
18980
18981 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
18982 {
18983 int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
18984 int noff = 6;
18985
18986 int nx = visited->get_xsize();
18987 int ny = visited->get_ysize();
18988 int nz = visited->get_zsize();
18989
18990 vector< point3d_t > pts;
18991 pts.push_back( point3d_t(ix, iy, iz) );
18992 visited->set_value_at( ix, iy, iz, (float)grpid );
18993
18994 int start = 0;
18995 int end = pts.size();
18996
18997 while( end > start ) {
18998 for(int i=start; i < end; ++i ) {
18999 int ix = pts[i].x;
19000 int iy = pts[i].y;
19001 int iz = pts[i].z;
19002
19003 for( int j=0; j < noff; ++j ) {
19004 int jx = ix + offs[j][0];
19005 int jy = iy + offs[j][1];
19006 int jz = iz + offs[j][2];
19007
19008 if( jx < 0 || jx >= nx ) continue;
19009 if( jy < 0 || jy >= ny ) continue;
19010 if( jz < 0 || jz >= nz ) continue;
19011
19012
19013 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
19014 pts.push_back( point3d_t(jx, jy, jz) );
19015 visited->set_value_at( jx, jy, jz, (float)grpid );
19016 }
19017
19018 }
19019 }
19020
19021 start = end;
19022 end = pts.size();
19023 }
19024 return pts.size();
19025 }
19026
19027
19028 EMData* Util::get_biggest_cluster( EMData* mg )
19029 {
19030 int nx = mg->get_xsize();
19031 int ny = mg->get_ysize();
19032 int nz = mg->get_zsize();
19033
19034 EMData* visited = new EMData();
19035 visited->set_size( nx, ny, nz );
19036 visited->to_zero();
19037 int grpid = 0;
19038 int maxgrp = 0;
19039 int maxsize = 0;
19040 for( int iz=0; iz < nz; ++iz ) {
19041 for( int iy=0; iy < ny; ++iy ) {
19042 for( int ix=0; ix < nx; ++ix ) {
19043 if( (*mg)(ix, iy, iz)==0.0 ) continue;
19044
19045 if( (*visited)(ix, iy, iz) > 0.0 ) {
19046
19047 continue;
19048 }
19049
19050 grpid++;
19051 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
19052 if( grpsize > maxsize ) {
19053 maxsize = grpsize;
19054 maxgrp = grpid;
19055 }
19056 }
19057 }
19058 }
19059
19060 Assert( maxgrp > 0 );
19061
19062 int npoint = 0;
19063 EMData* result = new EMData();
19064 result->set_size( nx, ny, nz );
19065 result->to_zero();
19066
19067 for( int iz=0; iz < nz; ++iz ) {
19068 for( int iy=0; iy < ny; ++iy ) {
19069 for( int ix=0; ix < nx; ++ix ) {
19070 if( (*visited)(ix, iy, iz)==maxgrp ) {
19071 (*result)(ix,iy,iz) = 1.0;
19072 npoint++;
19073 }
19074 }
19075 }
19076 }
19077
19078 Assert( npoint==maxsize );
19079 delete visited;
19080 return result;
19081
19082 }
19083
19084 EMData* Util::ctf_img(int nx, int ny, int nz, float dz,float ps,float voltage,float cs, float wgh,float b_factor,float dza, float azz, float sign)
19085 {
19086 int ix, iy, iz;
19087 int i, j, k;
19088 int nr2, nl2;
19089 float dzz, az, ak;
19090 float scx, scy, scz;
19091 int offset = 2 - nx%2;
19092 int lsm = nx + offset;
19093 EMData* ctf_img1 = new EMData();
19094 ctf_img1->set_size(lsm, ny, nz);
19095 float freq = 1.0f/(2.0f*ps);
19096 scx = 2.0f/float(nx);
19097 if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
19098 if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
19099 nr2 = ny/2 ;
19100 nl2 = nz/2 ;
19101 for ( k=0; k<nz;k++) {
19102 iz = k; if(k>nl2) iz=k-nz;
19103 for ( j=0; j<ny;j++) {
19104 iy = j; if(j>nr2) iy=j - ny;
19105 for ( i=0; i<lsm/2; i++) {
19106 ix=i;
19107 ak=pow(ix*ix*scx*scx+iy*scy*iy*scy+iz*scz*iz*scz, 0.5f)*freq;
19108 if(ak!=0) az=0.0; else az=M_PI;
19109 dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f));
19110 (*ctf_img1) (i*2,j,k) = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
19111 (*ctf_img1) (i*2+1,j,k) = 0.0f;
19112 }
19113 }
19114 }
19115 ctf_img1->update();
19116 ctf_img1->set_complex(true);
19117 ctf_img1->set_ri(true);
19118
19119
19120 if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
19121 return ctf_img1;
19122 }
19123
19124
19125
19126
19127
19128
19129
19130
19131
19132
19133
19134
19135
19136
19137
19138
19139
19140
19141
19142
19143
19144
19145
19146
19147
19148
19149
19150
19151
19152
19153
19154
19155
19156
19157
19158
19159
19160
19161
19162
19163
19164
19165
19166
19167
19168
19169
19170
19171
19172
19173
19174
19175
19176
19177
19178
19179
19180
19181
19182
19183
19184
19185
19186
19187
19188
19189
19190
19191
19192
19193
19194
19195
19196
19197
19198
19199
19200
19201
19202
19203
19204
19205
19206
19207
19208
19209
19210 #define cent(i) out[i+N]
19211 #define assign(i) out[i]
19212 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
19213 int nx = d->get_xsize();
19214 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19215 vector<float> out(N+K+2);
19216 if(N*(N-1)/2 != nx) {
19217
19218 return out;}
19219
19220 for(int i=0; i<N; i++) assign(i) = float(i);
19221
19222 for(int i=0; i<N; i++) {
19223 int j = Util::get_irand(0,N-1);
19224 float temp = assign(i);
19225 assign(i) = assign(j);
19226 assign(j) = temp;
19227 }
19228 for(int k=0; k<K; k++) cent(k) = float(assign(k));
19229
19230
19231 for(int i=0; i<N; i++) assign(i) = 0.0f;
19232 float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
19233 bool change = true;
19234 int it = -1;
19235 int ct = -1;
19236 while(change && disp < dispold || ct > 0) {
19237
19238 change = false;
19239 dispold = disp;
19240 it++;
19241
19242
19243 disp = 0.0f;
19244 ct = 0;
19245 for(int i=0; i<N; i++) {
19246 qm = 1.0e23f;
19247 for(int k=0; k<K; k++) {
19248 if(float(i) == cent(k)) {
19249 qm = 0.0f;
19250 na = (float)k;
19251 } else {
19252 float dt = (*d)(mono(i,int(cent(k))));
19253 if(dt < qm) {
19254 qm = dt;
19255 na = (float)k;
19256 }
19257 }
19258 }
19259
19260
19261
19262 if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
19263 na = (float)(Util::get_irand(0, K));
19264 qm = (*d)(mono(i,int(na)));
19265 ct++;
19266 }
19267
19268 disp += qm;
19269
19270 if(na != assign(i)) {
19271 assign(i) = na;
19272 change = true;
19273 }
19274 }
19275
19276
19277 T = T*F;
19278
19279
19280
19281
19282
19283 for(int k=0; k<K; k++) {
19284 qm = 1.0e23f;
19285 for(int i=0; i<N; i++) {
19286 if(assign(i) == float(k)) {
19287 float q = 0.0;
19288 for(int j=0; j<N; j++) {
19289 if(assign(j) == float(k)) {
19290
19291 if(i != j) q += (*d)(mono(i,j));
19292
19293 }
19294 }
19295 if(q < qm) {
19296
19297 qm = q;
19298 cent(k) = float(i);
19299 }
19300 }
19301 }
19302 }
19303
19304 }
19305 out[N+K] = disp;
19306 out[N+K+1] = float(it);
19307 return out;
19308 }
19309 #undef cent
19310 #undef assign
19311
19312
19313
19314
19315
19316
19317
19318
19319
19320
19321
19322
19323
19324
19325
19326
19327
19328
19329
19330
19331
19332
19333
19334
19335
19336
19337
19338
19339
19340
19341
19342
19343
19344
19345
19346
19347
19348
19349
19350
19351
19352
19353
19354
19355
19356
19357
19358
19359
19360
19361
19362
19363
19364
19365
19366
19367
19368
19369
19370
19371
19372
19373
19374
19375
19376
19377
19378
19379
19380
19381
19382
19383
19384
19385
19386
19387
19388
19389
19390
19391
19392
19393
19394
19395
19396
19397
19398
19399
19400
19401
19402
19403
19404
19405
19406
19407
19408
19409
19410
19411
19412
19413
19414
19415
19416
19417
19418
19419
19420 vector<float> Util::cluster_equalsize(EMData* d) {
19421
19422 int nx = d->get_xsize();
19423 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19424 int K = N/2;
19425 vector<float> group(N);
19426 if(N*(N-1)/2 != nx) {
19427
19428 return group;}
19429
19430 bool * active = new bool[N];
19431 for(int i=0; i<N; i++) active[i] = true;
19432
19433 float dm, qd;
19434 int ppi = 0, ppj = 0;
19435 for(int k=0; k<K; k++) {
19436
19437
19438 dm = 1.0e23f;
19439 for(int i=1; i<N; i++) {
19440 if(active[i]) {
19441 for(int j=0; j<i; j++) {
19442 if(active[j]) {
19443 qd = (*d)(i*(i - 1)/2 + j);
19444 if(qd < dm) {
19445 dm = qd;
19446 ppi = i;
19447 ppj = j;
19448 }
19449 }
19450 }
19451 }
19452 }
19453 group[2*k] = float(ppi);
19454 group[1+2*k] = float(ppj);
19455 active[ppi] = false;
19456 active[ppj] = false;
19457 }
19458
19459 delete [] active;
19460 active = NULL;
19461 return group;
19462 }
19463
19464
19465
19466
19467
19468
19469
19470
19471
19472
19473
19474
19475
19476
19477
19478
19479
19480
19481
19482
19483
19484
19485
19486
19487
19488
19489
19490
19491
19492
19493
19494
19495
19496
19497
19498
19499
19500
19501
19502
19503
19504
19505
19506
19507
19508
19509
19510
19511
19512
19513
19514
19515
19516
19517 #define data(i,j) group[i*ny+j]
19518 vector<float> Util::vareas(EMData* d) {
19519 const float step=0.001f;
19520 int ny = d->get_ysize();
19521
19522
19523
19524 vector<float> group(2*ny);
19525 for(int i=0; i<2*ny; i++) group[i] = 0.0f;
19526 int K = int(1.0f/step) +1;
19527 int hit = 0;
19528 for(int kx=0; kx<=K; kx++) {
19529 float tx = kx*step;
19530 for(int ky=0; ky<=K; ky++) {
19531 float ty = ky*step;
19532 float dm = 1.0e23f;
19533 for(int i=0; i<ny; i++) {
19534 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
19535 if( qd < dm) {
19536 dm = qd;
19537 hit = i;
19538 }
19539 }
19540 data(0,hit) += 1.0f;
19541 if(kx == 0 || ky == 0 || kx == K || ky == K) data(1,hit) = 1.0f;
19542 }
19543 }
19544 return group;
19545 }
19546 #undef data
19547
19548 EMData* Util::get_slice(EMData *vol, int dim, int index) {
19549
19550 int nx = vol->get_xsize();
19551 int ny = vol->get_ysize();
19552 int nz = vol->get_zsize();
19553 float *vol_data = vol->get_data();
19554 int new_nx, new_ny;
19555
19556 if (nz == 1)
19557 throw ImageDimensionException("Error: Input must be a 3-D object");
19558 if ((dim < 1) || (dim > 3))
19559 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
19560 if (((dim == 1) && (index < 0 || index > nx-1)) ||
19561 ((dim == 1) && (index < 0 || index > nx-1)) ||
19562 ((dim == 1) && (index < 0 || index > nx-1)))
19563 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
19564
19565 if (dim == 1) {
19566 new_nx = ny;
19567 new_ny = nz;
19568 } else if (dim == 2) {
19569 new_nx = nx;
19570 new_ny = nz;
19571 } else {
19572 new_nx = nx;
19573 new_ny = ny;
19574 }
19575
19576 EMData *slice = new EMData();
19577 slice->set_size(new_nx, new_ny, 1);
19578 float *slice_data = slice->get_data();
19579
19580 if (dim == 1) {
19581 for (int x=0; x<new_nx; x++)
19582 for (int y=0; y<new_ny; y++)
19583 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
19584 } else if (dim == 2) {
19585 for (int x=0; x<new_nx; x++)
19586 for (int y=0; y<new_ny; y++)
19587 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
19588 } else {
19589 for (int x=0; x<new_nx; x++)
19590 for (int y=0; y<new_ny; y++)
19591 slice_data[y*new_nx+x] = vol_data[(index*ny+y)*nx+x];
19592 }
19593
19594 return slice;
19595 }
19596
19597 void Util::image_mutation(EMData *img, float mutation_rate) {
19598 int nx = img->get_xsize();
19599 float min = img->get_attr("minimum");
19600 float max = img->get_attr("maximum");
19601 float* img_data = img->get_data();
19602 array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
19603 return;
19604 }
19605
19606
19607 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
19608
19609 if (is_mirror != 0) {
19610 for (int i=0; i<len_list; i++) {
19611 int r = rand()%10000;
19612 float f = r/10000.0f;
19613 if (f < mutation_rate) list[i] = 1-list[i];
19614 }
19615 } else {
19616 map<int, vector<int> > graycode;
19617 map<vector<int>, int> rev_graycode;
19618 vector <int> gray;
19619
19620 int K=1;
19621 for (int i=0; i<L; i++) K*=2;
19622
19623 for (int k=0; k<K; k++) {
19624 int shift = 0;
19625 vector <int> gray;
19626 for (int i=L-1; i>-1; i--) {
19627 int t = ((k>>i)%2-shift)%2;
19628 gray.push_back(t);
19629 shift += t-2;
19630 }
19631 graycode[k] = gray;
19632 rev_graycode[gray] = k;
19633 }
19634
19635 float gap = (K-1)/(max_val-min_val);
19636 for (int i=0; i<len_list; i++) {
19637 float val = list[i];
19638 if (val < min_val) { val = min_val; }
19639 else if (val > max_val) { val = max_val; }
19640 int k = int((val-min_val)*gap+0.5);
19641 vector<int> gray = graycode[k];
19642 bool changed = false;
19643 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
19644 int r = rand()%10000;
19645 float f = r/10000.0f;
19646 if (f < mutation_rate) {
19647 *p = 1-*p;
19648 changed = true;
19649 }
19650 }
19651 if (changed) {
19652 k = rev_graycode[gray];
19653 list[i] = k/gap+min_val;
19654 }
19655 }
19656 }
19657
19658 }
19659
19660 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
19661
19662 if (is_mirror != 0) {
19663 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
19664 int r = rand()%10000;
19665 float f = r/10000.0f;
19666 if (f < mutation_rate) *q = 1-*q;
19667 }
19668 } else {
19669 map<int, vector<int> > graycode;
19670 map<vector<int>, int> rev_graycode;
19671 vector <int> gray;
19672
19673 int K=1;
19674 for (int i=0; i<L; i++) K*=2;
19675
19676 for (int k=0; k<K; k++) {
19677 int shift = 0;
19678 vector <int> gray;
19679 for (int i=L-1; i>-1; i--) {
19680 int t = ((k>>i)%2-shift)%2;
19681 gray.push_back(t);
19682 shift += t-2;
19683 }
19684 graycode[k] = gray;
19685 rev_graycode[gray] = k;
19686 }
19687
19688 float gap = (K-1)/(max_val-min_val);
19689 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
19690 float val = *q;
19691 if (val < min_val) { val = min_val; }
19692 else if (val > max_val) { val = max_val; }
19693 int k = int((val-min_val)*gap+0.5);
19694 vector<int> gray = graycode[k];
19695 bool changed = false;
19696 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
19697 int r = rand()%10000;
19698 float f = r/10000.0f;
19699 if (f < mutation_rate) {
19700 *p = 1-*p;
19701 changed = true;
19702 }
19703 }
19704 if (changed) {
19705 k = rev_graycode[gray];
19706 *q = k/gap+min_val;
19707 }
19708 }
19709 }
19710 return list;
19711 }
19712
19713
19714 void Util::bb_enumerate_(int* argParts, int* dimClasses, int nParts, int K, int T,int n_guesses, int* Levels) {
19715
19716 int* Indices = new int[nParts*K];
19717
19718 vector <vector <int*> > Parts(nParts,vector<int*>(K));
19719 int ind_c = 0;
19720
19721 for (int i=0; i < nParts; i++){
19722 for(int j = 0; j < K; j++){
19723 Parts[i][j]=argParts + ind_c;
19724 Indices[i*K + j] = ind_c;
19725 ind_c = ind_c + *(dimClasses+i*K + j);
19726
19727 }
19728 }
19729
19730
19731
19732 vector <vector <int*> > oldParts = Parts;
19733
19734
19735
19736
19737 Util::initial_prune(Parts, dimClasses, nParts, K,T);
19738
19739
19740
19741 int numLevels = Parts[0].size();
19742 for (int i=1; i < nParts; i++){
19743 if (Parts[i].size() < numLevels) numLevels = Parts[i].size();
19744 }
19745
19746
19747
19748
19749
19750
19751
19752
19753
19754
19755
19756
19757
19758
19759
19760 for(int i = 0; i < nParts; i++){
19761 for(int j=0; j < K; j++){
19762 *(argParts + Indices[i*K + j]+1) = -1;
19763 }
19764 }
19765
19766 int num_classes;
19767 int old_index;
19768 for(int i=0; i<nParts; i++){
19769 num_classes = Parts[i].size();
19770
19771 for (int j=0; j < num_classes; j++){
19772 old_index = *(Parts[i][j]);
19773
19774 *(argParts + Indices[i*K + old_index]+1) = 1;
19775 }
19776 }
19777
19778
19779
19780
19781 cout <<"begin partition matching\n";
19782 int* output = Util::branch(argParts, Indices,dimClasses, nParts, K, T,Levels, numLevels,0,n_guesses);
19783 cout <<"done with partition matching \n";
19784 cout<<"total cost: "<<*output<<"\n";
19785 cout<<"number of matches: "<<*(output+1)<<"\n";
19786
19787 bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
19788 }
19789
19790 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
19791
19792 int total_cost = *output;
19793 int num_matches = *(output+1);
19794
19795 int cost=0;
19796 int* intx;
19797 int intx_size;
19798 int* intx_next;
19799 int intx_next_size;
19800 int curclass;
19801 int curclass_size;
19802
19803 for(int i = 0; i < num_matches; i++){
19804 curclass = *(output+2+ i*nParts);
19805
19806 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
19807 *(argParts + Indices[curclass]+1) = -5;
19808
19809 curclass_size = *(dimClasses+curclass)-2;
19810 intx = new int[curclass_size];
19811 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
19812 intx_size = curclass_size;
19813
19814 for (int j=1; j < nParts; j++){
19815 curclass = *(output+2+ i*nParts+j);
19816 if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
19817 *(argParts + Indices[j*K+curclass]+1)=-5;
19818
19819 intx_next_size = Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,0);
19820 intx_next = new int[intx_next_size];
19821 Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
19822 delete[] intx;
19823 intx=intx_next;
19824 intx_size= intx_next_size;
19825 if (j==nParts-1) delete[] intx_next;
19826 }
19827
19828 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
19829
19830 cost = cost + intx_next_size;
19831 }
19832
19833 if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
19834
19835 return 1;
19836
19837 }
19838
19839 int* Util::branch(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int curlevel,int n_guesses) {
19840
19841
19842 if (curlevel > nLevels-1){
19843 int* res = new int[2];
19844 *res = 0;
19845 *(res+1)=0;
19846 return res;
19847 }
19848
19849
19850 int nBranches = *(Levels + curlevel);
19851
19852
19853
19854
19855 int* matchlist = new int[nBranches*nParts];
19856 int* costlist = new int[nBranches];
19857
19858
19859
19860 for (int i=0; i < nBranches; i++) *(costlist+i)=0;
19861
19862
19863
19864
19865 Util::findTopLargest(argParts,Indices, dimClasses, nParts, K, T, matchlist, nBranches,costlist,n_guesses);
19866
19867
19868 if (costlist[0]<= T){
19869 int* res = new int[2];
19870 *res = 0;
19871 *(res+1)=0;
19872 return res;
19873 }
19874
19875 int* maxreturn = new int[2];
19876 *maxreturn=0;
19877 *(maxreturn+1)=0;
19878
19879
19880 int old_index;
19881 int totalcost;
19882 int nmatches;
19883
19884
19885 for(int i=0; i < nBranches ; i++){
19886
19887
19888
19889 if (costlist[i] <= T) break;
19890
19891
19892
19893
19894 for(int j=0; j < nParts; j++){
19895
19896 old_index=matchlist[i*nParts + j];
19897 *(argParts + Indices[j*K+old_index] + 1) = -2;
19898 }
19899
19900
19901 int* ret = Util::branch(argParts, Indices, dimClasses, nParts, K, T, Levels, nLevels, curlevel+1,n_guesses);
19902
19903
19904 totalcost = costlist[i] + *ret;
19905
19906
19907
19908
19909
19910 bool debug1 = 0;
19911 if (debug1){
19912 int maxLevel = 2;
19913 if (curlevel < maxLevel) cout<<"total cost level" << curlevel<<": "<<totalcost<<"\n";
19914 }
19915
19916
19917 bool debug2 = 0;
19918 if (debug2){
19919 int skip1 = 5;
19920 int max1=20;
19921 if ((curlevel <= max1) && (curlevel%skip1 == 0)) cout << "total cost level "<< curlevel<<": "<<totalcost<<"\n";
19922
19923 int skip2 = 10;
19924 int max2 = 70;
19925 if ((curlevel > max1 )&&(curlevel <= max2) && (curlevel%skip2 == 0)) cout << "total cost level "<< curlevel<<": "<<totalcost<<"\n";
19926 }
19927
19928
19929
19930
19931
19932
19933
19934
19935
19936
19937
19938 if (totalcost > *maxreturn)
19939
19940
19941
19942 {
19943 nmatches = 1 + *(ret+1);
19944 delete[] maxreturn;
19945 maxreturn = new int[2+nmatches*nParts];
19946 *maxreturn = totalcost;
19947 *(maxreturn + 1)= nmatches;
19948 int nret = 2+(nmatches-1)*nParts;
19949 for(int iret=2; iret <nret;iret++) *(maxreturn+iret)=*(ret+iret);
19950 for(int imax=0; imax<nParts;imax++) *(maxreturn+nret+imax)=matchlist[i*nParts + imax];
19951 }
19952
19953
19954 delete[] ret;
19955
19956
19957
19958 for(int j=0; j < nParts; j++){
19959
19960 old_index=matchlist[i*nParts + j];
19961 *(argParts + Indices[j*K+old_index] + 1) = 1;
19962 }
19963
19964 }
19965
19966 delete[] matchlist;
19967 delete[] costlist;
19968 return maxreturn;
19969
19970 }
19971
19972 int Util::findTopLargest(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int max_num_matches, int* costlist, int n_guesses){
19973 int guess;
19974 int* curmax = new int[nParts+1];
19975 int newT=T;
19976 int num_found=0;
19977
19978 for(int i=0; i < max_num_matches; i++){
19979 guess = Util::generatesubmax(argParts, Indices,dimClasses,nParts, K, T, n_guesses);
19980
19981 if (T < guess) newT = guess -1;
19982
19983 Util::search2(argParts, Indices,dimClasses,nParts, K, newT,curmax);
19984 if (*curmax <= T){
19985 max_num_matches=i;
19986 break;
19987 }
19988 else {
19989 *(costlist+i) = *curmax;
19990
19991 for (int j=0; j<nParts; j++){
19992 *(matchlist+i*nParts+j) = *(curmax+1+j);
19993 *(argParts + Indices[j*K+*(curmax+1+j)] + 1) = -3;
19994
19995 }
19996 num_found = num_found+1;
19997 }
19998
19999 }
20000
20001
20002 delete[] curmax;
20003
20004
20005 for (int i=0 ; i < max_num_matches; i++){
20006 for (int j = 0; j < nParts; j++){
20007 *(argParts + Indices[j*K+*(matchlist+i*nParts +j)] + 1) = 1;
20008 }
20009
20010 }
20011
20012
20013 return num_found;
20014 }
20015
20016
20017 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int newT, int* curmax){
20018
20019 *curmax= 0;
20020
20021 bool flag;
20022 int nintx;
20023 int* dummy;
20024 int* ret;
20025
20026 for(int a=0; a<K; a++)
20027 {
20028
20029 if (*(argParts + Indices[a] + 1) < 1) continue;
20030 if (*(dimClasses + a)-2 <= newT) continue;
20031
20032
20033
20034 for( int i=1; i < nParts; i++){
20035 flag = 0;
20036 for(int j=0; j < K; j++){
20037 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
20038 if (*(dimClasses + i*K+j)-2 <= newT) {*(argParts + Indices[i*K+j] + 1) =-4; continue;}
20039 nintx = Util::k_means_cont_table_(argParts + Indices[a]+2,argParts + Indices[i*K+j]+2, dummy, *(dimClasses + a)-2, *(dimClasses + i*K+j)-2,0);
20040 if (nintx > newT) flag=1;
20041 else *(argParts + Indices[i*K+j] + 1) =-4;
20042 }
20043 if (flag==0) {break;}
20044 }
20045
20046
20047 if (flag > 0){
20048 ret=Util::explore2(argParts, Indices, dimClasses, nParts, K, newT, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2, *(dimClasses+a)-2,0);
20049
20050 if (*ret > *curmax){
20051 *curmax = *ret;
20052 *(curmax+1)=a;
20053 for (int cp =0; cp < nParts-1; cp++) *(curmax+2+cp) = *(ret+1+cp);
20054
20055 }
20056 delete[] ret;
20057 }
20058
20059 for( int i=1; i < nParts; i++){
20060 for(int j=0; j < K; j++){
20061 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
20062
20063 }
20064 }
20065
20066
20067
20068 }
20069
20070
20071 }
20072
20073
20074 int* Util::explore2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int newT, int* curintx, int size_curintx, int* next, int size_next, int depth){
20075
20076 int* curintx2;
20077
20078 int nintx = size_curintx;
20079
20080
20081 if (depth >0){
20082 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
20083 if (nintx <= newT) {curintx2 = new int[1]; *curintx2=0;return curintx2;}
20084 }
20085
20086
20087 if (depth == (nParts-1)) { curintx2 = new int[1]; *curintx2 = nintx; return curintx2;}
20088
20089
20090
20091
20092 if (depth > 0){
20093 curintx2 = new int[nintx];
20094 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
20095 }
20096
20097 if (depth == 0){
20098
20099 curintx2 = new int[size_curintx];
20100 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
20101 }
20102
20103
20104
20105 depth=depth+1;
20106 int* curmax = new int[nParts-depth+1];
20107 *curmax=0;
20108 int* ret;
20109
20110 for (int i=0; i < K; i++){
20111
20112 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue;
20113 size_next = (*(dimClasses + depth*K+i ))-2;
20114 if (size_next <= newT) continue;
20115 ret = Util::explore2(argParts,Indices, dimClasses, nParts, K, newT, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth);
20116 if (*ret > *curmax && *ret > newT){
20117 *curmax = *ret;
20118 *(curmax+1)=i;
20119 for (int j=0; j<nParts-depth-1; j++) { *(curmax+2 + j) = *(ret+1+j);}
20120 }
20121 delete[] ret;
20122 }
20123
20124 delete[] curintx2;
20125 return curmax;
20126 }
20127
20128
20129 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
20130
20131
20132
20133
20134
20135
20136
20137
20138
20139
20140 int* dummy;
20141 int* cref;
20142 int cref_size;
20143 int* ccomp;
20144 int ccomp_size;
20145 int nintx;
20146 for (int i=0; i < nParts; i++){
20147 for (int j =0; j < K; j++){
20148
20149
20150 cref = Parts[i][j];
20151 cref_size = (*(dimClasses+i*K+(*cref)))-2;
20152
20153
20154 if (cref_size <= T){
20155
20156 *cref = -1;
20157 continue;
20158 }
20159 bool done = 0;
20160 for (int a = 0; a < nParts; a++){
20161 if (a == i) continue;
20162 bool hasActive=0;
20163 for (int b=0; b < Parts[a].size(); b++){
20164
20165
20166 ccomp = Parts[a][b];
20167 ccomp_size= (*(dimClasses+a*K+(*ccomp)))-2;
20168 nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
20169
20170
20171 if (nintx <= T)
20172 *(ccomp+1) = 0;
20173 else{
20174 *(ccomp+1)=1;
20175 hasActive=1;
20176 }
20177 }
20178
20179 if (hasActive < 1){
20180 done=1;
20181 break;
20182 }
20183
20184 }
20185
20186 if (done > 0){
20187
20188
20189 *cref = -1;
20190 continue;
20191 }
20192
20193
20194
20195
20196
20197
20198
20199
20200
20201
20202 bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
20203
20204 if (found<1){
20205
20206 *cref = -1;
20207 }
20208 }
20209
20210
20211
20212 for (int d = K-1; d > -1; d--){
20213 if (*(Parts[i][d]) < 0) Parts[i].erase(Parts[i].begin()+d);
20214 }
20215
20216 }
20217
20218
20219
20220
20221
20222 }
20223
20224
20225 bool Util::explore(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T, int partref, int* curintx, int size_curintx, int* next, int size_next, int depth) {
20226
20227
20228 if (size_next <= T) return 0;
20229
20230
20231 int* curintx2;
20232 int nintx = Util::k_means_cont_table_(curintx,next+2, curintx2, size_curintx, size_next,0);
20233 if (nintx <= T) return 0;
20234
20235 int old_depth=depth;
20236 if (depth == partref) depth = depth + 1;
20237 if (depth == (nParts)) { if (old_depth>0) return 1;}
20238
20239
20240
20241 curintx2 = new int[nintx];
20242 Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
20243
20244
20245
20246
20247 bool gt_thresh;
20248 int num_classes = (Parts[depth]).size();
20249
20250 for (int i=0; i < num_classes; i++){
20251 if (*(Parts[depth][i]+1) < 1) continue;
20252 size_next = (*(dimClasses + depth*K+(*(Parts[depth][i])) ))-2;
20253 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
20254 if (gt_thresh) return 1;
20255 }
20256 delete[] curintx2;
20257 return 0;
20258 }
20259
20260
20261 int Util::generatesubmax(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int n_guesses){
20262 int guess=0;
20263
20264 int* perm = new int[nParts];
20265 for(int i=0; i<nParts; i++) perm[i]=i;
20266
20267
20268 int* intx;
20269 int* intx_next;
20270 int nintx;
20271 int nintxmax=0;
20272 int class_max, class_max_next;
20273 int intx_size, part, part_next;
20274 int ipold,indsw;
20275
20276 for(int i=0; i< n_guesses; i++){
20277
20278 for(int ip = 0; ip<nParts; ip++){
20279 indsw = (rand())%nParts;
20280
20281 ipold = perm[ip];
20282 perm[ip]=perm[indsw];
20283 perm[indsw]=ipold;
20284 }
20285
20286
20287
20288 part=*perm;
20289 part_next=*(perm+1);
20290 for (int a=0; a < K; a++)
20291 {
20292 if (*(argParts + Indices[part*K+a] + 1) < 1) continue;
20293 for (int b=0; b < K; b++)
20294 {
20295 if (*(argParts + Indices[part_next*K + b] + 1) < 1) continue;
20296 nintx = Util::k_means_cont_table_(argParts + Indices[part*K+a]+2,argParts + Indices[part_next*K + b]+2, intx, *(dimClasses + part*K+a)-2, *(dimClasses + part_next*K + b)-2,0);
20297 if (nintx <= nintxmax) continue;
20298 nintxmax = nintx;
20299 class_max = a;
20300 class_max_next = b;
20301 }
20302 }
20303
20304
20305 if (nintxmax < 1) {continue;}
20306
20307 if (nParts > 2){
20308 intx = new int[nintxmax];
20309 intx_size = nintxmax;
20310 Util::k_means_cont_table_(argParts + Indices[part*K+class_max]+2,argParts + Indices[part_next*K + class_max_next]+2, intx, *(dimClasses + part*K+class_max)-2, *(dimClasses+part_next*K+class_max_next)-2,1);
20311 }
20312
20313
20314 for(int j = 2; j < nParts; j++){
20315 part = *(perm+j);
20316 nintxmax=0;
20317 for(int a = 0; a < K; a++){
20318 if (*(argParts + Indices[part*K+a] + 1) < 1) continue;
20319 nintx = Util::k_means_cont_table_(intx, argParts + Indices[part*K + a]+2, intx_next, intx_size, (*(dimClasses + part*K+a))-2,0);
20320 if (nintx <= nintxmax) continue;
20321 nintxmax = nintx;
20322 class_max = a;
20323 }
20324
20325
20326 if (nintxmax < 1) {
20327
20328 delete[] intx;
20329 break;
20330 }
20331
20332
20333 intx_next = new int[nintxmax];
20334 Util::k_means_cont_table_(intx, argParts + Indices[part*K + class_max]+2, intx_next, intx_size, *(dimClasses + part*K+class_max)-2,1);
20335 delete[] intx;
20336 intx = intx_next;
20337 intx_size = nintxmax;
20338 if (j==nParts - 1) delete[] intx_next;
20339
20340 }
20341
20342 if (nintxmax > guess) guess = nintxmax;
20343
20344 }
20345 delete[] perm;
20346 return guess;
20347 }
20348
20349
20350 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int nTop, int n_guesses, bool doMPI, int* Levels) {
20351
20352
20353
20354
20355 int* Indices = new int[nParts*K];
20356 int ind_c = 0;
20357 for (int i=0; i < nParts; i++){
20358 for(int j = 0; j < K; j++){
20359 Indices[i*K + j] = ind_c;
20360 ind_c = ind_c + *(dimClasses+i*K + j);
20361
20362 }
20363 }
20364
20365
20366 if (nTop > 0 && doMPI > 0){
20367
20368 int* matchlist = new int[nTop*nParts];
20369 int* costlist=new int[nTop];
20370 for (int i=0; i< nTop; i++) {*(costlist+i) = 0;}
20371 int matchesFound = Util::findTopLargest(argParts,Indices, dimClasses, nParts, K, T, matchlist, nTop,costlist,n_guesses);
20372 vector<int> ret(nTop*(nParts+1) + 1);
20373 ret[0] = matchesFound;
20374 int m = nParts + 1;
20375
20376 for(int i=0; i < nTop; i++){
20377 ret[1+i*m] = *(costlist+i);
20378 for (int j=0; j < nParts; j++){
20379 ret[1+i*m + 1 + j] = matchlist[i*nParts + j];
20380 }
20381 }
20382
20383 return ret;
20384
20385 }
20386
20387
20388
20389
20390 vector <vector <int*> > Parts(nParts,vector<int*>(K));
20391 ind_c = 0;
20392 int argParts_size=0;
20393 for (int i=0; i < nParts; i++){
20394 for(int j = 0; j < K; j++){
20395 Parts[i][j]=argParts + ind_c;
20396 ind_c = ind_c + *(dimClasses+i*K + j);
20397 argParts_size = argParts_size + *(dimClasses+i*K + j);
20398
20399 }
20400 }
20401
20402
20403
20404
20405
20406 Util::initial_prune(Parts, dimClasses, nParts, K,T);
20407 for(int i = 0; i < nParts; i++){
20408 for(int j=0; j < K; j++){
20409 *(argParts + Indices[i*K + j]+1) = -1;
20410 }
20411 }
20412
20413 int num_classes;
20414 int old_index;
20415 for(int i=0; i<nParts; i++){
20416 num_classes = Parts[i].size();
20417 for (int j=0; j < num_classes; j++){
20418 old_index = *(Parts[i][j]);
20419
20420 *(argParts + Indices[i*K + old_index]+1) = 1;
20421 }
20422 }
20423
20424
20425 if (doMPI > 0){
20426
20427 vector<int> ret(argParts_size);
20428 for(int i=0; i < argParts_size; i++)
20429 ret[i]= (*(argParts+i));
20430
20431 return ret;
20432 }
20433
20434
20435
20436 int* dummy;
20437 int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T,Levels, K,0,n_guesses,-1, dummy);
20438
20439
20440
20441
20442 bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
20443
20444
20445 if (correct < 1){
20446 cout << "something is wrong with output of branchMPI!\n";
20447 vector<int> ret(1);
20448 ret[0]=-1;
20449 return ret;
20450 }
20451
20452
20453
20454
20455
20456 int output_size = 2+ *(output+1) * nParts;
20457 vector<int> ret(output_size);
20458 for (int i = 0; i < output_size; i++){
20459 ret[i]=*(output+i);
20460 }
20461 return ret;
20462
20463 }
20464
20465 vector<int> Util::branchMPIpy_(int* argParts, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int n_guesses, int nFirst, int* firstmatches){
20466
20467
20468
20469 int num_active;
20470 int* Indices = new int[nParts*K];
20471
20472 int ind_c = 0;
20473 for (int i=0; i < nParts; i++){
20474 num_active = 0;
20475 for(int j = 0; j < K; j++){
20476 Indices[i*K + j] = ind_c;
20477 if (*(argParts+ind_c + 1) == 1) num_active = num_active + 1;
20478 ind_c = ind_c + *(dimClasses+i*K + j);
20479 }
20480
20481 if (num_active < nLevels) {nLevels = num_active;}
20482 }
20483
20484
20485
20486
20487
20488
20489
20490 int* output = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, Levels, nLevels, 0,n_guesses, nFirst,firstmatches);
20491
20492
20493
20494
20495
20496 bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
20497
20498
20499 if (correct < 1){
20500 cout << "something is wrong with output of branchMPI!\n";
20501 vector<int> ret(1);
20502 ret[0]=-1;
20503 return ret;
20504 }
20505
20506
20507
20508
20509
20510 int output_size = 2+ *(output+1) * nParts;
20511 vector<int> ret(output_size);
20512 for (int i = 0; i < output_size; i++){
20513 ret[i]=*(output+i);
20514 }
20515 return ret;
20516 }
20517
20518
20519 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int curlevel,int n_guesses, int nFirst, int* firstmatches) {
20520
20521
20522 if (curlevel > nLevels-1){
20523 int* res = new int[2];
20524 *res = 0;
20525 *(res+1)=0;
20526 return res;
20527 }
20528
20529
20530
20531 int nBranches = *(Levels + curlevel);
20532
20533
20534 if (curlevel==0 && nFirst > 0)
20535 {
20536 nBranches = nFirst;
20537 }
20538
20539
20540
20541 int* matchlist = new int[nBranches*nParts];
20542 int* costlist = new int[nBranches];
20543
20544
20545 for (int i=0; i < nBranches; i++)
20546 *(costlist+i)=0;
20547
20548
20549
20550
20551
20552 if (curlevel == 0 && nFirst > 0){
20553 for(int i = 0; i < nBranches; i++){
20554 *(costlist+i) = *(firstmatches +i*(nParts+1));
20555 for (int j=0; j< nParts; j++)
20556 *(matchlist + i*nParts +j) = *(firstmatches +i*(nParts+1) + 1 + j);
20557 }
20558 }
20559 else
20560 Util::findTopLargest(argParts,Indices, dimClasses, nParts, K, T, matchlist, nBranches,costlist,n_guesses);
20561
20562
20563 if (costlist[0]<= T){
20564 int* res = new int[2];
20565 *res = 0;
20566 *(res+1)=0;
20567 return res;
20568 }
20569
20570 int* maxreturn = new int[2];
20571 *maxreturn=0;
20572 *(maxreturn+1)=0;
20573
20574
20575 int old_index;
20576 int totalcost;
20577 int nmatches;
20578
20579
20580 for(int i=0; i < nBranches ; i++){
20581
20582
20583
20584 if (costlist[i] <= T) break;
20585
20586
20587
20588
20589 for(int j=0; j < nParts; j++){
20590
20591 old_index=matchlist[i*nParts + j];
20592 *(argParts + Indices[j*K+old_index] + 1) = -2;
20593 }
20594
20595
20596 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, Levels, nLevels, curlevel+1,n_guesses, nFirst, firstmatches);
20597
20598
20599 totalcost = costlist[i] + *ret;
20600
20601
20602 if (totalcost > *maxreturn)
20603 {
20604 nmatches = 1 + *(ret+1);
20605 delete[] maxreturn;
20606 maxreturn = new int[2+nmatches*nParts];
20607 *maxreturn = totalcost;
20608 *(maxreturn + 1)= nmatches;
20609 int nret = 2+(nmatches-1)*nParts;
20610 for(int iret=2; iret <nret;iret++) *(maxreturn+iret)=*(ret+iret);
20611 for(int imax=0; imax<nParts;imax++) *(maxreturn+nret+imax)=matchlist[i*nParts + imax];
20612 }
20613
20614
20615 delete[] ret;
20616
20617
20618
20619 for(int j=0; j < nParts; j++){
20620
20621 old_index=matchlist[i*nParts + j];
20622 *(argParts + Indices[j*K+old_index] + 1) = 1;
20623 }
20624
20625 }
20626
20627 delete[] matchlist;
20628 delete[] costlist;
20629
20630 return maxreturn;
20631
20632 }