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]*double(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 sig = static_cast<float>(sqrt((Sum2 - Sum1*Sum1/count)/(count-1)));
00131
00132 stats.push_back(avg);
00133 stats.push_back(sig);
00134 stats.push_back(MIN);
00135 stats.push_back(MAX);
00136
00137 return stats;
00138 }
00139
00140
00141
00142
00143 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00144 {
00145 ENTERFUNC;
00146
00147 if (!EMUtil::is_same_size(V1, V2)) {
00148 LOGERR("images not same size");
00149 throw ImageFormatException( "images not same size");
00150 }
00151
00152 size_t nx = V1->get_xsize();
00153 size_t ny = V1->get_ysize();
00154 size_t nz = V1->get_zsize();
00155 size_t size = nx*ny*nz;
00156
00157 EMData *BD = new EMData();
00158 BD->set_size(nx, ny, nz);
00159
00160 float *params = new float[2];
00161
00162 float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00163 long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00164 int nvox = 0L;
00165
00166 V1ptr = V1->get_data();
00167 V2ptr = V2->get_data();
00168 BDptr = BD->get_data();
00169
00170
00171 if(!mask){
00172 EMData * Mask = new EMData();
00173 Mask->set_size(nx,ny,nz);
00174 Mask->to_one();
00175 MASKptr = Mask->get_data();
00176 } else {
00177 if (!EMUtil::is_same_size(V1, mask)) {
00178 LOGERR("mask not same size");
00179 throw ImageFormatException( "mask not same size");
00180 }
00181
00182 MASKptr = mask->get_data();
00183 }
00184
00185
00186
00187
00188
00189 for (size_t i = 0L;i < size; i++) {
00190 if (MASKptr[i]>0.5f) {
00191 S1 += V1ptr[i]*V2ptr[i];
00192 S2 += V1ptr[i]*V1ptr[i];
00193 S3 += V2ptr[i];
00194 S4 += V1ptr[i];
00195 nvox ++;
00196 }
00197 }
00198
00199 if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00200 A =1.0f ;
00201 } else {
00202 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00203 }
00204 B = static_cast<float> (A*S4 - S3)/nvox;
00205
00206
00207
00208 for (size_t i = 0L;i < size; i++) {
00209 if (MASKptr[i]>0.5f) {
00210 BDptr[i] = A*V1ptr[i] - B - V2ptr[i];
00211 } else {
00212 BDptr[i] = 0.f;
00213 }
00214 }
00215
00216 BD->update();
00217
00218 params[0] = A;
00219 params[1] = B;
00220
00221 Dict BDnParams;
00222 BDnParams["imdiff"] = BD;
00223 BDnParams["A"] = params[0];
00224 BDnParams["B"] = params[1];
00225
00226 EXITFUNC;
00227 return BDnParams;
00228 }
00229
00230
00231
00232
00233
00234 EMData *Util::TwoDTestFunc(int Size, float p, float q, float a, float b, int flag, float alphaDeg)
00235 {
00236 ENTERFUNC;
00237 int Mid= (Size+1)/2;
00238
00239 if (flag==0) {
00240 EMData* ImBW = new EMData();
00241 ImBW->set_size(Size,Size,1);
00242 ImBW->to_zero();
00243
00244 float tempIm;
00245 float x,y;
00246
00247 for (int ix=(1-Mid); ix<Mid; ix++){
00248 for (int iy=(1-Mid); iy<Mid; iy++){
00249 x = (float)ix;
00250 y = (float)iy;
00251 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)) );
00252 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00253 }
00254 }
00255 ImBW->update();
00256 ImBW->set_complex(false);
00257 ImBW->set_ri(true);
00258
00259 return ImBW;
00260 }
00261 else if (flag==1) {
00262 EMData* ImBWFFT = new EMData();
00263 ImBWFFT ->set_size(2*Size,Size,1);
00264 ImBWFFT ->to_zero();
00265
00266 float r,s;
00267
00268 for (int ir=(1-Mid); ir<Mid; ir++){
00269 for (int is=(1-Mid); is<Mid; is++){
00270 r = (float)ir;
00271 s = (float)is;
00272 (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00273 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00274 }
00275 }
00276 ImBWFFT->update();
00277 ImBWFFT->set_complex(true);
00278 ImBWFFT->set_ri(true);
00279 ImBWFFT->set_shuffled(true);
00280 ImBWFFT->set_fftodd(true);
00281
00282 return ImBWFFT;
00283 }
00284 else if (flag==2 || flag==3) {
00285 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00286 float C=cos(alpha);
00287 float S=sin(alpha);
00288 float D= sqrt(S*S*b*b + C*C*a*a);
00289
00290
00291 float P = p * C *a*a/D ;
00292 float Q = q * S *b*b/D ;
00293
00294 if (flag==2) {
00295 EMData* pofalpha = new EMData();
00296 pofalpha ->set_size(Size,1,1);
00297 pofalpha ->to_zero();
00298
00299 float Norm0 = D*(float)sqrt(2*pi);
00300 float Norm1 = exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00301 float Norm2 = exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00302 float sD;
00303
00304 for (int is=(1-Mid); is<Mid; is++){
00305 sD = is/D ;
00306 (*pofalpha)(is+Mid-1) = Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00307 + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00308 }
00309 pofalpha-> update();
00310 pofalpha-> set_complex(false);
00311 pofalpha-> set_ri(true);
00312
00313 return pofalpha;
00314 }
00315 if (flag==3) {
00316 float vD;
00317
00318 EMData* pofalphak = new EMData();
00319 pofalphak ->set_size(2*Size,1,1);
00320 pofalphak ->to_zero();
00321
00322 for (int iv=(1-Mid); iv<Mid; iv++){
00323 vD = iv*D ;
00324 (*pofalphak)(2*(iv+Mid-1)) = exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00325 }
00326 pofalphak-> update();
00327 pofalphak-> set_complex(false);
00328 pofalphak-> set_ri(true);
00329
00330 return pofalphak;
00331 }
00332 }
00333 else if (flag==4) {
00334 cout <<" FH under construction";
00335 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00336 EMData* TryFH= OutFT -> real2FH(4.0);
00337 return TryFH;
00338 } else {
00339 cout <<" flag must be 0,1,2,3, or 4";
00340 }
00341
00342 EXITFUNC;
00343 return 0;
00344 }
00345
00346
00347 void Util::spline_mat(float *x, float *y, int n, float *xq, float *yq, int m)
00348 {
00349
00350 float x0= x[0];
00351 float x1= x[1];
00352 float x2= x[2];
00353 float y0= y[0];
00354 float y1= y[1];
00355 float y2= y[2];
00356 float yp1 = (y1-y0)/(x1-x0) + (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1) ;
00357 float xn = x[n];
00358 float xnm1= x[n-1];
00359 float xnm2= x[n-2];
00360 float yn = y[n];
00361 float ynm1= y[n-1];
00362 float ynm2= y[n-2];
00363 float ypn= (yn-ynm1)/(xn-xnm1) + (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00364 float *y2d = new float[n];
00365 Util::spline(x,y,n,yp1,ypn,y2d);
00366 Util::splint(x,y,y2d,n,xq,yq,m);
00367 delete [] y2d;
00368 return;
00369 }
00370
00371
00372 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2)
00373 {
00374 int i,k;
00375 float p, qn, sig, un, *u;
00376 u = new float[n-1];
00377
00378 if (yp1 > .99e30){
00379 y2[0]=u[0]=0.0;
00380 } else {
00381 y2[0]=-.5f;
00382 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00383 }
00384
00385 for (i=1; i < n-1; i++) {
00386 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00387 p = sig*y2[i-1] + 2.0f;
00388 y2[i] = (sig-1.0f)/p;
00389 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) - (y[i] - y[i-1] )/(x[i] -x[i-1]);
00390 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00391 }
00392
00393 if (ypn>.99e30){
00394 qn=0; un=0;
00395 } else {
00396 qn= .5f;
00397 un= (3.0f/(x[n-1] -x[n-2])) * (ypn - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00398 }
00399 y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00400 for (k=n-2; k>=0; k--){
00401 y2[k]=y2[k]*y2[k+1]+u[k];
00402 }
00403 delete [] u;
00404 }
00405
00406
00407 void Util::splint( float *xa, float *ya, float *y2a, int n, float *xq, float *yq, int m)
00408 {
00409 int klo, khi, k;
00410 float h, b, a;
00411
00412
00413 for (int j=0; j<m;j++){
00414 klo=0;
00415 khi=n-1;
00416 while (khi-klo >1) {
00417 k=(khi+klo) >>1;
00418 if (xa[k]>xq[j]){ khi=k;}
00419 else { klo=k;}
00420 }
00421 h=xa[khi]- xa[klo];
00422 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00423 a =(xa[khi]-xq[j])/h;
00424 b=(xq[j]-xa[klo])/h;
00425 yq[j]=a*ya[klo] + b*ya[khi]
00426 + ((a*a*a-a)*y2a[klo]
00427 +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00428 }
00429
00430 }
00431
00432
00433 void Util::Radialize(int *PermMatTr, float *kValsSorted,
00434 float *weightofkValsSorted, int Size, int *SizeReturned)
00435 {
00436 int iMax = (int) floor( (Size-1.0)/2 +.01);
00437 int CountMax = (iMax+2)*(iMax+1)/2;
00438 int Count=-1;
00439 float *kVals = new float[CountMax];
00440 float *weightMat = new float[CountMax];
00441 int *PermMat = new int[CountMax];
00442 SizeReturned[0] = CountMax;
00443
00444
00445 for (int jkx=0; jkx< iMax+1; jkx++) {
00446 for (int jky=0; jky< jkx+1; jky++) {
00447 Count++;
00448 kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00449 weightMat[Count]= 1.0;
00450 if (jkx!=0) { weightMat[Count] *=2;}
00451 if (jky!=0) { weightMat[Count] *=2;}
00452 if (jkx!=jky){ weightMat[Count] *=2;}
00453 PermMat[Count]=Count+1;
00454 }
00455 }
00456
00457 int lkVals = Count+1;
00458
00459
00460 sort_mat(&kVals[0],&kVals[Count],
00461 &PermMat[0], &PermMat[Count]);
00462
00463 fflush(stdout);
00464
00465 int newInd;
00466
00467 for (int iP=0; iP < lkVals ; iP++ ) {
00468 newInd = PermMat[iP];
00469 PermMatTr[newInd-1] = iP+1;
00470 }
00471
00472
00473
00474 int CountA=-1;
00475 int CountB=-1;
00476
00477 while (CountB< (CountMax-1)) {
00478 CountA++;
00479 CountB++;
00480
00481 kValsSorted[CountA] = kVals[CountB] ;
00482 if (CountB<(CountMax-1) ) {
00483 while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001 ) {
00484 SizeReturned[0]--;
00485 for (int iP=0; iP < lkVals; iP++){
00486
00487 if (PermMatTr[iP]>CountA+1) {
00488 PermMatTr[iP]--;
00489 }
00490 }
00491 CountB++;
00492 }
00493 }
00494 }
00495
00496
00497 for (int CountD=0; CountD < CountMax; CountD++) {
00498 newInd = PermMatTr[CountD];
00499 weightofkValsSorted[newInd-1] += weightMat[CountD];
00500 }
00501
00502 }
00503
00504
00505 vector<float>
00506 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00507 {
00508 vector<float> angles;
00509 float psi = 0.0;
00510 if ((0.0 == t1 && 0.0 == t2)||(t1 >= t2)) {
00511 t1 = 0.0f;
00512 t2 = 90.0f;
00513 }
00514 if ((0.0 == p1 && 0.0 == p2)||(p1 >= p2)) {
00515 p1 = 0.0f;
00516 p2 = 359.9f;
00517 }
00518 bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00519 for (float theta = t1; theta <= t2; theta += delta) {
00520 float detphi;
00521 int lt;
00522 if ((0.0 == theta)||(180.0 == theta)) {
00523 detphi = 360.0f;
00524 lt = 1;
00525 } else {
00526 detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00527 lt = int((p2 - p1)/detphi)-1;
00528 if (lt < 1) lt = 1;
00529 detphi = (p2 - p1)/lt;
00530 }
00531 for (int i = 0; i < lt; i++) {
00532 float phi = p1 + i*detphi;
00533 if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00534 angles.push_back(phi);
00535 angles.push_back(theta);
00536 angles.push_back(psi);
00537 }
00538 }
00539 return angles;
00540 }
00541
00542
00543 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00544
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 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00646 {
00647
00648
00649 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00650 float quadri;
00651 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00652
00653 x = xx;
00654 y = yy;
00655
00656
00657 while ( x < 1.0 ) x += nxdata;
00658 while ( x >= (float)(nxdata+1) ) x -= nxdata;
00659 while ( y < 1.0 ) y += nydata;
00660 while ( y >= (float)(nydata+1) ) y -= nydata;
00661
00662 i = (int) x;
00663 j = (int) y;
00664
00665 dx0 = x - i;
00666 dy0 = y - j;
00667
00668 ip1 = i + 1;
00669 im1 = i - 1;
00670 jp1 = j + 1;
00671 jm1 = j - 1;
00672
00673 if (ip1 > nxdata) ip1 -= nxdata;
00674 if (im1 < 1) im1 += nxdata;
00675 if (jp1 > nydata) jp1 -= nydata;
00676 if (jm1 < 1) jm1 += nydata;
00677
00678 f0 = fdata(i,j);
00679 c1 = fdata(ip1,j) - f0;
00680 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00681 c3 = fdata(i,jp1) - f0;
00682 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00683
00684 dxb = dx0 - 1;
00685 dyb = dy0 - 1;
00686
00687
00688 if (dx0 >= 0) hxc = 1; else hxc = -1;
00689 if (dy0 >= 0) hyc = 1; else hyc = -1;
00690
00691 ic = i + hxc;
00692 jc = j + hyc;
00693
00694 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00695 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00696
00697 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00698 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00699
00700
00701 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00702
00703 return quadri;
00704 }
00705
00706 #undef fdata
00707
00708 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00709 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00710 {
00711
00712
00713 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00714 float quadri;
00715 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00716
00717 x = xx;
00718 y = yy;
00719
00720
00721 if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00722 x = (float)xnew;
00723 y = (float)ynew;
00724 }
00725
00726
00727 i = (int) x;
00728 j = (int) y;
00729
00730 dx0 = x - i;
00731 dy0 = y - j;
00732
00733 ip1 = i + 1;
00734 im1 = i - 1;
00735 jp1 = j + 1;
00736 jm1 = j - 1;
00737
00738 if (ip1 > nxdata) ip1 -= nxdata;
00739 if (im1 < 1) im1 += nxdata;
00740 if (jp1 > nydata) jp1 -= nydata;
00741 if (jm1 < 1) jm1 += nydata;
00742
00743 f0 = fdata(i,j);
00744 c1 = fdata(ip1,j) - f0;
00745 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00746 c3 = fdata(i,jp1) - f0;
00747 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00748
00749 dxb = dx0 - 1;
00750 dyb = dy0 - 1;
00751
00752
00753 if (dx0 >= 0) hxc = 1; else hxc = -1;
00754 if (dy0 >= 0) hyc = 1; else hyc = -1;
00755
00756 ic = i + hxc;
00757 jc = j + hyc;
00758
00759 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00760 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00761
00762 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00763 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00764
00765
00766 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00767
00768 return quadri;
00769 }
00770
00771 #undef fdata
00772
00773
00774 float Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00775 int K = kb.get_window_size();
00776 int kbmin = -K/2;
00777 int kbmax = -kbmin;
00778 int kbc = kbmax+1;
00779
00780 float pixel =0.0f;
00781 float w=0.0f;
00782
00783 delx = restrict1(delx, nx);
00784 int inxold = int(round(delx));
00785 if ( ny < 2 ) {
00786 float tablex1 = kb.i0win_tab(delx-inxold+3);
00787 float tablex2 = kb.i0win_tab(delx-inxold+2);
00788 float tablex3 = kb.i0win_tab(delx-inxold+1);
00789 float tablex4 = kb.i0win_tab(delx-inxold);
00790 float tablex5 = kb.i0win_tab(delx-inxold-1);
00791 float tablex6 = kb.i0win_tab(delx-inxold-2);
00792 float tablex7 = kb.i0win_tab(delx-inxold-3);
00793
00794 int x1, x2, x3, x4, x5, x6, x7;
00795
00796 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
00797 x1 = (inxold-3+nx)%nx;
00798 x2 = (inxold-2+nx)%nx;
00799 x3 = (inxold-1+nx)%nx;
00800 x4 = (inxold +nx)%nx;
00801 x5 = (inxold+1+nx)%nx;
00802 x6 = (inxold+2+nx)%nx;
00803 x7 = (inxold+3+nx)%nx;
00804 } else {
00805 x1 = inxold-3;
00806 x2 = inxold-2;
00807 x3 = inxold-1;
00808 x4 = inxold;
00809 x5 = inxold+1;
00810 x6 = inxold+2;
00811 x7 = inxold+3;
00812 }
00813
00814 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00815 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00816 data[x7]*tablex7 ;
00817
00818 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00819 } else if ( nz < 2 ) {
00820 dely = restrict1(dely, ny);
00821 int inyold = int(round(dely));
00822 float tablex1 = kb.i0win_tab(delx-inxold+3);
00823 float tablex2 = kb.i0win_tab(delx-inxold+2);
00824 float tablex3 = kb.i0win_tab(delx-inxold+1);
00825 float tablex4 = kb.i0win_tab(delx-inxold);
00826 float tablex5 = kb.i0win_tab(delx-inxold-1);
00827 float tablex6 = kb.i0win_tab(delx-inxold-2);
00828 float tablex7 = kb.i0win_tab(delx-inxold-3);
00829
00830 float tabley1 = kb.i0win_tab(dely-inyold+3);
00831 float tabley2 = kb.i0win_tab(dely-inyold+2);
00832 float tabley3 = kb.i0win_tab(dely-inyold+1);
00833 float tabley4 = kb.i0win_tab(dely-inyold);
00834 float tabley5 = kb.i0win_tab(dely-inyold-1);
00835 float tabley6 = kb.i0win_tab(dely-inyold-2);
00836 float tabley7 = kb.i0win_tab(dely-inyold-3);
00837
00838 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00839
00840 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
00841 x1 = (inxold-3+nx)%nx;
00842 x2 = (inxold-2+nx)%nx;
00843 x3 = (inxold-1+nx)%nx;
00844 x4 = (inxold +nx)%nx;
00845 x5 = (inxold+1+nx)%nx;
00846 x6 = (inxold+2+nx)%nx;
00847 x7 = (inxold+3+nx)%nx;
00848
00849 y1 = ((inyold-3+ny)%ny)*nx;
00850 y2 = ((inyold-2+ny)%ny)*nx;
00851 y3 = ((inyold-1+ny)%ny)*nx;
00852 y4 = ((inyold +ny)%ny)*nx;
00853 y5 = ((inyold+1+ny)%ny)*nx;
00854 y6 = ((inyold+2+ny)%ny)*nx;
00855 y7 = ((inyold+3+ny)%ny)*nx;
00856 } else {
00857 x1 = inxold-3;
00858 x2 = inxold-2;
00859 x3 = inxold-1;
00860 x4 = inxold;
00861 x5 = inxold+1;
00862 x6 = inxold+2;
00863 x7 = inxold+3;
00864
00865 y1 = (inyold-3)*nx;
00866 y2 = (inyold-2)*nx;
00867 y3 = (inyold-1)*nx;
00868 y4 = inyold*nx;
00869 y5 = (inyold+1)*nx;
00870 y6 = (inyold+2)*nx;
00871 y7 = (inyold+3)*nx;
00872 }
00873
00874 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00875 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00876 data[x7+y1]*tablex7 ) * tabley1 +
00877 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00878 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00879 data[x7+y2]*tablex7 ) * tabley2 +
00880 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00881 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00882 data[x7+y3]*tablex7 ) * tabley3 +
00883 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00884 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00885 data[x7+y4]*tablex7 ) * tabley4 +
00886 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00887 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00888 data[x7+y5]*tablex7 ) * tabley5 +
00889 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00890 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00891 data[x7+y6]*tablex7 ) * tabley6 +
00892 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00893 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00894 data[x7+y7]*tablex7 ) * tabley7;
00895
00896 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00897 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00898 } else {
00899 dely = restrict1(dely, ny);
00900 int inyold = int(Util::round(dely));
00901 delz = restrict1(delz, nz);
00902 int inzold = int(Util::round(delz));
00903
00904 float tablex1 = kb.i0win_tab(delx-inxold+3);
00905 float tablex2 = kb.i0win_tab(delx-inxold+2);
00906 float tablex3 = kb.i0win_tab(delx-inxold+1);
00907 float tablex4 = kb.i0win_tab(delx-inxold);
00908 float tablex5 = kb.i0win_tab(delx-inxold-1);
00909 float tablex6 = kb.i0win_tab(delx-inxold-2);
00910 float tablex7 = kb.i0win_tab(delx-inxold-3);
00911
00912 float tabley1 = kb.i0win_tab(dely-inyold+3);
00913 float tabley2 = kb.i0win_tab(dely-inyold+2);
00914 float tabley3 = kb.i0win_tab(dely-inyold+1);
00915 float tabley4 = kb.i0win_tab(dely-inyold);
00916 float tabley5 = kb.i0win_tab(dely-inyold-1);
00917 float tabley6 = kb.i0win_tab(dely-inyold-2);
00918 float tabley7 = kb.i0win_tab(dely-inyold-3);
00919
00920 float tablez1 = kb.i0win_tab(delz-inzold+3);
00921 float tablez2 = kb.i0win_tab(delz-inzold+2);
00922 float tablez3 = kb.i0win_tab(delz-inzold+1);
00923 float tablez4 = kb.i0win_tab(delz-inzold);
00924 float tablez5 = kb.i0win_tab(delz-inzold-1);
00925 float tablez6 = kb.i0win_tab(delz-inzold-2);
00926 float tablez7 = kb.i0win_tab(delz-inzold-3);
00927
00928 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00929
00930 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
00931 x1 = (inxold-3+nx)%nx;
00932 x2 = (inxold-2+nx)%nx;
00933 x3 = (inxold-1+nx)%nx;
00934 x4 = (inxold +nx)%nx;
00935 x5 = (inxold+1+nx)%nx;
00936 x6 = (inxold+2+nx)%nx;
00937 x7 = (inxold+3+nx)%nx;
00938
00939 y1 = ((inyold-3+ny)%ny)*nx;
00940 y2 = ((inyold-2+ny)%ny)*nx;
00941 y3 = ((inyold-1+ny)%ny)*nx;
00942 y4 = ((inyold +ny)%ny)*nx;
00943 y5 = ((inyold+1+ny)%ny)*nx;
00944 y6 = ((inyold+2+ny)%ny)*nx;
00945 y7 = ((inyold+3+ny)%ny)*nx;
00946
00947 z1 = ((inzold-3+nz)%nz)*nx*ny;
00948 z2 = ((inzold-2+nz)%nz)*nx*ny;
00949 z3 = ((inzold-1+nz)%nz)*nx*ny;
00950 z4 = ((inzold +nz)%nz)*nx*ny;
00951 z5 = ((inzold+1+nz)%nz)*nx*ny;
00952 z6 = ((inzold+2+nz)%nz)*nx*ny;
00953 z7 = ((inzold+3+nz)%nz)*nx*ny;
00954 } else {
00955 x1 = inxold-3;
00956 x2 = inxold-2;
00957 x3 = inxold-1;
00958 x4 = inxold;
00959 x5 = inxold+1;
00960 x6 = inxold+2;
00961 x7 = inxold+3;
00962
00963 y1 = (inyold-3)*nx;
00964 y2 = (inyold-2)*nx;
00965 y3 = (inyold-1)*nx;
00966 y4 = inyold*nx;
00967 y5 = (inyold+1)*nx;
00968 y6 = (inyold+2)*nx;
00969 y7 = (inyold+3)*nx;
00970
00971 z1 = (inzold-3)*nx*ny;
00972 z2 = (inzold-2)*nx*ny;
00973 z3 = (inzold-1)*nx*ny;
00974 z4 = inzold*nx*ny;
00975 z5 = (inzold+1)*nx*ny;
00976 z6 = (inzold+2)*nx*ny;
00977 z7 = (inzold+3)*nx*ny;
00978 }
00979
00980 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00981 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00982 data[x7+y1+z1]*tablex7 ) * tabley1 +
00983 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00984 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00985 data[x7+y2+z1]*tablex7 ) * tabley2 +
00986 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00987 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00988 data[x7+y3+z1]*tablex7 ) * tabley3 +
00989 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00990 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00991 data[x7+y4+z1]*tablex7 ) * tabley4 +
00992 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
00993 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
00994 data[x7+y5+z1]*tablex7 ) * tabley5 +
00995 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
00996 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
00997 data[x7+y6+z1]*tablex7 ) * tabley6 +
00998 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
00999 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01000 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01001 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01002 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01003 data[x7+y1+z2]*tablex7 ) * tabley1 +
01004 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01005 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01006 data[x7+y2+z2]*tablex7 ) * tabley2 +
01007 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01008 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01009 data[x7+y3+z2]*tablex7 ) * tabley3 +
01010 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01011 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01012 data[x7+y4+z2]*tablex7 ) * tabley4 +
01013 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01014 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01015 data[x7+y5+z2]*tablex7 ) * tabley5 +
01016 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01017 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01018 data[x7+y6+z2]*tablex7 ) * tabley6 +
01019 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01020 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01021 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01022 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01023 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01024 data[x7+y1+z3]*tablex7 ) * tabley1 +
01025 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01026 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01027 data[x7+y2+z3]*tablex7 ) * tabley2 +
01028 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01029 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01030 data[x7+y3+z3]*tablex7 ) * tabley3 +
01031 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01032 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01033 data[x7+y4+z3]*tablex7 ) * tabley4 +
01034 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01035 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01036 data[x7+y5+z3]*tablex7 ) * tabley5 +
01037 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01038 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01039 data[x7+y6+z3]*tablex7 ) * tabley6 +
01040 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01041 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01042 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01043 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01044 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01045 data[x7+y1+z4]*tablex7 ) * tabley1 +
01046 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01047 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01048 data[x7+y2+z4]*tablex7 ) * tabley2 +
01049 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01050 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01051 data[x7+y3+z4]*tablex7 ) * tabley3 +
01052 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01053 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01054 data[x7+y4+z4]*tablex7 ) * tabley4 +
01055 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01056 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01057 data[x7+y5+z4]*tablex7 ) * tabley5 +
01058 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01059 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01060 data[x7+y6+z4]*tablex7 ) * tabley6 +
01061 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01062 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01063 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01064 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01065 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01066 data[x7+y1+z5]*tablex7 ) * tabley1 +
01067 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01068 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01069 data[x7+y2+z5]*tablex7 ) * tabley2 +
01070 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01071 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01072 data[x7+y3+z5]*tablex7 ) * tabley3 +
01073 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01074 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01075 data[x7+y4+z5]*tablex7 ) * tabley4 +
01076 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01077 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01078 data[x7+y5+z5]*tablex7 ) * tabley5 +
01079 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01080 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01081 data[x7+y6+z5]*tablex7 ) * tabley6 +
01082 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01083 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01084 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01085 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01086 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01087 data[x7+y1+z6]*tablex7 ) * tabley1 +
01088 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01089 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01090 data[x7+y2+z6]*tablex7 ) * tabley2 +
01091 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01092 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01093 data[x7+y3+z6]*tablex7 ) * tabley3 +
01094 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01095 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01096 data[x7+y4+z6]*tablex7 ) * tabley4 +
01097 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01098 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01099 data[x7+y5+z6]*tablex7 ) * tabley5 +
01100 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01101 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01102 data[x7+y6+z6]*tablex7 ) * tabley6 +
01103 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01104 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01105 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01106 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01107 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01108 data[x7+y1+z7]*tablex7 ) * tabley1 +
01109 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01110 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01111 data[x7+y2+z7]*tablex7 ) * tabley2 +
01112 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01113 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01114 data[x7+y3+z7]*tablex7 ) * tabley3 +
01115 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01116 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01117 data[x7+y4+z7]*tablex7 ) * tabley4 +
01118 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01119 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01120 data[x7+y5+z7]*tablex7 ) * tabley5 +
01121 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01122 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01123 data[x7+y6+z7]*tablex7 ) * tabley6 +
01124 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01125 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01126 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01127
01128 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01129 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01130 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01131 }
01132 return pixel/w;
01133 }
01134
01135 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) {
01136 int K = kb.get_window_size();
01137 int kbmin = -K/2;
01138 int kbmax = -kbmin;
01139 int kbc = kbmax+1;
01140
01141 float pixel =0.0f;
01142 float w=0.0f;
01143
01144 float argdelx = delx;
01145 delx = restrict1(delx, nx);
01146 int inxold = int(round(delx));
01147 if ( ny < 2 ) {
01148 float tablex1 = kb.i0win_tab(delx-inxold+3);
01149 float tablex2 = kb.i0win_tab(delx-inxold+2);
01150 float tablex3 = kb.i0win_tab(delx-inxold+1);
01151 float tablex4 = kb.i0win_tab(delx-inxold);
01152 float tablex5 = kb.i0win_tab(delx-inxold-1);
01153 float tablex6 = kb.i0win_tab(delx-inxold-2);
01154 float tablex7 = kb.i0win_tab(delx-inxold-3);
01155
01156 int x1, x2, x3, x4, x5, x6, x7;
01157
01158 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
01159 x1 = (inxold-3+nx)%nx;
01160 x2 = (inxold-2+nx)%nx;
01161 x3 = (inxold-1+nx)%nx;
01162 x4 = (inxold +nx)%nx;
01163 x5 = (inxold+1+nx)%nx;
01164 x6 = (inxold+2+nx)%nx;
01165 x7 = (inxold+3+nx)%nx;
01166 } else {
01167 x1 = inxold-3;
01168 x2 = inxold-2;
01169 x3 = inxold-1;
01170 x4 = inxold;
01171 x5 = inxold+1;
01172 x6 = inxold+2;
01173 x7 = inxold+3;
01174 }
01175
01176 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01177 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01178 data[x7]*tablex7 ;
01179
01180 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01181 } else if ( nz < 2 ) {
01182
01183 delx = argdelx;
01184
01185 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01186 delx = (float)xnew*2.0f;
01187 dely = (float)ynew*2.0f;
01188 }
01189
01190 int inxold = int(round(delx));
01191 int inyold = int(round(dely));
01192
01193 float tablex1 = kb.i0win_tab(delx-inxold+3);
01194 float tablex2 = kb.i0win_tab(delx-inxold+2);
01195 float tablex3 = kb.i0win_tab(delx-inxold+1);
01196 float tablex4 = kb.i0win_tab(delx-inxold);
01197 float tablex5 = kb.i0win_tab(delx-inxold-1);
01198 float tablex6 = kb.i0win_tab(delx-inxold-2);
01199 float tablex7 = kb.i0win_tab(delx-inxold-3);
01200
01201 float tabley1 = kb.i0win_tab(dely-inyold+3);
01202 float tabley2 = kb.i0win_tab(dely-inyold+2);
01203 float tabley3 = kb.i0win_tab(dely-inyold+1);
01204 float tabley4 = kb.i0win_tab(dely-inyold);
01205 float tabley5 = kb.i0win_tab(dely-inyold-1);
01206 float tabley6 = kb.i0win_tab(dely-inyold-2);
01207 float tabley7 = kb.i0win_tab(dely-inyold-3);
01208
01209 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01210
01211 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
01212 x1 = (inxold-3+nx)%nx;
01213 x2 = (inxold-2+nx)%nx;
01214 x3 = (inxold-1+nx)%nx;
01215 x4 = (inxold +nx)%nx;
01216 x5 = (inxold+1+nx)%nx;
01217 x6 = (inxold+2+nx)%nx;
01218 x7 = (inxold+3+nx)%nx;
01219
01220 y1 = ((inyold-3+ny)%ny)*nx;
01221 y2 = ((inyold-2+ny)%ny)*nx;
01222 y3 = ((inyold-1+ny)%ny)*nx;
01223 y4 = ((inyold +ny)%ny)*nx;
01224 y5 = ((inyold+1+ny)%ny)*nx;
01225 y6 = ((inyold+2+ny)%ny)*nx;
01226 y7 = ((inyold+3+ny)%ny)*nx;
01227 } else {
01228 x1 = inxold-3;
01229 x2 = inxold-2;
01230 x3 = inxold-1;
01231 x4 = inxold;
01232 x5 = inxold+1;
01233 x6 = inxold+2;
01234 x7 = inxold+3;
01235
01236 y1 = (inyold-3)*nx;
01237 y2 = (inyold-2)*nx;
01238 y3 = (inyold-1)*nx;
01239 y4 = inyold*nx;
01240 y5 = (inyold+1)*nx;
01241 y6 = (inyold+2)*nx;
01242 y7 = (inyold+3)*nx;
01243 }
01244
01245 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01246 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01247 data[x7+y1]*tablex7 ) * tabley1 +
01248 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01249 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01250 data[x7+y2]*tablex7 ) * tabley2 +
01251 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01252 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01253 data[x7+y3]*tablex7 ) * tabley3 +
01254 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01255 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01256 data[x7+y4]*tablex7 ) * tabley4 +
01257 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01258 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01259 data[x7+y5]*tablex7 ) * tabley5 +
01260 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01261 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01262 data[x7+y6]*tablex7 ) * tabley6 +
01263 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01264 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01265 data[x7+y7]*tablex7 ) * tabley7;
01266
01267 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01268 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01269 } else {
01270 dely = restrict1(dely, ny);
01271 int inyold = int(Util::round(dely));
01272 delz = restrict1(delz, nz);
01273 int inzold = int(Util::round(delz));
01274
01275 float tablex1 = kb.i0win_tab(delx-inxold+3);
01276 float tablex2 = kb.i0win_tab(delx-inxold+2);
01277 float tablex3 = kb.i0win_tab(delx-inxold+1);
01278 float tablex4 = kb.i0win_tab(delx-inxold);
01279 float tablex5 = kb.i0win_tab(delx-inxold-1);
01280 float tablex6 = kb.i0win_tab(delx-inxold-2);
01281 float tablex7 = kb.i0win_tab(delx-inxold-3);
01282
01283 float tabley1 = kb.i0win_tab(dely-inyold+3);
01284 float tabley2 = kb.i0win_tab(dely-inyold+2);
01285 float tabley3 = kb.i0win_tab(dely-inyold+1);
01286 float tabley4 = kb.i0win_tab(dely-inyold);
01287 float tabley5 = kb.i0win_tab(dely-inyold-1);
01288 float tabley6 = kb.i0win_tab(dely-inyold-2);
01289 float tabley7 = kb.i0win_tab(dely-inyold-3);
01290
01291 float tablez1 = kb.i0win_tab(delz-inzold+3);
01292 float tablez2 = kb.i0win_tab(delz-inzold+2);
01293 float tablez3 = kb.i0win_tab(delz-inzold+1);
01294 float tablez4 = kb.i0win_tab(delz-inzold);
01295 float tablez5 = kb.i0win_tab(delz-inzold-1);
01296 float tablez6 = kb.i0win_tab(delz-inzold-2);
01297 float tablez7 = kb.i0win_tab(delz-inzold-3);
01298
01299 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01300
01301 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
01302 x1 = (inxold-3+nx)%nx;
01303 x2 = (inxold-2+nx)%nx;
01304 x3 = (inxold-1+nx)%nx;
01305 x4 = (inxold +nx)%nx;
01306 x5 = (inxold+1+nx)%nx;
01307 x6 = (inxold+2+nx)%nx;
01308 x7 = (inxold+3+nx)%nx;
01309
01310 y1 = ((inyold-3+ny)%ny)*nx;
01311 y2 = ((inyold-2+ny)%ny)*nx;
01312 y3 = ((inyold-1+ny)%ny)*nx;
01313 y4 = ((inyold +ny)%ny)*nx;
01314 y5 = ((inyold+1+ny)%ny)*nx;
01315 y6 = ((inyold+2+ny)%ny)*nx;
01316 y7 = ((inyold+3+ny)%ny)*nx;
01317
01318 z1 = ((inzold-3+nz)%nz)*nx*ny;
01319 z2 = ((inzold-2+nz)%nz)*nx*ny;
01320 z3 = ((inzold-1+nz)%nz)*nx*ny;
01321 z4 = ((inzold +nz)%nz)*nx*ny;
01322 z5 = ((inzold+1+nz)%nz)*nx*ny;
01323 z6 = ((inzold+2+nz)%nz)*nx*ny;
01324 z7 = ((inzold+3+nz)%nz)*nx*ny;
01325 } else {
01326 x1 = inxold-3;
01327 x2 = inxold-2;
01328 x3 = inxold-1;
01329 x4 = inxold;
01330 x5 = inxold+1;
01331 x6 = inxold+2;
01332 x7 = inxold+3;
01333
01334 y1 = (inyold-3)*nx;
01335 y2 = (inyold-2)*nx;
01336 y3 = (inyold-1)*nx;
01337 y4 = inyold*nx;
01338 y5 = (inyold+1)*nx;
01339 y6 = (inyold+2)*nx;
01340 y7 = (inyold+3)*nx;
01341
01342 z1 = (inzold-3)*nx*ny;
01343 z2 = (inzold-2)*nx*ny;
01344 z3 = (inzold-1)*nx*ny;
01345 z4 = inzold*nx*ny;
01346 z5 = (inzold+1)*nx*ny;
01347 z6 = (inzold+2)*nx*ny;
01348 z7 = (inzold+3)*nx*ny;
01349 }
01350
01351 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01352 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01353 data[x7+y1+z1]*tablex7 ) * tabley1 +
01354 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01355 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01356 data[x7+y2+z1]*tablex7 ) * tabley2 +
01357 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01358 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01359 data[x7+y3+z1]*tablex7 ) * tabley3 +
01360 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01361 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01362 data[x7+y4+z1]*tablex7 ) * tabley4 +
01363 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01364 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01365 data[x7+y5+z1]*tablex7 ) * tabley5 +
01366 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01367 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01368 data[x7+y6+z1]*tablex7 ) * tabley6 +
01369 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01370 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01371 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01372 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01373 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01374 data[x7+y1+z2]*tablex7 ) * tabley1 +
01375 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01376 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01377 data[x7+y2+z2]*tablex7 ) * tabley2 +
01378 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01379 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01380 data[x7+y3+z2]*tablex7 ) * tabley3 +
01381 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01382 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01383 data[x7+y4+z2]*tablex7 ) * tabley4 +
01384 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01385 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01386 data[x7+y5+z2]*tablex7 ) * tabley5 +
01387 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01388 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01389 data[x7+y6+z2]*tablex7 ) * tabley6 +
01390 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01391 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01392 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01393 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01394 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01395 data[x7+y1+z3]*tablex7 ) * tabley1 +
01396 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01397 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01398 data[x7+y2+z3]*tablex7 ) * tabley2 +
01399 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01400 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01401 data[x7+y3+z3]*tablex7 ) * tabley3 +
01402 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01403 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01404 data[x7+y4+z3]*tablex7 ) * tabley4 +
01405 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01406 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01407 data[x7+y5+z3]*tablex7 ) * tabley5 +
01408 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01409 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01410 data[x7+y6+z3]*tablex7 ) * tabley6 +
01411 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01412 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01413 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01414 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01415 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01416 data[x7+y1+z4]*tablex7 ) * tabley1 +
01417 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01418 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01419 data[x7+y2+z4]*tablex7 ) * tabley2 +
01420 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01421 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01422 data[x7+y3+z4]*tablex7 ) * tabley3 +
01423 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01424 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01425 data[x7+y4+z4]*tablex7 ) * tabley4 +
01426 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01427 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01428 data[x7+y5+z4]*tablex7 ) * tabley5 +
01429 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01430 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01431 data[x7+y6+z4]*tablex7 ) * tabley6 +
01432 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01433 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01434 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01435 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01436 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01437 data[x7+y1+z5]*tablex7 ) * tabley1 +
01438 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01439 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01440 data[x7+y2+z5]*tablex7 ) * tabley2 +
01441 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01442 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01443 data[x7+y3+z5]*tablex7 ) * tabley3 +
01444 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01445 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01446 data[x7+y4+z5]*tablex7 ) * tabley4 +
01447 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01448 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01449 data[x7+y5+z5]*tablex7 ) * tabley5 +
01450 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01451 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01452 data[x7+y6+z5]*tablex7 ) * tabley6 +
01453 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01454 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01455 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01456 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01457 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01458 data[x7+y1+z6]*tablex7 ) * tabley1 +
01459 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01460 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01461 data[x7+y2+z6]*tablex7 ) * tabley2 +
01462 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01463 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01464 data[x7+y3+z6]*tablex7 ) * tabley3 +
01465 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01466 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01467 data[x7+y4+z6]*tablex7 ) * tabley4 +
01468 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01469 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01470 data[x7+y5+z6]*tablex7 ) * tabley5 +
01471 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01472 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01473 data[x7+y6+z6]*tablex7 ) * tabley6 +
01474 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01475 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01476 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01477 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01478 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01479 data[x7+y1+z7]*tablex7 ) * tabley1 +
01480 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01481 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01482 data[x7+y2+z7]*tablex7 ) * tabley2 +
01483 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01484 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01485 data[x7+y3+z7]*tablex7 ) * tabley3 +
01486 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01487 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01488 data[x7+y4+z7]*tablex7 ) * tabley4 +
01489 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01490 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01491 data[x7+y5+z7]*tablex7 ) * tabley5 +
01492 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01493 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01494 data[x7+y6+z7]*tablex7 ) * tabley6 +
01495 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01496 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01497 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01498
01499 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01500 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01501 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01502 }
01503 return pixel/w;
01504 }
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 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01685
01686 int nxreal = nx - 2;
01687 if (nxreal != ny)
01688 throw ImageDimensionException("extractpoint requires ny == nx");
01689 int nhalf = nxreal/2;
01690 bool flip = (nuxnew < 0.f);
01691 if (flip) {
01692 nuxnew *= -1;
01693 nuynew *= -1;
01694 }
01695 if (nuynew >= nhalf-0.5) {
01696 nuynew -= nxreal;
01697 } else if (nuynew < -nhalf-0.5) {
01698 nuynew += nxreal;
01699 }
01700
01701
01702
01703
01704 int ixn = int(Util::round(nuxnew));
01705 int iyn = int(Util::round(nuynew));
01706
01707
01708 static float wy[7];
01709 static float wx[7];
01710
01711 float iynn = nuynew - iyn;
01712 wy[0] = kb.i0win_tab(iynn+3);
01713 wy[1] = kb.i0win_tab(iynn+2);
01714 wy[2] = kb.i0win_tab(iynn+1);
01715 wy[3] = kb.i0win_tab(iynn);
01716 wy[4] = kb.i0win_tab(iynn-1);
01717 wy[5] = kb.i0win_tab(iynn-2);
01718 wy[6] = kb.i0win_tab(iynn-3);
01719
01720 float ixnn = nuxnew - ixn;
01721 wx[0] = kb.i0win_tab(ixnn+3);
01722 wx[1] = kb.i0win_tab(ixnn+2);
01723 wx[2] = kb.i0win_tab(ixnn+1);
01724 wx[3] = kb.i0win_tab(ixnn);
01725 wx[4] = kb.i0win_tab(ixnn-1);
01726 wx[5] = kb.i0win_tab(ixnn-2);
01727 wx[6] = kb.i0win_tab(ixnn-3);
01728
01729 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]);
01730
01731 complex<float> result(0.f,0.f);
01732 if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01733
01734 for (int iy = 0; iy < 7; iy++) {
01735 int iyp = iyn + iy - 3 ;
01736 for (int ix = 0; ix < 7; ix++) {
01737 int ixp = ixn + ix - 3;
01738 float w = wx[ix]*wy[iy];
01739 complex<float> val = fimage->cmplx(ixp,iyp);
01740 result += val*w;
01741 }
01742 }
01743 } else {
01744
01745 for (int iy = 0; iy < 7; iy++) {
01746 int iyp = iyn + iy - 3;
01747 for (int ix = 0; ix < 7; ix++) {
01748 int ixp = ixn + ix - 3;
01749 bool mirror = false;
01750 int ixt = ixp, iyt = iyp;
01751 if (ixt < 0) {
01752 ixt = -ixt;
01753 iyt = -iyt;
01754 mirror = !mirror;
01755 }
01756 if (ixt > nhalf) {
01757 ixt = nxreal - ixt;
01758 iyt = -iyt;
01759 mirror = !mirror;
01760 }
01761 if (iyt > nhalf-1) iyt -= nxreal;
01762 if (iyt < -nhalf) iyt += nxreal;
01763 float w = wx[ix]*wy[iy];
01764 complex<float> val = fimage->cmplx(ixt,iyt);
01765 if (mirror) result += conj(val)*w;
01766 else result += val*w;
01767 }
01768 }
01769 }
01770 if (flip) result = conj(result)/wsum;
01771 else result /= wsum;
01772 return result;
01773 }
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 float Util::triquad(float R, float S, float T, float* fdata)
01904 {
01905
01906 const float C2 = 0.5f;
01907 const float C4 = 0.25f;
01908 const float C8 = 0.125f;
01909
01910 float RS = R * S;
01911 float ST = S * T;
01912 float RT = R * T;
01913 float RST = R * ST;
01914
01915 float RSQ = 1-R*R;
01916 float SSQ = 1-S*S;
01917 float TSQ = 1-T*T;
01918
01919 float RM1 = (1-R);
01920 float SM1 = (1-S);
01921 float TM1 = (1-T);
01922
01923 float RP1 = (1+R);
01924 float SP1 = (1+S);
01925 float TP1 = (1+T);
01926
01927 float triquad =
01928 (-C8) * RST * RM1 * SM1 * TM1 * fdata[0] +
01929 ( C4) * ST * RSQ * SM1 * TM1 * fdata[1] +
01930 ( C8) * RST * RP1 * SM1 * TM1 * fdata[2] +
01931 ( C4) * RT * RM1 * SSQ * TM1 * fdata[3] +
01932 (-C2) * T * RSQ * SSQ * TM1 * fdata[4] +
01933 (-C4) * RT * RP1 * SSQ * TM1 * fdata[5] +
01934 ( C8) * RST * RM1 * SP1 * TM1 * fdata[6] +
01935 (-C4) * ST * RSQ * SP1 * TM1 * fdata[7] +
01936 (-C8) * RST * RP1 * SP1 * TM1 * fdata[8] +
01937
01938 ( C4) * RS * RM1 * SM1 * TSQ * fdata[9] +
01939 (-C2) * S * RSQ * SM1 * TSQ * fdata[10] +
01940 (-C4) * RS * RP1 * SM1 * TSQ * fdata[11] +
01941 (-C2) * R * RM1 * SSQ * TSQ * fdata[12] +
01942 RSQ * SSQ * TSQ * fdata[13] +
01943 ( C2) * R * RP1 * SSQ * TSQ * fdata[14] +
01944 (-C4) * RS * RM1 * SP1 * TSQ * fdata[15] +
01945 ( C2) * S * RSQ * SP1 * TSQ * fdata[16] +
01946 ( C4) * RS * RP1 * SP1 * TSQ * fdata[17] +
01947
01948 ( C8) * RST * RM1 * SM1 * TP1 * fdata[18] +
01949 (-C4) * ST * RSQ * SM1 * TP1 * fdata[19] +
01950 (-C8) * RST * RP1 * SM1 * TP1 * fdata[20] +
01951 (-C4) * RT * RM1 * SSQ * TP1 * fdata[21] +
01952 ( C2) * T * RSQ * SSQ * TP1 * fdata[22] +
01953 ( C4) * RT * RP1 * SSQ * TP1 * fdata[23] +
01954 (-C8) * RST * RM1 * SP1 * TP1 * fdata[24] +
01955 ( C4) * ST * RSQ * SP1 * TP1 * fdata[25] +
01956 ( C8) * RST * RP1 * SP1 * TP1 * fdata[26] ;
01957 return triquad;
01958 }
01959
01960 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01961 : M(M_), fc(fc_), ntable(ntable_) {
01962
01963 build_sBtable();
01964 }
01965
01966 void Util::sincBlackman::build_sBtable() {
01967 sBtable.resize(ntable+1);
01968 int ltab = int(round(float(ntable)/1.25f));
01969 int M2 = M/2;
01970 fltb = float(ltab)/M2;
01971 for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01972 float x = 1.0e-7f;
01973 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)));
01974 for (int i=1; i <= ltab; i++) {
01975 x = float(i)/fltb;
01976 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)));
01977
01978 }
01979 }
01980
01981 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01982 int N_, float vtable_, int ntable_)
01983 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01984 ntable(ntable_) {
01985
01986 if (0.f == v) v = float(K)/2;
01987 if (0.f == vtable) vtable = v;
01988 alphar = alpha*r;
01989 fac = static_cast<float>(twopi)*alphar*v;
01990 vadjust = 1.0f*v;
01991 facadj = static_cast<float>(twopi)*alphar*vadjust;
01992 build_I0table();
01993 }
01994
01995 float Util::KaiserBessel::i0win(float x) const {
01996 float val0 = float(gsl_sf_bessel_I0(facadj));
01997 float absx = fabs(x);
01998 if (absx > vadjust) return 0.f;
01999 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02000 float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02001 return res;
02002 }
02003
02004 void Util::KaiserBessel::build_I0table() {
02005 i0table.resize(ntable+1);
02006 int ltab = int(round(float(ntable)/1.25f));
02007 fltb = float(ltab)/(K/2);
02008 float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02009 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02010 for (int i=0; i <= ltab; i++) {
02011 float s = float(i)/fltb/N;
02012 if (s < vadjust) {
02013 float rt = sqrt(1.f - pow(s/vadjust, 2));
02014 i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02015 } else {
02016 i0table[i] = 0.f;
02017 }
02018
02019 }
02020 }
02021
02022 float Util::KaiserBessel::I0table_maxerror() {
02023 float maxdiff = 0.f;
02024 for (int i = 1; i <= ntable; i++) {
02025 float diff = fabs(i0table[i] - i0table[i-1]);
02026 if (diff > maxdiff) maxdiff = diff;
02027 }
02028 return maxdiff;
02029 }
02030
02031 float Util::KaiserBessel::sinhwin(float x) const {
02032 float val0 = sinh(fac)/fac;
02033 float absx = fabs(x);
02034 if (0.0 == x) {
02035 float res = 1.0f;
02036 return res;
02037 } else if (absx == alphar) {
02038 return 1.0f/val0;
02039 } else if (absx < alphar) {
02040 float rt = sqrt(1.0f - pow((x/alphar), 2));
02041 float facrt = fac*rt;
02042 float res = (sinh(facrt)/facrt)/val0;
02043 return res;
02044 } else {
02045 float rt = sqrt(pow((x/alphar),2) - 1.f);
02046 float facrt = fac*rt;
02047 float res = (sin(facrt)/facrt)/val0;
02048 return res;
02049 }
02050 }
02051
02052 float Util::FakeKaiserBessel::i0win(float x) const {
02053 float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02054 float absx = fabs(x);
02055 if (absx > vadjust) return 0.f;
02056 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02057 float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02058 return res;
02059 }
02060
02061 void Util::FakeKaiserBessel::build_I0table() {
02062 i0table.resize(ntable+1);
02063 int ltab = int(round(float(ntable)/1.1f));
02064 fltb = float(ltab)/(K/2);
02065 float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02066 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02067 for (int i=0; i <= ltab; i++) {
02068 float s = float(i)/fltb/N;
02069 if (s < vadjust) {
02070 float rt = sqrt(1.f - pow(s/vadjust, 2));
02071 i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02072 } else {
02073 i0table[i] = 0.f;
02074 }
02075 }
02076 }
02077
02078 float Util::FakeKaiserBessel::sinhwin(float x) const {
02079 float val0 = sinh(fac)/fac;
02080 float absx = fabs(x);
02081 if (0.0 == x) {
02082 float res = 1.0f;
02083 return res;
02084 } else if (absx == alphar) {
02085 return 1.0f/val0;
02086 } else if (absx < alphar) {
02087 float rt = sqrt(1.0f - pow((x/alphar), 2));
02088 float facrt = fac*rt;
02089 float res = (sinh(facrt)/facrt)/val0;
02090 return res;
02091 } else {
02092 float rt = sqrt(pow((x/alphar),2) - 1.f);
02093 float facrt = fac*rt;
02094 float res = (sin(facrt)/facrt)/val0;
02095 return res;
02096 }
02097 }
02098
02099 #if 0 // 1-st order KB window
02100 float Util::FakeKaiserBessel::sinhwin(float x) const {
02101
02102 float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02103 float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02104 float absx = fabs(x);
02105 if (0.0 == x) {
02106
02107 float res = val0;
02108 return res;
02109 } else if (absx == alphar) {
02110
02111 return prefix;
02112 } else if (absx < alphar) {
02113 float rt = sqrt(1.0f - pow((x/alphar), 2));
02114
02115 float facrt = facadj*rt;
02116
02117 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02118 return res;
02119 } else {
02120 float rt = sqrt(pow((x/alphar),2) - 1.f);
02121
02122 float facrt = facadj*rt;
02123
02124 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02125 return res;
02126 }
02127 }
02128 #endif // 0
02129
02130
02131
02132 #define circ(i) circ[i-1]
02133 #define numr(i,j) numr[(j-1)*3 + i-1]
02134 #define xim(i,j) xim[(j-1)*nsam + i-1]
02135
02136 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02137 int nsam = image->get_xsize();
02138 int nrow = image->get_ysize();
02139 int nring = numr.size()/3;
02140 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02141 EMData* out = new EMData();
02142 out->set_size(lcirc,1,1);
02143 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02144 float *xim = image->get_data();
02145 float *circ = out->get_data();
02146
02147
02148
02149
02150
02151
02152
02153
02154
02155
02156
02157
02158
02159
02160
02161
02162 double dfi, dpi;
02163 int ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02164 float yq, xold, yold, fi, x, y;
02165
02166 ns2 = nsam/2+1;
02167 nr2 = nrow/2+1;
02168 dpi = 2.0*atan(1.0);
02169
02170 for (i=1;i<=nring;i++) {
02171
02172 inr = numr(1,i);
02173 yq = static_cast<float>(inr);
02174 l = numr(3,i);
02175 if (mode == 'h' || mode == 'H') lt = l/2;
02176 else lt = l/4;
02177
02178 nsim = lt-1;
02179 dfi = dpi/(nsim+1);
02180 kcirc = numr(2,i);
02181 xold = 0.0f;
02182 yold = static_cast<float>(inr);
02183 circ(kcirc) = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02184 xold = static_cast<float>(inr);
02185 yold = 0.0f;
02186 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02187
02188 if (mode == 'f' || mode == 'F') {
02189 xold = 0.0f;
02190 yold = static_cast<float>(-inr);
02191 circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02192 xold = static_cast<float>(-inr);
02193 yold = 0.0f;
02194 circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02195 }
02196
02197 for (j=1;j<=nsim;j++) {
02198 fi = static_cast<float>(dfi*j);
02199 x = sin(fi)*yq;
02200 y = cos(fi)*yq;
02201 xold = x;
02202 yold = y;
02203 circ(j+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02204 xold = y;
02205 yold = -x;
02206 circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02207
02208 if (mode == 'f' || mode == 'F') {
02209 xold = -x;
02210 yold = -y;
02211 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02212 xold = -y;
02213 yold = x;
02214 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02215 }
02216 }
02217 }
02218 return out;
02219 }
02220
02221 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02222 int nsam = image->get_xsize();
02223 int nrow = image->get_ysize();
02224 int nring = numr.size()/3;
02225 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02226 EMData* out = new EMData();
02227 out->set_size(lcirc,1,1);
02228 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02229 float *xim = image->get_data();
02230 float *circ = out->get_data();
02231 double dpi, dfi;
02232 int it, jt, inr, l, nsim, kcirc, lt;
02233 float xold, yold, fi, x, y;
02234
02235
02236
02237 dpi = 2*atan(1.0);
02238 for (it=1; it<=nring; it++) {
02239
02240 inr = numr(1,it);
02241
02242
02243
02244
02245 l = numr(3,it);
02246 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02247 else lt = l / 4;
02248
02249 nsim = lt - 1;
02250 dfi = dpi / (nsim+1);
02251 kcirc = numr(2,it);
02252 xold = 0.0f+cns2;
02253 yold = inr+cnr2;
02254
02255 Assert( kcirc <= lcirc );
02256 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02257
02258 xold = inr+cns2;
02259 yold = 0.0f+cnr2;
02260 Assert( lt+kcirc <= lcirc );
02261 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02262
02263 if ( mode == 'f' || mode == 'F' ) {
02264 xold = 0.0f+cns2;
02265 yold = -inr+cnr2;
02266 Assert( lt+lt+kcirc <= lcirc );
02267 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02268
02269 xold = -inr+cns2;
02270 yold = 0.0f+cnr2;
02271 Assert(lt+lt+lt+kcirc <= lcirc );
02272 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02273 }
02274
02275 for (jt=1; jt<=nsim; jt++) {
02276 fi = static_cast<float>(dfi * jt);
02277 x = sin(fi) * inr;
02278 y = cos(fi) * inr;
02279
02280 xold = x+cns2;
02281 yold = y+cnr2;
02282
02283 Assert( jt+kcirc <= lcirc );
02284 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02285
02286 xold = y+cns2;
02287 yold = -x+cnr2;
02288
02289 Assert( jt+lt+kcirc <= lcirc );
02290 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02291
02292 if ( mode == 'f' || mode == 'F' ) {
02293 xold = -x+cns2;
02294 yold = -y+cnr2;
02295
02296 Assert( jt+lt+lt+kcirc <= lcirc );
02297 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02298
02299 xold = -y+cns2;
02300 yold = x+cnr2;
02301
02302 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02303 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02304 }
02305 }
02306 }
02307 return out;
02308 }
02309
02310 float Util::bilinear(float xold, float yold, int nsam, int nrow, float* xim)
02311 {
02312
02313
02314
02315
02316 float bilinear;
02317 int ixold, iyold;
02318
02319
02320
02321
02322
02323
02324
02325
02326
02327
02328
02329
02330
02331
02332
02333
02334
02335
02336
02337
02338
02339
02340
02341 float xdif, ydif;
02342
02343 ixold = (int) xold;
02344 iyold = (int) yold;
02345 ydif = yold - iyold;
02346
02347
02348
02349
02350
02351 xdif = xold - ixold;
02352 bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02353 xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02354 ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02355
02356 return bilinear;
02357 }
02358
02359 void Util::alrl_ms(float *xim, int nsam, int nrow, float cns2, float cnr2,
02360 int *numr, float *circ, int , int nring, char mode) {
02361 double dpi, dfi;
02362 int it, jt, inr, l, nsim, kcirc, lt;
02363 float xold, yold, fi, x, y;
02364
02365
02366
02367
02368 dpi = 2*atan(1.0);
02369 for (it=1; it<=nring; it++) {
02370
02371 inr = numr(1,it);
02372
02373 l = numr(3,it);
02374 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02375 else lt = l / 4;
02376
02377 nsim = lt - 1;
02378 dfi = dpi / (nsim+1);
02379 kcirc = numr(2,it);
02380
02381
02382 xold = 0.0f+cns2;
02383 yold = inr+cnr2;
02384
02385 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02386
02387 xold = inr+cns2;
02388 yold = 0.0f+cnr2;
02389 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02390
02391 if ( mode == 'f' || mode == 'F' ) {
02392 xold = 0.0f+cns2;
02393 yold = -inr+cnr2;
02394 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02395
02396 xold = -inr+cns2;
02397 yold = 0.0f+cnr2;
02398 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02399 }
02400
02401 for (jt=1; jt<=nsim; jt++) {
02402 fi = static_cast<float>(dfi * jt);
02403 x = sin(fi) * inr;
02404 y = cos(fi) * inr;
02405
02406 xold = x+cns2;
02407 yold = y+cnr2;
02408 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02409
02410 xold = y+cns2;
02411 yold = -x+cnr2;
02412 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02413
02414 if ( mode == 'f' || mode == 'F' ) {
02415 xold = -x+cns2;
02416 yold = -y+cnr2;
02417 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02418
02419 xold = -y+cns2;
02420 yold = x+cnr2;
02421 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02422 }
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 #undef xim
02503
02504 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02505
02506 int nring = numr.size()/3;
02507 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02508 EMData* out = new EMData();
02509 out->set_size(lcirc,1,1);
02510 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02511 float *circ = out->get_data();
02512 float *fimage = image->get_data();
02513 int nx = image->get_xsize();
02514 int ny = image->get_ysize();
02515 int nz = image->get_zsize();
02516 double dpi, dfi;
02517 int it, jt, inr, l, nsim, kcirc, lt;
02518 float yq, xold, yold, fi, x, y;
02519
02520
02521
02522
02523 dpi = 2*atan(1.0);
02524 for (it=1;it<=nring;it++) {
02525
02526 inr = numr(1,it);
02527 yq = static_cast<float>(inr);
02528
02529 l = numr(3,it);
02530 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02531 else lt = l / 4;
02532
02533 nsim = lt - 1;
02534 dfi = dpi / (nsim+1);
02535 kcirc = numr(2,it);
02536 xold = 0.0f;
02537 yold = static_cast<float>(inr);
02538 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02539
02540
02541 xold = static_cast<float>(inr);
02542 yold = 0.0f;
02543 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02544
02545
02546 if ( mode == 'f' || mode == 'F' ) {
02547 xold = 0.0f;
02548 yold = static_cast<float>(-inr);
02549 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02550
02551
02552 xold = static_cast<float>(-inr);
02553 yold = 0.0f;
02554 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);
02555
02556 }
02557
02558 for (jt=1;jt<=nsim;jt++) {
02559 fi = static_cast<float>(dfi * jt);
02560 x = sin(fi) * yq;
02561 y = cos(fi) * yq;
02562
02563 xold = x;
02564 yold = y;
02565 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02566
02567
02568 xold = y;
02569 yold = -x;
02570 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02571
02572
02573 if ( mode == 'f' || mode == 'F' ) {
02574 xold = -x;
02575 yold = -y;
02576 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);
02577
02578
02579 xold = -y;
02580 yold = x;
02581 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);
02582
02583 }
02584 }
02585 }
02586 return out;
02587 }
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 #define tab1(i) tab1[i-1]
02613 #define xcmplx(i,j) xcmplx [(j-1)*2 + i-1]
02614 #define br(i) br[i-1]
02615 #define bi(i) bi[i-1]
02616
02617 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02618 {
02619 double rni,sgn,tr1,tr2,ti1,ti2;
02620 double cc,c,ss,s,t,x2,x3,x4,x5;
02621 int b3,b4,b5,b6,b7,b56;
02622 int n, k, l, j, i, ix0, ix1, status=0;
02623
02624 const double tab1[] = {
02625 9.58737990959775e-5,
02626 1.91747597310703e-4,
02627 3.83495187571395e-4,
02628 7.66990318742704e-4,
02629 1.53398018628476e-3,
02630 3.06795676296598e-3,
02631 6.13588464915449e-3,
02632 1.22715382857199e-2,
02633 2.45412285229123e-2,
02634 4.90676743274181e-2,
02635 9.80171403295604e-2,
02636 1.95090322016128e-1,
02637 3.82683432365090e-1,
02638 7.07106781186546e-1,
02639 1.00000000000000,
02640 };
02641
02642 n=(int)pow(2.0f,ln);
02643
02644 k=abs(ks);
02645 l=16-ln;
02646 b3=n*k;
02647 b6=b3;
02648 b7=k;
02649 if (ks > 0) {
02650 sgn=1.0f;
02651 } else {
02652 sgn=-1.0f;
02653 rni=1.0f/(float)(n);
02654 j=1;
02655 for (i=1; i<=n; i++) {
02656 br(j)=br(j)*rni;
02657 bi(j)=bi(j)*rni;
02658 j=j+k;
02659 }
02660 }
02661
02662 L12:
02663 b6=b6/2;
02664 b5=b6;
02665 b4=2*b6;
02666 b56=b5-b6;
02667
02668 L14:
02669 tr1=br(b5+1);
02670 ti1=bi(b5+1);
02671 tr2=br(b56+1);
02672 ti2=bi(b56+1);
02673
02674 br(b5+1)=tr2-tr1;
02675 bi(b5+1)=ti2-ti1;
02676 br(b56+1)=tr1+tr2;
02677 bi(b56+1)=ti1+ti2;
02678
02679 b5=b5+b4;
02680 b56=b5-b6;
02681 if ( b5 <= b3 ) goto L14;
02682 if ( b6 == b7 ) goto L20;
02683
02684 b4=b7;
02685 cc=2.0f*pow(tab1(l),2);
02686 c=1.0f-cc;
02687 l++;
02688 ss=sgn*tab1(l);
02689 s=ss;
02690
02691 L16:
02692 b5=b6+b4;
02693 b4=2*b6;
02694 b56=b5-b6;
02695
02696 L18:
02697 tr1=br(b5+1);
02698 ti1=bi(b5+1);
02699 tr2=br(b56+1);
02700 ti2=bi(b56+1);
02701 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02702 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02703 br(b56+1)=tr1+tr2;
02704 bi(b56+1)=ti1+ti2;
02705
02706 b5=b5+b4;
02707 b56=b5-b6;
02708 if ( b5 <= b3 ) goto L18;
02709 b4=b5-b6;
02710 b5=b4-b3;
02711 c=-c;
02712 b4=b6-b5;
02713 if ( b5 < b4 ) goto L16;
02714 b4=b4+b7;
02715 if ( b4 >= b5 ) goto L12;
02716
02717 t=c-cc*c-ss*s;
02718 s=s+ss*c-cc*s;
02719 c=t;
02720 goto L16;
02721
02722 L20:
02723 ix0=b3/2;
02724 b3=b3-b7;
02725 b4=0;
02726 b5=0;
02727 b6=ix0;
02728 ix1=0;
02729 if (b6 == b7) goto EXIT;
02730
02731 L22:
02732 b4=b3-b4;
02733 b5=b3-b5;
02734 x2=br(b4+1);
02735 x3=br(b5+1);
02736 x4=bi(b4+1);
02737 x5=bi(b5+1);
02738 br(b4+1)=x3;
02739 br(b5+1)=x2;
02740 bi(b4+1)=x5;
02741 bi(b5+1)=x4;
02742 if(b6 < b4) goto L22;
02743
02744 L24:
02745 b4=b4+b7;
02746 b5=b6+b5;
02747 x2=br(b4+1);
02748 x3=br(b5+1);
02749 x4=bi(b4+1);
02750 x5=bi(b5+1);
02751 br(b4+1)=x3;
02752 br(b5+1)=x2;
02753 bi(b4+1)=x5;
02754 bi(b5+1)=x4;
02755 ix0=b6;
02756
02757 L26:
02758 ix0=ix0/2;
02759 ix1=ix1-ix0;
02760 if( ix1 >= 0) goto L26;
02761
02762 ix0=2*ix0;
02763 b4=b4+b7;
02764 ix1=ix1+ix0;
02765 b5=ix1;
02766 if ( b5 >= b4) goto L22;
02767 if ( b4 < b6) goto L24;
02768
02769 EXIT:
02770 status = 0;
02771 }
02772
02773
02774 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02775 {
02776
02777
02778 int b3,b4,b5,b6,b7,b56;
02779 int n, k, l, j, i, ix0, ix1;
02780 float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02781 int status=0;
02782
02783 const float tab1[] = {
02784 9.58737990959775e-5f,
02785 1.91747597310703e-4f,
02786 3.83495187571395e-4f,
02787 7.66990318742704e-4f,
02788 1.53398018628476e-3f,
02789 3.06795676296598e-3f,
02790 6.13588464915449e-3f,
02791 1.22715382857199e-2f,
02792 2.45412285229123e-2f,
02793 4.90676743274181e-2f,
02794 9.80171403295604e-2f,
02795 1.95090322016128e-1f,
02796 3.82683432365090e-1f,
02797 7.07106781186546e-1f,
02798 1.00000000000000f,
02799 };
02800
02801 n=(int)pow(2.0f,ln);
02802
02803 k=abs(ks);
02804 l=16-ln;
02805 b3=n*k;
02806 b6=b3;
02807 b7=k;
02808 if( ks > 0 ) {
02809 sgn=1.0f;
02810 } else {
02811 sgn=-1.0f;
02812 rni=1.0f/(float)n;
02813 j=1;
02814 for (i=1; i<=n; i++) {
02815 br(j)=br(j)*rni;
02816 bi(j)=bi(j)*rni;
02817 j=j+k;
02818 }
02819 }
02820 L12:
02821 b6=b6/2;
02822 b5=b6;
02823 b4=2*b6;
02824 b56=b5-b6;
02825 L14:
02826 tr1=br(b5+1);
02827 ti1=bi(b5+1);
02828
02829 tr2=br(b56+1);
02830 ti2=bi(b56+1);
02831
02832 br(b5+1)=tr2-tr1;
02833 bi(b5+1)=ti2-ti1;
02834 br(b56+1)=tr1+tr2;
02835 bi(b56+1)=ti1+ti2;
02836
02837 b5=b5+b4;
02838 b56=b5-b6;
02839 if ( b5 <= b3 ) goto L14;
02840 if ( b6 == b7 ) goto L20;
02841
02842 b4=b7;
02843 cc=2.0f*pow(tab1(l),2);
02844 c=1.0f-cc;
02845 l++;
02846 ss=sgn*tab1(l);
02847 s=ss;
02848 L16:
02849 b5=b6+b4;
02850 b4=2*b6;
02851 b56=b5-b6;
02852 L18:
02853 tr1=br(b5+1);
02854 ti1=bi(b5+1);
02855 tr2=br(b56+1);
02856 ti2=bi(b56+1);
02857 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02858 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02859 br(b56+1)=tr1+tr2;
02860 bi(b56+1)=ti1+ti2;
02861
02862 b5=b5+b4;
02863 b56=b5-b6;
02864 if(b5 <= b3) goto L18;
02865 b4=b5-b6;
02866 b5=b4-b3;
02867 c=-c;
02868 b4=b6-b5;
02869 if(b5 < b4) goto L16;
02870 b4=b4+b7;
02871 if(b4 >= b5) goto L12;
02872
02873 t=c-cc*c-ss*s;
02874 s=s+ss*c-cc*s;
02875 c=t;
02876 goto L16;
02877 L20:
02878 ix0=b3/2;
02879 b3=b3-b7;
02880 b4=0;
02881 b5=0;
02882 b6=ix0;
02883 ix1=0;
02884 if ( b6 == b7) goto EXIT;
02885 L22:
02886 b4=b3-b4;
02887 b5=b3-b5;
02888 x2=br(b4+1);
02889 x3=br(b5+1);
02890 x4=bi(b4+1);
02891 x5=bi(b5+1);
02892 br(b4+1)=x3;
02893 br(b5+1)=x2;
02894 bi(b4+1)=x5;
02895 bi(b5+1)=x4;
02896 if (b6 < b4) goto L22;
02897 L24:
02898 b4=b4+b7;
02899 b5=b6+b5;
02900 x2=br(b4+1);
02901 x3=br(b5+1);
02902 x4=bi(b4+1);
02903 x5=bi(b5+1);
02904 br(b4+1)=x3;
02905 br(b5+1)=x2;
02906 bi(b4+1)=x5;
02907 bi(b5+1)=x4;
02908 ix0=b6;
02909 L26:
02910 ix0=ix0/2;
02911 ix1=ix1-ix0;
02912 if(ix1 >= 0) goto L26;
02913
02914 ix0=2*ix0;
02915 b4=b4+b7;
02916 ix1=ix1+ix0;
02917 b5=ix1;
02918 if (b5 >= b4) goto L22;
02919 if (b4 < b6) goto L24;
02920 EXIT:
02921 status = 0;
02922 }
02923
02924 void Util::fftr_q(float *xcmplx, int nv)
02925 {
02926
02927
02928 int nu, inv, nu1, n, isub, n2, i1, i2, i;
02929 float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02930
02931 const float tab1[] = {
02932 9.58737990959775e-5f,
02933 1.91747597310703e-4f,
02934 3.83495187571395e-4f,
02935 7.66990318742704e-4f,
02936 1.53398018628476e-3f,
02937 3.06795676296598e-3f,
02938 6.13588464915449e-3f,
02939 1.22715382857199e-2f,
02940 2.45412285229123e-2f,
02941 4.90676743274181e-2f,
02942 9.80171403295604e-2f,
02943 1.95090322016128e-1f,
02944 3.82683432365090e-1f,
02945 7.07106781186546e-1f,
02946 1.00000000000000f,
02947 };
02948
02949 nu=abs(nv);
02950 inv=nv/nu;
02951 nu1=nu-1;
02952 n=(int)pow(2.f,nu1);
02953 isub=16-nu1;
02954
02955 ss=-tab1(isub);
02956 cc=-2.0f*pow(tab1(isub-1),2.f);
02957 c=1.0f;
02958 s=0.0f;
02959 n2=n/2;
02960 if ( inv > 0) {
02961 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02962 tr=xcmplx(1,1);
02963 ti=xcmplx(2,1);
02964 xcmplx(1,1)=tr+ti;
02965 xcmplx(2,1)=tr-ti;
02966 for (i=1;i<=n2;i++) {
02967 i1=i+1;
02968 i2=n-i+1;
02969 tr1=xcmplx(1,i1);
02970 tr2=xcmplx(1,i2);
02971 ti1=xcmplx(2,i1);
02972 ti2=xcmplx(2,i2);
02973 t=(cc*c-ss*s)+c;
02974 s=(cc*s+ss*c)+s;
02975 c=t;
02976 xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02977 xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02978 xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02979 xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02980 }
02981 } else {
02982 tr=xcmplx(1,1);
02983 ti=xcmplx(2,1);
02984 xcmplx(1,1)=0.5f*(tr+ti);
02985 xcmplx(2,1)=0.5f*(tr-ti);
02986 for (i=1; i<=n2; i++) {
02987 i1=i+1;
02988 i2=n-i+1;
02989 tr1=xcmplx(1,i1);
02990 tr2=xcmplx(1,i2);
02991 ti1=xcmplx(2,i1);
02992 ti2=xcmplx(2,i2);
02993 t=(cc*c-ss*s)+c;
02994 s=(cc*s+ss*c)+s;
02995 c=t;
02996 xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
02997 xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
02998 xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
02999 xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03000 }
03001 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03002 }
03003 }
03004
03005
03006 void Util::fftr_d(double *xcmplx, int nv)
03007 {
03008
03009 int i1, i2, nu, inv, nu1, n, isub, n2, i;
03010 double tr1,tr2,ti1,ti2,tr,ti;
03011 double cc,c,ss,s,t;
03012 const double tab1[] = {
03013 9.58737990959775e-5,
03014 1.91747597310703e-4,
03015 3.83495187571395e-4,
03016 7.66990318742704e-4,
03017 1.53398018628476e-3,
03018 3.06795676296598e-3,
03019 6.13588464915449e-3,
03020 1.22715382857199e-2,
03021 2.45412285229123e-2,
03022 4.90676743274181e-2,
03023 9.80171403295604e-2,
03024 1.95090322016128e-1,
03025 3.82683432365090e-1,
03026 7.07106781186546e-1,
03027 1.00000000000000,
03028 };
03029
03030 nu=abs(nv);
03031 inv=nv/nu;
03032 nu1=nu-1;
03033 n=(int)pow(2.0f,nu1);
03034 isub=16-nu1;
03035 ss=-tab1(isub);
03036 cc=-2.0*pow(tab1(isub-1),2);
03037 c=1.0f;
03038 s=0.0f;
03039 n2=n/2;
03040
03041 if ( inv > 0 ) {
03042 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03043 tr=xcmplx(1,1);
03044 ti=xcmplx(2,1);
03045 xcmplx(1,1)=tr+ti;
03046 xcmplx(2,1)=tr-ti;
03047 for (i=1;i<=n2;i++) {
03048 i1=i+1;
03049 i2=n-i+1;
03050 tr1=xcmplx(1,i1);
03051 tr2=xcmplx(1,i2);
03052 ti1=xcmplx(2,i1);
03053 ti2=xcmplx(2,i2);
03054 t=(cc*c-ss*s)+c;
03055 s=(cc*s+ss*c)+s;
03056 c=t;
03057 xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03058 xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03059 xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03060 xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03061 }
03062 } else {
03063 tr=xcmplx(1,1);
03064 ti=xcmplx(2,1);
03065 xcmplx(1,1)=0.5*(tr+ti);
03066 xcmplx(2,1)=0.5*(tr-ti);
03067 for (i=1; i<=n2; i++) {
03068 i1=i+1;
03069 i2=n-i+1;
03070 tr1=xcmplx(1,i1);
03071 tr2=xcmplx(1,i2);
03072 ti1=xcmplx(2,i1);
03073 ti2=xcmplx(2,i2);
03074 t=(cc*c-ss*s)+c;
03075 s=(cc*s+ss*c)+s;
03076 c=t;
03077 xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03078 xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03079 xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03080 xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03081 }
03082 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03083 }
03084 }
03085 #undef tab1
03086 #undef xcmplx
03087 #undef br
03088 #undef bi
03089
03090
03091 void Util::Frngs(EMData* circp, vector<int> numr){
03092 int nring = numr.size()/3;
03093 float *circ = circp->get_data();
03094 int i, l;
03095 for (i=1; i<=nring;i++) {
03096
03097 #ifdef _WIN32
03098 l = (int)( log((float)numr(3,i))/log(2.0f) );
03099 #else
03100 l=(int)(log2(numr(3,i)));
03101 #endif //_WIN32
03102
03103 fftr_q(&circ(numr(2,i)),l);
03104 }
03105 }
03106
03107 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03108 int nring = numr.size()/3;
03109 float *circ = circp->get_data();
03110 int i, l;
03111 for (i=1; i<=nring;i++) {
03112
03113 #ifdef _WIN32
03114 l = (int)( log((float)numr(3,i))/log(2.0f) );
03115 #else
03116 l=(int)(log2(numr(3,i)));
03117 #endif //_WIN32
03118
03119 fftr_q(&circ(numr(2,i)),-l);
03120 }
03121 }
03122 #undef circ
03123
03124 #define b(i) b[i-1]
03125 void Util::prb1d(double *b, int npoint, float *pos) {
03126 double c2,c3;
03127 int nhalf;
03128
03129 nhalf = npoint/2 + 1;
03130 *pos = 0.0;
03131
03132 if (npoint == 7) {
03133 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03134 - 6.*b(6) + 31.*b(7);
03135 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03136 }
03137 else if (npoint == 5) {
03138 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03139 + 46.*b(5) ) / (-70.);
03140 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03141 }
03142 else if (npoint == 3) {
03143 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03144 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03145 }
03146
03147 else {
03148 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03149 - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03150 + 1092.*b(9) ) / (-4620.);
03151 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03152 - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03153 }
03154 if (c3 != 0.0) *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03155 }
03156 #undef b
03157
03158 #define circ1(i) circ1[i-1]
03159 #define circ2(i) circ2[i-1]
03160 #define t(i) t[i-1]
03161 #define q(i) q[i-1]
03162 #define b(i) b[i-1]
03163 #define t7(i) t7[i-1]
03164 Dict Util::Crosrng_e(EMData* circ1p, EMData* circ2p, vector<int> numr, int neg) {
03165
03166 int nring = numr.size()/3;
03167
03168 int maxrin = numr[numr.size()-1];
03169 double qn; float tot;
03170 float *circ1 = circ1p->get_data();
03171 float *circ2 = circ2p->get_data();
03172
03173
03174
03175
03176
03177
03178
03179
03180
03181
03182
03183 float *t;
03184 double t7[7], *q;
03185 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03186 float pos;
03187
03188 #ifdef _WIN32
03189 ip = -(int)(log((float)maxrin)/log(2.0f));
03190 #else
03191 ip = -(int) (log2(maxrin));
03192 #endif //_WIN32
03193
03194 q = (double*)calloc(maxrin, sizeof(double));
03195 t = (float*)calloc(maxrin, sizeof(float));
03196
03197
03198 for (i=1; i<=nring; i++) {
03199 numr3i = numr(3,i);
03200 numr2i = numr(2,i);
03201
03202 t(1) = (circ1(numr2i)) * circ2(numr2i);
03203
03204 if (numr3i != maxrin) {
03205
03206 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03207 t(2) = 0.0;
03208
03209 if (neg) {
03210
03211 for (j=3;j<=numr3i;j=j+2) {
03212 jc = j+numr2i-1;
03213 t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03214 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03215 }
03216 } else {
03217 for (j=3;j<=numr3i;j=j+2) {
03218 jc = j+numr2i-1;
03219 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03220 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03221 }
03222 }
03223 for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03224 } else {
03225 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03226 if (neg) {
03227
03228 for (j=3;j<=maxrin;j=j+2) {
03229 jc = j+numr2i-1;
03230 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03231 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03232 }
03233 } else {
03234 for (j=3;j<=maxrin;j=j+2) {
03235 jc = j+numr2i-1;
03236 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03237 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03238 }
03239 }
03240 for (j = 1; j <= maxrin; j++) q(j) += t(j);
03241 }
03242 }
03243
03244 fftr_d(q,ip);
03245
03246 qn = -1.0e20;
03247 for (j=1;j<=maxrin;j++) {
03248 if (q(j) >= qn) {
03249 qn = q(j); jtot = j;
03250 }
03251 }
03252
03253 for (k=-3; k<=3; k++) {
03254 j = (jtot+k+maxrin-1)%maxrin + 1;
03255 t7(k+4) = q(j);
03256 }
03257
03258 prb1d(t7,7,&pos);
03259
03260 tot = (float)jtot + pos;
03261
03262 if (q) free(q);
03263 if (t) free(t);
03264
03265 Dict retvals;
03266 retvals["qn"] = qn;
03267 retvals["tot"] = tot;
03268 return retvals;
03269 }
03270
03271 Dict Util::Crosrng_ew(EMData* circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03272
03273 int nring = numr.size()/3;
03274
03275 int maxrin = numr[numr.size()-1];
03276 double qn; float tot;
03277 float *circ1 = circ1p->get_data();
03278 float *circ2 = circ2p->get_data();
03279
03280
03281
03282
03283
03284
03285
03286
03287
03288
03289
03290 float *t;
03291 double t7[7], *q;
03292 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03293 float pos;
03294
03295 #ifdef _WIN32
03296 ip = -(int)(log((float)maxrin)/log(2.0f));
03297 #else
03298 ip = -(int) (log2(maxrin));
03299 #endif //_WIN32
03300
03301 q = (double*)calloc(maxrin, sizeof(double));
03302 t = (float*)calloc(maxrin, sizeof(float));
03303
03304
03305 for (i=1;i<=nring;i++) {
03306 numr3i = numr(3,i);
03307 numr2i = numr(2,i);
03308
03309 t(1) = circ1(numr2i) * circ2(numr2i);
03310
03311 if (numr3i != maxrin) {
03312
03313 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03314 t(2) = 0.0;
03315
03316 if (neg) {
03317
03318 for (j=3; j<=numr3i; j=j+2) {
03319 jc = j+numr2i-1;
03320 t(j) = (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03321 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03322 }
03323 } else {
03324 for (j=3; j<=numr3i; j=j+2) {
03325 jc = j+numr2i-1;
03326 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03327 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03328 }
03329 }
03330 for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03331 } else {
03332 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03333 if (neg) {
03334
03335 for (j=3; j<=maxrin; j=j+2) {
03336 jc = j+numr2i-1;
03337 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03338 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03339 }
03340 } else {
03341 for (j=3; j<=maxrin; j=j+2) {
03342 jc = j+numr2i-1;
03343 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03344 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03345 }
03346 }
03347 for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03348 }
03349 }
03350
03351 fftr_d(q,ip);
03352
03353 qn = -1.0e20;
03354 for (j=1;j<=maxrin;j++) {
03355
03356 if (q(j) >= qn) {
03357 qn = q(j);
03358 jtot = j;
03359 }
03360 }
03361
03362 for (k=-3; k<=3; k++) {
03363 j = (jtot+k+maxrin-1)%maxrin + 1;
03364 t7(k+4) = q(j);
03365 }
03366
03367 prb1d(t7,7,&pos);
03368
03369 tot = (float)jtot + pos;
03370
03371
03372 if (t) free(t);
03373
03374 Dict retvals;
03375
03376
03377 retvals["qn"] = qn;
03378 retvals["tot"] = tot;
03379
03380 if (q) free(q);
03381
03382 return retvals;
03383 }
03384
03385 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03386 int nring = numr.size()/3;
03387
03388 int maxrin = numr[numr.size()-1];
03389 double qn; float tot; double qm; float tmt;
03390 float *circ1 = circ1p->get_data();
03391 float *circ2 = circ2p->get_data();
03392
03393
03394
03395
03396
03397
03398
03399
03400
03401
03402
03403
03404 double *t, *q, t7[7];
03405
03406 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03407 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03408
03409 qn = 0.0f;
03410 qm = 0.0f;
03411 tot = 0.0f;
03412 tmt = 0.0f;
03413 #ifdef _WIN32
03414 ip = -(int)(log((float)maxrin)/log(2.0f));
03415 #else
03416 ip = -(int)(log2(maxrin));
03417 #endif //_WIN32
03418
03419
03420
03421
03422
03423 q = (double*)calloc(maxrin,sizeof(double));
03424
03425
03426
03427 t = (double*)calloc(maxrin,sizeof(double));
03428
03429
03430 for (i=1; i<=nring; i++) {
03431
03432 numr3i = numr(3,i);
03433 numr2i = numr(2,i);
03434
03435 t1 = circ1(numr2i) * circ2(numr2i);
03436 q(1) += t1;
03437 t(1) += t1;
03438
03439 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03440 if (numr3i == maxrin) {
03441 q(2) += t1;
03442 t(2) += t1;
03443 } else {
03444 q(numr3i+1) += t1;
03445 t(numr3i+1) += t1;
03446 }
03447
03448 for (j=3; j<=numr3i; j += 2) {
03449 jc = j+numr2i-1;
03450
03451
03452
03453
03454
03455
03456
03457
03458 c1 = circ1(jc);
03459 c2 = circ1(jc+1);
03460 d1 = circ2(jc);
03461 d2 = circ2(jc+1);
03462
03463 t1 = c1 * d1;
03464 t2 = c2 * d2;
03465 t3 = c1 * d2;
03466 t4 = c2 * d1;
03467
03468 q(j) += t1 + t2;
03469 q(j+1) += -t3 + t4;
03470 t(j) += t1 - t2;
03471 t(j+1) += -t3 - t4;
03472 }
03473 }
03474
03475 fftr_d(q,ip);
03476
03477 qn = -1.0e20;
03478 for (j=1; j<=maxrin; j++) {
03479 if (q(j) >= qn) {
03480 qn = q(j);
03481 jtot = j;
03482 }
03483 }
03484
03485 for (k=-3; k<=3; k++) {
03486 j = ((jtot+k+maxrin-1)%maxrin)+1;
03487 t7(k+4) = q(j);
03488 }
03489
03490
03491 prb1d(t7,7,&pos);
03492 tot = (float)(jtot)+pos;
03493
03494
03495
03496
03497 fftr_d(t,ip);
03498
03499
03500 qm = -1.0e20;
03501 for (j=1; j<=maxrin;j++) {
03502 if ( t(j) >= qm ) {
03503 qm = t(j);
03504 jtot = j;
03505 }
03506 }
03507
03508 for (k=-3; k<=3; k++) {
03509 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03510 t7(k+4) = t(j);
03511 }
03512
03513
03514
03515 prb1d(t7,7,&pos);
03516 tmt = float(jtot) + pos;
03517
03518
03519
03520 free(t);
03521 free(q);
03522
03523 Dict retvals;
03524 retvals["qn"] = qn;
03525 retvals["tot"] = tot;
03526 retvals["qm"] = qm;
03527 retvals["tmt"] = tmt;
03528 return retvals;
03529 }
03530
03531 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03532 int nring = numr.size()/3;
03533
03534 int maxrin = numr[numr.size()-1];
03535 double qn; float tot; double qm; float tmt;
03536 float *circ1 = circ1p->get_data();
03537 float *circ2 = circ2p->get_data();
03538
03539
03540
03541
03542
03543
03544
03545
03546
03547
03548
03549
03550 double *t, *q;
03551
03552 int ip, jc, numr3i, numr2i, i, j, jtot = 0;
03553 float t1, t2, t3, t4, c1, c2, d1, d2;
03554
03555 qn = 0.0f;
03556 qm = 0.0f;
03557 tot = 0.0f;
03558 tmt = 0.0f;
03559 #ifdef _WIN32
03560 ip = -(int)(log((float)maxrin)/log(2.0f));
03561 #else
03562 ip = -(int)(log2(maxrin));
03563 #endif //_WIN32
03564
03565
03566
03567
03568
03569 q = (double*)calloc(maxrin,sizeof(double));
03570
03571
03572
03573 t = (double*)calloc(maxrin,sizeof(double));
03574
03575
03576 for (i=1; i<=nring; i++) {
03577
03578 numr3i = numr(3,i);
03579 numr2i = numr(2,i);
03580
03581 t1 = circ1(numr2i) * circ2(numr2i);
03582 q(1) += t1;
03583 t(1) += t1;
03584
03585 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03586 if (numr3i == maxrin) {
03587 q(2) += t1;
03588 t(2) += t1;
03589 } else {
03590 q(numr3i+1) += t1;
03591 t(numr3i+1) += t1;
03592 }
03593
03594 for (j=3; j<=numr3i; j += 2) {
03595 jc = j+numr2i-1;
03596
03597
03598
03599
03600
03601
03602
03603
03604 c1 = circ1(jc);
03605 c2 = circ1(jc+1);
03606 d1 = circ2(jc);
03607 d2 = circ2(jc+1);
03608
03609 t1 = c1 * d1;
03610 t2 = c2 * d2;
03611 t3 = c1 * d2;
03612 t4 = c2 * d1;
03613
03614 q(j) += t1 + t2;
03615 q(j+1) += -t3 + t4;
03616 t(j) += t1 - t2;
03617 t(j+1) += -t3 - t4;
03618 }
03619 }
03620
03621 fftr_d(q,ip);
03622
03623 qn = -1.0e20;
03624
03625 int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03626 int jstep = static_cast<int>(delta/360.0*maxrin);
03627 if (jstep < 1) { jstep = 1; }
03628
03629 for (j=jstart; j<=maxrin; j+=jstep) {
03630 if (q(j) >= qn) {
03631 qn = q(j);
03632 jtot = j;
03633 }
03634 }
03635
03636
03637
03638
03639
03640
03641
03642
03643
03644
03645 tot = (float)(jtot);
03646
03647
03648 fftr_d(t,ip);
03649
03650
03651 qm = -1.0e20;
03652 for (j=jstart; j<=maxrin;j+=jstep) {
03653 if ( t(j) >= qm ) {
03654 qm = t(j);
03655 jtot = j;
03656 }
03657 }
03658
03659
03660
03661
03662
03663
03664
03665
03666
03667
03668
03669 tmt = float(jtot);
03670
03671 free(t);
03672 free(q);
03673
03674 Dict retvals;
03675 retvals["qn"] = qn;
03676 retvals["tot"] = tot;
03677 retvals["qm"] = qm;
03678 retvals["tmt"] = tmt;
03679 return retvals;
03680 }
03681
03682
03683 Dict Util::Crosrng_psi_0_180(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03684 int nring = numr.size()/3;
03685
03686 int maxrin = numr[numr.size()-1];
03687 double qn; float tot; double qm; float tmt;
03688 float *circ1 = circ1p->get_data();
03689 float *circ2 = circ2p->get_data();
03690
03691
03692
03693
03694 double *t, *q, t7[7];
03695
03696 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03697 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03698
03699 qn = 0.0f;
03700 qm = 0.0f;
03701 tot = 0.0f;
03702 tmt = 0.0f;
03703 #ifdef _WIN32
03704 ip = -(int)(log((float)maxrin)/log(2.0f));
03705 #else
03706 ip = -(int)(log2(maxrin));
03707 #endif //_WIN32
03708
03709
03710
03711
03712
03713 q = (double*)calloc(maxrin,sizeof(double));
03714
03715
03716
03717 t = (double*)calloc(maxrin,sizeof(double));
03718
03719
03720 for (i=1; i<=nring; i++) {
03721
03722 numr3i = numr(3,i);
03723 numr2i = numr(2,i);
03724
03725 t1 = circ1(numr2i) * circ2(numr2i);
03726 q(1) += t1;
03727 t(1) += t1;
03728
03729 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03730 if (numr3i == maxrin) {
03731 q(2) += t1;
03732 t(2) += t1;
03733 } else {
03734 q(numr3i+1) += t1;
03735 t(numr3i+1) += t1;
03736 }
03737
03738 for (j=3; j<=numr3i; j += 2) {
03739 jc = j+numr2i-1;
03740
03741
03742
03743
03744
03745
03746
03747
03748 c1 = circ1(jc);
03749 c2 = circ1(jc+1);
03750 d1 = circ2(jc);
03751 d2 = circ2(jc+1);
03752
03753 t1 = c1 * d1;
03754 t2 = c2 * d2;
03755 t3 = c1 * d2;
03756 t4 = c2 * d1;
03757
03758 q(j) += t1 + t2;
03759 q(j+1) += -t3 + t4;
03760 t(j) += t1 - t2;
03761 t(j+1) += -t3 - t4;
03762 }
03763 }
03764
03765 fftr_d(q,ip);
03766
03767 int psi_range = int(psi_max/360.0*maxrin+0.5);
03768 const int psi_0 = 0;
03769 int psi_180 = int( 180.0/360.0*maxrin+0.5);
03770
03771 qn = -1.0e20;
03772 for (k=-psi_range; k<=psi_range; k++) {
03773 j = (k+psi_0+maxrin-1)%maxrin+1;
03774 if (q(j) >= qn) {
03775 qn = q(j);
03776 jtot = j;
03777 }
03778 }
03779
03780 for (k=-psi_range; k<=psi_range; k++) {
03781 j = (k+psi_180+maxrin-1)%maxrin+1;
03782 if (q(j) >= qn) {
03783 qn = q(j);
03784 jtot = j;
03785 }
03786 }
03787
03788 for (k=-3; k<=3; k++) {
03789 j = ((jtot+k+maxrin-1)%maxrin)+1;
03790 t7(k+4) = q(j);
03791 }
03792
03793
03794 prb1d(t7,7,&pos);
03795 tot = (float)(jtot)+pos;
03796
03797
03798
03799
03800 fftr_d(t,ip);
03801
03802
03803 qm = -1.0e20;
03804 for (k=-psi_range; k<=psi_range; k++) {
03805 j = (k+psi_0+maxrin-1)%maxrin+1;
03806 if (t(j) >= qm) {
03807 qm = t(j);
03808 jtot = j;
03809 }
03810 }
03811
03812 for (k=-psi_range; k<=psi_range; k++) {
03813 j = (k+psi_180+maxrin-1)%maxrin+1;
03814 if (t(j) >= qm) {
03815 qm = t(j);
03816 jtot = j;
03817 }
03818 }
03819
03820 for (k=-3; k<=3; k++) {
03821 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03822 t7(k+4) = t(j);
03823 }
03824
03825
03826
03827 prb1d(t7,7,&pos);
03828 tmt = float(jtot) + pos;
03829
03830
03831
03832 free(t);
03833 free(q);
03834
03835 Dict retvals;
03836 retvals["qn"] = qn;
03837 retvals["tot"] = tot;
03838 retvals["qm"] = qm;
03839 retvals["tmt"] = tmt;
03840 return retvals;
03841 }
03842
03843
03844 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag) {
03845
03846
03847 int nring = numr.size()/3;
03848 int maxrin = numr[numr.size()-1];
03849 double qn; float tot; double qm; float tmt;
03850 float *circ1 = circ1p->get_data();
03851 float *circ2 = circ2p->get_data();
03852
03853 double *q, t7[7];
03854
03855 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03856 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03857
03858 qn = 0.0f;
03859 qm = 0.0f;
03860 tot = 0.0f;
03861 tmt = 0.0f;
03862 #ifdef _WIN32
03863 ip = -(int)(log((float)maxrin)/log(2.0f));
03864 #else
03865 ip = -(int)(log2(maxrin));
03866 #endif //_WIN32
03867
03868
03869
03870
03871 q = (double*)calloc(maxrin,sizeof(double));
03872
03873
03874 if (flag==0) {
03875 for (i=1; i<=nring; i++) {
03876
03877 numr3i = numr(3,i);
03878 numr2i = numr(2,i);
03879
03880 t1 = circ1(numr2i) * circ2(numr2i);
03881 q(1) += t1;
03882
03883 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03884 if (numr3i == maxrin) {
03885 q(2) += t1;
03886 } else {
03887 q(numr3i+1) += t1;
03888 }
03889
03890 for (j=3; j<=numr3i; j += 2) {
03891 jc = j+numr2i-1;
03892
03893
03894
03895
03896
03897 c1 = circ1(jc);
03898 c2 = circ1(jc+1);
03899 d1 = circ2(jc);
03900 d2 = circ2(jc+1);
03901
03902 t1 = c1 * d1;
03903 t3 = c1 * d2;
03904 t2 = c2 * d2;
03905 t4 = c2 * d1;
03906
03907 q(j) += t1 + t2;
03908 q(j+1) += -t3 + t4;
03909 }
03910 }
03911 } else {
03912 for (i=1; i<=nring; i++) {
03913
03914 numr3i = numr(3,i);
03915 numr2i = numr(2,i);
03916
03917 t1 = circ1(numr2i) * circ2(numr2i);
03918 q(1) += t1;
03919
03920 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03921 if (numr3i == maxrin) {
03922 q(2) += t1;
03923 } else {
03924 q(numr3i+1) += t1;
03925 }
03926
03927 for (j=3; j<=numr3i; j += 2) {
03928 jc = j+numr2i-1;
03929
03930
03931
03932
03933
03934 c1 = circ1(jc);
03935 c2 = circ1(jc+1);
03936 d1 = circ2(jc);
03937 d2 = circ2(jc+1);
03938
03939 t1 = c1 * d1;
03940 t3 = c1 * d2;
03941 t2 = c2 * d2;
03942 t4 = c2 * d1;
03943
03944 q(j) += t1 - t2;
03945 q(j+1) += -t3 - t4;
03946 }
03947 }
03948 }
03949 fftr_d(q,ip);
03950
03951 qn = -1.0e20;
03952 int psi_pos = int(psi/360.0*maxrin+0.5);
03953
03954 for (k=-5; k<=5; k++) {
03955 j = (psi_pos+maxrin-1)%maxrin+1;
03956 if (q(j) >= qn) {
03957 qn = q(j);
03958 jtot = j;
03959 }
03960 }
03961
03962 for (k=-3; k<=3; k++) {
03963 j = ((jtot+k+maxrin-1)%maxrin)+1;
03964 t7(k+4) = q(j);
03965 }
03966
03967
03968 prb1d(t7,7,&pos);
03969 tot = (float)(jtot)+pos;
03970 free(q);
03971
03972 Dict retvals;
03973 retvals["qn"] = qn;
03974 retvals["tot"] = tot;
03975 return retvals;
03976 }
03977
03978 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03979 int nring = numr.size()/3;
03980
03981 int maxrin = numr[numr.size()-1];
03982 double qn; float tot;
03983 float *circ1 = circ1p->get_data();
03984 float *circ2 = circ2p->get_data();
03985
03986
03987
03988
03989
03990
03991
03992
03993
03994
03995
03996
03997 double *q, t7[7];
03998
03999 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04000 float c1, c2, d1, d2, pos;
04001
04002 qn = 0.0;
04003 tot = 0.0;
04004 #ifdef _WIN32
04005 ip = -(int)(log((float)maxrin)/log(2.0f));
04006 #else
04007 ip = -(int)(log2(maxrin));
04008 #endif //_WIN32
04009
04010
04011
04012
04013
04014 q = (double*)calloc(maxrin,sizeof(double));
04015
04016
04017 for (i=1; i<=nring; i++) {
04018
04019 numr3i = numr(3,i);
04020 numr2i = numr(2,i);
04021
04022 q(1) += circ1(numr2i) * circ2(numr2i);
04023
04024 if (numr3i == maxrin) q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04025 else q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04026
04027 for (j=3; j<=numr3i; j += 2) {
04028 jc = j+numr2i-1;
04029
04030
04031
04032
04033
04034 c1 = circ1(jc);
04035 c2 = circ1(jc+1);
04036 d1 = circ2(jc);
04037 d2 = circ2(jc+1);
04038
04039 q(j) += c1 * d1 + c2 * d2;
04040 q(j+1) += -c1 * d2 + c2 * d1;
04041 }
04042 }
04043
04044 fftr_d(q,ip);
04045
04046 qn = -1.0e20;
04047 for (j=1; j<=maxrin; j++) {
04048 if (q(j) >= qn) {
04049 qn = q(j);
04050 jtot = j;
04051 }
04052 }
04053
04054 for (k=-3; k<=3; k++) {
04055 j = ((jtot+k+maxrin-1)%maxrin)+1;
04056 t7(k+4) = q(j);
04057 }
04058
04059
04060 prb1d(t7,7,&pos);
04061 tot = (float)(jtot)+pos;
04062
04063
04064
04065 free(q);
04066
04067 Dict retvals;
04068 retvals["qn"] = qn;
04069 retvals["tot"] = tot;
04070 return retvals;
04071 }
04072
04073 #define dout(i,j) dout[i+maxrin*j]
04074 #define circ1b(i) circ1b[i-1]
04075 #define circ2b(i) circ2b[i-1]
04076
04077 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04078
04079
04080
04081 int ip, jc, numr3i, numr2i, i, j;
04082 float t1, t2, t3, t4, c1, c2, d1, d2;
04083
04084 int nring = numr.size()/3;
04085
04086 int maxrin = numr[numr.size()-1];
04087
04088 float* circ1b = circ1->get_data();
04089 float* circ2b = circ2->get_data();
04090
04091
04092 double *t, *q;
04093
04094 q = (double*)calloc(maxrin,sizeof(double));
04095 t = (double*)calloc(maxrin,sizeof(double));
04096
04097 #ifdef _WIN32
04098 ip = -(int)(log((float)maxrin)/log(2.0f));
04099 #else
04100 ip = -(int)(log2(maxrin));
04101 #endif //_WIN32
04102
04103
04104
04105
04106
04107
04108
04109 for (i=1; i<=nring; i++) {
04110
04111 numr3i = numr(3,i);
04112 numr2i = numr(2,i);
04113
04114 t1 = circ1b(numr2i) * circ2b(numr2i);
04115 q(1) = q(1)+t1;
04116 t(1) = t(1)+t1;
04117
04118 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04119 if (numr3i == maxrin) {
04120 q(2) += t1;
04121 t(2) += t1;
04122 } else {
04123 q(numr3i+1) += t1;
04124 t(numr3i+1) += t1;
04125 }
04126
04127 for (j=3; j<=numr3i; j=j+2) {
04128 jc = j+numr2i-1;
04129
04130 c1 = circ1b(jc);
04131 c2 = circ1b(jc+1);
04132 d1 = circ2b(jc);
04133 d2 = circ2b(jc+1);
04134
04135 t1 = c1 * d1;
04136 t3 = c1 * d2;
04137 t2 = c2 * d2;
04138 t4 = c2 * d1;
04139
04140 q(j) += t1 + t2;
04141 q(j+1) += - t3 + t4;
04142 t(j) += t1 - t2;
04143 t(j+1) += - t3 - t4;
04144 }
04145 }
04146
04147
04148 fftr_d(q,ip);
04149
04150
04151 fftr_d(t,ip);
04152
04153 EMData* out = new EMData();
04154 out->set_size(maxrin,2,1);
04155 float *dout = out->get_data();
04156 for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04157
04158
04159
04160 free(t);
04161 free(q);
04162 return out;
04163 }
04164
04165
04166 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04167
04168 int maxrin = numr[numr.size()-1];
04169
04170 vector<float> r(2*maxrin);
04171
04172 Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04173
04174 return r;
04175 }
04176
04177 #define dout(i,j) dout[i+maxrin*j]
04178 #define circ1b(i) circ1b[i-1]
04179 #define circ2b(i) circ2b[i-1]
04180
04181 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04182
04183
04184
04185 int ip, jc, numr3i, numr2i, i, j;
04186 float t1, t2, t3, t4, c1, c2, d1, d2;
04187
04188 int nring = numr.size()/3;
04189
04190 int maxrin = numr[numr.size()-1];
04191
04192 float* circ1b = circ1->get_data();
04193 float* circ2b = circ2->get_data();
04194
04195 #ifdef _WIN32
04196 ip = -(int)(log((float)maxrin)/log(2.0f));
04197 #else
04198 ip = -(int)(log2(maxrin));
04199 #endif //_WIN32
04200 for (int i=1; i<=maxrin; i++) {q(i) = 0.0f; t(i) = 0.0f;}
04201
04202
04203
04204
04205
04206 for (i=1; i<=nring; i++) {
04207
04208 numr3i = numr(3,i);
04209 numr2i = numr(2,i);
04210
04211 t1 = circ1b(numr2i) * circ2b(numr2i);
04212 q(1) += t1;
04213 t(1) += t1;
04214
04215 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04216 if (numr3i == maxrin) {
04217 q(2) += t1;
04218 t(2) += t1;
04219 } else {
04220 q(numr3i+1) += t1;
04221 t(numr3i+1) += t1;
04222 }
04223
04224 for (j=3; j<=numr3i; j=j+2) {
04225 jc = j+numr2i-1;
04226
04227 c1 = circ1b(jc);
04228 c2 = circ1b(jc+1);
04229 d1 = circ2b(jc);
04230 d2 = circ2b(jc+1);
04231
04232 t1 = c1 * d1;
04233 t3 = c1 * d2;
04234 t2 = c2 * d2;
04235 t4 = c2 * d1;
04236
04237 q(j) += t1 + t2;
04238 q(j+1) += -t3 + t4;
04239 t(j) += t1 - t2;
04240 t(j+1) += -t3 - t4;
04241 }
04242 }
04243
04244 fftr_q(q,ip);
04245
04246
04247
04248 fftr_q(t,ip);
04249 }
04250
04251
04252
04253 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04254 {
04255
04256 int ip, jc, numr3i, numr2i, i, j;
04257 float t1, t2, t3, t4, c1, c2, d1, d2;
04258
04259 int nring = numr.size()/3;
04260 int maxrin = numr[numr.size()-1];
04261
04262 float* circ1b = circ1->get_data();
04263 float* circ2b = circ2->get_data();
04264
04265 double *q;
04266
04267 q = (double*)calloc(maxrin,sizeof(double));
04268
04269 #ifdef _WIN32
04270 ip = -(int)(log((float)maxrin)/log(2.0f));
04271 #else
04272 ip = -(int)(log2(maxrin));
04273 #endif //_WIN32
04274
04275
04276
04277 for (i=1;i<=nring;i++) {
04278
04279 numr3i = numr(3,i);
04280 numr2i = numr(2,i);
04281
04282 t1 = circ1b(numr2i) * circ2b(numr2i);
04283 q(1) = q(1)+t1;
04284
04285 if (numr3i == maxrin) {
04286 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04287 q(2) = q(2)+t1;
04288 } else {
04289 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04290 q(numr3i+1) = q(numr3i+1)+t1;
04291 }
04292
04293 for (j=3;j<=numr3i;j=j+2) {
04294 jc = j+numr2i-1;
04295
04296 c1 = circ1b(jc);
04297 c2 = circ1b(jc+1);
04298 d1 = circ2b(jc);
04299 d2 = circ2b(jc+1);
04300
04301 t1 = c1 * d1;
04302 t3 = c1 * d2;
04303 t2 = c2 * d2;
04304 t4 = c2 * d1;
04305
04306 q(j) = q(j) + t1 + t2;
04307 q(j+1) = q(j+1) - t3 + t4;
04308 }
04309 }
04310
04311
04312 fftr_d(q,ip);
04313
04314 EMData* out = new EMData();
04315 out->set_size(maxrin,1,1);
04316 float *dout = out->get_data();
04317 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04318 free(q);
04319 return out;
04320
04321 }
04322
04323
04324 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04325 {
04326
04327 int ip, jc, numr3i, numr2i, i, j;
04328 float t1, t2, t3, t4, c1, c2, d1, d2;
04329
04330 int nring = numr.size()/3;
04331 int maxrin = numr[numr.size()-1];
04332
04333 float* circ1b = circ1->get_data();
04334 float* circ2b = circ2->get_data();
04335
04336 double *t;
04337
04338 t = (double*)calloc(maxrin,sizeof(double));
04339
04340 #ifdef _WIN32
04341 ip = -(int)(log((float)maxrin)/log(2.0f));
04342 #else
04343 ip = -(int)(log2(maxrin));
04344 #endif //_WIN32
04345
04346
04347
04348 for (i=1;i<=nring;i++) {
04349
04350 numr3i = numr(3,i);
04351 numr2i = numr(2,i);
04352
04353 t1 = circ1b(numr2i) * circ2b(numr2i);
04354 t(1) = t(1)+t1;
04355
04356 if (numr3i == maxrin) {
04357 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04358 t(2) = t(2)+t1;
04359 }
04360
04361 for (j=3;j<=numr3i;j=j+2) {
04362 jc = j+numr2i-1;
04363
04364 c1 = circ1b(jc);
04365 c2 = circ1b(jc+1);
04366 d1 = circ2b(jc);
04367 d2 = circ2b(jc+1);
04368
04369 t1 = c1 * d1;
04370 t3 = c1 * d2;
04371 t2 = c2 * d2;
04372 t4 = c2 * d1;
04373
04374 t(j) = t(j) + t1 - t2;
04375 t(j+1) = t(j+1) - t3 - t4;
04376 }
04377 }
04378
04379
04380 fftr_d(t,ip);
04381
04382 EMData* out = new EMData();
04383 out->set_size(maxrin,1,1);
04384 float *dout = out->get_data();
04385 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04386 free(t);
04387 return out;
04388
04389 }
04390
04391 #undef circ1b
04392 #undef circ2b
04393 #undef dout
04394
04395 #undef circ1
04396 #undef circ2
04397 #undef t
04398 #undef q
04399 #undef b
04400 #undef t7
04401
04402
04403 #define QUADPI 3.141592653589793238462643383279502884197
04404 #define PI2 2*QUADPI
04405
04406 float Util::ener(EMData* ave, vector<int> numr) {
04407 ENTERFUNC;
04408 long double ener,en;
04409
04410 int nring = numr.size()/3;
04411 float *aveptr = ave->get_data();
04412
04413 ener = 0.0;
04414 for (int i=1; i<=nring; i++) {
04415 int numr3i = numr(3,i);
04416 int np = numr(2,i)-1;
04417 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04418 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04419 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04420 ener += en/numr3i;
04421 }
04422 EXITFUNC;
04423 return static_cast<float>(ener);
04424 }
04425
04426 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04427 ENTERFUNC;
04428 long double ener, en;
04429 float arg, cs, si;
04430
04431 int nima = data.size();
04432 int nring = numr.size()/3;
04433 int maxrin = numr(3,nring);
04434
04435 ener = 0.0;
04436 for (int i=1; i<=nring; i++) {
04437 int numr3i = numr(3,i);
04438 int np = numr(2,i)-1;
04439 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04440 float temp1 = 0.0, temp2 = 0.0;
04441 for (int kk=0; kk<nima; kk++) {
04442 float *ptr = data[kk]->get_data();
04443 temp1 += ptr[np];
04444 temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04445 }
04446 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04447 for (int j=2; j<numr3i; j+=2) {
04448 float tempr = 0.0, tempi = 0.0;
04449 for (int kk=0; kk<nima; kk++) {
04450 float *ptr = data[kk]->get_data();
04451 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04452 cs = cos(arg);
04453 si = sin(arg);
04454 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04455 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04456 }
04457 en += tq*(tempr*tempr+tempi*tempi);
04458 }
04459 ener += en/numr3i;
04460 }
04461 EXITFUNC;
04462 return static_cast<float>(ener);
04463 }
04464
04465 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04466 int nring = numr.size()/3;
04467 float *ave = avep->get_data();
04468 float *dat = datp->get_data();
04469 int i, j, numr3i, np;
04470 float arg, cs, si;
04471 int maxrin = numr(3,nring);
04472 if(mirror == 1) {
04473 for (i=1; i<=nring; i++) {
04474 numr3i = numr(3,i);
04475 np = numr(2,i)-1;
04476 ave[np] += dat[np];
04477 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04478 for (j=2; j<numr3i; j=j+2) {
04479 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04480 cs = cos(arg);
04481 si = sin(arg);
04482
04483 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04484 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04485 }
04486 }
04487 } else {
04488 for (i=1; i<=nring; i++) {
04489 numr3i = numr(3,i);
04490 np = numr(2,i)-1;
04491 ave[np] += dat[np];
04492 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04493 for (j=2; j<numr3i; j=j+2) {
04494 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04495 cs = cos(arg);
04496 si = sin(arg);
04497
04498 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04499 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04500 }
04501 }
04502 }
04503 avep->update();
04504 EXITFUNC;
04505 }
04506
04507 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04508 int nring = numr.size()/3;
04509 float *ave = avep->get_data();
04510 float *dat = datp->get_data();
04511 int i, j, numr3i, np;
04512 float arg, cs, si;
04513 int maxrin = numr(3,nring);
04514 if(mirror == 1) {
04515 for (i=1; i<=nring; i++) {
04516 numr3i = numr(3,i);
04517 np = numr(2,i)-1;
04518 ave[np] -= dat[np];
04519 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04520 for (j=2; j<numr3i; j=j+2) {
04521 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04522 cs = cos(arg);
04523 si = sin(arg);
04524
04525 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04526 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04527 }
04528 }
04529 } else {
04530 for (i=1; i<=nring; i++) {
04531 numr3i = numr(3,i);
04532 np = numr(2,i)-1;
04533 ave[np] -= dat[np];
04534 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04535 for (j=2; j<numr3i; j=j+2) {
04536 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04537 cs = cos(arg);
04538 si = sin(arg);
04539
04540 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04541 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04542 }
04543 }
04544 }
04545 avep->update();
04546 EXITFUNC;
04547 }
04548
04549
04550 #undef QUADPI
04551 #undef PI2
04552
04553 #undef numr
04554 #undef circ
04555
04556
04557 #define QUADPI 3.141592653589793238462643383279502884197
04558 #define PI2 QUADPI*2
04559 #define deg_rad QUADPI/180.0
04560 #define rad_deg 180.0/QUADPI
04561
04562 struct ori_t
04563 {
04564 int iphi;
04565 int itht;
04566 int id;
04567 };
04568
04569
04570 struct cmpang
04571 {
04572 bool operator()( const ori_t& a, const ori_t& b )
04573 {
04574 if( a.itht != b.itht )
04575 {
04576 return a.itht < b.itht;
04577 }
04578
04579 return a.iphi < b.iphi;
04580 }
04581 };
04582
04583
04584 vector<double> Util::cml_weights(const vector<float>& cml){
04585 static const int NBIN = 100;
04586 int nline=cml.size()/2;
04587 vector<double> weights(nline);
04588
04589 vector<ori_t> angs(nline);
04590 for( int i=0; i < nline; ++i ) {
04591 angs[i].iphi = int( NBIN*cml[2*i] );
04592 angs[i].itht = int( NBIN*cml[2*i+1] );
04593 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04594 angs[i].id = i;
04595 }
04596
04597
04598
04599 std::sort( angs.begin(), angs.end(), cmpang() );
04600
04601 vector<float> newphi;
04602 vector<float> newtht;
04603 vector< vector<int> > indices;
04604
04605 int curt_iphi = -1;
04606 int curt_itht = -1;
04607 for(unsigned int i=0 ;i < angs.size(); ++i ) {
04608 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04609 Assert( indices.size() > 0 );
04610 indices.back().push_back(angs[i].id);
04611 } else {
04612 curt_iphi = angs[i].iphi;
04613 curt_itht = angs[i].itht;
04614
04615 newphi.push_back( float(curt_iphi)/NBIN );
04616 newtht.push_back( float(curt_itht)/NBIN );
04617 indices.push_back( vector<int>(1,angs[i].id) );
04618 }
04619 }
04620
04621
04622
04623
04624 int num_agl = newphi.size();
04625
04626 if(num_agl>2) {
04627 vector<double> w=Util::vrdg(newphi, newtht);
04628
04629 Assert( w.size()==newphi.size() );
04630 Assert( indices.size()==newphi.size() );
04631
04632 for(unsigned int i=0; i < newphi.size(); ++i ) {
04633
04634
04635
04636
04637
04638
04639
04640
04641 for(unsigned int j=0; j < indices[i].size(); ++j ) {
04642 int id = indices[i][j];
04643 weights[id] = w[i]/indices[i].size();
04644
04645 }
04646
04647
04648
04649 }
04650 } else {
04651 cout<<"warning in Util.cml_weights"<<endl;
04652 double val = PI2/float(nline);
04653 for(int i=0; i<nline; i++) weights[i]=val;
04654 }
04655
04656 return weights;
04657
04658 }
04659
04660
04661
04662
04663
04664 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04665 {
04666 int i;
04667 int nx=img->get_xsize();
04668 float *img_ptr = img->get_data();
04669 float *line_ptr = line->get_data();
04670 for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04671 img->update();
04672 }
04673
04674 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04675 int j;
04676 int nx = sino->get_xsize();
04677 int i = nx * pos_line;
04678 float r1, r2;
04679 float *line_ptr = line->get_data();
04680 float *sino_ptr = sino->get_data();
04681 for (j=ilf;j<=ihf; j += 2) {
04682 r1 = line_ptr[j];
04683 r2 = line_ptr[j + 1];
04684 sino_ptr[i + j - ilf] = r1;
04685 sino_ptr[i + j - ilf + 1] = r2;
04686 sino_ptr[i + nx * nblines + j - ilf] = r1;
04687 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04688 }
04689 sino->update();
04690 }
04691
04692 vector<double> Util::cml_init_rot(vector<float> Ori){
04693 int nb_ori = Ori.size() / 4;
04694 int i, ind;
04695 float ph, th, ps;
04696 double cph, cth, cps, sph, sth, sps;
04697 vector<double> Rot(nb_ori*9);
04698 for (i=0; i<nb_ori; ++i){
04699 ind = i*4;
04700
04701 ph = Ori[ind+2]-90;
04702 th = Ori[ind+1];
04703 ps = Ori[ind]+90;
04704 ph *= deg_rad;
04705 th *= deg_rad;
04706 ps *= deg_rad;
04707
04708 cph = cos(ph);
04709 cth = cos(th);
04710 cps = cos(ps);
04711 sph = sin(ph);
04712 sth = sin(th);
04713 sps = sin(ps);
04714
04715 ind = i*9;
04716 Rot[ind] = cph*cps-cth*sps*sph;
04717 Rot[ind+1] = cph*sps+cth*cps*sph;
04718 Rot[ind+2] = sth*sph;
04719 Rot[ind+3] = -sph*cps-cth*sps*cph;
04720 Rot[ind+4] = -sph*sps+cth*cps*cph;
04721 Rot[ind+5] = sth*cph;
04722 Rot[ind+6] = sth*sps;
04723 Rot[ind+7] = -sth*cps;
04724 Rot[ind+8] = cth;
04725 }
04726
04727 return Rot;
04728 }
04729
04730 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04731 float ph, ps;
04732 double cph, cth, cps, sph, sth, sps;
04733 int ind = iprj*9;
04734
04735 ph = nps-90;
04736 ps = nph+90;
04737 ph *= deg_rad;
04738 th *= deg_rad;
04739 ps *= deg_rad;
04740
04741 cph = cos(ph);
04742 cth = cos(th);
04743 cps = cos(ps);
04744 sph = sin(ph);
04745 sth = sin(th);
04746 sps = sin(ps);
04747
04748 Rot[ind] = (float)(cph*cps-cth*sps*sph);
04749 Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04750 Rot[ind+2] = (float)(sth*sph);
04751 Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04752 Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04753 Rot[ind+5] = (float)(sth*cph);
04754 Rot[ind+6] = (float)(sth*sps);
04755 Rot[ind+7] = (float)(-sth*cps);
04756 Rot[ind+8] = (float)(cth);
04757
04758 return Rot;
04759 }
04760
04761 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04762 vector<int> com(2*(n_prj - 1));
04763 int a = i_prj*9;
04764 int i, b, c;
04765 int n1=0, n2=0;
04766 float vmax = 1 - 1.0e-6f;
04767 double r11, r12, r13, r23, r31, r32, r33;
04768
04769 c = 0;
04770 for (i=0; i<n_prj; ++i){
04771 if (i!=i_prj){
04772 b = i*9;
04773
04774 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04775 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04776 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04777 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04778 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04779 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04780 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04781 if (r33 > vmax) {
04782 n2 = 270;
04783 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04784 }
04785 else if (r33 < -vmax) {
04786 n2 = 270;
04787 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04788 } else {
04789 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04790 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04791 if (n1 < 0) {n1 += 360;}
04792 if (n2 <= 0) {n2 = abs(n2);}
04793 else {n2 = 360 - n2;}
04794 }
04795
04796 if (n1 >= 360){n1 = n1 % 360;}
04797 if (n2 >= 360){n2 = n2 % 360;}
04798
04799
04800 b = c*2;
04801 com[b] = n1;
04802 com[b+1] = n2;
04803 ++c;
04804 }
04805 }
04806
04807 return com;
04808
04809 }
04810
04811 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int n_prj, int n_lines) {
04812 vector<int> com(2*n_lines);
04813 int a=0, b, c, l;
04814 int n1=0, n2=0, mem=-1;
04815 float vmax = 1 - 1.0e-6f;
04816 double r11, r12, r13, r23, r31, r32, r33;
04817 c = 0;
04818 for (l=0; l<n_lines; ++l){
04819 c = 2*l;
04820 if (seq[c]!=mem){
04821 mem = seq[c];
04822 a = seq[c]*9;
04823 }
04824 b = seq[c+1]*9;
04825
04826
04827 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04828 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04829 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04830 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04831 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04832 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04833 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04834 if (r33 > vmax) {
04835 n2 = 270;
04836 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04837 }
04838 else if (r33 < -vmax) {
04839 n2 = 270;
04840 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04841 } else {
04842 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04843 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04844 if (n1 < 0) {n1 += 360;}
04845 if (n2 <= 0) {n2 = abs(n2);}
04846 else {n2 = 360 - n2;}
04847 }
04848 if (n1 >= 360){n1 = n1 % 360;}
04849 if (n2 >= 360){n2 = n2 % 360;}
04850
04851
04852 com[c] = n1;
04853 com[c+1] = n2;
04854 }
04855
04856 return com;
04857
04858 }
04859
04860 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int nprj, int nlines){
04861
04862 vector<double> cml(2*nlines);
04863 float ph1, th1;
04864 float ph2, th2;
04865 double nx, ny, nz;
04866 double norm;
04867 double sth1=0, sph1=0, cth1=0, cph1=0;
04868 double sth2, sph2, cth2, cph2;
04869 int l, ind, c;
04870 int mem = -1;
04871 for (l=0; l<nlines; ++l){
04872 c = 2*l;
04873 if (seq[c]!=mem){
04874 mem = seq[c];
04875 ind = 4*seq[c];
04876 ph1 = Ori[ind]*deg_rad;
04877 th1 = Ori[ind+1]*deg_rad;
04878 sth1 = sin(th1);
04879 sph1 = sin(ph1);
04880 cth1 = cos(th1);
04881 cph1 = cos(ph1);
04882 }
04883 ind = 4*seq[c+1];
04884 ph2 = Ori[ind]*deg_rad;
04885 th2 = Ori[ind+1]*deg_rad;
04886 sth2 = sin(th2);
04887 cth2 = cos(th2);
04888 sph2 = sin(ph2);
04889 cph2 = cos(ph2);
04890
04891 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04892 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04893 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04894 norm = sqrt(nx*nx+ny*ny+nz*nz);
04895 nx /= norm;
04896 ny /= norm;
04897 nz /= norm;
04898
04899 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04900
04901 cml[c+1] = acos(nz);
04902 if (cml[c+1] == 0) {cml[c] = 0;}
04903 else {
04904 cml[c+1] *= rad_deg;
04905 if (cml[c+1] > 89.99) {cml[c+1] = 89.99;}
04906 cml[c] = rad_deg * atan2(nx, ny);
04907 cml[c] = fmod(360 + cml[c], 360);
04908
04909 }
04910 }
04911
04912 return cml;
04913 }
04914
04915 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04916 double res = 0;
04917 double buf = 0;
04918 float* line_1;
04919 float* line_2;
04920 int i, n, ind;
04921 int lnlen = data[0]->get_xsize();
04922 for (n=0; n<n_lines; ++n) {
04923 ind = n*2;
04924 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04925 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04926 buf = 0;
04927 for (i=0; i<lnlen; ++i) {
04928 buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04929 }
04930 res += buf * weights[n];
04931 }
04932
04933 return res;
04934
04935 }
04936
04937 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04938 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04939
04940
04941
04942 vector<double> res(2);
04943 int lnlen = data[0]->get_xsize();
04944 int end = 2*(n_prj-1);
04945 double disc, buf, bdisc, tmp;
04946 int n, i, ipsi, ind, bipsi, c;
04947 float* line_1;
04948 float* line_2;
04949 bdisc = 1.0e6;
04950 bipsi = -1;
04951
04952 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04953
04954 disc = 0;
04955 c = 0;
04956 for (n=0; n<n_prj; ++n) {
04957 if(n!=iprj) {
04958 ind = 2*c;
04959 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04960 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04961 buf = 0;
04962 for (i=0; i<lnlen; ++i) {
04963 tmp = line_1[i]-line_2[i];
04964 buf += tmp*tmp;
04965 }
04966 disc += buf * weights[iw[c]];
04967 ++c;
04968 }
04969 }
04970
04971 if (disc <= bdisc) {
04972 bdisc = disc;
04973 bipsi = ipsi;
04974 }
04975
04976 for (i=0; i<end; i+=2){
04977 com[i] += d_psi;
04978 if (com[i] >= n_psi) {com[i] = com[i] % n_psi;}
04979 }
04980 }
04981 res[0] = bdisc;
04982 res[1] = float(bipsi);
04983
04984 return res;
04985 }
04986
04987 #undef QUADPI
04988 #undef PI2
04989 #undef deg_rad
04990 #undef rad_deg
04991
04992
04993
04994
04995
04996
04997 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
04998 ENTERFUNC;
04999
05000 int nima = data.size();
05001 vector<float> res(nima);
05002 double result = 0.;
05003 double valmin = 1.0e20;
05004 int valpos = -1;
05005
05006 for (int kk=0; kk<nima; kk++){
05007 result = 0;
05008
05009 float *y_data = data[kk]->get_data();
05010 float *x_data = image->get_data();
05011 long totsize = image->get_xsize()*image->get_ysize();
05012 for (long i = 0; i < totsize; i++) {
05013 double temp = x_data[i]- y_data[i];
05014 result += temp*temp;
05015 }
05016 result /= totsize;
05017 res[kk] = (float)result;
05018
05019 if(result<valmin) {valmin = result; valpos = kk;}
05020
05021 }
05022
05023 Dict retvals;
05024 retvals["dist"] = res;
05025 retvals["pos"] = valpos;
05026
05027 EXITFUNC;
05028 return retvals;
05029
05030 }
05031
05032 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05033 ENTERFUNC;
05034
05035 int nima = data.size();
05036 vector<float> res(nima);
05037 double result = 0.;
05038 double valmin = 1.0e20;
05039 int valpos = -1;
05040
05041 for (int kk=0; kk<nima; kk++){
05042 result = 0;
05043
05044
05045 float *y_data = data[kk]->get_data();
05046 float *x_data = image->get_data();
05047
05048
05049 int nx = data[kk]->get_xsize();
05050 int ny = data[kk]->get_ysize();
05051 nx = (nx - 2 + data[kk]->is_fftodd());
05052 int lsd2 = (nx + 2 - nx%2) ;
05053
05054 int ixb = 2*((nx+1)%2);
05055 int iyb = ny%2;
05056 int iz = 0;
05057
05058 for ( int iy = 0; iy <= ny-1; iy++) {
05059 for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05060 int ii = ix + (iy + iz * ny)* lsd2;
05061 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05062 }
05063 }
05064 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05065 int ii = (iy + iz * ny)* lsd2;
05066 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05067 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05068 }
05069 if(nx%2 == 0) {
05070 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05071 int ii = lsd2 - 2 + (iy + iz * ny)* lsd2;
05072 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05073 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05074 }
05075
05076 }
05077 result *= 2;
05078 result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05079 if(ny%2 == 0) {
05080 int ii = (ny/2 + iz * ny)* lsd2;
05081 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05082 }
05083 if(nx%2 == 0) {
05084 int ii = lsd2 - 2 + (0 + iz * ny)* lsd2;
05085 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05086 if(ny%2 == 0) {
05087 int ii = lsd2 - 2 +(ny/2 + iz * ny)* lsd2;
05088 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05089 }
05090 }
05091
05092 result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05093 res[kk] = (float)result;
05094
05095 if(result<valmin) {valmin = result; valpos = kk;}
05096
05097 }
05098
05099 Dict retvals;
05100 retvals["dist"] = res;
05101 retvals["pos"] = valpos;
05102
05103 EXITFUNC;
05104 return retvals;
05105 }
05106
05107 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05108 long int d2 = group2[s2 - 1] - group2[0];
05109 long int p2 = 0;
05110 long int i1 = 0;
05111 long int i2 = 0;
05112 long int max = 0;
05113 long int cont = 0;
05114 long int i = 0;
05115 int stop1 = 0;
05116 int stop2 = 0;
05117
05118 for (i=0; i<s1; i++) {
05119 p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05120 if (p2 >= s2) {p2 = s2 - 1;}
05121 i1 = p2;
05122 i2 = p2;
05123 max = s2;
05124 if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05125
05126 stop1 = 0;
05127 stop2 = 0;
05128 while (max--) {
05129 if (group1[i] == group2[i1]) {
05130 if (flag) {stb[cont] = group1[i];}
05131 cont++;
05132 break;
05133 }
05134 if (group2[i1] < group1[i]) {stop1=1;}
05135 if (group1[i] == group2[i2]) {
05136 if (flag) {stb[cont] = group1[i];}
05137 cont++;
05138 break;
05139 }
05140 if (group2[i2] > group1[i]) {stop2=1;}
05141
05142
05143 if (stop1 & stop2) {break;}
05144 i1--;
05145 i2++;
05146 if (i1 < 0) {i1 = 0;}
05147 if (i2 >= s2) {i2 = s2 - 1;}
05148 }
05149
05150 }
05151
05152 return cont;
05153 }
05154
05155
05156
05157 #define old_ptr(i,j,k) old_ptr[i+(j+(k*ny))*nx]
05158 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*new_nx]
05159 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05160 {
05161
05162 if (!img) {
05163 throw NullPointerException("NULL input image");
05164 }
05165
05166
05167
05168 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05169
05170
05171
05172
05173 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)
05174 {
05175 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05176 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05177 }
05178
05179
05180
05181
05182 int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05183
05184
05185
05186
05187 int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05188 int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05189 int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05190 if(r1>1){r1=1;}
05191 if(r2>1){r2=1;}
05192 if(r3>1){r3=1;}
05193 int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05194
05195
05196
05197 EMData* img2 = new EMData();
05198 img2->set_size(new_nx,new_ny,new_nz);
05199 float *new_ptr = img2->get_data();
05200 float *old_ptr = img->get_data();
05201 int iptr, jptr, kptr = 0;
05202 for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05203 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05204 for (int i=new_st_x; i<nx; i+=x_step) {
05205 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05206 iptr++;}
05207 jptr++;}
05208 kptr++;}
05209 img2->update();
05210 return img2;
05211 }
05212 #undef old_ptr
05213 #undef new_ptr
05214
05215 #define inp(i,j,k) inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*nx]
05216 #define outp(i,j,k) outp[i+(j+(k*new_ny))*new_nx]
05217 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05218 {
05219
05220 if (!img) throw NullPointerException("NULL input image");
05221
05222
05223
05224 int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05225
05226
05227
05228 if(new_nx>nx || new_ny>ny || new_nz>nz)
05229 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size.");
05230 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)
05231 throw ImageDimensionException("The offset inconsistent with the input image size.");
05232 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))))
05233 throw ImageDimensionException("The offset inconsistent with the input image size.");
05234
05235
05236
05237 int new_st_x = nx/2-new_nx/2 + x_offset,
05238 new_st_y = ny/2-new_ny/2 + y_offset,
05239 new_st_z = nz/2-new_nz/2 + z_offset;
05240
05241
05242
05243 if (new_st_x<0 || new_st_y<0 || new_st_z<0)
05244 throw ImageDimensionException("The offset inconsistent with the input image size.");
05245
05246
05247 EMData* wind = img->copy_head();
05248 wind->set_size(new_nx, new_ny, new_nz);
05249 float *outp=wind->get_data();
05250 float *inp=img->get_data();
05251
05252 for (int k=0; k<new_nz; k++)
05253 for(int j=0; j<new_ny; j++)
05254 for(int i=0; i<new_nx; i++)
05255 outp(i,j,k) = inp(i,j,k);
05256 wind->update();
05257 return wind;
05258 }
05259 #undef inp
05260 #undef outp
05261
05262 #define inp(i,j,k) inp[i+(j+(k*ny))*nx]
05263 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*new_nx]
05264 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)
05265 {
05266
05267 if (!img) throw NullPointerException("NULL input image");
05268
05269
05270
05271 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05272
05273
05274
05275 if(new_nx<nx || new_ny<ny || new_nz<nz)
05276 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05277 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)
05278 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05279 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))))
05280 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05281
05282
05283 EMData* pading = img->copy_head();
05284 pading->set_size(new_nx, new_ny, new_nz);
05285 float *inp = img->get_data();
05286 float *outp = pading->get_data();
05287
05288
05289
05290
05291 float background;
05292
05293 if (strcmp(params,"average")==0) background = img->get_attr("mean");
05294 else if (strcmp(params,"circumference")==0) {
05295 float sum1=0.0f;
05296 int cnt=0;
05297 for(int i=0;i<nx;i++) {
05298 sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05299 cnt+=2;
05300 }
05301 if(nz-1 == 0) {
05302 for (int j=1;j<ny-1;j++) {
05303 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05304 cnt+=2;
05305 }
05306 } else {
05307 for (int k=1;k<nz-1;k++) {
05308 for (int j=1;j<ny-1;j++) {
05309 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05310 cnt+=2;
05311 }
05312 }
05313 }
05314 background = sum1/cnt;
05315 } else {
05316 background = static_cast<float>( atof( params ) );
05317 }
05318
05319
05320
05321 int new_st_x=0,new_st_y=0,new_st_z=0;
05322 for (int k=0;k<new_nz;k++)
05323 for(int j=0;j<new_ny;j++)
05324 for (int i=0;i<new_nx;i++)
05325 outp(i,j,k)=background;
05326
05327
05328
05329 new_st_x=int((new_nx/2-nx/2) + x_offset);
05330 new_st_y=int((new_ny/2-ny/2) + y_offset);
05331 new_st_z=int((new_nz/2-nz/2) + z_offset);
05332
05333
05334 for (int k=0;k<nz;k++)
05335 for(int j=0;j<ny;j++)
05336 for(int i=0;i<nx;i++)
05337 outp(i,j,k)=inp(i,j,k);
05338 pading->update();
05339 return pading;
05340 }
05341 #undef inp
05342 #undef outp
05343
05344
05345 void Util::colreverse(float* beg, float* end, int nx) {
05346 float* tmp = new float[nx];
05347 int n = (end - beg)/nx;
05348 int nhalf = n/2;
05349 for (int i = 0; i < nhalf; i++) {
05350
05351 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05352 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05353 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05354 }
05355 delete[] tmp;
05356 }
05357
05358 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05359 {
05360 int nxy = nx*ny;
05361 colreverse(beg, end, nxy);
05362 }
05363
05364
05365 void Util::cyclicshift(EMData *image, Dict params) {
05366
05367 if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05368
05369 int dx = params["dx"];
05370 int dy = params["dy"];
05371 int dz = params["dz"];
05372
05373
05374 int nx = image->get_xsize();
05375 dx %= nx;
05376 if (dx < 0) dx += nx;
05377 int ny = image->get_ysize();
05378 dy %= ny;
05379 if (dy < 0) dy += ny;
05380 int nz = image->get_zsize();
05381 dz %= nz;
05382 if (dz < 0) dz += nz;
05383
05384 int mx = -(dx - nx);
05385 int my = -(dy - ny);
05386 int mz = -(dz - nz);
05387
05388 float* data = image->get_data();
05389
05390 if (mx != 0) {
05391 for (int iz = 0; iz < nz; iz++)
05392 for (int iy = 0; iy < ny; iy++) {
05393
05394 int offset = nx*iy + nx*ny*iz;
05395 reverse(&data[offset],&data[offset+mx]);
05396 reverse(&data[offset+mx],&data[offset+nx]);
05397 reverse(&data[offset],&data[offset+nx]);
05398 }
05399 }
05400
05401 if (my != 0) {
05402 for (int iz = 0; iz < nz; iz++) {
05403 int offset = nx*ny*iz;
05404 colreverse(&data[offset], &data[offset + my*nx], nx);
05405 colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05406 colreverse(&data[offset], &data[offset + ny*nx], nx);
05407 }
05408 }
05409 if (mz != 0) {
05410 slicereverse(&data[0], &data[mz*ny*nx], nx, ny);
05411 slicereverse(&data[mz*ny*nx], &data[nz*ny*nx], nx, ny);
05412 slicereverse(&data[0], &data[nz*ny*nx], nx ,ny);
05413 }
05414 image->update();
05415 }
05416
05417
05418
05419
05420 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05421 {
05422 if (image->is_complex())
05423 throw ImageFormatException("Cannot do histogram on Fourier image");
05424
05425 float *imageptr=0, *maskptr=0;
05426 int nx=image->get_xsize();
05427 int ny=image->get_ysize();
05428 int nz=image->get_zsize();
05429
05430 if(mask != NULL){
05431 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05432 throw ImageDimensionException("The size of mask image should be of same size as the input image");
05433 maskptr =mask->get_data();
05434 }
05435 if( nbins == 0) nbins = nx;
05436 vector <float> freq(2*nbins, 0.0);
05437
05438 imageptr=image->get_data();
05439 if( hmin == hmax ) {
05440 if(mask == NULL) {
05441 hmax = image->get_attr("maximum");
05442 hmin = image->get_attr("minimum");
05443 } else {
05444 bool First = true;
05445 for (int i = 0;i < nx*ny*nz; i++) {
05446 if (maskptr[i]>=0.5f) {
05447 if(First) {
05448 hmax = imageptr[i];
05449 hmin = imageptr[i];
05450 First = false;
05451 } else {
05452 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05453 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05454 }
05455 }
05456 }
05457 }
05458 }
05459 float hdiff = hmax - hmin;
05460 float ff = (nbins-1)/hdiff;
05461 for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05462 if(mask == NULL) {
05463 for(int i = 0; i < nx*ny*nz; i++) {
05464 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05465 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05466 }
05467 } else {
05468 for(int i = 0; i < nx*ny*nz; i++) {
05469 if(maskptr[i] >= 0.5) {
05470 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05471 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05472 }
05473 }
05474 }
05475 return freq;
05476 }
05477
05478 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05479 {
05480
05481 if (img->is_complex() || ref->is_complex())
05482 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05483
05484 if(mask != NULL){
05485 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05486 throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05487
05488
05489
05490 int size_ref = ((ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05491 int size_img = ((img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05492
05493
05494
05495 float *ref_ptr = ref->get_data();
05496 float ref_h_min = ref->get_attr("minimum");
05497 float ref_h_max = ref->get_attr("maximum");
05498 float ref_h_avg = ref->get_attr("mean");
05499 float ref_h_sig = ref->get_attr("sigma");
05500
05501
05502
05503 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05504
05505 vector<float> img_data = Util::infomask(img, mask);
05506 float img_avg = img_data[0];
05507 float img_sig = img_data[1];
05508
05509
05510 int cnt=0;
05511 for(int i=0;i<size_img;i++)
05512 if (mask_ptr[i]>0.5f)
05513 cnt++;
05514
05515
05516
05517 float ref_h_diff = ref_h_max - ref_h_min;
05518
05519 #ifdef _WIN32
05520 int hist_len = _cpp_min((int)size_ref/16,_cpp_min((int)size_img/16,256));
05521 #else
05522 int hist_len = std::min((int)size_ref/16,std::min((int)size_img/16,256));
05523 #endif //_WIN32
05524
05525 float *ref_freq_bin = new float[3*hist_len];
05526
05527
05528 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05529
05530 for (int i = 0;i < size_ref;i++) {
05531 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05532 ref_freq_bin[L]++;
05533 }
05534 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05535
05536
05537 float A = ref_h_sig/img_sig;
05538 float B = ref_h_avg - (A*img_avg);
05539
05540 vector<float> args;
05541 args.push_back(A);
05542 args.push_back(B);
05543
05544 vector<float> scale;
05545 scale.push_back(1.e-7f*A);
05546 scale.push_back(-1.e-7f*B);
05547
05548 vector<float> ref_freq_hist;
05549 for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05550
05551 vector<float> data;
05552 data.push_back(ref_h_diff);
05553 data.push_back(ref_h_min);
05554
05555 Dict parameter;
05556
05557
05558 parameter["args"] = args;
05559 parameter["scale"]= scale;
05560 parameter["data"] = data;
05561 parameter["ref_freq_bin"] = ref_freq_hist;
05562 parameter["size_img"]=size_img;
05563 parameter["hist_len"]=hist_len;
05564
05565
05566 return parameter;
05567 }
05568
05569
05570 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)
05571 {
05572 float *img_ptr = img->get_data();
05573 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05574
05575 int *img_freq_bin = new int[3*hist_len];
05576 for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05577 for(int i = 0;i < size_img;i++) {
05578 if(mask_ptr[i] > 0.5f) {
05579 float img_xn = img_ptr[i]*PA + PB;
05580 int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05581 if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05582 }
05583 }
05584 int freq_hist = 0;
05585
05586 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);
05587 freq_hist = (-freq_hist);
05588 return static_cast<float>(freq_hist);
05589 }
05590
05591 #define QUADPI 3.141592653589793238462643383279502884197
05592 #define DGR_TO_RAD QUADPI/180
05593 #define DM(I) DM [I-1]
05594 #define SS(I) SS [I-1]
05595 Dict Util::CANG(float PHI,float THETA,float PSI)
05596 {
05597 double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05598 vector<float> DM,SS;
05599
05600 for(int i =0;i<9;i++) DM.push_back(0);
05601
05602 for(int i =0;i<6;i++) SS.push_back(0);
05603
05604 CPHI = cos(double(PHI)*DGR_TO_RAD);
05605 SPHI = sin(double(PHI)*DGR_TO_RAD);
05606 CTHE = cos(double(THETA)*DGR_TO_RAD);
05607 STHE = sin(double(THETA)*DGR_TO_RAD);
05608 CPSI = cos(double(PSI)*DGR_TO_RAD);
05609 SPSI = sin(double(PSI)*DGR_TO_RAD);
05610
05611 SS(1) = float(CPHI);
05612 SS(2) = float(SPHI);
05613 SS(3) = float(CTHE);
05614 SS(4) = float(STHE);
05615 SS(5) = float(CPSI);
05616 SS(6) = float(SPSI);
05617
05618 DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05619 DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05620 DM(3) = float(-STHE*CPSI);
05621 DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05622 DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05623 DM(6) = float(STHE*SPSI);
05624 DM(7) = float(STHE*CPHI);
05625 DM(8) = float(STHE*SPHI);
05626 DM(9) = float(CTHE);
05627
05628 Dict DMnSS;
05629 DMnSS["DM"] = DM;
05630 DMnSS["SS"] = SS;
05631
05632 return(DMnSS);
05633 }
05634 #undef SS
05635 #undef DM
05636 #undef QUADPI
05637 #undef DGR_TO_RAD
05638
05639 #define DM(I) DM[I-1]
05640 #define B(i,j) Bptr[i-1+((j-1)*NSAM)]
05641 #define CUBE(i,j,k) CUBEptr[(i-1)+((j-1)+((k-1)*NY3D))*NX3D]
05642
05643 void Util::BPCQ(EMData *B,EMData *CUBE, vector<float> DM)
05644 {
05645
05646 float *Bptr = B->get_data();
05647 float *CUBEptr = CUBE->get_data();
05648
05649 int NSAM,NROW,NX3D,NY3D,NZC,KZ,IQX,IQY,LDPX,LDPY,LDPZ,LDPNMX,LDPNMY,NZ1;
05650 float DIPX,DIPY,XB,YB,XBB,YBB;
05651
05652 Transform * t = B->get_attr("xform.projection");
05653 Dict d = t->get_params("spider");
05654 if(t) {delete t; t=0;}
05655
05656 float x_shift = d[ "tx" ];
05657 float y_shift = d[ "ty" ];
05658 x_shift = -x_shift;
05659 y_shift = -y_shift;
05660
05661 NSAM = B->get_xsize();
05662 NROW = B->get_ysize();
05663 NX3D = CUBE->get_xsize();
05664 NY3D = CUBE->get_ysize();
05665 NZC = CUBE->get_zsize();
05666
05667
05668 LDPX = NX3D/2 +1;
05669 LDPY = NY3D/2 +1;
05670 LDPZ = NZC/2 +1;
05671 LDPNMX = NSAM/2 +1;
05672 LDPNMY = NROW/2 +1;
05673 NZ1 = 1;
05674
05675 for(int K=1;K<=NZC;K++) {
05676 KZ=K-1+NZ1;
05677 for(int J=1;J<=NY3D;J++) {
05678 XBB = (1-LDPX)*DM(1)+(J-LDPY)*DM(2)+(KZ-LDPZ)*DM(3);
05679 YBB = (1-LDPX)*DM(4)+(J-LDPY)*DM(5)+(KZ-LDPZ)*DM(6);
05680 for(int I=1;I<=NX3D;I++) {
05681 XB = (I-1)*DM(1)+XBB-x_shift;
05682 IQX = int(XB+float(LDPNMX));
05683 if (IQX <1 || IQX >= NSAM) continue;
05684 YB = (I-1)*DM(4)+YBB-y_shift;
05685 IQY = int(YB+float(LDPNMY));
05686 if (IQY<1 || IQY>=NROW) continue;
05687 DIPX = XB+LDPNMX-IQX;
05688 DIPY = YB+LDPNMY-IQY;
05689
05690 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)));
05691 }
05692 }
05693 }
05694 }
05695
05696 #undef DM
05697 #undef B
05698 #undef CUBE
05699
05700
05701 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
05702 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
05703 #define SS(I,J) SS [I-1 + (J-1)*6]
05704
05705 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05706 {
05707 int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05708 float WW,OX,OY;
05709
05710 NSAM = PROJ->get_xsize();
05711 NROW = PROJ->get_ysize();
05712 int ntotal = NSAM*NROW;
05713 float q = 2.0f;
05714 float qt = 8.0f/q;
05715
05716 int ipad = 1;
05717 NSAM *= ipad;
05718 NROW *= ipad;
05719 NNNN = NSAM+2-(NSAM%2);
05720 int NX2 = NSAM/2;
05721 NR2 = NROW/2;
05722
05723 NANG = int(SS.size())/6;
05724
05725 EMData* W = new EMData();
05726 int Wnx = NNNN/2;
05727 W->set_size(Wnx,NROW,1);
05728 W->to_zero();
05729 float *Wptr = W->get_data();
05730 float *PROJptr = PROJ->get_data();
05731 for (L=1; L<=NANG; L++) {
05732 float tmp1 = SS(3,K)*SS(4,L)*(SS(1,K)*SS(1,L) + SS(2,K)*SS(2,L)) - SS(3,L)*SS(4,K);
05733 float tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) );
05734 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05735 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05736 if(OX < 0.0f) {
05737 OX = -OX;
05738 OY = -OY;
05739 }
05740
05741 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f ) {
05742 for(int J=1;J<=NROW;J++) {
05743 JY = (J-1);
05744 if(JY > NR2) JY -= NROW;
05745 #ifdef _WIN32
05746 int xma = _cpp_min(int(0.5f+(q-JY*OY)/OX),NX2);
05747 int xmi = _cpp_max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05748 #else
05749 int xma = std::min(int(0.5f+(q-JY*OY)/OX),NX2);
05750 int xmi = std::max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05751 #endif //_WIN32
05752 if( xmi <= xma) {
05753 for(int I=xmi;I<=xma;I++) {
05754 float Y = fabs(OX*I + OY*JY);
05755 W(I+1,J) += exp(-qt*Y*Y);
05756
05757 }
05758 }
05759 }
05760 } else {
05761 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J) += 1.0f;
05762 }
05763 }
05764 EMData* proj_in = PROJ;
05765
05766 PROJ = PROJ->norm_pad( false, ipad);
05767 PROJ->do_fft_inplace();
05768 PROJ->update();
05769
05770 PROJptr = PROJ->get_data();
05771
05772 float WNRMinv,temp;
05773 float osnr = 1.0f/SNR;
05774 WNRMinv = 1.0f/W(1,1);
05775 for(int J=1;J<=NROW;J++) {
05776 JY = J-1;
05777 if( JY > NR2) JY -= NROW;
05778 float sy = JY;
05779 sy /= NROW;
05780 sy *= sy;
05781 for(int I=1;I<=NNNN;I+=2) {
05782 KX = (I+1)/2;
05783 temp = W(KX,J)*WNRMinv;
05784 WW = temp/(temp*temp + osnr);
05785
05786 float sx = KX-1;
05787 sx /= NSAM;
05788 WW *= exp(qt*(sy + sx*sx));
05789 PROJ(I,J) *= WW;
05790 PROJ(I+1,J) *= WW;
05791 }
05792 }
05793 delete W; W = 0;
05794 PROJ->do_ift_inplace();
05795 PROJ->depad();
05796
05797 float* data_src = PROJ->get_data();
05798 float* data_dst = proj_in->get_data();
05799
05800 for( int i=0; i < ntotal; ++i ) data_dst[i] = data_src[i];
05801
05802 proj_in->update();
05803
05804 delete PROJ;
05805 }
05806
05807
05808
05809
05810
05811
05812
05813
05814
05815
05816
05817
05818
05819
05820
05821
05822
05823
05824
05825
05826
05827
05828
05829
05830
05831
05832
05833
05834
05835
05836
05837
05838
05839
05840
05841
05842
05843
05844
05845
05846
05847
05848
05849
05850
05851
05852
05853
05854
05855
05856
05857
05858
05859
05860
05861
05862
05863
05864
05865
05866
05867
05868
05869
05870
05871
05872
05873
05874
05875
05876
05877
05878
05879
05880
05881
05882
05883
05884
05885 #undef PROJ
05886 #undef W
05887 #undef SS
05888
05889 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
05890 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
05891 #define SS(I,J) SS [I-1 + (J-1)*6]
05892 #define RI(i,j) RI [(i-1) + ((j-1)*3)]
05893 #define CC(i) CC [i-1]
05894 #define CP(i) CP [i-1]
05895 #define VP(i) VP [i-1]
05896 #define VV(i) VV [i-1]
05897 #define AMAX1(i,j) i>j?i:j
05898 #define AMIN1(i,j) i<j?i:j
05899 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
05900 {
05901 float rad2deg =(180.0f/3.1415926f);
05902 float deg2rad = (3.1415926f/180.0f);
05903
05904 int NSAM,NROW,NNNN,NR2,NANG,L,JY;
05905
05906 NSAM = PROJ->get_xsize();
05907 NROW = PROJ->get_ysize();
05908 NNNN = NSAM+2-(NSAM%2);
05909 NR2 = NROW/2;
05910 NANG = int(SS.size())/6;
05911
05912 float RI[9];
05913 RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
05914 RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
05915 RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
05916 RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
05917 RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
05918 RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
05919 RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
05920 RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
05921 RI(3,3)=SS(3,NUMP);
05922
05923 float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
05924
05925 EMData* W = new EMData();
05926 int Wnx = NNNN/2;
05927 W->set_size(NNNN/2,NROW,1);
05928 W->to_one();
05929 float *Wptr = W->get_data();
05930
05931 float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
05932
05933 for (L=1; L<=NANG; L++) {
05934 if (L != NUMP) {
05935 CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
05936 CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
05937 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);
05938
05939 TMP = sqrt(CC(1)*CC(1) + CC(2)*CC(2) + CC(3)*CC(3));
05940 CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
05941 ALPHA=rad2deg*float(asin(CCN));
05942 if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
05943 if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
05944 if(ALPHA<1.0E-6) {
05945 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
05946 } else {
05947 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
05948 CC(1) = CC(1)/CCN;CC(2) = CC(2)/CCN;CC(3) = CC(3)/CCN;
05949 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
05950 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
05951 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
05952 CP(1) = 0.0;CP(2) = 0.0;
05953 VP(1) = 0.0;VP(2) = 0.0;
05954
05955 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
05956 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
05957 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
05958 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
05959
05960 TMP = CP(1)*VP(2)-CP(2)*VP(1);
05961
05962
05963 TMP = AMAX1(1.0E-4f,fabs(TMP));
05964 float tmpinv = 1.0f/TMP;
05965 for(int J=1;J<=NROW;J++) {
05966 JY = (J-1);
05967 if (JY>NR2) JY=JY-NROW;
05968 for(int I=1;I<=NNNN/2;I++) {
05969 FV = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
05970 RT = 1.0f-FV/FM;
05971 W(I,J) += ((RT>0.0f)*RT);
05972 }
05973 }
05974 }
05975 }
05976 }
05977
05978 EMData* proj_in = PROJ;
05979
05980 PROJ = PROJ->norm_pad( false, 1);
05981 PROJ->do_fft_inplace();
05982 PROJ->update();
05983 float *PROJptr = PROJ->get_data();
05984
05985 int KX;
05986 float WW;
05987 for(int J=1; J<=NROW; J++)
05988 for(int I=1; I<=NNNN; I+=2) {
05989 KX = (I+1)/2;
05990 WW = 1.0f/W(KX,J);
05991 PROJ(I,J) = PROJ(I,J)*WW;
05992 PROJ(I+1,J) = PROJ(I+1,J)*WW;
05993 }
05994 delete W; W = 0;
05995 PROJ->do_ift_inplace();
05996 PROJ->depad();
05997
05998 float* data_src = PROJ->get_data();
05999 float* data_dst = proj_in->get_data();
06000
06001 int ntotal = NSAM*NROW;
06002 for( int i=0; i < ntotal; ++i ) data_dst[i] = data_src[i];
06003
06004 proj_in->update();
06005 delete PROJ;
06006 }
06007 #undef AMAX1
06008 #undef AMIN1
06009 #undef RI
06010 #undef CC
06011 #undef CP
06012 #undef VV
06013 #undef VP
06014
06015
06016 #undef W
06017 #undef SS
06018 #undef PROJ
06019
06020 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06021 {
06022 float cst = cs*1.0e7f;
06023
06024 wgh /= 100.0;
06025 float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06026 float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06027 float ak2 = ak*ak;
06028 float g1 = dzz*1.0e4f*lambda*ak2;
06029 float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06030
06031 float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06032 if(b_factor != 0.0f) ctfv *= exp(-b_factor*ak2/4.0f);
06033
06034 return ctfv;
06035 }
06036
06037 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06038 {
06039
06040
06041
06042 int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();
06043
06044
06045
06046 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06047 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06048
06049 int i, size = nx*ny*nz;
06050
06051 float* img_ptr = image->get_data();
06052 float* mask_ptr = mask->get_data();
06053
06054 int ln=0;
06055 for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06056
06057 EMData* new_image = new EMData();
06058 new_image->set_size(ln,1,1);
06059 float *new_ptr = new_image->get_data();
06060
06061 ln=-1;
06062 for(i = 0;i < size;i++){
06063 if(mask_ptr[i] > 0.5f) {
06064 ln++;
06065 new_ptr[ln]=img_ptr[i];
06066 }
06067 }
06068
06069 return new_image;
06070 }
06071
06072 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06073 {
06074
06075
06076
06077 if(mask == NULL)
06078 throw ImageDimensionException("The mask cannot be an null image");
06079
06080
06081
06082
06083 int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06084
06085 int i,size = nx*ny*nz;
06086
06087 EMData *new_image = new EMData();
06088 new_image->set_size(nx,ny,nz);
06089 float *new_ptr = new_image->get_data();
06090 float *mask_ptr = mask->get_data();
06091 float *img_ptr = image->get_data();
06092 int count = 0;
06093 float sum_under_mask = 0.0 ;
06094 for(i = 0;i < size;i++){
06095 if(mask_ptr[i] > 0.5f){
06096 new_ptr[i] = img_ptr[count];
06097 sum_under_mask += img_ptr[count];
06098 count++;
06099 if( count > image->get_xsize() ) {
06100 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06101 }
06102 }
06103 }
06104
06105 if( count > image->get_xsize() ) {
06106 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06107 }
06108
06109 float avg_under_mask = sum_under_mask / count;
06110 for(i = 0;i < size;i++) {
06111 if(mask_ptr[i] <= 0.5f) new_ptr[i] = avg_under_mask;
06112 }
06113 new_image->update();
06114 return new_image;
06115 }
06116
06117
06118
06119 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06120 {
06121 vector<float>new_peak;
06122 int n1=peak1.size()/3;
06123 float p_size2=p_size*p_size;
06124 for (int i=0;i<n1;++i) {
06125 vector<float>::iterator it2= peak1.begin()+3*i;
06126 bool push_back1=true;
06127 int n2=peak2.size()/3;
06128
06129
06130 if(n2 ==0) {
06131 new_peak.push_back(*it2);
06132 new_peak.push_back(*(it2+1));
06133 new_peak.push_back(*(it2+2));
06134 } else {
06135 int j=0;
06136 while (j< n2-1 ) {
06137 vector<float>::iterator it3= peak2.begin()+3*j;
06138 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06139 if(d2< p_size2 ) {
06140 if( (*it2)<(*it3) ) {
06141 new_peak.push_back(*it3);
06142 new_peak.push_back(*(it3+1));
06143 new_peak.push_back(*(it3+2));
06144 peak2.erase(it3);
06145 peak2.erase(it3);
06146 peak2.erase(it3);
06147 push_back1=false;
06148 } else {
06149 peak2.erase(it3);
06150 peak2.erase(it3);
06151 peak2.erase(it3);
06152 }
06153 } else j=j+1;
06154 n2=peak2.size()/3;
06155 }
06156 if(push_back1) {
06157 new_peak.push_back(*it2);
06158 new_peak.push_back(*(it2+1));
06159 new_peak.push_back(*(it2+2));
06160 }
06161 }
06162 }
06163 return new_peak;
06164 }
06165
06166 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06167 {
06168
06169
06170
06171
06172
06173 ENTERFUNC;
06174
06175 int i;
06176
06177
06178 for ( i = 0 ; i < n * n ; i++ ) eigvec[i] = covmat[i];
06179
06180 char NEEDV = 'V';
06181 char UPLO = 'U';
06182 int lwork = -1;
06183 int info = 0;
06184 float *work, wsize;
06185
06186
06187 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06188 lwork = (int)wsize;
06189
06190 work = (float *)calloc(lwork, sizeof(float));
06191
06192 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06193 free(work);
06194 EXITFUNC;
06195 return info;
06196 }
06197
06198 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06199 {
06200
06201 ENTERFUNC;
06202 int len = covmatpy.size();
06203 float *eigvec;
06204 float *eigval;
06205 float *covmat;
06206 int status = 0;
06207 eigval = (float*)calloc(ncov,sizeof(float));
06208 eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06209 covmat = (float*)calloc(ncov*ncov, sizeof(float));
06210
06211 const float *covmat_ptr;
06212 covmat_ptr = &covmatpy[0];
06213 for(int i=0;i<len;i++){
06214 covmat[i] = covmat_ptr[i];
06215 }
06216
06217 status = Util::coveig(ncov, covmat, eigval, eigvec);
06218
06219 vector<float> eigval_py(ncov);
06220 const float *eigval_ptr;
06221 eigval_ptr = &eigval[0];
06222 for(int i=0;i<ncov;i++){
06223 eigval_py[i] = eigval_ptr[i];
06224 }
06225
06226 vector<float> eigvec_py(ncov*ncov);
06227 const float *eigvec_ptr;
06228 eigvec_ptr = &eigvec[0];
06229 for(int i=0;i<ncov*ncov;i++){
06230 eigvec_py[i] = eigvec_ptr[i];
06231 }
06232
06233 Dict res;
06234 res["eigval"] = eigval_py;
06235 res["eigvec"] = eigvec_py;
06236
06237 EXITFUNC;
06238 return res;
06239 }
06240
06241 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06242 {
06243 int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06244
06245 k=(int)pw.size();
06246 l=0;
06247 m=k;
06248 n2=n+2;
06249 n1=n+1;
06250 klmd=k+l+m;
06251 klm2d= k+l+m+2;
06252 nklmd=k+l+m+n;
06253 n2d=n+2;
06254
06255 n_larg=klmd*2;
06256 klm2d=n_larg+klm2d;
06257 klmd=n_larg+klmd;
06258 nklmd=n_larg+nklmd;
06259 int size_q=klm2d*n2d;
06260 int size_cu=nklmd*2;
06261 static int i__;
06262
06263 double *q ;
06264 double *x ;
06265 double *res;
06266 double *cu;
06267 float *q2;
06268 float *pw_;
06269 long int *iu;
06270 double *s;
06271 q = (double*)calloc(size_q,sizeof(double));
06272 x = (double*)calloc(n2d,sizeof(double));
06273 res = (double*)calloc(klmd,sizeof(double));
06274 cu =(double*)calloc(size_cu,sizeof(double));
06275 s = (double*)calloc(klmd,sizeof(double));
06276 q2 = (float*)calloc(size_q,sizeof(float));
06277 iu = (long int*)calloc(size_cu,sizeof(long int));
06278 pw_ = (float*)calloc(k,sizeof(float));
06279
06280 for( i__ =0;i__<k;++i__)
06281 {
06282 pw_[i__]=log(pw[i__]); }
06283 long int l_k=k;
06284 long int l_n=n;
06285 long int l_iswi=iswi;
06286 vector<float> cl1_res;
06287 cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06288 free(q);
06289 free(x);
06290 free(res);
06291 free(s);
06292 free(cu);
06293 free(q2);
06294 free(iu);
06295 free(pw_);
06296 return cl1_res;
06297 }
06298 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)
06299 {
06300 long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06301 float r__1;
06302 int tmp__i;
06303 long int i__, j;
06304 --s;
06305 --res;
06306 iu -= 3;
06307 cu -= 3;
06308 --x;
06309 long int klm2d;
06310 klm2d= *k+*k+2;
06311 klm2d=klm2d+klm2d;
06312 q_dim1 = klm2d;
06313 q_offset = 1 + q_dim1;
06314 q -= q_offset;
06315 q2_dim1 = klm2d;
06316 q2_offset = 1 + q2_dim1;
06317 q2 -= q2_offset;
06318 i__2=0;
06319 i__1 = *n - 1;
06320 tmp__i=0;
06321 for (j = 1; j <= i__1; ++j) {
06322 i__2 = *k;
06323 tmp__i+=1;
06324 for (i__ = 1; i__ <= i__2; ++i__) {
06325 r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06326 q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06327 }
06328 }
06329 for (i__ = 1; i__ <= i__2; ++i__)
06330 { q2[i__ + *n * q2_dim1] = 1.f;
06331 q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06332 }
06333 vector<float> fit_res;
06334 fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06335 return fit_res;
06336 }
06337 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)
06338 {
06339
06340 long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06341
06342
06343 long int i__, j, m, n1, ii, jj;
06344 double tmp;
06345 vector<float> p;
06346 --x;
06347 q_dim1 = *klm2d;
06348 q_offset = 1 + q_dim1;
06349 q -= q_offset;
06350 q1_dim1 = *klm2d;
06351 q1_offset = 1 + q1_dim1;
06352 q1 -= q1_offset;
06353 --s;
06354 --res;
06355 iu -= 3;
06356 cu -= 3;
06357
06358
06359 long int l = 0;
06360
06361
06362 m = *ks;
06363 n1 = *n + 1;
06364 if (*iswi == 1) {
06365 i__1 = n1;
06366 for (jj = 1; jj <= i__1; ++jj) {
06367 i__2 = *ks;
06368 for (ii = 1; ii <= i__2; ++ii) {
06369
06370
06371 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06372 ;
06373 }
06374 }
06375 } else if (*iswi == 2) {
06376 i__1 = *ks;
06377 for (ii = 1; ii <= i__1; ++ii) {
06378 i__2 = n1;
06379 for (jj = 1; jj <= i__2; ++jj) {
06380 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06381 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06382 }
06383 }
06384 } else if (*iswi == 3) {
06385 l = 2;
06386 i__1 = n1;
06387 for (jj = 1; jj <= i__1; ++jj) {
06388 i__2 = *ks + 2;
06389 for (ii = 1; ii <= i__2; ++ii) {
06390 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06391 }
06392 i__2 = *ks;
06393 for (ii = 1; ii <= i__2; ++ii) {
06394 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06395 }
06396 }
06397 } else if (*iswi == 4) {
06398 l = 2;
06399 i__1 = n1;
06400 for (jj = 1; jj <= i__1; ++jj) {
06401 i__2 = *ks + 2;
06402 for (ii = 1; ii <= i__2; ++ii) {
06403 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06404 }
06405 i__2 = *ks;
06406 for (ii = 1; ii <= i__2; ++ii) {
06407 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06408 }
06409 }
06410 } else if (*iswi == 5) {
06411 l = 1;
06412 i__1 = n1;
06413 for (jj = 1; jj <= i__1; ++jj) {
06414 i__2 = *ks + 1;
06415 for (ii = 1; ii <= i__2; ++ii) {
06416 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06417 }
06418 i__2 = *ks;
06419 for (ii = 1; ii <= i__2; ++ii) {
06420 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06421 }
06422 }
06423 } else if (*iswi == 6) {
06424 l = 1;
06425 i__1 = n1;
06426 for (jj = 1; jj <= i__1; ++jj) {
06427 i__2 = *ks + 1;
06428 for (ii = 1; ii <= i__2; ++ii) {
06429 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06430 }
06431 i__2 = *ks;
06432 for (ii = 1; ii <= i__2; ++ii) {
06433 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06434 }
06435 }
06436 } else if (*iswi == 7) {
06437 l = 3;
06438 i__1 = n1;
06439 for (jj = 1; jj <= i__1; ++jj) {
06440 i__2 = *ks + 3;
06441 for (ii = 1; ii <= i__2; ++ii) {
06442 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06443 }
06444 i__2 = *ks;
06445 for (ii = 1; ii <= i__2; ++ii) {
06446 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06447 }
06448 }
06449 } else if (*iswi == 8) {
06450 l = 4;
06451 i__1 = n1;
06452 for (jj = 1; jj <= i__1; ++jj) {
06453 i__2 = *ks + 4;
06454 for (ii = 1; ii <= i__2; ++ii) {
06455 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06456 }
06457 i__2 = *ks;
06458 for (ii = 1; ii <= i__2; ++ii) {
06459 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06460 }
06461 }
06462 }
06463
06464 Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06465 i__1 = *ks;
06466 int tmp__j=0;
06467 for (i__ = 1; i__ <= i__1; ++i__) {
06468 tmp = 0.f;
06469 i__2 = *n - 1;
06470 for (j = 1; j <= i__2; ++j) {
06471 tmp__j=j;
06472 tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06473 }
06474 tmp += x[*n];
06475 p.push_back(static_cast<float>(exp(tmp)));
06476 p.push_back(q1[i__ + q1_dim1]);
06477 }
06478 i__2=*n;
06479 for (i__=1;i__<=i__2;++i__)
06480 { p.push_back(static_cast<float>(x[i__]));}
06481 return p;
06482 }
06483 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06484 double *q, double *x, double *res, double *cu, long int *iu, double *s)
06485 {
06486
06487 long int q_dim1, q_offset, i__1, i__2;
06488 double d__1;
06489
06490 static long int i__, j;
06491 static double z__;
06492 static long int n1, n2, ia, ii, kk, in, nk, js;
06493 static double sn, zu, zv;
06494 static long int nk1, klm, nkl, jmn, jpn;
06495 static double cuv;
06496 static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06497 static float xmin;
06498 static double xmax;
06499 static long int iout;
06500 static double xsum;
06501 static long int iineg, maxit;
06502 static double toler;
06503 static float error;
06504 static double pivot;
06505 static long int kforce, iphase;
06506 static double tpivot;
06507
06508 --s;
06509 --res;
06510 iu -= 3;
06511 cu -= 3;
06512 --x;
06513 q_dim1 = *klm2d;
06514 q_offset = 1 + q_dim1;
06515 q -= q_offset;
06516
06517
06518 maxit = 500;
06519 kode = 0;
06520 toler = 1e-4f;
06521 iter = 0;
06522 n1 = *n + 1;
06523 n2 = *n + 2;
06524 nk = *n + *k;
06525 nk1 = nk + 1;
06526 nkl = nk + *l;
06527 nkl1 = nkl + 1;
06528 klm = *k + *l + *m;
06529 klm1 = klm + 1;
06530 klm2 = klm + 2;
06531 nklm = *n + klm;
06532 kforce = 1;
06533 iter = 0;
06534 js = 1;
06535 ia = 0;
06536
06537 i__1 = *n;
06538 for (j = 1; j <= i__1; ++j) {
06539 q[klm2 + j * q_dim1] = (double) j;
06540
06541 }
06542 i__1 = klm;
06543 for (i__ = 1; i__ <= i__1; ++i__) {
06544 q[i__ + n2 * q_dim1] = (double) (*n + i__);
06545 if (q[i__ + n1 * q_dim1] >= 0.f) {
06546 goto L30;
06547 }
06548 i__2 = n2;
06549 for (j = 1; j <= i__2; ++j) {
06550 q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06551
06552 }
06553 L30:
06554 ;
06555 }
06556
06557 iphase = 2;
06558 i__1 = nklm;
06559 for (j = 1; j <= i__1; ++j) {
06560 cu[(j << 1) + 1] = 0.f;
06561 cu[(j << 1) + 2] = 0.f;
06562 iu[(j << 1) + 1] = 0;
06563 iu[(j << 1) + 2] = 0;
06564
06565 }
06566 if (*l == 0) {
06567 goto L60;
06568 }
06569 i__1 = nkl;
06570 for (j = nk1; j <= i__1; ++j) {
06571 cu[(j << 1) + 1] = 1.f;
06572 cu[(j << 1) + 2] = 1.f;
06573 iu[(j << 1) + 1] = 1;
06574 iu[(j << 1) + 2] = 1;
06575
06576 }
06577 iphase = 1;
06578 L60:
06579 if (*m == 0) {
06580 goto L80;
06581 }
06582 i__1 = nklm;
06583 for (j = nkl1; j <= i__1; ++j) {
06584 cu[(j << 1) + 2] = 1.f;
06585 iu[(j << 1) + 2] = 1;
06586 jmn = j - *n;
06587 if (q[jmn + n2 * q_dim1] < 0.f) {
06588 iphase = 1;
06589 }
06590
06591 }
06592 L80:
06593 if (kode == 0) {
06594 goto L150;
06595 }
06596 i__1 = *n;
06597 for (j = 1; j <= i__1; ++j) {
06598 if ((d__1 = x[j]) < 0.) {
06599 goto L90;
06600 } else if (d__1 == 0) {
06601 goto L110;
06602 } else {
06603 goto L100;
06604 }
06605 L90:
06606 cu[(j << 1) + 1] = 1.f;
06607 iu[(j << 1) + 1] = 1;
06608 goto L110;
06609 L100:
06610 cu[(j << 1) + 2] = 1.f;
06611 iu[(j << 1) + 2] = 1;
06612 L110:
06613 ;
06614 }
06615 i__1 = *k;
06616 for (j = 1; j <= i__1; ++j) {
06617 jpn = j + *n;
06618 if ((d__1 = res[j]) < 0.) {
06619 goto L120;
06620 } else if (d__1 == 0) {
06621 goto L140;
06622 } else {
06623 goto L130;
06624 }
06625 L120:
06626 cu[(jpn << 1) + 1] = 1.f;
06627 iu[(jpn << 1) + 1] = 1;
06628 if (q[j + n2 * q_dim1] > 0.f) {
06629 iphase = 1;
06630 }
06631 goto L140;
06632 L130:
06633 cu[(jpn << 1) + 2] = 1.f;
06634 iu[(jpn << 1) + 2] = 1;
06635 if (q[j + n2 * q_dim1] < 0.f) {
06636 iphase = 1;
06637 }
06638 L140:
06639 ;
06640 }
06641 L150:
06642 if (iphase == 2) {
06643 goto L500;
06644 }
06645
06646 L160:
06647 i__1 = n1;
06648 for (j = js; j <= i__1; ++j) {
06649 xsum = 0.;
06650 i__2 = klm;
06651 for (i__ = 1; i__ <= i__2; ++i__) {
06652 ii = (long int) q[i__ + n2 * q_dim1];
06653 if (ii < 0) {
06654 goto L170;
06655 }
06656 z__ = cu[(ii << 1) + 1];
06657 goto L180;
06658 L170:
06659 iineg = -ii;
06660 z__ = cu[(iineg << 1) + 2];
06661 L180:
06662 xsum += q[i__ + j * q_dim1] * z__;
06663
06664
06665 }
06666 q[klm1 + j * q_dim1] = xsum;
06667
06668 }
06669 i__1 = *n;
06670 for (j = js; j <= i__1; ++j) {
06671 ii = (long int) q[klm2 + j * q_dim1];
06672 if (ii < 0) {
06673 goto L210;
06674 }
06675 z__ = cu[(ii << 1) + 1];
06676 goto L220;
06677 L210:
06678 iineg = -ii;
06679 z__ = cu[(iineg << 1) + 2];
06680 L220:
06681 q[klm1 + j * q_dim1] -= z__;
06682
06683 }
06684
06685 L240:
06686 xmax = 0.f;
06687 if (js > *n) {
06688 goto L490;
06689 }
06690 i__1 = *n;
06691 for (j = js; j <= i__1; ++j) {
06692 zu = q[klm1 + j * q_dim1];
06693 ii = (long int) q[klm2 + j * q_dim1];
06694 if (ii > 0) {
06695 goto L250;
06696 }
06697 ii = -ii;
06698 zv = zu;
06699 zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06700 goto L260;
06701 L250:
06702 zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06703 L260:
06704 if (kforce == 1 && ii > *n) {
06705 goto L280;
06706 }
06707 if (iu[(ii << 1) + 1] == 1) {
06708 goto L270;
06709 }
06710 if (zu <= xmax) {
06711 goto L270;
06712 }
06713 xmax = zu;
06714 in = j;
06715 L270:
06716 if (iu[(ii << 1) + 2] == 1) {
06717 goto L280;
06718 }
06719 if (zv <= xmax) {
06720 goto L280;
06721 }
06722 xmax = zv;
06723 in = j;
06724 L280:
06725 ;
06726 }
06727 if (xmax <= toler) {
06728 goto L490;
06729 }
06730 if (q[klm1 + in * q_dim1] == xmax) {
06731 goto L300;
06732 }
06733 i__1 = klm2;
06734 for (i__ = 1; i__ <= i__1; ++i__) {
06735 q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06736
06737 }
06738 q[klm1 + in * q_dim1] = xmax;
06739
06740 L300:
06741 if (iphase == 1 || ia == 0) {
06742 goto L330;
06743 }
06744 xmax = 0.f;
06745 i__1 = ia;
06746 for (i__ = 1; i__ <= i__1; ++i__) {
06747 z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06748 if (z__ <= xmax) {
06749 goto L310;
06750 }
06751 xmax = z__;
06752 iout = i__;
06753 L310:
06754 ;
06755 }
06756 if (xmax <= toler) {
06757 goto L330;
06758 }
06759 i__1 = n2;
06760 for (j = 1; j <= i__1; ++j) {
06761 z__ = q[ia + j * q_dim1];
06762 q[ia + j * q_dim1] = q[iout + j * q_dim1];
06763 q[iout + j * q_dim1] = z__;
06764
06765 }
06766 iout = ia;
06767 --ia;
06768 pivot = q[iout + in * q_dim1];
06769 goto L420;
06770 L330:
06771 kk = 0;
06772 i__1 = klm;
06773 for (i__ = 1; i__ <= i__1; ++i__) {
06774 z__ = q[i__ + in * q_dim1];
06775 if (z__ <= toler) {
06776 goto L340;
06777 }
06778 ++kk;
06779 res[kk] = q[i__ + n1 * q_dim1] / z__;
06780 s[kk] = (double) i__;
06781 L340:
06782 ;
06783 }
06784 L350:
06785 if (kk > 0) {
06786 goto L360;
06787 }
06788 kode = 2;
06789 goto L590;
06790 L360:
06791 xmin = static_cast<float>( res[1] );
06792 iout = (long int) s[1];
06793 j = 1;
06794 if (kk == 1) {
06795 goto L380;
06796 }
06797 i__1 = kk;
06798 for (i__ = 2; i__ <= i__1; ++i__) {
06799 if (res[i__] >= xmin) {
06800 goto L370;
06801 }
06802 j = i__;
06803 xmin = static_cast<float>( res[i__] );
06804 iout = (long int) s[i__];
06805 L370:
06806 ;
06807 }
06808 res[j] = res[kk];
06809 s[j] = s[kk];
06810 L380:
06811 --kk;
06812 pivot = q[iout + in * q_dim1];
06813 ii = (long int) q[iout + n2 * q_dim1];
06814 if (iphase == 1) {
06815 goto L400;
06816 }
06817 if (ii < 0) {
06818 goto L390;
06819 }
06820 if (iu[(ii << 1) + 2] == 1) {
06821 goto L420;
06822 }
06823 goto L400;
06824 L390:
06825 iineg = -ii;
06826 if (iu[(iineg << 1) + 1] == 1) {
06827 goto L420;
06828 }
06829
06830 L400:
06831 ii = abs(ii);
06832 cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06833 if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06834 goto L420;
06835 }
06836
06837 i__1 = n1;
06838 for (j = js; j <= i__1; ++j) {
06839 z__ = q[iout + j * q_dim1];
06840 q[klm1 + j * q_dim1] -= z__ * cuv;
06841 q[iout + j * q_dim1] = -z__;
06842
06843 }
06844 q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06845 goto L350;
06846
06847 L420:
06848 if (iter < maxit) {
06849 goto L430;
06850 }
06851 kode = 3;
06852 goto L590;
06853 L430:
06854 ++iter;
06855 i__1 = n1;
06856 for (j = js; j <= i__1; ++j) {
06857 if (j != in) {
06858 q[iout + j * q_dim1] /= pivot;
06859 }
06860
06861 }
06862
06863
06864
06865
06866
06867
06868
06869
06870 i__1 = n1;
06871 for (j = js; j <= i__1; ++j) {
06872 if (j == in) {
06873 goto L460;
06874 }
06875 z__ = -q[iout + j * q_dim1];
06876 i__2 = klm1;
06877 for (i__ = 1; i__ <= i__2; ++i__) {
06878 if (i__ != iout) {
06879 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06880 }
06881
06882 }
06883 L460:
06884 ;
06885 }
06886 tpivot = -pivot;
06887 i__1 = klm1;
06888 for (i__ = 1; i__ <= i__1; ++i__) {
06889 if (i__ != iout) {
06890 q[i__ + in * q_dim1] /= tpivot;
06891 }
06892
06893 }
06894 q[iout + in * q_dim1] = 1.f / pivot;
06895 z__ = q[iout + n2 * q_dim1];
06896 q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
06897 q[klm2 + in * q_dim1] = z__;
06898 ii = (long int) abs(z__);
06899 if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
06900 goto L240;
06901 }
06902 i__1 = klm2;
06903 for (i__ = 1; i__ <= i__1; ++i__) {
06904 z__ = q[i__ + in * q_dim1];
06905 q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
06906 q[i__ + js * q_dim1] = z__;
06907
06908 }
06909 ++js;
06910 goto L240;
06911
06912 L490:
06913 if (kforce == 0) {
06914 goto L580;
06915 }
06916 if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
06917 goto L500;
06918 }
06919 kforce = 0;
06920 goto L240;
06921
06922 L500:
06923 iphase = 2;
06924 i__1 = nklm;
06925 for (j = 1; j <= i__1; ++j) {
06926 cu[(j << 1) + 1] = 0.f;
06927 cu[(j << 1) + 2] = 0.f;
06928
06929 }
06930 i__1 = nk;
06931 for (j = n1; j <= i__1; ++j) {
06932 cu[(j << 1) + 1] = 1.f;
06933 cu[(j << 1) + 2] = 1.f;
06934
06935 }
06936 i__1 = klm;
06937 for (i__ = 1; i__ <= i__1; ++i__) {
06938 ii = (long int) q[i__ + n2 * q_dim1];
06939 if (ii > 0) {
06940 goto L530;
06941 }
06942 ii = -ii;
06943 if (iu[(ii << 1) + 2] == 0) {
06944 goto L560;
06945 }
06946 cu[(ii << 1) + 2] = 0.f;
06947 goto L540;
06948 L530:
06949 if (iu[(ii << 1) + 1] == 0) {
06950 goto L560;
06951 }
06952 cu[(ii << 1) + 1] = 0.f;
06953 L540:
06954 ++ia;
06955 i__2 = n2;
06956 for (j = 1; j <= i__2; ++j) {
06957 z__ = q[ia + j * q_dim1];
06958 q[ia + j * q_dim1] = q[i__ + j * q_dim1];
06959 q[i__ + j * q_dim1] = z__;
06960
06961 }
06962 L560:
06963 ;
06964 }
06965 goto L160;
06966 L570:
06967 if (q[klm1 + n1 * q_dim1] <= toler) {
06968 goto L500;
06969 }
06970 kode = 1;
06971 goto L590;
06972 L580:
06973 if (iphase == 1) {
06974 goto L570;
06975 }
06976
06977 kode = 0;
06978 L590:
06979 xsum = 0.;
06980 i__1 = *n;
06981 for (j = 1; j <= i__1; ++j) {
06982 x[j] = 0.f;
06983
06984 }
06985 i__1 = klm;
06986 for (i__ = 1; i__ <= i__1; ++i__) {
06987 res[i__] = 0.f;
06988
06989 }
06990 i__1 = klm;
06991 for (i__ = 1; i__ <= i__1; ++i__) {
06992 ii = (long int) q[i__ + n2 * q_dim1];
06993 sn = 1.f;
06994 if (ii > 0) {
06995 goto L620;
06996 }
06997 ii = -ii;
06998 sn = -1.f;
06999 L620:
07000 if (ii > *n) {
07001 goto L630;
07002 }
07003 x[ii] = sn * q[i__ + n1 * q_dim1];
07004 goto L640;
07005 L630:
07006 iimn = ii - *n;
07007 res[iimn] = sn * q[i__ + n1 * q_dim1];
07008 if (ii >= n1 && ii <= nk) {
07009 xsum += q[i__ + n1 * q_dim1];
07010 }
07011 L640:
07012 ;
07013 }
07014 error = (float)xsum;
07015 return;
07016 }
07017
07018 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07019 {
07020 int j,d;
07021 EMData * e = new EMData();
07022 float *eptr, *imgptr;
07023 imgptr = img->get_data();
07024 float SSE = 0.f;
07025 for (j = 0 ; j < N ; j++) {
07026 e->read_image(images,S[j]);
07027 eptr = e->get_data();
07028 for (d = 0; d < size; d++) {
07029 SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07030 }
07031 delete e;
07032 return SSE;
07033 }
07034
07035
07036 #define mymax(x,y) (((x)>(y))?(x):(y))
07037 #define mymin(x,y) (((x)<(y))?(x):(y))
07038 #define sign(x,y) (((((y)>0)?(1):(-1))*(y!=0))*(x))
07039
07040
07041 #define quadpi 3.141592653589793238462643383279502884197
07042 #define dgr_to_rad quadpi/180
07043 #define deg_to_rad quadpi/180
07044 #define rad_to_deg 180/quadpi
07045 #define rad_to_dgr 180/quadpi
07046 #define TRUE 1
07047 #define FALSE 0
07048
07049
07050 #define theta(i) theta [i-1]
07051 #define phi(i) phi [i-1]
07052 #define weight(i) weight [i-1]
07053 #define lband(i) lband [i-1]
07054 #define ts(i) ts [i-1]
07055 #define thetast(i) thetast [i-1]
07056 #define key(i) key [i-1]
07057
07058
07059 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07060 {
07061
07062 ENTERFUNC;
07063
07064 if ( th.size() != ph.size() ) {
07065 LOGERR("images not same size");
07066 throw ImageFormatException( "images not same size");
07067 }
07068
07069
07070 srand(10);
07071
07072 int i,*key;
07073 int len = th.size();
07074 double *theta,*phi,*weight;
07075 theta = (double*) calloc(len,sizeof(double));
07076 phi = (double*) calloc(len,sizeof(double));
07077 weight = (double*) calloc(len,sizeof(double));
07078 key = (int*) calloc(len,sizeof(int));
07079 const float *thptr, *phptr;
07080
07081 thptr = &th[0];
07082 phptr = &ph[0];
07083 for(i=1;i<=len;i++){
07084 key(i) = i;
07085 weight(i) = 0.0;
07086 }
07087
07088 for(i = 0;i<len;i++){
07089 theta[i] = thptr[i];
07090 phi[i] = phptr[i];
07091 }
07092
07093
07094 Util::hsortd(theta, phi, key, len, 1);
07095
07096
07097 Util::voronoi(phi, theta, weight, len);
07098
07099
07100 Util::hsortd(weight, weight, key, len, 2);
07101
07102 free(theta);
07103 free(phi);
07104 free(key);
07105 vector<double> wt;
07106 double count = 0;
07107 for(i=1; i<= len; i++)
07108 {
07109 wt.push_back(weight(i));
07110 count += weight(i);
07111 }
07112
07113
07114
07115
07116
07117
07118 free(weight);
07119
07120 EXITFUNC;
07121 return wt;
07122
07123 }
07124
07125 struct tmpstruct{
07126 double theta1,phi1;
07127 int key1;
07128 };
07129
07130 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07131 {
07132 ENTERFUNC;
07133 vector<tmpstruct> tmp(len);
07134 int i;
07135 for(i = 1;i<=len;i++)
07136 {
07137 tmp[i-1].theta1 = theta(i);
07138 tmp[i-1].phi1 = phi(i);
07139 tmp[i-1].key1 = key(i);
07140 }
07141
07142 if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07143 if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07144
07145 for(i = 1;i<=len;i++)
07146 {
07147 theta(i) = tmp[i-1].theta1;
07148 phi(i) = tmp[i-1].phi1;
07149 key(i) = tmp[i-1].key1;
07150 }
07151 EXITFUNC;
07152 }
07153
07154 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07155 {
07156 return(tmp1.theta1 < tmp2.theta1);
07157 }
07158
07159 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07160 {
07161 return(tmp1.key1 < tmp2.key1);
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
07409
07410
07411
07412
07413
07414
07415
07416
07417
07418
07419
07420
07421
07422
07423
07424
07425
07426
07427
07428
07429
07430
07431
07432
07433
07434
07435
07436
07437
07438
07439
07440
07441
07442
07443
07444
07445
07446
07447
07448
07449
07450
07451
07452
07453
07454
07455
07456
07457
07458
07459
07460
07461
07462
07463
07464
07465
07466
07467
07468
07469
07470
07471
07472
07473
07474
07475
07476
07477
07478
07479
07480
07481
07482 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07483 {
07484
07485 ENTERFUNC;
07486
07487 int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07488 int nt6, n, ier, nout, lnew, mdup, nd;
07489 int i,k,mt,status;
07490
07491
07492 double *ds, *x, *y, *z;
07493 double tol = 1.0e-8;
07494 double dtol = 15;
07495 double a;
07496
07497
07498
07499
07500
07501
07502
07503
07504
07505 n = nt + nt;
07506
07507 nt6 = n*6;
07508
07509 list = (int*)calloc(nt6,sizeof(int));
07510 lptr = (int*)calloc(nt6,sizeof(int));
07511 lend = (int*)calloc(n ,sizeof(int));
07512 iwk = (int*)calloc(n ,sizeof(int));
07513 good = (int*)calloc(n ,sizeof(int));
07514 key = (int*)calloc(n ,sizeof(int));
07515 indx = (int*)calloc(n ,sizeof(int));
07516 lcnt = (int*)calloc(n ,sizeof(int));
07517
07518 ds = (double*) calloc(n,sizeof(double));
07519 x = (double*) calloc(n,sizeof(double));
07520 y = (double*) calloc(n,sizeof(double));
07521 z = (double*) calloc(n,sizeof(double));
07522
07523 if (list == NULL ||
07524 lptr == NULL ||
07525 lend == NULL ||
07526 iwk == NULL ||
07527 good == NULL ||
07528 key == NULL ||
07529 indx == NULL ||
07530 lcnt == NULL ||
07531 x == NULL ||
07532 y == NULL ||
07533 z == NULL ||
07534 ds == NULL) {
07535 printf("memory allocation failure!\n");
07536 exit(1);
07537 }
07538
07539 bool colinear=true;
07540 while(colinear)
07541 {
07542
07543 L1:
07544 for(i = 0; i<nt; i++){
07545 x[i] = theta[i];
07546 y[i] = phi[i];
07547 x[nt+i] = 180.0 - x[i];
07548 y[nt+i] = 180.0 + y[i];
07549 }
07550
07551 Util::disorder2(x, y, key, n);
07552
07553
07554 double val;
07555 for(k=0; k<2; k++){
07556 for(i=k+1; i<3; i++){
07557 val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07558 if( val < dtol) {
07559 goto L1;
07560 }
07561 }
07562 }
07563
07564 Util::ang_to_xyz(x, y, z, n);
07565
07566
07567 bool dupnode=true;
07568 dupnode=true;
07569 while(dupnode)
07570 {
07571 for(k=0; k<2; k++){
07572 for(i=k+1; i<3; i++){
07573 if( x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07574 Util::flip23(x, y, z, key, k, n);
07575 continue;
07576 }
07577 }
07578 }
07579 dupnode = false;
07580 }
07581
07582
07583 ier = 0;
07584
07585 status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07586
07587 if (status != 0) {
07588 printf(" error in trmsh3 \n");
07589 exit(1);
07590 }
07591
07592 if (ier > 0) {
07593 printf("*** Error in TRMESH: duplicate nodes encountered ***\n");
07594 exit(1);
07595 }
07596
07597 mdup=n-nout;
07598 if (ier == -2) {
07599
07600 }
07601 else
07602 {
07603 colinear=false;
07604 }
07605 }
07606
07607
07608 Assert( ier != -2 );
07609
07610
07611 nd=0;
07612 for (k=1; k<=n; k++){
07613 if (indx[k-1]>0) {
07614 nd++;
07615 good[nd-1]=k;
07616 }
07617 }
07618
07619
07620
07621
07622 for(i = 1; i<=nout; i++) {
07623 k=good[i-1];
07624
07625 if (key[k-1] <= nt) {
07626
07627 a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07628 if (ier != 0){
07629
07630
07631 printf(" *** error in areav: ier = %d ***\n", ier);
07632 weight[key[k-1]-1] =-1.0;
07633 } else {
07634
07635 weight[key[k-1]-1]=a/lcnt[i-1];
07636 }
07637 }
07638 }
07639
07640
07641
07642 for(i = 1; i<=n; i++){
07643 mt =- indx[i-1];
07644 if (mt>0){
07645 k = good[mt-1];
07646
07647
07648
07649 if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07650 }
07651 }
07652
07653 free(list);
07654 free(lend);
07655 free(iwk);
07656 free(good);
07657 free(key);
07658 free(lptr);
07659 free(indx);
07660 free(lcnt);
07661 free(ds);
07662 free(x);
07663 free(y);
07664 free(z);
07665
07666
07667 EXITFUNC;
07668 }
07669
07670 void Util::disorder2(double *x,double *y, int *key, int len)
07671 {
07672 ENTERFUNC;
07673 int k, i;
07674 for(i=0; i<len; i++) key[i]=i+1;
07675
07676 for(i = 0; i<len;i++){
07677 k = rand()%len;
07678 std::swap(key[k], key[i]);
07679 std::swap(x[k], x[i]);
07680 std::swap(y[k], y[i]);
07681 }
07682 EXITFUNC;
07683 }
07684
07685 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07686 {
07687 ENTERFUNC;
07688 double costheta,sintheta,cosphi,sinphi;
07689 for(int i = 0; i<len; i++)
07690 {
07691 cosphi = cos(y[i]*dgr_to_rad);
07692 sinphi = sin(y[i]*dgr_to_rad);
07693 if(fabs(x[i]-90.0)< 1.0e-5){
07694 x[i] = cosphi;
07695 y[i] = sinphi;
07696 z[i] = 0.0;
07697 }
07698 else{
07699 costheta = cos(x[i]*dgr_to_rad);
07700 sintheta = sin(x[i]*dgr_to_rad);
07701 x[i] = cosphi*sintheta;
07702 y[i] = sinphi*sintheta;
07703 z[i] = costheta;
07704 }
07705 }
07706 EXITFUNC;
07707 }
07708
07709 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07710 {
07711 ENTERFUNC;
07712 int i = k;
07713 while( i == k ) i = rand()%len;
07714 std::swap(key[i], key[k]);
07715 std::swap(x[i], x[k]);
07716 std::swap(y[i], y[k]);
07717 std::swap(z[i], z[k]);
07718 EXITFUNC;
07719 }
07720
07721
07722 #undef mymax
07723 #undef mymin
07724 #undef sign
07725 #undef quadpi
07726 #undef dgr_to_rad
07727 #undef deg_to_rad
07728 #undef rad_to_deg
07729 #undef rad_to_dgr
07730 #undef TRUE
07731 #undef FALSE
07732 #undef theta
07733 #undef phi
07734 #undef weight
07735 #undef lband
07736 #undef ts
07737 #undef thetast
07738 #undef key
07739
07740
07741
07742
07743
07744
07745
07746
07747
07748
07749
07750 #define TRUE_ (1)
07751 #define FALSE_ (0)
07752 #define abs(x) ((x) >= 0 ? (x) : -(x))
07753
07754 struct stcom_{
07755 double y;
07756 };
07757 stcom_ stcom_1;
07758 #ifdef KR_headers
07759 double floor();
07760 int i_dnnt(x) double *x;
07761 #else
07762 int i_dnnt(double *x)
07763 #endif
07764 {
07765 return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07766 }
07767
07768
07769
07770
07771
07772 int Util::trmsh3_(int *n0, double *tol, double *x,
07773 double *y, double *z__, int *n, int *list, int *
07774 lptr, int *lend, int *lnew, int *indx, int *lcnt,
07775 int *near__, int *next, double *dist, int *ier)
07776 {
07777
07778 int i__1, i__2;
07779
07780
07781 static double d__;
07782 static int i__, j;
07783 static double d1, d2, d3;
07784 static int i0, lp, kt, ku, lpl, nku;
07785 extern long int left_(double *, double *, double *, double
07786 *, double *, double *, double *, double *,
07787 double *);
07788 static int nexti;
07789 extern int addnod_(int *, int *, double *,
07790 double *, double *, int *, int *, int *,
07791 int *, int *);
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
07870
07871
07872
07873
07874
07875
07876
07877
07878
07879
07880
07881
07882
07883
07884
07885
07886
07887
07888
07889
07890
07891
07892
07893
07894
07895
07896
07897
07898
07899
07900
07901
07902
07903
07904
07905
07906
07907
07908
07909
07910
07911
07912
07913
07914
07915
07916
07917
07918
07919
07920
07921
07922
07923
07924
07925
07926
07927
07928
07929
07930
07931
07932
07933
07934
07935
07936
07937
07938
07939
07940
07941
07942
07943 --dist;
07944 --next;
07945 --near__;
07946 --indx;
07947 --lend;
07948 --z__;
07949 --y;
07950 --x;
07951 --list;
07952 --lptr;
07953 --lcnt;
07954
07955
07956 if (*n0 < 3) {
07957 *n = 0;
07958 *ier = -1;
07959 return 0;
07960 }
07961
07962
07963
07964 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
07965 z__[3])) {
07966
07967
07968
07969 list[1] = 3;
07970 lptr[1] = 2;
07971 list[2] = -2;
07972 lptr[2] = 1;
07973 lend[1] = 2;
07974
07975 list[3] = 1;
07976 lptr[3] = 4;
07977 list[4] = -3;
07978 lptr[4] = 3;
07979 lend[2] = 4;
07980
07981 list[5] = 2;
07982 lptr[5] = 6;
07983 list[6] = -1;
07984 lptr[6] = 5;
07985 lend[3] = 6;
07986
07987 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
07988 y[3], &z__[3])) {
07989
07990
07991
07992
07993
07994 list[1] = 2;
07995 lptr[1] = 2;
07996 list[2] = -3;
07997 lptr[2] = 1;
07998 lend[1] = 2;
07999
08000 list[3] = 3;
08001 lptr[3] = 4;
08002 list[4] = -1;
08003 lptr[4] = 3;
08004 lend[2] = 4;
08005
08006 list[5] = 1;
08007 lptr[5] = 6;
08008 list[6] = -2;
08009 lptr[6] = 5;
08010 lend[3] = 6;
08011
08012
08013 } else {
08014
08015
08016
08017 *n = 0;
08018 *ier = -2;
08019 return 0;
08020 }
08021
08022
08023
08024
08025
08026 *lnew = 7;
08027 indx[1] = 1;
08028 indx[2] = 2;
08029 indx[3] = 3;
08030 lcnt[1] = 1;
08031 lcnt[2] = 1;
08032 lcnt[3] = 1;
08033 if (*n0 == 3) {
08034 *n = 3;
08035 *ier = 0;
08036 return 0;
08037 }
08038
08039
08040
08041
08042
08043
08044
08045
08046
08047
08048
08049
08050
08051
08052
08053
08054
08055
08056
08057
08058
08059
08060
08061
08062
08063
08064
08065 near__[1] = 0;
08066 near__[2] = 0;
08067 near__[3] = 0;
08068 for (ku = *n0; ku >= 4; --ku) {
08069 d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08070 d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08071 d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08072 if (d1 <= d2 && d1 <= d3) {
08073 near__[ku] = 1;
08074 dist[ku] = d1;
08075 next[ku] = near__[1];
08076 near__[1] = ku;
08077 } else if (d2 <= d1 && d2 <= d3) {
08078 near__[ku] = 2;
08079 dist[ku] = d2;
08080 next[ku] = near__[2];
08081 near__[2] = ku;
08082 } else {
08083 near__[ku] = 3;
08084 dist[ku] = d3;
08085 next[ku] = near__[3];
08086 near__[3] = ku;
08087 }
08088
08089 }
08090
08091
08092
08093
08094 kt = 3;
08095 i__1 = *n0;
08096 for (ku = 4; ku <= i__1; ++ku) {
08097 nku = near__[ku];
08098
08099
08100
08101 i__ = nku;
08102 if (near__[i__] == ku) {
08103 near__[i__] = next[ku];
08104 } else {
08105 i__ = near__[i__];
08106 L2:
08107 i0 = i__;
08108 i__ = next[i0];
08109 if (i__ != ku) {
08110 goto L2;
08111 }
08112 next[i0] = next[ku];
08113 }
08114 near__[ku] = 0;
08115
08116
08117
08118 if (dist[ku] <= *tol - 1.) {
08119 indx[ku] = -nku;
08120 ++lcnt[nku];
08121 goto L6;
08122 }
08123
08124
08125
08126 ++kt;
08127 x[kt] = x[ku];
08128 y[kt] = y[ku];
08129 z__[kt] = z__[ku];
08130 indx[ku] = kt;
08131 lcnt[kt] = 1;
08132 addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08133 , lnew, ier);
08134 if (*ier != 0) {
08135 *n = 0;
08136 *ier = -3;
08137 return 0;
08138 }
08139
08140
08141
08142 lpl = lend[kt];
08143 lp = lpl;
08144 L3:
08145 lp = lptr[lp];
08146 j = (i__2 = list[lp], abs(i__2));
08147
08148
08149
08150
08151
08152
08153
08154 i__ = near__[j];
08155 L4:
08156 if (i__ == 0) {
08157 goto L5;
08158 }
08159 nexti = next[i__];
08160
08161
08162
08163
08164 d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08165 if (d__ < dist[i__]) {
08166
08167
08168
08169
08170
08171 near__[i__] = kt;
08172 dist[i__] = d__;
08173 if (i__ == near__[j]) {
08174 near__[j] = nexti;
08175 } else {
08176 next[i0] = nexti;
08177 }
08178 next[i__] = near__[kt];
08179 near__[kt] = i__;
08180 } else {
08181 i0 = i__;
08182 }
08183
08184
08185
08186 i__ = nexti;
08187 goto L4;
08188
08189
08190
08191 L5:
08192 if (lp != lpl) {
08193 goto L3;
08194 }
08195 L6:
08196 ;
08197 }
08198 *n = kt;
08199 *ier = 0;
08200 return 0;
08201 }
08202
08203
08204 int addnod_(int *nst, int *k, double *x,
08205 double *y, double *z__, int *list, int *lptr, int
08206 *lend, int *lnew, int *ier)
08207 {
08208
08209
08210 static double tol = 0.;
08211
08212
08213 int i__1;
08214
08215
08216 static int l;
08217 static double p[3], b1, b2, b3;
08218 static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08219 extern int swap_(int *, int *, int *,
08220 int *, int *, int *, int *, int *);
08221 static int lpo1s;
08222 extern int bdyadd_(int *, int *, int *,
08223 int *, int *, int *, int *), intadd_(int *,
08224 int *, int *, int *, int *, int *, int *,
08225 int *), trfind_(int *, double *, int *,
08226 double *, double *, double *, int *, int *,
08227 int *, double *, double *, double *, int *,
08228 int *, int *), covsph_(int *, int *, int *,
08229 int *, int *, int *);
08230 extern int lstptr_(int *, int *, int *, int *);
08231 extern long int swptst_(int *, int *, int *, int *,
08232 double *, double *, double *);
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
08262
08263
08264
08265
08266
08267
08268
08269
08270
08271
08272
08273
08274
08275
08276
08277
08278
08279
08280
08281
08282
08283
08284
08285
08286
08287
08288
08289
08290
08291
08292
08293
08294
08295
08296
08297
08298
08299
08300
08301
08302
08303
08304
08305
08306
08307
08308
08309
08310
08311
08312
08313
08314
08315
08316
08317
08318
08319
08320
08321
08322
08323
08324
08325
08326
08327
08328
08329
08330
08331
08332
08333
08334
08335 --lend;
08336 --z__;
08337 --y;
08338 --x;
08339 --list;
08340 --lptr;
08341
08342
08343
08344 kk = *k;
08345 if (kk < 4) {
08346 goto L3;
08347 }
08348
08349
08350 km1 = kk - 1;
08351 ist = *nst;
08352 if (ist < 1) {
08353 ist = km1;
08354 }
08355 p[0] = x[kk];
08356 p[1] = y[kk];
08357 p[2] = z__[kk];
08358
08359
08360
08361
08362 trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08363 , &b1, &b2, &b3, &i1, &i2, &i3);
08364
08365
08366
08367 if (i1 == 0) {
08368 goto L4;
08369 }
08370 l = i1;
08371 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08372 goto L5;
08373 }
08374 l = i2;
08375 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08376 goto L5;
08377 }
08378 if (i3 != 0) {
08379 l = i3;
08380 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08381 goto L5;
08382 }
08383 intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08384 } else {
08385 if (i1 != i2) {
08386 bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08387 } else {
08388 covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08389 }
08390 }
08391 *ier = 0;
08392
08393
08394
08395 lp = lend[kk];
08396 lpf = lptr[lp];
08397 io2 = list[lpf];
08398 lpo1 = lptr[lpf];
08399 io1 = (i__1 = list[lpo1], abs(i__1));
08400
08401
08402
08403 L1:
08404 lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08405 if (list[lp] < 0) {
08406 goto L2;
08407 }
08408 lp = lptr[lp];
08409 in1 = (i__1 = list[lp], abs(i__1));
08410
08411
08412
08413
08414 lpo1s = lpo1;
08415 if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08416 goto L2;
08417 }
08418 swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08419 if (lpo1 == 0) {
08420
08421
08422
08423
08424
08425
08426 lpo1 = lpo1s;
08427 goto L2;
08428 }
08429 io1 = in1;
08430 goto L1;
08431
08432
08433
08434
08435 L2:
08436 if (lpo1 == lpf || list[lpo1] < 0) {
08437 return 0;
08438 }
08439 io2 = io1;
08440 lpo1 = lptr[lpo1];
08441 io1 = (i__1 = list[lpo1], abs(i__1));
08442 goto L1;
08443
08444
08445
08446 L3:
08447 *ier = -1;
08448 return 0;
08449
08450
08451
08452 L4:
08453 *ier = -2;
08454 return 0;
08455
08456
08457
08458 L5:
08459 *ier = l;
08460 return 0;
08461 }
08462
08463 double angle_(double *v1, double *v2, double *v3)
08464 {
08465
08466 double ret_val;
08467
08468
08469
08470
08471
08472 static double a;
08473 static int i__;
08474 static double ca, s21, s23, u21[3], u23[3];
08475 extern long int left_(double *, double *, double *, double
08476 *, double *, double *, double *, double *,
08477 double *);
08478
08479
08480
08481
08482
08483
08484
08485
08486
08487
08488
08489
08490
08491
08492
08493
08494
08495
08496
08497
08498
08499
08500
08501
08502
08503
08504
08505
08506
08507
08508
08509
08510
08511
08512
08513
08514
08515
08516
08517
08518
08519
08520
08521
08522
08523
08524
08525
08526
08527
08528
08529
08530
08531
08532
08533
08534
08535
08536
08537 --v3;
08538 --v2;
08539 --v1;
08540
08541
08542 u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08543 u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08544 u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08545
08546 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08547 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08548 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08549
08550
08551
08552 s21 = 0.;
08553 s23 = 0.;
08554 for (i__ = 1; i__ <= 3; ++i__) {
08555 s21 += u21[i__ - 1] * u21[i__ - 1];
08556 s23 += u23[i__ - 1] * u23[i__ - 1];
08557
08558 }
08559
08560
08561
08562
08563 if (s21 == 0. || s23 == 0.) {
08564 ret_val = 0.;
08565 return ret_val;
08566 }
08567 s21 = sqrt(s21);
08568 s23 = sqrt(s23);
08569 for (i__ = 1; i__ <= 3; ++i__) {
08570 u21[i__ - 1] /= s21;
08571 u23[i__ - 1] /= s23;
08572
08573 }
08574
08575
08576
08577
08578
08579 ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08580 if (ca < -1.) {
08581 ca = -1.;
08582 }
08583 if (ca > 1.) {
08584 ca = 1.;
08585 }
08586 a = acos(ca);
08587
08588
08589
08590
08591 if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08592 , &v3[3])) {
08593 a = acos(-1.) * 2. - a;
08594 }
08595 ret_val = a;
08596 return ret_val;
08597 }
08598
08599 double areas_(double *v1, double *v2, double *v3)
08600 {
08601
08602 double ret_val;
08603
08604
08605
08606
08607
08608 static int i__;
08609 static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08610 ca2, ca3;
08611
08612
08613
08614
08615
08616
08617
08618
08619
08620
08621
08622
08623
08624
08625
08626
08627
08628
08629
08630
08631
08632
08633
08634
08635
08636
08637
08638
08639
08640
08641
08642
08643
08644
08645
08646
08647
08648
08649
08650
08651
08652
08653
08654
08655
08656
08657
08658
08659
08660
08661
08662
08663
08664 --v3;
08665 --v2;
08666 --v1;
08667
08668
08669 u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08670 u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08671 u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08672
08673 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08674 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08675 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08676
08677 u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08678 u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08679 u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08680
08681
08682
08683 s12 = 0.;
08684 s23 = 0.;
08685 s31 = 0.;
08686 for (i__ = 1; i__ <= 3; ++i__) {
08687 s12 += u12[i__ - 1] * u12[i__ - 1];
08688 s23 += u23[i__ - 1] * u23[i__ - 1];
08689 s31 += u31[i__ - 1] * u31[i__ - 1];
08690
08691 }
08692
08693
08694
08695
08696 if (s12 == 0. || s23 == 0. || s31 == 0.) {
08697 ret_val = 0.;
08698 return ret_val;
08699 }
08700 s12 = sqrt(s12);
08701 s23 = sqrt(s23);
08702 s31 = sqrt(s31);
08703 for (i__ = 1; i__ <= 3; ++i__) {
08704 u12[i__ - 1] /= s12;
08705 u23[i__ - 1] /= s23;
08706 u31[i__ - 1] /= s31;
08707
08708 }
08709
08710
08711
08712
08713
08714
08715
08716 ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08717 ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08718 ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08719 if (ca1 < -1.) {
08720 ca1 = -1.;
08721 }
08722 if (ca1 > 1.) {
08723 ca1 = 1.;
08724 }
08725 if (ca2 < -1.) {
08726 ca2 = -1.;
08727 }
08728 if (ca2 > 1.) {
08729 ca2 = 1.;
08730 }
08731 if (ca3 < -1.) {
08732 ca3 = -1.;
08733 }
08734 if (ca3 > 1.) {
08735 ca3 = 1.;
08736 }
08737 a1 = acos(ca1);
08738 a2 = acos(ca2);
08739 a3 = acos(ca3);
08740
08741
08742
08743 ret_val = a1 + a2 + a3 - acos(-1.);
08744 if (ret_val < 0.) {
08745 ret_val = 0.;
08746 }
08747 return ret_val;
08748 }
08749
08750 double Util::areav_(int *k, int *n, double *x, double *y,
08751 double *z__, int *list, int *lptr, int *lend, int
08752 *ier)
08753 {
08754
08755
08756 static double amax = 6.28;
08757
08758
08759 double ret_val;
08760
08761
08762 static double a, c0[3], c2[3], c3[3];
08763 static int n1, n2, n3;
08764 static double v1[3], v2[3], v3[3];
08765 static int lp, lpl, ierr;
08766 static double asum;
08767 extern double areas_(double *, double *, double *);
08768 static long int first;
08769 extern int circum_(double *, double *,
08770 double *, double *, int *);
08771
08772
08773
08774
08775
08776
08777
08778
08779
08780
08781
08782
08783
08784
08785
08786
08787
08788
08789
08790
08791
08792
08793
08794
08795
08796
08797
08798
08799
08800
08801
08802
08803
08804
08805
08806
08807
08808
08809
08810
08811
08812
08813
08814
08815
08816
08817
08818
08819
08820
08821
08822
08823
08824
08825
08826
08827
08828
08829 --lend;
08830 --z__;
08831 --y;
08832 --x;
08833 --list;
08834 --lptr;
08835
08836
08837
08838
08839
08840 if (*k < 1 || *k > *n || *n <= 3) {
08841 goto L11;
08842 }
08843
08844
08845
08846
08847
08848 n1 = *k;
08849 v1[0] = x[n1];
08850 v1[1] = y[n1];
08851 v1[2] = z__[n1];
08852 lpl = lend[n1];
08853 n3 = list[lpl];
08854 if (n3 < 0) {
08855 goto L12;
08856 }
08857 lp = lpl;
08858 first = TRUE_;
08859 asum = 0.;
08860
08861
08862
08863 L1:
08864 n2 = n3;
08865 lp = lptr[lp];
08866 n3 = list[lp];
08867 v2[0] = x[n2];
08868 v2[1] = y[n2];
08869 v2[2] = z__[n2];
08870 v3[0] = x[n3];
08871 v3[1] = y[n3];
08872 v3[2] = z__[n3];
08873 if (first) {
08874
08875
08876
08877
08878 circum_(v1, v2, v3, c3, &ierr);
08879 if (ierr != 0) {
08880 goto L13;
08881 }
08882 c0[0] = c3[0];
08883 c0[1] = c3[1];
08884 c0[2] = c3[2];
08885 first = FALSE_;
08886 } else {
08887
08888
08889
08890
08891 c2[0] = c3[0];
08892 c2[1] = c3[1];
08893 c2[2] = c3[2];
08894 circum_(v1, v2, v3, c3, &ierr);
08895 if (ierr != 0) {
08896 goto L13;
08897 }
08898 a = areas_(v1, c2, c3);
08899 if (a > amax) {
08900 goto L14;
08901 }
08902 asum += a;
08903 }
08904
08905
08906
08907 if (lp != lpl) {
08908 goto L1;
08909 }
08910
08911
08912
08913 a = areas_(v1, c3, c0);
08914 if (a > amax) {
08915 goto L14;
08916 }
08917 asum += a;
08918
08919
08920
08921 *ier = 0;
08922 ret_val = asum;
08923 return ret_val;
08924
08925
08926
08927 L11:
08928 *ier = 1;
08929 ret_val = 0.;
08930 return ret_val;
08931
08932
08933
08934 L12:
08935 *ier = 2;
08936 ret_val = 0.;
08937 return ret_val;
08938
08939
08940
08941 L13:
08942 *ier = 3;
08943 ret_val = 0.;
08944 return ret_val;
08945
08946
08947
08948 L14:
08949 *ier = 4;
08950 ret_val = 0.;
08951 return ret_val;
08952 }
08953
08954 double areav_new__(int *k, int *n, double *x, double *y,
08955 double *z__, int *list, int *lptr, int *lend, int
08956 *ier)
08957 {
08958
08959 double ret_val = 0;
08960
08961
08962
08963
08964
08965 static int m;
08966 static double c1[3], c2[3], c3[3];
08967 static int n1, n2, n3;
08968 static double v1[3], v2[3], v3[3];
08969 static int lp;
08970 static double c1s[3], c2s[3];
08971 static int lpl, ierr;
08972 static double asum;
08973 extern double angle_(double *, double *, double *);
08974 static float areav;
08975 extern int circum_(double *, double *,
08976 double *, double *, int *);
08977
08978
08979
08980
08981
08982
08983
08984
08985
08986
08987
08988
08989
08990
08991
08992
08993
08994
08995
08996
08997
08998
08999
09000
09001
09002
09003
09004
09005
09006
09007
09008
09009
09010
09011
09012
09013
09014
09015
09016
09017
09018
09019
09020
09021
09022
09023
09024
09025
09026
09027
09028
09029
09030
09031
09032
09033
09034
09035
09036
09037
09038 --lend;
09039 --z__;
09040 --y;
09041 --x;
09042 --list;
09043 --lptr;
09044
09045
09046 if (*k < 1 || *k > *n || *n <= 3) {
09047 goto L11;
09048 }
09049
09050
09051
09052
09053
09054 n1 = *k;
09055 v1[0] = x[n1];
09056 v1[1] = y[n1];
09057 v1[2] = z__[n1];
09058 lpl = lend[n1];
09059 n3 = list[lpl];
09060 if (n3 < 0) {
09061 goto L12;
09062 }
09063 lp = lpl;
09064 m = 0;
09065 asum = 0.;
09066
09067
09068
09069 L1:
09070 ++m;
09071 n2 = n3;
09072 lp = lptr[lp];
09073 n3 = list[lp];
09074 v2[0] = x[n2];
09075 v2[1] = y[n2];
09076 v2[2] = z__[n2];
09077 v3[0] = x[n3];
09078 v3[1] = y[n3];
09079 v3[2] = z__[n3];
09080 if (m == 1) {
09081
09082
09083
09084
09085 circum_(v1, v2, v3, c2, &ierr);
09086 if (ierr != 0) {
09087 goto L13;
09088 }
09089 c1s[0] = c2[0];
09090 c1s[1] = c2[1];
09091 c1s[2] = c2[2];
09092 } else if (m == 2) {
09093
09094
09095
09096
09097 circum_(v1, v2, v3, c3, &ierr);
09098 if (ierr != 0) {
09099 goto L13;
09100 }
09101 c2s[0] = c3[0];
09102 c2s[1] = c3[1];
09103 c2s[2] = c3[2];
09104 } else {
09105
09106
09107
09108
09109
09110 c1[0] = c2[0];
09111 c1[1] = c2[1];
09112 c1[2] = c2[2];
09113 c2[0] = c3[0];
09114 c2[1] = c3[1];
09115 c2[2] = c3[2];
09116 circum_(v1, v2, v3, c3, &ierr);
09117 if (ierr != 0) {
09118 goto L13;
09119 }
09120 asum += angle_(c1, c2, c3);
09121 }
09122
09123
09124
09125 if (lp != lpl) {
09126 goto L1;
09127 }
09128
09129
09130
09131
09132 asum += angle_(c2, c3, c1s);
09133
09134
09135
09136
09137 asum += angle_(c3, c1s, c2s);
09138
09139
09140
09141 *ier = 0;
09142 ret_val = asum - (double) (m - 2) * acos(-1.);
09143 return ret_val;
09144
09145
09146
09147 L11:
09148 *ier = 1;
09149 areav = 0.f;
09150 return ret_val;
09151
09152
09153
09154 L12:
09155 *ier = 2;
09156 areav = 0.f;
09157 return ret_val;
09158
09159
09160
09161 L13:
09162 *ier = 3;
09163 areav = 0.f;
09164 return ret_val;
09165 }
09166
09167 int bdyadd_(int *kk, int *i1, int *i2, int *
09168 list, int *lptr, int *lend, int *lnew)
09169 {
09170 static int k, n1, n2, lp, lsav, nsav, next;
09171 extern int insert_(int *, int *, int *,
09172 int *, int *);
09173
09174
09175
09176
09177
09178
09179
09180
09181
09182
09183
09184
09185
09186
09187
09188
09189
09190
09191
09192
09193
09194
09195
09196
09197
09198
09199
09200
09201
09202
09203
09204
09205
09206
09207
09208
09209
09210
09211
09212
09213
09214
09215
09216
09217
09218
09219
09220
09221
09222
09223
09224
09225
09226
09227
09228
09229
09230
09231
09232
09233
09234
09235
09236
09237 --lend;
09238 --lptr;
09239 --list;
09240
09241
09242 k = *kk;
09243 n1 = *i1;
09244 n2 = *i2;
09245
09246
09247
09248 lp = lend[n1];
09249 lsav = lptr[lp];
09250 lptr[lp] = *lnew;
09251 list[*lnew] = -k;
09252 lptr[*lnew] = lsav;
09253 lend[n1] = *lnew;
09254 ++(*lnew);
09255 next = -list[lp];
09256 list[lp] = next;
09257 nsav = next;
09258
09259
09260
09261
09262 L1:
09263 lp = lend[next];
09264 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09265 if (next == n2) {
09266 goto L2;
09267 }
09268 next = -list[lp];
09269 list[lp] = next;
09270 goto L1;
09271
09272
09273
09274
09275 L2:
09276 lsav = *lnew;
09277 list[*lnew] = n1;
09278 lptr[*lnew] = *lnew + 1;
09279 ++(*lnew);
09280 next = nsav;
09281
09282 L3:
09283 if (next == n2) {
09284 goto L4;
09285 }
09286 list[*lnew] = next;
09287 lptr[*lnew] = *lnew + 1;
09288 ++(*lnew);
09289 lp = lend[next];
09290 next = list[lp];
09291 goto L3;
09292
09293 L4:
09294 list[*lnew] = -n2;
09295 lptr[*lnew] = lsav;
09296 lend[k] = *lnew;
09297 ++(*lnew);
09298 return 0;
09299 }
09300
09301 int bnodes_(int *n, int *list, int *lptr,
09302 int *lend, int *nodes, int *nb, int *na, int *nt)
09303 {
09304
09305 int i__1;
09306
09307
09308 static int k, n0, lp, nn, nst;
09309
09310
09311
09312
09313
09314
09315
09316
09317
09318
09319
09320
09321
09322
09323
09324
09325
09326
09327
09328
09329
09330
09331
09332
09333
09334
09335
09336
09337
09338
09339
09340
09341
09342
09343
09344
09345
09346
09347
09348
09349
09350
09351
09352
09353
09354
09355
09356
09357
09358
09359
09360
09361
09362
09363
09364
09365
09366
09367
09368
09369 --lend;
09370 --list;
09371 --lptr;
09372 --nodes;
09373
09374
09375 nn = *n;
09376
09377
09378
09379 i__1 = nn;
09380 for (nst = 1; nst <= i__1; ++nst) {
09381 lp = lend[nst];
09382 if (list[lp] < 0) {
09383 goto L2;
09384 }
09385
09386 }
09387
09388
09389
09390 *nb = 0;
09391 *na = (nn - 2) * 3;
09392 *nt = nn - (2<<1);
09393 return 0;
09394
09395
09396
09397
09398 L2:
09399 nodes[1] = nst;
09400 k = 1;
09401 n0 = nst;
09402
09403
09404
09405 L3:
09406 lp = lend[n0];
09407 lp = lptr[lp];
09408 n0 = list[lp];
09409 if (n0 == nst) {
09410 goto L4;
09411 }
09412 ++k;
09413 nodes[k] = n0;
09414 goto L3;
09415
09416
09417
09418 L4:
09419 *nb = k;
09420 *nt = (*n << 1) - *nb - 2;
09421 *na = *nt + *n - 1;
09422 return 0;
09423 }
09424
09425 int circle_(int *k, double *xc, double *yc,
09426 int *ier)
09427 {
09428
09429 int i__1;
09430
09431
09432
09433
09434
09435 static double a, c__;
09436 static int i__;
09437 static double s;
09438 static int k2, k3;
09439 static double x0, y0;
09440 static int kk, np1;
09441
09442
09443
09444
09445
09446
09447
09448
09449
09450
09451
09452
09453
09454
09455
09456
09457
09458
09459
09460
09461
09462
09463
09464
09465
09466
09467
09468
09469
09470
09471
09472
09473
09474
09475
09476
09477
09478
09479
09480
09481
09482
09483
09484
09485
09486
09487
09488
09489
09490
09491
09492
09493
09494
09495
09496
09497
09498
09499
09500
09501
09502
09503
09504
09505 --yc;
09506 --xc;
09507
09508
09509 kk = *k;
09510 k2 = kk << 1;
09511 k3 = kk * 3;
09512 np1 = (kk << 2) + 1;
09513
09514
09515
09516
09517 if (kk < 1) {
09518 goto L2;
09519 }
09520 a = atan(1.) * 2. / (double) kk;
09521 c__ = cos(a);
09522 s = sin(a);
09523 x0 = 1.;
09524 y0 = 0.;
09525
09526
09527
09528
09529
09530 i__1 = kk;
09531 for (i__ = 1; i__ <= i__1; ++i__) {
09532 xc[i__] = x0;
09533 yc[i__] = y0;
09534 xc[i__ + kk] = -y0;
09535 yc[i__ + kk] = x0;
09536 xc[i__ + k2] = -x0;
09537 yc[i__ + k2] = -y0;
09538 xc[i__ + k3] = y0;
09539 yc[i__ + k3] = -x0;
09540
09541
09542
09543 x0 = c__ * x0 - s * y0;
09544 y0 = s * x0 + c__ * y0;
09545
09546 }
09547
09548
09549
09550
09551 xc[np1] = xc[1];
09552 yc[np1] = yc[1];
09553 *ier = 0;
09554 return 0;
09555
09556
09557
09558 L2:
09559 *ier = 1;
09560 return 0;
09561 }
09562
09563 int circum_(double *v1, double *v2, double *v3,
09564 double *c__, int *ier)
09565 {
09566
09567
09568
09569
09570 static int i__;
09571 static double e1[3], e2[3], cu[3], cnorm;
09572
09573
09574
09575
09576
09577
09578
09579
09580
09581
09582
09583
09584
09585
09586
09587
09588
09589
09590
09591
09592
09593
09594
09595
09596
09597
09598
09599
09600
09601
09602
09603
09604
09605
09606
09607
09608
09609
09610
09611
09612
09613
09614
09615
09616
09617
09618
09619
09620
09621
09622
09623
09624
09625
09626
09627
09628 --c__;
09629 --v3;
09630 --v2;
09631 --v1;
09632
09633
09634 for (i__ = 1; i__ <= 3; ++i__) {
09635 e1[i__ - 1] = v2[i__] - v1[i__];
09636 e2[i__ - 1] = v3[i__] - v1[i__];
09637
09638 }
09639
09640
09641
09642 cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09643 cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09644 cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09645 cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09646
09647
09648
09649
09650 if (cnorm != 0.) {
09651
09652
09653
09654 cnorm = sqrt(cnorm);
09655 for (i__ = 1; i__ <= 3; ++i__) {
09656 c__[i__] = cu[i__ - 1] / cnorm;
09657
09658 }
09659
09660
09661
09662
09663
09664
09665 if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09666 c__[1] = -c__[1];
09667 c__[2] = -c__[2];
09668 c__[3] = -c__[3];
09669 }
09670 *ier = 0;
09671 } else {
09672
09673
09674
09675 *ier = 1;
09676 }
09677 return 0;
09678 }
09679
09680 int covsph_(int *kk, int *n0, int *list, int
09681 *lptr, int *lend, int *lnew)
09682 {
09683 static int k, lp, nst, lsav, next;
09684 extern int insert_(int *, int *, int *,
09685 int *, int *);
09686
09687
09688
09689
09690
09691
09692
09693
09694
09695
09696
09697
09698
09699
09700
09701
09702
09703
09704
09705
09706
09707
09708
09709
09710
09711
09712
09713
09714
09715
09716
09717
09718
09719
09720
09721
09722
09723
09724
09725
09726
09727
09728
09729
09730
09731
09732
09733
09734
09735
09736
09737
09738
09739
09740
09741
09742
09743 --lend;
09744 --lptr;
09745 --list;
09746
09747
09748 k = *kk;
09749 nst = *n0;
09750
09751
09752
09753
09754
09755 next = nst;
09756 L1:
09757 lp = lend[next];
09758 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09759 next = -list[lp];
09760 list[lp] = next;
09761 if (next != nst) {
09762 goto L1;
09763 }
09764
09765
09766
09767
09768 lsav = *lnew;
09769 L2:
09770 lp = lend[next];
09771 list[*lnew] = next;
09772 lptr[*lnew] = *lnew + 1;
09773 ++(*lnew);
09774 next = list[lp];
09775 if (next != nst) {
09776 goto L2;
09777 }
09778
09779 lptr[*lnew - 1] = lsav;
09780 lend[k] = *lnew - 1;
09781 return 0;
09782 }
09783
09784 int crlist_(int *n, int *ncol, double *x,
09785 double *y, double *z__, int *list, int *lend, int
09786 *lptr, int *lnew, int *ltri, int *listc, int *nb,
09787 double *xc, double *yc, double *zc, double *rc,
09788 int *ier)
09789 {
09790
09791 int i__1, i__2;
09792
09793
09794
09795
09796
09797 static double c__[3], t;
09798 static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09799 static double v1[3], v2[3], v3[3];
09800 static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09801 lpn;
09802 static long int swp;
09803 static int ierr;
09804 extern int circum_(double *, double *,
09805 double *, double *, int *);
09806 extern int lstptr_(int *, int *, int *, int *);
09807 extern long int swptst_(int *, int *, int *, int *,
09808 double *, double *, double *);
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
09912
09913
09914
09915
09916
09917
09918
09919
09920
09921
09922
09923
09924
09925
09926
09927
09928
09929
09930
09931
09932
09933
09934
09935
09936
09937
09938
09939
09940
09941
09942
09943
09944
09945
09946
09947
09948
09949
09950
09951
09952
09953
09954
09955
09956
09957
09958
09959
09960
09961
09962
09963
09964
09965
09966
09967
09968
09969
09970
09971
09972
09973
09974
09975
09976
09977
09978
09979
09980
09981
09982
09983
09984
09985 --lend;
09986 --z__;
09987 --y;
09988 --x;
09989 ltri -= 7;
09990 --list;
09991 --lptr;
09992 --listc;
09993 --xc;
09994 --yc;
09995 --zc;
09996 --rc;
09997
09998
09999 nn = *n;
10000 *nb = 0;
10001 nt = 0;
10002 if (nn < 3) {
10003 goto L21;
10004 }
10005
10006
10007
10008 i__1 = nn;
10009 for (n1 = 1; n1 <= i__1; ++n1) {
10010 lp = lend[n1];
10011 if (list[lp] < 0) {
10012 goto L2;
10013 }
10014
10015 }
10016
10017
10018
10019 goto L9;
10020
10021
10022
10023
10024
10025
10026
10027
10028 L2:
10029 n2 = -list[lp];
10030 lp = lptr[lp];
10031 n3 = list[lp];
10032
10033
10034
10035
10036
10037
10038 L3:
10039 ++nt;
10040 if (nt <= *ncol) {
10041 ltri[nt * 6 + 1] = n1;
10042 ltri[nt * 6 + 2] = n2;
10043 ltri[nt * 6 + 3] = n3;
10044 ltri[nt * 6 + 4] = nt + 1;
10045 ltri[nt * 6 + 5] = nt - 1;
10046 ltri[nt * 6 + 6] = 0;
10047 }
10048 n1 = n2;
10049 lp = lend[n1];
10050 n2 = -list[lp];
10051 if (n2 != n3) {
10052 goto L3;
10053 }
10054
10055 *nb = nt + 2;
10056 if (*ncol < nt) {
10057 goto L22;
10058 }
10059 ltri[nt * 6 + 4] = 0;
10060 if (nt == 1) {
10061 goto L7;
10062 }
10063
10064
10065
10066
10067
10068
10069
10070 L4:
10071 swp = FALSE_;
10072 i__1 = nt - 1;
10073 for (kt1 = 1; kt1 <= i__1; ++kt1) {
10074 for (i3 = 1; i3 <= 3; ++i3) {
10075 kt2 = ltri[i3 + 3 + kt1 * 6];
10076 if (kt2 <= kt1) {
10077 goto L5;
10078 }
10079
10080
10081
10082
10083 if (i3 == 1) {
10084 i1 = 2;
10085 i2 = 3;
10086 } else if (i3 == 2) {
10087 i1 = 3;
10088 i2 = 1;
10089 } else {
10090 i1 = 1;
10091 i2 = 2;
10092 }
10093 n1 = ltri[i1 + kt1 * 6];
10094 n2 = ltri[i2 + kt1 * 6];
10095 n3 = ltri[i3 + kt1 * 6];
10096
10097
10098
10099
10100 if (ltri[kt2 * 6 + 4] == kt1) {
10101 i4 = 1;
10102 } else if (ltri[kt2 * 6 + 5] == kt1) {
10103 i4 = 2;
10104 } else {
10105 i4 = 3;
10106 }
10107 n4 = ltri[i4 + kt2 * 6];
10108
10109
10110
10111
10112
10113 if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10114 goto L5;
10115 }
10116
10117
10118
10119
10120 swp = TRUE_;
10121 kt11 = ltri[i1 + 3 + kt1 * 6];
10122 kt12 = ltri[i2 + 3 + kt1 * 6];
10123 if (i4 == 1) {
10124 i2 = 2;
10125 i1 = 3;
10126 } else if (i4 == 2) {
10127 i2 = 3;
10128 i1 = 1;
10129 } else {
10130 i2 = 1;
10131 i1 = 2;
10132 }
10133 kt21 = ltri[i1 + 3 + kt2 * 6];
10134 kt22 = ltri[i2 + 3 + kt2 * 6];
10135 ltri[kt1 * 6 + 1] = n4;
10136 ltri[kt1 * 6 + 2] = n3;
10137 ltri[kt1 * 6 + 3] = n1;
10138 ltri[kt1 * 6 + 4] = kt12;
10139 ltri[kt1 * 6 + 5] = kt22;
10140 ltri[kt1 * 6 + 6] = kt2;
10141 ltri[kt2 * 6 + 1] = n3;
10142 ltri[kt2 * 6 + 2] = n4;
10143 ltri[kt2 * 6 + 3] = n2;
10144 ltri[kt2 * 6 + 4] = kt21;
10145 ltri[kt2 * 6 + 5] = kt11;
10146 ltri[kt2 * 6 + 6] = kt1;
10147
10148
10149
10150 if (kt11 != 0) {
10151 i4 = 4;
10152 if (ltri[kt11 * 6 + 4] != kt1) {
10153 i4 = 5;
10154 if (ltri[kt11 * 6 + 5] != kt1) {
10155 i4 = 6;
10156 }
10157 }
10158 ltri[i4 + kt11 * 6] = kt2;
10159 }
10160 if (kt22 != 0) {
10161 i4 = 4;
10162 if (ltri[kt22 * 6 + 4] != kt2) {
10163 i4 = 5;
10164 if (ltri[kt22 * 6 + 5] != kt2) {
10165 i4 = 6;
10166 }
10167 }
10168 ltri[i4 + kt22 * 6] = kt1;
10169 }
10170 L5:
10171 ;
10172 }
10173
10174 }
10175 if (swp) {
10176 goto L4;
10177 }
10178
10179
10180
10181
10182 L7:
10183 i__1 = nt;
10184 for (kt = 1; kt <= i__1; ++kt) {
10185 n1 = ltri[kt * 6 + 1];
10186 n2 = ltri[kt * 6 + 2];
10187 n3 = ltri[kt * 6 + 3];
10188 v1[0] = x[n1];
10189 v1[1] = y[n1];
10190 v1[2] = z__[n1];
10191 v2[0] = x[n2];
10192 v2[1] = y[n2];
10193 v2[2] = z__[n2];
10194 v3[0] = x[n3];
10195 v3[1] = y[n3];
10196 v3[2] = z__[n3];
10197 circum_(v2, v1, v3, c__, &ierr);
10198 if (ierr != 0) {
10199 goto L23;
10200 }
10201
10202
10203
10204
10205 xc[kt] = -c__[0];
10206 yc[kt] = -c__[1];
10207 zc[kt] = -c__[2];
10208 t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10209 if (t < -1.) {
10210 t = -1.;
10211 }
10212 if (t > 1.) {
10213 t = 1.;
10214 }
10215 rc[kt] = acos(t);
10216
10217 }
10218
10219
10220
10221
10222
10223
10224 L9:
10225 kt = nt;
10226
10227
10228
10229 nm2 = nn - 2;
10230 i__1 = nm2;
10231 for (n1 = 1; n1 <= i__1; ++n1) {
10232 lpl = lend[n1];
10233 lp = lpl;
10234 n3 = list[lp];
10235
10236
10237
10238
10239 L10:
10240 lp = lptr[lp];
10241 n2 = n3;
10242 n3 = (i__2 = list[lp], abs(i__2));
10243 if (n2 <= n1 || n3 <= n1) {
10244 goto L11;
10245 }
10246 ++kt;
10247
10248
10249
10250 v1[0] = x[n1];
10251 v1[1] = y[n1];
10252 v1[2] = z__[n1];
10253 v2[0] = x[n2];
10254 v2[1] = y[n2];
10255 v2[2] = z__[n2];
10256 v3[0] = x[n3];
10257 v3[1] = y[n3];
10258 v3[2] = z__[n3];
10259 circum_(v1, v2, v3, c__, &ierr);
10260 if (ierr != 0) {
10261 goto L23;
10262 }
10263
10264
10265
10266 xc[kt] = c__[0];
10267 yc[kt] = c__[1];
10268 zc[kt] = c__[2];
10269 t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10270 if (t < -1.) {
10271 t = -1.;
10272 }
10273 if (t > 1.) {
10274 t = 1.;
10275 }
10276 rc[kt] = acos(t);
10277
10278
10279
10280
10281
10282 lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10283 listc[lpn] = kt;
10284 lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10285 listc[lpn] = kt;
10286 lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10287 listc[lpn] = kt;
10288 L11:
10289 if (lp != lpl) {
10290 goto L10;
10291 }
10292
10293 }
10294 if (nt == 0) {
10295 goto L20;
10296 }
10297
10298
10299
10300
10301
10302
10303 kt1 = 0;
10304 L13:
10305 ++kt1;
10306 if (ltri[kt1 * 6 + 4] == 0) {
10307 i1 = 2;
10308 i2 = 3;
10309 i3 = 1;
10310 goto L14;
10311 } else if (ltri[kt1 * 6 + 5] == 0) {
10312 i1 = 3;
10313 i2 = 1;
10314 i3 = 2;
10315 goto L14;
10316 } else if (ltri[kt1 * 6 + 6] == 0) {
10317 i1 = 1;
10318 i2 = 2;
10319 i3 = 3;
10320 goto L14;
10321 }
10322 goto L13;
10323 L14:
10324 n1 = ltri[i1 + kt1 * 6];
10325 n0 = n1;
10326
10327
10328
10329
10330
10331
10332
10333
10334
10335 L15:
10336 lp = lend[n1];
10337 lpn = lptr[lp];
10338 listc[lp] = kt1;
10339
10340
10341
10342 L16:
10343 kt2 = ltri[i2 + 3 + kt1 * 6];
10344 if (kt2 != 0) {
10345
10346
10347
10348 lptr[lp] = *lnew;
10349 lp = *lnew;
10350 listc[lp] = kt2;
10351 ++(*lnew);
10352
10353
10354
10355
10356 kt1 = kt2;
10357 if (ltri[kt1 * 6 + 1] == n1) {
10358 i1 = 1;
10359 i2 = 2;
10360 i3 = 3;
10361 } else if (ltri[kt1 * 6 + 2] == n1) {
10362 i1 = 2;
10363 i2 = 3;
10364 i3 = 1;
10365 } else {
10366 i1 = 3;
10367 i2 = 1;
10368 i3 = 2;
10369 }
10370 goto L16;
10371 }
10372
10373
10374
10375
10376
10377
10378
10379 lptr[lp] = lpn;
10380 n1 = ltri[i3 + kt1 * 6];
10381 if (n1 != n0) {
10382 i4 = i3;
10383 i3 = i2;
10384 i2 = i1;
10385 i1 = i4;
10386 goto L15;
10387 }
10388
10389
10390
10391 L20:
10392 *ier = 0;
10393 return 0;
10394
10395
10396
10397 L21:
10398 *ier = 1;
10399 return 0;
10400
10401
10402
10403 L22:
10404 *ier = 2;
10405 return 0;
10406
10407
10408
10409 L23:
10410 *ier = 3;
10411 return 0;
10412 }
10413
10414 int delarc_(int *n, int *io1, int *io2, int *
10415 list, int *lptr, int *lend, int *lnew, int *ier)
10416 {
10417
10418 int i__1;
10419
10420
10421 static int n1, n2, n3, lp, lph, lpl;
10422 extern int delnb_(int *, int *, int *,
10423 int *, int *, int *, int *, int *);
10424 extern int lstptr_(int *, int *, int *, int *);
10425
10426
10427
10428
10429
10430
10431
10432
10433
10434
10435
10436
10437
10438
10439
10440
10441
10442
10443
10444
10445
10446
10447
10448
10449
10450
10451
10452
10453
10454
10455
10456
10457
10458
10459
10460
10461
10462
10463
10464
10465
10466
10467
10468
10469
10470
10471
10472
10473
10474
10475
10476
10477
10478
10479
10480
10481
10482
10483
10484
10485
10486
10487
10488
10489
10490
10491
10492
10493
10494
10495
10496
10497
10498 --lend;
10499 --list;
10500 --lptr;
10501
10502
10503 n1 = *io1;
10504 n2 = *io2;
10505
10506
10507
10508
10509
10510 if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10511 *ier = 1;
10512 return 0;
10513 }
10514
10515 lpl = lend[n2];
10516 if (-list[lpl] != n1) {
10517 n1 = n2;
10518 n2 = *io1;
10519 lpl = lend[n2];
10520 if (-list[lpl] != n1) {
10521 *ier = 2;
10522 return 0;
10523 }
10524 }
10525
10526
10527
10528
10529
10530 lpl = lend[n1];
10531 lp = lptr[lpl];
10532 lp = lptr[lp];
10533 n3 = (i__1 = list[lp], abs(i__1));
10534 lpl = lend[n3];
10535 if (list[lpl] <= 0) {
10536 *ier = 3;
10537 return 0;
10538 }
10539
10540
10541
10542
10543
10544
10545 delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10546 if (lph < 0) {
10547 *ier = 4;
10548 return 0;
10549 }
10550
10551
10552
10553
10554 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10555
10556
10557
10558
10559 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10560 lend[n3] = lp;
10561 list[lp] = -n1;
10562
10563
10564
10565 *ier = 0;
10566 return 0;
10567 }
10568
10569 int delnb_(int *n0, int *nb, int *n, int *
10570 list, int *lptr, int *lend, int *lnew, int *lph)
10571 {
10572
10573 int i__1;
10574
10575
10576 static int i__, lp, nn, lpb, lpl, lpp, lnw;
10577
10578
10579
10580
10581
10582
10583
10584
10585
10586
10587
10588
10589
10590
10591
10592
10593
10594
10595
10596
10597
10598
10599
10600
10601
10602
10603
10604
10605
10606
10607
10608
10609
10610
10611
10612
10613
10614
10615
10616
10617
10618
10619
10620
10621
10622
10623
10624
10625
10626
10627
10628
10629
10630
10631
10632
10633
10634
10635
10636
10637
10638
10639
10640
10641
10642
10643
10644
10645
10646
10647
10648 --lend;
10649 --list;
10650 --lptr;
10651
10652
10653 nn = *n;
10654
10655
10656
10657 if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10658 *lph = -1;
10659 return 0;
10660 }
10661
10662
10663
10664
10665
10666
10667
10668 lpl = lend[*n0];
10669 lpp = lpl;
10670 lpb = lptr[lpp];
10671 L1:
10672 if (list[lpb] == *nb) {
10673 goto L2;
10674 }
10675 lpp = lpb;
10676 lpb = lptr[lpp];
10677 if (lpb != lpl) {
10678 goto L1;
10679 }
10680
10681
10682
10683 if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10684 *lph = -2;
10685 return 0;
10686 }
10687
10688
10689
10690
10691
10692 lend[*n0] = lpp;
10693 lp = lend[*nb];
10694 if (list[lp] < 0) {
10695 list[lpp] = -list[lpp];
10696 }
10697 goto L3;
10698
10699
10700
10701
10702
10703 L2:
10704 lp = lend[*nb];
10705 if (list[lp] < 0 && list[lpl] > 0) {
10706 lend[*n0] = lpp;
10707 list[lpp] = -list[lpp];
10708 }
10709
10710
10711
10712
10713 L3:
10714 lptr[lpp] = lptr[lpb];
10715 lnw = *lnew - 1;
10716 list[lpb] = list[lnw];
10717 lptr[lpb] = lptr[lnw];
10718 for (i__ = nn; i__ >= 1; --i__) {
10719 if (lend[i__] == lnw) {
10720 lend[i__] = lpb;
10721 goto L5;
10722 }
10723
10724 }
10725
10726 L5:
10727 i__1 = lnw - 1;
10728 for (i__ = 1; i__ <= i__1; ++i__) {
10729 if (lptr[i__] == lnw) {
10730 lptr[i__] = lpb;
10731 }
10732
10733 }
10734
10735
10736
10737 *lnew = lnw;
10738 *lph = lpb;
10739 return 0;
10740 }
10741
10742 int delnod_(int *k, int *n, double *x,
10743 double *y, double *z__, int *list, int *lptr, int
10744 *lend, int *lnew, int *lwk, int *iwk, int *ier)
10745 {
10746
10747 int i__1;
10748
10749
10750 static int i__, j, n1, n2;
10751 static double x1, x2, y1, y2, z1, z2;
10752 static int nl, lp, nn, nr;
10753 static double xl, yl, zl, xr, yr, zr;
10754 static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10755 extern long int left_(double *, double *, double *, double
10756 *, double *, double *, double *, double *,
10757 double *);
10758 static long int bdry;
10759 static int ierr, lwkl;
10760 extern int swap_(int *, int *, int *,
10761 int *, int *, int *, int *, int *), delnb_(
10762 int *, int *, int *, int *, int *, int *,
10763 int *, int *);
10764 extern int nbcnt_(int *, int *);
10765 extern int optim_(double *, double *, double
10766 *, int *, int *, int *, int *, int *, int
10767 *, int *);
10768 static int nfrst;
10769 extern int lstptr_(int *, int *, int *, int *);
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
10840
10841
10842
10843
10844
10845
10846
10847
10848
10849
10850
10851
10852
10853
10854
10855
10856
10857
10858
10859
10860
10861
10862
10863
10864
10865
10866
10867
10868
10869
10870
10871
10872
10873
10874
10875
10876
10877
10878
10879
10880
10881
10882
10883
10884
10885
10886
10887
10888
10889
10890
10891
10892
10893
10894
10895
10896
10897
10898
10899
10900
10901
10902
10903
10904
10905
10906
10907
10908
10909
10910
10911
10912
10913 iwk -= 3;
10914 --lend;
10915 --lptr;
10916 --list;
10917 --z__;
10918 --y;
10919 --x;
10920
10921
10922 n1 = *k;
10923 nn = *n;
10924 if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
10925 goto L21;
10926 }
10927 lpl = lend[n1];
10928 lpf = lptr[lpl];
10929 nnb = nbcnt_(&lpl, &lptr[1]);
10930 bdry = list[lpl] < 0;
10931 if (bdry) {
10932 ++nnb;
10933 }
10934 if (nnb < 3) {
10935 goto L23;
10936 }
10937 lwkl = *lwk;
10938 *lwk = nnb - 3;
10939 if (lwkl < *lwk) {
10940 goto L22;
10941 }
10942 iwl = 0;
10943 if (nnb == 3) {
10944 goto L3;
10945 }
10946
10947
10948
10949
10950
10951
10952
10953 x1 = x[n1];
10954 y1 = y[n1];
10955 z1 = z__[n1];
10956 nfrst = list[lpf];
10957 nr = nfrst;
10958 xr = x[nr];
10959 yr = y[nr];
10960 zr = z__[nr];
10961 lp = lptr[lpf];
10962 n2 = list[lp];
10963 x2 = x[n2];
10964 y2 = y[n2];
10965 z2 = z__[n2];
10966 lp = lptr[lp];
10967
10968
10969
10970 L1:
10971 nl = (i__1 = list[lp], abs(i__1));
10972 if (nl == nfrst && bdry) {
10973 goto L3;
10974 }
10975 xl = x[nl];
10976 yl = y[nl];
10977 zl = z__[nl];
10978
10979
10980
10981
10982
10983
10984 lpl2 = lend[n2];
10985 if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
10986 list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
10987 z2)))) {
10988
10989
10990
10991 nr = n2;
10992 xr = x2;
10993 yr = y2;
10994 zr = z2;
10995 goto L2;
10996 }
10997
10998
10999
11000
11001
11002
11003
11004
11005 swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11006 if (lp21 == 0) {
11007 nr = n2;
11008 xr = x2;
11009 yr = y2;
11010 zr = z2;
11011 goto L2;
11012 }
11013 ++iwl;
11014 if (nl <= n1) {
11015 iwk[(iwl << 1) + 1] = nl;
11016 } else {
11017 iwk[(iwl << 1) + 1] = nl - 1;
11018 }
11019 if (nr <= n1) {
11020 iwk[(iwl << 1) + 2] = nr;
11021 } else {
11022 iwk[(iwl << 1) + 2] = nr - 1;
11023 }
11024
11025
11026
11027 lpl = lend[n1];
11028 --nnb;
11029 if (nnb == 3) {
11030 goto L3;
11031 }
11032 lpf = lptr[lpl];
11033 nfrst = list[lpf];
11034 lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11035 if (nr == nfrst) {
11036 goto L2;
11037 }
11038
11039
11040
11041
11042
11043
11044
11045 n2 = nr;
11046 x2 = xr;
11047 y2 = yr;
11048 z2 = zr;
11049 lp21 = lptr[lp21];
11050 lp21 = lptr[lp21];
11051 nr = (i__1 = list[lp21], abs(i__1));
11052 xr = x[nr];
11053 yr = y[nr];
11054 zr = z__[nr];
11055 goto L1;
11056
11057
11058
11059 L2:
11060 if (n2 == nfrst) {
11061 goto L3;
11062 }
11063 n2 = nl;
11064 x2 = xl;
11065 y2 = yl;
11066 z2 = zl;
11067 lp = lptr[lp];
11068 goto L1;
11069
11070
11071
11072
11073
11074
11075
11076
11077
11078
11079 L3:
11080 if (! bdry) {
11081 if (nnb > 3) {
11082 bdry = TRUE_;
11083 } else {
11084 lpf = lptr[lpl];
11085 nr = list[lpf];
11086 lp = lptr[lpf];
11087 n2 = list[lp];
11088 nl = list[lpl];
11089 bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11090 x[n2], &y[n2], &z__[n2]);
11091 }
11092 if (bdry) {
11093
11094
11095
11096
11097
11098
11099 i__1 = nn;
11100 for (i__ = 1; i__ <= i__1; ++i__) {
11101 if (list[lend[i__]] < 0) {
11102 bdry = FALSE_;
11103 goto L5;
11104 }
11105
11106 }
11107 list[lpl] = -list[lpl];
11108 ++nnb;
11109 }
11110 }
11111 L5:
11112 if (! bdry && nnb > 3) {
11113 goto L24;
11114 }
11115
11116
11117
11118
11119 lp = lpl;
11120 lnw = *lnew;
11121
11122
11123
11124 L6:
11125 lp = lptr[lp];
11126 n2 = (i__1 = list[lp], abs(i__1));
11127 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11128 if (lph < 0) {
11129 goto L23;
11130 }
11131
11132
11133
11134 if (lpl == lnw) {
11135 lpl = lph;
11136 }
11137 if (lp == lnw) {
11138 lp = lph;
11139 }
11140 if (lp != lpl) {
11141 goto L6;
11142 }
11143
11144
11145
11146
11147
11148 --nn;
11149 if (n1 > nn) {
11150 goto L9;
11151 }
11152 i__1 = nn;
11153 for (i__ = n1; i__ <= i__1; ++i__) {
11154 x[i__] = x[i__ + 1];
11155 y[i__] = y[i__ + 1];
11156 z__[i__] = z__[i__ + 1];
11157 lend[i__] = lend[i__ + 1];
11158
11159 }
11160
11161 i__1 = lnw - 1;
11162 for (i__ = 1; i__ <= i__1; ++i__) {
11163 if (list[i__] > n1) {
11164 --list[i__];
11165 }
11166 if (list[i__] < -n1) {
11167 ++list[i__];
11168 }
11169
11170 }
11171
11172
11173
11174
11175
11176
11177
11178
11179
11180
11181
11182 L9:
11183 if (bdry) {
11184 --nnb;
11185 }
11186 lpn = lpl;
11187 i__1 = nnb;
11188 for (j = 1; j <= i__1; ++j) {
11189 --lnw;
11190 lp = lpn;
11191 lpn = lptr[lp];
11192 list[lp] = list[lnw];
11193 lptr[lp] = lptr[lnw];
11194 if (lptr[lpn] == lnw) {
11195 lptr[lpn] = lp;
11196 }
11197 if (lpn == lnw) {
11198 lpn = lp;
11199 }
11200 for (i__ = nn; i__ >= 1; --i__) {
11201 if (lend[i__] == lnw) {
11202 lend[i__] = lp;
11203 goto L11;
11204 }
11205
11206 }
11207
11208 L11:
11209 for (i__ = lnw - 1; i__ >= 1; --i__) {
11210 if (lptr[i__] == lnw) {
11211 lptr[i__] = lp;
11212 }
11213
11214 }
11215
11216 }
11217
11218
11219
11220
11221
11222 *n = nn;
11223 *lnew = lnw;
11224 if (iwl > 0) {
11225 nit = iwl << 2;
11226 optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11227 nit, &iwk[3], &ierr);
11228 if (ierr != 0 && ierr != 1) {
11229 goto L25;
11230 }
11231 if (ierr == 1) {
11232 goto L26;
11233 }
11234 }
11235
11236
11237
11238 *ier = 0;
11239 return 0;
11240
11241
11242
11243 L21:
11244 *ier = 1;
11245 return 0;
11246
11247
11248
11249 L22:
11250 *ier = 2;
11251 return 0;
11252
11253
11254
11255
11256 L23:
11257 *ier = 3;
11258 return 0;
11259
11260
11261
11262 L24:
11263 *ier = 4;
11264 return 0;
11265
11266
11267
11268 L25:
11269 *ier = 5;
11270
11271
11272
11273 return 0;
11274
11275
11276
11277 L26:
11278 *ier = 6;
11279 return 0;
11280 }
11281
11282 int drwarc_(int *, double *p, double *q,
11283 double *tol, int *nseg)
11284 {
11285
11286 int i__1;
11287 double d__1;
11288
11289
11290
11291
11292
11293 static int i__, k;
11294 static double s, p1[3], p2[3], u1, u2, v1, v2;
11295 static int na;
11296 static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11297
11298
11299
11300
11301
11302
11303
11304
11305
11306
11307
11308
11309
11310
11311
11312
11313
11314
11315
11316
11317
11318
11319
11320
11321
11322
11323
11324
11325
11326
11327
11328
11329
11330
11331
11332
11333
11334
11335
11336
11337
11338
11339
11340
11341
11342
11343
11344
11345
11346
11347
11348
11349
11350
11351
11352
11353
11354
11355
11356
11357
11358
11359
11360
11361
11362
11363
11364
11365
11366
11367
11368
11369
11370
11371
11372
11373
11374 --q;
11375 --p;
11376
11377
11378 enrm = 0.;
11379 for (i__ = 1; i__ <= 3; ++i__) {
11380 pm[i__ - 1] = p[i__] + q[i__];
11381 enrm += pm[i__ - 1] * pm[i__ - 1];
11382
11383 }
11384 if (enrm == 0.) {
11385 goto L5;
11386 }
11387 enrm = sqrt(enrm);
11388 pm[0] /= enrm;
11389 pm[1] /= enrm;
11390 pm[2] /= enrm;
11391
11392
11393
11394
11395 u1 = p[1];
11396 v1 = p[2];
11397 u2 = q[1];
11398 v2 = q[2];
11399 um = pm[0];
11400 vm = pm[1];
11401
11402
11403
11404
11405
11406
11407 du = u2 - u1;
11408 dv = v2 - v1;
11409 enrm = du * du + dv * dv;
11410 if (enrm == 0.) {
11411 goto L5;
11412 }
11413 err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11414
11415
11416
11417
11418
11419 na = (int) (err / *tol + 1.);
11420
11421
11422
11423
11424
11425 s = 1. / (double) na;
11426 for (i__ = 1; i__ <= 3; ++i__) {
11427 dp[i__ - 1] = s * (q[i__] - p[i__]);
11428 pm[i__ - 1] = p[i__];
11429 p1[i__ - 1] = p[i__];
11430
11431 }
11432
11433
11434
11435
11436 i__1 = na - 1;
11437 for (k = 1; k <= i__1; ++k) {
11438 enrm = 0.;
11439 for (i__ = 1; i__ <= 3; ++i__) {
11440 pm[i__ - 1] += dp[i__ - 1];
11441 enrm += pm[i__ - 1] * pm[i__ - 1];
11442
11443 }
11444 if (enrm == 0.) {
11445 goto L5;
11446 }
11447 enrm = sqrt(enrm);
11448 p2[0] = pm[0] / enrm;
11449 p2[1] = pm[1] / enrm;
11450 p2[2] = pm[2] / enrm;
11451
11452
11453 p1[0] = p2[0];
11454 p1[1] = p2[1];
11455 p1[2] = p2[2];
11456
11457 }
11458
11459
11460
11461
11462 *nseg = na;
11463 return 0;
11464
11465
11466
11467 L5:
11468 *nseg = 0;
11469 return 0;
11470 }
11471
11472 int edge_(int *in1, int *in2, double *x,
11473 double *y, double *z__, int *lwk, int *iwk, int *
11474 list, int *lptr, int *lend, int *ier)
11475 {
11476
11477 int i__1;
11478
11479
11480 static int i__, n0, n1, n2;
11481 static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11482 static int nl, lp, nr;
11483 static double dp12;
11484 static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11485 static double dp1l, dp2l, dp1r, dp2r;
11486 extern long int left_(double *, double *, double *, double
11487 *, double *, double *, double *, double *,
11488 double *);
11489 static int ierr;
11490 extern int swap_(int *, int *, int *,
11491 int *, int *, int *, int *, int *);
11492 static int next, iwcp1, n1lst, iwend;
11493 extern int optim_(double *, double *, double
11494 *, int *, int *, int *, int *, int *, int
11495 *, int *);
11496 static int n1frst;
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
11561
11562
11563
11564
11565
11566
11567
11568
11569
11570
11571
11572
11573
11574
11575
11576
11577
11578
11579
11580
11581
11582
11583
11584
11585
11586
11587
11588
11589
11590
11591
11592
11593
11594
11595
11596
11597
11598
11599
11600
11601
11602
11603
11604
11605
11606
11607
11608
11609
11610
11611
11612
11613
11614
11615
11616
11617
11618
11619
11620
11621
11622
11623
11624
11625
11626
11627
11628
11629
11630
11631
11632
11633
11634 --lend;
11635 --lptr;
11636 --list;
11637 iwk -= 3;
11638 --z__;
11639 --y;
11640 --x;
11641
11642
11643 n1 = *in1;
11644 n2 = *in2;
11645 iwend = *lwk;
11646 if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11647 goto L31;
11648 }
11649
11650
11651
11652
11653 lpl = lend[n1];
11654 n0 = (i__1 = list[lpl], abs(i__1));
11655 lp = lpl;
11656 L1:
11657 if (n0 == n2) {
11658 goto L30;
11659 }
11660 lp = lptr[lp];
11661 n0 = list[lp];
11662 if (lp != lpl) {
11663 goto L1;
11664 }
11665
11666
11667
11668 iwl = 0;
11669 nit = 0;
11670
11671
11672
11673 L2:
11674 x1 = x[n1];
11675 y1 = y[n1];
11676 z1 = z__[n1];
11677 x2 = x[n2];
11678 y2 = y[n2];
11679 z2 = z__[n2];
11680
11681
11682
11683
11684
11685
11686
11687
11688
11689
11690 lpl = lend[n1];
11691 n1lst = list[lpl];
11692 lp = lptr[lpl];
11693 n1frst = list[lp];
11694 nl = n1frst;
11695 if (n1lst < 0) {
11696 goto L4;
11697 }
11698
11699
11700
11701
11702 L3:
11703 if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11704 goto L4;
11705 }
11706 lp = lptr[lp];
11707 nl = list[lp];
11708 if (nl != n1frst) {
11709 goto L3;
11710 }
11711
11712
11713
11714 goto L5;
11715
11716
11717
11718
11719 L4:
11720 nr = nl;
11721 lp = lptr[lp];
11722 nl = (i__1 = list[lp], abs(i__1));
11723 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11724
11725
11726
11727
11728
11729 dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11730 dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11731 dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11732 dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11733 dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11734 if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11735 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11736 goto L6;
11737 }
11738
11739
11740
11741
11742
11743 if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11744 goto L5;
11745 }
11746 }
11747
11748
11749
11750 if (nl != n1frst) {
11751 goto L4;
11752 }
11753
11754
11755
11756
11757
11758
11759
11760 L5:
11761 if (nit > 0) {
11762 goto L33;
11763 }
11764 nit = 1;
11765 n1 = n2;
11766 n2 = *in1;
11767 goto L2;
11768
11769
11770
11771
11772 L6:
11773 ++iwl;
11774 if (iwl > iwend) {
11775 goto L32;
11776 }
11777 iwk[(iwl << 1) + 1] = nl;
11778 iwk[(iwl << 1) + 2] = nr;
11779
11780
11781
11782 lpl = lend[nl];
11783 lp = lptr[lpl];
11784
11785
11786
11787
11788 L7:
11789 if (list[lp] == nr) {
11790 goto L8;
11791 }
11792 lp = lptr[lp];
11793 if (lp != lpl) {
11794 goto L7;
11795 }
11796
11797
11798
11799
11800 if (list[lp] != nr) {
11801 goto L33;
11802 }
11803
11804
11805
11806
11807 L8:
11808 lp = lptr[lp];
11809 next = (i__1 = list[lp], abs(i__1));
11810 if (next == n2) {
11811 goto L9;
11812 }
11813
11814
11815
11816 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11817 nl = next;
11818 } else {
11819 nr = next;
11820 }
11821 goto L6;
11822
11823
11824
11825
11826 L9:
11827 *lwk = iwl;
11828 iwend = iwl;
11829
11830
11831
11832
11833
11834
11835
11836
11837 iwf = 1;
11838
11839
11840
11841
11842
11843 L10:
11844 lft = 0;
11845 n0 = n1;
11846 x0 = x1;
11847 y0 = y1;
11848 z0 = z1;
11849 nl = iwk[(iwf << 1) + 1];
11850 nr = iwk[(iwf << 1) + 2];
11851 iwc = iwf;
11852
11853
11854
11855
11856 L11:
11857 if (iwc == iwl) {
11858 goto L21;
11859 }
11860 iwcp1 = iwc + 1;
11861 next = iwk[(iwcp1 << 1) + 1];
11862 if (next != nl) {
11863 goto L16;
11864 }
11865 next = iwk[(iwcp1 << 1) + 2];
11866
11867
11868
11869
11870 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11871 z__[next])) {
11872 goto L14;
11873 }
11874 if (lft >= 0) {
11875 goto L12;
11876 }
11877 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11878 z__[next])) {
11879 goto L14;
11880 }
11881
11882
11883
11884 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11885 iwk[(iwc << 1) + 1] = n0;
11886 iwk[(iwc << 1) + 2] = next;
11887 goto L15;
11888
11889
11890
11891
11892
11893 L12:
11894 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11895 i__1 = iwl;
11896 for (i__ = iwcp1; i__ <= i__1; ++i__) {
11897 iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11898 iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11899
11900 }
11901 iwk[(iwl << 1) + 1] = n0;
11902 iwk[(iwl << 1) + 2] = next;
11903 --iwl;
11904 nr = next;
11905 goto L11;
11906
11907
11908
11909 L14:
11910 n0 = nr;
11911 x0 = x[n0];
11912 y0 = y[n0];
11913 z0 = z__[n0];
11914 lft = 1;
11915
11916
11917
11918 L15:
11919 nr = next;
11920 ++iwc;
11921 goto L11;
11922
11923
11924
11925
11926 L16:
11927 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11928 z__[next])) {
11929 goto L19;
11930 }
11931 if (lft <= 0) {
11932 goto L17;
11933 }
11934 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11935 z__[next])) {
11936 goto L19;
11937 }
11938
11939
11940
11941 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11942 iwk[(iwc << 1) + 1] = next;
11943 iwk[(iwc << 1) + 2] = n0;
11944 goto L20;
11945
11946
11947
11948
11949
11950 L17:
11951 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11952 i__1 = iwf;
11953 for (i__ = iwc - 1; i__ >= i__1; --i__) {
11954 iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11955 iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11956
11957 }
11958 iwk[(iwf << 1) + 1] = n0;
11959 iwk[(iwf << 1) + 2] = next;
11960 ++iwf;
11961 goto L20;
11962
11963
11964
11965 L19:
11966 n0 = nl;
11967 x0 = x[n0];
11968 y0 = y[n0];
11969 z0 = z__[n0];
11970 lft = -1;
11971
11972
11973
11974 L20:
11975 nl = next;
11976 ++iwc;
11977 goto L11;
11978
11979
11980
11981 L21:
11982 if (n0 == n1) {
11983 goto L24;
11984 }
11985 if (lft < 0) {
11986 goto L22;
11987 }
11988
11989
11990
11991 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
11992 goto L10;
11993 }
11994
11995
11996
11997
11998 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11999 iwk[(iwl << 1) + 1] = n0;
12000 iwk[(iwl << 1) + 2] = n2;
12001 --iwl;
12002 goto L10;
12003
12004
12005
12006 L22:
12007 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12008 goto L10;
12009 }
12010
12011
12012
12013
12014 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12015 i__ = iwl;
12016 L23:
12017 iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12018 iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12019 --i__;
12020 if (i__ > iwf) {
12021 goto L23;
12022 }
12023 iwk[(iwf << 1) + 1] = n0;
12024 iwk[(iwf << 1) + 2] = n2;
12025 ++iwf;
12026 goto L10;
12027
12028
12029
12030
12031 L24:
12032 swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12033 iwk[(iwc << 1) + 1] = 0;
12034 iwk[(iwc << 1) + 2] = 0;
12035
12036
12037
12038 *ier = 0;
12039 if (iwc > 1) {
12040
12041
12042
12043 nit = iwc - (1<<2);
12044 i__1 = iwc - 1;
12045 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12046 nit, &iwk[3], &ierr);
12047 if (ierr != 0 && ierr != 1) {
12048 goto L34;
12049 }
12050 if (ierr == 1) {
12051 *ier = 5;
12052 }
12053 }
12054 if (iwc < iwend) {
12055
12056
12057
12058 nit = iwend - (iwc<<2);
12059 i__1 = iwend - iwc;
12060 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12061 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12062 if (ierr != 0 && ierr != 1) {
12063 goto L34;
12064 }
12065 if (ierr == 1) {
12066 goto L35;
12067 }
12068 }
12069 if (*ier == 5) {
12070 goto L35;
12071 }
12072
12073
12074
12075 return 0;
12076
12077
12078
12079 L30:
12080 *ier = 0;
12081 return 0;
12082
12083
12084
12085 L31:
12086 *ier = 1;
12087 return 0;
12088
12089
12090
12091 L32:
12092 *ier = 2;
12093 return 0;
12094
12095
12096
12097
12098 L33:
12099 *ier = 3;
12100
12101
12102
12103
12104 return 0;
12105
12106
12107
12108 L34:
12109 *ier = 4;
12110
12111
12112
12113 return 0;
12114
12115
12116
12117 L35:
12118 *ier = 5;
12119 return 0;
12120 }
12121
12122 int getnp_(double *x, double *y, double *z__,
12123 int *list, int *lptr, int *lend, int *l, int *
12124 npts, double *df, int *ier)
12125 {
12126
12127 int i__1, i__2;
12128
12129
12130 static int i__, n1;
12131 static double x1, y1, z1;
12132 static int nb, ni, lp, np, lm1;
12133 static double dnb, dnp;
12134 static int lpl;
12135
12136
12137
12138
12139
12140
12141
12142
12143
12144
12145
12146
12147
12148
12149
12150
12151
12152
12153
12154
12155
12156
12157
12158
12159
12160
12161
12162
12163
12164
12165
12166
12167
12168
12169
12170
12171
12172
12173
12174
12175
12176
12177
12178
12179
12180
12181
12182
12183
12184
12185
12186
12187
12188
12189
12190
12191
12192
12193
12194
12195
12196
12197
12198
12199
12200
12201
12202
12203
12204
12205
12206
12207
12208
12209
12210
12211
12212
12213
12214 --x;
12215 --y;
12216 --z__;
12217 --list;
12218 --lptr;
12219 --lend;
12220 --npts;
12221
12222
12223 lm1 = *l - 1;
12224 if (lm1 < 1) {
12225 goto L6;
12226 }
12227 *ier = 0;
12228
12229
12230
12231 n1 = npts[1];
12232 x1 = x[n1];
12233 y1 = y[n1];
12234 z1 = z__[n1];
12235 i__1 = lm1;
12236 for (i__ = 1; i__ <= i__1; ++i__) {
12237 ni = npts[i__];
12238 lend[ni] = -lend[ni];
12239
12240 }
12241
12242
12243
12244
12245
12246 dnp = 2.;
12247
12248
12249
12250 i__1 = lm1;
12251 for (i__ = 1; i__ <= i__1; ++i__) {
12252 ni = npts[i__];
12253 lpl = -lend[ni];
12254 lp = lpl;
12255
12256
12257
12258 L2:
12259 nb = (i__2 = list[lp], abs(i__2));
12260 if (lend[nb] < 0) {
12261 goto L3;
12262 }
12263
12264
12265
12266
12267 dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12268 if (dnb >= dnp) {
12269 goto L3;
12270 }
12271 np = nb;
12272 dnp = dnb;
12273 L3:
12274 lp = lptr[lp];
12275 if (lp != lpl) {
12276 goto L2;
12277 }
12278
12279 }
12280 npts[*l] = np;
12281 *df = dnp;
12282
12283
12284
12285 i__1 = lm1;
12286 for (i__ = 1; i__ <= i__1; ++i__) {
12287 ni = npts[i__];
12288 lend[ni] = -lend[ni];
12289
12290 }
12291 return 0;
12292
12293
12294
12295 L6:
12296 *ier = 1;
12297 return 0;
12298 }
12299
12300 int insert_(int *k, int *lp, int *list, int *
12301 lptr, int *lnew)
12302 {
12303 static int lsav;
12304
12305
12306
12307
12308
12309
12310
12311
12312
12313
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 --lptr;
12348 --list;
12349
12350
12351 lsav = lptr[*lp];
12352 lptr[*lp] = *lnew;
12353 list[*lnew] = *k;
12354 lptr[*lnew] = lsav;
12355 ++(*lnew);
12356 return 0;
12357 }
12358
12359 long int inside_(double *p, int *lv, double *xv, double *yv,
12360 double *zv, int *nv, int *listv, int *ier)
12361 {
12362
12363
12364 static double eps = .001;
12365
12366
12367 int i__1;
12368 long int ret_val = 0;
12369
12370
12371
12372
12373
12374 static double b[3], d__;
12375 static int k, n;
12376 static double q[3];
12377 static int i1, i2, k0;
12378 static double v1[3], v2[3], cn[3], bp, bq;
12379 static int ni;
12380 static double pn[3], qn[3], vn[3];
12381 static int imx;
12382 static long int lft1, lft2, even;
12383 static int ierr;
12384 static long int pinr, qinr;
12385 static double qnrm, vnrm;
12386 extern int intrsc_(double *, double *,
12387 double *, double *, int *);
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
12467
12468
12469
12470
12471
12472
12473
12474
12475
12476
12477
12478
12479
12480
12481
12482
12483
12484
12485
12486
12487
12488
12489
12490
12491
12492
12493
12494
12495
12496
12497
12498
12499
12500
12501
12502
12503
12504
12505
12506
12507
12508
12509
12510
12511
12512
12513
12514
12515
12516
12517
12518
12519
12520
12521
12522
12523
12524
12525
12526
12527
12528
12529
12530
12531
12532
12533
12534
12535
12536
12537
12538
12539
12540 --p;
12541 --zv;
12542 --yv;
12543 --xv;
12544 --listv;
12545
12546
12547
12548
12549
12550
12551 imx = *lv;
12552 n = *nv;
12553 if (n < 3 || n > imx) {
12554 goto L11;
12555 }
12556 k0 = 0;
12557 i1 = listv[1];
12558 if (i1 < 1 || i1 > imx) {
12559 goto L12;
12560 }
12561
12562
12563
12564
12565
12566 L1:
12567 ++k0;
12568 if (k0 > n) {
12569 goto L14;
12570 }
12571 i1 = listv[k0];
12572 if (k0 < n) {
12573 i2 = listv[k0 + 1];
12574 } else {
12575 i2 = listv[1];
12576 }
12577 if (i2 < 1 || i2 > imx) {
12578 goto L12;
12579 }
12580 vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12581 vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12582 vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12583 vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12584 if (vnrm == 0.) {
12585 goto L1;
12586 }
12587 q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12588 q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12589 q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12590 qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12591 q[0] /= qnrm;
12592 q[1] /= qnrm;
12593 q[2] /= qnrm;
12594
12595
12596
12597 cn[0] = q[1] * p[3] - q[2] * p[2];
12598 cn[1] = q[2] * p[1] - q[0] * p[3];
12599 cn[2] = q[0] * p[2] - q[1] * p[1];
12600 if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12601 goto L1;
12602 }
12603 pn[0] = p[2] * cn[2] - p[3] * cn[1];
12604 pn[1] = p[3] * cn[0] - p[1] * cn[2];
12605 pn[2] = p[1] * cn[1] - p[2] * cn[0];
12606 qn[0] = cn[1] * q[2] - cn[2] * q[1];
12607 qn[1] = cn[2] * q[0] - cn[0] * q[2];
12608 qn[2] = cn[0] * q[1] - cn[1] * q[0];
12609
12610
12611
12612 ni = 0;
12613 even = TRUE_;
12614 bp = -2.;
12615 bq = -2.;
12616 pinr = TRUE_;
12617 qinr = TRUE_;
12618 i2 = listv[n];
12619 if (i2 < 1 || i2 > imx) {
12620 goto L12;
12621 }
12622 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12623
12624
12625
12626 i__1 = n;
12627 for (k = 1; k <= i__1; ++k) {
12628 i1 = i2;
12629 lft1 = lft2;
12630 i2 = listv[k];
12631 if (i2 < 1 || i2 > imx) {
12632 goto L12;
12633 }
12634 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12635 if (lft1 == lft2) {
12636 goto L2;
12637 }
12638
12639
12640
12641
12642 ++ni;
12643 v1[0] = xv[i1];
12644 v1[1] = yv[i1];
12645 v1[2] = zv[i1];
12646 v2[0] = xv[i2];
12647 v2[1] = yv[i2];
12648 v2[2] = zv[i2];
12649 intrsc_(v1, v2, cn, b, &ierr);
12650
12651
12652
12653
12654
12655 if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12656 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12657
12658
12659
12660 even = ! even;
12661 d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12662 if (d__ > bq) {
12663 bq = d__;
12664 qinr = lft2;
12665 }
12666 d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12667 if (d__ > bp) {
12668 bp = d__;
12669 pinr = lft1;
12670 }
12671 }
12672 L2:
12673 ;
12674 }
12675
12676
12677
12678
12679 if (ni != ni / 2 << 1 || ! qinr) {
12680 goto L1;
12681 }
12682
12683
12684
12685 if (pinr != even) {
12686 goto L13;
12687 }
12688
12689
12690
12691 *ier = 0;
12692 ret_val = even;
12693 return ret_val;
12694
12695
12696
12697 L11:
12698 *ier = 1;
12699 return ret_val;
12700
12701
12702
12703 L12:
12704 *ier = 2;
12705 return ret_val;
12706
12707
12708
12709 L13:
12710 *ier = 3;
12711 return ret_val;
12712
12713
12714
12715 L14:
12716 *ier = 4;
12717 return ret_val;
12718 }
12719
12720 int intadd_(int *kk, int *i1, int *i2, int *
12721 i3, int *list, int *lptr, int *lend, int *lnew)
12722 {
12723 static int k, n1, n2, n3, lp;
12724 extern int insert_(int *, int *, int *,
12725 int *, int *);
12726 extern int lstptr_(int *, int *, int *, int *);
12727
12728
12729
12730
12731
12732
12733
12734
12735
12736
12737
12738
12739
12740
12741
12742
12743
12744
12745
12746
12747
12748
12749
12750
12751
12752
12753
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 --lend;
12785 --lptr;
12786 --list;
12787
12788
12789 k = *kk;
12790
12791
12792
12793 n1 = *i1;
12794 n2 = *i2;
12795 n3 = *i3;
12796
12797
12798
12799 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12800 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12801 lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12802 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12803 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12804 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12805
12806
12807
12808 list[*lnew] = n1;
12809 list[*lnew + 1] = n2;
12810 list[*lnew + 2] = n3;
12811 lptr[*lnew] = *lnew + 1;
12812 lptr[*lnew + 1] = *lnew + 2;
12813 lptr[*lnew + 2] = *lnew;
12814 lend[k] = *lnew + 2;
12815 *lnew += 3;
12816 return 0;
12817 }
12818
12819 int intrsc_(double *p1, double *p2, double *cn,
12820 double *p, int *ier)
12821 {
12822
12823
12824
12825
12826 static int i__;
12827 static double t, d1, d2, pp[3], ppn;
12828
12829
12830
12831
12832
12833
12834
12835
12836
12837
12838
12839
12840
12841
12842
12843
12844
12845
12846
12847
12848
12849
12850
12851
12852
12853
12854
12855
12856
12857
12858
12859
12860
12861
12862
12863
12864
12865
12866
12867
12868
12869
12870
12871
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 --p;
12898 --cn;
12899 --p2;
12900 --p1;
12901
12902
12903 d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
12904 d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
12905
12906 if (d1 == d2) {
12907 *ier = 1;
12908 return 0;
12909 }
12910
12911
12912
12913 t = d1 / (d1 - d2);
12914 ppn = 0.;
12915 for (i__ = 1; i__ <= 3; ++i__) {
12916 pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
12917 ppn += pp[i__ - 1] * pp[i__ - 1];
12918
12919 }
12920
12921
12922
12923 if (ppn == 0.) {
12924 *ier = 2;
12925 return 0;
12926 }
12927 ppn = sqrt(ppn);
12928
12929
12930
12931 for (i__ = 1; i__ <= 3; ++i__) {
12932 p[i__] = pp[i__ - 1] / ppn;
12933
12934 }
12935 *ier = 0;
12936 return 0;
12937 }
12938
12939 int jrand_(int *n, int *ix, int *iy, int *iz)
12940 {
12941
12942 int ret_val;
12943
12944
12945 static float u, x;
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
12980
12981
12982
12983
12984
12985
12986
12987
12988
12989
12990
12991
12992
12993
12994
12995
12996
12997
12998
12999 *ix = *ix * 171 % 30269;
13000 *iy = *iy * 172 % 30307;
13001 *iz = *iz * 170 % 30323;
13002 x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13003 30323.f;
13004 u = x - (int) x;
13005 ret_val = (int) ((float) (*n) * u + 1.f);
13006 return ret_val;
13007 }
13008
13009 long int left_(double *x1, double *y1, double *z1, double *x2,
13010 double *y2, double *z2, double *x0, double *y0,
13011 double *z0)
13012 {
13013
13014 long int ret_val;
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
13041
13042
13043
13044
13045
13046
13047
13048
13049
13050
13051
13052
13053 ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13054 *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13055
13056
13057 return ret_val;
13058 }
13059
13060 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13061 {
13062
13063 int ret_val;
13064
13065
13066 static int nd, 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
13110
13111
13112
13113
13114 --lptr;
13115 --list;
13116
13117
13118 lp = lptr[*lpl];
13119 L1:
13120 nd = list[lp];
13121 if (nd == *nb) {
13122 goto L2;
13123 }
13124 lp = lptr[lp];
13125 if (lp != *lpl) {
13126 goto L1;
13127 }
13128
13129 L2:
13130 ret_val = lp;
13131 return ret_val;
13132 }
13133
13134 int nbcnt_(int *lpl, int *lptr)
13135 {
13136
13137 int ret_val;
13138
13139
13140 static int k, lp;
13141
13142
13143
13144
13145
13146
13147
13148
13149
13150
13151
13152
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 --lptr;
13184
13185
13186 lp = *lpl;
13187 k = 1;
13188
13189 L1:
13190 lp = lptr[lp];
13191 if (lp == *lpl) {
13192 goto L2;
13193 }
13194 ++k;
13195 goto L1;
13196
13197 L2:
13198 ret_val = k;
13199 return ret_val;
13200 }
13201
13202 int nearnd_(double *p, int *ist, int *n, double *x,
13203 double *y, double *z__, int *list, int *lptr, int
13204 *lend, double *al)
13205 {
13206
13207 int ret_val, i__1;
13208
13209
13210
13211
13212
13213 static int l;
13214 static double b1, b2, b3;
13215 static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13216 static double ds1;
13217 static int lp1, lp2;
13218 static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13219 static int lpl;
13220 static double dsr;
13221 static int nst, listp[25], lptrp[25];
13222 extern int trfind_(int *, double *, int *,
13223 double *, double *, double *, int *, int *,
13224 int *, double *, double *, double *, int *,
13225 int *, int *);
13226 extern int lstptr_(int *, int *, int *, int *);
13227
13228
13229
13230
13231
13232
13233
13234
13235
13236
13237
13238
13239
13240
13241
13242
13243
13244
13245
13246
13247
13248
13249
13250
13251
13252
13253
13254
13255
13256
13257
13258
13259
13260
13261
13262
13263
13264
13265
13266
13267
13268
13269
13270
13271
13272
13273
13274
13275
13276
13277
13278
13279
13280
13281
13282
13283
13284
13285
13286
13287
13288
13289
13290
13291
13292
13293
13294
13295
13296
13297
13298
13299
13300
13301
13302
13303
13304
13305
13306
13307
13308
13309
13310
13311
13312
13313
13314
13315
13316
13317
13318
13319
13320
13321
13322
13323 --p;
13324 --lend;
13325 --z__;
13326 --y;
13327 --x;
13328 --list;
13329 --lptr;
13330
13331
13332 nn = *n;
13333 if (nn < 3) {
13334 goto L6;
13335 }
13336 nst = *ist;
13337 if (nst < 1 || nst > nn) {
13338 nst = 1;
13339 }
13340
13341
13342
13343
13344
13345 trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13346 1], &b1, &b2, &b3, &i1, &i2, &i3);
13347
13348
13349
13350 if (i1 == 0) {
13351 goto L6;
13352 }
13353
13354
13355
13356
13357
13358
13359
13360 if (i3 != 0) {
13361 listp[0] = i1;
13362 lptrp[0] = 2;
13363 listp[1] = i2;
13364 lptrp[1] = 3;
13365 listp[2] = i3;
13366 lptrp[2] = 1;
13367 l = 3;
13368 } else {
13369 n1 = i1;
13370 l = 1;
13371 lp1 = 2;
13372 listp[l - 1] = n1;
13373 lptrp[l - 1] = lp1;
13374
13375
13376
13377
13378 L1:
13379 lpl = lend[n1];
13380 n1 = -list[lpl];
13381 l = lp1;
13382 lp1 = l + 1;
13383 listp[l - 1] = n1;
13384 lptrp[l - 1] = lp1;
13385 if (n1 != i2 && lp1 < 25) {
13386 goto L1;
13387 }
13388 l = lp1;
13389 listp[l - 1] = 0;
13390 lptrp[l - 1] = 1;
13391 }
13392
13393
13394
13395
13396
13397
13398 lp2 = 1;
13399 n2 = i1;
13400 lp1 = lptrp[0];
13401 n1 = listp[lp1 - 1];
13402
13403
13404
13405 L2:
13406 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13407 if (list[lp] < 0) {
13408 goto L3;
13409 }
13410 lp = lptr[lp];
13411 n3 = (i__1 = list[lp], abs(i__1));
13412
13413
13414
13415 if (l == 25) {
13416 goto L4;
13417 }
13418 dx1 = x[n1] - p[1];
13419 dy1 = y[n1] - p[2];
13420 dz1 = z__[n1] - p[3];
13421
13422 dx2 = x[n2] - p[1];
13423 dy2 = y[n2] - p[2];
13424 dz2 = z__[n2] - p[3];
13425
13426 dx3 = x[n3] - p[1];
13427 dy3 = y[n3] - p[2];
13428 dz3 = z__[n3] - p[3];
13429 if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13430 (dx2 * dy1 - dx1 * dy2) <= 0.) {
13431 goto L3;
13432 }
13433
13434
13435
13436
13437 ++l;
13438 lptrp[lp2 - 1] = l;
13439 listp[l - 1] = n3;
13440 lptrp[l - 1] = lp1;
13441 lp1 = l;
13442 n1 = n3;
13443 goto L2;
13444
13445
13446
13447
13448 L3:
13449 if (lp1 == 1) {
13450 goto L4;
13451 }
13452 lp2 = lp1;
13453 n2 = n1;
13454 lp1 = lptrp[lp1 - 1];
13455 n1 = listp[lp1 - 1];
13456 if (n1 == 0) {
13457 goto L4;
13458 }
13459 goto L2;
13460
13461
13462
13463
13464
13465 L4:
13466 nr = i1;
13467 dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13468 i__1 = l;
13469 for (lp = 2; lp <= i__1; ++lp) {
13470 n1 = listp[lp - 1];
13471 if (n1 == 0) {
13472 goto L5;
13473 }
13474 ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13475 if (ds1 < dsr) {
13476 nr = n1;
13477 dsr = ds1;
13478 }
13479 L5:
13480 ;
13481 }
13482 dsr = -dsr;
13483 if (dsr > 1.) {
13484 dsr = 1.;
13485 }
13486 *al = acos(dsr);
13487 ret_val = nr;
13488 return ret_val;
13489
13490
13491
13492 L6:
13493 ret_val = 0;
13494 return ret_val;
13495 }
13496
13497 int optim_(double *x, double *y, double *z__,
13498 int *na, int *list, int *lptr, int *lend, int *
13499 nit, int *iwk, int *ier)
13500 {
13501
13502 int i__1, i__2;
13503
13504
13505 static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13506 static long int swp;
13507 static int iter;
13508 extern int swap_(int *, int *, int *,
13509 int *, int *, int *, int *, int *);
13510 static int maxit;
13511 extern long int swptst_(int *, int *, int *, int *,
13512 double *, double *, double *);
13513
13514
13515
13516
13517
13518
13519
13520
13521
13522
13523
13524
13525
13526
13527
13528
13529
13530
13531
13532
13533
13534
13535
13536
13537
13538
13539
13540
13541
13542
13543
13544
13545
13546
13547
13548
13549
13550
13551
13552
13553
13554
13555
13556
13557
13558
13559
13560
13561
13562
13563
13564
13565
13566
13567
13568
13569
13570
13571
13572
13573
13574
13575
13576
13577
13578
13579
13580
13581
13582
13583
13584
13585
13586
13587
13588
13589
13590
13591
13592
13593
13594
13595
13596
13597
13598
13599
13600
13601
13602
13603
13604
13605
13606
13607
13608
13609 --x;
13610 --y;
13611 --z__;
13612 iwk -= 3;
13613 --list;
13614 --lptr;
13615 --lend;
13616
13617
13618 nna = *na;
13619 maxit = *nit;
13620 if (nna < 0 || maxit < 1) {
13621 goto L7;
13622 }
13623
13624
13625
13626 iter = 0;
13627 if (nna == 0) {
13628 goto L5;
13629 }
13630
13631
13632
13633
13634 L1:
13635 if (iter == maxit) {
13636 goto L6;
13637 }
13638 ++iter;
13639 swp = FALSE_;
13640
13641
13642
13643 i__1 = nna;
13644 for (i__ = 1; i__ <= i__1; ++i__) {
13645 io1 = iwk[(i__ << 1) + 1];
13646 io2 = iwk[(i__ << 1) + 2];
13647
13648
13649
13650
13651
13652
13653
13654
13655 lpl = lend[io1];
13656 lpp = lpl;
13657 lp = lptr[lpp];
13658 L2:
13659 if (list[lp] == io2) {
13660 goto L3;
13661 }
13662 lpp = lp;
13663 lp = lptr[lpp];
13664 if (lp != lpl) {
13665 goto L2;
13666 }
13667
13668
13669
13670
13671
13672 if ((i__2 = list[lp], abs(i__2)) != io2) {
13673 goto L8;
13674 }
13675 if (list[lp] < 0) {
13676 goto L4;
13677 }
13678
13679
13680
13681
13682 L3:
13683 n2 = list[lpp];
13684 if (n2 < 0) {
13685 goto L4;
13686 }
13687 lp = lptr[lp];
13688 n1 = (i__2 = list[lp], abs(i__2));
13689
13690
13691
13692 if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13693 goto L4;
13694 }
13695 swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13696 if (lp21 == 0) {
13697 goto L9;
13698 }
13699 swp = TRUE_;
13700 iwk[(i__ << 1) + 1] = n1;
13701 iwk[(i__ << 1) + 2] = n2;
13702 L4:
13703 ;
13704 }
13705 if (swp) {
13706 goto L1;
13707 }
13708
13709
13710
13711 L5:
13712 *nit = iter;
13713 *ier = 0;
13714 return 0;
13715
13716
13717
13718 L6:
13719 *nit = maxit;
13720 *ier = 1;
13721 return 0;
13722
13723
13724
13725 L7:
13726 *nit = 0;
13727 *ier = 2;
13728 return 0;
13729
13730
13731
13732 L8:
13733 *nit = iter;
13734 *ier = 3;
13735 return 0;
13736
13737
13738
13739 L9:
13740 *nit = iter;
13741 *ier = 4;
13742 return 0;
13743 }
13744
13745 int projct_(double *px, double *py, double *pz,
13746 double *ox, double *oy, double *oz, double *ex,
13747 double *ey, double *ez, double *vx, double *vy,
13748 double *vz, long int *init, double *x, double *y,
13749 double *z__, int *ier)
13750 {
13751
13752
13753
13754
13755 static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13756 oes, xoe, yoe, zoe, xep, yep, zep;
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
13798
13799
13800
13801
13802
13803
13804
13805
13806
13807
13808
13809
13810
13811
13812
13813
13814
13815
13816
13817
13818
13819
13820
13821
13822
13823
13824
13825
13826
13827
13828
13829
13830
13831
13832
13833
13834
13835
13836
13837
13838
13839
13840
13841
13842
13843
13844
13845
13846
13847
13848
13849
13850
13851
13852
13853
13854
13855
13856
13857
13858
13859
13860
13861
13862
13863
13864
13865
13866
13867
13868
13869
13870
13871 if (*init) {
13872
13873
13874
13875
13876
13877
13878
13879
13880 xe = *ex;
13881 ye = *ey;
13882 ze = *ez;
13883 xoe = xe - *ox;
13884 yoe = ye - *oy;
13885 zoe = ze - *oz;
13886 oes = xoe * xoe + yoe * yoe + zoe * zoe;
13887 if (oes == 0.) {
13888 goto L1;
13889 }
13890
13891
13892
13893 s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
13894 xv = *vx - s * xoe;
13895 yv = *vy - s * yoe;
13896 zv = *vz - s * zoe;
13897
13898
13899
13900 sc = xv * xv + yv * yv + zv * zv;
13901 if (sc == 0.) {
13902 goto L2;
13903 }
13904 sc = 1. / sqrt(sc);
13905 xv = sc * xv;
13906 yv = sc * yv;
13907 zv = sc * zv;
13908
13909
13910
13911 xh = yv * zoe - yoe * zv;
13912 yh = xoe * zv - xv * zoe;
13913 zh = xv * yoe - xoe * yv;
13914 sc = sqrt(xh * xh + yh * yh + zh * zh);
13915 if (sc == 0.) {
13916 goto L2;
13917 }
13918 sc = 1. / sc;
13919 xh = sc * xh;
13920 yh = sc * yh;
13921 zh = sc * zh;
13922 }
13923
13924
13925
13926
13927
13928
13929 xep = *px - xe;
13930 yep = *py - ye;
13931 zep = *pz - ze;
13932 s = xoe * xep + yoe * yep + zoe * zep;
13933 if (s >= 0.) {
13934 goto L1;
13935 }
13936 s = oes / s;
13937 xw = xoe - s * xep;
13938 yw = yoe - s * yep;
13939 zw = zoe - s * zep;
13940
13941
13942
13943
13944 *x = xw * xh + yw * yh + zw * zh;
13945 *y = xw * xv + yw * yv + zw * zv;
13946 *z__ = s + 1.;
13947 *init = FALSE_;
13948 *ier = 0;
13949 return 0;
13950
13951
13952
13953 L1:
13954 *ier = 1;
13955 return 0;
13956
13957
13958
13959 L2:
13960 *ier = 2;
13961 return 0;
13962 }
13963
13964 int scoord_(double *px, double *py, double *pz,
13965 double *plat, double *plon, double *pnrm)
13966 {
13967
13968
13969
13970
13971
13972
13973
13974
13975
13976
13977
13978
13979
13980
13981
13982
13983
13984
13985
13986
13987
13988
13989
13990
13991
13992
13993
13994
13995
13996
13997
13998
13999
14000
14001
14002
14003
14004
14005
14006
14007
14008
14009 *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14010 if (*px != 0. || *py != 0.) {
14011 *plon = atan2(*py, *px);
14012 } else {
14013 *plon = 0.;
14014 }
14015 if (*pnrm != 0.) {
14016 *plat = asin(*pz / *pnrm);
14017 } else {
14018 *plat = 0.;
14019 }
14020 return 0;
14021 }
14022
14023 double store_(double *x)
14024 {
14025
14026 double ret_val;
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 stcom_1.y = *x;
14062 ret_val = stcom_1.y;
14063 return ret_val;
14064 }
14065
14066 int swap_(int *in1, int *in2, int *io1, int *
14067 io2, int *list, int *lptr, int *lend, int *lp21)
14068 {
14069
14070 int i__1;
14071
14072
14073 static int lp, lph, lpsav;
14074 extern int lstptr_(int *, int *, int *, int *);
14075
14076
14077
14078
14079
14080
14081
14082
14083
14084
14085
14086
14087
14088
14089
14090
14091
14092
14093
14094
14095
14096
14097
14098
14099
14100
14101
14102
14103
14104
14105
14106
14107
14108
14109
14110
14111
14112
14113
14114
14115
14116
14117
14118
14119
14120
14121
14122
14123
14124
14125
14126
14127
14128
14129
14130
14131
14132
14133
14134
14135 --lend;
14136 --lptr;
14137 --list;
14138
14139
14140 lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14141 if ((i__1 = list[lp], abs(i__1)) == *in2) {
14142 *lp21 = 0;
14143 return 0;
14144 }
14145
14146
14147
14148 lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14149 lph = lptr[lp];
14150 lptr[lp] = lptr[lph];
14151
14152
14153
14154
14155 if (lend[*io1] == lph) {
14156 lend[*io1] = lp;
14157 }
14158
14159
14160
14161
14162 lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14163 lpsav = lptr[lp];
14164 lptr[lp] = lph;
14165 list[lph] = *in2;
14166 lptr[lph] = lpsav;
14167
14168
14169
14170 lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14171 lph = lptr[lp];
14172 lptr[lp] = lptr[lph];
14173
14174
14175
14176
14177 if (lend[*io2] == lph) {
14178 lend[*io2] = lp;
14179 }
14180
14181
14182
14183 lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14184 lpsav = lptr[lp];
14185 lptr[lp] = lph;
14186 list[lph] = *in1;
14187 lptr[lph] = lpsav;
14188 *lp21 = lph;
14189 return 0;
14190 }
14191
14192 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14193 double *x, double *y, double *z__)
14194 {
14195
14196 long int ret_val;
14197
14198
14199 static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14200
14201
14202
14203
14204
14205
14206
14207
14208
14209
14210
14211
14212
14213
14214
14215
14216
14217
14218
14219
14220
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 --z__;
14257 --y;
14258 --x;
14259
14260
14261 x4 = x[*n4];
14262 y4 = y[*n4];
14263 z4 = z__[*n4];
14264 dx1 = x[*n1] - x4;
14265 dx2 = x[*n2] - x4;
14266 dx3 = x[*n3] - x4;
14267 dy1 = y[*n1] - y4;
14268 dy2 = y[*n2] - y4;
14269 dy3 = y[*n3] - y4;
14270 dz1 = z__[*n1] - z4;
14271 dz2 = z__[*n2] - z4;
14272 dz3 = z__[*n3] - z4;
14273
14274
14275
14276
14277
14278 ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14279 dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14280 return ret_val;
14281 }
14282
14283 int trans_(int *n, double *rlat, double *rlon,
14284 double *x, double *y, double *z__)
14285 {
14286
14287 int i__1;
14288
14289
14290
14291
14292
14293 static int i__, nn;
14294 static double phi, theta, cosphi;
14295
14296
14297
14298
14299
14300
14301
14302
14303
14304
14305
14306
14307
14308
14309
14310
14311
14312
14313
14314
14315
14316
14317
14318
14319
14320
14321
14322
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 --z__;
14350 --y;
14351 --x;
14352 --rlon;
14353 --rlat;
14354
14355
14356 nn = *n;
14357 i__1 = nn;
14358 for (i__ = 1; i__ <= i__1; ++i__) {
14359 phi = rlat[i__];
14360 theta = rlon[i__];
14361 cosphi = cos(phi);
14362 x[i__] = cosphi * cos(theta);
14363 y[i__] = cosphi * sin(theta);
14364 z__[i__] = sin(phi);
14365
14366 }
14367 return 0;
14368 }
14369
14370 int trfind_(int *nst, double *p, int *n,
14371 double *x, double *y, double *z__, int *list, int
14372 *lptr, int *lend, double *b1, double *b2, double *b3,
14373 int *i1, int *i2, int *i3)
14374 {
14375
14376
14377 static int ix = 1;
14378 static int iy = 2;
14379 static int iz = 3;
14380
14381
14382 int i__1;
14383 double d__1, d__2;
14384
14385
14386 static double q[3];
14387 static int n0, n1, n2, n3, n4, nf;
14388 static double s12;
14389 static int nl, lp;
14390 static double xp, yp, zp;
14391 static int n1s, n2s;
14392 static double eps, tol, ptn1, ptn2;
14393 static int next;
14394 extern int jrand_(int *, int *, int *, int *);
14395 extern double store_(double *);
14396 extern int lstptr_(int *, int *, int *, int *);
14397
14398
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
14448
14449
14450
14451
14452
14453
14454
14455
14456
14457
14458
14459
14460
14461
14462
14463
14464
14465
14466 --p;
14467 --lend;
14468 --z__;
14469 --y;
14470 --x;
14471 --list;
14472 --lptr;
14473
14474
14475
14476
14477
14478
14479
14480
14481
14482
14483
14484
14485
14486
14487
14488
14489
14490
14491
14492
14493
14494
14495
14496
14497
14498
14499
14500
14501
14502
14503
14504
14505
14506
14507
14508
14509
14510
14511
14512
14513
14514
14515
14516
14517
14518
14519
14520
14521 xp = p[1];
14522 yp = p[2];
14523 zp = p[3];
14524 n0 = *nst;
14525 if (n0 < 1 || n0 > *n) {
14526 n0 = jrand_(n, &ix, &iy, &iz);
14527 }
14528
14529
14530
14531 eps = 1.;
14532 L1:
14533 eps /= 2.;
14534 d__1 = eps + 1.;
14535 if (store_(&d__1) > 1.) {
14536 goto L1;
14537 }
14538 eps *= 2.;
14539 tol = eps * 4.;
14540
14541
14542
14543
14544 L2:
14545 lp = lend[n0];
14546 nl = list[lp];
14547 lp = lptr[lp];
14548 nf = list[lp];
14549 n1 = nf;
14550
14551
14552
14553
14554 if (nl > 0) {
14555
14556
14557
14558 L3:
14559 if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14560 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14561 -1e-10) {
14562 lp = lptr[lp];
14563 n1 = list[lp];
14564 if (n1 == nl) {
14565 goto L6;
14566 }
14567 goto L3;
14568 }
14569 } else {
14570
14571
14572
14573 nl = -nl;
14574 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14575 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14576 -1e-10) {
14577
14578
14579
14580 n1 = n0;
14581 n2 = nf;
14582 goto L9;
14583 }
14584 if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14585 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14586 -1e-10) {
14587
14588
14589
14590 n1 = nl;
14591 n2 = n0;
14592 goto L9;
14593 }
14594 }
14595
14596
14597
14598
14599 L4:
14600 lp = lptr[lp];
14601 n2 = (i__1 = list[lp], abs(i__1));
14602 if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14603 n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14604 goto L7;
14605 }
14606 n1 = n2;
14607 if (n1 != nl) {
14608 goto L4;
14609 }
14610 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14611 nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14612 goto L6;
14613 }
14614
14615
14616
14617
14618 d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14619 if (store_(&d__2) < 1. - eps * 4.) {
14620
14621
14622
14623
14624
14625 L5:
14626 if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14627 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14628 -1e-10) {
14629 lp = lptr[lp];
14630 n1 = (i__1 = list[lp], abs(i__1));
14631 if (n1 == nl) {
14632 goto L14;
14633 }
14634 goto L5;
14635 }
14636 }
14637
14638
14639
14640
14641 n0 = n1;
14642 goto L2;
14643
14644
14645
14646 L6:
14647 n2 = nf;
14648
14649
14650
14651
14652
14653 L7:
14654 n3 = n0;
14655 n1s = n1;
14656 n2s = n2;
14657
14658
14659
14660 L8:
14661
14662 *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14663 x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14664 if (*b3 < -1e-10) {
14665
14666
14667
14668
14669 lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14670 if (list[lp] < 0) {
14671 goto L9;
14672 }
14673 lp = lptr[lp];
14674 n4 = (i__1 = list[lp], abs(i__1));
14675
14676
14677
14678 if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14679 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14680 -1e-10) {
14681 n3 = n2;
14682 n2 = n4;
14683 n1s = n1;
14684 if (n2 != n2s && n2 != n0) {
14685 goto L8;
14686 }
14687 } else {
14688 n3 = n1;
14689 n1 = n4;
14690 n2s = n2;
14691 if (n1 != n1s && n1 != n0) {
14692 goto L8;
14693 }
14694 }
14695
14696
14697
14698
14699
14700 n0 = jrand_(n, &ix, &iy, &iz);
14701 goto L2;
14702 }
14703
14704
14705
14706
14707 if (*b3 >= eps) {
14708
14709
14710
14711 *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14712 - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14713 *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14714 - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14715 if (*b1 < -tol || *b2 < -tol) {
14716
14717
14718
14719 n0 = jrand_(n, &ix, &iy, &iz);
14720 goto L2;
14721 }
14722 } else {
14723
14724
14725
14726
14727 *b3 = 0.;
14728 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14729 ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14730 ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14731 *b1 = ptn1 - s12 * ptn2;
14732 *b2 = ptn2 - s12 * ptn1;
14733 if (*b1 < -tol || *b2 < -tol) {
14734
14735
14736
14737 n0 = jrand_(n, &ix, &iy, &iz);
14738 goto L2;
14739 }
14740 }
14741
14742
14743
14744 *i1 = n1;
14745 *i2 = n2;
14746 *i3 = n3;
14747 if (*b1 < 0.f) {
14748 *b1 = 0.f;
14749 }
14750 if (*b2 < 0.f) {
14751 *b2 = 0.f;
14752 }
14753 return 0;
14754
14755
14756
14757
14758
14759 L9:
14760 n1s = n1;
14761 n2s = n2;
14762 nl = 0;
14763
14764
14765
14766 L10:
14767
14768 lp = lend[n2];
14769 lp = lptr[lp];
14770 next = list[lp];
14771 if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14772 - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14773 >= -1e-10) {
14774
14775
14776
14777
14778 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14779 q[0] = x[n1] - s12 * x[n2];
14780 q[1] = y[n1] - s12 * y[n2];
14781 q[2] = z__[n1] - s12 * z__[n2];
14782 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14783 goto L11;
14784 }
14785 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14786 goto L11;
14787 }
14788
14789
14790
14791
14792 nl = n2;
14793 }
14794
14795
14796
14797 n1 = n2;
14798 n2 = next;
14799 if (n2 != n1s) {
14800 goto L10;
14801 }
14802
14803
14804
14805 *i1 = n1s;
14806 *i2 = n1s;
14807 *i3 = 0;
14808 return 0;
14809
14810
14811
14812 L11:
14813 nf = n2;
14814 if (nl == 0) {
14815
14816
14817
14818
14819 n2 = n2s;
14820 n1 = n1s;
14821
14822
14823
14824 L12:
14825 lp = lend[n1];
14826 next = -list[lp];
14827 if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14828 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14829 y[next]) >= -1e-10) {
14830
14831
14832
14833
14834 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14835 q[0] = x[n2] - s12 * x[n1];
14836 q[1] = y[n2] - s12 * y[n1];
14837 q[2] = z__[n2] - s12 * z__[n1];
14838 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14839 goto L13;
14840 }
14841 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14842 goto L13;
14843 }
14844
14845
14846
14847
14848 nf = n1;
14849 }
14850
14851
14852
14853 n2 = n1;
14854 n1 = next;
14855 if (n1 != n1s) {
14856 goto L12;
14857 }
14858
14859
14860
14861 *i1 = n1;
14862 *i2 = n1;
14863 *i3 = 0;
14864 return 0;
14865
14866
14867
14868 L13:
14869 nl = n1;
14870 }
14871
14872
14873
14874 *i1 = nf;
14875 *i2 = nl;
14876 *i3 = 0;
14877 return 0;
14878
14879
14880
14881 L14:
14882 *i1 = 0;
14883 *i2 = 0;
14884 *i3 = 0;
14885 return 0;
14886 }
14887
14888 int trlist_(int *n, int *list, int *lptr,
14889 int *lend, int *nrow, int *nt, int *ltri, int *
14890 ier)
14891 {
14892
14893 int ltri_dim1, ltri_offset, i__1, i__2;
14894
14895
14896 static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
14897 lpl, isv;
14898 static long int arcs;
14899 static int lpln1;
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
14931
14932
14933
14934
14935
14936
14937
14938
14939
14940
14941
14942
14943
14944
14945
14946
14947
14948
14949
14950
14951
14952
14953
14954
14955
14956
14957
14958
14959
14960
14961
14962
14963
14964
14965
14966
14967
14968
14969
14970
14971
14972
14973
14974
14975
14976
14977
14978
14979
14980
14981
14982
14983
14984
14985
14986
14987
14988
14989
14990
14991
14992
14993
14994
14995
14996
14997
14998
14999
15000
15001
15002
15003
15004 --lend;
15005 --list;
15006 --lptr;
15007 ltri_dim1 = *nrow;
15008 ltri_offset = 1 + ltri_dim1;
15009 ltri -= ltri_offset;
15010
15011
15012 if (*n < 3 || (*nrow != 6 && *nrow != 9)) {
15013 goto L11;
15014 }
15015
15016
15017
15018
15019
15020
15021
15022
15023 arcs = *nrow == 9;
15024 ka = 0;
15025 kt = 0;
15026 nm2 = *n - 2;
15027
15028
15029
15030 i__1 = nm2;
15031 for (n1 = 1; n1 <= i__1; ++n1) {
15032
15033
15034
15035
15036 lpln1 = lend[n1];
15037 lp2 = lpln1;
15038 L1:
15039 lp2 = lptr[lp2];
15040 n2 = list[lp2];
15041 lp = lptr[lp2];
15042 n3 = (i__2 = list[lp], abs(i__2));
15043 if (n2 < n1 || n3 < n1) {
15044 goto L8;
15045 }
15046
15047
15048
15049 ++kt;
15050 ltri[kt * ltri_dim1 + 1] = n1;
15051 ltri[kt * ltri_dim1 + 2] = n2;
15052 ltri[kt * ltri_dim1 + 3] = n3;
15053
15054
15055
15056
15057 for (i__ = 1; i__ <= 3; ++i__) {
15058 if (i__ == 1) {
15059 i1 = n3;
15060 i2 = n2;
15061 } else if (i__ == 2) {
15062 i1 = n1;
15063 i2 = n3;
15064 } else {
15065 i1 = n2;
15066 i2 = n1;
15067 }
15068
15069
15070
15071
15072 lpl = lend[i1];
15073 lp = lptr[lpl];
15074 L2:
15075 if (list[lp] == i2) {
15076 goto L3;
15077 }
15078 lp = lptr[lp];
15079 if (lp != lpl) {
15080 goto L2;
15081 }
15082
15083
15084
15085
15086
15087 if ((i__2 = list[lp], abs(i__2)) != i2) {
15088 goto L12;
15089 }
15090 kn = 0;
15091 if (list[lp] < 0) {
15092 goto L6;
15093 }
15094
15095
15096
15097
15098 L3:
15099 lp = lptr[lp];
15100 i3 = (i__2 = list[lp], abs(i__2));
15101
15102
15103
15104
15105
15106 if (i1 < i2 && i1 < i3) {
15107 j = 3;
15108 } else if (i2 < i3) {
15109 j = 2;
15110 isv = i1;
15111 i1 = i2;
15112 i2 = i3;
15113 i3 = isv;
15114 } else {
15115 j = 1;
15116 isv = i1;
15117 i1 = i3;
15118 i3 = i2;
15119 i2 = isv;
15120 }
15121
15122
15123
15124 if (i1 > n1) {
15125 goto L7;
15126 }
15127
15128
15129
15130
15131 for (kn = kt - 1; kn >= 1; --kn) {
15132 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15133 == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15134 goto L5;
15135 }
15136
15137 }
15138 goto L7;
15139
15140
15141
15142 L5:
15143 ltri[j + 3 + kn * ltri_dim1] = kt;
15144
15145
15146
15147 L6:
15148 ltri[i__ + 3 + kt * ltri_dim1] = kn;
15149 if (arcs) {
15150 ++ka;
15151 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15152 if (kn != 0) {
15153 ltri[j + 6 + kn * ltri_dim1] = ka;
15154 }
15155 }
15156 L7:
15157 ;
15158 }
15159
15160
15161
15162 L8:
15163 if (lp2 != lpln1) {
15164 goto L1;
15165 }
15166
15167 }
15168
15169
15170
15171 *nt = kt;
15172 *ier = 0;
15173 return 0;
15174
15175
15176
15177 L11:
15178 *nt = 0;
15179 *ier = 1;
15180 return 0;
15181
15182
15183
15184
15185 L12:
15186 *nt = 0;
15187 *ier = 2;
15188 return 0;
15189 }
15190
15191 int trlprt_(int *n, double *x, double *y,
15192 double *z__, int *iflag, int *nrow, int *nt, int *
15193 ltri, int *lout)
15194 {
15195
15196
15197 static int nmax = 9999;
15198 static int nlmax = 58;
15199
15200
15201 int ltri_dim1, ltri_offset, i__1;
15202
15203
15204 static int i__, k, na, nb, nl, lun;
15205
15206
15207
15208
15209
15210
15211
15212
15213
15214
15215
15216
15217
15218
15219
15220
15221
15222
15223
15224
15225
15226
15227
15228
15229
15230
15231
15232
15233
15234
15235
15236
15237
15238
15239
15240
15241
15242
15243
15244
15245
15246
15247
15248
15249
15250
15251
15252
15253
15254
15255
15256
15257
15258
15259
15260
15261
15262
15263
15264
15265
15266
15267
15268
15269
15270
15271
15272
15273
15274
15275
15276
15277
15278 --z__;
15279 --y;
15280 --x;
15281 ltri_dim1 = *nrow;
15282 ltri_offset = 1 + ltri_dim1;
15283 ltri -= ltri_offset;
15284
15285
15286
15287
15288
15289
15290
15291
15292
15293
15294
15295
15296
15297
15298
15299
15300 lun = *lout;
15301 if (lun < 0 || lun > 99) {
15302 lun = 6;
15303 }
15304
15305
15306
15307
15308 nl = 3;
15309 if (*n < 3 || *n > nmax || (*nrow != 6 && *nrow != 9) || *nt < 1 || *nt >
15310 nmax) {
15311
15312
15313
15314
15315 return 0;
15316 }
15317 if (*iflag == 0) {
15318
15319
15320
15321
15322 nl = 6;
15323 i__1 = *n;
15324 for (i__ = 1; i__ <= i__1; ++i__) {
15325 if (nl >= nlmax) {
15326
15327 nl = 0;
15328 }
15329
15330 ++nl;
15331
15332 }
15333 } else if (*iflag > 0) {
15334
15335
15336
15337
15338 nl = 6;
15339 i__1 = *n;
15340 for (i__ = 1; i__ <= i__1; ++i__) {
15341 if (nl >= nlmax) {
15342
15343 nl = 0;
15344 }
15345
15346 ++nl;
15347
15348 }
15349 }
15350
15351
15352
15353 if (nl > nlmax / 2) {
15354
15355 nl = 0;
15356 }
15357 if (*nrow == 6) {
15358
15359 } else {
15360
15361 }
15362 nl += 5;
15363 i__1 = *nt;
15364 for (k = 1; k <= i__1; ++k) {
15365 if (nl >= nlmax) {
15366
15367 nl = 0;
15368 }
15369
15370 ++nl;
15371
15372 }
15373
15374
15375
15376
15377 nb = (*n << 1) - *nt - 2;
15378 if (nb < 3) {
15379 nb = 0;
15380 na = *n * 3 - 6;
15381 } else {
15382 na = *nt + *n - 1;
15383 }
15384
15385 return 0;
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 int trmesh_(int *n, double *x, double *y,
15412 double *z__, int *list, int *lptr, int *lend, int
15413 *lnew, int *near__, int *next, double *dist, int *ier)
15414 {
15415
15416 int i__1, i__2;
15417
15418
15419 static double d__;
15420 static int i__, j, k;
15421 static double d1, d2, d3;
15422 static int i0, lp, nn, lpl;
15423 extern long int left_(double *, double *, double *, double
15424 *, double *, double *, double *, double *,
15425 double *);
15426 static int nexti;
15427 extern int addnod_(int *, int *, double *,
15428 double *, double *, int *, int *, int *,
15429 int *, int *);
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
15583
15584
15585
15586
15587
15588
15589
15590
15591
15592
15593
15594
15595
15596
15597
15598
15599
15600
15601
15602
15603
15604
15605
15606
15607
15608
15609
15610
15611
15612
15613
15614
15615
15616
15617
15618
15619
15620
15621
15622
15623
15624
15625
15626
15627
15628
15629
15630
15631
15632
15633
15634
15635
15636
15637
15638
15639
15640
15641
15642
15643
15644
15645
15646
15647
15648
15649
15650
15651
15652
15653
15654
15655
15656 --dist;
15657 --next;
15658 --near__;
15659 --lend;
15660 --z__;
15661 --y;
15662 --x;
15663 --list;
15664 --lptr;
15665
15666
15667 nn = *n;
15668 if (nn < 3) {
15669 *ier = -1;
15670 return 0;
15671 }
15672
15673
15674
15675 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15676 z__[3])) {
15677
15678
15679
15680 list[1] = 3;
15681 lptr[1] = 2;
15682 list[2] = -2;
15683 lptr[2] = 1;
15684 lend[1] = 2;
15685
15686 list[3] = 1;
15687 lptr[3] = 4;
15688 list[4] = -3;
15689 lptr[4] = 3;
15690 lend[2] = 4;
15691
15692 list[5] = 2;
15693 lptr[5] = 6;
15694 list[6] = -1;
15695 lptr[6] = 5;
15696 lend[3] = 6;
15697
15698 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15699 y[3], &z__[3])) {
15700
15701
15702
15703
15704
15705 list[1] = 2;
15706 lptr[1] = 2;
15707 list[2] = -3;
15708 lptr[2] = 1;
15709 lend[1] = 2;
15710
15711 list[3] = 3;
15712 lptr[3] = 4;
15713 list[4] = -1;
15714 lptr[4] = 3;
15715 lend[2] = 4;
15716
15717 list[5] = 1;
15718 lptr[5] = 6;
15719 list[6] = -2;
15720 lptr[6] = 5;
15721 lend[3] = 6;
15722
15723 } else {
15724
15725
15726
15727 *ier = -2;
15728 return 0;
15729 }
15730
15731
15732
15733 *lnew = 7;
15734 if (nn == 3) {
15735 *ier = 0;
15736 return 0;
15737 }
15738
15739
15740
15741
15742
15743
15744
15745
15746
15747
15748
15749
15750
15751
15752
15753
15754
15755
15756
15757
15758
15759
15760
15761
15762
15763
15764
15765 near__[1] = 0;
15766 near__[2] = 0;
15767 near__[3] = 0;
15768 for (k = nn; k >= 4; --k) {
15769 d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15770 d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15771 d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15772 if (d1 <= d2 && d1 <= d3) {
15773 near__[k] = 1;
15774 dist[k] = d1;
15775 next[k] = near__[1];
15776 near__[1] = k;
15777 } else if (d2 <= d1 && d2 <= d3) {
15778 near__[k] = 2;
15779 dist[k] = d2;
15780 next[k] = near__[2];
15781 near__[2] = k;
15782 } else {
15783 near__[k] = 3;
15784 dist[k] = d3;
15785 next[k] = near__[3];
15786 near__[3] = k;
15787 }
15788
15789 }
15790
15791
15792
15793 i__1 = nn;
15794 for (k = 4; k <= i__1; ++k) {
15795 addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15796 lend[1], lnew, ier);
15797 if (*ier != 0) {
15798 return 0;
15799 }
15800
15801
15802
15803
15804 i__ = near__[k];
15805 if (near__[i__] == k) {
15806 near__[i__] = next[k];
15807 } else {
15808 i__ = near__[i__];
15809 L2:
15810 i0 = i__;
15811 i__ = next[i0];
15812 if (i__ != k) {
15813 goto L2;
15814 }
15815 next[i0] = next[k];
15816 }
15817 near__[k] = 0;
15818
15819
15820
15821 lpl = lend[k];
15822 lp = lpl;
15823 L3:
15824 lp = lptr[lp];
15825 j = (i__2 = list[lp], abs(i__2));
15826
15827
15828
15829
15830
15831
15832
15833 i__ = near__[j];
15834 L4:
15835 if (i__ == 0) {
15836 goto L5;
15837 }
15838 nexti = next[i__];
15839
15840
15841
15842
15843 d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15844 if (d__ < dist[i__]) {
15845
15846
15847
15848
15849
15850 near__[i__] = k;
15851 dist[i__] = d__;
15852 if (i__ == near__[j]) {
15853 near__[j] = nexti;
15854 } else {
15855 next[i0] = nexti;
15856 }
15857 next[i__] = near__[k];
15858 near__[k] = i__;
15859 } else {
15860 i0 = i__;
15861 }
15862
15863
15864
15865 i__ = nexti;
15866 goto L4;
15867
15868
15869
15870 L5:
15871 if (lp != lpl) {
15872 goto L3;
15873 }
15874
15875 }
15876 return 0;
15877 }
15878
15879 int trplot_(int *lun, double *pltsiz, double *
15880 elat, double *elon, double *a, int *n, double *x,
15881 double *y, double *z__, int *list, int *lptr, int
15882 *lend, char *, long int *numbr, int *ier, short )
15883 {
15884
15885
15886 static long int annot = TRUE_;
15887 static double fsizn = 10.;
15888 static double fsizt = 16.;
15889 static double tol = .5;
15890
15891
15892 int i__1, i__2;
15893 double d__1;
15894
15895
15896
15897
15898
15899
15900
15901 static double t;
15902 static int n0, n1;
15903 static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
15904 static int ir, lp;
15905 static double ex, ey, ez, wr, tx, ty;
15906 static int lpl;
15907 static double wrs;
15908 static int ipx1, ipx2, ipy1, ipy2, nseg;
15909 extern int drwarc_(int *, double *, double *,
15910 double *, int *);
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
15944
15945
15946
15947
15948
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
16005
16006
16007
16008
16009
16010
16011
16012
16013
16014
16015
16016
16017 --lend;
16018 --z__;
16019 --y;
16020 --x;
16021 --list;
16022 --lptr;
16023
16024
16025
16026
16027
16028
16029
16030
16031
16032
16033
16034
16035
16036
16037
16038
16039
16040
16041
16042
16043
16044
16045
16046
16047
16048
16049
16050
16051
16052
16053
16054
16055
16056
16057
16058
16059
16060
16061
16062
16063
16064
16065
16066
16067
16068
16069
16070
16071
16072
16073
16074
16075
16076
16077
16078 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16079 goto L11;
16080 }
16081 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16082 goto L12;
16083 }
16084
16085
16086
16087
16088 cf = atan(1.) / 45.;
16089 wr = sin(cf * *a);
16090 wrs = wr * wr;
16091
16092
16093
16094
16095
16096
16097
16098
16099
16100
16101 d__1 = *pltsiz * 36.;
16102 ir = i_dnnt(&d__1);
16103 ipx1 = 306 - ir;
16104 ipx2 = ir + 306;
16105 ipy1 = 396 - ir;
16106 ipy2 = ir + 396;
16107
16108
16109
16110
16111
16112
16113
16114
16115
16116
16117
16118
16119
16120
16121 d__1 = (double) ir * .88;
16122 ir = i_dnnt(&d__1);
16123 ipx1 = 306 - ir;
16124 ipx2 = ir + 306;
16125 ipy1 = 396 - ir;
16126 ipy2 = ir + 396;
16127
16128
16129
16130
16131 t = 2.;
16132
16133
16134
16135
16136
16137
16138
16139
16140
16141
16142 sf = (double) ir / wr;
16143 tx = ipx1 + sf * wr;
16144 ty = ipy1 + sf * wr;
16145
16146
16147
16148
16149
16150
16151
16152
16153 t = 1. / sf;
16154
16155
16156
16157
16158
16159
16160
16161
16162
16163
16164
16165
16166
16167
16168
16169
16170
16171
16172
16173
16174
16175
16176
16177 t = cf * *elon;
16178 ct = cos(cf * *elat);
16179 ex = ct * cos(t);
16180 ey = ct * sin(t);
16181 ez = sin(cf * *elat);
16182 if (ct != 0.) {
16183 r11 = -ey / ct;
16184 r12 = ex / ct;
16185 } else {
16186 r11 = 0.;
16187 r12 = 1.;
16188 }
16189 r21 = -ez * r12;
16190 r22 = ez * r11;
16191 r23 = ct;
16192
16193
16194
16195
16196 i__1 = *n;
16197 for (n0 = 1; n0 <= i__1; ++n0) {
16198 p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16199 if (p0[2] < 0.) {
16200 goto L3;
16201 }
16202 p0[0] = r11 * x[n0] + r12 * y[n0];
16203 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16204 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16205 goto L3;
16206 }
16207 lpl = lend[n0];
16208 lp = lpl;
16209
16210
16211
16212
16213 L1:
16214 lp = lptr[lp];
16215 n1 = (i__2 = list[lp], abs(i__2));
16216 p1[0] = r11 * x[n1] + r12 * y[n1];
16217 p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16218 p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16219 if (p1[2] < 0.) {
16220
16221
16222
16223
16224
16225 p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16226 p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16227 t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16228 p1[0] /= t;
16229 p1[1] /= t;
16230 }
16231
16232
16233
16234
16235 if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16236 goto L2;
16237 }
16238
16239
16240
16241
16242 if (p1[2] < 0.) {
16243 p1[2] = 0.;
16244 }
16245 d__1 = tol / sf;
16246 drwarc_(lun, p0, p1, &d__1, &nseg);
16247
16248
16249
16250 L2:
16251 if (lp != lpl) {
16252 goto L1;
16253 }
16254 L3:
16255 ;
16256 }
16257
16258
16259
16260
16261
16262
16263
16264 if (*numbr) {
16265
16266
16267
16268
16269
16270 t = fsizn / sf;
16271
16272
16273
16274
16275
16276
16277
16278 i__1 = *n;
16279 for (n0 = 1; n0 <= i__1; ++n0) {
16280 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16281 goto L4;
16282 }
16283 p0[0] = r11 * x[n0] + r12 * y[n0];
16284 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16285 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16286 goto L4;
16287 }
16288
16289
16290
16291
16292
16293
16294
16295
16296
16297 L4:
16298 ;
16299 }
16300 }
16301
16302
16303
16304
16305 t = fsizt / sf;
16306
16307
16308
16309
16310 p0[1] = wr + t * 3.;
16311
16312
16313
16314
16315
16316 if (annot) {
16317
16318
16319
16320 p0[0] = -wr;
16321 p0[1] = -wr - 50. / sf;
16322
16323
16324 p0[1] -= t * 2.;
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 *ier = 0;
16350 return 0;
16351
16352
16353
16354 L11:
16355 *ier = 1;
16356 return 0;
16357
16358
16359
16360 L12:
16361 *ier = 2;
16362 return 0;
16363
16364
16365
16366
16367 *ier = 3;
16368 return 0;
16369 }
16370
16371 int trprnt_(int *n, double *x, double *y,
16372 double *z__, int *iflag, int *list, int *lptr,
16373 int *lend, int *lout)
16374 {
16375
16376
16377 static int nmax = 9999;
16378 static int nlmax = 58;
16379
16380
16381 int i__1;
16382
16383
16384 static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16385 400];
16386
16387
16388
16389
16390
16391
16392
16393
16394
16395
16396
16397
16398
16399
16400
16401
16402
16403
16404
16405
16406
16407
16408
16409
16410
16411
16412
16413
16414
16415
16416
16417
16418
16419
16420
16421
16422
16423
16424
16425
16426
16427
16428
16429
16430
16431
16432
16433
16434
16435
16436
16437
16438
16439
16440
16441
16442
16443
16444
16445
16446
16447
16448 --lend;
16449 --z__;
16450 --y;
16451 --x;
16452 --list;
16453 --lptr;
16454
16455
16456
16457
16458
16459
16460
16461
16462
16463
16464
16465
16466
16467
16468
16469
16470
16471
16472
16473
16474
16475
16476
16477
16478
16479
16480
16481 nn = *n;
16482 lun = *lout;
16483 if (lun < 0 || lun > 99) {
16484 lun = 6;
16485 }
16486
16487
16488
16489
16490 if (nn < 3 || nn > nmax) {
16491
16492
16493
16494
16495 return 0;
16496 }
16497
16498
16499
16500
16501 nl = 6;
16502 nb = 0;
16503 if (*iflag < 0) {
16504
16505
16506
16507
16508
16509 i__1 = nn;
16510 for (node = 1; node <= i__1; ++node) {
16511 lpl = lend[node];
16512 lp = lpl;
16513 k = 0;
16514
16515 L1:
16516 ++k;
16517 lp = lptr[lp];
16518 nd = list[lp];
16519 nabor[k - 1] = nd;
16520 if (lp != lpl) {
16521 goto L1;
16522 }
16523 if (nd <= 0) {
16524
16525
16526
16527
16528
16529 nabor[k - 1] = -nd;
16530 ++k;
16531 nabor[k - 1] = 0;
16532 ++nb;
16533 }
16534
16535
16536
16537 inc = (k - 1) / 14 + 2;
16538 nl += inc;
16539 if (nl > nlmax) {
16540
16541 nl = inc;
16542 }
16543
16544
16545
16546
16547 }
16548 } else if (*iflag > 0) {
16549
16550
16551
16552
16553 i__1 = nn;
16554 for (node = 1; node <= i__1; ++node) {
16555 lpl = lend[node];
16556 lp = lpl;
16557 k = 0;
16558
16559 L3:
16560 ++k;
16561 lp = lptr[lp];
16562 nd = list[lp];
16563 nabor[k - 1] = nd;
16564 if (lp != lpl) {
16565 goto L3;
16566 }
16567 if (nd <= 0) {
16568
16569
16570
16571 nabor[k - 1] = -nd;
16572 ++k;
16573 nabor[k - 1] = 0;
16574 ++nb;
16575 }
16576
16577
16578
16579 inc = (k - 1) / 8 + 2;
16580 nl += inc;
16581 if (nl > nlmax) {
16582
16583 nl = inc;
16584 }
16585
16586
16587
16588
16589
16590 }
16591 } else {
16592
16593
16594
16595
16596 i__1 = nn;
16597 for (node = 1; node <= i__1; ++node) {
16598 lpl = lend[node];
16599 lp = lpl;
16600 k = 0;
16601
16602 L5:
16603 ++k;
16604 lp = lptr[lp];
16605 nd = list[lp];
16606 nabor[k - 1] = nd;
16607 if (lp != lpl) {
16608 goto L5;
16609 }
16610 if (nd <= 0) {
16611
16612
16613
16614 nabor[k - 1] = -nd;
16615 ++k;
16616 nabor[k - 1] = 0;
16617 ++nb;
16618 }
16619
16620
16621
16622 inc = (k - 1) / 5 + 2;
16623 nl += inc;
16624 if (nl > nlmax) {
16625
16626 nl = inc;
16627 }
16628
16629
16630
16631
16632
16633 }
16634 }
16635
16636
16637
16638
16639 if (nb != 0) {
16640 na = nn * 3 - nb - 3;
16641 nt = (nn << 1) - nb - 2;
16642 } else {
16643 na = nn * 3 - 6;
16644 nt = (nn << 1) - 4;
16645 }
16646
16647 return 0;
16648
16649
16650
16651
16652
16653
16654
16655
16656
16657
16658
16659
16660
16661
16662
16663
16664
16665
16666
16667
16668 }
16669
16670 int vrplot_(int *lun, double *pltsiz, double *
16671 elat, double *elon, double *a, int *n, double *x,
16672 double *y, double *z__, int *nt, int *listc, int *
16673 lptr, int *lend, double *xc, double *yc, double *zc,
16674 char *, long int *numbr, int *ier, short)
16675 {
16676
16677
16678 static long int annot = TRUE_;
16679 static double fsizn = 10.;
16680 static double fsizt = 16.;
16681 static double tol = .5;
16682
16683
16684 int i__1;
16685 double d__1;
16686
16687
16688
16689
16690
16691
16692
16693 static double t;
16694 static int n0;
16695 static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16696 sf;
16697 static int ir, lp;
16698 static double ex, ey, ez, wr, tx, ty;
16699 static long int in1, in2;
16700 static int kv1, kv2, lpl;
16701 static double wrs;
16702 static int ipx1, ipx2, ipy1, ipy2, nseg;
16703 extern int drwarc_(int *, double *, double *,
16704 double *, int *);
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
16774
16775
16776
16777
16778
16779
16780
16781
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
16841
16842
16843
16844
16845
16846
16847 --lend;
16848 --z__;
16849 --y;
16850 --x;
16851 --zc;
16852 --yc;
16853 --xc;
16854 --listc;
16855 --lptr;
16856
16857
16858
16859
16860
16861
16862
16863
16864
16865
16866
16867
16868
16869
16870
16871
16872
16873
16874
16875
16876
16877
16878
16879
16880
16881
16882
16883
16884
16885
16886
16887
16888
16889
16890
16891
16892
16893
16894
16895
16896
16897
16898
16899
16900
16901
16902
16903
16904
16905
16906
16907
16908
16909
16910
16911
16912
16913
16914 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
16915 nt != 2 * *n - 4) {
16916 goto L11;
16917 }
16918 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16919 goto L12;
16920 }
16921
16922
16923
16924
16925 cf = atan(1.) / 45.;
16926 wr = sin(cf * *a);
16927 wrs = wr * wr;
16928
16929
16930
16931
16932
16933
16934
16935
16936
16937
16938 d__1 = *pltsiz * 36.;
16939 ir = i_dnnt(&d__1);
16940 ipx1 = 306 - ir;
16941 ipx2 = ir + 306;
16942 ipy1 = 396 - ir;
16943 ipy2 = ir + 396;
16944
16945
16946
16947
16948
16949
16950
16951
16952
16953
16954
16955
16956
16957 d__1 = (double) ir * .88;
16958 ir = i_dnnt(&d__1);
16959 ipx1 = 306 - ir;
16960 ipx2 = ir + 306;
16961 ipy1 = 396 - ir;
16962 ipy2 = ir + 396;
16963
16964
16965
16966
16967 t = 2.;
16968
16969
16970
16971
16972
16973
16974
16975
16976
16977
16978 sf = (double) ir / wr;
16979 tx = ipx1 + sf * wr;
16980 ty = ipy1 + sf * wr;
16981
16982
16983
16984
16985
16986
16987
16988
16989 t = 1. / sf;
16990
16991
16992
16993
16994
16995
16996
16997
16998
16999
17000
17001
17002
17003
17004
17005
17006
17007
17008
17009
17010
17011
17012
17013 t = cf * *elon;
17014 ct = cos(cf * *elat);
17015 ex = ct * cos(t);
17016 ey = ct * sin(t);
17017 ez = sin(cf * *elat);
17018 if (ct != 0.) {
17019 r11 = -ey / ct;
17020 r12 = ex / ct;
17021 } else {
17022 r11 = 0.;
17023 r12 = 1.;
17024 }
17025 r21 = -ez * r12;
17026 r22 = ez * r11;
17027 r23 = ct;
17028
17029
17030
17031
17032 i__1 = *n;
17033 for (n0 = 1; n0 <= i__1; ++n0) {
17034 lpl = lend[n0];
17035
17036
17037
17038
17039 kv2 = listc[lpl];
17040 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17041 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17042 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17043
17044
17045
17046 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17047
17048
17049
17050
17051 lp = lpl;
17052 L1:
17053 lp = lptr[lp];
17054 kv1 = kv2;
17055 p1[0] = p2[0];
17056 p1[1] = p2[1];
17057 p1[2] = p2[2];
17058 in1 = in2;
17059 kv2 = listc[lp];
17060
17061
17062
17063 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17064 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17065 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17066 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17067
17068
17069
17070
17071
17072 if (! in1 || (in2 && kv2 <= kv1)) {
17073 goto L2;
17074 }
17075 if (p2[2] < 0.) {
17076
17077
17078
17079
17080
17081 p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17082 p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17083 t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17084 p2[0] /= t;
17085 p2[1] /= t;
17086 }
17087
17088
17089
17090
17091 if (p2[2] < 0.) {
17092 p2[2] = 0.f;
17093 }
17094 d__1 = tol / sf;
17095 drwarc_(lun, p1, p2, &d__1, &nseg);
17096
17097
17098
17099 L2:
17100 if (lp != lpl) {
17101 goto L1;
17102 }
17103
17104 }
17105
17106
17107
17108
17109
17110
17111
17112 if (*numbr) {
17113
17114
17115
17116
17117
17118 t = fsizn / sf;
17119
17120
17121
17122
17123
17124
17125
17126 i__1 = *n;
17127 for (n0 = 1; n0 <= i__1; ++n0) {
17128 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17129 goto L4;
17130 }
17131 x0 = r11 * x[n0] + r12 * y[n0];
17132 y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17133 if (x0 * x0 + y0 * y0 > wrs) {
17134 goto L4;
17135 }
17136
17137
17138
17139
17140
17141
17142
17143
17144 L4:
17145 ;
17146 }
17147 }
17148
17149
17150
17151
17152 t = fsizt / sf;
17153
17154
17155
17156
17157 y0 = wr + t * 3.;
17158
17159
17160
17161
17162
17163 if (annot) {
17164
17165
17166
17167 x0 = -wr;
17168 y0 = -wr - 50. / sf;
17169
17170
17171 y0 -= t * 2.;
17172
17173
17174
17175
17176
17177 }
17178
17179
17180
17181
17182
17183
17184
17185
17186
17187
17188
17189
17190
17191
17192
17193
17194
17195
17196 *ier = 0;
17197 return 0;
17198
17199
17200
17201 L11:
17202 *ier = 1;
17203 return 0;
17204
17205
17206
17207 L12:
17208 *ier = 2;
17209 return 0;
17210
17211
17212
17213
17214 *ier = 3;
17215 return 0;
17216 }
17217
17218 int random_(int *ix, int *iy, int *iz,
17219 double *rannum)
17220 {
17221 static double x;
17222
17223
17224
17225
17226
17227
17228
17229
17230
17231
17232
17233
17234
17235
17236
17237 *ix = *ix * 171 % 30269;
17238 *iy = *iy * 172 % 30307;
17239 *iz = *iz * 170 % 30323;
17240 x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17241 double) (*iz) / 30323.;
17242 *rannum = x - (int) x;
17243 return 0;
17244 }
17245
17246 #undef TRUE_
17247 #undef FALSE_
17248 #undef abs
17249
17250
17251
17252
17253
17254
17255
17256
17257
17258 EMData* Util::mult_scalar(EMData* img, float scalar)
17259 {
17260 ENTERFUNC;
17261
17262 if (!img) {
17263 throw NullPointerException("NULL input image");
17264 }
17265
17266
17267 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17268 int size = nx*ny*nz;
17269 EMData * img2 = img->copy_head();
17270 float *img_ptr =img->get_data();
17271 float *img2_ptr = img2->get_data();
17272 for (int i=0;i<size;i++)img2_ptr[i] = img_ptr[i]*scalar;
17273 img2->update();
17274
17275 if(img->is_complex()) {
17276 img2->set_complex(true);
17277 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17278 }
17279 EXITFUNC;
17280 return img2;
17281 }
17282
17283 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17284 {
17285 ENTERFUNC;
17286
17287 if (!img) {
17288 throw NullPointerException("NULL input image");
17289 }
17290
17291
17292 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17293 int size = nx*ny*nz;
17294 EMData * img2 = img->copy_head();
17295 float *img_ptr =img->get_data();
17296 float *img2_ptr = img2->get_data();
17297 float *img1_ptr = img1->get_data();
17298 for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17299 img2->update();
17300 if(img->is_complex()) {
17301 img2->set_complex(true);
17302 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17303 }
17304
17305 EXITFUNC;
17306 return img2;
17307 }
17308
17309 EMData* Util::addn_img(EMData* img, EMData* img1)
17310 {
17311 ENTERFUNC;
17312
17313 if (!img) {
17314 throw NullPointerException("NULL input image");
17315 }
17316
17317
17318 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17319 int size = nx*ny*nz;
17320 EMData * img2 = img->copy_head();
17321 float *img_ptr =img->get_data();
17322 float *img2_ptr = img2->get_data();
17323 float *img1_ptr = img1->get_data();
17324 for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17325 img2->update();
17326 if(img->is_complex()) {
17327 img2->set_complex(true);
17328 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17329 }
17330
17331 EXITFUNC;
17332 return img2;
17333 }
17334
17335 EMData* Util::subn_img(EMData* img, EMData* img1)
17336 {
17337 ENTERFUNC;
17338
17339 if (!img) {
17340 throw NullPointerException("NULL input image");
17341 }
17342
17343
17344 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17345 int size = nx*ny*nz;
17346 EMData * img2 = img->copy_head();
17347 float *img_ptr =img->get_data();
17348 float *img2_ptr = img2->get_data();
17349 float *img1_ptr = img1->get_data();
17350 for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17351 img2->update();
17352 if(img->is_complex()) {
17353 img2->set_complex(true);
17354 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17355 }
17356
17357 EXITFUNC;
17358 return img2;
17359 }
17360
17361 EMData* Util::muln_img(EMData* img, EMData* img1)
17362 {
17363 ENTERFUNC;
17364
17365 if (!img) {
17366 throw NullPointerException("NULL input image");
17367 }
17368
17369
17370 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17371 int size = nx*ny*nz;
17372 EMData * img2 = img->copy_head();
17373 float *img_ptr =img->get_data();
17374 float *img2_ptr = img2->get_data();
17375 float *img1_ptr = img1->get_data();
17376 if(img->is_complex()) {
17377 for (int i=0; i<size; i+=2) {
17378 img2_ptr[i] = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17379 img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17380 }
17381 img2->set_complex(true);
17382 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17383 } else {
17384 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17385 img2->update();
17386 }
17387
17388 EXITFUNC;
17389 return img2;
17390 }
17391
17392 EMData* Util::divn_img(EMData* img, EMData* img1)
17393 {
17394 ENTERFUNC;
17395
17396 if (!img) {
17397 throw NullPointerException("NULL input image");
17398 }
17399
17400
17401 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17402 int size = nx*ny*nz;
17403 EMData * img2 = img->copy_head();
17404 float *img_ptr =img->get_data();
17405 float *img2_ptr = img2->get_data();
17406 float *img1_ptr = img1->get_data();
17407 if(img->is_complex()) {
17408 float sq2;
17409 for (int i=0; i<size; i+=2) {
17410 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17411 img2_ptr[i] = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17412 img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17413 }
17414 img2->set_complex(true);
17415 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17416 } else {
17417 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17418 img2->update();
17419 }
17420
17421 EXITFUNC;
17422 return img2;
17423 }
17424
17425 EMData* Util::divn_filter(EMData* img, EMData* img1)
17426 {
17427 ENTERFUNC;
17428
17429 if (!img) {
17430 throw NullPointerException("NULL input image");
17431 }
17432
17433
17434 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17435 int size = nx*ny*nz;
17436 EMData * img2 = img->copy_head();
17437 float *img_ptr =img->get_data();
17438 float *img1_ptr = img1->get_data();
17439 float *img2_ptr = img2->get_data();
17440 if(img->is_complex()) {
17441 for (int i=0; i<size; i+=2) {
17442 if(img1_ptr[i] > 1.e-10f) {
17443 img2_ptr[i] = img_ptr[i] /img1_ptr[i];
17444 img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17445 } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17446 }
17447 } else throw ImageFormatException("Only Fourier image allowed");
17448
17449 img->update();
17450
17451 EXITFUNC;
17452 return img2;
17453 }
17454
17455 void Util::mul_scalar(EMData* img, float scalar)
17456 {
17457 ENTERFUNC;
17458
17459 if (!img) {
17460 throw NullPointerException("NULL input image");
17461 }
17462
17463
17464 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17465 int size = nx*ny*nz;
17466 float *img_ptr =img->get_data();
17467 for (int i=0;i<size;i++) img_ptr[i] *= scalar;
17468 img->update();
17469
17470 EXITFUNC;
17471 }
17472
17473 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17474 {
17475 ENTERFUNC;
17476
17477 if (!img) {
17478 throw NullPointerException("NULL input image");
17479 }
17480
17481
17482 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17483 int size = nx*ny*nz;
17484 float *img_ptr =img->get_data();
17485 float *img1_ptr = img1->get_data();
17486 for (int i=0;i<size;i++)img_ptr[i] += img1_ptr[i]*scalar;
17487 img1->update();
17488
17489 EXITFUNC;
17490 }
17491
17492 void Util::add_img(EMData* img, EMData* img1)
17493 {
17494 ENTERFUNC;
17495
17496 if (!img) {
17497 throw NullPointerException("NULL input image");
17498 }
17499
17500
17501 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17502 int size = nx*ny*nz;
17503 float *img_ptr = img->get_data();
17504 float *img1_ptr = img1->get_data();
17505 for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i];
17506 img->update();
17507
17508 EXITFUNC;
17509 }
17510
17511 void Util::add_img_abs(EMData* img, EMData* img1)
17512 {
17513 ENTERFUNC;
17514
17515 if (!img) {
17516 throw NullPointerException("NULL input image");
17517 }
17518
17519
17520 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17521 int size = nx*ny*nz;
17522 float *img_ptr = img->get_data();
17523 float *img1_ptr = img1->get_data();
17524 for (int i=0;i<size;i++) img_ptr[i] += abs(img1_ptr[i]);
17525 img->update();
17526
17527 EXITFUNC;
17528 }
17529
17530 void Util::add_img2(EMData* img, EMData* img1)
17531 {
17532 ENTERFUNC;
17533
17534 if (!img) {
17535 throw NullPointerException("NULL input image");
17536 }
17537
17538
17539 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17540 int size = nx*ny*nz;
17541 float *img_ptr = img->get_data();
17542 float *img1_ptr = img1->get_data();
17543 if(img->is_complex()) {
17544 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] ;
17545 } else {
17546 for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17547 }
17548 img->update();
17549
17550 EXITFUNC;
17551 }
17552
17553 void Util::sub_img(EMData* img, EMData* img1)
17554 {
17555 ENTERFUNC;
17556
17557 if (!img) {
17558 throw NullPointerException("NULL input image");
17559 }
17560
17561
17562 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17563 int size = nx*ny*nz;
17564 float *img_ptr = img->get_data();
17565 float *img1_ptr = img1->get_data();
17566 for (int i=0;i<size;i++) img_ptr[i] -= img1_ptr[i];
17567 img->update();
17568
17569 EXITFUNC;
17570 }
17571
17572 void Util::mul_img(EMData* img, EMData* img1)
17573 {
17574 ENTERFUNC;
17575
17576 if (!img) {
17577 throw NullPointerException("NULL input image");
17578 }
17579
17580
17581 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17582 int size = nx*ny*nz;
17583 float *img_ptr = img->get_data();
17584 float *img1_ptr = img1->get_data();
17585 if(img->is_complex()) {
17586 for (int i=0; i<size; i+=2) {
17587 float tmp = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17588 img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17589 img_ptr[i] = tmp;
17590
17591 }
17592 } else {
17593 for (int i=0;i<size;i++) img_ptr[i] *= img1_ptr[i];
17594 }
17595 img->update();
17596
17597 EXITFUNC;
17598 }
17599
17600 void Util::div_img(EMData* img, EMData* img1)
17601 {
17602 ENTERFUNC;
17603
17604 if (!img) {
17605 throw NullPointerException("NULL input image");
17606 }
17607
17608
17609 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17610 int size = nx*ny*nz;
17611 float *img_ptr = img->get_data();
17612 float *img1_ptr = img1->get_data();
17613 if(img->is_complex()) {
17614 float sq2;
17615 for (int i=0; i<size; i+=2) {
17616 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17617 float tmp = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17618 img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17619 img_ptr[i] = tmp;
17620 }
17621 } else {
17622 for (int i=0; i<size; i++) img_ptr[i] /= img1_ptr[i];
17623 }
17624 img->update();
17625
17626 EXITFUNC;
17627 }
17628
17629 void Util::div_filter(EMData* img, EMData* img1)
17630 {
17631 ENTERFUNC;
17632
17633 if (!img) {
17634 throw NullPointerException("NULL input image");
17635 }
17636
17637
17638 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17639 int size = nx*ny*nz;
17640 float *img_ptr = img->get_data();
17641 float *img1_ptr = img1->get_data();
17642 if(img->is_complex()) {
17643 for (int i=0; i<size; i+=2) {
17644 if(img1_ptr[i] > 1.e-10f) {
17645 img_ptr[i] /= img1_ptr[i];
17646 img_ptr[i+1] /= img1_ptr[i];
17647 } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17648 }
17649 } else throw ImageFormatException("Only Fourier image allowed");
17650
17651 img->update();
17652
17653 EXITFUNC;
17654 }
17655
17656 #define img_ptr(i,j,k) img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*nxo]
17657
17658 EMData* Util::pack_complex_to_real(EMData* img)
17659 {
17660 ENTERFUNC;
17661
17662 if (!img) {
17663 throw NullPointerException("NULL input image");
17664 }
17665
17666
17667
17668 int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17669 int nx = nxo - 2 + img->is_fftodd();
17670 int lsd2 = (nx + 2 - nx%2) / 2;
17671 int nyt, nzt;
17672 int nx2 = nx/2;
17673 int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17674 int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17675 int nx2p = nx2+nx%2;
17676 int ny2p = ny2+ny%2;
17677 int nz2p = nz2+nz%2;
17678 EMData& power = *(new EMData());
17679 power.set_size(nx, ny, nz);
17680 power.set_array_offsets(-nx2,-ny2,-nz2);
17681
17682 float *img_ptr = img->get_data();
17683 for (int iz = 1; iz <= nz; iz++) {
17684 int jz=iz-1;
17685 if(jz>=nz2p) jz=jz-nzt;
17686 for (int iy = 1; iy <= ny; iy++) {
17687 int jy=iy-1;
17688 if(jy>=ny2p) jy=jy-nyt;
17689 for (int ix = 1; ix <= lsd2; ix++) {
17690 int jx=ix-1;
17691 if(jx>=nx2p) jx=jx-nx;
17692 power(jx,jy,jz) = img_ptr(ix,iy,iz);
17693 }
17694 }
17695 }
17696
17697 int nzb, nze, nyb, nye, nxb, nxe;
17698 nxb =-nx2+(nx+1)%2;
17699 nxe = nx2-(nx+1)%2;
17700 if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17701 if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17702 for (int iz = nzb; iz <= nze; iz++) {
17703 for (int iy = nyb; iy <= nye; iy++) {
17704 for (int ix = 1; ix <= nxe; ix++) {
17705 power(-ix,-iy,-iz) = power(ix,iy,iz);
17706 }
17707 }
17708 }
17709 if(ny2 != 0) {
17710 if(nz2 != 0) {
17711 if(nz%2 == 0) {
17712 for (int iy = nyb; iy <= nye; iy++) {
17713 for (int ix = nxb; ix <= -1; ix++) {
17714 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17715 }
17716 }
17717 if(ny%2 == 0) {
17718 for (int ix = nxb; ix <= -1; ix++) {
17719 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17720 }
17721 }
17722 }
17723 }
17724 if(ny%2 == 0) {
17725 for (int iz = nzb; iz <= nze; iz++) {
17726 for (int ix = nxb; ix <= -1; ix++) {
17727 power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17728 }
17729 }
17730 }
17731
17732 }
17733 power.update();
17734 power.set_array_offsets(0,0,0);
17735 return &power;
17736 }
17737 #undef img_ptr
17738
17739 float Util::ang_n(float peakp, string mode, int maxrin)
17740 {
17741 if (mode == "f" || mode == "F")
17742 return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17743 else
17744 return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17745 }
17746
17747
17748 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17749 {
17750 float* data = ring->get_data();
17751 float av=0.0;
17752 float sq=0.0;
17753 float nn=0.0;
17754 int nring = numr.size()/3;
17755 for( int i=0; i < nring; ++i )
17756 {
17757 int numr3i = numr[3*i+2];
17758 int numr2i = numr[3*i+1]-1;
17759 float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17760 for( int j=0; j < numr3i; ++j )
17761 {
17762 int jc = numr2i+j;
17763 av += data[jc] * w;
17764 sq += data[jc] * data[jc] * w;
17765 nn += w;
17766 }
17767 }
17768
17769 float avg = av/nn;
17770 float sgm = sqrt( (sq-av*av/nn)/nn );
17771 int n = ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17772 for( int i=0; i < n; ++i )
17773 {
17774 data[i] -= avg;
17775 data[i] /= sgm;
17776 }
17777
17778 ring->update();
17779 }
17780
17781 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17782 float xrng, float yrng, float step, string mode,
17783 vector<int>numr, float cnx, float cny) {
17784
17785
17786
17787
17788
17789
17790
17791
17792
17793
17794
17795
17796 size_t crefim_len = crefim.size();
17797
17798 int ky = int(2*yrng/step+0.5)/2;
17799 int kx = int(2*xrng/step+0.5)/2;
17800 int iref, nref=0, mirror=0;
17801 float iy, ix, sx=0, sy=0;
17802 float peak = -1.0E23f;
17803 float ang=0.0f;
17804 for (int i = -ky; i <= ky; i++) {
17805 iy = i * step ;
17806 for (int j = -kx; j <= kx; j++) {
17807 ix = j*step ;
17808 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17809
17810 Normalize_ring( cimage, numr );
17811
17812 Frngs(cimage, numr);
17813
17814
17815 for ( iref = 0; iref < (int)crefim_len; iref++) {
17816 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17817 double qn = retvals["qn"];
17818 double qm = retvals["qm"];
17819 if(qn >= peak || qm >= peak) {
17820 sx = -ix;
17821 sy = -iy;
17822 nref = iref;
17823 if (qn >= qm) {
17824 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17825 peak = static_cast<float>(qn);
17826 mirror = 0;
17827 } else {
17828 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17829 peak = static_cast<float>(qm);
17830 mirror = 1;
17831 }
17832 }
17833 } delete cimage; cimage = 0;
17834 }
17835 }
17836 float co, so, sxs, sys;
17837 co = static_cast<float>( cos(ang*pi/180.0) );
17838 so = static_cast<float>( -sin(ang*pi/180.0) );
17839 sxs = sx*co - sy*so;
17840 sys = sx*so + sy*co;
17841 vector<float> res;
17842 res.push_back(ang);
17843 res.push_back(sxs);
17844 res.push_back(sys);
17845 res.push_back(static_cast<float>(mirror));
17846 res.push_back(static_cast<float>(nref));
17847 res.push_back(peak);
17848 return res;
17849 }
17850
17851 vector<float> Util::multiref_polar_ali_2d_peaklist(EMData* image, const vector< EMData* >& crefim,
17852 float xrng, float yrng, float step, string mode,
17853 vector<int>numr, float cnx, float cny) {
17854
17855 size_t crefim_len = crefim.size();
17856
17857 int ky = int(2*yrng/step+0.5)/2;
17858 int kx = int(2*xrng/step+0.5)/2;
17859 float iy, ix;
17860 vector<float> peak(crefim_len*5, -1.0e23f);
17861 for (int i = -ky; i <= ky; i++) {
17862 iy = i * step ;
17863 for (int j = -kx; j <= kx; j++) {
17864 ix = j*step ;
17865 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17866 Normalize_ring( cimage, numr );
17867 Frngs(cimage, numr);
17868 for (int iref = 0; iref < (int)crefim_len; iref++) {
17869 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17870 double qn = retvals["qn"];
17871 double qm = retvals["qm"];
17872 if(qn >= peak[iref*5] || qm >= peak[iref*5]) {
17873 if (qn >= qm) {
17874 peak[iref*5] = static_cast<float>(qn);
17875 peak[iref*5+1] = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17876 peak[iref*5+2] = -ix;
17877 peak[iref*5+3] = -iy;
17878 peak[iref*5+4] = 0;
17879 } else {
17880 peak[iref*5] = static_cast<float>(qm);
17881 peak[iref*5+1] = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17882 peak[iref*5+2] = -ix;
17883 peak[iref*5+3] = -iy;
17884 peak[iref*5+4] = 1;
17885 }
17886 }
17887 } delete cimage; cimage = 0;
17888 }
17889 }
17890 for (int iref = 0; iref < (int)crefim_len; iref++) {
17891 float ang = peak[iref*5+1];
17892 float sx = peak[iref*5+2];
17893 float sy = peak[iref*5+3];
17894 float co = cos(ang*pi/180.0);
17895 float so = -sin(ang*pi/180.0);
17896 float sxs = sx*co - sy*so;
17897 float sys = sx*so + sy*co;
17898 peak[iref*5+2] = sxs;
17899 peak[iref*5+3] = sys;
17900 }
17901 return peak;
17902 }
17903
17904
17905 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
17906 float xrng, float yrng, float step, string mode,
17907 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
17908
17909
17910
17911
17912
17913
17914
17915
17916
17917
17918
17919
17920 size_t crefim_len = crefim.size();
17921
17922 int ky = int(2*yrng/step+0.5)/2;
17923 int kx = int(2*xrng/step+0.5)/2;
17924 int iref, nref=0, mirror=0;
17925 float iy, ix, sx=0, sy=0;
17926 float peak = -1.0E23f;
17927 float ang=0.0f;
17928 for (int i = -ky; i <= ky; i++) {
17929 iy = i * step ;
17930 for (int j = -kx; j <= kx; j++) {
17931 ix = j*step ;
17932 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17933
17934 Normalize_ring( cimage, numr );
17935
17936 Frngs(cimage, numr);
17937
17938
17939 for ( iref = 0; iref < (int)crefim_len; iref++) {
17940 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
17941 double qn = retvals["qn"];
17942 double qm = retvals["qm"];
17943 if(qn >= peak || qm >= peak) {
17944 sx = -ix;
17945 sy = -iy;
17946 nref = iref;
17947 if (qn >= qm) {
17948 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17949 peak = static_cast<float>(qn);
17950 mirror = 0;
17951 } else {
17952 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17953 peak = static_cast<float>(qm);
17954 mirror = 1;
17955 }
17956 }
17957 } delete cimage; cimage = 0;
17958 }
17959 }
17960 float co, so, sxs, sys;
17961 co = static_cast<float>( cos(ang*pi/180.0) );
17962 so = static_cast<float>( -sin(ang*pi/180.0) );
17963 sxs = sx*co - sy*so;
17964 sys = sx*so + sy*co;
17965 vector<float> res;
17966 res.push_back(ang);
17967 res.push_back(sxs);
17968 res.push_back(sys);
17969 res.push_back(static_cast<float>(mirror));
17970 res.push_back(static_cast<float>(nref));
17971 res.push_back(peak);
17972 return res;
17973 }
17974
17975 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
17976 float xrng, float yrng, float step, string mode,
17977 vector< int >numr, float cnx, float cny) {
17978
17979
17980
17981
17982
17983
17984
17985
17986
17987
17988
17989 size_t crefim_len = crefim.size();
17990
17991 int ky = int(2*yrng/step+0.5)/2;
17992 int kx = int(2*xrng/step+0.5)/2;
17993 int iref, nref=0;
17994 float iy, ix, sx=0, sy=0;
17995 float peak = -1.0E23f;
17996 float ang=0.0f;
17997 for (int i = -ky; i <= ky; i++) {
17998 iy = i * step ;
17999 for (int j = -kx; j <= kx; j++) {
18000 ix = j*step ;
18001 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18002 Frngs(cimage, numr);
18003
18004
18005 for ( iref = 0; iref < (int)crefim_len; iref++) {
18006 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
18007 double qn = retvals["qn"];
18008 if(qn >= peak) {
18009 sx = -ix;
18010 sy = -iy;
18011 nref = iref;
18012 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18013 peak = static_cast<float>(qn);
18014 }
18015 } delete cimage; cimage = 0;
18016 }
18017 }
18018 float co, so, sxs, sys;
18019 co = static_cast<float>( cos(ang*pi/180.0) );
18020 so = static_cast<float>( -sin(ang*pi/180.0) );
18021 sxs = sx*co - sy*so;
18022 sys = sx*so + sy*co;
18023 vector<float> res;
18024 res.push_back(ang);
18025 res.push_back(sxs);
18026 res.push_back(sys);
18027 res.push_back(static_cast<float>(nref));
18028 res.push_back(peak);
18029 return res;
18030 }
18031
18032 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
18033 float xrng, float yrng, float step, float ant, string mode,
18034 vector<int>numr, float cnx, float cny) {
18035
18036
18037
18038
18039
18040
18041
18042
18043
18044
18045
18046 size_t crefim_len = crefim.size();
18047 const float qv = static_cast<float>( pi/180.0 );
18048
18049 Transform * t = image->get_attr("xform.projection");
18050 Dict d = t->get_params("spider");
18051 if(t) {delete t; t=0;}
18052 float phi = d["phi"];
18053 float theta = d["theta"];
18054 int ky = int(2*yrng/step+0.5)/2;
18055 int kx = int(2*xrng/step+0.5)/2;
18056 int iref, nref=0, mirror=0;
18057 float iy, ix, sx=0, sy=0;
18058 float peak = -1.0E23f;
18059 float ang=0.0f;
18060 float imn1 = sin(theta*qv)*cos(phi*qv);
18061 float imn2 = sin(theta*qv)*sin(phi*qv);
18062 float imn3 = cos(theta*qv);
18063 vector<float> n1(crefim_len);
18064 vector<float> n2(crefim_len);
18065 vector<float> n3(crefim_len);
18066 for ( iref = 0; iref < (int)crefim_len; iref++) {
18067 n1[iref] = crefim[iref]->get_attr("n1");
18068 n2[iref] = crefim[iref]->get_attr("n2");
18069 n3[iref] = crefim[iref]->get_attr("n3");
18070 }
18071 for (int i = -ky; i <= ky; i++) {
18072 iy = i * step ;
18073 for (int j = -kx; j <= kx; j++) {
18074 ix = j*step;
18075 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18076
18077 Normalize_ring( cimage, numr );
18078
18079 Frngs(cimage, numr);
18080
18081
18082 for ( iref = 0; iref < (int)crefim_len; iref++) {
18083 if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18084 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18085 double qn = retvals["qn"];
18086 double qm = retvals["qm"];
18087 if(qn >= peak || qm >= peak) {
18088 sx = -ix;
18089 sy = -iy;
18090 nref = iref;
18091 if (qn >= qm) {
18092 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18093 peak = static_cast<float>( qn );
18094 mirror = 0;
18095 } else {
18096 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18097 peak = static_cast<float>( qm );
18098 mirror = 1;
18099 }
18100 }
18101 }
18102 } delete cimage; cimage = 0;
18103 }
18104 }
18105 float co, so, sxs, sys;
18106 if(peak == -1.0E23) {
18107 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18108 nref = -1;
18109 } else {
18110 co = cos(ang*qv);
18111 so = -sin(ang*qv);
18112 sxs = sx*co - sy*so;
18113 sys = sx*so + sy*co;
18114 }
18115 vector<float> res;
18116 res.push_back(ang);
18117 res.push_back(sxs);
18118 res.push_back(sys);
18119 res.push_back(static_cast<float>(mirror));
18120 res.push_back(static_cast<float>(nref));
18121 res.push_back(peak);
18122 return res;
18123 }
18124
18125 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18126 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18127 vector<int>numr, float cnx, float cny) {
18128
18129
18130
18131
18132
18133
18134
18135
18136
18137
18138
18139 size_t crefim_len = crefim.size();
18140 const float qv = static_cast<float>(pi/180.0);
18141
18142 Transform* t = image->get_attr("xform.projection");
18143 Dict d = t->get_params("spider");
18144 if(t) {delete t; t=0;}
18145 float phi = d["phi"];
18146 float theta = d["theta"];
18147 float psi = d["psi"];
18148 int ky = int(2*yrng/step+0.5)/2;
18149 int kx = int(2*xrng/step+0.5)/2;
18150 int iref, nref = 0, mirror = 0;
18151 float iy, ix, sx = 0, sy = 0;
18152 float peak = -1.0E23f;
18153 float ang = 0.0f;
18154 float imn1 = sin(theta*qv)*cos(phi*qv);
18155 float imn2 = sin(theta*qv)*sin(phi*qv);
18156 float imn3 = cos(theta*qv);
18157 vector<float> n1(crefim_len);
18158 vector<float> n2(crefim_len);
18159 vector<float> n3(crefim_len);
18160 for (iref = 0; iref < (int)crefim_len; iref++) {
18161 n1[iref] = crefim[iref]->get_attr("n1");
18162 n2[iref] = crefim[iref]->get_attr("n2");
18163 n3[iref] = crefim[iref]->get_attr("n3");
18164 }
18165 bool nomirror = (theta<90.0) || ((theta==90.0) && (psi<psi_max));
18166 if (!nomirror) {
18167 phi = fmod(phi+540.0f, 360.0f);
18168 theta = 180-theta;
18169 psi = fmod(540.0f-psi, 360.0f);
18170 }
18171 for (int i = -ky; i <= ky; i++) {
18172 iy = i * step ;
18173 for (int j = -kx; j <= kx; j++) {
18174 ix = j*step;
18175 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18176
18177 Normalize_ring(cimage, numr);
18178
18179 Frngs(cimage, numr);
18180
18181
18182 for (iref = 0; iref < (int)crefim_len; iref++) {
18183 if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18184 if (nomirror) {
18185 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 0);
18186 double qn = retvals["qn"];
18187 if (qn >= peak) {
18188 sx = -ix;
18189 sy = -iy;
18190 nref = iref;
18191 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18192 peak = static_cast<float>(qn);
18193 mirror = 0;
18194 }
18195 } else {
18196 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 1);
18197 double qn = retvals["qn"];
18198 if (qn >= peak) {
18199 sx = -ix;
18200 sy = -iy;
18201 nref = iref;
18202 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18203 peak = static_cast<float>(qn);
18204 mirror = 1;
18205 }
18206 }
18207 }
18208 } delete cimage; cimage = 0;
18209 }
18210 }
18211 float co, so, sxs, sys;
18212 if(peak == -1.0E23) {
18213 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18214 nref = -1;
18215 } else {
18216 co = cos(ang*qv);
18217 so = -sin(ang*qv);
18218 sxs = sx*co - sy*so;
18219 sys = sx*so + sy*co;
18220 }
18221 vector<float> res;
18222 res.push_back(ang);
18223 res.push_back(sxs);
18224 res.push_back(sys);
18225 res.push_back(static_cast<float>(mirror));
18226 res.push_back(static_cast<float>(nref));
18227 res.push_back(peak);
18228 return res;
18229 }
18230
18231
18232 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18233 float xrng, float yrng, float step, float psi_max, string mode,
18234 vector<int>numr, float cnx, float cny, int ynumber) {
18235
18236 size_t crefim_len = crefim.size();
18237
18238 int iref, nref=0, mirror=0;
18239 float iy, ix, sx=0, sy=0;
18240 float peak = -1.0E23f;
18241 float ang=0.0f;
18242 int kx = int(2*xrng/step+0.5)/2;
18243
18244 if(ynumber==-1) {
18245 int ky = int(2*yrng/step+0.5)/2;
18246 for (int i = -ky; i <= ky; i++) {
18247 iy = i * step ;
18248 for (int j = -kx; j <= kx; j++) {
18249 ix = j*step ;
18250 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18251
18252 Normalize_ring( cimage, numr );
18253
18254 Frngs(cimage, numr);
18255
18256
18257 for ( iref = 0; iref < (int)crefim_len; iref++) {
18258 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18259 double qn = retvals["qn"];
18260 double qm = retvals["qm"];
18261 if(qn >= peak || qm >= peak) {
18262 sx = -ix;
18263 sy = -iy;
18264 nref = iref;
18265 if (qn >= qm) {
18266 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18267 peak = static_cast<float>(qn);
18268 mirror = 0;
18269 } else {
18270 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18271 peak = static_cast<float>(qm);
18272 mirror = 1;
18273 }
18274 }
18275 }
18276 delete cimage; cimage = 0;
18277 }
18278 }
18279 }
18280
18281 else if(ynumber==0) {
18282 sy = 0.0f;
18283 for (int j = -kx; j <= kx; j++) {
18284 ix = j*step ;
18285 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18286
18287 Normalize_ring( cimage, numr );
18288
18289 Frngs(cimage, numr);
18290
18291
18292 for ( iref = 0; iref < (int)crefim_len; iref++) {
18293 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18294 double qn = retvals["qn"];
18295 double qm = retvals["qm"];
18296 if(qn >= peak || qm >= peak) {
18297 sx = -ix;
18298 nref = iref;
18299 if (qn >= qm) {
18300 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18301 peak = static_cast<float>(qn);
18302 mirror = 0;
18303 } else {
18304 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18305 peak = static_cast<float>(qm);
18306 mirror = 1;
18307 }
18308 }
18309 }
18310 delete cimage; cimage = 0;
18311 }
18312 } else {
18313 int ky = int(ynumber/2);
18314 float stepy=2*yrng/ynumber;
18315
18316 for (int i = -ky+1; i <= ky; i++) {
18317 iy = i * stepy ;
18318 for (int j = -kx; j <= kx; j++) {
18319 ix = j*step ;
18320 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18321
18322 Normalize_ring( cimage, numr );
18323
18324 Frngs(cimage, numr);
18325
18326
18327 for ( iref = 0; iref < (int)crefim_len; iref++) {
18328 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18329 double qn = retvals["qn"];
18330 double qm = retvals["qm"];
18331 if(qn >= peak || qm >= peak) {
18332 sx = -ix;
18333 sy = -iy;
18334 nref = iref;
18335 if (qn >= qm) {
18336 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18337 peak = static_cast<float>(qn);
18338 mirror = 0;
18339 } else {
18340 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18341 peak = static_cast<float>(qm);
18342 mirror = 1;
18343 }
18344 }
18345 }
18346 delete cimage; cimage = 0;
18347 }
18348 }
18349 }
18350 float co, so, sxs, sys;
18351 co = static_cast<float>( cos(ang*pi/180.0) );
18352 so = static_cast<float>( -sin(ang*pi/180.0) );
18353 sxs = sx*co - sy*so;
18354 sys = sx*so + sy*co;
18355 vector<float> res;
18356 res.push_back(ang);
18357 res.push_back(sxs);
18358 res.push_back(sys);
18359 res.push_back(static_cast<float>(mirror));
18360 res.push_back(static_cast<float>(nref));
18361 res.push_back(peak);
18362 return res;
18363 }
18364
18365 void Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
18366 float xrng, float yrng, float step, string mode,
18367 vector< int >numr, float cnx, float cny,
18368 EMData *peaks, EMData *peakm) {
18369
18370 int maxrin = numr[numr.size()-1];
18371
18372 int ky = int(2*yrng/step+0.5)/2;
18373 int kx = int(2*xrng/step+0.5)/2;
18374
18375 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18376 float *p_ccf1ds = peaks->get_data();
18377
18378 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18379 float *p_ccf1dm = peakm->get_data();
18380
18381 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18382 p_ccf1ds[i] = -1.e20f;
18383 p_ccf1dm[i] = -1.e20f;
18384 }
18385
18386 for (int i = -ky; i <= ky; i++) {
18387 float iy = i * step;
18388 for (int j = -kx; j <= kx; j++) {
18389 float ix = j*step;
18390 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18391 Frngs(cimage, numr);
18392 Crosrng_msg_vec(crefim, cimage, numr,
18393 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18394 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18395 delete cimage; cimage = 0;
18396 }
18397 }
18398 return;
18399 }
18400
18401 void Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
18402 float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
18403 EMData *peaks_compress, EMData *peakm_compress) {
18404
18405 int maxrin = numr[numr.size()-1];
18406
18407 int ky = int(2*yrng/step+0.5)/2;
18408 int kx = int(2*xrng/step+0.5)/2;
18409
18410 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18411 float *p_ccf1ds = peaks->get_data();
18412
18413 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18414 float *p_ccf1dm = peakm->get_data();
18415
18416 peaks_compress->set_size(maxrin, 1, 1);
18417 float *p_ccf1ds_compress = peaks_compress->get_data();
18418
18419 peakm_compress->set_size(maxrin, 1, 1);
18420 float *p_ccf1dm_compress = peakm_compress->get_data();
18421
18422 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18423 p_ccf1ds[i] = -1.e20f;
18424 p_ccf1dm[i] = -1.e20f;
18425 }
18426
18427 for (int i = -ky; i <= ky; i++) {
18428 float iy = i * step;
18429 for (int j = -kx; j <= kx; j++) {
18430 float ix = j*step;
18431 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18432 Frngs(cimage, numr);
18433 Crosrng_msg_vec(crefim, cimage, numr,
18434 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18435 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18436 delete cimage; cimage = 0;
18437 }
18438 }
18439 for (int x=0; x<maxrin; x++) {
18440 float maxs = -1.0e22f;
18441 float maxm = -1.0e22f;
18442 for (int i=1; i<=2*ky+1; i++) {
18443 for (int j=1; j<=2*kx+1; j++) {
18444 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
18445 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
18446 }
18447 }
18448 p_ccf1ds_compress[x] = maxs;
18449 p_ccf1dm_compress[x] = maxm;
18450 }
18451 return;
18452 }
18453
18454 struct ccf_point
18455 {
18456 float value;
18457 int i;
18458 int j;
18459 int k;
18460 int mirror;
18461 };
18462
18463
18464 struct ccf_value
18465 {
18466 bool operator()( const ccf_point& a, const ccf_point& b )
18467 {
18468 return a.value > b.value;
18469 }
18470 };
18471
18472
18473 vector<float> Util::ali2d_ccf_list(EMData* image, EMData* crefim,
18474 float xrng, float yrng, float step, string mode,
18475 vector< int >numr, float cnx, float cny, double T) {
18476
18477 int maxrin = numr[numr.size()-1];
18478
18479 int ky = int(2*yrng/step+0.5)/2;
18480 int kx = int(2*xrng/step+0.5)/2;
18481
18482 float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
18483 float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
18484 int vol = maxrin*(2*kx+1)*(2*ky+1);
18485 vector<ccf_point> ccf(2*vol);
18486 ccf_point temp;
18487
18488 int index = 0;
18489 for (int i = -ky; i <= ky; i++) {
18490 float iy = i * step;
18491 for (int j = -kx; j <= kx; j++) {
18492 float ix = j*step;
18493 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18494 Frngs(cimage, numr);
18495 Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
18496 for (int k=0; k<maxrin; k++) {
18497 temp.value = p_ccf1ds[k];
18498 temp.i = k;
18499 temp.j = j;
18500 temp.k = i;
18501 temp.mirror = 0;
18502 ccf[index] = temp;
18503 index++;
18504 temp.value = p_ccf1dm[k];
18505 temp.mirror = 1;
18506 ccf[index] = temp;
18507 index++;
18508 }
18509 delete cimage; cimage = 0;
18510 }
18511 }
18512
18513 delete p_ccf1ds;
18514 delete p_ccf1dm;
18515 std::sort(ccf.begin(), ccf.end(), ccf_value());
18516
18517 double qt = (double)ccf[0].value;
18518 vector <double> p(2*vol), cp(2*vol);
18519
18520 double sump = 0.0;
18521 for (int i=0; i<2*vol; i++) {
18522 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
18523 sump += p[i];
18524 }
18525 for (int i=0; i<2*vol; i++) {
18526 p[i] /= sump;
18527 }
18528 for (int i=1; i<2*vol; i++) {
18529 p[i] += p[i-1];
18530 }
18531 p[2*vol-1] = 2.0;
18532
18533 float t = get_frand(0.0f, 1.0f);
18534 int select = 0;
18535 while (p[select] < t) select += 1;
18536
18537 vector<float> a(6);
18538 a[0] = ccf[select].value;
18539 a[1] = (float)ccf[select].i;
18540 a[2] = (float)ccf[select].j;
18541 a[3] = (float)ccf[select].k;
18542 a[4] = (float)ccf[select].mirror;
18543 a[5] = (float)select;
18544 return a;
18545 }
18546
18547
18548
18549
18550
18551
18552
18553
18554
18555
18556
18557
18558
18559
18560
18561
18562
18563
18564
18565
18566
18567
18568
18569
18570
18571
18572
18573
18574
18575
18576
18577
18578
18579
18580
18581
18582
18583
18584
18585
18586
18587
18588
18589
18590
18591
18592
18593
18594
18595
18596
18597
18598
18599
18600
18601
18602
18603
18604
18605
18606
18607
18608
18609
18610
18611
18612
18613
18614 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18615
18616 EMData *rot;
18617
18618 const int nmax=3, mmax=3;
18619 char task[60], csave[60];
18620 long int lsave[4];
18621 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18622 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];
18623 long int SIXTY=60;
18624
18625
18626 iprint = -1;
18627
18628
18629 factr=1.0e1;
18630 pgtol=1.0e-5;
18631
18632
18633
18634
18635 n=3;
18636 m=3;
18637
18638
18639
18640
18641
18642 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18643 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18644 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18645
18646
18647
18648
18649 strcpy(task,"START");
18650 for (int i=5;i<60;i++) task[i]=' ';
18651
18652
18653
18654 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18655
18656
18657
18658 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18659
18660 if (strncmp(task,"FG",2)==0) {
18661
18662
18663
18664
18665 rot = new EMData();
18666 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
18667 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18668
18669 delete rot;
18670
18671
18672 float dt = 1.0e-3f;
18673 rot = new EMData();
18674 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
18675 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18676
18677 g[0] = (f1-f)/dt;
18678 delete rot;
18679
18680 dt = 1.0e-2f;
18681 rot = new EMData();
18682 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
18683 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18684
18685 g[1] = (f2-f)/dt;
18686 delete rot;
18687
18688 rot = new EMData();
18689 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
18690 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18691
18692 g[2] = (f3-f)/dt;
18693 delete rot;
18694 }
18695
18696
18697 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18698
18699 }
18700
18701
18702 vector<float> res;
18703 res.push_back(static_cast<float>(x[0]));
18704 res.push_back(static_cast<float>(x[1]));
18705 res.push_back(static_cast<float>(x[2]));
18706
18707 return res;
18708 }
18709
18710 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
18711
18712 EMData *rot;
18713
18714 const int nmax=3, mmax=3;
18715 char task[60], csave[60];
18716 long int lsave[4];
18717 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18718 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];
18719 long int SIXTY=60;
18720
18721
18722 iprint = -1;
18723
18724
18725 factr=1.0e1;
18726 pgtol=1.0e-5;
18727
18728
18729
18730
18731 n=3;
18732 m=3;
18733
18734
18735
18736
18737
18738 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18739 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18740 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18741
18742
18743
18744
18745 strcpy(task,"START");
18746 for (int i=5;i<60;i++) task[i]=' ';
18747
18748
18749
18750 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18751
18752
18753
18754 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18755
18756 if (strncmp(task,"FG",2)==0) {
18757
18758
18759
18760
18761 rot = new EMData();
18762 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18763 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18764
18765 delete rot;
18766
18767
18768 float dt = 1.0e-3f;
18769 rot = new EMData();
18770 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18771 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18772
18773 g[0] = (f1-f)/dt;
18774 delete rot;
18775
18776 rot = new EMData();
18777 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
18778 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18779
18780 g[1] = (f2-f)/dt;
18781 delete rot;
18782
18783 rot = new EMData();
18784 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
18785 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18786
18787 g[2] = (f3-f)/dt;
18788 delete rot;
18789 }
18790
18791
18792 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18793
18794 }
18795
18796
18797 vector<float> res;
18798 res.push_back(static_cast<float>(x[0]));
18799 res.push_back(static_cast<float>(x[1]));
18800 res.push_back(static_cast<float>(x[2]));
18801
18802 return res;
18803 }
18804
18805 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) {
18806
18807 EMData *proj, *proj2;
18808
18809 const int nmax=5, mmax=5;
18810 char task[60], csave[60];
18811 long int lsave[4];
18812 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18813 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];
18814 long int SIXTY=60;
18815
18816
18817 iprint = -1;
18818
18819
18820 factr=1.0e1;
18821 pgtol=1.0e-5;
18822
18823
18824
18825
18826 n=5;
18827 m=5;
18828
18829
18830
18831
18832
18833 x[0] = phi; nbd[0] = 2; l[0] = phi-2.0; u[0] = phi+2.0;
18834 x[1] = theta; nbd[1] = 2; l[1] = theta-2.0; u[1] = theta+2.0;
18835 x[2] = psi; nbd[2] = 2; l[2] = psi-2.0; u[2] = psi+2.0;
18836 x[3] = sxs; nbd[3] = 2; l[3] = sxs-2.0; u[3] = sxs+2.0;
18837 x[4] = sys; nbd[4] = 2; l[4] = sys-2.0; u[4] = sys+2.0;
18838
18839
18840
18841
18842 strcpy(task,"START");
18843 for (int i=5;i<60;i++) task[i]=' ';
18844
18845
18846
18847 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18848 int step = 1;
18849
18850
18851 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18852
18853 if (strncmp(task,"FG",2)==0) {
18854
18855
18856
18857
18858 proj = new EMData();
18859 proj2 = new EMData();
18860 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18861 proj->fft_shuffle();
18862 proj->center_origin_fft();
18863 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18864 proj->do_ift_inplace();
18865 int M = proj->get_ysize()/2;
18866 proj2 = proj->window_center(M);
18867 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18868
18869 delete proj;
18870 delete proj2;
18871
18872
18873 float dt = 1.0e-3f;
18874 proj = new EMData();
18875 proj2 = new EMData();
18876 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
18877 proj->fft_shuffle();
18878 proj->center_origin_fft();
18879 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18880 proj->do_ift_inplace();
18881 proj2 = proj->window_center(M);
18882 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18883
18884 delete proj;
18885 delete proj2;
18886 g[0] = (ft-f)/dt;
18887
18888 proj = new EMData();
18889 proj2 = new EMData();
18890 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
18891 proj->fft_shuffle();
18892 proj->center_origin_fft();
18893 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18894 proj->do_ift_inplace();
18895 proj2 = proj->window_center(M);
18896 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18897
18898 delete proj;
18899 delete proj2;
18900 g[1] = (ft-f)/dt;
18901
18902 proj = new EMData();
18903 proj2 = new EMData();
18904 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
18905 proj->fft_shuffle();
18906 proj->center_origin_fft();
18907 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18908 proj->do_ift_inplace();
18909 proj2 = proj->window_center(M);
18910 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18911
18912 delete proj;
18913 delete proj2;
18914 g[2] = (ft-f)/dt;
18915
18916 proj = new EMData();
18917 proj2 = new EMData();
18918 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18919 proj->fft_shuffle();
18920 proj->center_origin_fft();
18921 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
18922 proj->do_ift_inplace();
18923 proj2 = proj->window_center(M);
18924 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18925
18926 delete proj;
18927 delete proj2;
18928 g[3] = (ft-f)/dt;
18929
18930 proj = new EMData();
18931 proj2 = new EMData();
18932 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18933 proj->fft_shuffle();
18934 proj->center_origin_fft();
18935 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
18936 proj->do_ift_inplace();
18937 proj2 = proj->window_center(M);
18938 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18939
18940 delete proj;
18941 delete proj2;
18942 g[4] = (ft-f)/dt;
18943 }
18944
18945
18946 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18947 step++;
18948 }
18949
18950
18951 vector<float> res;
18952 res.push_back(static_cast<float>(x[0]));
18953 res.push_back(static_cast<float>(x[1]));
18954 res.push_back(static_cast<float>(x[2]));
18955 res.push_back(static_cast<float>(x[3]));
18956 res.push_back(static_cast<float>(x[4]));
18957
18958 return res;
18959 }
18960
18961
18962 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18963
18964 double x[4];
18965 int n;
18966 int l = 3;
18967 int m = 200;
18968 double e = 1e-9;
18969 double step = 0.01;
18970 float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
18971
18972 x[1] = ang;
18973 x[2] = sxs;
18974 x[3] = sys;
18975
18976 Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);
18977
18978
18979 vector<float> res;
18980 res.push_back(static_cast<float>(x[1]));
18981 res.push_back(static_cast<float>(x[2]));
18982 res.push_back(static_cast<float>(x[3]));
18983 res.push_back(static_cast<float>(n));
18984 return res;
18985 }
18986
18987 vector<float> Util::multi_align_error(vector<float> args, vector<float> all_ali_params) {
18988
18989 const int nmax=args.size(), mmax=nmax;
18990 char task[60], csave[60];
18991 long int lsave[4];
18992 long int n, m, iprint, isave[44];
18993 long int* nbd = new long int[nmax];
18994 long int* iwa = new long int[3*nmax];
18995 double f, factr, pgtol;
18996 double* x = new double[nmax];
18997 double* l = new double[nmax];
18998 double* u = new double[nmax];
18999 double* g = new double[nmax];
19000 double dsave[29];
19001 double* wa = new double[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
19002 long int SIXTY=60;
19003
19004 int num_ali = nmax/3+1;
19005 int nima = all_ali_params.size()/(num_ali*4);
19006
19007
19008 iprint = -1;
19009
19010
19011 factr=1.0e1;
19012 pgtol=1.0e-9;
19013
19014
19015
19016
19017 n=nmax;
19018 m=mmax;
19019
19020
19021
19022
19023
19024 for (int i=0; i<nmax; i++) {
19025 x[i] = args[i];
19026 nbd[i] = 0;
19027 }
19028
19029
19030
19031 strcpy(task,"START");
19032 for (int i=5;i<60;i++) task[i]=' ';
19033
19034
19035
19036 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19037 int step = 1;
19038
19039
19040 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19041
19042 if (strncmp(task,"FG",2)==0) {
19043
19044
19045
19046
19047 f = multi_align_error_func(x, all_ali_params, nima, num_ali);
19048
19049
19050 multi_align_error_dfunc(x, all_ali_params, nima, num_ali, g);
19051 }
19052
19053
19054 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19055 step++;
19056 }
19057
19058
19059 vector<float> res;
19060 for (int i=0; i<nmax; i++) res.push_back(static_cast<float>(x[i]));
19061 res.push_back(f);
19062
19063 delete[] nbd;
19064 delete[] iwa;
19065 delete[] x;
19066 delete[] l;
19067 delete[] u;
19068 delete[] g;
19069 delete[] wa;
19070
19071 return res;
19072
19073 }
19074
19075 float Util::multi_align_error_func(double* x, vector<float> all_ali_params, int nima, int num_ali) {
19076
19077 float x1 = 1.0;
19078 float y1 = 0.0;
19079 float x2 = 0.0;
19080 float y2 = 1.0;
19081
19082 float all_var = 0;
19083 float* x1_new = new float[num_ali];
19084 float* y1_new = new float[num_ali];
19085 float* x2_new = new float[num_ali];
19086 float* y2_new = new float[num_ali];
19087
19088 for (int i=0; i<nima; i++) {
19089 float alpha2 = all_ali_params[(num_ali-1)*(nima*4)+i*4];
19090 float sx2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+1];
19091 float sy2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+2];
19092
19093 rot_shift(x1, y1, alpha2, sx2, sy2, x1_new+num_ali-1, y1_new+num_ali-1);
19094 rot_shift(x2, y2, alpha2, sx2, sy2, x2_new+num_ali-1, y2_new+num_ali-1);
19095 for (int j=0; j<num_ali-1; j++) {
19096 float alpha1 = all_ali_params[j*(nima*4)+i*4];
19097 float sx1 = all_ali_params[j*(nima*4)+i*4+1];
19098 float sy1 = all_ali_params[j*(nima*4)+i*4+2];
19099 int mirror1 = static_cast<int>(all_ali_params[j*(nima*4)+i*4+3]);
19100
19101 float alphai = x[j*3];
19102 float sxi = x[j*3+1];
19103 float syi = x[j*3+2];
19104
19105 float alpha12, sx12, sy12;
19106 int mirror12;
19107 if (mirror1 == 0) {
19108 alpha12 = fmod(alpha1+alphai, 360.0f);
19109 rot_shift(sx1, sy1, alphai, sxi, syi, &sx12, &sy12);
19110 mirror12 = 0;
19111 } else {
19112 alpha12 = fmod(alpha1-alphai, 360.0f);
19113 rot_shift(sx1, sy1, -alphai, -sxi, syi, &sx12, &sy12);
19114 mirror12 = 1;
19115 }
19116
19117 rot_shift(x1, y1, alpha12, sx12, sy12, x1_new+j, y1_new+j);
19118 rot_shift(x2, y2, alpha12, sx12, sy12, x2_new+j, y2_new+j);
19119 }
19120
19121 float p = var(x1_new, num_ali)+var(y1_new, num_ali)+var(x2_new, num_ali)+var(y2_new, num_ali);
19122 all_var += p;
19123 }
19124 delete[] x1_new;
19125 delete[] y1_new;
19126 delete[] x2_new;
19127 delete[] y2_new;
19128 return all_var/static_cast<float>(nima);
19129 }
19130
19131 void Util::multi_align_error_dfunc(double* x, vector<float> all_ali_params, int nima, int num_ali, double* g) {
19132
19133
19134 for (int i=0; i<num_ali*3-3; i++) g[i] = 0.0;
19135
19136 float x1 = 1.0;
19137 float y1 = 0.0;
19138 float x2 = 0.0;
19139 float y2 = 1.0;
19140
19141 float* x1_new = new float[num_ali];
19142 float* y1_new = new float[num_ali];
19143 float* x2_new = new float[num_ali];
19144 float* y2_new = new float[num_ali];
19145
19146 float* alpha12_0 = new float[num_ali-1];
19147 float* dalpha12 = new float[num_ali-1];
19148 float* dsx12 = new float[num_ali-1];
19149 float* dsy12 = new float[num_ali-1];
19150 float* mirror1_0 = new float[num_ali-1];
19151
19152 for (int i=0; i<nima; i++) {
19153
19154 float alpha2 = all_ali_params[(num_ali-1)*(nima*4)+i*4];
19155 float sx2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+1];
19156 float sy2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+2];
19157
19158 rot_shift(x1, y1, alpha2, sx2, sy2, x1_new+num_ali-1, y1_new+num_ali-1);
19159 rot_shift(x2, y2, alpha2, sx2, sy2, x2_new+num_ali-1, y2_new+num_ali-1);
19160
19161 for (int j=0; j<num_ali-1; j++) {
19162 float alpha1 = all_ali_params[j*(nima*4)+i*4];
19163 float sx1 = all_ali_params[j*(nima*4)+i*4+1];
19164 float sy1 = all_ali_params[j*(nima*4)+i*4+2];
19165 int mirror1 = static_cast<int>(all_ali_params[j*(nima*4)+i*4+3]);
19166
19167 float alphai = x[j*3];
19168 float sxi = x[j*3+1];
19169 float syi = x[j*3+2];
19170
19171 float cosi = cos(alphai/180.0f*M_PI);
19172 float sini = sin(alphai/180.0f*M_PI);
19173
19174 float alpha12, sx12, sy12;
19175 int mirror12;
19176 if (mirror1 == 0) {
19177 alpha12 = fmod(alpha1+alphai, 360.0f);
19178 rot_shift(sx1, sy1, alphai, sxi, syi, &sx12, &sy12);
19179 mirror12 = 0;
19180 } else {
19181 alpha12 = fmod(alpha1-alphai, 360.0f);
19182 rot_shift(sx1, sy1, -alphai, -sxi, syi, &sx12, &sy12);
19183 mirror12 = 1;
19184 }
19185
19186 rot_shift(x1, y1, alpha12, sx12, sy12, x1_new+j, y1_new+j);
19187 rot_shift(x2, y2, alpha12, sx12, sy12, x2_new+j, y2_new+j);
19188
19189 alpha12_0[j] = alpha12;
19190 mirror1_0[j] = mirror1;
19191 if (mirror1 == 0) {
19192 dalpha12[j] = M_PI/180.0f;
19193 dsx12[j] = (-sini*sx1+cosi*sy1)/180.0f*M_PI;
19194 dsy12[j] = (-cosi*sx1-sini*sy1)/180.0f*M_PI;
19195 } else {
19196 dalpha12[j] = -M_PI/180.0f;
19197 dsx12[j] = (sini*(-sx1)-cosi*sy1)/180.0f*M_PI;
19198 dsy12[j] = (-cosi*(-sx1)-sini*sy1)/180.0f*M_PI;
19199 }
19200 }
19201
19202 for (int j=0; j<num_ali-1; j++) {
19203 float cosa = cos(alpha12_0[j]/180.0f*M_PI);
19204 float sina = sin(alpha12_0[j]/180.0f*M_PI);
19205 float diffx1 = x1_new[j]-mean(x1_new, num_ali);
19206 float diffx2 = x2_new[j]-mean(x2_new, num_ali);
19207 float diffy1 = y1_new[j]-mean(y1_new, num_ali);
19208 float diffy2 = y2_new[j]-mean(y2_new, num_ali);
19209
19210 float p = diffx1*((-x1*sina+y1*cosa)*dalpha12[j]+dsx12[j])+diffx2*((-x2*sina+y2*cosa)*dalpha12[j]+dsx12[j])+diffy1*((-x1*cosa-y1*sina)*dalpha12[j]+dsy12[j])+diffy2*((-x2*cosa-y2*sina)*dalpha12[j]+dsy12[j]);
19211 g[j*3] += p;
19212
19213 p = diffx1+diffx2;
19214 if (mirror1_0[j] == 0) g[j*3+1] += p;
19215 else g[j*3+1] -= p;
19216
19217 p = diffy1+diffy2;
19218 g[j*3+2] += p;
19219 }
19220 }
19221
19222 delete[] x1_new;
19223 delete[] y1_new;
19224 delete[] x2_new;
19225 delete[] y2_new;
19226 delete[] alpha12_0;
19227 delete[] dalpha12;
19228 delete[] dsx12;
19229 delete[] dsy12;
19230 delete[] mirror1_0;
19231
19232 }
19233
19234 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
19235
19236 EMData *rot= new EMData();
19237 float ccc;
19238
19239 rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
19240 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19241 delete rot;
19242 return ccc;
19243 }
19244
19245 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19246
19247 double x[4];
19248 int n;
19249 int l = 3;
19250 int m = 200;
19251 double e = 1e-9;
19252 double step = 0.001;
19253 float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
19254
19255 x[1] = ang;
19256 x[2] = sxs;
19257 x[3] = sys;
19258
19259 Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);
19260
19261
19262 vector<float> res;
19263 res.push_back(static_cast<float>(x[1]));
19264 res.push_back(static_cast<float>(x[2]));
19265 res.push_back(static_cast<float>(x[3]));
19266 res.push_back(static_cast<float>(n));
19267 return res;
19268 }
19269
19270
19271 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
19272
19273 EMData *rot= new EMData();
19274 float ccc;
19275
19276 rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
19277 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19278 delete rot;
19279 return ccc;
19280 }
19281
19282 #define img_ptr(i,j,k) img_ptr[i+(j+(k*ny))*nx]
19283 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*nx]
19284 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
19285 {
19286 ENTERFUNC;
19287
19288 if (!img) {
19289 throw NullPointerException("NULL input image");
19290 }
19291
19292 int newx, newy, newz;
19293 bool keep_going;
19294 cout << " entered " <<endl;
19295 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
19296
19297 EMData * img2 = new EMData();
19298 img2->set_size(nx,ny,nz);
19299 img2->to_zero();
19300 float *img_ptr =img->get_data();
19301 float *img2_ptr = img2->get_data();
19302 int r2 = ro*ro;
19303 int r3 = r2*ro;
19304 int ri2 = ri*ri;
19305 int ri3 = ri2*ri;
19306
19307 int n2 = nx/2;
19308
19309 for (int k=-n2; k<=n2; k++) {
19310 float z2 = static_cast<float>(k*k);
19311 for (int j=-n2; j<=n2; j++) {
19312 float y2 = z2 + j*j;
19313 if(y2 <= r2) {
19314
19315
19316 for (int i=-n2; i<=n2; i++) {
19317 float x2 = y2 + i*i;
19318 if(x2 <= r3) {
19319
19320 int ib = i+n2; int jb = j+n2; int kb = k+n2;
19321 if(x2 >= ri3) {
19322
19323 if(img_ptr(ib,jb,kb) == 1.0f) {
19324
19325 if(Util::get_frand(0.0f, 1.0f) > qprob){
19326 img2_ptr(ib,jb,kb) = 0.0f;
19327 keep_going = true;
19328
19329 while(keep_going) {
19330 newx = Util::get_irand(-ro,ro);
19331 newy = Util::get_irand(-ro,ro);
19332 newz = Util::get_irand(-ro,ro);
19333 if(newx*newx+newy*newy+newz*newz <= r3) {
19334 newx += n2; newy += n2; newz += n2;
19335 if( img_ptr(newx,newy,newz) == 0.0f) {
19336 img2_ptr(newx,newy,newz) = 1.0f;
19337 keep_going = false; }
19338 }
19339 }
19340 } else img2_ptr(ib,jb,kb) = 1.0f;
19341 }
19342 } else {
19343
19344 if(img_ptr(ib,jb,kb) == 1.0) {
19345 if(Util::get_frand(0.0f,1.0f) > qprob) {
19346
19347 float numn = -1.0f;
19348 for (newz = -1; newz <= 1; newz++)
19349 for (newy = -1; newy <= 1; newy++)
19350 for (newx = -1; newx <= 1; newx++)
19351 numn += img_ptr(ib+newx,jb+newy,kb+newz);
19352 img2_ptr(ib,jb,kb) = 0.0;
19353 if(numn == 26.0f) {
19354
19355 keep_going = true;
19356 while(keep_going) {
19357 newx = Util::get_irand(-ro,ro);
19358 newy = Util::get_irand(-ro,ro);
19359 newz = Util::get_irand(-ro,ro);
19360 if(newx*newx+newy*newy+newz*newz < r3) {
19361 newx += n2; newy += n2; newz += n2;
19362 if( img_ptr(newx,newy,newz) == 0.0f) {
19363 if(newx*newx+newy*newy+newz*newz < r3) {
19364 if(newx*newx+newy*newy+newz*newz < r3) {
19365 newx += n2; newy += n2; newz += n2;
19366 if( img_ptr(newx,newy,newz) == 0.0f) {
19367 img2_ptr(newx,newy,newz) = 1.0f;
19368 keep_going = false; }
19369 }
19370 }
19371 }
19372 }
19373 }
19374 } else if(numn == 25.0f) {
19375
19376 for (newz = -1; newz <= 1; newz++) {
19377 for (newy = -1; newy <= 1; newy++) {
19378 for (newx = -1; newx <= 1; newx++) {
19379 if( newx != 0 && newy != 0 && newz != 0) {
19380 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
19381 img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
19382 }
19383 }
19384 }
19385 }
19386 }
19387 } else {
19388
19389 keep_going = true;
19390 while(keep_going) {
19391 newx = Util::get_irand(-1,1);
19392 newy = Util::get_irand(-1,1);
19393 newz = Util::get_irand(-1,1);
19394 if(newx != 0 && newy != 0 && newz != 0) {
19395 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
19396 img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;
19397 keep_going = false;
19398 }
19399 }
19400 }
19401 }
19402 } else img2_ptr(ib,jb,kb) = 1.0f;
19403 }
19404 }
19405 }
19406 }
19407 }
19408 }
19409 }
19410
19411 img2->update();
19412
19413 EXITFUNC;
19414 return img2;
19415 }
19416 #undef img_ptr
19417 #undef img2_ptr
19418
19419 struct point3d_t
19420 {
19421 point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
19422
19423 int x;
19424 int y;
19425 int z;
19426 };
19427
19428
19429 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
19430 {
19431 int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
19432 int noff = 6;
19433
19434 int nx = visited->get_xsize();
19435 int ny = visited->get_ysize();
19436 int nz = visited->get_zsize();
19437
19438 vector< point3d_t > pts;
19439 pts.push_back( point3d_t(ix, iy, iz) );
19440 visited->set_value_at( ix, iy, iz, (float)grpid );
19441
19442 int start = 0;
19443 int end = pts.size();
19444
19445 while( end > start ) {
19446 for(int i=start; i < end; ++i ) {
19447 int ix = pts[i].x;
19448 int iy = pts[i].y;
19449 int iz = pts[i].z;
19450
19451 for( int j=0; j < noff; ++j ) {
19452 int jx = ix + offs[j][0];
19453 int jy = iy + offs[j][1];
19454 int jz = iz + offs[j][2];
19455
19456 if( jx < 0 || jx >= nx ) continue;
19457 if( jy < 0 || jy >= ny ) continue;
19458 if( jz < 0 || jz >= nz ) continue;
19459
19460
19461 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
19462 pts.push_back( point3d_t(jx, jy, jz) );
19463 visited->set_value_at( jx, jy, jz, (float)grpid );
19464 }
19465
19466 }
19467 }
19468
19469 start = end;
19470 end = pts.size();
19471 }
19472 return pts.size();
19473 }
19474
19475
19476 EMData* Util::get_biggest_cluster( EMData* mg )
19477 {
19478 int nx = mg->get_xsize();
19479 int ny = mg->get_ysize();
19480 int nz = mg->get_zsize();
19481
19482 EMData* visited = new EMData();
19483 visited->set_size( nx, ny, nz );
19484 visited->to_zero();
19485 int grpid = 0;
19486 int maxgrp = 0;
19487 int maxsize = 0;
19488 for( int iz=0; iz < nz; ++iz ) {
19489 for( int iy=0; iy < ny; ++iy ) {
19490 for( int ix=0; ix < nx; ++ix ) {
19491 if( (*mg)(ix, iy, iz)==0.0 ) continue;
19492
19493 if( (*visited)(ix, iy, iz) > 0.0 ) {
19494
19495 continue;
19496 }
19497
19498 grpid++;
19499 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
19500 if( grpsize > maxsize ) {
19501 maxsize = grpsize;
19502 maxgrp = grpid;
19503 }
19504 }
19505 }
19506 }
19507
19508 Assert( maxgrp > 0 );
19509
19510 int npoint = 0;
19511 EMData* result = new EMData();
19512 result->set_size( nx, ny, nz );
19513 result->to_zero();
19514
19515 for( int iz=0; iz < nz; ++iz ) {
19516 for( int iy=0; iy < ny; ++iy ) {
19517 for( int ix=0; ix < nx; ++ix ) {
19518 if( (*visited)(ix, iy, iz)==maxgrp ) {
19519 (*result)(ix,iy,iz) = 1.0;
19520 npoint++;
19521 }
19522 }
19523 }
19524 }
19525
19526 Assert( npoint==maxsize );
19527 delete visited;
19528 return result;
19529
19530 }
19531
19532 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)
19533 {
19534 int ix, iy, iz;
19535 int i, j, k;
19536 int nr2, nl2;
19537 float dzz, az, ak;
19538 float scx, scy, scz;
19539 int offset = 2 - nx%2;
19540 int lsm = nx + offset;
19541 EMData* ctf_img1 = new EMData();
19542 ctf_img1->set_size(lsm, ny, nz);
19543 float freq = 1.0f/(2.0f*ps);
19544 scx = 2.0f/float(nx);
19545 if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
19546 if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
19547 nr2 = ny/2 ;
19548 nl2 = nz/2 ;
19549 for ( k=0; k<nz;k++) {
19550 iz = k; if(k>nl2) iz=k-nz;
19551 for ( j=0; j<ny;j++) {
19552 iy = j; if(j>nr2) iy=j - ny;
19553 for ( i=0; i<lsm/2; i++) {
19554 ix=i;
19555 ak=pow(ix*ix*scx*scx+iy*scy*iy*scy+iz*scz*iz*scz, 0.5f)*freq;
19556 if(ak!=0) az=0.0; else az=M_PI;
19557 dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f));
19558 (*ctf_img1) (i*2,j,k) = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
19559 (*ctf_img1) (i*2+1,j,k) = 0.0f;
19560 }
19561 }
19562 }
19563 ctf_img1->update();
19564 ctf_img1->set_complex(true);
19565 ctf_img1->set_ri(true);
19566
19567
19568 if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
19569 return ctf_img1;
19570 }
19571
19572
19573
19574
19575
19576
19577
19578
19579
19580
19581
19582
19583
19584
19585
19586
19587
19588
19589
19590
19591
19592
19593
19594
19595
19596
19597
19598
19599
19600
19601
19602
19603
19604
19605
19606
19607
19608
19609
19610
19611
19612
19613
19614
19615
19616
19617
19618
19619
19620
19621
19622
19623
19624
19625
19626
19627
19628
19629
19630
19631
19632
19633
19634
19635
19636
19637
19638
19639
19640
19641
19642
19643
19644
19645
19646
19647
19648
19649
19650
19651
19652
19653
19654
19655
19656
19657
19658 #define cent(i) out[i+N]
19659 #define assign(i) out[i]
19660 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
19661 int nx = d->get_xsize();
19662 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19663 vector<float> out(N+K+2);
19664 if(N*(N-1)/2 != nx) {
19665
19666 return out;}
19667
19668 for(int i=0; i<N; i++) assign(i) = float(i);
19669
19670 for(int i=0; i<N; i++) {
19671 int j = Util::get_irand(0,N-1);
19672 float temp = assign(i);
19673 assign(i) = assign(j);
19674 assign(j) = temp;
19675 }
19676 for(int k=0; k<K; k++) cent(k) = float(assign(k));
19677
19678
19679 for(int i=0; i<N; i++) assign(i) = 0.0f;
19680 float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
19681 bool change = true;
19682 int it = -1;
19683 int ct = -1;
19684 while ((change && disp < dispold) || ct > 0) {
19685
19686 change = false;
19687 dispold = disp;
19688 it++;
19689
19690
19691 disp = 0.0f;
19692 ct = 0;
19693 for(int i=0; i<N; i++) {
19694 qm = 1.0e23f;
19695 for(int k=0; k<K; k++) {
19696 if(float(i) == cent(k)) {
19697 qm = 0.0f;
19698 na = (float)k;
19699 } else {
19700 float dt = (*d)(mono(i,int(cent(k))));
19701 if(dt < qm) {
19702 qm = dt;
19703 na = (float)k;
19704 }
19705 }
19706 }
19707
19708
19709
19710 if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
19711 na = (float)(Util::get_irand(0, K));
19712 qm = (*d)(mono(i,int(na)));
19713 ct++;
19714 }
19715
19716 disp += qm;
19717
19718 if(na != assign(i)) {
19719 assign(i) = na;
19720 change = true;
19721 }
19722 }
19723
19724
19725 T = T*F;
19726
19727
19728
19729
19730
19731 for(int k=0; k<K; k++) {
19732 qm = 1.0e23f;
19733 for(int i=0; i<N; i++) {
19734 if(assign(i) == float(k)) {
19735 float q = 0.0;
19736 for(int j=0; j<N; j++) {
19737 if(assign(j) == float(k)) {
19738
19739 if(i != j) q += (*d)(mono(i,j));
19740
19741 }
19742 }
19743 if(q < qm) {
19744
19745 qm = q;
19746 cent(k) = float(i);
19747 }
19748 }
19749 }
19750 }
19751
19752 }
19753 out[N+K] = disp;
19754 out[N+K+1] = float(it);
19755 return out;
19756 }
19757 #undef cent
19758 #undef assign
19759
19760
19761
19762
19763
19764
19765
19766
19767
19768
19769
19770
19771
19772
19773
19774
19775
19776
19777
19778
19779
19780
19781
19782
19783
19784
19785
19786
19787
19788
19789
19790
19791
19792
19793
19794
19795
19796
19797
19798
19799
19800
19801
19802
19803
19804
19805
19806
19807
19808
19809
19810
19811
19812
19813
19814
19815
19816
19817
19818
19819
19820
19821
19822
19823
19824
19825
19826
19827
19828
19829
19830
19831
19832
19833
19834
19835
19836
19837
19838
19839
19840
19841
19842
19843
19844
19845
19846
19847
19848
19849
19850
19851
19852
19853
19854
19855
19856
19857
19858
19859
19860
19861
19862
19863
19864
19865
19866
19867
19868 vector<float> Util::cluster_equalsize(EMData* d) {
19869
19870 int nx = d->get_xsize();
19871 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19872 int K = N/2;
19873 vector<float> group(N);
19874 if(N*(N-1)/2 != nx) {
19875
19876 return group;}
19877
19878 bool * active = new bool[N];
19879 for(int i=0; i<N; i++) active[i] = true;
19880
19881 float dm, qd;
19882 int ppi = 0, ppj = 0;
19883 for(int k=0; k<K; k++) {
19884
19885
19886 dm = 1.0e23f;
19887 for(int i=1; i<N; i++) {
19888 if(active[i]) {
19889 for(int j=0; j<i; j++) {
19890 if(active[j]) {
19891 qd = (*d)(i*(i - 1)/2 + j);
19892 if(qd < dm) {
19893 dm = qd;
19894 ppi = i;
19895 ppj = j;
19896 }
19897 }
19898 }
19899 }
19900 }
19901 group[2*k] = float(ppi);
19902 group[1+2*k] = float(ppj);
19903 active[ppi] = false;
19904 active[ppj] = false;
19905 }
19906
19907 delete [] active;
19908 active = NULL;
19909 return group;
19910 }
19911
19912
19913
19914
19915
19916
19917
19918
19919
19920
19921
19922
19923
19924
19925
19926
19927
19928
19929
19930
19931
19932
19933
19934
19935
19936
19937
19938
19939
19940
19941
19942
19943
19944
19945
19946
19947
19948
19949
19950
19951
19952
19953
19954
19955
19956
19957
19958
19959
19960
19961
19962
19963
19964
19965 #define data(i,j) group[i*ny+j]
19966 vector<float> Util::vareas(EMData* d) {
19967 const float step=0.001f;
19968 int ny = d->get_ysize();
19969
19970
19971
19972 vector<float> group(2*ny);
19973 for(int i=0; i<2*ny; i++) group[i] = 0.0f;
19974 int K = int(1.0f/step) +1;
19975 int hit = 0;
19976 for(int kx=0; kx<=K; kx++) {
19977 float tx = kx*step;
19978 for(int ky=0; ky<=K; ky++) {
19979 float ty = ky*step;
19980 float dm = 1.0e23f;
19981 for(int i=0; i<ny; i++) {
19982 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
19983 if( qd < dm) {
19984 dm = qd;
19985 hit = i;
19986 }
19987 }
19988 data(0,hit) += 1.0f;
19989 if(kx == 0 || ky == 0 || kx == K || ky == K) data(1,hit) = 1.0f;
19990 }
19991 }
19992 return group;
19993 }
19994 #undef data
19995
19996 EMData* Util::get_slice(EMData *vol, int dim, int index) {
19997
19998 int nx = vol->get_xsize();
19999 int ny = vol->get_ysize();
20000 int nz = vol->get_zsize();
20001 float *vol_data = vol->get_data();
20002 int new_nx, new_ny;
20003
20004 if (nz == 1)
20005 throw ImageDimensionException("Error: Input must be a 3-D object");
20006 if ((dim < 1) || (dim > 3))
20007 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
20008 if (((dim == 1) && (index < 0 || index > nx-1)) ||
20009 ((dim == 1) && (index < 0 || index > nx-1)) ||
20010 ((dim == 1) && (index < 0 || index > nx-1)))
20011 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
20012
20013 if (dim == 1) {
20014 new_nx = ny;
20015 new_ny = nz;
20016 } else if (dim == 2) {
20017 new_nx = nx;
20018 new_ny = nz;
20019 } else {
20020 new_nx = nx;
20021 new_ny = ny;
20022 }
20023
20024 EMData *slice = new EMData();
20025 slice->set_size(new_nx, new_ny, 1);
20026 float *slice_data = slice->get_data();
20027
20028 if (dim == 1) {
20029 for (int x=0; x<new_nx; x++)
20030 for (int y=0; y<new_ny; y++)
20031 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
20032 } else if (dim == 2) {
20033 for (int x=0; x<new_nx; x++)
20034 for (int y=0; y<new_ny; y++)
20035 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
20036 } else {
20037 for (int x=0; x<new_nx; x++)
20038 for (int y=0; y<new_ny; y++)
20039 slice_data[y*new_nx+x] = vol_data[(index*ny+y)*nx+x];
20040 }
20041
20042 return slice;
20043 }
20044
20045 void Util::image_mutation(EMData *img, float mutation_rate) {
20046 int nx = img->get_xsize();
20047 float min = img->get_attr("minimum");
20048 float max = img->get_attr("maximum");
20049 float* img_data = img->get_data();
20050 array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
20051 return;
20052 }
20053
20054
20055 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20056
20057 if (is_mirror != 0) {
20058 for (int i=0; i<len_list; i++) {
20059 int r = rand()%10000;
20060 float f = r/10000.0f;
20061 if (f < mutation_rate) list[i] = 1-list[i];
20062 }
20063 } else {
20064 map<int, vector<int> > graycode;
20065 map<vector<int>, int> rev_graycode;
20066 vector <int> gray;
20067
20068 int K=1;
20069 for (int i=0; i<L; i++) K*=2;
20070
20071 for (int k=0; k<K; k++) {
20072 int shift = 0;
20073 vector <int> gray;
20074 for (int i=L-1; i>-1; i--) {
20075 int t = ((k>>i)%2-shift)%2;
20076 gray.push_back(t);
20077 shift += t-2;
20078 }
20079 graycode[k] = gray;
20080 rev_graycode[gray] = k;
20081 }
20082
20083 float gap = (K-1)/(max_val-min_val);
20084 for (int i=0; i<len_list; i++) {
20085 float val = list[i];
20086 if (val < min_val) { val = min_val; }
20087 else if (val > max_val) { val = max_val; }
20088 int k = int((val-min_val)*gap+0.5);
20089 vector<int> gray = graycode[k];
20090 bool changed = false;
20091 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20092 int r = rand()%10000;
20093 float f = r/10000.0f;
20094 if (f < mutation_rate) {
20095 *p = 1-*p;
20096 changed = true;
20097 }
20098 }
20099 if (changed) {
20100 k = rev_graycode[gray];
20101 list[i] = k/gap+min_val;
20102 }
20103 }
20104 }
20105
20106 }
20107
20108 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20109
20110 if (is_mirror != 0) {
20111 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20112 int r = rand()%10000;
20113 float f = r/10000.0f;
20114 if (f < mutation_rate) *q = 1-*q;
20115 }
20116 } else {
20117 map<int, vector<int> > graycode;
20118 map<vector<int>, int> rev_graycode;
20119 vector <int> gray;
20120
20121 int K=1;
20122 for (int i=0; i<L; i++) K*=2;
20123
20124 for (int k=0; k<K; k++) {
20125 int shift = 0;
20126 vector <int> gray;
20127 for (int i=L-1; i>-1; i--) {
20128 int t = ((k>>i)%2-shift)%2;
20129 gray.push_back(t);
20130 shift += t-2;
20131 }
20132 graycode[k] = gray;
20133 rev_graycode[gray] = k;
20134 }
20135
20136 float gap = (K-1)/(max_val-min_val);
20137 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20138 float val = *q;
20139 if (val < min_val) { val = min_val; }
20140 else if (val > max_val) { val = max_val; }
20141 int k = int((val-min_val)*gap+0.5);
20142 vector<int> gray = graycode[k];
20143 bool changed = false;
20144 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20145 int r = rand()%10000;
20146 float f = r/10000.0f;
20147 if (f < mutation_rate) {
20148 *p = 1-*p;
20149 changed = true;
20150 }
20151 }
20152 if (changed) {
20153 k = rev_graycode[gray];
20154 *q = k/gap+min_val;
20155 }
20156 }
20157 }
20158 return list;
20159 }
20160
20161
20162 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
20163
20164 int total_cost = *output;
20165 int num_matches = *(output+1);
20166
20167 int cost=0;
20168 int* intx;
20169 int intx_size;
20170 int* intx_next(0);
20171 int intx_next_size = 0;
20172 int curclass;
20173 int curclass_size;
20174
20175 for(int i = 0; i < num_matches; i++){
20176 curclass = *(output+2+ i*nParts);
20177
20178 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
20179 *(argParts + Indices[curclass]+1) = -5;
20180
20181 curclass_size = *(dimClasses+curclass)-2;
20182 intx = new int[curclass_size];
20183 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
20184 intx_size = curclass_size;
20185
20186 for (int j=1; j < nParts; j++){
20187 curclass = *(output+2+ i*nParts+j);
20188 if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
20189 *(argParts + Indices[j*K+curclass]+1)=-5;
20190
20191 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);
20192 intx_next = new int[intx_next_size];
20193 Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
20194 delete[] intx;
20195 intx=intx_next;
20196 intx_size= intx_next_size;
20197 if (j==nParts-1) delete[] intx_next;
20198 }
20199
20200 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
20201
20202 cost = cost + intx_next_size;
20203 }
20204
20205 if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
20206
20207 return 1;
20208
20209 }
20210
20211
20212
20213
20214
20215
20216 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int* costlist, int J){
20217
20218
20219 bool flag = 0;
20220 int nintx;
20221 int* dummy(0);
20222
20223 int* curbranch = new int[nParts];
20224
20225
20226 for(int jit= 0; jit< J; jit++) *(costlist+jit) = 0;
20227
20228
20229 for(int a=0; a<K; a++)
20230 {
20231
20232
20233 if (*(argParts + Indices[a] + 1) < 1) continue;
20234 if (*(dimClasses + a)-2 <= T) continue;
20235
20236
20237
20238 for( int i=1; i < nParts; i++){
20239 flag = 0;
20240 for(int j=0; j < K; j++){
20241 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
20242 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);
20243 if (nintx > T) flag=1;
20244 else *(argParts + Indices[i*K+j] + 1) =-4;
20245 }
20246 if (flag==0) {break;}
20247 }
20248
20249
20250 *curbranch = a;
20251
20252 if (flag > 0)
20253 Util::explore2(argParts, Indices, dimClasses, nParts, K, T, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2,
20254 *(dimClasses+a)-2,0, J, matchlist, costlist, curbranch);
20255
20256
20257 for( int i=1; i < nParts; i++){
20258 for(int j=0; j < K; j++){
20259 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
20260
20261 }
20262 }
20263 }
20264
20265 delete[] curbranch;
20266 }
20267
20268
20269 void Util::explore2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* curintx, int size_curintx, int* next, int size_next, int depth, int J, int* matchlist, int*costlist, int* curbranch){
20270
20271
20272 int* curintx2(0);
20273 int nintx = size_curintx;
20274
20275
20276
20277 if (depth >0){
20278 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
20279 if (nintx <= T) return;
20280 }
20281
20282
20283 if (depth == (nParts-1)) {
20284
20285 int replace = 0;
20286 int ind_smallest = -1;
20287 int smallest_cost = -1;
20288
20289 for (int jit = 0; jit < J; jit++){
20290 if (*(costlist+jit) < nintx){
20291 replace = 1;
20292 if (ind_smallest == -1) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20293 if (*(costlist+jit) < smallest_cost) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20294 }
20295 }
20296
20297 if (replace > 0){
20298
20299 *(costlist + ind_smallest) = nintx;
20300 for (int xit = 0; xit < nParts; xit++)
20301 *(matchlist + ind_smallest*nParts + xit) = *(curbranch+xit);
20302
20303 }
20304
20305 return;
20306 }
20307
20308
20309
20310
20311 if (depth > 0){
20312 curintx2 = new int[nintx];
20313 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
20314 }
20315
20316 if (depth == 0){
20317
20318 curintx2 = new int[size_curintx];
20319 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
20320 }
20321
20322
20323
20324 depth=depth+1;
20325
20326 for (int i=0; i < K; i++){
20327
20328 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue;
20329 size_next = (*(dimClasses + depth*K+i ))-2;
20330 if (size_next <= T) continue;
20331 *(curbranch+depth) = i;
20332 Util::explore2(argParts,Indices, dimClasses, nParts, K, T, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth,J, matchlist,
20333 costlist, curbranch);
20334
20335 }
20336
20337 delete[] curintx2;
20338 }
20339
20340 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
20341
20342
20343
20344
20345
20346
20347
20348
20349
20350
20351 int* dummy(0);
20352 int* cref;
20353 int cref_size;
20354 int* ccomp;
20355 int ccomp_size;
20356 int nintx;
20357 for (int i=0; i < nParts; i++){
20358 for (int j =0; j < K; j++){
20359
20360
20361 cref = Parts[i][j];
20362 cref_size = (*(dimClasses+i*K+(*cref)))-2;
20363
20364
20365 if (cref_size <= T){
20366
20367 *cref = -1;
20368 continue;
20369 }
20370 bool done = 0;
20371 for (int a = 0; a < nParts; a++){
20372 if (a == i) continue;
20373 bool hasActive=0;
20374 for (unsigned int b=0; b < Parts[a].size(); b++){
20375
20376
20377 ccomp = Parts[a][b];
20378 ccomp_size= (*(dimClasses+a*K+(*ccomp)))-2;
20379 nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
20380
20381
20382 if (nintx <= T)
20383 *(ccomp+1) = 0;
20384 else{
20385 *(ccomp+1)=1;
20386 hasActive=1;
20387 }
20388 }
20389
20390 if (hasActive < 1){
20391 done=1;
20392 break;
20393 }
20394
20395 }
20396
20397 if (done > 0){
20398
20399
20400 *cref = -1;
20401 continue;
20402 }
20403
20404
20405
20406
20407
20408
20409
20410
20411
20412
20413 bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
20414
20415 if (found<1){
20416
20417 *cref = -1;
20418 }
20419 }
20420
20421
20422
20423 for (int d = K-1; d > -1; d--){
20424 if (*(Parts[i][d]) < 0) Parts[i].erase(Parts[i].begin()+d);
20425 }
20426
20427 }
20428
20429
20430
20431
20432
20433 }
20434
20435
20436 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) {
20437
20438
20439 if (size_next <= T) return 0;
20440
20441
20442 int* curintx2(0);
20443 int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
20444 if (nintx <= T) return 0;
20445
20446 int old_depth=depth;
20447 if (depth == partref) depth = depth + 1;
20448 if (depth == (nParts)) { if (old_depth>0) return 1;}
20449
20450
20451
20452 curintx2 = new int[nintx];
20453 Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
20454
20455
20456
20457
20458 bool gt_thresh;
20459 int num_classes = (Parts[depth]).size();
20460
20461 for (int i=0; i < num_classes; i++){
20462 if (*(Parts[depth][i]+1) < 1) continue;
20463 size_next = (*(dimClasses + depth*K+(*(Parts[depth][i])) ))-2;
20464 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
20465 if (gt_thresh) return 1;
20466 }
20467 delete[] curintx2;
20468 return 0;
20469 }
20470
20471
20472
20473
20474 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int n_guesses, int LARGEST_CLASS, int J,
20475 int max_branching, float stmult, int branchfunc, int LIM) {
20476
20477
20478
20479
20480
20481 int* Indices = new int[nParts*K];
20482 int ind_c = 0;
20483 for (int i=0; i < nParts; i++){
20484 for(int j = 0; j < K; j++){
20485 Indices[i*K + j] = ind_c;
20486 ind_c = ind_c + *(dimClasses+i*K + j);
20487
20488 }
20489 }
20490
20491
20492
20493
20494 vector <vector <int*> > Parts(nParts,vector<int*>(K));
20495 ind_c = 0;
20496 int argParts_size=0;
20497 for (int i=0; i < nParts; i++){
20498 for(int j = 0; j < K; j++){
20499 Parts[i][j]=argParts + ind_c;
20500 ind_c = ind_c + *(dimClasses+i*K + j);
20501 argParts_size = argParts_size + *(dimClasses+i*K + j);
20502
20503 }
20504 }
20505
20506
20507
20508
20509
20510
20511 Util::initial_prune(Parts, dimClasses, nParts, K,T);
20512 for(int i = 0; i < nParts; i++){
20513 for(int j=0; j < K; j++){
20514 *(argParts + Indices[i*K + j]+1) = -1;
20515 }
20516 }
20517
20518 int num_classes;
20519 int old_index;
20520 for(int i=0; i<nParts; i++){
20521 num_classes = Parts[i].size();
20522 for (int j=0; j < num_classes; j++){
20523 old_index = *(Parts[i][j]);
20524
20525 *(argParts + Indices[i*K + old_index]+1) = 1;
20526 }
20527 }
20528
20529
20530
20531
20532
20533 int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T,0,n_guesses,LARGEST_CLASS, J, max_branching, stmult,
20534 branchfunc, LIM);
20535
20536
20537
20538
20539 bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
20540
20541
20542 if (correct < 1){
20543 cout << "something is wrong with output of branchMPI!\n";
20544 vector<int> ret(1);
20545 ret[0]=-1;
20546 return ret;
20547 }
20548
20549
20550
20551
20552
20553 int output_size = 2+ *(output+1) * nParts;
20554 vector<int> ret(output_size);
20555 for (int i = 0; i < output_size; i++){
20556 ret[i]=*(output+i);
20557 }
20558 return ret;
20559
20560 }
20561
20562
20563 int branch_all=0;
20564 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int curlevel,int n_guesses, int
20565 LARGEST_CLASS, int J, int max_branching, float stmult, int branchfunc, int LIM) {
20566
20567
20568
20569 if (1 == 0){
20570 cout <<"begin test search2\n";
20571 int* matchlist = new int[J*nParts];
20572 int* costlist = new int[J];
20573 for (int jit = 0; jit < nParts; jit++) *(costlist+jit) = 0;
20574 Util::search2(argParts,Indices, dimClasses, nParts, K, T, matchlist,costlist,J);
20575
20576 for (int jit = 0; jit < J; jit++) {
20577 cout << *(costlist +jit)<<": ";
20578 for (int yit = 0; yit < nParts; yit++)
20579 cout << *(matchlist + jit*nParts + yit)<<",";
20580 cout <<"\n";
20581
20582 }
20583 cout <<"end test search2\n";
20584 int* ret = new int[1];
20585 *ret=1;
20586 delete [] matchlist;
20587 delete [] costlist;
20588 return ret;
20589 }
20590
20591
20592
20593 if (curlevel > K -1){
20594 int* res = new int[2];
20595 *res = 0;
20596 *(res+1)=0;
20597 return res;
20598 }
20599
20600
20601
20602 int* matchlist = new int[J*nParts];
20603 int* costlist = new int[J];
20604 Util::search2(argParts,Indices, dimClasses, nParts, K, T, matchlist,costlist,J);
20605
20606
20607
20608
20609
20610
20611 for (int jit = 0; jit < J ; jit++){
20612
20613 if (costlist[jit] > T) break;
20614 if (jit == J-1){
20615 int* res = new int[2];
20616 *res = 0;
20617 *(res+1)=0;
20618 return res;
20619 }
20620 }
20621
20622
20623
20624
20625 if (curlevel==0) branch_all = 0;
20626 int* newcostlist;
20627 int* newmatchlist;
20628
20629 int nBranches = -1;
20630
20631 if (branchfunc == 2)
20632 nBranches = branch_factor_2(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM);
20633
20634 if (branchfunc == 3)
20635 nBranches = branch_factor_3(costlist,matchlist,J, T, nParts, curlevel, max_branching, K, LIM);
20636
20637 if (branchfunc == 4)
20638 nBranches = branch_factor_4(costlist,matchlist,J, T, nParts, curlevel, max_branching, stmult);
20639
20640 newcostlist= new int[nBranches];
20641 newmatchlist = new int[nBranches*nParts];
20642 for (int i=0; i<nBranches; i++){
20643 *(newcostlist + i) = *(costlist+i);
20644 for (int j=0; j< nParts; j++)
20645 *(newmatchlist + i*nParts + j) = *(matchlist + i*nParts+j);
20646 }
20647
20648 delete[] costlist;
20649 delete[] matchlist;
20650
20651 int* maxreturn = new int[2];
20652 *maxreturn=0;
20653 *(maxreturn+1)=0;
20654
20655 int old_index;
20656 int totalcost;
20657 int nmatches;
20658
20659
20660 for(int i=0; i < nBranches ; i++){
20661
20662
20663
20664
20665
20666
20667
20668 for(int j=0; j < nParts; j++){
20669
20670 old_index=newmatchlist[i*nParts + j];
20671 *(argParts + Indices[j*K+old_index] + 1) = -2;
20672 }
20673
20674
20675 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T,curlevel+1,n_guesses, LARGEST_CLASS,
20676 J,max_branching, stmult,branchfunc, LIM);
20677
20678
20679 totalcost = newcostlist[i] + *ret;
20680
20681
20682
20683
20684
20685 if (totalcost > *maxreturn)
20686 {
20687 nmatches = 1 + *(ret+1);
20688 delete[] maxreturn;
20689 maxreturn = new int[2+nmatches*nParts];
20690 *maxreturn = totalcost;
20691
20692 *(maxreturn + 1)= nmatches;
20693 int nret = 2+(nmatches-1)*nParts;
20694 for(int iret=2; iret <nret;iret++) *(maxreturn+iret)=*(ret+iret);
20695 for(int imax=0; imax<nParts;imax++) *(maxreturn+nret+imax)=newmatchlist[i*nParts + imax];
20696 }
20697
20698
20699 delete[] ret;
20700
20701
20702
20703 for(int j=0; j < nParts; j++){
20704 old_index=newmatchlist[i*nParts + j];
20705 *(argParts + Indices[j*K+old_index] + 1) = 1;
20706 }
20707
20708 }
20709
20710 delete[] newmatchlist;
20711 delete[] newcostlist;
20712
20713 return maxreturn;
20714
20715 }
20716
20717 int* costlist_global;
20718
20719
20720 bool jiafunc(int i, int j){
20721 return (costlist_global[j] < costlist_global[i]) ;
20722
20723 }
20724
20725
20726
20727
20728
20729
20730 int Util::branch_factor_2(int* costlist, int* matchlist, int J, int T, int nParts, int curlevel, int max_branching, int LIM){
20731
20732 int ntot=0;
20733 for (int jit=0; jit < J; jit++){
20734 if (*(costlist+jit) > T) ntot++;
20735 }
20736
20737 int cur;
20738
20739 int* indx = new int[J];
20740 for (int jit=0; jit < J; jit++) indx[jit]=jit;
20741 vector<int> myindx (indx, indx+J);
20742 vector<int>::iterator it;
20743 costlist_global=costlist;
20744 sort(myindx.begin(), myindx.begin()+J, jiafunc);
20745
20746
20747 int* templist = new int[J];
20748 int* temp2list = new int[J*nParts];
20749 int next = 0;
20750
20751 for (it=myindx.begin(); it!=myindx.end();++it){
20752 cur = *(costlist + *it);
20753 if (cur > T){
20754
20755 templist[next] = cur;
20756 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
20757 next = next + 1;
20758 }
20759 }
20760
20761 for (int jit=0; jit < ntot; jit++){
20762 *(costlist+jit)=*(templist + jit);
20763
20764 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
20765 }
20766
20767
20768 delete [] indx;
20769
20770
20771
20772
20773
20774
20775 int B=1;
20776 int B_init=B;
20777 int infeasible=0;
20778
20779 for (int i=B_init; i<ntot; i++){
20780 if (i==ntot) continue;
20781
20782
20783 infeasible = 0;
20784 if (LIM < 0) LIM = B;
20785 for (int j=0; j<B; j++){
20786
20787 for (int vit=0; vit<nParts; vit++){
20788 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
20789 }
20790 if (infeasible >= LIM) break;
20791 }
20792
20793 if (infeasible >= LIM){
20794 *(costlist+B)=*(templist+i);
20795 for (int vit=0; vit < nParts; vit++)
20796 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
20797 B=B+1;
20798 }
20799 }
20800
20801 delete [] templist;
20802 delete [] temp2list;
20803
20804
20805 if (branch_all < max_branching){
20806 if (B>1)
20807 {branch_all = branch_all + B -1 ; }
20808 }
20809 else B=1;
20810
20811 return B;
20812
20813
20814 }
20815
20816
20817
20818 int Util::branch_factor_3(int* costlist, int* matchlist, int J, int T, int nParts, int curlevel, int max_branching, int K, int LIM){
20819
20820 int ntot=0;
20821 for (int jit=0; jit < J; jit++){
20822 if (*(costlist+jit) > T) ntot++;
20823 }
20824
20825 int cur;
20826
20827 int* indx = new int[J];
20828 for (int jit=0; jit < J; jit++) indx[jit]=jit;
20829 vector<int> myindx (indx, indx+J);
20830 vector<int>::iterator it;
20831 costlist_global=costlist;
20832 sort(myindx.begin(), myindx.begin()+J, jiafunc);
20833
20834
20835 int* templist = new int[J];
20836 int* temp2list = new int[J*nParts];
20837 int next = 0;
20838
20839 for (it=myindx.begin(); it!=myindx.end();++it){
20840 cur = *(costlist + *it);
20841 if (cur > T){
20842
20843 templist[next] = cur;
20844 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
20845 next = next + 1;
20846 }
20847 }
20848
20849 for (int jit=0; jit < ntot; jit++){
20850 *(costlist+jit)=*(templist + jit);
20851
20852 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
20853 }
20854
20855
20856 delete [] indx;
20857
20858
20859
20860
20861
20862
20863 int B=1;
20864 int B_init=B;
20865 int infeasible=0;
20866
20867
20868 if (LIM < 0) LIM = ntot-1;
20869 for (int i=B_init; i<ntot; i++){
20870 if (i==ntot) continue;
20871
20872
20873 infeasible = 0;
20874
20875 for (int j=0; j<ntot; j++){
20876 if (j == i) continue;
20877 for (int vit=0; vit<nParts; vit++){
20878 if (temp2list[i*nParts+vit] == temp2list[j*nParts+vit]) {infeasible++; break;}
20879 }
20880 if (infeasible >= LIM) break;
20881 }
20882
20883 if (infeasible >= LIM){
20884 *(costlist+B)=*(templist+i);
20885 for (int vit=0; vit < nParts; vit++)
20886 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
20887 B=B+1;
20888 }
20889 }
20890
20891 delete [] templist;
20892 delete [] temp2list;
20893
20894
20895
20896 if (branch_all < max_branching){
20897 if (B>1)
20898 {branch_all = branch_all + B-1;}
20899 }
20900 else B=1;
20901
20902 return B;
20903
20904
20905 }
20906
20907
20908
20909
20910
20911 int Util::branch_factor_4(int* costlist, int* matchlist, int J, int T, int nParts, int curlevel, int max_branching, float stmult){
20912 int sum=0;
20913 float average =0;
20914 int ntot=0;
20915 for (int jit=0; jit < J; jit++){
20916 if (*(costlist+jit) > T) {ntot++; sum = sum +*(costlist+jit);}
20917 }
20918 average = ((float)sum)/((float)ntot);
20919 int cur;
20920
20921 int* indx = new int[J];
20922 for (int jit=0; jit < J; jit++) indx[jit]=jit;
20923 vector<int> myindx (indx, indx+J);
20924 vector<int>::iterator it;
20925 costlist_global=costlist;
20926 sort(myindx.begin(), myindx.begin()+J, jiafunc);
20927
20928
20929 int* templist = new int[J];
20930 int* temp2list = new int[J*nParts];
20931 int next = 0;
20932
20933 for (it=myindx.begin(); it!=myindx.end();++it){
20934 cur = *(costlist + *it);
20935 if (cur > T){
20936
20937 templist[next] = cur;
20938 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
20939 next = next + 1;
20940 }
20941 }
20942
20943 for (int jit=0; jit < ntot; jit++){
20944 *(costlist+jit)=*(templist + jit);
20945
20946 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
20947 }
20948
20949
20950 delete [] indx;
20951 delete [] templist;
20952 delete [] temp2list;
20953
20954 if (ntot == 1) return 1;
20955
20956
20957
20958 float sq_sum=0.0;
20959
20960 for (int i=0; i< ntot; i++){
20961 sq_sum = sq_sum + (float) pow((float) *(costlist+i) - average, (float)2.0);
20962
20963 }
20964
20965
20966 float variance = sq_sum/ntot;
20967 float stdev = (float)pow((float)variance,(float)0.5);
20968
20969
20970
20971 int B=1;
20972 int largest = *costlist;
20973
20974 for (int i=1; i<ntot; i++){
20975 int cur = *(costlist+i);
20976 if (largest-cur < (float)(stdev*stmult)) B++;
20977 else break;
20978
20979 }
20980
20981 if (branch_all < max_branching){
20982 if (B>1)
20983 {branch_all = branch_all + B-1;}
20984 }
20985 else B=1;
20986
20987 return B;
20988
20989
20990 }