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
00063 int circum_(double *, double *, double *, double *, int *);
00064 long int left_(double *, double *, double *, double *, double *, double *, double *, double *, double *);
00065 int addnod_(int *, int *, double *, double *, double *, int *, int *, int *, int *, int *);
00066
00067 vector<float> Util::infomask(EMData* Vol, EMData* mask, bool flip = false)
00068
00069
00070 {
00071 ENTERFUNC;
00072 vector<float> stats;
00073 float *Volptr, *maskptr,MAX,MIN;
00074 long double Sum1,Sum2;
00075 long count;
00076
00077 MAX = -FLT_MAX;
00078 MIN = FLT_MAX;
00079 count = 0L;
00080 Sum1 = 0.0L;
00081 Sum2 = 0.0L;
00082
00083 if (mask == NULL) {
00084
00085 stats.push_back(Vol->get_attr("mean"));
00086 stats.push_back(Vol->get_attr("sigma"));
00087 stats.push_back(Vol->get_attr("minimum"));
00088 stats.push_back(Vol->get_attr("maximum"));
00089 return stats;
00090 }
00091
00092
00093
00094 size_t nx = Vol->get_xsize();
00095 size_t ny = Vol->get_ysize();
00096 size_t nz = Vol->get_zsize();
00097
00098 size_t mask_nx = mask->get_xsize();
00099 size_t mask_ny = mask->get_ysize();
00100 size_t mask_nz = mask->get_zsize();
00101
00102 if (nx != mask_nx || ny != mask_ny || nz != mask_nz )
00103 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116 Volptr = Vol->get_data();
00117 maskptr = mask->get_data();
00118
00119 for (size_t i = 0; i < (size_t)nx*ny*nz; ++i) {
00120 if ((maskptr[i]>0.5f) == flip) {
00121 Sum1 += Volptr[i];
00122 Sum2 += Volptr[i]*double(Volptr[i]);
00123 MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00124 MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00125 count++;
00126 }
00127 }
00128
00129 if (count == 0) {
00130 LOGERR("Invalid mask");
00131 throw ImageFormatException( "Invalid mask");
00132 }
00133
00134 float avg = static_cast<float>(Sum1/count);
00135 float sig = static_cast<float>(sqrt((Sum2 - Sum1*Sum1/count)/(count-1)));
00136
00137 stats.push_back(avg);
00138 stats.push_back(sig);
00139 stats.push_back(MIN);
00140 stats.push_back(MAX);
00141
00142 return stats;
00143 }
00144
00145
00146
00147
00148 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00149 {
00150 ENTERFUNC;
00151
00152 if (!EMUtil::is_same_size(V1, V2)) {
00153 LOGERR("images not same size");
00154 throw ImageFormatException( "images not same size");
00155 }
00156
00157 size_t nx = V1->get_xsize();
00158 size_t ny = V1->get_ysize();
00159 size_t nz = V1->get_zsize();
00160 size_t size = (size_t)nx*ny*nz;
00161
00162 EMData *BD = new EMData();
00163 BD->set_size(nx, ny, nz);
00164
00165 float *params = new float[2];
00166
00167 float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00168 long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00169 int nvox = 0L;
00170
00171 V1ptr = V1->get_data();
00172 V2ptr = V2->get_data();
00173 BDptr = BD->get_data();
00174
00175
00176 if(!mask){
00177 EMData * Mask = new EMData();
00178 Mask->set_size(nx,ny,nz);
00179 Mask->to_one();
00180 MASKptr = Mask->get_data();
00181 } else {
00182 if (!EMUtil::is_same_size(V1, mask)) {
00183 LOGERR("mask not same size");
00184 throw ImageFormatException( "mask not same size");
00185 }
00186
00187 MASKptr = mask->get_data();
00188 }
00189
00190
00191
00192
00193
00194 for (size_t i = 0L;i < size; i++) {
00195 if (MASKptr[i]>0.5f) {
00196 S1 += V1ptr[i]*V2ptr[i];
00197 S2 += V1ptr[i]*V1ptr[i];
00198 S3 += V2ptr[i];
00199 S4 += V1ptr[i];
00200 nvox ++;
00201 }
00202 }
00203
00204 if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00205 A =1.0f ;
00206 } else {
00207 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00208 }
00209 B = static_cast<float> (A*S4 - S3)/nvox;
00210
00211
00212
00213 for (size_t i = 0L;i < size; i++) {
00214 if (MASKptr[i]>0.5f) {
00215 BDptr[i] = A*V1ptr[i] - B - V2ptr[i];
00216 } else {
00217 BDptr[i] = 0.f;
00218 }
00219 }
00220
00221 BD->update();
00222
00223 params[0] = A;
00224 params[1] = B;
00225
00226 Dict BDnParams;
00227 BDnParams["imdiff"] = BD;
00228 BDnParams["A"] = params[0];
00229 BDnParams["B"] = params[1];
00230
00231 EXITFUNC;
00232 return BDnParams;
00233 }
00234
00235
00236
00237
00238
00239 EMData *Util::TwoDTestFunc(int Size, float p, float q, float a, float b, int flag, float alphaDeg)
00240 {
00241 ENTERFUNC;
00242 int Mid= (Size+1)/2;
00243
00244 if (flag==0) {
00245 EMData* ImBW = new EMData();
00246 ImBW->set_size(Size,Size,1);
00247 ImBW->to_zero();
00248
00249 float tempIm;
00250 float x,y;
00251
00252 for (int ix=(1-Mid); ix<Mid; ix++){
00253 for (int iy=(1-Mid); iy<Mid; iy++){
00254 x = (float)ix;
00255 y = (float)iy;
00256 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)) );
00257 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00258 }
00259 }
00260 ImBW->update();
00261 ImBW->set_complex(false);
00262 ImBW->set_ri(true);
00263
00264 return ImBW;
00265 }
00266 else if (flag==1) {
00267 EMData* ImBWFFT = new EMData();
00268 ImBWFFT ->set_size(2*Size,Size,1);
00269 ImBWFFT ->to_zero();
00270
00271 float r,s;
00272
00273 for (int ir=(1-Mid); ir<Mid; ir++){
00274 for (int is=(1-Mid); is<Mid; is++){
00275 r = (float)ir;
00276 s = (float)is;
00277 (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00278 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00279 }
00280 }
00281 ImBWFFT->update();
00282 ImBWFFT->set_complex(true);
00283 ImBWFFT->set_ri(true);
00284 ImBWFFT->set_shuffled(true);
00285 ImBWFFT->set_fftodd(true);
00286
00287 return ImBWFFT;
00288 }
00289 else if (flag==2 || flag==3) {
00290 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00291 float C=cos(alpha);
00292 float S=sin(alpha);
00293 float D= sqrt(S*S*b*b + C*C*a*a);
00294
00295
00296 float P = p * C *a*a/D ;
00297 float Q = q * S *b*b/D ;
00298
00299 if (flag==2) {
00300 EMData* pofalpha = new EMData();
00301 pofalpha ->set_size(Size,1,1);
00302 pofalpha ->to_zero();
00303
00304 float Norm0 = D*(float)sqrt(2*pi);
00305 float Norm1 = exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00306 float Norm2 = exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00307 float sD;
00308
00309 for (int is=(1-Mid); is<Mid; is++){
00310 sD = is/D ;
00311 (*pofalpha)(is+Mid-1) = Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00312 + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00313 }
00314 pofalpha-> update();
00315 pofalpha-> set_complex(false);
00316 pofalpha-> set_ri(true);
00317
00318 return pofalpha;
00319 }
00320 if (flag==3) {
00321 float vD;
00322
00323 EMData* pofalphak = new EMData();
00324 pofalphak ->set_size(2*Size,1,1);
00325 pofalphak ->to_zero();
00326
00327 for (int iv=(1-Mid); iv<Mid; iv++){
00328 vD = iv*D ;
00329 (*pofalphak)(2*(iv+Mid-1)) = exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00330 }
00331 pofalphak-> update();
00332 pofalphak-> set_complex(false);
00333 pofalphak-> set_ri(true);
00334
00335 return pofalphak;
00336 }
00337 }
00338 else if (flag==4) {
00339 cout <<" FH under construction";
00340 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00341 EMData* TryFH= OutFT -> real2FH(4.0);
00342 return TryFH;
00343 } else {
00344 cout <<" flag must be 0,1,2,3, or 4";
00345 }
00346
00347 EXITFUNC;
00348 return 0;
00349 }
00350
00351
00352 void Util::spline_mat(float *x, float *y, int n, float *xq, float *yq, int m)
00353 {
00354
00355 float x0= x[0];
00356 float x1= x[1];
00357 float x2= x[2];
00358 float y0= y[0];
00359 float y1= y[1];
00360 float y2= y[2];
00361 float yp1 = (y1-y0)/(x1-x0) + (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1) ;
00362 float xn = x[n];
00363 float xnm1= x[n-1];
00364 float xnm2= x[n-2];
00365 float yn = y[n];
00366 float ynm1= y[n-1];
00367 float ynm2= y[n-2];
00368 float ypn= (yn-ynm1)/(xn-xnm1) + (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00369 float *y2d = new float[n];
00370 Util::spline(x,y,n,yp1,ypn,y2d);
00371 Util::splint(x,y,y2d,n,xq,yq,m);
00372 delete [] y2d;
00373 return;
00374 }
00375
00376
00377 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2)
00378 {
00379 int i,k;
00380 float p, qn, sig, un, *u;
00381 u = new float[n-1];
00382
00383 if (yp1 > .99e30){
00384 y2[0]=u[0]=0.0;
00385 } else {
00386 y2[0]=-.5f;
00387 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00388 }
00389
00390 for (i=1; i < n-1; i++) {
00391 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00392 p = sig*y2[i-1] + 2.0f;
00393 y2[i] = (sig-1.0f)/p;
00394 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) - (y[i] - y[i-1] )/(x[i] -x[i-1]);
00395 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00396 }
00397
00398 if (ypn>.99e30){
00399 qn=0; un=0;
00400 } else {
00401 qn= .5f;
00402 un= (3.0f/(x[n-1] -x[n-2])) * (ypn - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00403 }
00404 y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00405 for (k=n-2; k>=0; k--){
00406 y2[k]=y2[k]*y2[k+1]+u[k];
00407 }
00408 delete [] u;
00409 }
00410
00411
00412 void Util::splint( float *xa, float *ya, float *y2a, int n, float *xq, float *yq, int m)
00413 {
00414 int klo, khi, k;
00415 float h, b, a;
00416
00417
00418 for (int j=0; j<m;j++){
00419 klo=0;
00420 khi=n-1;
00421 while (khi-klo >1) {
00422 k=(khi+klo) >>1;
00423 if (xa[k]>xq[j]){ khi=k;}
00424 else { klo=k;}
00425 }
00426 h=xa[khi]- xa[klo];
00427 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00428 a =(xa[khi]-xq[j])/h;
00429 b=(xq[j]-xa[klo])/h;
00430 yq[j]=a*ya[klo] + b*ya[khi]
00431 + ((a*a*a-a)*y2a[klo]
00432 +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00433 }
00434
00435 }
00436
00437
00438 void Util::Radialize(int *PermMatTr, float *kValsSorted,
00439 float *weightofkValsSorted, int Size, int *SizeReturned)
00440 {
00441 int iMax = (int) floor( (Size-1.0)/2 +.01);
00442 int CountMax = (iMax+2)*(iMax+1)/2;
00443 int Count=-1;
00444 float *kVals = new float[CountMax];
00445 float *weightMat = new float[CountMax];
00446 int *PermMat = new int[CountMax];
00447 SizeReturned[0] = CountMax;
00448
00449
00450 for (int jkx=0; jkx< iMax+1; jkx++) {
00451 for (int jky=0; jky< jkx+1; jky++) {
00452 Count++;
00453 kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00454 weightMat[Count]= 1.0;
00455 if (jkx!=0) { weightMat[Count] *=2;}
00456 if (jky!=0) { weightMat[Count] *=2;}
00457 if (jkx!=jky){ weightMat[Count] *=2;}
00458 PermMat[Count]=Count+1;
00459 }
00460 }
00461
00462 int lkVals = Count+1;
00463
00464
00465 sort_mat(&kVals[0],&kVals[Count],
00466 &PermMat[0], &PermMat[Count]);
00467
00468 fflush(stdout);
00469
00470 int newInd;
00471
00472 for (int iP=0; iP < lkVals ; iP++ ) {
00473 newInd = PermMat[iP];
00474 PermMatTr[newInd-1] = iP+1;
00475 }
00476
00477
00478
00479 int CountA=-1;
00480 int CountB=-1;
00481
00482 while (CountB< (CountMax-1)) {
00483 CountA++;
00484 CountB++;
00485
00486 kValsSorted[CountA] = kVals[CountB] ;
00487 if (CountB<(CountMax-1) ) {
00488 while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001 ) {
00489 SizeReturned[0]--;
00490 for (int iP=0; iP < lkVals; iP++){
00491
00492 if (PermMatTr[iP]>CountA+1) {
00493 PermMatTr[iP]--;
00494 }
00495 }
00496 CountB++;
00497 }
00498 }
00499 }
00500
00501
00502 for (int CountD=0; CountD < CountMax; CountD++) {
00503 newInd = PermMatTr[CountD];
00504 weightofkValsSorted[newInd-1] += weightMat[CountD];
00505 }
00506
00507 }
00508
00509
00510 vector<float>
00511 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00512 {
00513 vector<float> angles;
00514 float psi = 0.0;
00515 if ((0.0 == t1 && 0.0 == t2)||(t1 >= t2)) {
00516 t1 = 0.0f;
00517 t2 = 90.0f;
00518 }
00519 if ((0.0 == p1 && 0.0 == p2)||(p1 >= p2)) {
00520 p1 = 0.0f;
00521 p2 = 359.9f;
00522 }
00523 bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00524 for (float theta = t1; theta <= t2; theta += delta) {
00525 float detphi;
00526 int lt;
00527 if ((0.0 == theta)||(180.0 == theta)) {
00528 detphi = 360.0f;
00529 lt = 1;
00530 } else {
00531 detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00532 lt = int((p2 - p1)/detphi)-1;
00533 if (lt < 1) lt = 1;
00534 detphi = (p2 - p1)/lt;
00535 }
00536 for (int i = 0; i < lt; i++) {
00537 float phi = p1 + i*detphi;
00538 if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00539 angles.push_back(phi);
00540 angles.push_back(theta);
00541 angles.push_back(psi);
00542 }
00543 }
00544 return angles;
00545 }
00546
00547
00548 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00651 {
00652
00653
00654 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00655 float quadri;
00656 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00657
00658 x = xx;
00659 y = yy;
00660
00661
00662 while ( x < 1.0 ) x += nxdata;
00663 while ( x >= (float)(nxdata+1) ) x -= nxdata;
00664 while ( y < 1.0 ) y += nydata;
00665 while ( y >= (float)(nydata+1) ) y -= nydata;
00666
00667 i = (int) x;
00668 j = (int) y;
00669
00670 dx0 = x - i;
00671 dy0 = y - j;
00672
00673 ip1 = i + 1;
00674 im1 = i - 1;
00675 jp1 = j + 1;
00676 jm1 = j - 1;
00677
00678 if (ip1 > nxdata) ip1 -= nxdata;
00679 if (im1 < 1) im1 += nxdata;
00680 if (jp1 > nydata) jp1 -= nydata;
00681 if (jm1 < 1) jm1 += nydata;
00682
00683 f0 = fdata(i,j);
00684 c1 = fdata(ip1,j) - f0;
00685 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00686 c3 = fdata(i,jp1) - f0;
00687 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00688
00689 dxb = dx0 - 1;
00690 dyb = dy0 - 1;
00691
00692
00693 if (dx0 >= 0) hxc = 1; else hxc = -1;
00694 if (dy0 >= 0) hyc = 1; else hyc = -1;
00695
00696 ic = i + hxc;
00697 jc = j + hyc;
00698
00699 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00700 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00701
00702 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00703 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00704
00705
00706 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00707
00708 return quadri;
00709 }
00710
00711 #undef fdata
00712
00713 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00714 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00715 {
00716
00717
00718 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00719 float quadri;
00720 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00721
00722 x = xx;
00723 y = yy;
00724
00725
00726 if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00727 x = (float)xnew;
00728 y = (float)ynew;
00729 }
00730
00731
00732 i = (int) x;
00733 j = (int) y;
00734
00735 dx0 = x - i;
00736 dy0 = y - j;
00737
00738 ip1 = i + 1;
00739 im1 = i - 1;
00740 jp1 = j + 1;
00741 jm1 = j - 1;
00742
00743 if (ip1 > nxdata) ip1 -= nxdata;
00744 if (im1 < 1) im1 += nxdata;
00745 if (jp1 > nydata) jp1 -= nydata;
00746 if (jm1 < 1) jm1 += nydata;
00747
00748 f0 = fdata(i,j);
00749 c1 = fdata(ip1,j) - f0;
00750 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00751 c3 = fdata(i,jp1) - f0;
00752 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00753
00754 dxb = dx0 - 1;
00755 dyb = dy0 - 1;
00756
00757
00758 if (dx0 >= 0) hxc = 1; else hxc = -1;
00759 if (dy0 >= 0) hyc = 1; else hyc = -1;
00760
00761 ic = i + hxc;
00762 jc = j + hyc;
00763
00764 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00765 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00766
00767 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00768 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00769
00770
00771 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00772
00773 return quadri;
00774 }
00775
00776 #undef fdata
00777
00778
00779 float Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00780 int K = kb.get_window_size();
00781 int kbmin = -K/2;
00782 int kbmax = -kbmin;
00783 int kbc = kbmax+1;
00784
00785 float pixel =0.0f;
00786 float w=0.0f;
00787
00788 delx = restrict1(delx, nx);
00789 int inxold = int(round(delx));
00790 if ( ny < 2 ) {
00791 float tablex1 = kb.i0win_tab(delx-inxold+3);
00792 float tablex2 = kb.i0win_tab(delx-inxold+2);
00793 float tablex3 = kb.i0win_tab(delx-inxold+1);
00794 float tablex4 = kb.i0win_tab(delx-inxold);
00795 float tablex5 = kb.i0win_tab(delx-inxold-1);
00796 float tablex6 = kb.i0win_tab(delx-inxold-2);
00797 float tablex7 = kb.i0win_tab(delx-inxold-3);
00798
00799 int x1, x2, x3, x4, x5, x6, x7;
00800
00801 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
00802 x1 = (inxold-3+nx)%nx;
00803 x2 = (inxold-2+nx)%nx;
00804 x3 = (inxold-1+nx)%nx;
00805 x4 = (inxold +nx)%nx;
00806 x5 = (inxold+1+nx)%nx;
00807 x6 = (inxold+2+nx)%nx;
00808 x7 = (inxold+3+nx)%nx;
00809 } else {
00810 x1 = inxold-3;
00811 x2 = inxold-2;
00812 x3 = inxold-1;
00813 x4 = inxold;
00814 x5 = inxold+1;
00815 x6 = inxold+2;
00816 x7 = inxold+3;
00817 }
00818
00819 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00820 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00821 data[x7]*tablex7 ;
00822
00823 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00824 } else if ( nz < 2 ) {
00825 dely = restrict1(dely, ny);
00826 int inyold = int(round(dely));
00827 float tablex1 = kb.i0win_tab(delx-inxold+3);
00828 float tablex2 = kb.i0win_tab(delx-inxold+2);
00829 float tablex3 = kb.i0win_tab(delx-inxold+1);
00830 float tablex4 = kb.i0win_tab(delx-inxold);
00831 float tablex5 = kb.i0win_tab(delx-inxold-1);
00832 float tablex6 = kb.i0win_tab(delx-inxold-2);
00833 float tablex7 = kb.i0win_tab(delx-inxold-3);
00834
00835 float tabley1 = kb.i0win_tab(dely-inyold+3);
00836 float tabley2 = kb.i0win_tab(dely-inyold+2);
00837 float tabley3 = kb.i0win_tab(dely-inyold+1);
00838 float tabley4 = kb.i0win_tab(dely-inyold);
00839 float tabley5 = kb.i0win_tab(dely-inyold-1);
00840 float tabley6 = kb.i0win_tab(dely-inyold-2);
00841 float tabley7 = kb.i0win_tab(dely-inyold-3);
00842
00843 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00844
00845 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
00846 x1 = (inxold-3+nx)%nx;
00847 x2 = (inxold-2+nx)%nx;
00848 x3 = (inxold-1+nx)%nx;
00849 x4 = (inxold +nx)%nx;
00850 x5 = (inxold+1+nx)%nx;
00851 x6 = (inxold+2+nx)%nx;
00852 x7 = (inxold+3+nx)%nx;
00853
00854 y1 = ((inyold-3+ny)%ny)*nx;
00855 y2 = ((inyold-2+ny)%ny)*nx;
00856 y3 = ((inyold-1+ny)%ny)*nx;
00857 y4 = ((inyold +ny)%ny)*nx;
00858 y5 = ((inyold+1+ny)%ny)*nx;
00859 y6 = ((inyold+2+ny)%ny)*nx;
00860 y7 = ((inyold+3+ny)%ny)*nx;
00861 } else {
00862 x1 = inxold-3;
00863 x2 = inxold-2;
00864 x3 = inxold-1;
00865 x4 = inxold;
00866 x5 = inxold+1;
00867 x6 = inxold+2;
00868 x7 = inxold+3;
00869
00870 y1 = (inyold-3)*nx;
00871 y2 = (inyold-2)*nx;
00872 y3 = (inyold-1)*nx;
00873 y4 = inyold*nx;
00874 y5 = (inyold+1)*nx;
00875 y6 = (inyold+2)*nx;
00876 y7 = (inyold+3)*nx;
00877 }
00878
00879 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00880 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00881 data[x7+y1]*tablex7 ) * tabley1 +
00882 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00883 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00884 data[x7+y2]*tablex7 ) * tabley2 +
00885 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00886 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00887 data[x7+y3]*tablex7 ) * tabley3 +
00888 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00889 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00890 data[x7+y4]*tablex7 ) * tabley4 +
00891 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00892 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00893 data[x7+y5]*tablex7 ) * tabley5 +
00894 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00895 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00896 data[x7+y6]*tablex7 ) * tabley6 +
00897 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00898 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00899 data[x7+y7]*tablex7 ) * tabley7;
00900
00901 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00902 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00903 } else {
00904 dely = restrict1(dely, ny);
00905 int inyold = int(Util::round(dely));
00906 delz = restrict1(delz, nz);
00907 int inzold = int(Util::round(delz));
00908
00909 float tablex1 = kb.i0win_tab(delx-inxold+3);
00910 float tablex2 = kb.i0win_tab(delx-inxold+2);
00911 float tablex3 = kb.i0win_tab(delx-inxold+1);
00912 float tablex4 = kb.i0win_tab(delx-inxold);
00913 float tablex5 = kb.i0win_tab(delx-inxold-1);
00914 float tablex6 = kb.i0win_tab(delx-inxold-2);
00915 float tablex7 = kb.i0win_tab(delx-inxold-3);
00916
00917 float tabley1 = kb.i0win_tab(dely-inyold+3);
00918 float tabley2 = kb.i0win_tab(dely-inyold+2);
00919 float tabley3 = kb.i0win_tab(dely-inyold+1);
00920 float tabley4 = kb.i0win_tab(dely-inyold);
00921 float tabley5 = kb.i0win_tab(dely-inyold-1);
00922 float tabley6 = kb.i0win_tab(dely-inyold-2);
00923 float tabley7 = kb.i0win_tab(dely-inyold-3);
00924
00925 float tablez1 = kb.i0win_tab(delz-inzold+3);
00926 float tablez2 = kb.i0win_tab(delz-inzold+2);
00927 float tablez3 = kb.i0win_tab(delz-inzold+1);
00928 float tablez4 = kb.i0win_tab(delz-inzold);
00929 float tablez5 = kb.i0win_tab(delz-inzold-1);
00930 float tablez6 = kb.i0win_tab(delz-inzold-2);
00931 float tablez7 = kb.i0win_tab(delz-inzold-3);
00932
00933 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00934
00935 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
00936 x1 = (inxold-3+nx)%nx;
00937 x2 = (inxold-2+nx)%nx;
00938 x3 = (inxold-1+nx)%nx;
00939 x4 = (inxold +nx)%nx;
00940 x5 = (inxold+1+nx)%nx;
00941 x6 = (inxold+2+nx)%nx;
00942 x7 = (inxold+3+nx)%nx;
00943
00944 y1 = ((inyold-3+ny)%ny)*nx;
00945 y2 = ((inyold-2+ny)%ny)*nx;
00946 y3 = ((inyold-1+ny)%ny)*nx;
00947 y4 = ((inyold +ny)%ny)*nx;
00948 y5 = ((inyold+1+ny)%ny)*nx;
00949 y6 = ((inyold+2+ny)%ny)*nx;
00950 y7 = ((inyold+3+ny)%ny)*nx;
00951
00952 z1 = ((inzold-3+nz)%nz)*nx*ny;
00953 z2 = ((inzold-2+nz)%nz)*nx*ny;
00954 z3 = ((inzold-1+nz)%nz)*nx*ny;
00955 z4 = ((inzold +nz)%nz)*nx*ny;
00956 z5 = ((inzold+1+nz)%nz)*nx*ny;
00957 z6 = ((inzold+2+nz)%nz)*nx*ny;
00958 z7 = ((inzold+3+nz)%nz)*nx*ny;
00959 } else {
00960 x1 = inxold-3;
00961 x2 = inxold-2;
00962 x3 = inxold-1;
00963 x4 = inxold;
00964 x5 = inxold+1;
00965 x6 = inxold+2;
00966 x7 = inxold+3;
00967
00968 y1 = (inyold-3)*nx;
00969 y2 = (inyold-2)*nx;
00970 y3 = (inyold-1)*nx;
00971 y4 = inyold*nx;
00972 y5 = (inyold+1)*nx;
00973 y6 = (inyold+2)*nx;
00974 y7 = (inyold+3)*nx;
00975
00976 z1 = (inzold-3)*nx*ny;
00977 z2 = (inzold-2)*nx*ny;
00978 z3 = (inzold-1)*nx*ny;
00979 z4 = inzold*nx*ny;
00980 z5 = (inzold+1)*nx*ny;
00981 z6 = (inzold+2)*nx*ny;
00982 z7 = (inzold+3)*nx*ny;
00983 }
00984
00985 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00986 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00987 data[x7+y1+z1]*tablex7 ) * tabley1 +
00988 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00989 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00990 data[x7+y2+z1]*tablex7 ) * tabley2 +
00991 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00992 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00993 data[x7+y3+z1]*tablex7 ) * tabley3 +
00994 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00995 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00996 data[x7+y4+z1]*tablex7 ) * tabley4 +
00997 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
00998 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
00999 data[x7+y5+z1]*tablex7 ) * tabley5 +
01000 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01001 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01002 data[x7+y6+z1]*tablex7 ) * tabley6 +
01003 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01004 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01005 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01006 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01007 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01008 data[x7+y1+z2]*tablex7 ) * tabley1 +
01009 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01010 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01011 data[x7+y2+z2]*tablex7 ) * tabley2 +
01012 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01013 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01014 data[x7+y3+z2]*tablex7 ) * tabley3 +
01015 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01016 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01017 data[x7+y4+z2]*tablex7 ) * tabley4 +
01018 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01019 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01020 data[x7+y5+z2]*tablex7 ) * tabley5 +
01021 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01022 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01023 data[x7+y6+z2]*tablex7 ) * tabley6 +
01024 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01025 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01026 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01027 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01028 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01029 data[x7+y1+z3]*tablex7 ) * tabley1 +
01030 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01031 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01032 data[x7+y2+z3]*tablex7 ) * tabley2 +
01033 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01034 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01035 data[x7+y3+z3]*tablex7 ) * tabley3 +
01036 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01037 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01038 data[x7+y4+z3]*tablex7 ) * tabley4 +
01039 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01040 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01041 data[x7+y5+z3]*tablex7 ) * tabley5 +
01042 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01043 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01044 data[x7+y6+z3]*tablex7 ) * tabley6 +
01045 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01046 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01047 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01048 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01049 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01050 data[x7+y1+z4]*tablex7 ) * tabley1 +
01051 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01052 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01053 data[x7+y2+z4]*tablex7 ) * tabley2 +
01054 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01055 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01056 data[x7+y3+z4]*tablex7 ) * tabley3 +
01057 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01058 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01059 data[x7+y4+z4]*tablex7 ) * tabley4 +
01060 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01061 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01062 data[x7+y5+z4]*tablex7 ) * tabley5 +
01063 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01064 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01065 data[x7+y6+z4]*tablex7 ) * tabley6 +
01066 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01067 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01068 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01069 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01070 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01071 data[x7+y1+z5]*tablex7 ) * tabley1 +
01072 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01073 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01074 data[x7+y2+z5]*tablex7 ) * tabley2 +
01075 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01076 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01077 data[x7+y3+z5]*tablex7 ) * tabley3 +
01078 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01079 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01080 data[x7+y4+z5]*tablex7 ) * tabley4 +
01081 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01082 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01083 data[x7+y5+z5]*tablex7 ) * tabley5 +
01084 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01085 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01086 data[x7+y6+z5]*tablex7 ) * tabley6 +
01087 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01088 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01089 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01090 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01091 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01092 data[x7+y1+z6]*tablex7 ) * tabley1 +
01093 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01094 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01095 data[x7+y2+z6]*tablex7 ) * tabley2 +
01096 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01097 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01098 data[x7+y3+z6]*tablex7 ) * tabley3 +
01099 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01100 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01101 data[x7+y4+z6]*tablex7 ) * tabley4 +
01102 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01103 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01104 data[x7+y5+z6]*tablex7 ) * tabley5 +
01105 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01106 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01107 data[x7+y6+z6]*tablex7 ) * tabley6 +
01108 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01109 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01110 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01111 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01112 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01113 data[x7+y1+z7]*tablex7 ) * tabley1 +
01114 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01115 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01116 data[x7+y2+z7]*tablex7 ) * tabley2 +
01117 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01118 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01119 data[x7+y3+z7]*tablex7 ) * tabley3 +
01120 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01121 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01122 data[x7+y4+z7]*tablex7 ) * tabley4 +
01123 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01124 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01125 data[x7+y5+z7]*tablex7 ) * tabley5 +
01126 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01127 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01128 data[x7+y6+z7]*tablex7 ) * tabley6 +
01129 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01130 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01131 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01132
01133 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01134 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01135 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01136 }
01137 return pixel/w;
01138 }
01139
01140 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) {
01141 int K = kb.get_window_size();
01142 int kbmin = -K/2;
01143 int kbmax = -kbmin;
01144 int kbc = kbmax+1;
01145
01146 float pixel =0.0f;
01147 float w=0.0f;
01148
01149 float argdelx = delx;
01150 delx = restrict1(delx, nx);
01151 int inxold = int(round(delx));
01152 if ( ny < 2 ) {
01153 float tablex1 = kb.i0win_tab(delx-inxold+3);
01154 float tablex2 = kb.i0win_tab(delx-inxold+2);
01155 float tablex3 = kb.i0win_tab(delx-inxold+1);
01156 float tablex4 = kb.i0win_tab(delx-inxold);
01157 float tablex5 = kb.i0win_tab(delx-inxold-1);
01158 float tablex6 = kb.i0win_tab(delx-inxold-2);
01159 float tablex7 = kb.i0win_tab(delx-inxold-3);
01160
01161 int x1, x2, x3, x4, x5, x6, x7;
01162
01163 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
01164 x1 = (inxold-3+nx)%nx;
01165 x2 = (inxold-2+nx)%nx;
01166 x3 = (inxold-1+nx)%nx;
01167 x4 = (inxold +nx)%nx;
01168 x5 = (inxold+1+nx)%nx;
01169 x6 = (inxold+2+nx)%nx;
01170 x7 = (inxold+3+nx)%nx;
01171 } else {
01172 x1 = inxold-3;
01173 x2 = inxold-2;
01174 x3 = inxold-1;
01175 x4 = inxold;
01176 x5 = inxold+1;
01177 x6 = inxold+2;
01178 x7 = inxold+3;
01179 }
01180
01181 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01182 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01183 data[x7]*tablex7 ;
01184
01185 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01186 } else if ( nz < 2 ) {
01187
01188 delx = argdelx;
01189
01190 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01191 delx = (float)xnew*2.0f;
01192 dely = (float)ynew*2.0f;
01193 }
01194
01195 int inxold = int(round(delx));
01196 int inyold = int(round(dely));
01197
01198 float tablex1 = kb.i0win_tab(delx-inxold+3);
01199 float tablex2 = kb.i0win_tab(delx-inxold+2);
01200 float tablex3 = kb.i0win_tab(delx-inxold+1);
01201 float tablex4 = kb.i0win_tab(delx-inxold);
01202 float tablex5 = kb.i0win_tab(delx-inxold-1);
01203 float tablex6 = kb.i0win_tab(delx-inxold-2);
01204 float tablex7 = kb.i0win_tab(delx-inxold-3);
01205
01206 float tabley1 = kb.i0win_tab(dely-inyold+3);
01207 float tabley2 = kb.i0win_tab(dely-inyold+2);
01208 float tabley3 = kb.i0win_tab(dely-inyold+1);
01209 float tabley4 = kb.i0win_tab(dely-inyold);
01210 float tabley5 = kb.i0win_tab(dely-inyold-1);
01211 float tabley6 = kb.i0win_tab(dely-inyold-2);
01212 float tabley7 = kb.i0win_tab(dely-inyold-3);
01213
01214 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01215
01216 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
01217 x1 = (inxold-3+nx)%nx;
01218 x2 = (inxold-2+nx)%nx;
01219 x3 = (inxold-1+nx)%nx;
01220 x4 = (inxold +nx)%nx;
01221 x5 = (inxold+1+nx)%nx;
01222 x6 = (inxold+2+nx)%nx;
01223 x7 = (inxold+3+nx)%nx;
01224
01225 y1 = ((inyold-3+ny)%ny)*nx;
01226 y2 = ((inyold-2+ny)%ny)*nx;
01227 y3 = ((inyold-1+ny)%ny)*nx;
01228 y4 = ((inyold +ny)%ny)*nx;
01229 y5 = ((inyold+1+ny)%ny)*nx;
01230 y6 = ((inyold+2+ny)%ny)*nx;
01231 y7 = ((inyold+3+ny)%ny)*nx;
01232 } else {
01233 x1 = inxold-3;
01234 x2 = inxold-2;
01235 x3 = inxold-1;
01236 x4 = inxold;
01237 x5 = inxold+1;
01238 x6 = inxold+2;
01239 x7 = inxold+3;
01240
01241 y1 = (inyold-3)*nx;
01242 y2 = (inyold-2)*nx;
01243 y3 = (inyold-1)*nx;
01244 y4 = inyold*nx;
01245 y5 = (inyold+1)*nx;
01246 y6 = (inyold+2)*nx;
01247 y7 = (inyold+3)*nx;
01248 }
01249
01250 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01251 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01252 data[x7+y1]*tablex7 ) * tabley1 +
01253 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01254 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01255 data[x7+y2]*tablex7 ) * tabley2 +
01256 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01257 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01258 data[x7+y3]*tablex7 ) * tabley3 +
01259 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01260 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01261 data[x7+y4]*tablex7 ) * tabley4 +
01262 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01263 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01264 data[x7+y5]*tablex7 ) * tabley5 +
01265 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01266 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01267 data[x7+y6]*tablex7 ) * tabley6 +
01268 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01269 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01270 data[x7+y7]*tablex7 ) * tabley7;
01271
01272 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01273 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01274 } else {
01275 dely = restrict1(dely, ny);
01276 int inyold = int(Util::round(dely));
01277 delz = restrict1(delz, nz);
01278 int inzold = int(Util::round(delz));
01279
01280 float tablex1 = kb.i0win_tab(delx-inxold+3);
01281 float tablex2 = kb.i0win_tab(delx-inxold+2);
01282 float tablex3 = kb.i0win_tab(delx-inxold+1);
01283 float tablex4 = kb.i0win_tab(delx-inxold);
01284 float tablex5 = kb.i0win_tab(delx-inxold-1);
01285 float tablex6 = kb.i0win_tab(delx-inxold-2);
01286 float tablex7 = kb.i0win_tab(delx-inxold-3);
01287
01288 float tabley1 = kb.i0win_tab(dely-inyold+3);
01289 float tabley2 = kb.i0win_tab(dely-inyold+2);
01290 float tabley3 = kb.i0win_tab(dely-inyold+1);
01291 float tabley4 = kb.i0win_tab(dely-inyold);
01292 float tabley5 = kb.i0win_tab(dely-inyold-1);
01293 float tabley6 = kb.i0win_tab(dely-inyold-2);
01294 float tabley7 = kb.i0win_tab(dely-inyold-3);
01295
01296 float tablez1 = kb.i0win_tab(delz-inzold+3);
01297 float tablez2 = kb.i0win_tab(delz-inzold+2);
01298 float tablez3 = kb.i0win_tab(delz-inzold+1);
01299 float tablez4 = kb.i0win_tab(delz-inzold);
01300 float tablez5 = kb.i0win_tab(delz-inzold-1);
01301 float tablez6 = kb.i0win_tab(delz-inzold-2);
01302 float tablez7 = kb.i0win_tab(delz-inzold-3);
01303
01304 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01305
01306 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
01307 x1 = (inxold-3+nx)%nx;
01308 x2 = (inxold-2+nx)%nx;
01309 x3 = (inxold-1+nx)%nx;
01310 x4 = (inxold +nx)%nx;
01311 x5 = (inxold+1+nx)%nx;
01312 x6 = (inxold+2+nx)%nx;
01313 x7 = (inxold+3+nx)%nx;
01314
01315 y1 = ((inyold-3+ny)%ny)*nx;
01316 y2 = ((inyold-2+ny)%ny)*nx;
01317 y3 = ((inyold-1+ny)%ny)*nx;
01318 y4 = ((inyold +ny)%ny)*nx;
01319 y5 = ((inyold+1+ny)%ny)*nx;
01320 y6 = ((inyold+2+ny)%ny)*nx;
01321 y7 = ((inyold+3+ny)%ny)*nx;
01322
01323 z1 = ((inzold-3+nz)%nz)*nx*ny;
01324 z2 = ((inzold-2+nz)%nz)*nx*ny;
01325 z3 = ((inzold-1+nz)%nz)*nx*ny;
01326 z4 = ((inzold +nz)%nz)*nx*ny;
01327 z5 = ((inzold+1+nz)%nz)*nx*ny;
01328 z6 = ((inzold+2+nz)%nz)*nx*ny;
01329 z7 = ((inzold+3+nz)%nz)*nx*ny;
01330 } else {
01331 x1 = inxold-3;
01332 x2 = inxold-2;
01333 x3 = inxold-1;
01334 x4 = inxold;
01335 x5 = inxold+1;
01336 x6 = inxold+2;
01337 x7 = inxold+3;
01338
01339 y1 = (inyold-3)*nx;
01340 y2 = (inyold-2)*nx;
01341 y3 = (inyold-1)*nx;
01342 y4 = inyold*nx;
01343 y5 = (inyold+1)*nx;
01344 y6 = (inyold+2)*nx;
01345 y7 = (inyold+3)*nx;
01346
01347 z1 = (inzold-3)*nx*ny;
01348 z2 = (inzold-2)*nx*ny;
01349 z3 = (inzold-1)*nx*ny;
01350 z4 = inzold*nx*ny;
01351 z5 = (inzold+1)*nx*ny;
01352 z6 = (inzold+2)*nx*ny;
01353 z7 = (inzold+3)*nx*ny;
01354 }
01355
01356 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01357 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01358 data[x7+y1+z1]*tablex7 ) * tabley1 +
01359 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01360 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01361 data[x7+y2+z1]*tablex7 ) * tabley2 +
01362 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01363 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01364 data[x7+y3+z1]*tablex7 ) * tabley3 +
01365 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01366 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01367 data[x7+y4+z1]*tablex7 ) * tabley4 +
01368 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01369 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01370 data[x7+y5+z1]*tablex7 ) * tabley5 +
01371 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01372 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01373 data[x7+y6+z1]*tablex7 ) * tabley6 +
01374 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01375 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01376 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01377 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01378 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01379 data[x7+y1+z2]*tablex7 ) * tabley1 +
01380 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01381 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01382 data[x7+y2+z2]*tablex7 ) * tabley2 +
01383 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01384 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01385 data[x7+y3+z2]*tablex7 ) * tabley3 +
01386 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01387 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01388 data[x7+y4+z2]*tablex7 ) * tabley4 +
01389 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01390 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01391 data[x7+y5+z2]*tablex7 ) * tabley5 +
01392 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01393 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01394 data[x7+y6+z2]*tablex7 ) * tabley6 +
01395 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01396 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01397 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01398 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01399 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01400 data[x7+y1+z3]*tablex7 ) * tabley1 +
01401 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01402 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01403 data[x7+y2+z3]*tablex7 ) * tabley2 +
01404 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01405 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01406 data[x7+y3+z3]*tablex7 ) * tabley3 +
01407 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01408 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01409 data[x7+y4+z3]*tablex7 ) * tabley4 +
01410 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01411 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01412 data[x7+y5+z3]*tablex7 ) * tabley5 +
01413 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01414 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01415 data[x7+y6+z3]*tablex7 ) * tabley6 +
01416 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01417 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01418 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01419 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01420 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01421 data[x7+y1+z4]*tablex7 ) * tabley1 +
01422 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01423 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01424 data[x7+y2+z4]*tablex7 ) * tabley2 +
01425 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01426 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01427 data[x7+y3+z4]*tablex7 ) * tabley3 +
01428 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01429 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01430 data[x7+y4+z4]*tablex7 ) * tabley4 +
01431 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01432 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01433 data[x7+y5+z4]*tablex7 ) * tabley5 +
01434 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01435 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01436 data[x7+y6+z4]*tablex7 ) * tabley6 +
01437 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01438 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01439 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01440 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01441 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01442 data[x7+y1+z5]*tablex7 ) * tabley1 +
01443 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01444 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01445 data[x7+y2+z5]*tablex7 ) * tabley2 +
01446 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01447 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01448 data[x7+y3+z5]*tablex7 ) * tabley3 +
01449 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01450 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01451 data[x7+y4+z5]*tablex7 ) * tabley4 +
01452 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01453 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01454 data[x7+y5+z5]*tablex7 ) * tabley5 +
01455 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01456 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01457 data[x7+y6+z5]*tablex7 ) * tabley6 +
01458 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01459 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01460 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01461 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01462 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01463 data[x7+y1+z6]*tablex7 ) * tabley1 +
01464 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01465 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01466 data[x7+y2+z6]*tablex7 ) * tabley2 +
01467 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01468 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01469 data[x7+y3+z6]*tablex7 ) * tabley3 +
01470 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01471 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01472 data[x7+y4+z6]*tablex7 ) * tabley4 +
01473 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01474 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01475 data[x7+y5+z6]*tablex7 ) * tabley5 +
01476 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01477 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01478 data[x7+y6+z6]*tablex7 ) * tabley6 +
01479 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01480 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01481 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01482 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01483 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01484 data[x7+y1+z7]*tablex7 ) * tabley1 +
01485 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01486 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01487 data[x7+y2+z7]*tablex7 ) * tabley2 +
01488 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01489 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01490 data[x7+y3+z7]*tablex7 ) * tabley3 +
01491 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01492 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01493 data[x7+y4+z7]*tablex7 ) * tabley4 +
01494 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01495 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01496 data[x7+y5+z7]*tablex7 ) * tabley5 +
01497 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01498 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01499 data[x7+y6+z7]*tablex7 ) * tabley6 +
01500 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01501 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01502 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01503
01504 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01505 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01506 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01507 }
01508 return pixel/w;
01509 }
01510
01511
01512
01513
01514
01515
01516
01517
01518
01519
01520
01521
01522
01523
01524
01525
01526
01527
01528
01529
01530
01531
01532
01533
01534
01535
01536
01537
01538
01539
01540
01541
01542
01543
01544
01545
01546
01547
01548
01549
01550
01551
01552
01553
01554
01555
01556
01557
01558
01559
01560
01561
01562
01563
01564
01565
01566
01567
01568
01569
01570
01571
01572
01573
01574
01575
01576
01577
01578
01579
01580
01581
01582
01583
01584
01585
01586
01587
01588
01589
01590
01591
01592
01593
01594
01595
01596
01597
01598
01599
01600
01601
01602
01603
01604
01605
01606
01607
01608
01609
01610
01611
01612
01613
01614
01615
01616
01617
01618
01619
01620
01621
01622
01623
01624
01625
01626
01627
01628
01629
01630
01631
01632
01633
01634
01635
01636
01637
01638
01639
01640
01641
01642
01643
01644
01645
01646
01647
01648
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672
01673
01674
01675
01676
01677
01678
01679
01680
01681
01682
01683
01684
01685
01686
01687
01688
01689 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01690
01691 int nxreal = nx - 2;
01692 if (nxreal != ny)
01693 throw ImageDimensionException("extractpoint requires ny == nx");
01694 int nhalf = nxreal/2;
01695 bool flip = (nuxnew < 0.f);
01696 if (flip) {
01697 nuxnew *= -1;
01698 nuynew *= -1;
01699 }
01700 if (nuynew >= nhalf-0.5) {
01701 nuynew -= nxreal;
01702 } else if (nuynew < -nhalf-0.5) {
01703 nuynew += nxreal;
01704 }
01705
01706
01707
01708
01709 int ixn = int(Util::round(nuxnew));
01710 int iyn = int(Util::round(nuynew));
01711
01712
01713 static float wy[7];
01714 static float wx[7];
01715
01716 float iynn = nuynew - iyn;
01717 wy[0] = kb.i0win_tab(iynn+3);
01718 wy[1] = kb.i0win_tab(iynn+2);
01719 wy[2] = kb.i0win_tab(iynn+1);
01720 wy[3] = kb.i0win_tab(iynn);
01721 wy[4] = kb.i0win_tab(iynn-1);
01722 wy[5] = kb.i0win_tab(iynn-2);
01723 wy[6] = kb.i0win_tab(iynn-3);
01724
01725 float ixnn = nuxnew - ixn;
01726 wx[0] = kb.i0win_tab(ixnn+3);
01727 wx[1] = kb.i0win_tab(ixnn+2);
01728 wx[2] = kb.i0win_tab(ixnn+1);
01729 wx[3] = kb.i0win_tab(ixnn);
01730 wx[4] = kb.i0win_tab(ixnn-1);
01731 wx[5] = kb.i0win_tab(ixnn-2);
01732 wx[6] = kb.i0win_tab(ixnn-3);
01733
01734 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]);
01735
01736 complex<float> result(0.f,0.f);
01737 if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01738
01739 for (int iy = 0; iy < 7; iy++) {
01740 int iyp = iyn + iy - 3 ;
01741 for (int ix = 0; ix < 7; ix++) {
01742 int ixp = ixn + ix - 3;
01743 float w = wx[ix]*wy[iy];
01744 complex<float> val = fimage->cmplx(ixp,iyp);
01745 result += val*w;
01746 }
01747 }
01748 } else {
01749
01750 for (int iy = 0; iy < 7; iy++) {
01751 int iyp = iyn + iy - 3;
01752 for (int ix = 0; ix < 7; ix++) {
01753 int ixp = ixn + ix - 3;
01754 bool mirror = false;
01755 int ixt = ixp, iyt = iyp;
01756 if (ixt < 0) {
01757 ixt = -ixt;
01758 iyt = -iyt;
01759 mirror = !mirror;
01760 }
01761 if (ixt > nhalf) {
01762 ixt = nxreal - ixt;
01763 iyt = -iyt;
01764 mirror = !mirror;
01765 }
01766 if (iyt > nhalf-1) iyt -= nxreal;
01767 if (iyt < -nhalf) iyt += nxreal;
01768 float w = wx[ix]*wy[iy];
01769 complex<float> val = fimage->cmplx(ixt,iyt);
01770 if (mirror) result += conj(val)*w;
01771 else result += val*w;
01772 }
01773 }
01774 }
01775 if (flip) result = conj(result)/wsum;
01776 else result /= wsum;
01777 return result;
01778 }
01779
01780
01781
01782
01783
01784
01785
01786
01787
01788
01789
01790
01791
01792
01793
01794
01795
01796
01797
01798
01799
01800
01801
01802
01803
01804
01805
01806
01807
01808
01809
01810
01811
01812
01813
01814
01815
01816
01817
01818
01819
01820
01821
01822
01823
01824
01825
01826
01827
01828
01829
01830
01831
01832
01833
01834
01835
01836
01837
01838
01839
01840
01841
01842
01843
01844
01845
01846
01847
01848
01849
01850
01851
01852
01853
01854
01855
01856
01857
01858
01859
01860
01861
01862
01863
01864
01865
01866
01867
01868
01869
01870
01871
01872
01873
01874
01875
01876
01877
01878
01879
01880
01881
01882
01883
01884
01885
01886
01887
01888
01889
01890
01891
01892
01893
01894
01895
01896
01897
01898
01899
01900
01901
01902
01903
01904
01905
01906
01907
01908 float Util::triquad(float R, float S, float T, float* fdata)
01909 {
01910
01911 const float C2 = 0.5f;
01912 const float C4 = 0.25f;
01913 const float C8 = 0.125f;
01914
01915 float RS = R * S;
01916 float ST = S * T;
01917 float RT = R * T;
01918 float RST = R * ST;
01919
01920 float RSQ = 1-R*R;
01921 float SSQ = 1-S*S;
01922 float TSQ = 1-T*T;
01923
01924 float RM1 = (1-R);
01925 float SM1 = (1-S);
01926 float TM1 = (1-T);
01927
01928 float RP1 = (1+R);
01929 float SP1 = (1+S);
01930 float TP1 = (1+T);
01931
01932 float triquad =
01933 (-C8) * RST * RM1 * SM1 * TM1 * fdata[0] +
01934 ( C4) * ST * RSQ * SM1 * TM1 * fdata[1] +
01935 ( C8) * RST * RP1 * SM1 * TM1 * fdata[2] +
01936 ( C4) * RT * RM1 * SSQ * TM1 * fdata[3] +
01937 (-C2) * T * RSQ * SSQ * TM1 * fdata[4] +
01938 (-C4) * RT * RP1 * SSQ * TM1 * fdata[5] +
01939 ( C8) * RST * RM1 * SP1 * TM1 * fdata[6] +
01940 (-C4) * ST * RSQ * SP1 * TM1 * fdata[7] +
01941 (-C8) * RST * RP1 * SP1 * TM1 * fdata[8] +
01942
01943 ( C4) * RS * RM1 * SM1 * TSQ * fdata[9] +
01944 (-C2) * S * RSQ * SM1 * TSQ * fdata[10] +
01945 (-C4) * RS * RP1 * SM1 * TSQ * fdata[11] +
01946 (-C2) * R * RM1 * SSQ * TSQ * fdata[12] +
01947 RSQ * SSQ * TSQ * fdata[13] +
01948 ( C2) * R * RP1 * SSQ * TSQ * fdata[14] +
01949 (-C4) * RS * RM1 * SP1 * TSQ * fdata[15] +
01950 ( C2) * S * RSQ * SP1 * TSQ * fdata[16] +
01951 ( C4) * RS * RP1 * SP1 * TSQ * fdata[17] +
01952
01953 ( C8) * RST * RM1 * SM1 * TP1 * fdata[18] +
01954 (-C4) * ST * RSQ * SM1 * TP1 * fdata[19] +
01955 (-C8) * RST * RP1 * SM1 * TP1 * fdata[20] +
01956 (-C4) * RT * RM1 * SSQ * TP1 * fdata[21] +
01957 ( C2) * T * RSQ * SSQ * TP1 * fdata[22] +
01958 ( C4) * RT * RP1 * SSQ * TP1 * fdata[23] +
01959 (-C8) * RST * RM1 * SP1 * TP1 * fdata[24] +
01960 ( C4) * ST * RSQ * SP1 * TP1 * fdata[25] +
01961 ( C8) * RST * RP1 * SP1 * TP1 * fdata[26] ;
01962 return triquad;
01963 }
01964
01965 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01966 : M(M_), fc(fc_), ntable(ntable_) {
01967
01968 build_sBtable();
01969 }
01970
01971 void Util::sincBlackman::build_sBtable() {
01972 sBtable.resize(ntable+1);
01973 int ltab = int(round(float(ntable)/1.25f));
01974 int M2 = M/2;
01975 fltb = float(ltab)/M2;
01976 for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01977 float x = 1.0e-7f;
01978 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)));
01979 for (int i=1; i <= ltab; i++) {
01980 x = float(i)/fltb;
01981 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)));
01982
01983 }
01984 }
01985
01986 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01987 int N_, float vtable_, int ntable_)
01988 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01989 ntable(ntable_) {
01990
01991 if (0.f == v) v = float(K)/2;
01992 if (0.f == vtable) vtable = v;
01993 alphar = alpha*r;
01994 fac = static_cast<float>(twopi)*alphar*v;
01995 vadjust = 1.0f*v;
01996 facadj = static_cast<float>(twopi)*alphar*vadjust;
01997 build_I0table();
01998 }
01999
02000 float Util::KaiserBessel::i0win(float x) const {
02001 float val0 = float(gsl_sf_bessel_I0(facadj));
02002 float absx = fabs(x);
02003 if (absx > vadjust) return 0.f;
02004 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02005 float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02006 return res;
02007 }
02008
02009 void Util::KaiserBessel::build_I0table() {
02010 i0table.resize(ntable+1);
02011 int ltab = int(round(float(ntable)/1.25f));
02012 fltb = float(ltab)/(K/2);
02013 float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02014 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02015 for (int i=0; i <= ltab; i++) {
02016 float s = float(i)/fltb/N;
02017 if (s < vadjust) {
02018 float rt = sqrt(1.f - pow(s/vadjust, 2));
02019 i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02020 } else {
02021 i0table[i] = 0.f;
02022 }
02023
02024 }
02025 }
02026
02027 float Util::KaiserBessel::I0table_maxerror() {
02028 float maxdiff = 0.f;
02029 for (int i = 1; i <= ntable; i++) {
02030 float diff = fabs(i0table[i] - i0table[i-1]);
02031 if (diff > maxdiff) maxdiff = diff;
02032 }
02033 return maxdiff;
02034 }
02035
02036 float Util::KaiserBessel::sinhwin(float x) const {
02037 float val0 = sinh(fac)/fac;
02038 float absx = fabs(x);
02039 if (0.0 == x) {
02040 float res = 1.0f;
02041 return res;
02042 } else if (absx == alphar) {
02043 return 1.0f/val0;
02044 } else if (absx < alphar) {
02045 float rt = sqrt(1.0f - pow((x/alphar), 2));
02046 float facrt = fac*rt;
02047 float res = (sinh(facrt)/facrt)/val0;
02048 return res;
02049 } else {
02050 float rt = sqrt(pow((x/alphar),2) - 1.f);
02051 float facrt = fac*rt;
02052 float res = (sin(facrt)/facrt)/val0;
02053 return res;
02054 }
02055 }
02056
02057 float Util::FakeKaiserBessel::i0win(float x) const {
02058 float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02059 float absx = fabs(x);
02060 if (absx > vadjust) return 0.f;
02061 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02062 float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02063 return res;
02064 }
02065
02066 void Util::FakeKaiserBessel::build_I0table() {
02067 i0table.resize(ntable+1);
02068 int ltab = int(round(float(ntable)/1.1f));
02069 fltb = float(ltab)/(K/2);
02070 float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02071 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02072 for (int i=0; i <= ltab; i++) {
02073 float s = float(i)/fltb/N;
02074 if (s < vadjust) {
02075 float rt = sqrt(1.f - pow(s/vadjust, 2));
02076 i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02077 } else {
02078 i0table[i] = 0.f;
02079 }
02080 }
02081 }
02082
02083 float Util::FakeKaiserBessel::sinhwin(float x) const {
02084 float val0 = sinh(fac)/fac;
02085 float absx = fabs(x);
02086 if (0.0 == x) {
02087 float res = 1.0f;
02088 return res;
02089 } else if (absx == alphar) {
02090 return 1.0f/val0;
02091 } else if (absx < alphar) {
02092 float rt = sqrt(1.0f - pow((x/alphar), 2));
02093 float facrt = fac*rt;
02094 float res = (sinh(facrt)/facrt)/val0;
02095 return res;
02096 } else {
02097 float rt = sqrt(pow((x/alphar),2) - 1.f);
02098 float facrt = fac*rt;
02099 float res = (sin(facrt)/facrt)/val0;
02100 return res;
02101 }
02102 }
02103
02104 #if 0 // 1-st order KB window
02105 float Util::FakeKaiserBessel::sinhwin(float x) const {
02106
02107 float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02108 float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02109 float absx = fabs(x);
02110 if (0.0 == x) {
02111
02112 float res = val0;
02113 return res;
02114 } else if (absx == alphar) {
02115
02116 return prefix;
02117 } else if (absx < alphar) {
02118 float rt = sqrt(1.0f - pow((x/alphar), 2));
02119
02120 float facrt = facadj*rt;
02121
02122 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02123 return res;
02124 } else {
02125 float rt = sqrt(pow((x/alphar),2) - 1.f);
02126
02127 float facrt = facadj*rt;
02128
02129 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02130 return res;
02131 }
02132 }
02133 #endif // 0
02134
02135
02136
02137 #define circ(i) circ[i-1]
02138 #define numr(i,j) numr[(j-1)*3 + i-1]
02139 #define xim(i,j) xim[(j-1)*nsam + i-1]
02140
02141 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02142 int nsam = image->get_xsize();
02143 int nrow = image->get_ysize();
02144 int nring = numr.size()/3;
02145 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02146 EMData* out = new EMData();
02147 out->set_size(lcirc,1,1);
02148 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02149 float *xim = image->get_data();
02150 float *circ = out->get_data();
02151
02152
02153
02154
02155
02156
02157
02158
02159
02160
02161
02162
02163
02164
02165
02166
02167 double dfi, dpi;
02168 int ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02169 float yq, xold, yold, fi, x, y;
02170
02171 ns2 = nsam/2+1;
02172 nr2 = nrow/2+1;
02173 dpi = 2.0*atan(1.0);
02174
02175 for (i=1;i<=nring;i++) {
02176
02177 inr = numr(1,i);
02178 yq = static_cast<float>(inr);
02179 l = numr(3,i);
02180 if (mode == 'h' || mode == 'H') lt = l/2;
02181 else lt = l/4;
02182
02183 nsim = lt-1;
02184 dfi = dpi/(nsim+1);
02185 kcirc = numr(2,i);
02186 xold = 0.0f;
02187 yold = static_cast<float>(inr);
02188 circ(kcirc) = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02189 xold = static_cast<float>(inr);
02190 yold = 0.0f;
02191 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02192
02193 if (mode == 'f' || mode == 'F') {
02194 xold = 0.0f;
02195 yold = static_cast<float>(-inr);
02196 circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02197 xold = static_cast<float>(-inr);
02198 yold = 0.0f;
02199 circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02200 }
02201
02202 for (j=1;j<=nsim;j++) {
02203 fi = static_cast<float>(dfi*j);
02204 x = sin(fi)*yq;
02205 y = cos(fi)*yq;
02206 xold = x;
02207 yold = y;
02208 circ(j+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02209 xold = y;
02210 yold = -x;
02211 circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02212
02213 if (mode == 'f' || mode == 'F') {
02214 xold = -x;
02215 yold = -y;
02216 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02217 xold = -y;
02218 yold = x;
02219 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02220 }
02221 }
02222 }
02223 return out;
02224 }
02225
02226 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02227 int nsam = image->get_xsize();
02228 int nrow = image->get_ysize();
02229 int nring = numr.size()/3;
02230 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02231 EMData* out = new EMData();
02232 out->set_size(lcirc,1,1);
02233 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02234 float *xim = image->get_data();
02235 float *circ = out->get_data();
02236 double dpi, dfi;
02237 int it, jt, inr, l, nsim, kcirc, lt;
02238 float xold, yold, fi, x, y;
02239
02240
02241
02242 dpi = 2*atan(1.0);
02243 for (it=1; it<=nring; it++) {
02244
02245 inr = numr(1,it);
02246
02247
02248
02249
02250 l = numr(3,it);
02251 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02252 else lt = l / 4;
02253
02254 nsim = lt - 1;
02255 dfi = dpi / (nsim+1);
02256 kcirc = numr(2,it);
02257 xold = 0.0f+cns2;
02258 yold = inr+cnr2;
02259
02260 Assert( kcirc <= lcirc );
02261 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02262
02263 xold = inr+cns2;
02264 yold = 0.0f+cnr2;
02265 Assert( lt+kcirc <= lcirc );
02266 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02267
02268 if ( mode == 'f' || mode == 'F' ) {
02269 xold = 0.0f+cns2;
02270 yold = -inr+cnr2;
02271 Assert( lt+lt+kcirc <= lcirc );
02272 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02273
02274 xold = -inr+cns2;
02275 yold = 0.0f+cnr2;
02276 Assert(lt+lt+lt+kcirc <= lcirc );
02277 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02278 }
02279
02280 for (jt=1; jt<=nsim; jt++) {
02281 fi = static_cast<float>(dfi * jt);
02282 x = sin(fi) * inr;
02283 y = cos(fi) * inr;
02284
02285 xold = x+cns2;
02286 yold = y+cnr2;
02287
02288 Assert( jt+kcirc <= lcirc );
02289 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02290
02291 xold = y+cns2;
02292 yold = -x+cnr2;
02293
02294 Assert( jt+lt+kcirc <= lcirc );
02295 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02296
02297 if ( mode == 'f' || mode == 'F' ) {
02298 xold = -x+cns2;
02299 yold = -y+cnr2;
02300
02301 Assert( jt+lt+lt+kcirc <= lcirc );
02302 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02303
02304 xold = -y+cns2;
02305 yold = x+cnr2;
02306
02307 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02308 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02309 }
02310 }
02311 }
02312 return out;
02313 }
02314
02315 float Util::bilinear(float xold, float yold, int nsam, int, float* xim)
02316 {
02317
02318
02319
02320
02321 float bilinear;
02322 int ixold, iyold;
02323
02324
02325
02326
02327
02328
02329
02330
02331
02332
02333
02334
02335
02336
02337
02338
02339
02340
02341
02342
02343
02344
02345
02346 float xdif, ydif;
02347
02348 ixold = (int) xold;
02349 iyold = (int) yold;
02350 ydif = yold - iyold;
02351
02352
02353
02354
02355
02356 xdif = xold - ixold;
02357 bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02358 xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02359 ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02360
02361 return bilinear;
02362 }
02363
02364 void Util::alrl_ms(float *xim, int nsam, int nrow, float cns2, float cnr2,
02365 int *numr, float *circ, int , int nring, char mode) {
02366 double dpi, dfi;
02367 int it, jt, inr, l, nsim, kcirc, lt;
02368 float xold, yold, fi, x, y;
02369
02370
02371
02372
02373 dpi = 2*atan(1.0);
02374 for (it=1; it<=nring; it++) {
02375
02376 inr = numr(1,it);
02377
02378 l = numr(3,it);
02379 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02380 else lt = l / 4;
02381
02382 nsim = lt - 1;
02383 dfi = dpi / (nsim+1);
02384 kcirc = numr(2,it);
02385
02386
02387 xold = 0.0f+cns2;
02388 yold = inr+cnr2;
02389
02390 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02391
02392 xold = inr+cns2;
02393 yold = 0.0f+cnr2;
02394 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02395
02396 if ( mode == 'f' || mode == 'F' ) {
02397 xold = 0.0f+cns2;
02398 yold = -inr+cnr2;
02399 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02400
02401 xold = -inr+cns2;
02402 yold = 0.0f+cnr2;
02403 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02404 }
02405
02406 for (jt=1; jt<=nsim; jt++) {
02407 fi = static_cast<float>(dfi * jt);
02408 x = sin(fi) * inr;
02409 y = cos(fi) * inr;
02410
02411 xold = x+cns2;
02412 yold = y+cnr2;
02413 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02414
02415 xold = y+cns2;
02416 yold = -x+cnr2;
02417 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02418
02419 if ( mode == 'f' || mode == 'F' ) {
02420 xold = -x+cns2;
02421 yold = -y+cnr2;
02422 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02423
02424 xold = -y+cns2;
02425 yold = x+cnr2;
02426 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02427 }
02428 }
02429 }
02430 }
02431
02432
02433
02434
02435
02436
02437
02438
02439
02440
02441
02442
02443
02444
02445
02446
02447
02448
02449
02450
02451
02452
02453
02454
02455
02456
02457
02458
02459
02460
02461
02462
02463
02464
02465
02466
02467
02468
02469
02470
02471
02472
02473
02474
02475
02476
02477
02478
02479
02480
02481
02482
02483
02484
02485
02486
02487
02488
02489
02490
02491
02492
02493
02494
02495
02496
02497
02498
02499
02500
02501
02502
02503
02504
02505
02506
02507 #undef xim
02508
02509 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02510
02511 int nring = numr.size()/3;
02512 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02513 EMData* out = new EMData();
02514 out->set_size(lcirc,1,1);
02515 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02516 float *circ = out->get_data();
02517 float *fimage = image->get_data();
02518 int nx = image->get_xsize();
02519 int ny = image->get_ysize();
02520 int nz = image->get_zsize();
02521 double dpi, dfi;
02522 int it, jt, inr, l, nsim, kcirc, lt;
02523 float yq, xold, yold, fi, x, y;
02524
02525
02526
02527
02528 dpi = 2*atan(1.0);
02529 for (it=1;it<=nring;it++) {
02530
02531 inr = numr(1,it);
02532 yq = static_cast<float>(inr);
02533
02534 l = numr(3,it);
02535 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02536 else lt = l / 4;
02537
02538 nsim = lt - 1;
02539 dfi = dpi / (nsim+1);
02540 kcirc = numr(2,it);
02541 xold = 0.0f;
02542 yold = static_cast<float>(inr);
02543 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02544
02545
02546 xold = static_cast<float>(inr);
02547 yold = 0.0f;
02548 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02549
02550
02551 if ( mode == 'f' || mode == 'F' ) {
02552 xold = 0.0f;
02553 yold = static_cast<float>(-inr);
02554 circ(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 xold = static_cast<float>(-inr);
02558 yold = 0.0f;
02559 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);
02560
02561 }
02562
02563 for (jt=1;jt<=nsim;jt++) {
02564 fi = static_cast<float>(dfi * jt);
02565 x = sin(fi) * yq;
02566 y = cos(fi) * yq;
02567
02568 xold = x;
02569 yold = y;
02570 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02571
02572
02573 xold = y;
02574 yold = -x;
02575 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02576
02577
02578 if ( mode == 'f' || mode == 'F' ) {
02579 xold = -x;
02580 yold = -y;
02581 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);
02582
02583
02584 xold = -y;
02585 yold = x;
02586 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);
02587
02588 }
02589 }
02590 }
02591 return out;
02592 }
02593
02594
02595
02596
02597
02598
02599
02600
02601
02602
02603
02604
02605
02606
02607
02608
02609
02610
02611
02612
02613
02614
02615
02616
02617 #define tab1(i) tab1[i-1]
02618 #define xcmplx(i,j) xcmplx [(j-1)*2 + i-1]
02619 #define br(i) br[i-1]
02620 #define bi(i) bi[i-1]
02621
02622 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02623 {
02624 double rni,sgn,tr1,tr2,ti1,ti2;
02625 double cc,c,ss,s,t,x2,x3,x4,x5;
02626 int b3,b4,b5,b6,b7,b56;
02627 int n, k, l, j, i, ix0, ix1, status=0;
02628
02629 const double tab1[] = {
02630 9.58737990959775e-5,
02631 1.91747597310703e-4,
02632 3.83495187571395e-4,
02633 7.66990318742704e-4,
02634 1.53398018628476e-3,
02635 3.06795676296598e-3,
02636 6.13588464915449e-3,
02637 1.22715382857199e-2,
02638 2.45412285229123e-2,
02639 4.90676743274181e-2,
02640 9.80171403295604e-2,
02641 1.95090322016128e-1,
02642 3.82683432365090e-1,
02643 7.07106781186546e-1,
02644 1.00000000000000,
02645 };
02646
02647 n=(int)pow(2.0f,ln);
02648
02649 k=abs(ks);
02650 l=16-ln;
02651 b3=n*k;
02652 b6=b3;
02653 b7=k;
02654 if (ks > 0) {
02655 sgn=1.0f;
02656 } else {
02657 sgn=-1.0f;
02658 rni=1.0f/(float)(n);
02659 j=1;
02660 for (i=1; i<=n; i++) {
02661 br(j)=br(j)*rni;
02662 bi(j)=bi(j)*rni;
02663 j=j+k;
02664 }
02665 }
02666
02667 L12:
02668 b6=b6/2;
02669 b5=b6;
02670 b4=2*b6;
02671 b56=b5-b6;
02672
02673 L14:
02674 tr1=br(b5+1);
02675 ti1=bi(b5+1);
02676 tr2=br(b56+1);
02677 ti2=bi(b56+1);
02678
02679 br(b5+1)=tr2-tr1;
02680 bi(b5+1)=ti2-ti1;
02681 br(b56+1)=tr1+tr2;
02682 bi(b56+1)=ti1+ti2;
02683
02684 b5=b5+b4;
02685 b56=b5-b6;
02686 if ( b5 <= b3 ) goto L14;
02687 if ( b6 == b7 ) goto L20;
02688
02689 b4=b7;
02690 cc=2.0f*pow(tab1(l),2);
02691 c=1.0f-cc;
02692 l++;
02693 ss=sgn*tab1(l);
02694 s=ss;
02695
02696 L16:
02697 b5=b6+b4;
02698 b4=2*b6;
02699 b56=b5-b6;
02700
02701 L18:
02702 tr1=br(b5+1);
02703 ti1=bi(b5+1);
02704 tr2=br(b56+1);
02705 ti2=bi(b56+1);
02706 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02707 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02708 br(b56+1)=tr1+tr2;
02709 bi(b56+1)=ti1+ti2;
02710
02711 b5=b5+b4;
02712 b56=b5-b6;
02713 if ( b5 <= b3 ) goto L18;
02714 b4=b5-b6;
02715 b5=b4-b3;
02716 c=-c;
02717 b4=b6-b5;
02718 if ( b5 < b4 ) goto L16;
02719 b4=b4+b7;
02720 if ( b4 >= b5 ) goto L12;
02721
02722 t=c-cc*c-ss*s;
02723 s=s+ss*c-cc*s;
02724 c=t;
02725 goto L16;
02726
02727 L20:
02728 ix0=b3/2;
02729 b3=b3-b7;
02730 b4=0;
02731 b5=0;
02732 b6=ix0;
02733 ix1=0;
02734 if (b6 == b7) goto EXIT;
02735
02736 L22:
02737 b4=b3-b4;
02738 b5=b3-b5;
02739 x2=br(b4+1);
02740 x3=br(b5+1);
02741 x4=bi(b4+1);
02742 x5=bi(b5+1);
02743 br(b4+1)=x3;
02744 br(b5+1)=x2;
02745 bi(b4+1)=x5;
02746 bi(b5+1)=x4;
02747 if(b6 < b4) goto L22;
02748
02749 L24:
02750 b4=b4+b7;
02751 b5=b6+b5;
02752 x2=br(b4+1);
02753 x3=br(b5+1);
02754 x4=bi(b4+1);
02755 x5=bi(b5+1);
02756 br(b4+1)=x3;
02757 br(b5+1)=x2;
02758 bi(b4+1)=x5;
02759 bi(b5+1)=x4;
02760 ix0=b6;
02761
02762 L26:
02763 ix0=ix0/2;
02764 ix1=ix1-ix0;
02765 if( ix1 >= 0) goto L26;
02766
02767 ix0=2*ix0;
02768 b4=b4+b7;
02769 ix1=ix1+ix0;
02770 b5=ix1;
02771 if ( b5 >= b4) goto L22;
02772 if ( b4 < b6) goto L24;
02773
02774 EXIT:
02775 status = 0;
02776 }
02777
02778
02779 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02780 {
02781
02782
02783 int b3,b4,b5,b6,b7,b56;
02784 int n, k, l, j, i, ix0, ix1;
02785 float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02786 int status=0;
02787
02788 const float tab1[] = {
02789 9.58737990959775e-5f,
02790 1.91747597310703e-4f,
02791 3.83495187571395e-4f,
02792 7.66990318742704e-4f,
02793 1.53398018628476e-3f,
02794 3.06795676296598e-3f,
02795 6.13588464915449e-3f,
02796 1.22715382857199e-2f,
02797 2.45412285229123e-2f,
02798 4.90676743274181e-2f,
02799 9.80171403295604e-2f,
02800 1.95090322016128e-1f,
02801 3.82683432365090e-1f,
02802 7.07106781186546e-1f,
02803 1.00000000000000f,
02804 };
02805
02806 n=(int)pow(2.0f,ln);
02807
02808 k=abs(ks);
02809 l=16-ln;
02810 b3=n*k;
02811 b6=b3;
02812 b7=k;
02813 if( ks > 0 ) {
02814 sgn=1.0f;
02815 } else {
02816 sgn=-1.0f;
02817 rni=1.0f/(float)n;
02818 j=1;
02819 for (i=1; i<=n; i++) {
02820 br(j)=br(j)*rni;
02821 bi(j)=bi(j)*rni;
02822 j=j+k;
02823 }
02824 }
02825 L12:
02826 b6=b6/2;
02827 b5=b6;
02828 b4=2*b6;
02829 b56=b5-b6;
02830 L14:
02831 tr1=br(b5+1);
02832 ti1=bi(b5+1);
02833
02834 tr2=br(b56+1);
02835 ti2=bi(b56+1);
02836
02837 br(b5+1)=tr2-tr1;
02838 bi(b5+1)=ti2-ti1;
02839 br(b56+1)=tr1+tr2;
02840 bi(b56+1)=ti1+ti2;
02841
02842 b5=b5+b4;
02843 b56=b5-b6;
02844 if ( b5 <= b3 ) goto L14;
02845 if ( b6 == b7 ) goto L20;
02846
02847 b4=b7;
02848 cc=2.0f*pow(tab1(l),2);
02849 c=1.0f-cc;
02850 l++;
02851 ss=sgn*tab1(l);
02852 s=ss;
02853 L16:
02854 b5=b6+b4;
02855 b4=2*b6;
02856 b56=b5-b6;
02857 L18:
02858 tr1=br(b5+1);
02859 ti1=bi(b5+1);
02860 tr2=br(b56+1);
02861 ti2=bi(b56+1);
02862 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02863 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02864 br(b56+1)=tr1+tr2;
02865 bi(b56+1)=ti1+ti2;
02866
02867 b5=b5+b4;
02868 b56=b5-b6;
02869 if(b5 <= b3) goto L18;
02870 b4=b5-b6;
02871 b5=b4-b3;
02872 c=-c;
02873 b4=b6-b5;
02874 if(b5 < b4) goto L16;
02875 b4=b4+b7;
02876 if(b4 >= b5) goto L12;
02877
02878 t=c-cc*c-ss*s;
02879 s=s+ss*c-cc*s;
02880 c=t;
02881 goto L16;
02882 L20:
02883 ix0=b3/2;
02884 b3=b3-b7;
02885 b4=0;
02886 b5=0;
02887 b6=ix0;
02888 ix1=0;
02889 if ( b6 == b7) goto EXIT;
02890 L22:
02891 b4=b3-b4;
02892 b5=b3-b5;
02893 x2=br(b4+1);
02894 x3=br(b5+1);
02895 x4=bi(b4+1);
02896 x5=bi(b5+1);
02897 br(b4+1)=x3;
02898 br(b5+1)=x2;
02899 bi(b4+1)=x5;
02900 bi(b5+1)=x4;
02901 if (b6 < b4) goto L22;
02902 L24:
02903 b4=b4+b7;
02904 b5=b6+b5;
02905 x2=br(b4+1);
02906 x3=br(b5+1);
02907 x4=bi(b4+1);
02908 x5=bi(b5+1);
02909 br(b4+1)=x3;
02910 br(b5+1)=x2;
02911 bi(b4+1)=x5;
02912 bi(b5+1)=x4;
02913 ix0=b6;
02914 L26:
02915 ix0=ix0/2;
02916 ix1=ix1-ix0;
02917 if(ix1 >= 0) goto L26;
02918
02919 ix0=2*ix0;
02920 b4=b4+b7;
02921 ix1=ix1+ix0;
02922 b5=ix1;
02923 if (b5 >= b4) goto L22;
02924 if (b4 < b6) goto L24;
02925 EXIT:
02926 status = 0;
02927 }
02928
02929 void Util::fftr_q(float *xcmplx, int nv)
02930 {
02931
02932
02933 int nu, inv, nu1, n, isub, n2, i1, i2, i;
02934 float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02935
02936 const float tab1[] = {
02937 9.58737990959775e-5f,
02938 1.91747597310703e-4f,
02939 3.83495187571395e-4f,
02940 7.66990318742704e-4f,
02941 1.53398018628476e-3f,
02942 3.06795676296598e-3f,
02943 6.13588464915449e-3f,
02944 1.22715382857199e-2f,
02945 2.45412285229123e-2f,
02946 4.90676743274181e-2f,
02947 9.80171403295604e-2f,
02948 1.95090322016128e-1f,
02949 3.82683432365090e-1f,
02950 7.07106781186546e-1f,
02951 1.00000000000000f,
02952 };
02953
02954 nu=abs(nv);
02955 inv=nv/nu;
02956 nu1=nu-1;
02957 n=(int)pow(2.f,nu1);
02958 isub=16-nu1;
02959
02960 ss=-tab1(isub);
02961 cc=-2.0f*pow(tab1(isub-1),2.f);
02962 c=1.0f;
02963 s=0.0f;
02964 n2=n/2;
02965 if ( inv > 0) {
02966 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02967 tr=xcmplx(1,1);
02968 ti=xcmplx(2,1);
02969 xcmplx(1,1)=tr+ti;
02970 xcmplx(2,1)=tr-ti;
02971 for (i=1;i<=n2;i++) {
02972 i1=i+1;
02973 i2=n-i+1;
02974 tr1=xcmplx(1,i1);
02975 tr2=xcmplx(1,i2);
02976 ti1=xcmplx(2,i1);
02977 ti2=xcmplx(2,i2);
02978 t=(cc*c-ss*s)+c;
02979 s=(cc*s+ss*c)+s;
02980 c=t;
02981 xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02982 xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02983 xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02984 xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02985 }
02986 } else {
02987 tr=xcmplx(1,1);
02988 ti=xcmplx(2,1);
02989 xcmplx(1,1)=0.5f*(tr+ti);
02990 xcmplx(2,1)=0.5f*(tr-ti);
02991 for (i=1; i<=n2; i++) {
02992 i1=i+1;
02993 i2=n-i+1;
02994 tr1=xcmplx(1,i1);
02995 tr2=xcmplx(1,i2);
02996 ti1=xcmplx(2,i1);
02997 ti2=xcmplx(2,i2);
02998 t=(cc*c-ss*s)+c;
02999 s=(cc*s+ss*c)+s;
03000 c=t;
03001 xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03002 xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03003 xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03004 xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03005 }
03006 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03007 }
03008 }
03009
03010
03011 void Util::fftr_d(double *xcmplx, int nv)
03012 {
03013
03014 int i1, i2, nu, inv, nu1, n, isub, n2, i;
03015 double tr1,tr2,ti1,ti2,tr,ti;
03016 double cc,c,ss,s,t;
03017 const double tab1[] = {
03018 9.58737990959775e-5,
03019 1.91747597310703e-4,
03020 3.83495187571395e-4,
03021 7.66990318742704e-4,
03022 1.53398018628476e-3,
03023 3.06795676296598e-3,
03024 6.13588464915449e-3,
03025 1.22715382857199e-2,
03026 2.45412285229123e-2,
03027 4.90676743274181e-2,
03028 9.80171403295604e-2,
03029 1.95090322016128e-1,
03030 3.82683432365090e-1,
03031 7.07106781186546e-1,
03032 1.00000000000000,
03033 };
03034
03035 nu=abs(nv);
03036 inv=nv/nu;
03037 nu1=nu-1;
03038 n=(int)pow(2.0f,nu1);
03039 isub=16-nu1;
03040 ss=-tab1(isub);
03041 cc=-2.0*pow(tab1(isub-1),2);
03042 c=1.0f;
03043 s=0.0f;
03044 n2=n/2;
03045
03046 if ( inv > 0 ) {
03047 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03048 tr=xcmplx(1,1);
03049 ti=xcmplx(2,1);
03050 xcmplx(1,1)=tr+ti;
03051 xcmplx(2,1)=tr-ti;
03052 for (i=1;i<=n2;i++) {
03053 i1=i+1;
03054 i2=n-i+1;
03055 tr1=xcmplx(1,i1);
03056 tr2=xcmplx(1,i2);
03057 ti1=xcmplx(2,i1);
03058 ti2=xcmplx(2,i2);
03059 t=(cc*c-ss*s)+c;
03060 s=(cc*s+ss*c)+s;
03061 c=t;
03062 xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03063 xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03064 xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03065 xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03066 }
03067 } else {
03068 tr=xcmplx(1,1);
03069 ti=xcmplx(2,1);
03070 xcmplx(1,1)=0.5*(tr+ti);
03071 xcmplx(2,1)=0.5*(tr-ti);
03072 for (i=1; i<=n2; i++) {
03073 i1=i+1;
03074 i2=n-i+1;
03075 tr1=xcmplx(1,i1);
03076 tr2=xcmplx(1,i2);
03077 ti1=xcmplx(2,i1);
03078 ti2=xcmplx(2,i2);
03079 t=(cc*c-ss*s)+c;
03080 s=(cc*s+ss*c)+s;
03081 c=t;
03082 xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03083 xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03084 xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03085 xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03086 }
03087 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03088 }
03089 }
03090 #undef tab1
03091 #undef xcmplx
03092 #undef br
03093 #undef bi
03094
03095
03096 void Util::Frngs(EMData* circp, vector<int> numr){
03097 int nring = numr.size()/3;
03098 float *circ = circp->get_data();
03099 int i, l;
03100 for (i=1; i<=nring;i++) {
03101
03102 #ifdef _WIN32
03103 l = (int)( log((float)numr(3,i))/log(2.0f) );
03104 #else
03105 l=(int)(log2(numr(3,i)));
03106 #endif //_WIN32
03107
03108 fftr_q(&circ(numr(2,i)),l);
03109 }
03110 }
03111
03112 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03113 int nring = numr.size()/3;
03114 float *circ = circp->get_data();
03115 int i, l;
03116 for (i=1; i<=nring;i++) {
03117
03118 #ifdef _WIN32
03119 l = (int)( log((float)numr(3,i))/log(2.0f) );
03120 #else
03121 l=(int)(log2(numr(3,i)));
03122 #endif //_WIN32
03123
03124 fftr_q(&circ(numr(2,i)),-l);
03125 }
03126 }
03127 #undef circ
03128
03129 #define b(i) b[i-1]
03130 void Util::prb1d(double *b, int npoint, float *pos) {
03131 double c2,c3;
03132 int nhalf;
03133
03134 nhalf = npoint/2 + 1;
03135 *pos = 0.0;
03136
03137 if (npoint == 7) {
03138 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03139 - 6.*b(6) + 31.*b(7);
03140 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03141 }
03142 else if (npoint == 5) {
03143 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03144 + 46.*b(5) ) / (-70.);
03145 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03146 }
03147 else if (npoint == 3) {
03148 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03149 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03150 }
03151
03152 else {
03153 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03154 - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03155 + 1092.*b(9) ) / (-4620.);
03156 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03157 - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03158 }
03159 if (c3 != 0.0) *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03160 }
03161 #undef b
03162
03163 #define circ1(i) circ1[i-1]
03164 #define circ2(i) circ2[i-1]
03165 #define t(i) t[i-1]
03166 #define q(i) q[i-1]
03167 #define b(i) b[i-1]
03168 #define t7(i) t7[i-1]
03169 Dict Util::Crosrng_e(EMData* circ1p, EMData* circ2p, vector<int> numr, int neg) {
03170
03171 int nring = numr.size()/3;
03172
03173 int maxrin = numr[numr.size()-1];
03174 double qn; float tot;
03175 float *circ1 = circ1p->get_data();
03176 float *circ2 = circ2p->get_data();
03177
03178
03179
03180
03181
03182
03183
03184
03185
03186
03187
03188 float *t;
03189 double t7[7], *q;
03190 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03191 float pos;
03192
03193 #ifdef _WIN32
03194 ip = -(int)(log((float)maxrin)/log(2.0f));
03195 #else
03196 ip = -(int) (log2(maxrin));
03197 #endif //_WIN32
03198
03199 q = (double*)calloc(maxrin, sizeof(double));
03200 t = (float*)calloc(maxrin, sizeof(float));
03201
03202
03203 for (i=1; i<=nring; i++) {
03204 numr3i = numr(3,i);
03205 numr2i = numr(2,i);
03206
03207 t(1) = (circ1(numr2i)) * circ2(numr2i);
03208
03209 if (numr3i != maxrin) {
03210
03211 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03212 t(2) = 0.0;
03213
03214 if (neg) {
03215
03216 for (j=3;j<=numr3i;j=j+2) {
03217 jc = j+numr2i-1;
03218 t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03219 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03220 }
03221 } else {
03222 for (j=3;j<=numr3i;j=j+2) {
03223 jc = j+numr2i-1;
03224 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03225 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03226 }
03227 }
03228 for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03229 } else {
03230 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03231 if (neg) {
03232
03233 for (j=3;j<=maxrin;j=j+2) {
03234 jc = j+numr2i-1;
03235 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03236 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03237 }
03238 } else {
03239 for (j=3;j<=maxrin;j=j+2) {
03240 jc = j+numr2i-1;
03241 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03242 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03243 }
03244 }
03245 for (j = 1; j <= maxrin; j++) q(j) += t(j);
03246 }
03247 }
03248
03249 fftr_d(q,ip);
03250
03251 qn = -1.0e20;
03252 for (j=1;j<=maxrin;j++) {
03253 if (q(j) >= qn) {
03254 qn = q(j); jtot = j;
03255 }
03256 }
03257
03258 for (k=-3; k<=3; k++) {
03259 j = (jtot+k+maxrin-1)%maxrin + 1;
03260 t7(k+4) = q(j);
03261 }
03262
03263 prb1d(t7,7,&pos);
03264
03265 tot = (float)jtot + pos;
03266
03267 if (q) free(q);
03268 if (t) free(t);
03269
03270 Dict retvals;
03271 retvals["qn"] = qn;
03272 retvals["tot"] = tot;
03273 return retvals;
03274 }
03275
03276 Dict Util::Crosrng_ew(EMData* circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03277
03278 int nring = numr.size()/3;
03279
03280 int maxrin = numr[numr.size()-1];
03281 double qn; float tot;
03282 float *circ1 = circ1p->get_data();
03283 float *circ2 = circ2p->get_data();
03284
03285
03286
03287
03288
03289
03290
03291
03292
03293
03294
03295 float *t;
03296 double t7[7], *q;
03297 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03298 float pos;
03299
03300 #ifdef _WIN32
03301 ip = -(int)(log((float)maxrin)/log(2.0f));
03302 #else
03303 ip = -(int) (log2(maxrin));
03304 #endif //_WIN32
03305
03306 q = (double*)calloc(maxrin, sizeof(double));
03307 t = (float*)calloc(maxrin, sizeof(float));
03308
03309
03310 for (i=1;i<=nring;i++) {
03311 numr3i = numr(3,i);
03312 numr2i = numr(2,i);
03313
03314 t(1) = circ1(numr2i) * circ2(numr2i);
03315
03316 if (numr3i != maxrin) {
03317
03318 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03319 t(2) = 0.0;
03320
03321 if (neg) {
03322
03323 for (j=3; j<=numr3i; j=j+2) {
03324 jc = j+numr2i-1;
03325 t(j) = (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03326 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03327 }
03328 } else {
03329 for (j=3; j<=numr3i; j=j+2) {
03330 jc = j+numr2i-1;
03331 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03332 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03333 }
03334 }
03335 for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03336 } else {
03337 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03338 if (neg) {
03339
03340 for (j=3; j<=maxrin; j=j+2) {
03341 jc = j+numr2i-1;
03342 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03343 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03344 }
03345 } else {
03346 for (j=3; j<=maxrin; j=j+2) {
03347 jc = j+numr2i-1;
03348 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03349 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03350 }
03351 }
03352 for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03353 }
03354 }
03355
03356 fftr_d(q,ip);
03357
03358 qn = -1.0e20;
03359 for (j=1;j<=maxrin;j++) {
03360
03361 if (q(j) >= qn) {
03362 qn = q(j);
03363 jtot = j;
03364 }
03365 }
03366
03367 for (k=-3; k<=3; k++) {
03368 j = (jtot+k+maxrin-1)%maxrin + 1;
03369 t7(k+4) = q(j);
03370 }
03371
03372 prb1d(t7,7,&pos);
03373
03374 tot = (float)jtot + pos;
03375
03376
03377 if (t) free(t);
03378
03379 Dict retvals;
03380
03381
03382 retvals["qn"] = qn;
03383 retvals["tot"] = tot;
03384
03385 if (q) free(q);
03386
03387 return retvals;
03388 }
03389
03390 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03391 int nring = numr.size()/3;
03392
03393 int maxrin = numr[numr.size()-1];
03394 double qn; float tot; double qm; float tmt;
03395 float *circ1 = circ1p->get_data();
03396 float *circ2 = circ2p->get_data();
03397
03398
03399
03400
03401
03402
03403
03404
03405
03406
03407
03408
03409 double *t, *q, t7[7];
03410
03411 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03412 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03413
03414 qn = 0.0f;
03415 qm = 0.0f;
03416 tot = 0.0f;
03417 tmt = 0.0f;
03418 #ifdef _WIN32
03419 ip = -(int)(log((float)maxrin)/log(2.0f));
03420 #else
03421 ip = -(int)(log2(maxrin));
03422 #endif //_WIN32
03423
03424
03425
03426
03427
03428 q = (double*)calloc(maxrin,sizeof(double));
03429
03430
03431
03432 t = (double*)calloc(maxrin,sizeof(double));
03433
03434
03435 for (i=1; i<=nring; i++) {
03436
03437 numr3i = numr(3,i);
03438 numr2i = numr(2,i);
03439
03440 t1 = circ1(numr2i) * circ2(numr2i);
03441 q(1) += t1;
03442 t(1) += t1;
03443
03444 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03445 if (numr3i == maxrin) {
03446 q(2) += t1;
03447 t(2) += t1;
03448 } else {
03449 q(numr3i+1) += t1;
03450 t(numr3i+1) += t1;
03451 }
03452
03453 for (j=3; j<=numr3i; j += 2) {
03454 jc = j+numr2i-1;
03455
03456
03457
03458
03459
03460
03461
03462
03463 c1 = circ1(jc);
03464 c2 = circ1(jc+1);
03465 d1 = circ2(jc);
03466 d2 = circ2(jc+1);
03467
03468 t1 = c1 * d1;
03469 t2 = c2 * d2;
03470 t3 = c1 * d2;
03471 t4 = c2 * d1;
03472
03473 q(j) += t1 + t2;
03474 q(j+1) += -t3 + t4;
03475 t(j) += t1 - t2;
03476 t(j+1) += -t3 - t4;
03477 }
03478 }
03479
03480 fftr_d(q,ip);
03481
03482 qn = -1.0e20;
03483 for (j=1; j<=maxrin; j++) {
03484 if (q(j) >= qn) {
03485 qn = q(j);
03486 jtot = j;
03487 }
03488 }
03489
03490 for (k=-3; k<=3; k++) {
03491 j = ((jtot+k+maxrin-1)%maxrin)+1;
03492 t7(k+4) = q(j);
03493 }
03494
03495
03496 prb1d(t7,7,&pos);
03497 tot = (float)(jtot)+pos;
03498
03499
03500
03501
03502 fftr_d(t,ip);
03503
03504
03505 qm = -1.0e20;
03506 for (j=1; j<=maxrin;j++) {
03507 if ( t(j) >= qm ) {
03508 qm = t(j);
03509 jtot = j;
03510 }
03511 }
03512
03513 for (k=-3; k<=3; k++) {
03514 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03515 t7(k+4) = t(j);
03516 }
03517
03518
03519
03520 prb1d(t7,7,&pos);
03521 tmt = float(jtot) + pos;
03522
03523
03524
03525 free(t);
03526 free(q);
03527
03528 Dict retvals;
03529 retvals["qn"] = qn;
03530 retvals["tot"] = tot;
03531 retvals["qm"] = qm;
03532 retvals["tmt"] = tmt;
03533 return retvals;
03534 }
03535
03536 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03537 int nring = numr.size()/3;
03538
03539 int maxrin = numr[numr.size()-1];
03540 double qn; float tot; double qm; float tmt;
03541 float *circ1 = circ1p->get_data();
03542 float *circ2 = circ2p->get_data();
03543
03544
03545
03546
03547
03548
03549
03550
03551
03552
03553
03554
03555 double *t, *q;
03556
03557 int ip, jc, numr3i, numr2i, i, j, jtot = 0;
03558 float t1, t2, t3, t4, c1, c2, d1, d2;
03559
03560 qn = 0.0f;
03561 qm = 0.0f;
03562 tot = 0.0f;
03563 tmt = 0.0f;
03564 #ifdef _WIN32
03565 ip = -(int)(log((float)maxrin)/log(2.0f));
03566 #else
03567 ip = -(int)(log2(maxrin));
03568 #endif //_WIN32
03569
03570
03571
03572
03573
03574 q = (double*)calloc(maxrin,sizeof(double));
03575
03576
03577
03578 t = (double*)calloc(maxrin,sizeof(double));
03579
03580
03581 for (i=1; i<=nring; i++) {
03582
03583 numr3i = numr(3,i);
03584 numr2i = numr(2,i);
03585
03586 t1 = circ1(numr2i) * circ2(numr2i);
03587 q(1) += t1;
03588 t(1) += t1;
03589
03590 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03591 if (numr3i == maxrin) {
03592 q(2) += t1;
03593 t(2) += t1;
03594 } else {
03595 q(numr3i+1) += t1;
03596 t(numr3i+1) += t1;
03597 }
03598
03599 for (j=3; j<=numr3i; j += 2) {
03600 jc = j+numr2i-1;
03601
03602
03603
03604
03605
03606
03607
03608
03609 c1 = circ1(jc);
03610 c2 = circ1(jc+1);
03611 d1 = circ2(jc);
03612 d2 = circ2(jc+1);
03613
03614 t1 = c1 * d1;
03615 t2 = c2 * d2;
03616 t3 = c1 * d2;
03617 t4 = c2 * d1;
03618
03619 q(j) += t1 + t2;
03620 q(j+1) += -t3 + t4;
03621 t(j) += t1 - t2;
03622 t(j+1) += -t3 - t4;
03623 }
03624 }
03625
03626 fftr_d(q,ip);
03627
03628 qn = -1.0e20;
03629
03630 int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03631 int jstep = static_cast<int>(delta/360.0*maxrin);
03632 if (jstep < 1) { jstep = 1; }
03633
03634 for (j=jstart; j<=maxrin; j+=jstep) {
03635 if (q(j) >= qn) {
03636 qn = q(j);
03637 jtot = j;
03638 }
03639 }
03640
03641
03642
03643
03644
03645
03646
03647
03648
03649
03650 tot = (float)(jtot);
03651
03652
03653 fftr_d(t,ip);
03654
03655
03656 qm = -1.0e20;
03657 for (j=jstart; j<=maxrin;j+=jstep) {
03658 if ( t(j) >= qm ) {
03659 qm = t(j);
03660 jtot = j;
03661 }
03662 }
03663
03664
03665
03666
03667
03668
03669
03670
03671
03672
03673
03674 tmt = float(jtot);
03675
03676 free(t);
03677 free(q);
03678
03679 Dict retvals;
03680 retvals["qn"] = qn;
03681 retvals["tot"] = tot;
03682 retvals["qm"] = qm;
03683 retvals["tmt"] = tmt;
03684 return retvals;
03685 }
03686
03687
03688 Dict Util::Crosrng_psi_0_180(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03689 int nring = numr.size()/3;
03690
03691 int maxrin = numr[numr.size()-1];
03692 double qn; float tot; double qm; float tmt;
03693 float *circ1 = circ1p->get_data();
03694 float *circ2 = circ2p->get_data();
03695
03696
03697
03698
03699 double *t, *q, t7[7];
03700
03701 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03702 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03703
03704 qn = 0.0f;
03705 qm = 0.0f;
03706 tot = 0.0f;
03707 tmt = 0.0f;
03708 #ifdef _WIN32
03709 ip = -(int)(log((float)maxrin)/log(2.0f));
03710 #else
03711 ip = -(int)(log2(maxrin));
03712 #endif //_WIN32
03713
03714
03715
03716
03717
03718 q = (double*)calloc(maxrin,sizeof(double));
03719
03720
03721
03722 t = (double*)calloc(maxrin,sizeof(double));
03723
03724
03725 for (i=1; i<=nring; i++) {
03726
03727 numr3i = numr(3,i);
03728 numr2i = numr(2,i);
03729
03730 t1 = circ1(numr2i) * circ2(numr2i);
03731 q(1) += t1;
03732 t(1) += t1;
03733
03734 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03735 if (numr3i == maxrin) {
03736 q(2) += t1;
03737 t(2) += t1;
03738 } else {
03739 q(numr3i+1) += t1;
03740 t(numr3i+1) += t1;
03741 }
03742
03743 for (j=3; j<=numr3i; j += 2) {
03744 jc = j+numr2i-1;
03745
03746
03747
03748
03749
03750
03751
03752
03753 c1 = circ1(jc);
03754 c2 = circ1(jc+1);
03755 d1 = circ2(jc);
03756 d2 = circ2(jc+1);
03757
03758 t1 = c1 * d1;
03759 t2 = c2 * d2;
03760 t3 = c1 * d2;
03761 t4 = c2 * d1;
03762
03763 q(j) += t1 + t2;
03764 q(j+1) += -t3 + t4;
03765 t(j) += t1 - t2;
03766 t(j+1) += -t3 - t4;
03767 }
03768 }
03769
03770 fftr_d(q,ip);
03771
03772 int psi_range = int(psi_max/360.0*maxrin+0.5);
03773 const int psi_0 = 0;
03774 int psi_180 = int( 180.0/360.0*maxrin+0.5);
03775
03776 qn = -1.0e20;
03777 for (k=-psi_range; k<=psi_range; k++) {
03778 j = (k+psi_0+maxrin-1)%maxrin+1;
03779 if (q(j) >= qn) {
03780 qn = q(j);
03781 jtot = j;
03782 }
03783 }
03784
03785 for (k=-psi_range; k<=psi_range; k++) {
03786 j = (k+psi_180+maxrin-1)%maxrin+1;
03787 if (q(j) >= qn) {
03788 qn = q(j);
03789 jtot = j;
03790 }
03791 }
03792
03793 for (k=-3; k<=3; k++) {
03794 j = ((jtot+k+maxrin-1)%maxrin)+1;
03795 t7(k+4) = q(j);
03796 }
03797
03798
03799 prb1d(t7,7,&pos);
03800 tot = (float)(jtot)+pos;
03801
03802
03803
03804
03805 fftr_d(t,ip);
03806
03807
03808 qm = -1.0e20;
03809 for (k=-psi_range; k<=psi_range; k++) {
03810 j = (k+psi_0+maxrin-1)%maxrin+1;
03811 if (t(j) >= qm) {
03812 qm = t(j);
03813 jtot = j;
03814 }
03815 }
03816
03817 for (k=-psi_range; k<=psi_range; k++) {
03818 j = (k+psi_180+maxrin-1)%maxrin+1;
03819 if (t(j) >= qm) {
03820 qm = t(j);
03821 jtot = j;
03822 }
03823 }
03824
03825 for (k=-3; k<=3; k++) {
03826 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03827 t7(k+4) = t(j);
03828 }
03829
03830
03831
03832 prb1d(t7,7,&pos);
03833 tmt = float(jtot) + pos;
03834
03835
03836
03837 free(t);
03838 free(q);
03839
03840 Dict retvals;
03841 retvals["qn"] = qn;
03842 retvals["tot"] = tot;
03843 retvals["qm"] = qm;
03844 retvals["tmt"] = tmt;
03845 return retvals;
03846 }
03847
03848 Dict Util::Crosrng_psi_0_180_no_mirror(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03849 int nring = numr.size()/3;
03850
03851 int maxrin = numr[numr.size()-1];
03852 double qn; float tot;
03853 float *circ1 = circ1p->get_data();
03854 float *circ2 = circ2p->get_data();
03855
03856
03857
03858
03859 double *q, t7[7];
03860
03861 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03862 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03863
03864 qn = 0.0f;
03865 tot = 0.0f;
03866 #ifdef _WIN32
03867 ip = -(int)(log((float)maxrin)/log(2.0f));
03868 #else
03869 ip = -(int)(log2(maxrin));
03870 #endif //_WIN32
03871
03872
03873
03874
03875
03876 q = (double*)calloc(maxrin,sizeof(double));
03877
03878
03879 for (i=1; i<=nring; i++) {
03880
03881 numr3i = numr(3,i);
03882 numr2i = numr(2,i);
03883
03884 t1 = circ1(numr2i) * circ2(numr2i);
03885 q(1) += t1;
03886
03887
03888 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03889 if (numr3i == maxrin) {
03890 q(2) += t1;
03891
03892 } else {
03893 q(numr3i+1) += t1;
03894 }
03895
03896 for (j=3; j<=numr3i; j += 2) {
03897 jc = j+numr2i-1;
03898
03899
03900
03901
03902
03903
03904
03905
03906 c1 = circ1(jc);
03907 c2 = circ1(jc+1);
03908 d1 = circ2(jc);
03909 d2 = circ2(jc+1);
03910
03911 t1 = c1 * d1;
03912 t2 = c2 * d2;
03913 t3 = c1 * d2;
03914 t4 = c2 * d1;
03915
03916 q(j) += t1 + t2;
03917 q(j+1) += -t3 + t4;
03918
03919 }
03920 }
03921
03922 fftr_d(q,ip);
03923
03924 int psi_range = int(psi_max/360.0*maxrin+0.5);
03925 const int psi_0 = 0;
03926 int psi_180 = int( 180.0/360.0*maxrin+0.5);
03927
03928 qn = -1.0e20;
03929 for (k=-psi_range; k<=psi_range; k++) {
03930 j = (k+psi_0+maxrin-1)%maxrin+1;
03931 if (q(j) >= qn) {
03932 qn = q(j);
03933 jtot = j;
03934 }
03935 }
03936
03937 for (k=-psi_range; k<=psi_range; k++) {
03938 j = (k+psi_180+maxrin-1)%maxrin+1;
03939 if (q(j) >= qn) {
03940 qn = q(j);
03941 jtot = j;
03942 }
03943 }
03944
03945 for (k=-3; k<=3; k++) {
03946 j = ((jtot+k+maxrin-1)%maxrin)+1;
03947 t7(k+4) = q(j);
03948 }
03949
03950
03951 prb1d(t7,7,&pos);
03952 tot = (float)(jtot)+pos;
03953
03954
03955
03956 free(q);
03957
03958 Dict retvals;
03959 retvals["qn"] = qn;
03960 retvals["tot"] = tot;
03961
03962 return retvals;
03963 }
03964
03965
03966
03967 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag) {
03968
03969
03970 int nring = numr.size()/3;
03971 int maxrin = numr[numr.size()-1];
03972 double qn; float tot; double qm; float tmt;
03973 float *circ1 = circ1p->get_data();
03974 float *circ2 = circ2p->get_data();
03975
03976 double *q, t7[7];
03977
03978 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03979 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03980
03981 qn = 0.0f;
03982 qm = 0.0f;
03983 tot = 0.0f;
03984 tmt = 0.0f;
03985 #ifdef _WIN32
03986 ip = -(int)(log((float)maxrin)/log(2.0f));
03987 #else
03988 ip = -(int)(log2(maxrin));
03989 #endif //_WIN32
03990
03991
03992
03993
03994 q = (double*)calloc(maxrin,sizeof(double));
03995
03996
03997 if (flag==0) {
03998 for (i=1; i<=nring; i++) {
03999
04000 numr3i = numr(3,i);
04001 numr2i = numr(2,i);
04002
04003 t1 = circ1(numr2i) * circ2(numr2i);
04004 q(1) += t1;
04005
04006 t1 = circ1(numr2i+1) * circ2(numr2i+1);
04007 if (numr3i == maxrin) {
04008 q(2) += t1;
04009 } else {
04010 q(numr3i+1) += t1;
04011 }
04012
04013 for (j=3; j<=numr3i; j += 2) {
04014 jc = j+numr2i-1;
04015
04016
04017
04018
04019
04020 c1 = circ1(jc);
04021 c2 = circ1(jc+1);
04022 d1 = circ2(jc);
04023 d2 = circ2(jc+1);
04024
04025 t1 = c1 * d1;
04026 t3 = c1 * d2;
04027 t2 = c2 * d2;
04028 t4 = c2 * d1;
04029
04030 q(j) += t1 + t2;
04031 q(j+1) += -t3 + t4;
04032 }
04033 }
04034 } else {
04035 for (i=1; i<=nring; i++) {
04036
04037 numr3i = numr(3,i);
04038 numr2i = numr(2,i);
04039
04040 t1 = circ1(numr2i) * circ2(numr2i);
04041 q(1) += t1;
04042
04043 t1 = circ1(numr2i+1) * circ2(numr2i+1);
04044 if (numr3i == maxrin) {
04045 q(2) += t1;
04046 } else {
04047 q(numr3i+1) += t1;
04048 }
04049
04050 for (j=3; j<=numr3i; j += 2) {
04051 jc = j+numr2i-1;
04052
04053
04054
04055
04056
04057 c1 = circ1(jc);
04058 c2 = circ1(jc+1);
04059 d1 = circ2(jc);
04060 d2 = circ2(jc+1);
04061
04062 t1 = c1 * d1;
04063 t3 = c1 * d2;
04064 t2 = c2 * d2;
04065 t4 = c2 * d1;
04066
04067 q(j) += t1 - t2;
04068 q(j+1) += -t3 - t4;
04069 }
04070 }
04071 }
04072 fftr_d(q,ip);
04073
04074 qn = -1.0e20;
04075 int psi_pos = int(psi/360.0*maxrin+0.5);
04076
04077 for (k=-5; k<=5; k++) {
04078 j = (psi_pos+maxrin-1)%maxrin+1;
04079 if (q(j) >= qn) {
04080 qn = q(j);
04081 jtot = j;
04082 }
04083 }
04084
04085 for (k=-3; k<=3; k++) {
04086 j = ((jtot+k+maxrin-1)%maxrin)+1;
04087 t7(k+4) = q(j);
04088 }
04089
04090
04091 prb1d(t7,7,&pos);
04092 tot = (float)(jtot)+pos;
04093 free(q);
04094
04095 Dict retvals;
04096 retvals["qn"] = qn;
04097 retvals["tot"] = tot;
04098 return retvals;
04099 }
04100
04101 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
04102 int nring = numr.size()/3;
04103
04104 int maxrin = numr[numr.size()-1];
04105 double qn; float tot;
04106 float *circ1 = circ1p->get_data();
04107 float *circ2 = circ2p->get_data();
04108
04109
04110
04111
04112
04113
04114
04115
04116
04117
04118
04119
04120 double *q, t7[7];
04121
04122 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04123 float c1, c2, d1, d2, pos;
04124
04125 qn = 0.0;
04126 tot = 0.0;
04127 #ifdef _WIN32
04128 ip = -(int)(log((float)maxrin)/log(2.0f));
04129 #else
04130 ip = -(int)(log2(maxrin));
04131 #endif //_WIN32
04132
04133
04134
04135
04136
04137 q = (double*)calloc(maxrin,sizeof(double));
04138
04139
04140 for (i=1; i<=nring; i++) {
04141
04142 numr3i = numr(3,i);
04143 numr2i = numr(2,i);
04144
04145 q(1) += circ1(numr2i) * circ2(numr2i);
04146
04147 if (numr3i == maxrin) q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04148 else q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04149
04150 for (j=3; j<=numr3i; j += 2) {
04151 jc = j+numr2i-1;
04152
04153
04154
04155
04156
04157 c1 = circ1(jc);
04158 c2 = circ1(jc+1);
04159 d1 = circ2(jc);
04160 d2 = circ2(jc+1);
04161
04162 q(j) += c1 * d1 + c2 * d2;
04163 q(j+1) += -c1 * d2 + c2 * d1;
04164 }
04165 }
04166
04167 fftr_d(q,ip);
04168
04169 qn = -1.0e20;
04170 for (j=1; j<=maxrin; j++) {
04171 if (q(j) >= qn) {
04172 qn = q(j);
04173 jtot = j;
04174 }
04175 }
04176
04177 for (k=-3; k<=3; k++) {
04178 j = ((jtot+k+maxrin-1)%maxrin)+1;
04179 t7(k+4) = q(j);
04180 }
04181
04182
04183 prb1d(t7,7,&pos);
04184 tot = (float)(jtot)+pos;
04185
04186
04187
04188 free(q);
04189
04190 Dict retvals;
04191 retvals["qn"] = qn;
04192 retvals["tot"] = tot;
04193 return retvals;
04194 }
04195
04196 #define dout(i,j) dout[i+maxrin*j]
04197 #define circ1b(i) circ1b[i-1]
04198 #define circ2b(i) circ2b[i-1]
04199
04200 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04201
04202
04203
04204 int ip, jc, numr3i, numr2i, i, j;
04205 float t1, t2, t3, t4, c1, c2, d1, d2;
04206
04207 int nring = numr.size()/3;
04208
04209 int maxrin = numr[numr.size()-1];
04210
04211 float* circ1b = circ1->get_data();
04212 float* circ2b = circ2->get_data();
04213
04214
04215 double *t, *q;
04216
04217 q = (double*)calloc(maxrin,sizeof(double));
04218 t = (double*)calloc(maxrin,sizeof(double));
04219
04220 #ifdef _WIN32
04221 ip = -(int)(log((float)maxrin)/log(2.0f));
04222 #else
04223 ip = -(int)(log2(maxrin));
04224 #endif //_WIN32
04225
04226
04227
04228
04229
04230
04231
04232 for (i=1; i<=nring; i++) {
04233
04234 numr3i = numr(3,i);
04235 numr2i = numr(2,i);
04236
04237 t1 = circ1b(numr2i) * circ2b(numr2i);
04238 q(1) = q(1)+t1;
04239 t(1) = t(1)+t1;
04240
04241 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04242 if (numr3i == maxrin) {
04243 q(2) += t1;
04244 t(2) += t1;
04245 } else {
04246 q(numr3i+1) += t1;
04247 t(numr3i+1) += t1;
04248 }
04249
04250 for (j=3; j<=numr3i; j=j+2) {
04251 jc = j+numr2i-1;
04252
04253 c1 = circ1b(jc);
04254 c2 = circ1b(jc+1);
04255 d1 = circ2b(jc);
04256 d2 = circ2b(jc+1);
04257
04258 t1 = c1 * d1;
04259 t3 = c1 * d2;
04260 t2 = c2 * d2;
04261 t4 = c2 * d1;
04262
04263 q(j) += t1 + t2;
04264 q(j+1) += - t3 + t4;
04265 t(j) += t1 - t2;
04266 t(j+1) += - t3 - t4;
04267 }
04268 }
04269
04270
04271 fftr_d(q,ip);
04272
04273
04274 fftr_d(t,ip);
04275
04276 EMData* out = new EMData();
04277 out->set_size(maxrin,2,1);
04278 float *dout = out->get_data();
04279 for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04280
04281
04282
04283 free(t);
04284 free(q);
04285 return out;
04286 }
04287
04288
04289 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04290
04291 int maxrin = numr[numr.size()-1];
04292
04293 vector<float> r(2*maxrin);
04294
04295 Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04296
04297 return r;
04298 }
04299
04300 #define dout(i,j) dout[i+maxrin*j]
04301 #define circ1b(i) circ1b[i-1]
04302 #define circ2b(i) circ2b[i-1]
04303
04304 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04305
04306
04307
04308 int ip, jc, numr3i, numr2i, i, j;
04309 float t1, t2, t3, t4, c1, c2, d1, d2;
04310
04311 int nring = numr.size()/3;
04312
04313 int maxrin = numr[numr.size()-1];
04314
04315 float* circ1b = circ1->get_data();
04316 float* circ2b = circ2->get_data();
04317
04318 #ifdef _WIN32
04319 ip = -(int)(log((float)maxrin)/log(2.0f));
04320 #else
04321 ip = -(int)(log2(maxrin));
04322 #endif //_WIN32
04323 for (int i=1; i<=maxrin; i++) {q(i) = 0.0f; t(i) = 0.0f;}
04324
04325
04326
04327
04328
04329 for (i=1; i<=nring; i++) {
04330
04331 numr3i = numr(3,i);
04332 numr2i = numr(2,i);
04333
04334 t1 = circ1b(numr2i) * circ2b(numr2i);
04335 q(1) += t1;
04336 t(1) += t1;
04337
04338 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04339 if (numr3i == maxrin) {
04340 q(2) += t1;
04341 t(2) += t1;
04342 } else {
04343 q(numr3i+1) += t1;
04344 t(numr3i+1) += t1;
04345 }
04346
04347 for (j=3; j<=numr3i; j=j+2) {
04348 jc = j+numr2i-1;
04349
04350 c1 = circ1b(jc);
04351 c2 = circ1b(jc+1);
04352 d1 = circ2b(jc);
04353 d2 = circ2b(jc+1);
04354
04355 t1 = c1 * d1;
04356 t3 = c1 * d2;
04357 t2 = c2 * d2;
04358 t4 = c2 * d1;
04359
04360 q(j) += t1 + t2;
04361 q(j+1) += -t3 + t4;
04362 t(j) += t1 - t2;
04363 t(j+1) += -t3 - t4;
04364 }
04365 }
04366
04367 fftr_q(q,ip);
04368
04369
04370
04371 fftr_q(t,ip);
04372 }
04373
04374
04375
04376 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04377 {
04378
04379 int ip, jc, numr3i, numr2i, i, j;
04380 float t1, t2, t3, t4, c1, c2, d1, d2;
04381
04382 int nring = numr.size()/3;
04383 int maxrin = numr[numr.size()-1];
04384
04385 float* circ1b = circ1->get_data();
04386 float* circ2b = circ2->get_data();
04387
04388 double *q;
04389
04390 q = (double*)calloc(maxrin,sizeof(double));
04391
04392 #ifdef _WIN32
04393 ip = -(int)(log((float)maxrin)/log(2.0f));
04394 #else
04395 ip = -(int)(log2(maxrin));
04396 #endif //_WIN32
04397
04398
04399
04400 for (i=1;i<=nring;i++) {
04401
04402 numr3i = numr(3,i);
04403 numr2i = numr(2,i);
04404
04405 t1 = circ1b(numr2i) * circ2b(numr2i);
04406 q(1) = q(1)+t1;
04407
04408 if (numr3i == maxrin) {
04409 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04410 q(2) = q(2)+t1;
04411 } else {
04412 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04413 q(numr3i+1) = q(numr3i+1)+t1;
04414 }
04415
04416 for (j=3;j<=numr3i;j=j+2) {
04417 jc = j+numr2i-1;
04418
04419 c1 = circ1b(jc);
04420 c2 = circ1b(jc+1);
04421 d1 = circ2b(jc);
04422 d2 = circ2b(jc+1);
04423
04424 t1 = c1 * d1;
04425 t3 = c1 * d2;
04426 t2 = c2 * d2;
04427 t4 = c2 * d1;
04428
04429 q(j) = q(j) + t1 + t2;
04430 q(j+1) = q(j+1) - t3 + t4;
04431 }
04432 }
04433
04434
04435 fftr_d(q,ip);
04436
04437 EMData* out = new EMData();
04438 out->set_size(maxrin,1,1);
04439 float *dout = out->get_data();
04440 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04441 free(q);
04442 return out;
04443
04444 }
04445
04446
04447 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04448 {
04449
04450 int ip, jc, numr3i, numr2i, i, j;
04451 float t1, t2, t3, t4, c1, c2, d1, d2;
04452
04453 int nring = numr.size()/3;
04454 int maxrin = numr[numr.size()-1];
04455
04456 float* circ1b = circ1->get_data();
04457 float* circ2b = circ2->get_data();
04458
04459 double *t;
04460
04461 t = (double*)calloc(maxrin,sizeof(double));
04462
04463 #ifdef _WIN32
04464 ip = -(int)(log((float)maxrin)/log(2.0f));
04465 #else
04466 ip = -(int)(log2(maxrin));
04467 #endif //_WIN32
04468
04469
04470
04471 for (i=1;i<=nring;i++) {
04472
04473 numr3i = numr(3,i);
04474 numr2i = numr(2,i);
04475
04476 t1 = circ1b(numr2i) * circ2b(numr2i);
04477 t(1) = t(1)+t1;
04478
04479 if (numr3i == maxrin) {
04480 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04481 t(2) = t(2)+t1;
04482 }
04483
04484 for (j=3;j<=numr3i;j=j+2) {
04485 jc = j+numr2i-1;
04486
04487 c1 = circ1b(jc);
04488 c2 = circ1b(jc+1);
04489 d1 = circ2b(jc);
04490 d2 = circ2b(jc+1);
04491
04492 t1 = c1 * d1;
04493 t3 = c1 * d2;
04494 t2 = c2 * d2;
04495 t4 = c2 * d1;
04496
04497 t(j) = t(j) + t1 - t2;
04498 t(j+1) = t(j+1) - t3 - t4;
04499 }
04500 }
04501
04502
04503 fftr_d(t,ip);
04504
04505 EMData* out = new EMData();
04506 out->set_size(maxrin,1,1);
04507 float *dout = out->get_data();
04508 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04509 free(t);
04510 return out;
04511
04512 }
04513
04514 #undef circ1b
04515 #undef circ2b
04516 #undef dout
04517
04518 #undef circ1
04519 #undef circ2
04520 #undef t
04521 #undef q
04522 #undef b
04523 #undef t7
04524
04525
04526 #define QUADPI 3.141592653589793238462643383279502884197
04527 #define PI2 2*QUADPI
04528
04529 float Util::ener(EMData* ave, vector<int> numr) {
04530 ENTERFUNC;
04531 long double ener,en;
04532
04533 int nring = numr.size()/3;
04534 float *aveptr = ave->get_data();
04535
04536 ener = 0.0;
04537 for (int i=1; i<=nring; i++) {
04538 int numr3i = numr(3,i);
04539 int np = numr(2,i)-1;
04540 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04541 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04542 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04543 ener += en/numr3i;
04544 }
04545 EXITFUNC;
04546 return static_cast<float>(ener);
04547 }
04548
04549 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04550 ENTERFUNC;
04551 long double ener, en;
04552 float arg, cs, si;
04553
04554 int nima = data.size();
04555 int nring = numr.size()/3;
04556 int maxrin = numr(3,nring);
04557
04558 ener = 0.0;
04559 for (int i=1; i<=nring; i++) {
04560 int numr3i = numr(3,i);
04561 int np = numr(2,i)-1;
04562 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04563 float temp1 = 0.0, temp2 = 0.0;
04564 for (int kk=0; kk<nima; kk++) {
04565 float *ptr = data[kk]->get_data();
04566 temp1 += ptr[np];
04567 temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04568 }
04569 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04570 for (int j=2; j<numr3i; j+=2) {
04571 float tempr = 0.0, tempi = 0.0;
04572 for (int kk=0; kk<nima; kk++) {
04573 float *ptr = data[kk]->get_data();
04574 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04575 cs = cos(arg);
04576 si = sin(arg);
04577 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04578 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04579 }
04580 en += tq*(tempr*tempr+tempi*tempi);
04581 }
04582 ener += en/numr3i;
04583 }
04584 EXITFUNC;
04585 return static_cast<float>(ener);
04586 }
04587
04588 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04589 int nring = numr.size()/3;
04590 float *ave = avep->get_data();
04591 float *dat = datp->get_data();
04592 int i, j, numr3i, np;
04593 float arg, cs, si;
04594 int maxrin = numr(3,nring);
04595 if(mirror == 1) {
04596 for (i=1; i<=nring; i++) {
04597 numr3i = numr(3,i);
04598 np = numr(2,i)-1;
04599 ave[np] += dat[np];
04600 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04601 for (j=2; j<numr3i; j=j+2) {
04602 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04603 cs = cos(arg);
04604 si = sin(arg);
04605
04606 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04607 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04608 }
04609 }
04610 } else {
04611 for (i=1; i<=nring; i++) {
04612 numr3i = numr(3,i);
04613 np = numr(2,i)-1;
04614 ave[np] += dat[np];
04615 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04616 for (j=2; j<numr3i; j=j+2) {
04617 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04618 cs = cos(arg);
04619 si = sin(arg);
04620
04621 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04622 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04623 }
04624 }
04625 }
04626 avep->update();
04627 EXITFUNC;
04628 }
04629
04630 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04631 int nring = numr.size()/3;
04632 float *ave = avep->get_data();
04633 float *dat = datp->get_data();
04634 int i, j, numr3i, np;
04635 float arg, cs, si;
04636 int maxrin = numr(3,nring);
04637 if(mirror == 1) {
04638 for (i=1; i<=nring; i++) {
04639 numr3i = numr(3,i);
04640 np = numr(2,i)-1;
04641 ave[np] -= dat[np];
04642 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04643 for (j=2; j<numr3i; j=j+2) {
04644 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04645 cs = cos(arg);
04646 si = sin(arg);
04647
04648 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04649 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04650 }
04651 }
04652 } else {
04653 for (i=1; i<=nring; i++) {
04654 numr3i = numr(3,i);
04655 np = numr(2,i)-1;
04656 ave[np] -= dat[np];
04657 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04658 for (j=2; j<numr3i; j=j+2) {
04659 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04660 cs = cos(arg);
04661 si = sin(arg);
04662
04663 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04664 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04665 }
04666 }
04667 }
04668 avep->update();
04669 EXITFUNC;
04670 }
04671
04672
04673 #undef QUADPI
04674 #undef PI2
04675
04676 #undef numr
04677 #undef circ
04678
04679
04680 #define QUADPI 3.141592653589793238462643383279502884197
04681 #define PI2 QUADPI*2
04682 #define deg_rad QUADPI/180.0
04683 #define rad_deg 180.0/QUADPI
04684
04685 struct ori_t
04686 {
04687 int iphi;
04688 int itht;
04689 int id;
04690 };
04691
04692
04693 struct cmpang
04694 {
04695 bool operator()( const ori_t& a, const ori_t& b )
04696 {
04697 if( a.itht != b.itht )
04698 {
04699 return a.itht < b.itht;
04700 }
04701
04702 return a.iphi < b.iphi;
04703 }
04704 };
04705
04706
04707 vector<double> Util::cml_weights(const vector<float>& cml){
04708 static const int NBIN = 100;
04709 int nline=cml.size()/2;
04710 vector<double> weights(nline);
04711
04712 vector<ori_t> angs(nline);
04713 for( int i=0; i < nline; ++i ) {
04714 angs[i].iphi = int( NBIN*cml[2*i] );
04715 angs[i].itht = int( NBIN*cml[2*i+1] );
04716 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04717 angs[i].id = i;
04718 }
04719
04720
04721
04722 std::sort( angs.begin(), angs.end(), cmpang() );
04723
04724 vector<float> newphi;
04725 vector<float> newtht;
04726 vector< vector<int> > indices;
04727
04728 int curt_iphi = -1;
04729 int curt_itht = -1;
04730 for(unsigned int i=0 ;i < angs.size(); ++i ) {
04731 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04732 Assert( indices.size() > 0 );
04733 indices.back().push_back(angs[i].id);
04734 } else {
04735 curt_iphi = angs[i].iphi;
04736 curt_itht = angs[i].itht;
04737
04738 newphi.push_back( float(curt_iphi)/NBIN );
04739 newtht.push_back( float(curt_itht)/NBIN );
04740 indices.push_back( vector<int>(1,angs[i].id) );
04741 }
04742 }
04743
04744
04745
04746
04747 int num_agl = newphi.size();
04748
04749 if(num_agl>2) {
04750 vector<double> w=Util::vrdg(newphi, newtht);
04751
04752 Assert( w.size()==newphi.size() );
04753 Assert( indices.size()==newphi.size() );
04754
04755 for(unsigned int i=0; i < newphi.size(); ++i ) {
04756
04757
04758
04759
04760
04761
04762
04763
04764 for(unsigned int j=0; j < indices[i].size(); ++j ) {
04765 int id = indices[i][j];
04766 weights[id] = w[i]/indices[i].size();
04767
04768 }
04769
04770
04771
04772 }
04773 } else {
04774 cout<<"warning in Util.cml_weights"<<endl;
04775 double val = PI2/float(nline);
04776 for(int i=0; i<nline; i++) weights[i]=val;
04777 }
04778
04779 return weights;
04780
04781 }
04782
04783
04784
04785
04786
04787 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04788 {
04789 int i;
04790 int nx=img->get_xsize();
04791 float *img_ptr = img->get_data();
04792 float *line_ptr = line->get_data();
04793 for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04794 img->update();
04795 }
04796
04797 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04798 int j;
04799 int nx = sino->get_xsize();
04800 int i = nx * pos_line;
04801 float r1, r2;
04802 float *line_ptr = line->get_data();
04803 float *sino_ptr = sino->get_data();
04804 for (j=ilf;j<=ihf; j += 2) {
04805 r1 = line_ptr[j];
04806 r2 = line_ptr[j + 1];
04807 sino_ptr[i + j - ilf] = r1;
04808 sino_ptr[i + j - ilf + 1] = r2;
04809 sino_ptr[i + nx * nblines + j - ilf] = r1;
04810 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04811 }
04812 sino->update();
04813 }
04814
04815 vector<double> Util::cml_init_rot(vector<float> Ori){
04816 int nb_ori = Ori.size() / 4;
04817 int i, ind;
04818 float ph, th, ps;
04819 double cph, cth, cps, sph, sth, sps;
04820 vector<double> Rot(nb_ori*9);
04821 for (i=0; i<nb_ori; ++i){
04822 ind = i*4;
04823
04824 ph = Ori[ind+2]-90;
04825 th = Ori[ind+1];
04826 ps = Ori[ind]+90;
04827 ph *= deg_rad;
04828 th *= deg_rad;
04829 ps *= deg_rad;
04830
04831 cph = cos(ph);
04832 cth = cos(th);
04833 cps = cos(ps);
04834 sph = sin(ph);
04835 sth = sin(th);
04836 sps = sin(ps);
04837
04838 ind = i*9;
04839 Rot[ind] = cph*cps-cth*sps*sph;
04840 Rot[ind+1] = cph*sps+cth*cps*sph;
04841 Rot[ind+2] = sth*sph;
04842 Rot[ind+3] = -sph*cps-cth*sps*cph;
04843 Rot[ind+4] = -sph*sps+cth*cps*cph;
04844 Rot[ind+5] = sth*cph;
04845 Rot[ind+6] = sth*sps;
04846 Rot[ind+7] = -sth*cps;
04847 Rot[ind+8] = cth;
04848 }
04849
04850 return Rot;
04851 }
04852
04853 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04854 float ph, ps;
04855 double cph, cth, cps, sph, sth, sps;
04856 int ind = iprj*9;
04857
04858 ph = nps-90;
04859 ps = nph+90;
04860 ph *= deg_rad;
04861 th *= deg_rad;
04862 ps *= deg_rad;
04863
04864 cph = cos(ph);
04865 cth = cos(th);
04866 cps = cos(ps);
04867 sph = sin(ph);
04868 sth = sin(th);
04869 sps = sin(ps);
04870
04871 Rot[ind] = (float)(cph*cps-cth*sps*sph);
04872 Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04873 Rot[ind+2] = (float)(sth*sph);
04874 Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04875 Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04876 Rot[ind+5] = (float)(sth*cph);
04877 Rot[ind+6] = (float)(sth*sps);
04878 Rot[ind+7] = (float)(-sth*cps);
04879 Rot[ind+8] = (float)(cth);
04880
04881 return Rot;
04882 }
04883
04884 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04885 vector<int> com(2*(n_prj - 1));
04886 int a = i_prj*9;
04887 int i, b, c;
04888 int n1=0, n2=0;
04889 float vmax = 1 - 1.0e-6f;
04890 double r11, r12, r13, r23, r31, r32, r33;
04891
04892 c = 0;
04893 for (i=0; i<n_prj; ++i){
04894 if (i!=i_prj){
04895 b = i*9;
04896
04897 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04898 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04899 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04900 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04901 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04902 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04903 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04904 if (r33 > vmax) {
04905 n2 = 270;
04906 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04907 }
04908 else if (r33 < -vmax) {
04909 n2 = 270;
04910 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04911 } else {
04912 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04913 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04914 if (n1 < 0) {n1 += 360;}
04915 if (n2 <= 0) {n2 = abs(n2);}
04916 else {n2 = 360 - n2;}
04917 }
04918
04919 if (n1 >= 360){n1 = n1 % 360;}
04920 if (n2 >= 360){n2 = n2 % 360;}
04921
04922
04923 b = c*2;
04924 com[b] = n1;
04925 com[b+1] = n2;
04926 ++c;
04927 }
04928 }
04929
04930 return com;
04931
04932 }
04933
04934 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int, int n_lines) {
04935 vector<int> com(2*n_lines);
04936 int a=0, b, c, l;
04937 int n1=0, n2=0, mem=-1;
04938 float vmax = 1 - 1.0e-6f;
04939 double r11, r12, r13, r23, r31, r32, r33;
04940 c = 0;
04941 for (l=0; l<n_lines; ++l){
04942 c = 2*l;
04943 if (seq[c]!=mem){
04944 mem = seq[c];
04945 a = seq[c]*9;
04946 }
04947 b = seq[c+1]*9;
04948
04949
04950 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04951 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04952 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04953 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04954 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04955 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04956 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04957 if (r33 > vmax) {
04958 n2 = 270;
04959 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04960 }
04961 else if (r33 < -vmax) {
04962 n2 = 270;
04963 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04964 } else {
04965 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04966 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04967 if (n1 < 0) {n1 += 360;}
04968 if (n2 <= 0) {n2 = abs(n2);}
04969 else {n2 = 360 - n2;}
04970 }
04971 if (n1 >= 360){n1 = n1 % 360;}
04972 if (n2 >= 360){n2 = n2 % 360;}
04973
04974
04975 com[c] = n1;
04976 com[c+1] = n2;
04977 }
04978
04979 return com;
04980
04981 }
04982
04983 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int, int nlines){
04984
04985 vector<double> cml(2*nlines);
04986 float ph1, th1;
04987 float ph2, th2;
04988 double nx, ny, nz;
04989 double norm;
04990 double sth1=0, sph1=0, cth1=0, cph1=0;
04991 double sth2, sph2, cth2, cph2;
04992 int l, ind, c;
04993 int mem = -1;
04994 for (l=0; l<nlines; ++l){
04995 c = 2*l;
04996 if (seq[c]!=mem){
04997 mem = seq[c];
04998 ind = 4*seq[c];
04999 ph1 = Ori[ind]*deg_rad;
05000 th1 = Ori[ind+1]*deg_rad;
05001 sth1 = sin(th1);
05002 sph1 = sin(ph1);
05003 cth1 = cos(th1);
05004 cph1 = cos(ph1);
05005 }
05006 ind = 4*seq[c+1];
05007 ph2 = Ori[ind]*deg_rad;
05008 th2 = Ori[ind+1]*deg_rad;
05009 sth2 = sin(th2);
05010 cth2 = cos(th2);
05011 sph2 = sin(ph2);
05012 cph2 = cos(ph2);
05013
05014 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
05015 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
05016 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
05017 norm = sqrt(nx*nx+ny*ny+nz*nz);
05018 nx /= norm;
05019 ny /= norm;
05020 nz /= norm;
05021
05022 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
05023
05024 cml[c+1] = acos(nz);
05025 if (cml[c+1] == 0) {cml[c] = 0;}
05026 else {
05027 cml[c+1] *= rad_deg;
05028 if (cml[c+1] > 89.99) {cml[c+1] = 89.99;}
05029 cml[c] = rad_deg * atan2(nx, ny);
05030 cml[c] = fmod(360 + cml[c], 360);
05031
05032 }
05033 }
05034
05035 return cml;
05036 }
05037
05038 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
05039 double res = 0;
05040 double buf = 0;
05041 float* line_1;
05042 float* line_2;
05043 int i, n, ind;
05044 int lnlen = data[0]->get_xsize();
05045 for (n=0; n<n_lines; ++n) {
05046 ind = n*2;
05047 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
05048 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
05049 buf = 0;
05050 for (i=0; i<lnlen; ++i) {
05051 buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
05052 }
05053 res += buf * weights[n];
05054 }
05055
05056 return res;
05057
05058 }
05059
05060 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
05061 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
05062
05063
05064
05065 vector<double> res(2);
05066 int lnlen = data[0]->get_xsize();
05067 int end = 2*(n_prj-1);
05068 double disc, buf, bdisc, tmp;
05069 int n, i, ipsi, ind, bipsi, c;
05070 float* line_1;
05071 float* line_2;
05072 bdisc = 1.0e6;
05073 bipsi = -1;
05074
05075 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
05076
05077 disc = 0;
05078 c = 0;
05079 for (n=0; n<n_prj; ++n) {
05080 if(n!=iprj) {
05081 ind = 2*c;
05082 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
05083 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
05084 buf = 0;
05085 for (i=0; i<lnlen; ++i) {
05086 tmp = line_1[i]-line_2[i];
05087 buf += tmp*tmp;
05088 }
05089 disc += buf * weights[iw[c]];
05090 ++c;
05091 }
05092 }
05093
05094 if (disc <= bdisc) {
05095 bdisc = disc;
05096 bipsi = ipsi;
05097 }
05098
05099 for (i=0; i<end; i+=2){
05100 com[i] += d_psi;
05101 if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05102 }
05103 }
05104 res[0] = bdisc;
05105 res[1] = float(bipsi);
05106
05107 return res;
05108 }
05109
05110 vector<double> Util::cml_spin_psi_now(const vector<EMData*>& data, vector<int> com, \
05111 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
05112
05113
05114
05115 vector<double> res(2);
05116 int lnlen = data[0]->get_xsize();
05117 int end = 2*(n_prj-1);
05118 double disc, buf, bdisc, tmp;
05119 int n, i, ipsi, ind, bipsi, c;
05120 float* line_1;
05121 float* line_2;
05122 bdisc = 1.0e6;
05123 bipsi = -1;
05124
05125 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
05126
05127 disc = 0;
05128 c = 0;
05129 for (n=0; n<n_prj; ++n) {
05130 if(n!=iprj) {
05131 ind = 2*c;
05132 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
05133 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
05134 buf = 0;
05135 for (i=0; i<lnlen; ++i) {
05136 tmp = line_1[i]-line_2[i];
05137 buf += tmp*tmp;
05138 }
05139 disc += buf;
05140 ++c;
05141 }
05142 }
05143
05144 if (disc <= bdisc) {
05145 bdisc = disc;
05146 bipsi = ipsi;
05147 }
05148
05149 for (i=0; i<end; i+=2){
05150 com[i] += d_psi;
05151 if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05152 }
05153 }
05154 res[0] = bdisc;
05155 res[1] = float(bipsi);
05156
05157 return res;
05158 }
05159
05160 #undef QUADPI
05161 #undef PI2
05162 #undef deg_rad
05163 #undef rad_deg
05164
05165
05166
05167
05168
05169
05170 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
05171 ENTERFUNC;
05172
05173 int nima = data.size();
05174 vector<float> res(nima);
05175 double result = 0.;
05176 double valmin = 1.0e20;
05177 int valpos = -1;
05178
05179 for (int kk=0; kk<nima; kk++){
05180 result = 0;
05181
05182 float *y_data = data[kk]->get_data();
05183 float *x_data = image->get_data();
05184 long totsize = image->get_xsize()*image->get_ysize();
05185 for (long i = 0; i < totsize; i++) {
05186 double temp = x_data[i]- y_data[i];
05187 result += temp*temp;
05188 }
05189 result /= totsize;
05190 res[kk] = (float)result;
05191
05192 if(result<valmin) {valmin = result; valpos = kk;}
05193
05194 }
05195
05196 Dict retvals;
05197 retvals["dist"] = res;
05198 retvals["pos"] = valpos;
05199
05200 EXITFUNC;
05201 return retvals;
05202
05203 }
05204
05205 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05206 ENTERFUNC;
05207
05208 int nima = data.size();
05209 vector<float> res(nima);
05210 double result = 0.;
05211 double valmin = 1.0e20;
05212 int valpos = -1;
05213
05214 for (int kk=0; kk<nima; kk++){
05215 result = 0;
05216
05217
05218 float *y_data = data[kk]->get_data();
05219 float *x_data = image->get_data();
05220
05221
05222 int nx = data[kk]->get_xsize();
05223 int ny = data[kk]->get_ysize();
05224 nx = (nx - 2 + data[kk]->is_fftodd());
05225 int lsd2 = (nx + 2 - nx%2) ;
05226
05227 int ixb = 2*((nx+1)%2);
05228 int iyb = ny%2;
05229 int iz = 0;
05230
05231 for ( int iy = 0; iy <= ny-1; iy++) {
05232 for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05233 int ii = ix + (iy + iz * ny)* lsd2;
05234 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05235 }
05236 }
05237 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05238 int ii = (iy + iz * ny)* lsd2;
05239 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05240 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05241 }
05242 if(nx%2 == 0) {
05243 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05244 int ii = lsd2 - 2 + (iy + iz * ny)* lsd2;
05245 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05246 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05247 }
05248
05249 }
05250 result *= 2;
05251 result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05252 if(ny%2 == 0) {
05253 int ii = (ny/2 + iz * ny)* lsd2;
05254 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05255 }
05256 if(nx%2 == 0) {
05257 int ii = lsd2 - 2 + (0 + iz * ny)* lsd2;
05258 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05259 if(ny%2 == 0) {
05260 int ii = lsd2 - 2 +(ny/2 + iz * ny)* lsd2;
05261 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05262 }
05263 }
05264
05265 result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05266 res[kk] = (float)result;
05267
05268 if(result<valmin) {valmin = result; valpos = kk;}
05269
05270 }
05271
05272 Dict retvals;
05273 retvals["dist"] = res;
05274 retvals["pos"] = valpos;
05275
05276 EXITFUNC;
05277 return retvals;
05278 }
05279
05280 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05281 long int d2 = group2[s2 - 1] - group2[0];
05282 long int p2 = 0;
05283 long int i1 = 0;
05284 long int i2 = 0;
05285 long int max = 0;
05286 long int cont = 0;
05287 long int i = 0;
05288 int stop1 = 0;
05289 int stop2 = 0;
05290
05291 for (i=0; i<s1; i++) {
05292 p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05293 if (p2 >= s2) {p2 = s2 - 1;}
05294 i1 = p2;
05295 i2 = p2;
05296 max = s2;
05297 if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05298
05299 stop1 = 0;
05300 stop2 = 0;
05301 while (max--) {
05302 if (group1[i] == group2[i1]) {
05303 if (flag) {stb[cont] = group1[i];}
05304 cont++;
05305 break;
05306 }
05307 if (group2[i1] < group1[i]) {stop1=1;}
05308 if (group1[i] == group2[i2]) {
05309 if (flag) {stb[cont] = group1[i];}
05310 cont++;
05311 break;
05312 }
05313 if (group2[i2] > group1[i]) {stop2=1;}
05314
05315
05316 if (stop1 & stop2) {break;}
05317 i1--;
05318 i2++;
05319 if (i1 < 0) {i1 = 0;}
05320 if (i2 >= s2) {i2 = s2 - 1;}
05321 }
05322
05323 }
05324
05325 return cont;
05326 }
05327
05328
05329
05330 #define old_ptr(i,j,k) old_ptr[i+(j+(k*ny))*(size_t)nx]
05331 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*(size_t)new_nx]
05332 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05333 {
05334
05335 if (!img) {
05336 throw NullPointerException("NULL input image");
05337 }
05338
05339
05340
05341 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05342
05343
05344
05345
05346 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)
05347 {
05348 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05349 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05350 }
05351
05352
05353
05354
05355 int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05356
05357
05358
05359
05360 int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05361 int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05362 int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05363 if(r1>1){r1=1;}
05364 if(r2>1){r2=1;}
05365 if(r3>1){r3=1;}
05366 int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05367
05368
05369
05370 EMData* img2 = new EMData();
05371 img2->set_size(new_nx,new_ny,new_nz);
05372 float *new_ptr = img2->get_data();
05373 float *old_ptr = img->get_data();
05374 int iptr, jptr, kptr = 0;
05375 for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05376 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05377 for (int i=new_st_x; i<nx; i+=x_step) {
05378 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05379 iptr++;}
05380 jptr++;}
05381 kptr++;}
05382 img2->update();
05383 return img2;
05384 }
05385 #undef old_ptr
05386 #undef new_ptr
05387
05388 #define inp(i,j,k) inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*(size_t)nx]
05389 #define outp(i,j,k) outp[i+(j+(k*new_ny))*(size_t)new_nx]
05390 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05391 {
05392
05393 if (!img) throw NullPointerException("NULL input image");
05394
05395
05396
05397 int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05398
05399
05400
05401 if(new_nx>nx || new_ny>ny || new_nz>nz)
05402 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size.");
05403 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)
05404 throw ImageDimensionException("The offset inconsistent with the input image size.");
05405 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))))
05406 throw ImageDimensionException("The offset inconsistent with the input image size.");
05407
05408
05409
05410 int new_st_x = nx/2-new_nx/2 + x_offset,
05411 new_st_y = ny/2-new_ny/2 + y_offset,
05412 new_st_z = nz/2-new_nz/2 + z_offset;
05413
05414
05415
05416 if (new_st_x<0 || new_st_y<0 || new_st_z<0)
05417 throw ImageDimensionException("The offset inconsistent with the input image size.");
05418
05419
05420 EMData* wind = img->copy_empty_head();
05421 wind->set_size(new_nx, new_ny, new_nz);
05422 float *outp=wind->get_data();
05423 float *inp=img->get_data();
05424
05425 for (int k=0; k<new_nz; k++)
05426 for(int j=0; j<new_ny; j++)
05427 for(int i=0; i<new_nx; i++)
05428 outp(i,j,k) = inp(i,j,k);
05429 wind->update();
05430 return wind;
05431 }
05432 #undef inp
05433 #undef outp
05434
05435 #define inp(i,j,k) inp[i+(j+(k*ny))*(size_t)nx]
05436 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*(size_t)new_nx]
05437 EMData *Util::pad(EMData* img,int new_nx, int new_ny, int new_nz, int x_offset, int y_offset, int z_offset, const char *params)
05438 {
05439
05440 if (!img) throw NullPointerException("NULL input image");
05441
05442
05443
05444 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05445
05446
05447
05448 if(new_nx<nx || new_ny<ny || new_nz<nz)
05449 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05450 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)
05451 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05452 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))))
05453 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05454
05455
05456 EMData* pading = img->copy_head();
05457 pading->set_size(new_nx, new_ny, new_nz);
05458 float *inp = img->get_data();
05459 float *outp = pading->get_data();
05460
05461
05462
05463
05464 float background;
05465
05466 if (strcmp(params,"average")==0) background = img->get_attr("mean");
05467 else if (strcmp(params,"circumference")==0) {
05468 float sum1=0.0f;
05469 size_t cnt=0;
05470 for(int i=0;i<nx;i++) {
05471 sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05472 cnt+=2;
05473 }
05474 if(nz-1 == 0) {
05475 for (int j=1;j<ny-1;j++) {
05476 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05477 cnt+=2;
05478 }
05479 } else {
05480 for (int k=1;k<nz-1;k++) {
05481 for (int j=1;j<ny-1;j++) {
05482 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05483 cnt+=2;
05484 }
05485 }
05486 }
05487 background = sum1/cnt;
05488 } else {
05489 background = static_cast<float>( atof( params ) );
05490 }
05491
05492
05493
05494 int new_st_x=0,new_st_y=0,new_st_z=0;
05495 for (int k=0;k<new_nz;k++)
05496 for(int j=0;j<new_ny;j++)
05497 for (int i=0;i<new_nx;i++)
05498 outp(i,j,k)=background;
05499
05500
05501
05502 new_st_x=int((new_nx/2-nx/2) + x_offset);
05503 new_st_y=int((new_ny/2-ny/2) + y_offset);
05504 new_st_z=int((new_nz/2-nz/2) + z_offset);
05505
05506
05507 for (int k=0;k<nz;k++)
05508 for(int j=0;j<ny;j++)
05509 for(int i=0;i<nx;i++)
05510 outp(i,j,k)=inp(i,j,k);
05511 pading->update();
05512 return pading;
05513 }
05514 #undef inp
05515 #undef outp
05516
05517
05518 void Util::colreverse(float* beg, float* end, int nx) {
05519 float* tmp = new float[nx];
05520 int n = (end - beg)/nx;
05521 int nhalf = n/2;
05522 for (int i = 0; i < nhalf; i++) {
05523
05524 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05525 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05526 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05527 }
05528 delete[] tmp;
05529 }
05530
05531 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05532 {
05533 int nxy = nx*ny;
05534 colreverse(beg, end, nxy);
05535 }
05536
05537
05538 void Util::cyclicshift(EMData *image, Dict params) {
05539
05540 if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05541
05542 int dx = params["dx"];
05543 int dy = params["dy"];
05544 int dz = params["dz"];
05545
05546
05547 int nx = image->get_xsize();
05548 dx %= nx;
05549 if (dx < 0) dx += nx;
05550 int ny = image->get_ysize();
05551 dy %= ny;
05552 if (dy < 0) dy += ny;
05553 int nz = image->get_zsize();
05554 dz %= nz;
05555 if (dz < 0) dz += nz;
05556
05557 int mx = -(dx - nx);
05558 int my = -(dy - ny);
05559 int mz = -(dz - nz);
05560
05561 float* data = image->get_data();
05562
05563 if (mx != 0) {
05564 for (int iz = 0; iz < nz; iz++)
05565 for (int iy = 0; iy < ny; iy++) {
05566
05567 size_t offset = nx*iy + (size_t)nx*ny*iz;
05568 reverse(&data[offset],&data[offset+mx]);
05569 reverse(&data[offset+mx],&data[offset+nx]);
05570 reverse(&data[offset],&data[offset+nx]);
05571 }
05572 }
05573
05574 if (my != 0) {
05575 for (int iz = 0; iz < nz; iz++) {
05576 size_t offset = (size_t)nx*ny*iz;
05577 colreverse(&data[offset], &data[offset + my*nx], nx);
05578 colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05579 colreverse(&data[offset], &data[offset + ny*nx], nx);
05580 }
05581 }
05582 if (mz != 0) {
05583 slicereverse(&data[0], &data[(size_t)mz*ny*nx], nx, ny);
05584 slicereverse(&data[mz*ny*nx], &data[(size_t)nz*ny*nx], nx, ny);
05585 slicereverse(&data[0], &data[(size_t)nz*ny*nx], nx ,ny);
05586 }
05587 image->update();
05588 }
05589
05590
05591
05592
05593 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05594 {
05595 if (image->is_complex())
05596 throw ImageFormatException("Cannot do histogram on Fourier image");
05597
05598 float *imageptr=0, *maskptr=0;
05599 int nx=image->get_xsize();
05600 int ny=image->get_ysize();
05601 int nz=image->get_zsize();
05602
05603 if(mask != NULL){
05604 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05605 throw ImageDimensionException("The size of mask image should be of same size as the input image");
05606 maskptr =mask->get_data();
05607 }
05608 if( nbins == 0) nbins = nx;
05609 vector <float> freq(2*nbins, 0.0);
05610
05611 imageptr=image->get_data();
05612 if( hmin == hmax ) {
05613 if(mask == NULL) {
05614 hmax = image->get_attr("maximum");
05615 hmin = image->get_attr("minimum");
05616 } else {
05617 bool First = true;
05618 for (size_t i = 0;i < (size_t)nx*ny*nz; i++) {
05619 if (maskptr[i]>=0.5f) {
05620 if(First) {
05621 hmax = imageptr[i];
05622 hmin = imageptr[i];
05623 First = false;
05624 } else {
05625 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05626 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05627 }
05628 }
05629 }
05630 }
05631 }
05632 float hdiff = hmax - hmin;
05633 float ff = (nbins-1)/hdiff;
05634 for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05635 if(mask == NULL) {
05636 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05637 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05638 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05639 }
05640 } else {
05641 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05642 if(maskptr[i] >= 0.5) {
05643 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05644 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05645 }
05646 }
05647 }
05648 return freq;
05649 }
05650
05651 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05652 {
05653
05654 if (img->is_complex() || ref->is_complex())
05655 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05656
05657 if(mask != NULL){
05658 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05659 throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05660
05661
05662
05663 size_t size_ref = ((size_t)(ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05664 size_t size_img = ((size_t)(img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05665
05666
05667
05668 float *ref_ptr = ref->get_data();
05669 float ref_h_min = ref->get_attr("minimum");
05670 float ref_h_max = ref->get_attr("maximum");
05671 float ref_h_avg = ref->get_attr("mean");
05672 float ref_h_sig = ref->get_attr("sigma");
05673
05674
05675
05676 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05677
05678 vector<float> img_data = Util::infomask(img, mask);
05679 float img_avg = img_data[0];
05680 float img_sig = img_data[1];
05681
05682
05683 int cnt=0;
05684 for(size_t i=0;i<size_img;++i)
05685 if (mask_ptr[i]>0.5f)
05686 cnt++;
05687
05688
05689
05690 float ref_h_diff = ref_h_max - ref_h_min;
05691
05692 #ifdef _WIN32
05693 int hist_len = _cpp_min((unsigned long)size_ref/16,_cpp_min((unsigned long)size_img/16,256lu));
05694 #else
05695 int hist_len = std::min((unsigned long)size_ref/16,std::min((unsigned long)size_img/16,256lu));
05696 #endif //_WIN32
05697
05698 float *ref_freq_bin = new float[3*hist_len];
05699
05700
05701 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05702
05703 for (size_t i = 0;i < size_ref;++i) {
05704 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05705 ref_freq_bin[L]++;
05706 }
05707 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05708
05709
05710 float A = ref_h_sig/img_sig;
05711 float B = ref_h_avg - (A*img_avg);
05712
05713 vector<float> args;
05714 args.push_back(A);
05715 args.push_back(B);
05716
05717 vector<float> scale;
05718 scale.push_back(1.e-7f*A);
05719 scale.push_back(-1.e-7f*B);
05720
05721 vector<float> ref_freq_hist;
05722 for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05723
05724 vector<float> data;
05725 data.push_back(ref_h_diff);
05726 data.push_back(ref_h_min);
05727
05728 Dict parameter;
05729
05730
05731 parameter["args"] = args;
05732 parameter["scale"]= scale;
05733 parameter["data"] = data;
05734 parameter["ref_freq_bin"] = ref_freq_hist;
05735 parameter["size_img"]=(double)size_img;
05736 parameter["hist_len"]=hist_len;
05737
05738
05739 return parameter;
05740 }
05741
05742
05743 float Util::hist_comp_freq(float PA,float PB,size_t size_img, int hist_len, EMData *img, vector<float> ref_freq_hist, EMData *mask, float ref_h_diff, float ref_h_min)
05744 {
05745 float *img_ptr = img->get_data();
05746 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05747
05748 int *img_freq_bin = new int[3*hist_len];
05749 for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05750 for(size_t i = 0;i < size_img;++i) {
05751 if(mask_ptr[i] > 0.5f) {
05752 float img_xn = img_ptr[i]*PA + PB;
05753 int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05754 if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05755 }
05756 }
05757 int freq_hist = 0;
05758
05759 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);
05760 freq_hist = (-freq_hist);
05761 return static_cast<float>(freq_hist);
05762 }
05763
05764 #define QUADPI 3.141592653589793238462643383279502884197
05765 #define DGR_TO_RAD QUADPI/180
05766 #define DM(I) DM [I-1]
05767 #define SS(I) SS [I-1]
05768 Dict Util::CANG(float PHI,float THETA,float PSI)
05769 {
05770 double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05771 vector<float> DM,SS;
05772
05773 for(int i =0;i<9;i++) DM.push_back(0);
05774
05775 for(int i =0;i<6;i++) SS.push_back(0);
05776
05777 CPHI = cos(double(PHI)*DGR_TO_RAD);
05778 SPHI = sin(double(PHI)*DGR_TO_RAD);
05779 CTHE = cos(double(THETA)*DGR_TO_RAD);
05780 STHE = sin(double(THETA)*DGR_TO_RAD);
05781 CPSI = cos(double(PSI)*DGR_TO_RAD);
05782 SPSI = sin(double(PSI)*DGR_TO_RAD);
05783
05784 SS(1) = float(CPHI);
05785 SS(2) = float(SPHI);
05786 SS(3) = float(CTHE);
05787 SS(4) = float(STHE);
05788 SS(5) = float(CPSI);
05789 SS(6) = float(SPSI);
05790
05791 DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05792 DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05793 DM(3) = float(-STHE*CPSI);
05794 DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05795 DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05796 DM(6) = float(STHE*SPSI);
05797 DM(7) = float(STHE*CPHI);
05798 DM(8) = float(STHE*SPHI);
05799 DM(9) = float(CTHE);
05800
05801 Dict DMnSS;
05802 DMnSS["DM"] = DM;
05803 DMnSS["SS"] = SS;
05804
05805 return(DMnSS);
05806 }
05807 #undef SS
05808 #undef DM
05809 #undef QUADPI
05810 #undef DGR_TO_RAD
05811
05812 #define DM(I) DM[I-1]
05813 #define B(i,j) Bptr[i-1+((j-1)*NSAM)]
05814 #define CUBE(i,j,k) CUBEptr[(i-1)+((j-1)+((k-1)*NY3D))*(size_t)NX3D]
05815
05816 void Util::BPCQ(EMData *B,EMData *CUBE, vector<float> DM)
05817 {
05818
05819 float *Bptr = B->get_data();
05820 float *CUBEptr = CUBE->get_data();
05821
05822 int NSAM,NROW,NX3D,NY3D,NZC,KZ,IQX,IQY,LDPX,LDPY,LDPZ,LDPNMX,LDPNMY,NZ1;
05823 float DIPX,DIPY,XB,YB,XBB,YBB;
05824
05825 Transform * t = B->get_attr("xform.projection");
05826 Dict d = t->get_params("spider");
05827 if(t) {delete t; t=0;}
05828
05829 float x_shift = d[ "tx" ];
05830 float y_shift = d[ "ty" ];
05831 x_shift = -x_shift;
05832 y_shift = -y_shift;
05833
05834 NSAM = B->get_xsize();
05835 NROW = B->get_ysize();
05836 NX3D = CUBE->get_xsize();
05837 NY3D = CUBE->get_ysize();
05838 NZC = CUBE->get_zsize();
05839
05840
05841 LDPX = NX3D/2 +1;
05842 LDPY = NY3D/2 +1;
05843 LDPZ = NZC/2 +1;
05844 LDPNMX = NSAM/2 +1;
05845 LDPNMY = NROW/2 +1;
05846 NZ1 = 1;
05847
05848 for(int K=1;K<=NZC;K++) {
05849 KZ=K-1+NZ1;
05850 for(int J=1;J<=NY3D;J++) {
05851 XBB = (1-LDPX)*DM(1)+(J-LDPY)*DM(2)+(KZ-LDPZ)*DM(3);
05852 YBB = (1-LDPX)*DM(4)+(J-LDPY)*DM(5)+(KZ-LDPZ)*DM(6);
05853 for(int I=1;I<=NX3D;I++) {
05854 XB = (I-1)*DM(1)+XBB-x_shift;
05855 IQX = int(XB+float(LDPNMX));
05856 if (IQX <1 || IQX >= NSAM) continue;
05857 YB = (I-1)*DM(4)+YBB-y_shift;
05858 IQY = int(YB+float(LDPNMY));
05859 if (IQY<1 || IQY>=NROW) continue;
05860 DIPX = XB+LDPNMX-IQX;
05861 DIPY = YB+LDPNMY-IQY;
05862
05863 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)));
05864 }
05865 }
05866 }
05867 }
05868
05869 #undef DM
05870 #undef B
05871 #undef CUBE
05872
05873
05874 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
05875 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
05876 #define SS(I,J) SS [I-1 + (J-1)*6]
05877
05878 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05879 {
05880 int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05881 float WW,OX,OY;
05882
05883 NSAM = PROJ->get_xsize();
05884 NROW = PROJ->get_ysize();
05885 int ntotal = NSAM*NROW;
05886 float q = 2.0f;
05887 float qt = 8.0f/q;
05888
05889 int ipad = 1;
05890 NSAM *= ipad;
05891 NROW *= ipad;
05892 NNNN = NSAM+2-(NSAM%2);
05893 int NX2 = NSAM/2;
05894 NR2 = NROW/2;
05895
05896 NANG = int(SS.size())/6;
05897
05898 EMData* W = new EMData();
05899 int Wnx = NNNN/2;
05900 W->set_size(Wnx,NROW,1);
05901 W->to_zero();
05902 float *Wptr = W->get_data();
05903 float *PROJptr = PROJ->get_data();
05904 for (L=1; L<=NANG; L++) {
05905 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);
05906 float tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) );
05907 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05908 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05909 if(OX < 0.0f) {
05910 OX = -OX;
05911 OY = -OY;
05912 }
05913
05914 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f ) {
05915 for(int J=1;J<=NROW;J++) {
05916 JY = (J-1);
05917 if(JY > NR2) JY -= NROW;
05918 #ifdef _WIN32
05919 int xma = _cpp_min(int(0.5f+(q-JY*OY)/OX),NX2);
05920 int xmi = _cpp_max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05921 #else
05922 int xma = std::min(int(0.5f+(q-JY*OY)/OX),NX2);
05923 int xmi = std::max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05924 #endif //_WIN32
05925 if( xmi <= xma) {
05926 for(int I=xmi;I<=xma;I++) {
05927 float Y = fabs(OX*I + OY*JY);
05928 W(I+1,J) += exp(-qt*Y*Y);
05929
05930 }
05931 }
05932 }
05933 } else {
05934 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J) += 1.0f;
05935 }
05936 }
05937 EMData* proj_in = PROJ;
05938
05939 PROJ = PROJ->norm_pad( false, ipad);
05940 PROJ->do_fft_inplace();
05941 PROJ->update();
05942
05943 PROJptr = PROJ->get_data();
05944
05945 float WNRMinv,temp;
05946 float osnr = 1.0f/SNR;
05947 WNRMinv = 1.0f/W(1,1);
05948 for(int J=1;J<=NROW;J++) {
05949 JY = J-1;
05950 if( JY > NR2) JY -= NROW;
05951 float sy = JY;
05952 sy /= NROW;
05953 sy *= sy;
05954 for(int I=1;I<=NNNN;I+=2) {
05955 KX = (I+1)/2;
05956 temp = W(KX,J)*WNRMinv;
05957 WW = temp/(temp*temp + osnr);
05958
05959 float sx = KX-1;
05960 sx /= NSAM;
05961 WW *= exp(qt*(sy + sx*sx));
05962 PROJ(I,J) *= WW;
05963 PROJ(I+1,J) *= WW;
05964 }
05965 }
05966 delete W; W = 0;
05967 PROJ->do_ift_inplace();
05968 PROJ->depad();
05969
05970 float* data_src = PROJ->get_data();
05971 float* data_dst = proj_in->get_data();
05972
05973 for( int i=0; i < ntotal; ++i ) data_dst[i] = data_src[i];
05974
05975 proj_in->update();
05976
05977 delete PROJ;
05978 }
05979
05980
05981
05982
05983
05984
05985
05986
05987
05988
05989
05990
05991
05992
05993
05994
05995
05996
05997
05998
05999
06000
06001
06002
06003
06004
06005
06006
06007
06008
06009
06010
06011
06012
06013
06014
06015
06016
06017
06018
06019
06020
06021
06022
06023
06024
06025
06026
06027
06028
06029
06030
06031
06032
06033
06034
06035
06036
06037
06038
06039
06040
06041
06042
06043
06044
06045
06046
06047
06048
06049
06050
06051
06052
06053
06054
06055
06056
06057
06058 #undef PROJ
06059 #undef W
06060 #undef SS
06061
06062 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
06063 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
06064 #define SS(I,J) SS [I-1 + (J-1)*6]
06065 #define RI(i,j) RI [(i-1) + ((j-1)*3)]
06066 #define CC(i) CC [i-1]
06067 #define CP(i) CP [i-1]
06068 #define VP(i) VP [i-1]
06069 #define VV(i) VV [i-1]
06070 #define AMAX1(i,j) i>j?i:j
06071 #define AMIN1(i,j) i<j?i:j
06072 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
06073 {
06074 float rad2deg =(180.0f/3.1415926f);
06075 float deg2rad = (3.1415926f/180.0f);
06076
06077 int NSAM,NROW,NNNN,NR2,NANG,L,JY;
06078
06079 NSAM = PROJ->get_xsize();
06080 NROW = PROJ->get_ysize();
06081 NNNN = NSAM+2-(NSAM%2);
06082 NR2 = NROW/2;
06083 NANG = int(SS.size())/6;
06084
06085 float RI[9];
06086 RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
06087 RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
06088 RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
06089 RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
06090 RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
06091 RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
06092 RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
06093 RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
06094 RI(3,3)=SS(3,NUMP);
06095
06096 float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
06097
06098 EMData* W = new EMData();
06099 int Wnx = NNNN/2;
06100 W->set_size(NNNN/2,NROW,1);
06101 W->to_one();
06102 float *Wptr = W->get_data();
06103
06104 float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
06105
06106 for (L=1; L<=NANG; L++) {
06107 if (L != NUMP) {
06108 CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
06109 CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
06110 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);
06111
06112 TMP = sqrt(CC(1)*CC(1) + CC(2)*CC(2) + CC(3)*CC(3));
06113 CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
06114 ALPHA=rad2deg*float(asin(CCN));
06115 if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
06116 if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
06117 if(ALPHA<1.0E-6) {
06118 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
06119 } else {
06120 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
06121 CC(1) = CC(1)/CCN;CC(2) = CC(2)/CCN;CC(3) = CC(3)/CCN;
06122 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
06123 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
06124 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
06125 CP(1) = 0.0;CP(2) = 0.0;
06126 VP(1) = 0.0;VP(2) = 0.0;
06127
06128 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
06129 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
06130 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
06131 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
06132
06133 TMP = CP(1)*VP(2)-CP(2)*VP(1);
06134
06135
06136 TMP = AMAX1(1.0E-4f,fabs(TMP));
06137 float tmpinv = 1.0f/TMP;
06138 for(int J=1;J<=NROW;J++) {
06139 JY = (J-1);
06140 if (JY>NR2) JY=JY-NROW;
06141 for(int I=1;I<=NNNN/2;I++) {
06142 FV = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
06143 RT = 1.0f-FV/FM;
06144 W(I,J) += ((RT>0.0f)*RT);
06145 }
06146 }
06147 }
06148 }
06149 }
06150
06151 EMData* proj_in = PROJ;
06152
06153 PROJ = PROJ->norm_pad( false, 1);
06154 PROJ->do_fft_inplace();
06155 PROJ->update();
06156 float *PROJptr = PROJ->get_data();
06157
06158 int KX;
06159 float WW;
06160 for(int J=1; J<=NROW; J++)
06161 for(int I=1; I<=NNNN; I+=2) {
06162 KX = (I+1)/2;
06163 WW = 1.0f/W(KX,J);
06164 PROJ(I,J) = PROJ(I,J)*WW;
06165 PROJ(I+1,J) = PROJ(I+1,J)*WW;
06166 }
06167 delete W; W = 0;
06168 PROJ->do_ift_inplace();
06169 PROJ->depad();
06170
06171 float* data_src = PROJ->get_data();
06172 float* data_dst = proj_in->get_data();
06173
06174 int ntotal = NSAM*NROW;
06175 for( int i=0; i < ntotal; ++i ) data_dst[i] = data_src[i];
06176
06177 proj_in->update();
06178 delete PROJ;
06179 }
06180 #undef AMAX1
06181 #undef AMIN1
06182 #undef RI
06183 #undef CC
06184 #undef CP
06185 #undef VV
06186 #undef VP
06187
06188
06189 #undef W
06190 #undef SS
06191 #undef PROJ
06192
06193 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06194 {
06195 float cst = cs*1.0e7f;
06196
06197 wgh /= 100.0;
06198 float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06199 float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06200 float ak2 = ak*ak;
06201 float g1 = dzz*1.0e4f*lambda*ak2;
06202 float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06203
06204 float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06205 if(b_factor != 0.0f) ctfv *= exp(-b_factor*ak2/4.0f);
06206
06207 return ctfv;
06208 }
06209
06210 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06211 {
06212
06213
06214
06215 int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();
06216
06217
06218
06219 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06220 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06221
06222 size_t i, size = (size_t)nx*ny*nz;
06223
06224 float* img_ptr = image->get_data();
06225 float* mask_ptr = mask->get_data();
06226
06227 int ln=0;
06228 for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06229
06230 EMData* new_image = new EMData();
06231 new_image->set_size(ln,1,1);
06232 float *new_ptr = new_image->get_data();
06233
06234 ln=-1;
06235 for(i = 0;i < size;i++){
06236 if(mask_ptr[i] > 0.5f) {
06237 ln++;
06238 new_ptr[ln]=img_ptr[i];
06239 }
06240 }
06241
06242 return new_image;
06243 }
06244
06245 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06246 {
06247
06248
06249
06250 if(mask == NULL)
06251 throw ImageDimensionException("The mask cannot be an null image");
06252
06253
06254
06255
06256 int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06257
06258 size_t i,size = (size_t)nx*ny*nz;
06259
06260 EMData *new_image = new EMData();
06261 new_image->set_size(nx,ny,nz);
06262 float *new_ptr = new_image->get_data();
06263 float *mask_ptr = mask->get_data();
06264 float *img_ptr = image->get_data();
06265 int count = 0;
06266 float sum_under_mask = 0.0 ;
06267 for(i = 0;i < size;i++){
06268 if(mask_ptr[i] > 0.5f){
06269 new_ptr[i] = img_ptr[count];
06270 sum_under_mask += img_ptr[count];
06271 count++;
06272 if( count > image->get_xsize() ) {
06273 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06274 }
06275 }
06276 }
06277
06278 if( count > image->get_xsize() ) {
06279 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06280 }
06281
06282 float avg_under_mask = sum_under_mask / count;
06283 for(i = 0;i < size;i++) {
06284 if(mask_ptr[i] <= 0.5f) new_ptr[i] = avg_under_mask;
06285 }
06286 new_image->update();
06287 return new_image;
06288 }
06289
06290
06291
06292 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06293 {
06294 vector<float>new_peak;
06295 int n1=peak1.size()/3;
06296 float p_size2=p_size*p_size;
06297 for (int i=0;i<n1;++i) {
06298 vector<float>::iterator it2= peak1.begin()+3*i;
06299 bool push_back1=true;
06300 int n2=peak2.size()/3;
06301
06302
06303 if(n2 ==0) {
06304 new_peak.push_back(*it2);
06305 new_peak.push_back(*(it2+1));
06306 new_peak.push_back(*(it2+2));
06307 } else {
06308 int j=0;
06309 while (j< n2-1 ) {
06310 vector<float>::iterator it3= peak2.begin()+3*j;
06311 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06312 if(d2< p_size2 ) {
06313 if( (*it2)<(*it3) ) {
06314 new_peak.push_back(*it3);
06315 new_peak.push_back(*(it3+1));
06316 new_peak.push_back(*(it3+2));
06317 peak2.erase(it3);
06318 peak2.erase(it3);
06319 peak2.erase(it3);
06320 push_back1=false;
06321 } else {
06322 peak2.erase(it3);
06323 peak2.erase(it3);
06324 peak2.erase(it3);
06325 }
06326 } else j=j+1;
06327 n2=peak2.size()/3;
06328 }
06329 if(push_back1) {
06330 new_peak.push_back(*it2);
06331 new_peak.push_back(*(it2+1));
06332 new_peak.push_back(*(it2+2));
06333 }
06334 }
06335 }
06336 return new_peak;
06337 }
06338
06339 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06340 {
06341
06342
06343
06344
06345
06346 ENTERFUNC;
06347
06348 int i;
06349
06350
06351 for ( i = 0 ; i < n * n ; i++ ) eigvec[i] = covmat[i];
06352
06353 char NEEDV = 'V';
06354 char UPLO = 'U';
06355 int lwork = -1;
06356 int info = 0;
06357 float *work, wsize;
06358
06359
06360 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06361 lwork = (int)wsize;
06362
06363 work = (float *)calloc(lwork, sizeof(float));
06364
06365 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06366 free(work);
06367 EXITFUNC;
06368 return info;
06369 }
06370
06371 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06372 {
06373
06374 ENTERFUNC;
06375 int len = covmatpy.size();
06376 float *eigvec;
06377 float *eigval;
06378 float *covmat;
06379 int status = 0;
06380 eigval = (float*)calloc(ncov,sizeof(float));
06381 eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06382 covmat = (float*)calloc(ncov*ncov, sizeof(float));
06383
06384 const float *covmat_ptr;
06385 covmat_ptr = &covmatpy[0];
06386 for(int i=0;i<len;i++){
06387 covmat[i] = covmat_ptr[i];
06388 }
06389
06390 status = Util::coveig(ncov, covmat, eigval, eigvec);
06391
06392 vector<float> eigval_py(ncov);
06393 const float *eigval_ptr;
06394 eigval_ptr = &eigval[0];
06395 for(int i=0;i<ncov;i++){
06396 eigval_py[i] = eigval_ptr[i];
06397 }
06398
06399 vector<float> eigvec_py(ncov*ncov);
06400 const float *eigvec_ptr;
06401 eigvec_ptr = &eigvec[0];
06402 for(int i=0;i<ncov*ncov;i++){
06403 eigvec_py[i] = eigvec_ptr[i];
06404 }
06405
06406 Dict res;
06407 res["eigval"] = eigval_py;
06408 res["eigvec"] = eigvec_py;
06409
06410 EXITFUNC;
06411 return res;
06412 }
06413
06414 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06415 {
06416 int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06417
06418 k=(int)pw.size();
06419 l=0;
06420 m=k;
06421 n2=n+2;
06422 n1=n+1;
06423 klmd=k+l+m;
06424 klm2d= k+l+m+2;
06425 nklmd=k+l+m+n;
06426 n2d=n+2;
06427
06428 n_larg=klmd*2;
06429 klm2d=n_larg+klm2d;
06430 klmd=n_larg+klmd;
06431 nklmd=n_larg+nklmd;
06432 int size_q=klm2d*n2d;
06433 int size_cu=nklmd*2;
06434 static int i__;
06435
06436 double *q ;
06437 double *x ;
06438 double *res;
06439 double *cu;
06440 float *q2;
06441 float *pw_;
06442 long int *iu;
06443 double *s;
06444 q = (double*)calloc(size_q,sizeof(double));
06445 x = (double*)calloc(n2d,sizeof(double));
06446 res = (double*)calloc(klmd,sizeof(double));
06447 cu =(double*)calloc(size_cu,sizeof(double));
06448 s = (double*)calloc(klmd,sizeof(double));
06449 q2 = (float*)calloc(size_q,sizeof(float));
06450 iu = (long int*)calloc(size_cu,sizeof(long int));
06451 pw_ = (float*)calloc(k,sizeof(float));
06452
06453 for( i__ =0;i__<k;++i__)
06454 {
06455 pw_[i__]=log(pw[i__]); }
06456 long int l_k=k;
06457 long int l_n=n;
06458 long int l_iswi=iswi;
06459 vector<float> cl1_res;
06460 cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06461 free(q);
06462 free(x);
06463 free(res);
06464 free(s);
06465 free(cu);
06466 free(q2);
06467 free(iu);
06468 free(pw_);
06469 return cl1_res;
06470 }
06471 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)
06472 {
06473 long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06474 float r__1;
06475 int tmp__i;
06476 long int i__, j;
06477 --s;
06478 --res;
06479 iu -= 3;
06480 cu -= 3;
06481 --x;
06482 long int klm2d;
06483 klm2d= *k+*k+2;
06484 klm2d=klm2d+klm2d;
06485 q_dim1 = klm2d;
06486 q_offset = 1 + q_dim1;
06487 q -= q_offset;
06488 q2_dim1 = klm2d;
06489 q2_offset = 1 + q2_dim1;
06490 q2 -= q2_offset;
06491 i__2=0;
06492 i__1 = *n - 1;
06493 tmp__i=0;
06494 for (j = 1; j <= i__1; ++j) {
06495 i__2 = *k;
06496 tmp__i+=1;
06497 for (i__ = 1; i__ <= i__2; ++i__) {
06498 r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06499 q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06500 }
06501 }
06502 for (i__ = 1; i__ <= i__2; ++i__)
06503 { q2[i__ + *n * q2_dim1] = 1.f;
06504 q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06505 }
06506 vector<float> fit_res;
06507 fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06508 return fit_res;
06509 }
06510 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)
06511 {
06512
06513 long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06514
06515
06516 long int i__, j, m, n1, ii, jj;
06517 double tmp;
06518 vector<float> p;
06519 --x;
06520 q_dim1 = *klm2d;
06521 q_offset = 1 + q_dim1;
06522 q -= q_offset;
06523 q1_dim1 = *klm2d;
06524 q1_offset = 1 + q1_dim1;
06525 q1 -= q1_offset;
06526 --s;
06527 --res;
06528 iu -= 3;
06529 cu -= 3;
06530
06531
06532 long int l = 0;
06533
06534
06535 m = *ks;
06536 n1 = *n + 1;
06537 if (*iswi == 1) {
06538 i__1 = n1;
06539 for (jj = 1; jj <= i__1; ++jj) {
06540 i__2 = *ks;
06541 for (ii = 1; ii <= i__2; ++ii) {
06542
06543
06544 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06545 ;
06546 }
06547 }
06548 } else if (*iswi == 2) {
06549 i__1 = *ks;
06550 for (ii = 1; ii <= i__1; ++ii) {
06551 i__2 = n1;
06552 for (jj = 1; jj <= i__2; ++jj) {
06553 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06554 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06555 }
06556 }
06557 } else if (*iswi == 3) {
06558 l = 2;
06559 i__1 = n1;
06560 for (jj = 1; jj <= i__1; ++jj) {
06561 i__2 = *ks + 2;
06562 for (ii = 1; ii <= i__2; ++ii) {
06563 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06564 }
06565 i__2 = *ks;
06566 for (ii = 1; ii <= i__2; ++ii) {
06567 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06568 }
06569 }
06570 } else if (*iswi == 4) {
06571 l = 2;
06572 i__1 = n1;
06573 for (jj = 1; jj <= i__1; ++jj) {
06574 i__2 = *ks + 2;
06575 for (ii = 1; ii <= i__2; ++ii) {
06576 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06577 }
06578 i__2 = *ks;
06579 for (ii = 1; ii <= i__2; ++ii) {
06580 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06581 }
06582 }
06583 } else if (*iswi == 5) {
06584 l = 1;
06585 i__1 = n1;
06586 for (jj = 1; jj <= i__1; ++jj) {
06587 i__2 = *ks + 1;
06588 for (ii = 1; ii <= i__2; ++ii) {
06589 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06590 }
06591 i__2 = *ks;
06592 for (ii = 1; ii <= i__2; ++ii) {
06593 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06594 }
06595 }
06596 } else if (*iswi == 6) {
06597 l = 1;
06598 i__1 = n1;
06599 for (jj = 1; jj <= i__1; ++jj) {
06600 i__2 = *ks + 1;
06601 for (ii = 1; ii <= i__2; ++ii) {
06602 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06603 }
06604 i__2 = *ks;
06605 for (ii = 1; ii <= i__2; ++ii) {
06606 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06607 }
06608 }
06609 } else if (*iswi == 7) {
06610 l = 3;
06611 i__1 = n1;
06612 for (jj = 1; jj <= i__1; ++jj) {
06613 i__2 = *ks + 3;
06614 for (ii = 1; ii <= i__2; ++ii) {
06615 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06616 }
06617 i__2 = *ks;
06618 for (ii = 1; ii <= i__2; ++ii) {
06619 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06620 }
06621 }
06622 } else if (*iswi == 8) {
06623 l = 4;
06624 i__1 = n1;
06625 for (jj = 1; jj <= i__1; ++jj) {
06626 i__2 = *ks + 4;
06627 for (ii = 1; ii <= i__2; ++ii) {
06628 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06629 }
06630 i__2 = *ks;
06631 for (ii = 1; ii <= i__2; ++ii) {
06632 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06633 }
06634 }
06635 }
06636
06637 Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06638 i__1 = *ks;
06639 int tmp__j=0;
06640 for (i__ = 1; i__ <= i__1; ++i__) {
06641 tmp = 0.f;
06642 i__2 = *n - 1;
06643 for (j = 1; j <= i__2; ++j) {
06644 tmp__j=j;
06645 tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06646 }
06647 tmp += x[*n];
06648 p.push_back(static_cast<float>(exp(tmp)));
06649 p.push_back(q1[i__ + q1_dim1]);
06650 }
06651 i__2=*n;
06652 for (i__=1;i__<=i__2;++i__)
06653 { p.push_back(static_cast<float>(x[i__]));}
06654 return p;
06655 }
06656 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06657 double *q, double *x, double *res, double *cu, long int *iu, double *s)
06658 {
06659
06660 long int q_dim1, q_offset, i__1, i__2;
06661 double d__1;
06662
06663 static long int i__, j;
06664 static double z__;
06665 static long int n1, n2, ia, ii, kk, in, nk, js;
06666 static double sn, zu, zv;
06667 static long int nk1, klm, nkl, jmn, jpn;
06668 static double cuv;
06669 static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06670 static float xmin;
06671 static double xmax;
06672 static long int iout;
06673 static double xsum;
06674 static long int iineg, maxit;
06675 static double toler;
06676 static float error;
06677 static double pivot;
06678 static long int kforce, iphase;
06679 static double tpivot;
06680
06681 --s;
06682 --res;
06683 iu -= 3;
06684 cu -= 3;
06685 --x;
06686 q_dim1 = *klm2d;
06687 q_offset = 1 + q_dim1;
06688 q -= q_offset;
06689
06690
06691 maxit = 500;
06692 kode = 0;
06693 toler = 1e-4f;
06694 iter = 0;
06695 n1 = *n + 1;
06696 n2 = *n + 2;
06697 nk = *n + *k;
06698 nk1 = nk + 1;
06699 nkl = nk + *l;
06700 nkl1 = nkl + 1;
06701 klm = *k + *l + *m;
06702 klm1 = klm + 1;
06703 klm2 = klm + 2;
06704 nklm = *n + klm;
06705 kforce = 1;
06706 iter = 0;
06707 js = 1;
06708 ia = 0;
06709
06710 i__1 = *n;
06711 for (j = 1; j <= i__1; ++j) {
06712 q[klm2 + j * q_dim1] = (double) j;
06713
06714 }
06715 i__1 = klm;
06716 for (i__ = 1; i__ <= i__1; ++i__) {
06717 q[i__ + n2 * q_dim1] = (double) (*n + i__);
06718 if (q[i__ + n1 * q_dim1] >= 0.f) {
06719 goto L30;
06720 }
06721 i__2 = n2;
06722 for (j = 1; j <= i__2; ++j) {
06723 q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06724
06725 }
06726 L30:
06727 ;
06728 }
06729
06730 iphase = 2;
06731 i__1 = nklm;
06732 for (j = 1; j <= i__1; ++j) {
06733 cu[(j << 1) + 1] = 0.f;
06734 cu[(j << 1) + 2] = 0.f;
06735 iu[(j << 1) + 1] = 0;
06736 iu[(j << 1) + 2] = 0;
06737
06738 }
06739 if (*l == 0) {
06740 goto L60;
06741 }
06742 i__1 = nkl;
06743 for (j = nk1; j <= i__1; ++j) {
06744 cu[(j << 1) + 1] = 1.f;
06745 cu[(j << 1) + 2] = 1.f;
06746 iu[(j << 1) + 1] = 1;
06747 iu[(j << 1) + 2] = 1;
06748
06749 }
06750 iphase = 1;
06751 L60:
06752 if (*m == 0) {
06753 goto L80;
06754 }
06755 i__1 = nklm;
06756 for (j = nkl1; j <= i__1; ++j) {
06757 cu[(j << 1) + 2] = 1.f;
06758 iu[(j << 1) + 2] = 1;
06759 jmn = j - *n;
06760 if (q[jmn + n2 * q_dim1] < 0.f) {
06761 iphase = 1;
06762 }
06763
06764 }
06765 L80:
06766 if (kode == 0) {
06767 goto L150;
06768 }
06769 i__1 = *n;
06770 for (j = 1; j <= i__1; ++j) {
06771 if ((d__1 = x[j]) < 0.) {
06772 goto L90;
06773 } else if (d__1 == 0) {
06774 goto L110;
06775 } else {
06776 goto L100;
06777 }
06778 L90:
06779 cu[(j << 1) + 1] = 1.f;
06780 iu[(j << 1) + 1] = 1;
06781 goto L110;
06782 L100:
06783 cu[(j << 1) + 2] = 1.f;
06784 iu[(j << 1) + 2] = 1;
06785 L110:
06786 ;
06787 }
06788 i__1 = *k;
06789 for (j = 1; j <= i__1; ++j) {
06790 jpn = j + *n;
06791 if ((d__1 = res[j]) < 0.) {
06792 goto L120;
06793 } else if (d__1 == 0) {
06794 goto L140;
06795 } else {
06796 goto L130;
06797 }
06798 L120:
06799 cu[(jpn << 1) + 1] = 1.f;
06800 iu[(jpn << 1) + 1] = 1;
06801 if (q[j + n2 * q_dim1] > 0.f) {
06802 iphase = 1;
06803 }
06804 goto L140;
06805 L130:
06806 cu[(jpn << 1) + 2] = 1.f;
06807 iu[(jpn << 1) + 2] = 1;
06808 if (q[j + n2 * q_dim1] < 0.f) {
06809 iphase = 1;
06810 }
06811 L140:
06812 ;
06813 }
06814 L150:
06815 if (iphase == 2) {
06816 goto L500;
06817 }
06818
06819 L160:
06820 i__1 = n1;
06821 for (j = js; j <= i__1; ++j) {
06822 xsum = 0.;
06823 i__2 = klm;
06824 for (i__ = 1; i__ <= i__2; ++i__) {
06825 ii = (long int) q[i__ + n2 * q_dim1];
06826 if (ii < 0) {
06827 goto L170;
06828 }
06829 z__ = cu[(ii << 1) + 1];
06830 goto L180;
06831 L170:
06832 iineg = -ii;
06833 z__ = cu[(iineg << 1) + 2];
06834 L180:
06835 xsum += q[i__ + j * q_dim1] * z__;
06836
06837
06838 }
06839 q[klm1 + j * q_dim1] = xsum;
06840
06841 }
06842 i__1 = *n;
06843 for (j = js; j <= i__1; ++j) {
06844 ii = (long int) q[klm2 + j * q_dim1];
06845 if (ii < 0) {
06846 goto L210;
06847 }
06848 z__ = cu[(ii << 1) + 1];
06849 goto L220;
06850 L210:
06851 iineg = -ii;
06852 z__ = cu[(iineg << 1) + 2];
06853 L220:
06854 q[klm1 + j * q_dim1] -= z__;
06855
06856 }
06857
06858 L240:
06859 xmax = 0.f;
06860 if (js > *n) {
06861 goto L490;
06862 }
06863 i__1 = *n;
06864 for (j = js; j <= i__1; ++j) {
06865 zu = q[klm1 + j * q_dim1];
06866 ii = (long int) q[klm2 + j * q_dim1];
06867 if (ii > 0) {
06868 goto L250;
06869 }
06870 ii = -ii;
06871 zv = zu;
06872 zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06873 goto L260;
06874 L250:
06875 zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06876 L260:
06877 if (kforce == 1 && ii > *n) {
06878 goto L280;
06879 }
06880 if (iu[(ii << 1) + 1] == 1) {
06881 goto L270;
06882 }
06883 if (zu <= xmax) {
06884 goto L270;
06885 }
06886 xmax = zu;
06887 in = j;
06888 L270:
06889 if (iu[(ii << 1) + 2] == 1) {
06890 goto L280;
06891 }
06892 if (zv <= xmax) {
06893 goto L280;
06894 }
06895 xmax = zv;
06896 in = j;
06897 L280:
06898 ;
06899 }
06900 if (xmax <= toler) {
06901 goto L490;
06902 }
06903 if (q[klm1 + in * q_dim1] == xmax) {
06904 goto L300;
06905 }
06906 i__1 = klm2;
06907 for (i__ = 1; i__ <= i__1; ++i__) {
06908 q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06909
06910 }
06911 q[klm1 + in * q_dim1] = xmax;
06912
06913 L300:
06914 if (iphase == 1 || ia == 0) {
06915 goto L330;
06916 }
06917 xmax = 0.f;
06918 i__1 = ia;
06919 for (i__ = 1; i__ <= i__1; ++i__) {
06920 z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06921 if (z__ <= xmax) {
06922 goto L310;
06923 }
06924 xmax = z__;
06925 iout = i__;
06926 L310:
06927 ;
06928 }
06929 if (xmax <= toler) {
06930 goto L330;
06931 }
06932 i__1 = n2;
06933 for (j = 1; j <= i__1; ++j) {
06934 z__ = q[ia + j * q_dim1];
06935 q[ia + j * q_dim1] = q[iout + j * q_dim1];
06936 q[iout + j * q_dim1] = z__;
06937
06938 }
06939 iout = ia;
06940 --ia;
06941 pivot = q[iout + in * q_dim1];
06942 goto L420;
06943 L330:
06944 kk = 0;
06945 i__1 = klm;
06946 for (i__ = 1; i__ <= i__1; ++i__) {
06947 z__ = q[i__ + in * q_dim1];
06948 if (z__ <= toler) {
06949 goto L340;
06950 }
06951 ++kk;
06952 res[kk] = q[i__ + n1 * q_dim1] / z__;
06953 s[kk] = (double) i__;
06954 L340:
06955 ;
06956 }
06957 L350:
06958 if (kk > 0) {
06959 goto L360;
06960 }
06961 kode = 2;
06962 goto L590;
06963 L360:
06964 xmin = static_cast<float>( res[1] );
06965 iout = (long int) s[1];
06966 j = 1;
06967 if (kk == 1) {
06968 goto L380;
06969 }
06970 i__1 = kk;
06971 for (i__ = 2; i__ <= i__1; ++i__) {
06972 if (res[i__] >= xmin) {
06973 goto L370;
06974 }
06975 j = i__;
06976 xmin = static_cast<float>( res[i__] );
06977 iout = (long int) s[i__];
06978 L370:
06979 ;
06980 }
06981 res[j] = res[kk];
06982 s[j] = s[kk];
06983 L380:
06984 --kk;
06985 pivot = q[iout + in * q_dim1];
06986 ii = (long int) q[iout + n2 * q_dim1];
06987 if (iphase == 1) {
06988 goto L400;
06989 }
06990 if (ii < 0) {
06991 goto L390;
06992 }
06993 if (iu[(ii << 1) + 2] == 1) {
06994 goto L420;
06995 }
06996 goto L400;
06997 L390:
06998 iineg = -ii;
06999 if (iu[(iineg << 1) + 1] == 1) {
07000 goto L420;
07001 }
07002
07003 L400:
07004 ii = abs(ii);
07005 cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
07006 if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
07007 goto L420;
07008 }
07009
07010 i__1 = n1;
07011 for (j = js; j <= i__1; ++j) {
07012 z__ = q[iout + j * q_dim1];
07013 q[klm1 + j * q_dim1] -= z__ * cuv;
07014 q[iout + j * q_dim1] = -z__;
07015
07016 }
07017 q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
07018 goto L350;
07019
07020 L420:
07021 if (iter < maxit) {
07022 goto L430;
07023 }
07024 kode = 3;
07025 goto L590;
07026 L430:
07027 ++iter;
07028 i__1 = n1;
07029 for (j = js; j <= i__1; ++j) {
07030 if (j != in) {
07031 q[iout + j * q_dim1] /= pivot;
07032 }
07033
07034 }
07035
07036
07037
07038
07039
07040
07041
07042
07043 i__1 = n1;
07044 for (j = js; j <= i__1; ++j) {
07045 if (j == in) {
07046 goto L460;
07047 }
07048 z__ = -q[iout + j * q_dim1];
07049 i__2 = klm1;
07050 for (i__ = 1; i__ <= i__2; ++i__) {
07051 if (i__ != iout) {
07052 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
07053 }
07054
07055 }
07056 L460:
07057 ;
07058 }
07059 tpivot = -pivot;
07060 i__1 = klm1;
07061 for (i__ = 1; i__ <= i__1; ++i__) {
07062 if (i__ != iout) {
07063 q[i__ + in * q_dim1] /= tpivot;
07064 }
07065
07066 }
07067 q[iout + in * q_dim1] = 1.f / pivot;
07068 z__ = q[iout + n2 * q_dim1];
07069 q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
07070 q[klm2 + in * q_dim1] = z__;
07071 ii = (long int) abs(z__);
07072 if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
07073 goto L240;
07074 }
07075 i__1 = klm2;
07076 for (i__ = 1; i__ <= i__1; ++i__) {
07077 z__ = q[i__ + in * q_dim1];
07078 q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
07079 q[i__ + js * q_dim1] = z__;
07080
07081 }
07082 ++js;
07083 goto L240;
07084
07085 L490:
07086 if (kforce == 0) {
07087 goto L580;
07088 }
07089 if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
07090 goto L500;
07091 }
07092 kforce = 0;
07093 goto L240;
07094
07095 L500:
07096 iphase = 2;
07097 i__1 = nklm;
07098 for (j = 1; j <= i__1; ++j) {
07099 cu[(j << 1) + 1] = 0.f;
07100 cu[(j << 1) + 2] = 0.f;
07101
07102 }
07103 i__1 = nk;
07104 for (j = n1; j <= i__1; ++j) {
07105 cu[(j << 1) + 1] = 1.f;
07106 cu[(j << 1) + 2] = 1.f;
07107
07108 }
07109 i__1 = klm;
07110 for (i__ = 1; i__ <= i__1; ++i__) {
07111 ii = (long int) q[i__ + n2 * q_dim1];
07112 if (ii > 0) {
07113 goto L530;
07114 }
07115 ii = -ii;
07116 if (iu[(ii << 1) + 2] == 0) {
07117 goto L560;
07118 }
07119 cu[(ii << 1) + 2] = 0.f;
07120 goto L540;
07121 L530:
07122 if (iu[(ii << 1) + 1] == 0) {
07123 goto L560;
07124 }
07125 cu[(ii << 1) + 1] = 0.f;
07126 L540:
07127 ++ia;
07128 i__2 = n2;
07129 for (j = 1; j <= i__2; ++j) {
07130 z__ = q[ia + j * q_dim1];
07131 q[ia + j * q_dim1] = q[i__ + j * q_dim1];
07132 q[i__ + j * q_dim1] = z__;
07133
07134 }
07135 L560:
07136 ;
07137 }
07138 goto L160;
07139 L570:
07140 if (q[klm1 + n1 * q_dim1] <= toler) {
07141 goto L500;
07142 }
07143 kode = 1;
07144 goto L590;
07145 L580:
07146 if (iphase == 1) {
07147 goto L570;
07148 }
07149
07150 kode = 0;
07151 L590:
07152 xsum = 0.;
07153 i__1 = *n;
07154 for (j = 1; j <= i__1; ++j) {
07155 x[j] = 0.f;
07156
07157 }
07158 i__1 = klm;
07159 for (i__ = 1; i__ <= i__1; ++i__) {
07160 res[i__] = 0.f;
07161
07162 }
07163 i__1 = klm;
07164 for (i__ = 1; i__ <= i__1; ++i__) {
07165 ii = (long int) q[i__ + n2 * q_dim1];
07166 sn = 1.f;
07167 if (ii > 0) {
07168 goto L620;
07169 }
07170 ii = -ii;
07171 sn = -1.f;
07172 L620:
07173 if (ii > *n) {
07174 goto L630;
07175 }
07176 x[ii] = sn * q[i__ + n1 * q_dim1];
07177 goto L640;
07178 L630:
07179 iimn = ii - *n;
07180 res[iimn] = sn * q[i__ + n1 * q_dim1];
07181 if (ii >= n1 && ii <= nk) {
07182 xsum += q[i__ + n1 * q_dim1];
07183 }
07184 L640:
07185 ;
07186 }
07187 error = (float)xsum;
07188 return;
07189 }
07190
07191 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07192 {
07193 int j,d;
07194 EMData * e = new EMData();
07195 float *eptr, *imgptr;
07196 imgptr = img->get_data();
07197 float SSE = 0.f;
07198 for (j = 0 ; j < N ; j++) {
07199 e->read_image(images,S[j]);
07200 eptr = e->get_data();
07201 for (d = 0; d < size; d++) {
07202 SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07203 }
07204 delete e;
07205 return SSE;
07206 }
07207
07208
07209 #define mymax(x,y) (((x)>(y))?(x):(y))
07210 #define mymin(x,y) (((x)<(y))?(x):(y))
07211 #define sign(x,y) (((((y)>0)?(1):(-1))*(y!=0))*(x))
07212
07213
07214 #define quadpi 3.141592653589793238462643383279502884197
07215 #define dgr_to_rad quadpi/180
07216 #define deg_to_rad quadpi/180
07217 #define rad_to_deg 180/quadpi
07218 #define rad_to_dgr 180/quadpi
07219 #define TRUE 1
07220 #define FALSE 0
07221
07222
07223 #define theta(i) theta [i-1]
07224 #define phi(i) phi [i-1]
07225 #define weight(i) weight [i-1]
07226 #define lband(i) lband [i-1]
07227 #define ts(i) ts [i-1]
07228 #define thetast(i) thetast [i-1]
07229 #define key(i) key [i-1]
07230
07231
07232 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07233 {
07234
07235 ENTERFUNC;
07236
07237 if ( th.size() != ph.size() ) {
07238 LOGERR("images not same size");
07239 throw ImageFormatException( "images not same size");
07240 }
07241
07242
07243 srand(10);
07244
07245 int i,*key;
07246 int len = th.size();
07247 double *theta,*phi,*weight;
07248 theta = (double*) calloc(len,sizeof(double));
07249 phi = (double*) calloc(len,sizeof(double));
07250 weight = (double*) calloc(len,sizeof(double));
07251 key = (int*) calloc(len,sizeof(int));
07252 const float *thptr, *phptr;
07253
07254 thptr = &th[0];
07255 phptr = &ph[0];
07256 for(i=1;i<=len;i++){
07257 key(i) = i;
07258 weight(i) = 0.0;
07259 }
07260
07261 for(i = 0;i<len;i++){
07262 theta[i] = thptr[i];
07263 phi[i] = phptr[i];
07264 }
07265
07266
07267 Util::hsortd(theta, phi, key, len, 1);
07268
07269
07270 Util::voronoi(phi, theta, weight, len);
07271
07272
07273 Util::hsortd(weight, weight, key, len, 2);
07274
07275 free(theta);
07276 free(phi);
07277 free(key);
07278 vector<double> wt;
07279 double count = 0;
07280 for(i=1; i<= len; i++)
07281 {
07282 wt.push_back(weight(i));
07283 count += weight(i);
07284 }
07285
07286
07287
07288
07289
07290
07291 free(weight);
07292
07293 EXITFUNC;
07294 return wt;
07295
07296 }
07297
07298 struct tmpstruct{
07299 double theta1,phi1;
07300 int key1;
07301 };
07302
07303 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07304 {
07305 ENTERFUNC;
07306 vector<tmpstruct> tmp(len);
07307 int i;
07308 for(i = 1;i<=len;i++)
07309 {
07310 tmp[i-1].theta1 = theta(i);
07311 tmp[i-1].phi1 = phi(i);
07312 tmp[i-1].key1 = key(i);
07313 }
07314
07315 if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07316 if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07317
07318 for(i = 1;i<=len;i++)
07319 {
07320 theta(i) = tmp[i-1].theta1;
07321 phi(i) = tmp[i-1].phi1;
07322 key(i) = tmp[i-1].key1;
07323 }
07324 EXITFUNC;
07325 }
07326
07327 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07328 {
07329 return(tmp1.theta1 < tmp2.theta1);
07330 }
07331
07332 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07333 {
07334 return(tmp1.key1 < tmp2.key1);
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
07483
07484
07485
07486
07487
07488
07489
07490
07491
07492
07493
07494
07495
07496
07497
07498
07499
07500
07501
07502
07503
07504
07505
07506
07507
07508
07509
07510
07511
07512
07513
07514
07515
07516
07517
07518
07519
07520
07521
07522
07523
07524
07525
07526
07527
07528
07529
07530
07531
07532
07533
07534
07535
07536
07537
07538
07539
07540
07541
07542
07543
07544
07545
07546
07547
07548
07549
07550
07551
07552
07553
07554
07555
07556
07557
07558
07559
07560
07561
07562
07563
07564
07565
07566
07567
07568
07569
07570
07571
07572
07573
07574
07575
07576
07577
07578
07579
07580
07581
07582
07583
07584
07585
07586
07587
07588
07589
07590
07591
07592
07593
07594
07595
07596
07597
07598
07599
07600
07601
07602
07603
07604
07605
07606
07607
07608
07609
07610
07611
07612
07613
07614
07615
07616
07617
07618
07619
07620
07621
07622
07623
07624
07625
07626
07627
07628
07629
07630
07631
07632
07633
07634
07635
07636
07637
07638
07639
07640
07641
07642
07643
07644
07645
07646
07647
07648
07649
07650
07651
07652
07653
07654
07655 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07656 {
07657
07658 ENTERFUNC;
07659
07660 int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07661 int nt6, n, ier, nout, lnew, mdup, nd;
07662 int i,k,mt,status;
07663
07664
07665 double *ds, *x, *y, *z;
07666 double tol = 1.0e-8;
07667 double dtol = 15;
07668 double a;
07669
07670
07671
07672
07673
07674
07675
07676
07677
07678 n = nt + nt;
07679
07680 nt6 = n*6;
07681
07682 list = (int*)calloc(nt6,sizeof(int));
07683 lptr = (int*)calloc(nt6,sizeof(int));
07684 lend = (int*)calloc(n ,sizeof(int));
07685 iwk = (int*)calloc(n ,sizeof(int));
07686 good = (int*)calloc(n ,sizeof(int));
07687 key = (int*)calloc(n ,sizeof(int));
07688 indx = (int*)calloc(n ,sizeof(int));
07689 lcnt = (int*)calloc(n ,sizeof(int));
07690
07691 ds = (double*) calloc(n,sizeof(double));
07692 x = (double*) calloc(n,sizeof(double));
07693 y = (double*) calloc(n,sizeof(double));
07694 z = (double*) calloc(n,sizeof(double));
07695
07696 if (list == NULL ||
07697 lptr == NULL ||
07698 lend == NULL ||
07699 iwk == NULL ||
07700 good == NULL ||
07701 key == NULL ||
07702 indx == NULL ||
07703 lcnt == NULL ||
07704 x == NULL ||
07705 y == NULL ||
07706 z == NULL ||
07707 ds == NULL) {
07708 printf("memory allocation failure!\n");
07709 exit(1);
07710 }
07711
07712 bool colinear=true;
07713 while(colinear)
07714 {
07715
07716 L1:
07717 for(i = 0; i<nt; i++){
07718 x[i] = theta[i];
07719 y[i] = phi[i];
07720 x[nt+i] = 180.0 - x[i];
07721 y[nt+i] = 180.0 + y[i];
07722 }
07723
07724 Util::disorder2(x, y, key, n);
07725
07726
07727 double val;
07728 for(k=0; k<2; k++){
07729 for(i=k+1; i<3; i++){
07730 val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07731 if( val < dtol) {
07732 goto L1;
07733 }
07734 }
07735 }
07736
07737 Util::ang_to_xyz(x, y, z, n);
07738
07739
07740 bool dupnode=true;
07741 dupnode=true;
07742 while(dupnode)
07743 {
07744 for(k=0; k<2; k++){
07745 for(i=k+1; i<3; i++){
07746 if( x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07747 Util::flip23(x, y, z, key, k, n);
07748 continue;
07749 }
07750 }
07751 }
07752 dupnode = false;
07753 }
07754
07755
07756 ier = 0;
07757
07758 status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07759
07760 if (status != 0) {
07761 printf(" error in trmsh3 \n");
07762 exit(1);
07763 }
07764
07765 if (ier > 0) {
07766 printf("*** Error in TRMESH: duplicate nodes encountered ***\n");
07767 exit(1);
07768 }
07769
07770 mdup=n-nout;
07771 if (ier == -2) {
07772
07773 }
07774 else
07775 {
07776 colinear=false;
07777 }
07778 }
07779
07780
07781 Assert( ier != -2 );
07782
07783
07784 nd=0;
07785 for (k=1; k<=n; k++){
07786 if (indx[k-1]>0) {
07787 nd++;
07788 good[nd-1]=k;
07789 }
07790 }
07791
07792
07793
07794
07795 for(i = 1; i<=nout; i++) {
07796 k=good[i-1];
07797
07798 if (key[k-1] <= nt) {
07799
07800 a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07801 if (ier != 0){
07802
07803
07804 printf(" *** error in areav: ier = %d ***\n", ier);
07805 weight[key[k-1]-1] =-1.0;
07806 } else {
07807
07808 weight[key[k-1]-1]=a/lcnt[i-1];
07809 }
07810 }
07811 }
07812
07813
07814
07815 for(i = 1; i<=n; i++){
07816 mt =- indx[i-1];
07817 if (mt>0){
07818 k = good[mt-1];
07819
07820
07821
07822 if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07823 }
07824 }
07825
07826 free(list);
07827 free(lend);
07828 free(iwk);
07829 free(good);
07830 free(key);
07831 free(lptr);
07832 free(indx);
07833 free(lcnt);
07834 free(ds);
07835 free(x);
07836 free(y);
07837 free(z);
07838
07839
07840 EXITFUNC;
07841 }
07842
07843 void Util::disorder2(double *x,double *y, int *key, int len)
07844 {
07845 ENTERFUNC;
07846 int k, i;
07847 for(i=0; i<len; i++) key[i]=i+1;
07848
07849 for(i = 0; i<len;i++){
07850 k = rand()%len;
07851 std::swap(key[k], key[i]);
07852 std::swap(x[k], x[i]);
07853 std::swap(y[k], y[i]);
07854 }
07855 EXITFUNC;
07856 }
07857
07858 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07859 {
07860 ENTERFUNC;
07861 double costheta,sintheta,cosphi,sinphi;
07862 for(int i = 0; i<len; i++)
07863 {
07864 cosphi = cos(y[i]*dgr_to_rad);
07865 sinphi = sin(y[i]*dgr_to_rad);
07866 if(fabs(x[i]-90.0)< 1.0e-5){
07867 x[i] = cosphi;
07868 y[i] = sinphi;
07869 z[i] = 0.0;
07870 }
07871 else{
07872 costheta = cos(x[i]*dgr_to_rad);
07873 sintheta = sin(x[i]*dgr_to_rad);
07874 x[i] = cosphi*sintheta;
07875 y[i] = sinphi*sintheta;
07876 z[i] = costheta;
07877 }
07878 }
07879 EXITFUNC;
07880 }
07881
07882 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07883 {
07884 ENTERFUNC;
07885 int i = k;
07886 while( i == k ) i = rand()%len;
07887 std::swap(key[i], key[k]);
07888 std::swap(x[i], x[k]);
07889 std::swap(y[i], y[k]);
07890 std::swap(z[i], z[k]);
07891 EXITFUNC;
07892 }
07893
07894
07895 #undef mymax
07896 #undef mymin
07897 #undef sign
07898 #undef quadpi
07899 #undef dgr_to_rad
07900 #undef deg_to_rad
07901 #undef rad_to_deg
07902 #undef rad_to_dgr
07903 #undef TRUE
07904 #undef FALSE
07905 #undef theta
07906 #undef phi
07907 #undef weight
07908 #undef lband
07909 #undef ts
07910 #undef thetast
07911 #undef key
07912
07913
07914
07915
07916
07917
07918
07919
07920
07921
07922
07923 #define TRUE_ (1)
07924 #define FALSE_ (0)
07925 #define abs(x) ((x) >= 0 ? (x) : -(x))
07926
07927 struct stcom_{
07928 double y;
07929 };
07930 stcom_ stcom_1;
07931 #ifdef KR_headers
07932 double floor();
07933 int i_dnnt(x) double *x;
07934 #else
07935 int i_dnnt(double *x)
07936 #endif
07937 {
07938 return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07939 }
07940
07941
07942
07943
07944
07945 int Util::trmsh3_(int *n0, double *tol, double *x,
07946 double *y, double *z__, int *n, int *list, int *
07947 lptr, int *lend, int *lnew, int *indx, int *lcnt,
07948 int *near__, int *next, double *dist, int *ier)
07949 {
07950
07951 int i__1, i__2;
07952
07953
07954 static double d__;
07955 static int i__, j;
07956 static double d1, d2, d3;
07957 static int i0, lp, kt, ku, lpl, nku;
07958 static int nexti;
07959
07960
07961
07962
07963
07964
07965
07966
07967
07968
07969
07970
07971
07972
07973
07974
07975
07976
07977
07978
07979
07980
07981
07982
07983
07984
07985
07986
07987
07988
07989
07990
07991
07992
07993
07994
07995
07996
07997
07998
07999
08000
08001
08002
08003
08004
08005
08006
08007
08008
08009
08010
08011
08012
08013
08014
08015
08016
08017
08018
08019
08020
08021
08022
08023
08024
08025
08026
08027
08028
08029
08030
08031
08032
08033
08034
08035
08036
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
08066
08067
08068
08069
08070
08071
08072
08073
08074
08075
08076
08077
08078
08079
08080
08081
08082
08083
08084
08085
08086
08087
08088
08089
08090
08091
08092
08093
08094
08095
08096
08097
08098
08099
08100
08101
08102
08103
08104
08105
08106
08107
08108
08109
08110 --dist;
08111 --next;
08112 --near__;
08113 --indx;
08114 --lend;
08115 --z__;
08116 --y;
08117 --x;
08118 --list;
08119 --lptr;
08120 --lcnt;
08121
08122
08123 if (*n0 < 3) {
08124 *n = 0;
08125 *ier = -1;
08126 return 0;
08127 }
08128
08129
08130
08131 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
08132 z__[3])) {
08133
08134
08135
08136 list[1] = 3;
08137 lptr[1] = 2;
08138 list[2] = -2;
08139 lptr[2] = 1;
08140 lend[1] = 2;
08141
08142 list[3] = 1;
08143 lptr[3] = 4;
08144 list[4] = -3;
08145 lptr[4] = 3;
08146 lend[2] = 4;
08147
08148 list[5] = 2;
08149 lptr[5] = 6;
08150 list[6] = -1;
08151 lptr[6] = 5;
08152 lend[3] = 6;
08153
08154 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
08155 y[3], &z__[3])) {
08156
08157
08158
08159
08160
08161 list[1] = 2;
08162 lptr[1] = 2;
08163 list[2] = -3;
08164 lptr[2] = 1;
08165 lend[1] = 2;
08166
08167 list[3] = 3;
08168 lptr[3] = 4;
08169 list[4] = -1;
08170 lptr[4] = 3;
08171 lend[2] = 4;
08172
08173 list[5] = 1;
08174 lptr[5] = 6;
08175 list[6] = -2;
08176 lptr[6] = 5;
08177 lend[3] = 6;
08178
08179
08180 } else {
08181
08182
08183
08184 *n = 0;
08185 *ier = -2;
08186 return 0;
08187 }
08188
08189
08190
08191
08192
08193 *lnew = 7;
08194 indx[1] = 1;
08195 indx[2] = 2;
08196 indx[3] = 3;
08197 lcnt[1] = 1;
08198 lcnt[2] = 1;
08199 lcnt[3] = 1;
08200 if (*n0 == 3) {
08201 *n = 3;
08202 *ier = 0;
08203 return 0;
08204 }
08205
08206
08207
08208
08209
08210
08211
08212
08213
08214
08215
08216
08217
08218
08219
08220
08221
08222
08223
08224
08225
08226
08227
08228
08229
08230
08231
08232 near__[1] = 0;
08233 near__[2] = 0;
08234 near__[3] = 0;
08235 for (ku = *n0; ku >= 4; --ku) {
08236 d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08237 d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08238 d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08239 if (d1 <= d2 && d1 <= d3) {
08240 near__[ku] = 1;
08241 dist[ku] = d1;
08242 next[ku] = near__[1];
08243 near__[1] = ku;
08244 } else if (d2 <= d1 && d2 <= d3) {
08245 near__[ku] = 2;
08246 dist[ku] = d2;
08247 next[ku] = near__[2];
08248 near__[2] = ku;
08249 } else {
08250 near__[ku] = 3;
08251 dist[ku] = d3;
08252 next[ku] = near__[3];
08253 near__[3] = ku;
08254 }
08255
08256 }
08257
08258
08259
08260
08261 kt = 3;
08262 i__1 = *n0;
08263 for (ku = 4; ku <= i__1; ++ku) {
08264 nku = near__[ku];
08265
08266
08267
08268 i__ = nku;
08269 if (near__[i__] == ku) {
08270 near__[i__] = next[ku];
08271 } else {
08272 i__ = near__[i__];
08273 L2:
08274 i0 = i__;
08275 i__ = next[i0];
08276 if (i__ != ku) {
08277 goto L2;
08278 }
08279 next[i0] = next[ku];
08280 }
08281 near__[ku] = 0;
08282
08283
08284
08285 if (dist[ku] <= *tol - 1.) {
08286 indx[ku] = -nku;
08287 ++lcnt[nku];
08288 goto L6;
08289 }
08290
08291
08292
08293 ++kt;
08294 x[kt] = x[ku];
08295 y[kt] = y[ku];
08296 z__[kt] = z__[ku];
08297 indx[ku] = kt;
08298 lcnt[kt] = 1;
08299 addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08300 , lnew, ier);
08301 if (*ier != 0) {
08302 *n = 0;
08303 *ier = -3;
08304 return 0;
08305 }
08306
08307
08308
08309 lpl = lend[kt];
08310 lp = lpl;
08311 L3:
08312 lp = lptr[lp];
08313 j = (i__2 = list[lp], abs(i__2));
08314
08315
08316
08317
08318
08319
08320
08321 i__ = near__[j];
08322 L4:
08323 if (i__ == 0) {
08324 goto L5;
08325 }
08326 nexti = next[i__];
08327
08328
08329
08330
08331 d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08332 if (d__ < dist[i__]) {
08333
08334
08335
08336
08337
08338 near__[i__] = kt;
08339 dist[i__] = d__;
08340 if (i__ == near__[j]) {
08341 near__[j] = nexti;
08342 } else {
08343 next[i0] = nexti;
08344 }
08345 next[i__] = near__[kt];
08346 near__[kt] = i__;
08347 } else {
08348 i0 = i__;
08349 }
08350
08351
08352
08353 i__ = nexti;
08354 goto L4;
08355
08356
08357
08358 L5:
08359 if (lp != lpl) {
08360 goto L3;
08361 }
08362 L6:
08363 ;
08364 }
08365 *n = kt;
08366 *ier = 0;
08367 return 0;
08368 }
08369
08370
08371 int addnod_(int *nst, int *k, double *x,
08372 double *y, double *z__, int *list, int *lptr, int
08373 *lend, int *lnew, int *ier)
08374 {
08375
08376
08377 static double tol = 0.;
08378
08379
08380 int i__1;
08381
08382
08383 static int l;
08384 static double p[3], b1, b2, b3;
08385 static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08386 int swap_(int *, int *, int *,
08387 int *, int *, int *, int *, int *);
08388 static int lpo1s;
08389 int bdyadd_(int *, int *, int *,
08390 int *, int *, int *, int *), intadd_(int *,
08391 int *, int *, int *, int *, int *, int *,
08392 int *), trfind_(int *, double *, int *,
08393 double *, double *, double *, int *, int *,
08394 int *, double *, double *, double *, int *,
08395 int *, int *), covsph_(int *, int *, int *,
08396 int *, int *, int *);
08397 int lstptr_(int *, int *, int *, int *);
08398 long int swptst_(int *, int *, int *, int *,
08399 double *, double *, double *);
08400
08401
08402
08403
08404
08405
08406
08407
08408
08409
08410
08411
08412
08413
08414
08415
08416
08417
08418
08419
08420
08421
08422
08423
08424
08425
08426
08427
08428
08429
08430
08431
08432
08433
08434
08435
08436
08437
08438
08439
08440
08441
08442
08443
08444
08445
08446
08447
08448
08449
08450
08451
08452
08453
08454
08455
08456
08457
08458
08459
08460
08461
08462
08463
08464
08465
08466
08467
08468
08469
08470
08471
08472
08473
08474
08475
08476
08477
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 --lend;
08503 --z__;
08504 --y;
08505 --x;
08506 --list;
08507 --lptr;
08508
08509
08510
08511 kk = *k;
08512 if (kk < 4) {
08513 goto L3;
08514 }
08515
08516
08517 km1 = kk - 1;
08518 ist = *nst;
08519 if (ist < 1) {
08520 ist = km1;
08521 }
08522 p[0] = x[kk];
08523 p[1] = y[kk];
08524 p[2] = z__[kk];
08525
08526
08527
08528
08529 trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08530 , &b1, &b2, &b3, &i1, &i2, &i3);
08531
08532
08533
08534 if (i1 == 0) {
08535 goto L4;
08536 }
08537 l = i1;
08538 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08539 goto L5;
08540 }
08541 l = i2;
08542 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08543 goto L5;
08544 }
08545 if (i3 != 0) {
08546 l = i3;
08547 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08548 goto L5;
08549 }
08550 intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08551 } else {
08552 if (i1 != i2) {
08553 bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08554 } else {
08555 covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08556 }
08557 }
08558 *ier = 0;
08559
08560
08561
08562 lp = lend[kk];
08563 lpf = lptr[lp];
08564 io2 = list[lpf];
08565 lpo1 = lptr[lpf];
08566 io1 = (i__1 = list[lpo1], abs(i__1));
08567
08568
08569
08570 L1:
08571 lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08572 if (list[lp] < 0) {
08573 goto L2;
08574 }
08575 lp = lptr[lp];
08576 in1 = (i__1 = list[lp], abs(i__1));
08577
08578
08579
08580
08581 lpo1s = lpo1;
08582 if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08583 goto L2;
08584 }
08585 swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08586 if (lpo1 == 0) {
08587
08588
08589
08590
08591
08592
08593 lpo1 = lpo1s;
08594 goto L2;
08595 }
08596 io1 = in1;
08597 goto L1;
08598
08599
08600
08601
08602 L2:
08603 if (lpo1 == lpf || list[lpo1] < 0) {
08604 return 0;
08605 }
08606 io2 = io1;
08607 lpo1 = lptr[lpo1];
08608 io1 = (i__1 = list[lpo1], abs(i__1));
08609 goto L1;
08610
08611
08612
08613 L3:
08614 *ier = -1;
08615 return 0;
08616
08617
08618
08619 L4:
08620 *ier = -2;
08621 return 0;
08622
08623
08624
08625 L5:
08626 *ier = l;
08627 return 0;
08628 }
08629
08630 double angle_(double *v1, double *v2, double *v3)
08631 {
08632
08633 double ret_val;
08634
08635
08636
08637
08638
08639 static double a;
08640 static int i__;
08641 static double ca, s21, s23, u21[3], u23[3];
08642
08643
08644
08645
08646
08647
08648
08649
08650
08651
08652
08653
08654
08655
08656
08657
08658
08659
08660
08661
08662
08663
08664
08665
08666
08667
08668
08669
08670
08671
08672
08673
08674
08675
08676
08677
08678
08679
08680
08681
08682
08683
08684
08685
08686
08687
08688
08689
08690
08691
08692
08693
08694
08695
08696
08697
08698
08699
08700
08701 --v3;
08702 --v2;
08703 --v1;
08704
08705
08706 u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08707 u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08708 u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08709
08710 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08711 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08712 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08713
08714
08715
08716 s21 = 0.;
08717 s23 = 0.;
08718 for (i__ = 1; i__ <= 3; ++i__) {
08719 s21 += u21[i__ - 1] * u21[i__ - 1];
08720 s23 += u23[i__ - 1] * u23[i__ - 1];
08721
08722 }
08723
08724
08725
08726
08727 if (s21 == 0. || s23 == 0.) {
08728 ret_val = 0.;
08729 return ret_val;
08730 }
08731 s21 = sqrt(s21);
08732 s23 = sqrt(s23);
08733 for (i__ = 1; i__ <= 3; ++i__) {
08734 u21[i__ - 1] /= s21;
08735 u23[i__ - 1] /= s23;
08736
08737 }
08738
08739
08740
08741
08742
08743 ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08744 if (ca < -1.) {
08745 ca = -1.;
08746 }
08747 if (ca > 1.) {
08748 ca = 1.;
08749 }
08750 a = acos(ca);
08751
08752
08753
08754
08755 if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08756 , &v3[3])) {
08757 a = acos(-1.) * 2. - a;
08758 }
08759 ret_val = a;
08760 return ret_val;
08761 }
08762
08763 double areas_(double *v1, double *v2, double *v3)
08764 {
08765
08766 double ret_val;
08767
08768
08769
08770
08771
08772 static int i__;
08773 static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08774 ca2, ca3;
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 --v3;
08829 --v2;
08830 --v1;
08831
08832
08833 u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08834 u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08835 u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08836
08837 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08838 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08839 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08840
08841 u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08842 u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08843 u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08844
08845
08846
08847 s12 = 0.;
08848 s23 = 0.;
08849 s31 = 0.;
08850 for (i__ = 1; i__ <= 3; ++i__) {
08851 s12 += u12[i__ - 1] * u12[i__ - 1];
08852 s23 += u23[i__ - 1] * u23[i__ - 1];
08853 s31 += u31[i__ - 1] * u31[i__ - 1];
08854
08855 }
08856
08857
08858
08859
08860 if (s12 == 0. || s23 == 0. || s31 == 0.) {
08861 ret_val = 0.;
08862 return ret_val;
08863 }
08864 s12 = sqrt(s12);
08865 s23 = sqrt(s23);
08866 s31 = sqrt(s31);
08867 for (i__ = 1; i__ <= 3; ++i__) {
08868 u12[i__ - 1] /= s12;
08869 u23[i__ - 1] /= s23;
08870 u31[i__ - 1] /= s31;
08871
08872 }
08873
08874
08875
08876
08877
08878
08879
08880 ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08881 ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08882 ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08883 if (ca1 < -1.) {
08884 ca1 = -1.;
08885 }
08886 if (ca1 > 1.) {
08887 ca1 = 1.;
08888 }
08889 if (ca2 < -1.) {
08890 ca2 = -1.;
08891 }
08892 if (ca2 > 1.) {
08893 ca2 = 1.;
08894 }
08895 if (ca3 < -1.) {
08896 ca3 = -1.;
08897 }
08898 if (ca3 > 1.) {
08899 ca3 = 1.;
08900 }
08901 a1 = acos(ca1);
08902 a2 = acos(ca2);
08903 a3 = acos(ca3);
08904
08905
08906
08907 ret_val = a1 + a2 + a3 - acos(-1.);
08908 if (ret_val < 0.) {
08909 ret_val = 0.;
08910 }
08911 return ret_val;
08912 }
08913
08914
08915
08916 double Util::areav_(int *k, int *n, double *x, double *y,
08917 double *z__, int *list, int *lptr, int *lend, int
08918 *ier)
08919 {
08920
08921
08922 static double amax = 6.28;
08923
08924
08925 double ret_val;
08926
08927
08928 static double a, c0[3], c2[3], c3[3];
08929 static int n1, n2, n3;
08930 static double v1[3], v2[3], v3[3];
08931 static int lp, lpl, ierr;
08932 static double asum;
08933 static long int first;
08934
08935
08936
08937
08938
08939
08940
08941
08942
08943
08944
08945
08946
08947
08948
08949
08950
08951
08952
08953
08954
08955
08956
08957
08958
08959
08960
08961
08962
08963
08964
08965
08966
08967
08968
08969
08970
08971
08972
08973
08974
08975
08976
08977
08978
08979
08980
08981
08982
08983
08984
08985
08986
08987
08988
08989
08990
08991
08992 --lend;
08993 --z__;
08994 --y;
08995 --x;
08996 --list;
08997 --lptr;
08998
08999
09000
09001
09002
09003 if (*k < 1 || *k > *n || *n <= 3) {
09004 goto L11;
09005 }
09006
09007
09008
09009
09010
09011 n1 = *k;
09012 v1[0] = x[n1];
09013 v1[1] = y[n1];
09014 v1[2] = z__[n1];
09015 lpl = lend[n1];
09016 n3 = list[lpl];
09017 if (n3 < 0) {
09018 goto L12;
09019 }
09020 lp = lpl;
09021 first = TRUE_;
09022 asum = 0.;
09023
09024
09025
09026 L1:
09027 n2 = n3;
09028 lp = lptr[lp];
09029 n3 = list[lp];
09030 v2[0] = x[n2];
09031 v2[1] = y[n2];
09032 v2[2] = z__[n2];
09033 v3[0] = x[n3];
09034 v3[1] = y[n3];
09035 v3[2] = z__[n3];
09036 if (first) {
09037
09038
09039
09040
09041 circum_(v1, v2, v3, c3, &ierr);
09042 if (ierr != 0) {
09043 goto L13;
09044 }
09045 c0[0] = c3[0];
09046 c0[1] = c3[1];
09047 c0[2] = c3[2];
09048 first = FALSE_;
09049 } else {
09050
09051
09052
09053
09054 c2[0] = c3[0];
09055 c2[1] = c3[1];
09056 c2[2] = c3[2];
09057 circum_(v1, v2, v3, c3, &ierr);
09058 if (ierr != 0) {
09059 goto L13;
09060 }
09061 a = areas_(v1, c2, c3);
09062 if (a > amax) {
09063 goto L14;
09064 }
09065 asum += a;
09066 }
09067
09068
09069
09070 if (lp != lpl) {
09071 goto L1;
09072 }
09073
09074
09075
09076 a = areas_(v1, c3, c0);
09077 if (a > amax) {
09078 goto L14;
09079 }
09080 asum += a;
09081
09082
09083
09084 *ier = 0;
09085 ret_val = asum;
09086 return ret_val;
09087
09088
09089
09090 L11:
09091 *ier = 1;
09092 ret_val = 0.;
09093 return ret_val;
09094
09095
09096
09097 L12:
09098 *ier = 2;
09099 ret_val = 0.;
09100 return ret_val;
09101
09102
09103
09104 L13:
09105 *ier = 3;
09106 ret_val = 0.;
09107 return ret_val;
09108
09109
09110
09111 L14:
09112 *ier = 4;
09113 ret_val = 0.;
09114 return ret_val;
09115 }
09116
09117 double areav_new__(int *k, int *n, double *x, double *y,
09118 double *z__, int *list, int *lptr, int *lend, int
09119 *ier)
09120 {
09121
09122 double ret_val = 0;
09123
09124
09125
09126
09127
09128 static int m;
09129 static double c1[3], c2[3], c3[3];
09130 static int n1, n2, n3;
09131 static double v1[3], v2[3], v3[3];
09132 static int lp;
09133 static double c1s[3], c2s[3];
09134 static int lpl, ierr;
09135 static double asum;
09136 double angle_(double *, double *, double *);
09137 static float areav;
09138
09139
09140
09141
09142
09143
09144
09145
09146
09147
09148
09149
09150
09151
09152
09153
09154
09155
09156
09157
09158
09159
09160
09161
09162
09163
09164
09165
09166
09167
09168
09169
09170
09171
09172
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 --lend;
09200 --z__;
09201 --y;
09202 --x;
09203 --list;
09204 --lptr;
09205
09206
09207 if (*k < 1 || *k > *n || *n <= 3) {
09208 goto L11;
09209 }
09210
09211
09212
09213
09214
09215 n1 = *k;
09216 v1[0] = x[n1];
09217 v1[1] = y[n1];
09218 v1[2] = z__[n1];
09219 lpl = lend[n1];
09220 n3 = list[lpl];
09221 if (n3 < 0) {
09222 goto L12;
09223 }
09224 lp = lpl;
09225 m = 0;
09226 asum = 0.;
09227
09228
09229
09230 L1:
09231 ++m;
09232 n2 = n3;
09233 lp = lptr[lp];
09234 n3 = list[lp];
09235 v2[0] = x[n2];
09236 v2[1] = y[n2];
09237 v2[2] = z__[n2];
09238 v3[0] = x[n3];
09239 v3[1] = y[n3];
09240 v3[2] = z__[n3];
09241 if (m == 1) {
09242
09243
09244
09245
09246 circum_(v1, v2, v3, c2, &ierr);
09247 if (ierr != 0) {
09248 goto L13;
09249 }
09250 c1s[0] = c2[0];
09251 c1s[1] = c2[1];
09252 c1s[2] = c2[2];
09253 } else if (m == 2) {
09254
09255
09256
09257
09258 circum_(v1, v2, v3, c3, &ierr);
09259 if (ierr != 0) {
09260 goto L13;
09261 }
09262 c2s[0] = c3[0];
09263 c2s[1] = c3[1];
09264 c2s[2] = c3[2];
09265 } else {
09266
09267
09268
09269
09270
09271 c1[0] = c2[0];
09272 c1[1] = c2[1];
09273 c1[2] = c2[2];
09274 c2[0] = c3[0];
09275 c2[1] = c3[1];
09276 c2[2] = c3[2];
09277 circum_(v1, v2, v3, c3, &ierr);
09278 if (ierr != 0) {
09279 goto L13;
09280 }
09281 asum += angle_(c1, c2, c3);
09282 }
09283
09284
09285
09286 if (lp != lpl) {
09287 goto L1;
09288 }
09289
09290
09291
09292
09293 asum += angle_(c2, c3, c1s);
09294
09295
09296
09297
09298 asum += angle_(c3, c1s, c2s);
09299
09300
09301
09302 *ier = 0;
09303 ret_val = asum - (double) (m - 2) * acos(-1.);
09304 return ret_val;
09305
09306
09307
09308 L11:
09309 *ier = 1;
09310 areav = 0.f;
09311 return ret_val;
09312
09313
09314
09315 L12:
09316 *ier = 2;
09317 areav = 0.f;
09318 return ret_val;
09319
09320
09321
09322 L13:
09323 *ier = 3;
09324 areav = 0.f;
09325 return ret_val;
09326 }
09327
09328 int bdyadd_(int *kk, int *i1, int *i2, int *
09329 list, int *lptr, int *lend, int *lnew)
09330 {
09331 static int k, n1, n2, lp, lsav, nsav, next;
09332 int insert_(int *, int *, int *,
09333 int *, int *);
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
09370
09371
09372
09373
09374
09375
09376
09377
09378
09379
09380
09381
09382
09383
09384
09385
09386
09387
09388
09389
09390
09391
09392
09393
09394
09395
09396
09397
09398 --lend;
09399 --lptr;
09400 --list;
09401
09402
09403 k = *kk;
09404 n1 = *i1;
09405 n2 = *i2;
09406
09407
09408
09409 lp = lend[n1];
09410 lsav = lptr[lp];
09411 lptr[lp] = *lnew;
09412 list[*lnew] = -k;
09413 lptr[*lnew] = lsav;
09414 lend[n1] = *lnew;
09415 ++(*lnew);
09416 next = -list[lp];
09417 list[lp] = next;
09418 nsav = next;
09419
09420
09421
09422
09423 L1:
09424 lp = lend[next];
09425 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09426 if (next == n2) {
09427 goto L2;
09428 }
09429 next = -list[lp];
09430 list[lp] = next;
09431 goto L1;
09432
09433
09434
09435
09436 L2:
09437 lsav = *lnew;
09438 list[*lnew] = n1;
09439 lptr[*lnew] = *lnew + 1;
09440 ++(*lnew);
09441 next = nsav;
09442
09443 L3:
09444 if (next == n2) {
09445 goto L4;
09446 }
09447 list[*lnew] = next;
09448 lptr[*lnew] = *lnew + 1;
09449 ++(*lnew);
09450 lp = lend[next];
09451 next = list[lp];
09452 goto L3;
09453
09454 L4:
09455 list[*lnew] = -n2;
09456 lptr[*lnew] = lsav;
09457 lend[k] = *lnew;
09458 ++(*lnew);
09459 return 0;
09460 }
09461
09462 int bnodes_(int *n, int *list, int *lptr,
09463 int *lend, int *nodes, int *nb, int *na, int *nt)
09464 {
09465
09466 int i__1;
09467
09468
09469 static int k, n0, lp, nn, nst;
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
09506
09507
09508
09509
09510
09511
09512
09513
09514
09515
09516
09517
09518
09519
09520
09521
09522
09523
09524
09525
09526
09527
09528
09529
09530 --lend;
09531 --list;
09532 --lptr;
09533 --nodes;
09534
09535
09536 nn = *n;
09537
09538
09539
09540 i__1 = nn;
09541 for (nst = 1; nst <= i__1; ++nst) {
09542 lp = lend[nst];
09543 if (list[lp] < 0) {
09544 goto L2;
09545 }
09546
09547 }
09548
09549
09550
09551 *nb = 0;
09552 *na = (nn - 2) * 3;
09553 *nt = nn - (2<<1);
09554 return 0;
09555
09556
09557
09558
09559 L2:
09560 nodes[1] = nst;
09561 k = 1;
09562 n0 = nst;
09563
09564
09565
09566 L3:
09567 lp = lend[n0];
09568 lp = lptr[lp];
09569 n0 = list[lp];
09570 if (n0 == nst) {
09571 goto L4;
09572 }
09573 ++k;
09574 nodes[k] = n0;
09575 goto L3;
09576
09577
09578
09579 L4:
09580 *nb = k;
09581 *nt = (*n << 1) - *nb - 2;
09582 *na = *nt + *n - 1;
09583 return 0;
09584 }
09585
09586 int circle_(int *k, double *xc, double *yc,
09587 int *ier)
09588 {
09589
09590 int i__1;
09591
09592
09593
09594
09595
09596 static double a, c__;
09597 static int i__;
09598 static double s;
09599 static int k2, k3;
09600 static double x0, y0;
09601 static int kk, np1;
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
09629
09630
09631
09632
09633
09634
09635
09636
09637
09638
09639
09640
09641
09642
09643
09644
09645
09646
09647
09648
09649
09650
09651
09652
09653
09654
09655
09656
09657
09658
09659
09660
09661
09662
09663
09664
09665
09666 --yc;
09667 --xc;
09668
09669
09670 kk = *k;
09671 k2 = kk << 1;
09672 k3 = kk * 3;
09673 np1 = (kk << 2) + 1;
09674
09675
09676
09677
09678 if (kk < 1) {
09679 goto L2;
09680 }
09681 a = atan(1.) * 2. / (double) kk;
09682 c__ = cos(a);
09683 s = sin(a);
09684 x0 = 1.;
09685 y0 = 0.;
09686
09687
09688
09689
09690
09691 i__1 = kk;
09692 for (i__ = 1; i__ <= i__1; ++i__) {
09693 xc[i__] = x0;
09694 yc[i__] = y0;
09695 xc[i__ + kk] = -y0;
09696 yc[i__ + kk] = x0;
09697 xc[i__ + k2] = -x0;
09698 yc[i__ + k2] = -y0;
09699 xc[i__ + k3] = y0;
09700 yc[i__ + k3] = -x0;
09701
09702
09703
09704 x0 = c__ * x0 - s * y0;
09705 y0 = s * x0 + c__ * y0;
09706
09707 }
09708
09709
09710
09711
09712 xc[np1] = xc[1];
09713 yc[np1] = yc[1];
09714 *ier = 0;
09715 return 0;
09716
09717
09718
09719 L2:
09720 *ier = 1;
09721 return 0;
09722 }
09723
09724 int circum_(double *v1, double *v2, double *v3,
09725 double *c__, int *ier)
09726 {
09727
09728
09729
09730
09731 static int i__;
09732 static double e1[3], e2[3], cu[3], cnorm;
09733
09734
09735
09736
09737
09738
09739
09740
09741
09742
09743
09744
09745
09746
09747
09748
09749
09750
09751
09752
09753
09754
09755
09756
09757
09758
09759
09760
09761
09762
09763
09764
09765
09766
09767
09768
09769
09770
09771
09772
09773
09774
09775
09776
09777
09778
09779
09780
09781
09782
09783
09784
09785
09786
09787
09788
09789 --c__;
09790 --v3;
09791 --v2;
09792 --v1;
09793
09794
09795 for (i__ = 1; i__ <= 3; ++i__) {
09796 e1[i__ - 1] = v2[i__] - v1[i__];
09797 e2[i__ - 1] = v3[i__] - v1[i__];
09798
09799 }
09800
09801
09802
09803 cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09804 cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09805 cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09806 cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09807
09808
09809
09810
09811 if (cnorm != 0.) {
09812
09813
09814
09815 cnorm = sqrt(cnorm);
09816 for (i__ = 1; i__ <= 3; ++i__) {
09817 c__[i__] = cu[i__ - 1] / cnorm;
09818
09819 }
09820
09821
09822
09823
09824
09825
09826 if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09827 c__[1] = -c__[1];
09828 c__[2] = -c__[2];
09829 c__[3] = -c__[3];
09830 }
09831 *ier = 0;
09832 } else {
09833
09834
09835
09836 *ier = 1;
09837 }
09838 return 0;
09839 }
09840
09841 int covsph_(int *kk, int *n0, int *list, int
09842 *lptr, int *lend, int *lnew)
09843 {
09844 static int k, lp, nst, lsav, next;
09845 int insert_(int *, int *, int *,
09846 int *, int *);
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 --lend;
09905 --lptr;
09906 --list;
09907
09908
09909 k = *kk;
09910 nst = *n0;
09911
09912
09913
09914
09915
09916 next = nst;
09917 L1:
09918 lp = lend[next];
09919 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09920 next = -list[lp];
09921 list[lp] = next;
09922 if (next != nst) {
09923 goto L1;
09924 }
09925
09926
09927
09928
09929 lsav = *lnew;
09930 L2:
09931 lp = lend[next];
09932 list[*lnew] = next;
09933 lptr[*lnew] = *lnew + 1;
09934 ++(*lnew);
09935 next = list[lp];
09936 if (next != nst) {
09937 goto L2;
09938 }
09939
09940 lptr[*lnew - 1] = lsav;
09941 lend[k] = *lnew - 1;
09942 return 0;
09943 }
09944
09945
09946 int crlist_(int *n, int *ncol, double *x,
09947 double *y, double *z__, int *list, int *lend, int
09948 *lptr, int *lnew, int *ltri, int *listc, int *nb,
09949 double *xc, double *yc, double *zc, double *rc,
09950 int *ier)
09951 {
09952
09953 int i__1, i__2;
09954
09955
09956
09957
09958
09959 static double c__[3], t;
09960 static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09961 static double v1[3], v2[3], v3[3];
09962 static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09963 lpn;
09964 static long int swp;
09965 static int ierr;
09966 int lstptr_(int *, int *, int *, int *);
09967 long int swptst_(int *, int *, int *, int *,
09968 double *, double *, double *);
09969
09970
09971
09972
09973
09974
09975
09976
09977
09978
09979
09980
09981
09982
09983
09984
09985
09986
09987
09988
09989
09990
09991
09992
09993
09994
09995
09996
09997
09998
09999
10000
10001
10002
10003
10004
10005
10006
10007
10008
10009
10010
10011
10012
10013
10014
10015
10016
10017
10018
10019
10020
10021
10022
10023
10024
10025
10026
10027
10028
10029
10030
10031
10032
10033
10034
10035
10036
10037
10038
10039
10040
10041
10042
10043
10044
10045
10046
10047
10048
10049
10050
10051
10052
10053
10054
10055
10056
10057
10058
10059
10060
10061
10062
10063
10064
10065
10066
10067
10068
10069
10070
10071
10072
10073
10074
10075
10076
10077
10078
10079
10080
10081
10082
10083
10084
10085
10086
10087
10088
10089
10090
10091
10092
10093
10094
10095
10096
10097
10098
10099
10100
10101
10102
10103
10104
10105
10106
10107
10108
10109
10110
10111
10112
10113
10114
10115
10116
10117
10118
10119
10120
10121
10122
10123
10124
10125
10126
10127
10128
10129
10130
10131
10132
10133
10134
10135
10136
10137
10138
10139
10140
10141
10142
10143
10144
10145 --lend;
10146 --z__;
10147 --y;
10148 --x;
10149 ltri -= 7;
10150 --list;
10151 --lptr;
10152 --listc;
10153 --xc;
10154 --yc;
10155 --zc;
10156 --rc;
10157
10158
10159 nn = *n;
10160 *nb = 0;
10161 nt = 0;
10162 if (nn < 3) {
10163 goto L21;
10164 }
10165
10166
10167
10168 i__1 = nn;
10169 for (n1 = 1; n1 <= i__1; ++n1) {
10170 lp = lend[n1];
10171 if (list[lp] < 0) {
10172 goto L2;
10173 }
10174
10175 }
10176
10177
10178
10179 goto L9;
10180
10181
10182
10183
10184
10185
10186
10187
10188 L2:
10189 n2 = -list[lp];
10190 lp = lptr[lp];
10191 n3 = list[lp];
10192
10193
10194
10195
10196
10197
10198 L3:
10199 ++nt;
10200 if (nt <= *ncol) {
10201 ltri[nt * 6 + 1] = n1;
10202 ltri[nt * 6 + 2] = n2;
10203 ltri[nt * 6 + 3] = n3;
10204 ltri[nt * 6 + 4] = nt + 1;
10205 ltri[nt * 6 + 5] = nt - 1;
10206 ltri[nt * 6 + 6] = 0;
10207 }
10208 n1 = n2;
10209 lp = lend[n1];
10210 n2 = -list[lp];
10211 if (n2 != n3) {
10212 goto L3;
10213 }
10214
10215 *nb = nt + 2;
10216 if (*ncol < nt) {
10217 goto L22;
10218 }
10219 ltri[nt * 6 + 4] = 0;
10220 if (nt == 1) {
10221 goto L7;
10222 }
10223
10224
10225
10226
10227
10228
10229
10230 L4:
10231 swp = FALSE_;
10232 i__1 = nt - 1;
10233 for (kt1 = 1; kt1 <= i__1; ++kt1) {
10234 for (i3 = 1; i3 <= 3; ++i3) {
10235 kt2 = ltri[i3 + 3 + kt1 * 6];
10236 if (kt2 <= kt1) {
10237 goto L5;
10238 }
10239
10240
10241
10242
10243 if (i3 == 1) {
10244 i1 = 2;
10245 i2 = 3;
10246 } else if (i3 == 2) {
10247 i1 = 3;
10248 i2 = 1;
10249 } else {
10250 i1 = 1;
10251 i2 = 2;
10252 }
10253 n1 = ltri[i1 + kt1 * 6];
10254 n2 = ltri[i2 + kt1 * 6];
10255 n3 = ltri[i3 + kt1 * 6];
10256
10257
10258
10259
10260 if (ltri[kt2 * 6 + 4] == kt1) {
10261 i4 = 1;
10262 } else if (ltri[kt2 * 6 + 5] == kt1) {
10263 i4 = 2;
10264 } else {
10265 i4 = 3;
10266 }
10267 n4 = ltri[i4 + kt2 * 6];
10268
10269
10270
10271
10272
10273 if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10274 goto L5;
10275 }
10276
10277
10278
10279
10280 swp = TRUE_;
10281 kt11 = ltri[i1 + 3 + kt1 * 6];
10282 kt12 = ltri[i2 + 3 + kt1 * 6];
10283 if (i4 == 1) {
10284 i2 = 2;
10285 i1 = 3;
10286 } else if (i4 == 2) {
10287 i2 = 3;
10288 i1 = 1;
10289 } else {
10290 i2 = 1;
10291 i1 = 2;
10292 }
10293 kt21 = ltri[i1 + 3 + kt2 * 6];
10294 kt22 = ltri[i2 + 3 + kt2 * 6];
10295 ltri[kt1 * 6 + 1] = n4;
10296 ltri[kt1 * 6 + 2] = n3;
10297 ltri[kt1 * 6 + 3] = n1;
10298 ltri[kt1 * 6 + 4] = kt12;
10299 ltri[kt1 * 6 + 5] = kt22;
10300 ltri[kt1 * 6 + 6] = kt2;
10301 ltri[kt2 * 6 + 1] = n3;
10302 ltri[kt2 * 6 + 2] = n4;
10303 ltri[kt2 * 6 + 3] = n2;
10304 ltri[kt2 * 6 + 4] = kt21;
10305 ltri[kt2 * 6 + 5] = kt11;
10306 ltri[kt2 * 6 + 6] = kt1;
10307
10308
10309
10310 if (kt11 != 0) {
10311 i4 = 4;
10312 if (ltri[kt11 * 6 + 4] != kt1) {
10313 i4 = 5;
10314 if (ltri[kt11 * 6 + 5] != kt1) {
10315 i4 = 6;
10316 }
10317 }
10318 ltri[i4 + kt11 * 6] = kt2;
10319 }
10320 if (kt22 != 0) {
10321 i4 = 4;
10322 if (ltri[kt22 * 6 + 4] != kt2) {
10323 i4 = 5;
10324 if (ltri[kt22 * 6 + 5] != kt2) {
10325 i4 = 6;
10326 }
10327 }
10328 ltri[i4 + kt22 * 6] = kt1;
10329 }
10330 L5:
10331 ;
10332 }
10333
10334 }
10335 if (swp) {
10336 goto L4;
10337 }
10338
10339
10340
10341
10342 L7:
10343 i__1 = nt;
10344 for (kt = 1; kt <= i__1; ++kt) {
10345 n1 = ltri[kt * 6 + 1];
10346 n2 = ltri[kt * 6 + 2];
10347 n3 = ltri[kt * 6 + 3];
10348 v1[0] = x[n1];
10349 v1[1] = y[n1];
10350 v1[2] = z__[n1];
10351 v2[0] = x[n2];
10352 v2[1] = y[n2];
10353 v2[2] = z__[n2];
10354 v3[0] = x[n3];
10355 v3[1] = y[n3];
10356 v3[2] = z__[n3];
10357 circum_(v2, v1, v3, c__, &ierr);
10358 if (ierr != 0) {
10359 goto L23;
10360 }
10361
10362
10363
10364
10365 xc[kt] = -c__[0];
10366 yc[kt] = -c__[1];
10367 zc[kt] = -c__[2];
10368 t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10369 if (t < -1.) {
10370 t = -1.;
10371 }
10372 if (t > 1.) {
10373 t = 1.;
10374 }
10375 rc[kt] = acos(t);
10376
10377 }
10378
10379
10380
10381
10382
10383
10384 L9:
10385 kt = nt;
10386
10387
10388
10389 nm2 = nn - 2;
10390 i__1 = nm2;
10391 for (n1 = 1; n1 <= i__1; ++n1) {
10392 lpl = lend[n1];
10393 lp = lpl;
10394 n3 = list[lp];
10395
10396
10397
10398
10399 L10:
10400 lp = lptr[lp];
10401 n2 = n3;
10402 n3 = (i__2 = list[lp], abs(i__2));
10403 if (n2 <= n1 || n3 <= n1) {
10404 goto L11;
10405 }
10406 ++kt;
10407
10408
10409
10410 v1[0] = x[n1];
10411 v1[1] = y[n1];
10412 v1[2] = z__[n1];
10413 v2[0] = x[n2];
10414 v2[1] = y[n2];
10415 v2[2] = z__[n2];
10416 v3[0] = x[n3];
10417 v3[1] = y[n3];
10418 v3[2] = z__[n3];
10419 circum_(v1, v2, v3, c__, &ierr);
10420 if (ierr != 0) {
10421 goto L23;
10422 }
10423
10424
10425
10426 xc[kt] = c__[0];
10427 yc[kt] = c__[1];
10428 zc[kt] = c__[2];
10429 t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10430 if (t < -1.) {
10431 t = -1.;
10432 }
10433 if (t > 1.) {
10434 t = 1.;
10435 }
10436 rc[kt] = acos(t);
10437
10438
10439
10440
10441
10442 lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10443 listc[lpn] = kt;
10444 lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10445 listc[lpn] = kt;
10446 lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10447 listc[lpn] = kt;
10448 L11:
10449 if (lp != lpl) {
10450 goto L10;
10451 }
10452
10453 }
10454 if (nt == 0) {
10455 goto L20;
10456 }
10457
10458
10459
10460
10461
10462
10463 kt1 = 0;
10464 L13:
10465 ++kt1;
10466 if (ltri[kt1 * 6 + 4] == 0) {
10467 i1 = 2;
10468 i2 = 3;
10469 i3 = 1;
10470 goto L14;
10471 } else if (ltri[kt1 * 6 + 5] == 0) {
10472 i1 = 3;
10473 i2 = 1;
10474 i3 = 2;
10475 goto L14;
10476 } else if (ltri[kt1 * 6 + 6] == 0) {
10477 i1 = 1;
10478 i2 = 2;
10479 i3 = 3;
10480 goto L14;
10481 }
10482 goto L13;
10483 L14:
10484 n1 = ltri[i1 + kt1 * 6];
10485 n0 = n1;
10486
10487
10488
10489
10490
10491
10492
10493
10494
10495 L15:
10496 lp = lend[n1];
10497 lpn = lptr[lp];
10498 listc[lp] = kt1;
10499
10500
10501
10502 L16:
10503 kt2 = ltri[i2 + 3 + kt1 * 6];
10504 if (kt2 != 0) {
10505
10506
10507
10508 lptr[lp] = *lnew;
10509 lp = *lnew;
10510 listc[lp] = kt2;
10511 ++(*lnew);
10512
10513
10514
10515
10516 kt1 = kt2;
10517 if (ltri[kt1 * 6 + 1] == n1) {
10518 i1 = 1;
10519 i2 = 2;
10520 i3 = 3;
10521 } else if (ltri[kt1 * 6 + 2] == n1) {
10522 i1 = 2;
10523 i2 = 3;
10524 i3 = 1;
10525 } else {
10526 i1 = 3;
10527 i2 = 1;
10528 i3 = 2;
10529 }
10530 goto L16;
10531 }
10532
10533
10534
10535
10536
10537
10538
10539 lptr[lp] = lpn;
10540 n1 = ltri[i3 + kt1 * 6];
10541 if (n1 != n0) {
10542 i4 = i3;
10543 i3 = i2;
10544 i2 = i1;
10545 i1 = i4;
10546 goto L15;
10547 }
10548
10549
10550
10551 L20:
10552 *ier = 0;
10553 return 0;
10554
10555
10556
10557 L21:
10558 *ier = 1;
10559 return 0;
10560
10561
10562
10563 L22:
10564 *ier = 2;
10565 return 0;
10566
10567
10568
10569 L23:
10570 *ier = 3;
10571 return 0;
10572 }
10573
10574 int delarc_(int *n, int *io1, int *io2, int *
10575 list, int *lptr, int *lend, int *lnew, int *ier)
10576 {
10577
10578 int i__1;
10579
10580
10581 static int n1, n2, n3, lp, lph, lpl;
10582 int delnb_(int *, int *, int *,
10583 int *, int *, int *, int *, int *);
10584 int lstptr_(int *, int *, int *, int *);
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
10649
10650
10651
10652
10653
10654
10655
10656
10657
10658 --lend;
10659 --list;
10660 --lptr;
10661
10662
10663 n1 = *io1;
10664 n2 = *io2;
10665
10666
10667
10668
10669
10670 if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10671 *ier = 1;
10672 return 0;
10673 }
10674
10675 lpl = lend[n2];
10676 if (-list[lpl] != n1) {
10677 n1 = n2;
10678 n2 = *io1;
10679 lpl = lend[n2];
10680 if (-list[lpl] != n1) {
10681 *ier = 2;
10682 return 0;
10683 }
10684 }
10685
10686
10687
10688
10689
10690 lpl = lend[n1];
10691 lp = lptr[lpl];
10692 lp = lptr[lp];
10693 n3 = (i__1 = list[lp], abs(i__1));
10694 lpl = lend[n3];
10695 if (list[lpl] <= 0) {
10696 *ier = 3;
10697 return 0;
10698 }
10699
10700
10701
10702
10703
10704
10705 delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10706 if (lph < 0) {
10707 *ier = 4;
10708 return 0;
10709 }
10710
10711
10712
10713
10714 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10715
10716
10717
10718
10719 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10720 lend[n3] = lp;
10721 list[lp] = -n1;
10722
10723
10724
10725 *ier = 0;
10726 return 0;
10727 }
10728
10729 int delnb_(int *n0, int *nb, int *n, int *
10730 list, int *lptr, int *lend, int *lnew, int *lph)
10731 {
10732
10733 int i__1;
10734
10735
10736 static int i__, lp, nn, lpb, lpl, lpp, lnw;
10737
10738
10739
10740
10741
10742
10743
10744
10745
10746
10747
10748
10749
10750
10751
10752
10753
10754
10755
10756
10757
10758
10759
10760
10761
10762
10763
10764
10765
10766
10767
10768
10769
10770
10771
10772
10773
10774
10775
10776
10777
10778
10779
10780
10781
10782
10783
10784
10785
10786
10787
10788
10789
10790
10791
10792
10793
10794
10795
10796
10797
10798
10799
10800
10801
10802
10803
10804
10805
10806
10807
10808 --lend;
10809 --list;
10810 --lptr;
10811
10812
10813 nn = *n;
10814
10815
10816
10817 if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10818 *lph = -1;
10819 return 0;
10820 }
10821
10822
10823
10824
10825
10826
10827
10828 lpl = lend[*n0];
10829 lpp = lpl;
10830 lpb = lptr[lpp];
10831 L1:
10832 if (list[lpb] == *nb) {
10833 goto L2;
10834 }
10835 lpp = lpb;
10836 lpb = lptr[lpp];
10837 if (lpb != lpl) {
10838 goto L1;
10839 }
10840
10841
10842
10843 if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10844 *lph = -2;
10845 return 0;
10846 }
10847
10848
10849
10850
10851
10852 lend[*n0] = lpp;
10853 lp = lend[*nb];
10854 if (list[lp] < 0) {
10855 list[lpp] = -list[lpp];
10856 }
10857 goto L3;
10858
10859
10860
10861
10862
10863 L2:
10864 lp = lend[*nb];
10865 if (list[lp] < 0 && list[lpl] > 0) {
10866 lend[*n0] = lpp;
10867 list[lpp] = -list[lpp];
10868 }
10869
10870
10871
10872
10873 L3:
10874 lptr[lpp] = lptr[lpb];
10875 lnw = *lnew - 1;
10876 list[lpb] = list[lnw];
10877 lptr[lpb] = lptr[lnw];
10878 for (i__ = nn; i__ >= 1; --i__) {
10879 if (lend[i__] == lnw) {
10880 lend[i__] = lpb;
10881 goto L5;
10882 }
10883
10884 }
10885
10886 L5:
10887 i__1 = lnw - 1;
10888 for (i__ = 1; i__ <= i__1; ++i__) {
10889 if (lptr[i__] == lnw) {
10890 lptr[i__] = lpb;
10891 }
10892
10893 }
10894
10895
10896
10897 *lnew = lnw;
10898 *lph = lpb;
10899 return 0;
10900 }
10901
10902 int delnod_(int *k, int *n, double *x,
10903 double *y, double *z__, int *list, int *lptr, int
10904 *lend, int *lnew, int *lwk, int *iwk, int *ier)
10905 {
10906
10907 int i__1;
10908
10909
10910 static int i__, j, n1, n2;
10911 static double x1, x2, y1, y2, z1, z2;
10912 static int nl, lp, nn, nr;
10913 static double xl, yl, zl, xr, yr, zr;
10914 static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10915 static long int bdry;
10916 static int ierr, lwkl;
10917 int swap_(int *, int *, int *,
10918 int *, int *, int *, int *, int *), delnb_(
10919 int *, int *, int *, int *, int *, int *,
10920 int *, int *);
10921 int nbcnt_(int *, int *);
10922 int optim_(double *, double *, double
10923 *, int *, int *, int *, int *, int *, int
10924 *, int *);
10925 static int nfrst;
10926 int lstptr_(int *, int *, int *, int *);
10927
10928
10929
10930
10931
10932
10933
10934
10935
10936
10937
10938
10939
10940
10941
10942
10943
10944
10945
10946
10947
10948
10949
10950
10951
10952
10953
10954
10955
10956
10957
10958
10959
10960
10961
10962
10963
10964
10965
10966
10967
10968
10969
10970
10971
10972
10973
10974
10975
10976
10977
10978
10979
10980
10981
10982
10983
10984
10985
10986
10987
10988
10989
10990
10991
10992
10993
10994
10995
10996
10997
10998
10999
11000
11001
11002
11003
11004
11005
11006
11007
11008
11009
11010
11011
11012
11013
11014
11015
11016
11017
11018
11019
11020
11021
11022
11023
11024
11025
11026
11027
11028
11029
11030
11031
11032
11033
11034
11035
11036
11037
11038
11039
11040
11041
11042
11043
11044
11045
11046
11047
11048
11049
11050
11051
11052
11053
11054
11055
11056
11057
11058
11059
11060
11061
11062
11063
11064
11065
11066
11067
11068
11069
11070 iwk -= 3;
11071 --lend;
11072 --lptr;
11073 --list;
11074 --z__;
11075 --y;
11076 --x;
11077
11078
11079 n1 = *k;
11080 nn = *n;
11081 if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
11082 goto L21;
11083 }
11084 lpl = lend[n1];
11085 lpf = lptr[lpl];
11086 nnb = nbcnt_(&lpl, &lptr[1]);
11087 bdry = list[lpl] < 0;
11088 if (bdry) {
11089 ++nnb;
11090 }
11091 if (nnb < 3) {
11092 goto L23;
11093 }
11094 lwkl = *lwk;
11095 *lwk = nnb - 3;
11096 if (lwkl < *lwk) {
11097 goto L22;
11098 }
11099 iwl = 0;
11100 if (nnb == 3) {
11101 goto L3;
11102 }
11103
11104
11105
11106
11107
11108
11109
11110 x1 = x[n1];
11111 y1 = y[n1];
11112 z1 = z__[n1];
11113 nfrst = list[lpf];
11114 nr = nfrst;
11115 xr = x[nr];
11116 yr = y[nr];
11117 zr = z__[nr];
11118 lp = lptr[lpf];
11119 n2 = list[lp];
11120 x2 = x[n2];
11121 y2 = y[n2];
11122 z2 = z__[n2];
11123 lp = lptr[lp];
11124
11125
11126
11127 L1:
11128 nl = (i__1 = list[lp], abs(i__1));
11129 if (nl == nfrst && bdry) {
11130 goto L3;
11131 }
11132 xl = x[nl];
11133 yl = y[nl];
11134 zl = z__[nl];
11135
11136
11137
11138
11139
11140
11141 lpl2 = lend[n2];
11142 if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
11143 list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
11144 z2)))) {
11145
11146
11147
11148 nr = n2;
11149 xr = x2;
11150 yr = y2;
11151 zr = z2;
11152 goto L2;
11153 }
11154
11155
11156
11157
11158
11159
11160
11161
11162 swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11163 if (lp21 == 0) {
11164 nr = n2;
11165 xr = x2;
11166 yr = y2;
11167 zr = z2;
11168 goto L2;
11169 }
11170 ++iwl;
11171 if (nl <= n1) {
11172 iwk[(iwl << 1) + 1] = nl;
11173 } else {
11174 iwk[(iwl << 1) + 1] = nl - 1;
11175 }
11176 if (nr <= n1) {
11177 iwk[(iwl << 1) + 2] = nr;
11178 } else {
11179 iwk[(iwl << 1) + 2] = nr - 1;
11180 }
11181
11182
11183
11184 lpl = lend[n1];
11185 --nnb;
11186 if (nnb == 3) {
11187 goto L3;
11188 }
11189 lpf = lptr[lpl];
11190 nfrst = list[lpf];
11191 lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11192 if (nr == nfrst) {
11193 goto L2;
11194 }
11195
11196
11197
11198
11199
11200
11201
11202 n2 = nr;
11203 x2 = xr;
11204 y2 = yr;
11205 z2 = zr;
11206 lp21 = lptr[lp21];
11207 lp21 = lptr[lp21];
11208 nr = (i__1 = list[lp21], abs(i__1));
11209 xr = x[nr];
11210 yr = y[nr];
11211 zr = z__[nr];
11212 goto L1;
11213
11214
11215
11216 L2:
11217 if (n2 == nfrst) {
11218 goto L3;
11219 }
11220 n2 = nl;
11221 x2 = xl;
11222 y2 = yl;
11223 z2 = zl;
11224 lp = lptr[lp];
11225 goto L1;
11226
11227
11228
11229
11230
11231
11232
11233
11234
11235
11236 L3:
11237 if (! bdry) {
11238 if (nnb > 3) {
11239 bdry = TRUE_;
11240 } else {
11241 lpf = lptr[lpl];
11242 nr = list[lpf];
11243 lp = lptr[lpf];
11244 n2 = list[lp];
11245 nl = list[lpl];
11246 bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11247 x[n2], &y[n2], &z__[n2]);
11248 }
11249 if (bdry) {
11250
11251
11252
11253
11254
11255
11256 i__1 = nn;
11257 for (i__ = 1; i__ <= i__1; ++i__) {
11258 if (list[lend[i__]] < 0) {
11259 bdry = FALSE_;
11260 goto L5;
11261 }
11262
11263 }
11264 list[lpl] = -list[lpl];
11265 ++nnb;
11266 }
11267 }
11268 L5:
11269 if (! bdry && nnb > 3) {
11270 goto L24;
11271 }
11272
11273
11274
11275
11276 lp = lpl;
11277 lnw = *lnew;
11278
11279
11280
11281 L6:
11282 lp = lptr[lp];
11283 n2 = (i__1 = list[lp], abs(i__1));
11284 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11285 if (lph < 0) {
11286 goto L23;
11287 }
11288
11289
11290
11291 if (lpl == lnw) {
11292 lpl = lph;
11293 }
11294 if (lp == lnw) {
11295 lp = lph;
11296 }
11297 if (lp != lpl) {
11298 goto L6;
11299 }
11300
11301
11302
11303
11304
11305 --nn;
11306 if (n1 > nn) {
11307 goto L9;
11308 }
11309 i__1 = nn;
11310 for (i__ = n1; i__ <= i__1; ++i__) {
11311 x[i__] = x[i__ + 1];
11312 y[i__] = y[i__ + 1];
11313 z__[i__] = z__[i__ + 1];
11314 lend[i__] = lend[i__ + 1];
11315
11316 }
11317
11318 i__1 = lnw - 1;
11319 for (i__ = 1; i__ <= i__1; ++i__) {
11320 if (list[i__] > n1) {
11321 --list[i__];
11322 }
11323 if (list[i__] < -n1) {
11324 ++list[i__];
11325 }
11326
11327 }
11328
11329
11330
11331
11332
11333
11334
11335
11336
11337
11338
11339 L9:
11340 if (bdry) {
11341 --nnb;
11342 }
11343 lpn = lpl;
11344 i__1 = nnb;
11345 for (j = 1; j <= i__1; ++j) {
11346 --lnw;
11347 lp = lpn;
11348 lpn = lptr[lp];
11349 list[lp] = list[lnw];
11350 lptr[lp] = lptr[lnw];
11351 if (lptr[lpn] == lnw) {
11352 lptr[lpn] = lp;
11353 }
11354 if (lpn == lnw) {
11355 lpn = lp;
11356 }
11357 for (i__ = nn; i__ >= 1; --i__) {
11358 if (lend[i__] == lnw) {
11359 lend[i__] = lp;
11360 goto L11;
11361 }
11362
11363 }
11364
11365 L11:
11366 for (i__ = lnw - 1; i__ >= 1; --i__) {
11367 if (lptr[i__] == lnw) {
11368 lptr[i__] = lp;
11369 }
11370
11371 }
11372
11373 }
11374
11375
11376
11377
11378
11379 *n = nn;
11380 *lnew = lnw;
11381 if (iwl > 0) {
11382 nit = iwl << 2;
11383 optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11384 nit, &iwk[3], &ierr);
11385 if (ierr != 0 && ierr != 1) {
11386 goto L25;
11387 }
11388 if (ierr == 1) {
11389 goto L26;
11390 }
11391 }
11392
11393
11394
11395 *ier = 0;
11396 return 0;
11397
11398
11399
11400 L21:
11401 *ier = 1;
11402 return 0;
11403
11404
11405
11406 L22:
11407 *ier = 2;
11408 return 0;
11409
11410
11411
11412
11413 L23:
11414 *ier = 3;
11415 return 0;
11416
11417
11418
11419 L24:
11420 *ier = 4;
11421 return 0;
11422
11423
11424
11425 L25:
11426 *ier = 5;
11427
11428
11429
11430 return 0;
11431
11432
11433
11434 L26:
11435 *ier = 6;
11436 return 0;
11437 }
11438
11439 int drwarc_(int *, double *p, double *q,
11440 double *tol, int *nseg)
11441 {
11442
11443 int i__1;
11444 double d__1;
11445
11446
11447
11448
11449
11450 static int i__, k;
11451 static double s, p1[3], p2[3], u1, u2, v1, v2;
11452 static int na;
11453 static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11454
11455
11456
11457
11458
11459
11460
11461
11462
11463
11464
11465
11466
11467
11468
11469
11470
11471
11472
11473
11474
11475
11476
11477
11478
11479
11480
11481
11482
11483
11484
11485
11486
11487
11488
11489
11490
11491
11492
11493
11494
11495
11496
11497
11498
11499
11500
11501
11502
11503
11504
11505
11506
11507
11508
11509
11510
11511
11512
11513
11514
11515
11516
11517
11518
11519
11520
11521
11522
11523
11524
11525
11526
11527
11528
11529
11530
11531 --q;
11532 --p;
11533
11534
11535 enrm = 0.;
11536 for (i__ = 1; i__ <= 3; ++i__) {
11537 pm[i__ - 1] = p[i__] + q[i__];
11538 enrm += pm[i__ - 1] * pm[i__ - 1];
11539
11540 }
11541 if (enrm == 0.) {
11542 goto L5;
11543 }
11544 enrm = sqrt(enrm);
11545 pm[0] /= enrm;
11546 pm[1] /= enrm;
11547 pm[2] /= enrm;
11548
11549
11550
11551
11552 u1 = p[1];
11553 v1 = p[2];
11554 u2 = q[1];
11555 v2 = q[2];
11556 um = pm[0];
11557 vm = pm[1];
11558
11559
11560
11561
11562
11563
11564 du = u2 - u1;
11565 dv = v2 - v1;
11566 enrm = du * du + dv * dv;
11567 if (enrm == 0.) {
11568 goto L5;
11569 }
11570 err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11571
11572
11573
11574
11575
11576 na = (int) (err / *tol + 1.);
11577
11578
11579
11580
11581
11582 s = 1. / (double) na;
11583 for (i__ = 1; i__ <= 3; ++i__) {
11584 dp[i__ - 1] = s * (q[i__] - p[i__]);
11585 pm[i__ - 1] = p[i__];
11586 p1[i__ - 1] = p[i__];
11587
11588 }
11589
11590
11591
11592
11593 i__1 = na - 1;
11594 for (k = 1; k <= i__1; ++k) {
11595 enrm = 0.;
11596 for (i__ = 1; i__ <= 3; ++i__) {
11597 pm[i__ - 1] += dp[i__ - 1];
11598 enrm += pm[i__ - 1] * pm[i__ - 1];
11599
11600 }
11601 if (enrm == 0.) {
11602 goto L5;
11603 }
11604 enrm = sqrt(enrm);
11605 p2[0] = pm[0] / enrm;
11606 p2[1] = pm[1] / enrm;
11607 p2[2] = pm[2] / enrm;
11608
11609
11610 p1[0] = p2[0];
11611 p1[1] = p2[1];
11612 p1[2] = p2[2];
11613
11614 }
11615
11616
11617
11618
11619 *nseg = na;
11620 return 0;
11621
11622
11623
11624 L5:
11625 *nseg = 0;
11626 return 0;
11627 }
11628
11629 int edge_(int *in1, int *in2, double *x,
11630 double *y, double *z__, int *lwk, int *iwk, int *
11631 list, int *lptr, int *lend, int *ier)
11632 {
11633
11634 int i__1;
11635
11636
11637 static int i__, n0, n1, n2;
11638 static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11639 static int nl, lp, nr;
11640 static double dp12;
11641 static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11642 static double dp1l, dp2l, dp1r, dp2r;
11643 static int ierr;
11644 int swap_(int *, int *, int *,
11645 int *, int *, int *, int *, int *);
11646 static int next, iwcp1, n1lst, iwend;
11647 int optim_(double *, double *, double
11648 *, int *, int *, int *, int *, int *, int
11649 *, int *);
11650 static int n1frst;
11651
11652
11653
11654
11655
11656
11657
11658
11659
11660
11661
11662
11663
11664
11665
11666
11667
11668
11669
11670
11671
11672
11673
11674
11675
11676
11677
11678
11679
11680
11681
11682
11683
11684
11685
11686
11687
11688
11689
11690
11691
11692
11693
11694
11695
11696
11697
11698
11699
11700
11701
11702
11703
11704
11705
11706
11707
11708
11709
11710
11711
11712
11713
11714
11715
11716
11717
11718
11719
11720
11721
11722
11723
11724
11725
11726
11727
11728
11729
11730
11731
11732
11733
11734
11735
11736
11737
11738
11739
11740
11741
11742
11743
11744
11745
11746
11747
11748
11749
11750
11751
11752
11753
11754
11755
11756
11757
11758
11759
11760
11761
11762
11763
11764
11765
11766
11767
11768
11769
11770
11771
11772
11773
11774
11775
11776
11777
11778
11779
11780
11781
11782
11783
11784
11785
11786
11787
11788 --lend;
11789 --lptr;
11790 --list;
11791 iwk -= 3;
11792 --z__;
11793 --y;
11794 --x;
11795
11796
11797 n1 = *in1;
11798 n2 = *in2;
11799 iwend = *lwk;
11800 if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11801 goto L31;
11802 }
11803
11804
11805
11806
11807 lpl = lend[n1];
11808 n0 = (i__1 = list[lpl], abs(i__1));
11809 lp = lpl;
11810 L1:
11811 if (n0 == n2) {
11812 goto L30;
11813 }
11814 lp = lptr[lp];
11815 n0 = list[lp];
11816 if (lp != lpl) {
11817 goto L1;
11818 }
11819
11820
11821
11822 iwl = 0;
11823 nit = 0;
11824
11825
11826
11827 L2:
11828 x1 = x[n1];
11829 y1 = y[n1];
11830 z1 = z__[n1];
11831 x2 = x[n2];
11832 y2 = y[n2];
11833 z2 = z__[n2];
11834
11835
11836
11837
11838
11839
11840
11841
11842
11843
11844 lpl = lend[n1];
11845 n1lst = list[lpl];
11846 lp = lptr[lpl];
11847 n1frst = list[lp];
11848 nl = n1frst;
11849 if (n1lst < 0) {
11850 goto L4;
11851 }
11852
11853
11854
11855
11856 L3:
11857 if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11858 goto L4;
11859 }
11860 lp = lptr[lp];
11861 nl = list[lp];
11862 if (nl != n1frst) {
11863 goto L3;
11864 }
11865
11866
11867
11868 goto L5;
11869
11870
11871
11872
11873 L4:
11874 nr = nl;
11875 lp = lptr[lp];
11876 nl = (i__1 = list[lp], abs(i__1));
11877 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11878
11879
11880
11881
11882
11883 dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11884 dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11885 dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11886 dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11887 dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11888 if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11889 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11890 goto L6;
11891 }
11892
11893
11894
11895
11896
11897 if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11898 goto L5;
11899 }
11900 }
11901
11902
11903
11904 if (nl != n1frst) {
11905 goto L4;
11906 }
11907
11908
11909
11910
11911
11912
11913
11914 L5:
11915 if (nit > 0) {
11916 goto L33;
11917 }
11918 nit = 1;
11919 n1 = n2;
11920 n2 = *in1;
11921 goto L2;
11922
11923
11924
11925
11926 L6:
11927 ++iwl;
11928 if (iwl > iwend) {
11929 goto L32;
11930 }
11931 iwk[(iwl << 1) + 1] = nl;
11932 iwk[(iwl << 1) + 2] = nr;
11933
11934
11935
11936 lpl = lend[nl];
11937 lp = lptr[lpl];
11938
11939
11940
11941
11942 L7:
11943 if (list[lp] == nr) {
11944 goto L8;
11945 }
11946 lp = lptr[lp];
11947 if (lp != lpl) {
11948 goto L7;
11949 }
11950
11951
11952
11953
11954 if (list[lp] != nr) {
11955 goto L33;
11956 }
11957
11958
11959
11960
11961 L8:
11962 lp = lptr[lp];
11963 next = (i__1 = list[lp], abs(i__1));
11964 if (next == n2) {
11965 goto L9;
11966 }
11967
11968
11969
11970 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11971 nl = next;
11972 } else {
11973 nr = next;
11974 }
11975 goto L6;
11976
11977
11978
11979
11980 L9:
11981 *lwk = iwl;
11982 iwend = iwl;
11983
11984
11985
11986
11987
11988
11989
11990
11991 iwf = 1;
11992
11993
11994
11995
11996
11997 L10:
11998 lft = 0;
11999 n0 = n1;
12000 x0 = x1;
12001 y0 = y1;
12002 z0 = z1;
12003 nl = iwk[(iwf << 1) + 1];
12004 nr = iwk[(iwf << 1) + 2];
12005 iwc = iwf;
12006
12007
12008
12009
12010 L11:
12011 if (iwc == iwl) {
12012 goto L21;
12013 }
12014 iwcp1 = iwc + 1;
12015 next = iwk[(iwcp1 << 1) + 1];
12016 if (next != nl) {
12017 goto L16;
12018 }
12019 next = iwk[(iwcp1 << 1) + 2];
12020
12021
12022
12023
12024 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
12025 z__[next])) {
12026 goto L14;
12027 }
12028 if (lft >= 0) {
12029 goto L12;
12030 }
12031 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
12032 z__[next])) {
12033 goto L14;
12034 }
12035
12036
12037
12038 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12039 iwk[(iwc << 1) + 1] = n0;
12040 iwk[(iwc << 1) + 2] = next;
12041 goto L15;
12042
12043
12044
12045
12046
12047 L12:
12048 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12049 i__1 = iwl;
12050 for (i__ = iwcp1; i__ <= i__1; ++i__) {
12051 iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
12052 iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
12053
12054 }
12055 iwk[(iwl << 1) + 1] = n0;
12056 iwk[(iwl << 1) + 2] = next;
12057 --iwl;
12058 nr = next;
12059 goto L11;
12060
12061
12062
12063 L14:
12064 n0 = nr;
12065 x0 = x[n0];
12066 y0 = y[n0];
12067 z0 = z__[n0];
12068 lft = 1;
12069
12070
12071
12072 L15:
12073 nr = next;
12074 ++iwc;
12075 goto L11;
12076
12077
12078
12079
12080 L16:
12081 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
12082 z__[next])) {
12083 goto L19;
12084 }
12085 if (lft <= 0) {
12086 goto L17;
12087 }
12088 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
12089 z__[next])) {
12090 goto L19;
12091 }
12092
12093
12094
12095 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12096 iwk[(iwc << 1) + 1] = next;
12097 iwk[(iwc << 1) + 2] = n0;
12098 goto L20;
12099
12100
12101
12102
12103
12104 L17:
12105 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12106 i__1 = iwf;
12107 for (i__ = iwc - 1; i__ >= i__1; --i__) {
12108 iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
12109 iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
12110
12111 }
12112 iwk[(iwf << 1) + 1] = n0;
12113 iwk[(iwf << 1) + 2] = next;
12114 ++iwf;
12115 goto L20;
12116
12117
12118
12119 L19:
12120 n0 = nl;
12121 x0 = x[n0];
12122 y0 = y[n0];
12123 z0 = z__[n0];
12124 lft = -1;
12125
12126
12127
12128 L20:
12129 nl = next;
12130 ++iwc;
12131 goto L11;
12132
12133
12134
12135 L21:
12136 if (n0 == n1) {
12137 goto L24;
12138 }
12139 if (lft < 0) {
12140 goto L22;
12141 }
12142
12143
12144
12145 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
12146 goto L10;
12147 }
12148
12149
12150
12151
12152 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12153 iwk[(iwl << 1) + 1] = n0;
12154 iwk[(iwl << 1) + 2] = n2;
12155 --iwl;
12156 goto L10;
12157
12158
12159
12160 L22:
12161 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12162 goto L10;
12163 }
12164
12165
12166
12167
12168 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12169 i__ = iwl;
12170 L23:
12171 iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12172 iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12173 --i__;
12174 if (i__ > iwf) {
12175 goto L23;
12176 }
12177 iwk[(iwf << 1) + 1] = n0;
12178 iwk[(iwf << 1) + 2] = n2;
12179 ++iwf;
12180 goto L10;
12181
12182
12183
12184
12185 L24:
12186 swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12187 iwk[(iwc << 1) + 1] = 0;
12188 iwk[(iwc << 1) + 2] = 0;
12189
12190
12191
12192 *ier = 0;
12193 if (iwc > 1) {
12194
12195
12196
12197 nit = iwc - (1<<2);
12198 i__1 = iwc - 1;
12199 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12200 nit, &iwk[3], &ierr);
12201 if (ierr != 0 && ierr != 1) {
12202 goto L34;
12203 }
12204 if (ierr == 1) {
12205 *ier = 5;
12206 }
12207 }
12208 if (iwc < iwend) {
12209
12210
12211
12212 nit = iwend - (iwc<<2);
12213 i__1 = iwend - iwc;
12214 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12215 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12216 if (ierr != 0 && ierr != 1) {
12217 goto L34;
12218 }
12219 if (ierr == 1) {
12220 goto L35;
12221 }
12222 }
12223 if (*ier == 5) {
12224 goto L35;
12225 }
12226
12227
12228
12229 return 0;
12230
12231
12232
12233 L30:
12234 *ier = 0;
12235 return 0;
12236
12237
12238
12239 L31:
12240 *ier = 1;
12241 return 0;
12242
12243
12244
12245 L32:
12246 *ier = 2;
12247 return 0;
12248
12249
12250
12251
12252 L33:
12253 *ier = 3;
12254
12255
12256
12257
12258 return 0;
12259
12260
12261
12262 L34:
12263 *ier = 4;
12264
12265
12266
12267 return 0;
12268
12269
12270
12271 L35:
12272 *ier = 5;
12273 return 0;
12274 }
12275
12276 int getnp_(double *x, double *y, double *z__,
12277 int *list, int *lptr, int *lend, int *l, int *
12278 npts, double *df, int *ier)
12279 {
12280
12281 int i__1, i__2;
12282
12283
12284 static int i__, n1;
12285 static double x1, y1, z1;
12286 static int nb, ni, lp, np, lm1;
12287 static double dnb, dnp;
12288 static int lpl;
12289
12290
12291
12292
12293
12294
12295
12296
12297
12298
12299
12300
12301
12302
12303
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
12348
12349
12350
12351
12352
12353
12354
12355
12356
12357
12358
12359
12360
12361
12362
12363
12364
12365
12366
12367
12368 --x;
12369 --y;
12370 --z__;
12371 --list;
12372 --lptr;
12373 --lend;
12374 --npts;
12375
12376
12377 lm1 = *l - 1;
12378 if (lm1 < 1) {
12379 goto L6;
12380 }
12381 *ier = 0;
12382
12383
12384
12385 n1 = npts[1];
12386 x1 = x[n1];
12387 y1 = y[n1];
12388 z1 = z__[n1];
12389 i__1 = lm1;
12390 for (i__ = 1; i__ <= i__1; ++i__) {
12391 ni = npts[i__];
12392 lend[ni] = -lend[ni];
12393
12394 }
12395
12396
12397
12398
12399
12400 dnp = 2.;
12401
12402
12403
12404 i__1 = lm1;
12405 for (i__ = 1; i__ <= i__1; ++i__) {
12406 ni = npts[i__];
12407 lpl = -lend[ni];
12408 lp = lpl;
12409
12410
12411
12412 L2:
12413 nb = (i__2 = list[lp], abs(i__2));
12414 if (lend[nb] < 0) {
12415 goto L3;
12416 }
12417
12418
12419
12420
12421 dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12422 if (dnb >= dnp) {
12423 goto L3;
12424 }
12425 np = nb;
12426 dnp = dnb;
12427 L3:
12428 lp = lptr[lp];
12429 if (lp != lpl) {
12430 goto L2;
12431 }
12432
12433 }
12434 npts[*l] = np;
12435 *df = dnp;
12436
12437
12438
12439 i__1 = lm1;
12440 for (i__ = 1; i__ <= i__1; ++i__) {
12441 ni = npts[i__];
12442 lend[ni] = -lend[ni];
12443
12444 }
12445 return 0;
12446
12447
12448
12449 L6:
12450 *ier = 1;
12451 return 0;
12452 }
12453
12454 int insert_(int *k, int *lp, int *list, int *
12455 lptr, int *lnew)
12456 {
12457 static int lsav;
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 --lptr;
12502 --list;
12503
12504
12505 lsav = lptr[*lp];
12506 lptr[*lp] = *lnew;
12507 list[*lnew] = *k;
12508 lptr[*lnew] = lsav;
12509 ++(*lnew);
12510 return 0;
12511 }
12512
12513 long int inside_(double *p, int *lv, double *xv, double *yv,
12514 double *zv, int *nv, int *listv, int *ier)
12515 {
12516
12517
12518 static double eps = .001;
12519
12520
12521 int i__1;
12522 long int ret_val = 0;
12523
12524
12525
12526
12527
12528 static double b[3], d__;
12529 static int k, n;
12530 static double q[3];
12531 static int i1, i2, k0;
12532 static double v1[3], v2[3], cn[3], bp, bq;
12533 static int ni;
12534 static double pn[3], qn[3], vn[3];
12535 static int imx;
12536 static long int lft1, lft2, even;
12537 static int ierr;
12538 static long int pinr, qinr;
12539 static double qnrm, vnrm;
12540 int intrsc_(double *, double *,
12541 double *, double *, int *);
12542
12543
12544
12545
12546
12547
12548
12549
12550
12551
12552
12553
12554
12555
12556
12557
12558
12559
12560
12561
12562
12563
12564
12565
12566
12567
12568
12569
12570
12571
12572
12573
12574
12575
12576
12577
12578
12579
12580
12581
12582
12583
12584
12585
12586
12587
12588
12589
12590
12591
12592
12593
12594
12595
12596
12597
12598
12599
12600
12601
12602
12603
12604
12605
12606
12607
12608
12609
12610
12611
12612
12613
12614
12615
12616
12617
12618
12619
12620
12621
12622
12623
12624
12625
12626
12627
12628
12629
12630
12631
12632
12633
12634
12635
12636
12637
12638
12639
12640
12641
12642
12643
12644
12645
12646
12647
12648
12649
12650
12651
12652
12653
12654
12655
12656
12657
12658
12659
12660
12661
12662
12663
12664
12665
12666
12667
12668
12669
12670
12671
12672
12673
12674
12675
12676
12677
12678
12679
12680
12681
12682
12683
12684
12685
12686
12687
12688
12689
12690
12691
12692
12693
12694 --p;
12695 --zv;
12696 --yv;
12697 --xv;
12698 --listv;
12699
12700
12701
12702
12703
12704
12705 imx = *lv;
12706 n = *nv;
12707 if (n < 3 || n > imx) {
12708 goto L11;
12709 }
12710 k0 = 0;
12711 i1 = listv[1];
12712 if (i1 < 1 || i1 > imx) {
12713 goto L12;
12714 }
12715
12716
12717
12718
12719
12720 L1:
12721 ++k0;
12722 if (k0 > n) {
12723 goto L14;
12724 }
12725 i1 = listv[k0];
12726 if (k0 < n) {
12727 i2 = listv[k0 + 1];
12728 } else {
12729 i2 = listv[1];
12730 }
12731 if (i2 < 1 || i2 > imx) {
12732 goto L12;
12733 }
12734 vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12735 vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12736 vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12737 vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12738 if (vnrm == 0.) {
12739 goto L1;
12740 }
12741 q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12742 q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12743 q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12744 qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12745 q[0] /= qnrm;
12746 q[1] /= qnrm;
12747 q[2] /= qnrm;
12748
12749
12750
12751 cn[0] = q[1] * p[3] - q[2] * p[2];
12752 cn[1] = q[2] * p[1] - q[0] * p[3];
12753 cn[2] = q[0] * p[2] - q[1] * p[1];
12754 if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12755 goto L1;
12756 }
12757 pn[0] = p[2] * cn[2] - p[3] * cn[1];
12758 pn[1] = p[3] * cn[0] - p[1] * cn[2];
12759 pn[2] = p[1] * cn[1] - p[2] * cn[0];
12760 qn[0] = cn[1] * q[2] - cn[2] * q[1];
12761 qn[1] = cn[2] * q[0] - cn[0] * q[2];
12762 qn[2] = cn[0] * q[1] - cn[1] * q[0];
12763
12764
12765
12766 ni = 0;
12767 even = TRUE_;
12768 bp = -2.;
12769 bq = -2.;
12770 pinr = TRUE_;
12771 qinr = TRUE_;
12772 i2 = listv[n];
12773 if (i2 < 1 || i2 > imx) {
12774 goto L12;
12775 }
12776 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12777
12778
12779
12780 i__1 = n;
12781 for (k = 1; k <= i__1; ++k) {
12782 i1 = i2;
12783 lft1 = lft2;
12784 i2 = listv[k];
12785 if (i2 < 1 || i2 > imx) {
12786 goto L12;
12787 }
12788 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12789 if (lft1 == lft2) {
12790 goto L2;
12791 }
12792
12793
12794
12795
12796 ++ni;
12797 v1[0] = xv[i1];
12798 v1[1] = yv[i1];
12799 v1[2] = zv[i1];
12800 v2[0] = xv[i2];
12801 v2[1] = yv[i2];
12802 v2[2] = zv[i2];
12803 intrsc_(v1, v2, cn, b, &ierr);
12804
12805
12806
12807
12808
12809 if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12810 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12811
12812
12813
12814 even = ! even;
12815 d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12816 if (d__ > bq) {
12817 bq = d__;
12818 qinr = lft2;
12819 }
12820 d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12821 if (d__ > bp) {
12822 bp = d__;
12823 pinr = lft1;
12824 }
12825 }
12826 L2:
12827 ;
12828 }
12829
12830
12831
12832
12833 if (ni != ni / 2 << 1 || ! qinr) {
12834 goto L1;
12835 }
12836
12837
12838
12839 if (pinr != even) {
12840 goto L13;
12841 }
12842
12843
12844
12845 *ier = 0;
12846 ret_val = even;
12847 return ret_val;
12848
12849
12850
12851 L11:
12852 *ier = 1;
12853 return ret_val;
12854
12855
12856
12857 L12:
12858 *ier = 2;
12859 return ret_val;
12860
12861
12862
12863 L13:
12864 *ier = 3;
12865 return ret_val;
12866
12867
12868
12869 L14:
12870 *ier = 4;
12871 return ret_val;
12872 }
12873
12874 int intadd_(int *kk, int *i1, int *i2, int *
12875 i3, int *list, int *lptr, int *lend, int *lnew)
12876 {
12877 static int k, n1, n2, n3, lp;
12878 int insert_(int *, int *, int *,
12879 int *, int *);
12880 int lstptr_(int *, int *, int *, int *);
12881
12882
12883
12884
12885
12886
12887
12888
12889
12890
12891
12892
12893
12894
12895
12896
12897
12898
12899
12900
12901
12902
12903
12904
12905
12906
12907
12908
12909
12910
12911
12912
12913
12914
12915
12916
12917
12918
12919
12920
12921
12922
12923
12924
12925
12926
12927
12928
12929
12930
12931
12932
12933
12934
12935
12936
12937
12938 --lend;
12939 --lptr;
12940 --list;
12941
12942
12943 k = *kk;
12944
12945
12946
12947 n1 = *i1;
12948 n2 = *i2;
12949 n3 = *i3;
12950
12951
12952
12953 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12954 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12955 lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12956 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12957 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12958 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12959
12960
12961
12962 list[*lnew] = n1;
12963 list[*lnew + 1] = n2;
12964 list[*lnew + 2] = n3;
12965 lptr[*lnew] = *lnew + 1;
12966 lptr[*lnew + 1] = *lnew + 2;
12967 lptr[*lnew + 2] = *lnew;
12968 lend[k] = *lnew + 2;
12969 *lnew += 3;
12970 return 0;
12971 }
12972
12973 int intrsc_(double *p1, double *p2, double *cn,
12974 double *p, int *ier)
12975 {
12976
12977
12978
12979
12980 static int i__;
12981 static double t, d1, d2, pp[3], ppn;
12982
12983
12984
12985
12986
12987
12988
12989
12990
12991
12992
12993
12994
12995
12996
12997
12998
12999
13000
13001
13002
13003
13004
13005
13006
13007
13008
13009
13010
13011
13012
13013
13014
13015
13016
13017
13018
13019
13020
13021
13022
13023
13024
13025
13026
13027
13028
13029
13030
13031
13032
13033
13034
13035
13036
13037
13038
13039
13040
13041
13042
13043
13044
13045
13046
13047
13048
13049
13050
13051 --p;
13052 --cn;
13053 --p2;
13054 --p1;
13055
13056
13057 d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
13058 d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
13059
13060 if (d1 == d2) {
13061 *ier = 1;
13062 return 0;
13063 }
13064
13065
13066
13067 t = d1 / (d1 - d2);
13068 ppn = 0.;
13069 for (i__ = 1; i__ <= 3; ++i__) {
13070 pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
13071 ppn += pp[i__ - 1] * pp[i__ - 1];
13072
13073 }
13074
13075
13076
13077 if (ppn == 0.) {
13078 *ier = 2;
13079 return 0;
13080 }
13081 ppn = sqrt(ppn);
13082
13083
13084
13085 for (i__ = 1; i__ <= 3; ++i__) {
13086 p[i__] = pp[i__ - 1] / ppn;
13087
13088 }
13089 *ier = 0;
13090 return 0;
13091 }
13092
13093 int jrand_(int *n, int *ix, int *iy, int *iz)
13094 {
13095
13096 int ret_val;
13097
13098
13099 static float u, x;
13100
13101
13102
13103
13104
13105
13106
13107
13108
13109
13110
13111
13112
13113
13114
13115
13116
13117
13118
13119
13120
13121
13122
13123
13124
13125
13126
13127
13128
13129
13130
13131
13132
13133
13134
13135
13136
13137
13138
13139
13140
13141
13142
13143
13144
13145
13146
13147
13148
13149
13150
13151
13152
13153 *ix = *ix * 171 % 30269;
13154 *iy = *iy * 172 % 30307;
13155 *iz = *iz * 170 % 30323;
13156 x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13157 30323.f;
13158 u = x - (int) x;
13159 ret_val = (int) ((float) (*n) * u + 1.f);
13160 return ret_val;
13161 }
13162
13163 long int left_(double *x1, double *y1, double *z1, double *x2,
13164 double *y2, double *z2, double *x0, double *y0,
13165 double *z0)
13166 {
13167
13168 long int ret_val;
13169
13170
13171
13172
13173
13174
13175
13176
13177
13178
13179
13180
13181
13182
13183
13184
13185
13186
13187
13188
13189
13190
13191
13192
13193
13194
13195
13196
13197
13198
13199
13200
13201
13202
13203
13204
13205
13206
13207 ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13208 *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13209
13210
13211 return ret_val;
13212 }
13213
13214 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13215 {
13216
13217 int ret_val;
13218
13219
13220 static int nd, lp;
13221
13222
13223
13224
13225
13226
13227
13228
13229
13230
13231
13232
13233
13234
13235
13236
13237
13238
13239
13240
13241
13242
13243
13244
13245
13246
13247
13248
13249
13250
13251
13252
13253
13254
13255
13256
13257
13258
13259
13260
13261
13262
13263
13264
13265
13266
13267
13268 --lptr;
13269 --list;
13270
13271
13272 lp = lptr[*lpl];
13273 L1:
13274 nd = list[lp];
13275 if (nd == *nb) {
13276 goto L2;
13277 }
13278 lp = lptr[lp];
13279 if (lp != *lpl) {
13280 goto L1;
13281 }
13282
13283 L2:
13284 ret_val = lp;
13285 return ret_val;
13286 }
13287
13288 int nbcnt_(int *lpl, int *lptr)
13289 {
13290
13291 int ret_val;
13292
13293
13294 static int k, lp;
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
13324
13325
13326
13327
13328
13329
13330
13331
13332
13333
13334
13335
13336
13337 --lptr;
13338
13339
13340 lp = *lpl;
13341 k = 1;
13342
13343 L1:
13344 lp = lptr[lp];
13345 if (lp == *lpl) {
13346 goto L2;
13347 }
13348 ++k;
13349 goto L1;
13350
13351 L2:
13352 ret_val = k;
13353 return ret_val;
13354 }
13355
13356 int nearnd_(double *p, int *ist, int *n, double *x,
13357 double *y, double *z__, int *list, int *lptr, int
13358 *lend, double *al)
13359 {
13360
13361 int ret_val, i__1;
13362
13363
13364
13365
13366
13367 static int l;
13368 static double b1, b2, b3;
13369 static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13370 static double ds1;
13371 static int lp1, lp2;
13372 static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13373 static int lpl;
13374 static double dsr;
13375 static int nst, listp[25], lptrp[25];
13376 int trfind_(int *, double *, int *,
13377 double *, double *, double *, int *, int *,
13378 int *, double *, double *, double *, int *,
13379 int *, int *);
13380 int lstptr_(int *, int *, int *, int *);
13381
13382
13383
13384
13385
13386
13387
13388
13389
13390
13391
13392
13393
13394
13395
13396
13397
13398
13399
13400
13401
13402
13403
13404
13405
13406
13407
13408
13409
13410
13411
13412
13413
13414
13415
13416
13417
13418
13419
13420
13421
13422
13423
13424
13425
13426
13427
13428
13429
13430
13431
13432
13433
13434
13435
13436
13437
13438
13439
13440
13441
13442
13443
13444
13445
13446
13447
13448
13449
13450
13451
13452
13453
13454
13455
13456
13457
13458
13459
13460
13461
13462
13463
13464
13465
13466
13467
13468
13469
13470
13471
13472
13473
13474
13475
13476
13477 --p;
13478 --lend;
13479 --z__;
13480 --y;
13481 --x;
13482 --list;
13483 --lptr;
13484
13485
13486 nn = *n;
13487 if (nn < 3) {
13488 goto L6;
13489 }
13490 nst = *ist;
13491 if (nst < 1 || nst > nn) {
13492 nst = 1;
13493 }
13494
13495
13496
13497
13498
13499 trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13500 1], &b1, &b2, &b3, &i1, &i2, &i3);
13501
13502
13503
13504 if (i1 == 0) {
13505 goto L6;
13506 }
13507
13508
13509
13510
13511
13512
13513
13514 if (i3 != 0) {
13515 listp[0] = i1;
13516 lptrp[0] = 2;
13517 listp[1] = i2;
13518 lptrp[1] = 3;
13519 listp[2] = i3;
13520 lptrp[2] = 1;
13521 l = 3;
13522 } else {
13523 n1 = i1;
13524 l = 1;
13525 lp1 = 2;
13526 listp[l - 1] = n1;
13527 lptrp[l - 1] = lp1;
13528
13529
13530
13531
13532 L1:
13533 lpl = lend[n1];
13534 n1 = -list[lpl];
13535 l = lp1;
13536 lp1 = l + 1;
13537 listp[l - 1] = n1;
13538 lptrp[l - 1] = lp1;
13539 if (n1 != i2 && lp1 < 25) {
13540 goto L1;
13541 }
13542 l = lp1;
13543 listp[l - 1] = 0;
13544 lptrp[l - 1] = 1;
13545 }
13546
13547
13548
13549
13550
13551
13552 lp2 = 1;
13553 n2 = i1;
13554 lp1 = lptrp[0];
13555 n1 = listp[lp1 - 1];
13556
13557
13558
13559 L2:
13560 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13561 if (list[lp] < 0) {
13562 goto L3;
13563 }
13564 lp = lptr[lp];
13565 n3 = (i__1 = list[lp], abs(i__1));
13566
13567
13568
13569 if (l == 25) {
13570 goto L4;
13571 }
13572 dx1 = x[n1] - p[1];
13573 dy1 = y[n1] - p[2];
13574 dz1 = z__[n1] - p[3];
13575
13576 dx2 = x[n2] - p[1];
13577 dy2 = y[n2] - p[2];
13578 dz2 = z__[n2] - p[3];
13579
13580 dx3 = x[n3] - p[1];
13581 dy3 = y[n3] - p[2];
13582 dz3 = z__[n3] - p[3];
13583 if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13584 (dx2 * dy1 - dx1 * dy2) <= 0.) {
13585 goto L3;
13586 }
13587
13588
13589
13590
13591 ++l;
13592 lptrp[lp2 - 1] = l;
13593 listp[l - 1] = n3;
13594 lptrp[l - 1] = lp1;
13595 lp1 = l;
13596 n1 = n3;
13597 goto L2;
13598
13599
13600
13601
13602 L3:
13603 if (lp1 == 1) {
13604 goto L4;
13605 }
13606 lp2 = lp1;
13607 n2 = n1;
13608 lp1 = lptrp[lp1 - 1];
13609 n1 = listp[lp1 - 1];
13610 if (n1 == 0) {
13611 goto L4;
13612 }
13613 goto L2;
13614
13615
13616
13617
13618
13619 L4:
13620 nr = i1;
13621 dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13622 i__1 = l;
13623 for (lp = 2; lp <= i__1; ++lp) {
13624 n1 = listp[lp - 1];
13625 if (n1 == 0) {
13626 goto L5;
13627 }
13628 ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13629 if (ds1 < dsr) {
13630 nr = n1;
13631 dsr = ds1;
13632 }
13633 L5:
13634 ;
13635 }
13636 dsr = -dsr;
13637 if (dsr > 1.) {
13638 dsr = 1.;
13639 }
13640 *al = acos(dsr);
13641 ret_val = nr;
13642 return ret_val;
13643
13644
13645
13646 L6:
13647 ret_val = 0;
13648 return ret_val;
13649 }
13650
13651 int optim_(double *x, double *y, double *z__,
13652 int *na, int *list, int *lptr, int *lend, int *
13653 nit, int *iwk, int *ier)
13654 {
13655
13656 int i__1, i__2;
13657
13658
13659 static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13660 static long int swp;
13661 static int iter;
13662 int swap_(int *, int *, int *,
13663 int *, int *, int *, int *, int *);
13664 static int maxit;
13665 long int swptst_(int *, int *, int *, int *,
13666 double *, double *, double *);
13667
13668
13669
13670
13671
13672
13673
13674
13675
13676
13677
13678
13679
13680
13681
13682
13683
13684
13685
13686
13687
13688
13689
13690
13691
13692
13693
13694
13695
13696
13697
13698
13699
13700
13701
13702
13703
13704
13705
13706
13707
13708
13709
13710
13711
13712
13713
13714
13715
13716
13717
13718
13719
13720
13721
13722
13723
13724
13725
13726
13727
13728
13729
13730
13731
13732
13733
13734
13735
13736
13737
13738
13739
13740
13741
13742
13743
13744
13745
13746
13747
13748
13749
13750
13751
13752
13753
13754
13755
13756
13757
13758
13759
13760
13761
13762
13763 --x;
13764 --y;
13765 --z__;
13766 iwk -= 3;
13767 --list;
13768 --lptr;
13769 --lend;
13770
13771
13772 nna = *na;
13773 maxit = *nit;
13774 if (nna < 0 || maxit < 1) {
13775 goto L7;
13776 }
13777
13778
13779
13780 iter = 0;
13781 if (nna == 0) {
13782 goto L5;
13783 }
13784
13785
13786
13787
13788 L1:
13789 if (iter == maxit) {
13790 goto L6;
13791 }
13792 ++iter;
13793 swp = FALSE_;
13794
13795
13796
13797 i__1 = nna;
13798 for (i__ = 1; i__ <= i__1; ++i__) {
13799 io1 = iwk[(i__ << 1) + 1];
13800 io2 = iwk[(i__ << 1) + 2];
13801
13802
13803
13804
13805
13806
13807
13808
13809 lpl = lend[io1];
13810 lpp = lpl;
13811 lp = lptr[lpp];
13812 L2:
13813 if (list[lp] == io2) {
13814 goto L3;
13815 }
13816 lpp = lp;
13817 lp = lptr[lpp];
13818 if (lp != lpl) {
13819 goto L2;
13820 }
13821
13822
13823
13824
13825
13826 if ((i__2 = list[lp], abs(i__2)) != io2) {
13827 goto L8;
13828 }
13829 if (list[lp] < 0) {
13830 goto L4;
13831 }
13832
13833
13834
13835
13836 L3:
13837 n2 = list[lpp];
13838 if (n2 < 0) {
13839 goto L4;
13840 }
13841 lp = lptr[lp];
13842 n1 = (i__2 = list[lp], abs(i__2));
13843
13844
13845
13846 if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13847 goto L4;
13848 }
13849 swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13850 if (lp21 == 0) {
13851 goto L9;
13852 }
13853 swp = TRUE_;
13854 iwk[(i__ << 1) + 1] = n1;
13855 iwk[(i__ << 1) + 2] = n2;
13856 L4:
13857 ;
13858 }
13859 if (swp) {
13860 goto L1;
13861 }
13862
13863
13864
13865 L5:
13866 *nit = iter;
13867 *ier = 0;
13868 return 0;
13869
13870
13871
13872 L6:
13873 *nit = maxit;
13874 *ier = 1;
13875 return 0;
13876
13877
13878
13879 L7:
13880 *nit = 0;
13881 *ier = 2;
13882 return 0;
13883
13884
13885
13886 L8:
13887 *nit = iter;
13888 *ier = 3;
13889 return 0;
13890
13891
13892
13893 L9:
13894 *nit = iter;
13895 *ier = 4;
13896 return 0;
13897 }
13898
13899 int projct_(double *px, double *py, double *pz,
13900 double *ox, double *oy, double *oz, double *ex,
13901 double *ey, double *ez, double *vx, double *vy,
13902 double *vz, long int *init, double *x, double *y,
13903 double *z__, int *ier)
13904 {
13905
13906
13907
13908
13909 static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13910 oes, xoe, yoe, zoe, xep, yep, zep;
13911
13912
13913
13914
13915
13916
13917
13918
13919
13920
13921
13922
13923
13924
13925
13926
13927
13928
13929
13930
13931
13932
13933
13934
13935
13936
13937
13938
13939
13940
13941
13942
13943
13944
13945
13946
13947
13948
13949
13950
13951
13952
13953
13954
13955
13956
13957
13958
13959
13960
13961
13962
13963
13964
13965
13966
13967
13968
13969
13970
13971
13972
13973
13974
13975
13976
13977
13978
13979
13980
13981
13982
13983
13984
13985
13986
13987
13988
13989
13990
13991
13992
13993
13994
13995
13996
13997
13998
13999
14000
14001
14002
14003
14004
14005
14006
14007
14008
14009
14010
14011
14012
14013
14014
14015
14016
14017
14018
14019
14020
14021
14022
14023
14024
14025 if (*init) {
14026
14027
14028
14029
14030
14031
14032
14033
14034 xe = *ex;
14035 ye = *ey;
14036 ze = *ez;
14037 xoe = xe - *ox;
14038 yoe = ye - *oy;
14039 zoe = ze - *oz;
14040 oes = xoe * xoe + yoe * yoe + zoe * zoe;
14041 if (oes == 0.) {
14042 goto L1;
14043 }
14044
14045
14046
14047 s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
14048 xv = *vx - s * xoe;
14049 yv = *vy - s * yoe;
14050 zv = *vz - s * zoe;
14051
14052
14053
14054 sc = xv * xv + yv * yv + zv * zv;
14055 if (sc == 0.) {
14056 goto L2;
14057 }
14058 sc = 1. / sqrt(sc);
14059 xv = sc * xv;
14060 yv = sc * yv;
14061 zv = sc * zv;
14062
14063
14064
14065 xh = yv * zoe - yoe * zv;
14066 yh = xoe * zv - xv * zoe;
14067 zh = xv * yoe - xoe * yv;
14068 sc = sqrt(xh * xh + yh * yh + zh * zh);
14069 if (sc == 0.) {
14070 goto L2;
14071 }
14072 sc = 1. / sc;
14073 xh = sc * xh;
14074 yh = sc * yh;
14075 zh = sc * zh;
14076 }
14077
14078
14079
14080
14081
14082
14083 xep = *px - xe;
14084 yep = *py - ye;
14085 zep = *pz - ze;
14086 s = xoe * xep + yoe * yep + zoe * zep;
14087 if (s >= 0.) {
14088 goto L1;
14089 }
14090 s = oes / s;
14091 xw = xoe - s * xep;
14092 yw = yoe - s * yep;
14093 zw = zoe - s * zep;
14094
14095
14096
14097
14098 *x = xw * xh + yw * yh + zw * zh;
14099 *y = xw * xv + yw * yv + zw * zv;
14100 *z__ = s + 1.;
14101 *init = FALSE_;
14102 *ier = 0;
14103 return 0;
14104
14105
14106
14107 L1:
14108 *ier = 1;
14109 return 0;
14110
14111
14112
14113 L2:
14114 *ier = 2;
14115 return 0;
14116 }
14117
14118 int scoord_(double *px, double *py, double *pz,
14119 double *plat, double *plon, double *pnrm)
14120 {
14121
14122
14123
14124
14125
14126
14127
14128
14129
14130
14131
14132
14133
14134
14135
14136
14137
14138
14139
14140
14141
14142
14143
14144
14145
14146
14147
14148
14149
14150
14151
14152
14153
14154
14155
14156
14157
14158
14159
14160
14161
14162
14163 *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14164 if (*px != 0. || *py != 0.) {
14165 *plon = atan2(*py, *px);
14166 } else {
14167 *plon = 0.;
14168 }
14169 if (*pnrm != 0.) {
14170 *plat = asin(*pz / *pnrm);
14171 } else {
14172 *plat = 0.;
14173 }
14174 return 0;
14175 }
14176
14177 double store_(double *x)
14178 {
14179
14180 double ret_val;
14181
14182
14183
14184
14185
14186
14187
14188
14189
14190
14191
14192
14193
14194
14195
14196
14197
14198
14199
14200
14201
14202
14203
14204
14205
14206
14207
14208
14209
14210
14211
14212
14213
14214
14215 stcom_1.y = *x;
14216 ret_val = stcom_1.y;
14217 return ret_val;
14218 }
14219
14220 int swap_(int *in1, int *in2, int *io1, int *
14221 io2, int *list, int *lptr, int *lend, int *lp21)
14222 {
14223
14224 int i__1;
14225
14226
14227 static int lp, lph, lpsav;
14228 int lstptr_(int *, int *, int *, int *);
14229
14230
14231
14232
14233
14234
14235
14236
14237
14238
14239
14240
14241
14242
14243
14244
14245
14246
14247
14248
14249
14250
14251
14252
14253
14254
14255
14256
14257
14258
14259
14260
14261
14262
14263
14264
14265
14266
14267
14268
14269
14270
14271
14272
14273
14274
14275
14276
14277
14278
14279
14280
14281
14282
14283
14284
14285
14286
14287
14288
14289 --lend;
14290 --lptr;
14291 --list;
14292
14293
14294 lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14295 if ((i__1 = list[lp], abs(i__1)) == *in2) {
14296 *lp21 = 0;
14297 return 0;
14298 }
14299
14300
14301
14302 lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14303 lph = lptr[lp];
14304 lptr[lp] = lptr[lph];
14305
14306
14307
14308
14309 if (lend[*io1] == lph) {
14310 lend[*io1] = lp;
14311 }
14312
14313
14314
14315
14316 lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14317 lpsav = lptr[lp];
14318 lptr[lp] = lph;
14319 list[lph] = *in2;
14320 lptr[lph] = lpsav;
14321
14322
14323
14324 lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14325 lph = lptr[lp];
14326 lptr[lp] = lptr[lph];
14327
14328
14329
14330
14331 if (lend[*io2] == lph) {
14332 lend[*io2] = lp;
14333 }
14334
14335
14336
14337 lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14338 lpsav = lptr[lp];
14339 lptr[lp] = lph;
14340 list[lph] = *in1;
14341 lptr[lph] = lpsav;
14342 *lp21 = lph;
14343 return 0;
14344 }
14345
14346 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14347 double *x, double *y, double *z__)
14348 {
14349
14350 long int ret_val;
14351
14352
14353 static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14354
14355
14356
14357
14358
14359
14360
14361
14362
14363
14364
14365
14366
14367
14368
14369
14370
14371
14372
14373
14374
14375
14376
14377
14378
14379
14380
14381
14382
14383
14384
14385
14386
14387
14388
14389
14390
14391
14392
14393
14394
14395
14396
14397
14398
14399
14400
14401
14402
14403
14404
14405
14406
14407
14408
14409
14410 --z__;
14411 --y;
14412 --x;
14413
14414
14415 x4 = x[*n4];
14416 y4 = y[*n4];
14417 z4 = z__[*n4];
14418 dx1 = x[*n1] - x4;
14419 dx2 = x[*n2] - x4;
14420 dx3 = x[*n3] - x4;
14421 dy1 = y[*n1] - y4;
14422 dy2 = y[*n2] - y4;
14423 dy3 = y[*n3] - y4;
14424 dz1 = z__[*n1] - z4;
14425 dz2 = z__[*n2] - z4;
14426 dz3 = z__[*n3] - z4;
14427
14428
14429
14430
14431
14432 ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14433 dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14434 return ret_val;
14435 }
14436
14437 int trans_(int *n, double *rlat, double *rlon,
14438 double *x, double *y, double *z__)
14439 {
14440
14441 int i__1;
14442
14443
14444
14445
14446
14447 static int i__, nn;
14448 static double phi, theta, cosphi;
14449
14450
14451
14452
14453
14454
14455
14456
14457
14458
14459
14460
14461
14462
14463
14464
14465
14466
14467
14468
14469
14470
14471
14472
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 --z__;
14504 --y;
14505 --x;
14506 --rlon;
14507 --rlat;
14508
14509
14510 nn = *n;
14511 i__1 = nn;
14512 for (i__ = 1; i__ <= i__1; ++i__) {
14513 phi = rlat[i__];
14514 theta = rlon[i__];
14515 cosphi = cos(phi);
14516 x[i__] = cosphi * cos(theta);
14517 y[i__] = cosphi * sin(theta);
14518 z__[i__] = sin(phi);
14519
14520 }
14521 return 0;
14522 }
14523
14524 int trfind_(int *nst, double *p, int *n,
14525 double *x, double *y, double *z__, int *list, int
14526 *lptr, int *lend, double *b1, double *b2, double *b3,
14527 int *i1, int *i2, int *i3)
14528 {
14529
14530
14531 static int ix = 1;
14532 static int iy = 2;
14533 static int iz = 3;
14534
14535
14536 int i__1;
14537 double d__1, d__2;
14538
14539
14540 static double q[3];
14541 static int n0, n1, n2, n3, n4, nf;
14542 static double s12;
14543 static int nl, lp;
14544 static double xp, yp, zp;
14545 static int n1s, n2s;
14546 static double eps, tol, ptn1, ptn2;
14547 static int next;
14548 int jrand_(int *, int *, int *, int *);
14549 double store_(double *);
14550 int lstptr_(int *, int *, int *, int *);
14551
14552
14553
14554
14555
14556
14557
14558
14559
14560
14561
14562
14563
14564
14565
14566
14567
14568
14569
14570
14571
14572
14573
14574
14575
14576
14577
14578
14579
14580
14581
14582
14583
14584
14585
14586
14587
14588
14589
14590
14591
14592
14593
14594
14595
14596
14597
14598
14599
14600
14601
14602
14603
14604
14605
14606
14607
14608
14609
14610
14611
14612
14613
14614
14615
14616
14617
14618
14619
14620 --p;
14621 --lend;
14622 --z__;
14623 --y;
14624 --x;
14625 --list;
14626 --lptr;
14627
14628
14629
14630
14631
14632
14633
14634
14635
14636
14637
14638
14639
14640
14641
14642
14643
14644
14645
14646
14647
14648
14649
14650
14651
14652
14653
14654
14655
14656
14657
14658
14659
14660
14661
14662
14663
14664
14665
14666
14667
14668
14669
14670
14671
14672
14673
14674
14675 xp = p[1];
14676 yp = p[2];
14677 zp = p[3];
14678 n0 = *nst;
14679 if (n0 < 1 || n0 > *n) {
14680 n0 = jrand_(n, &ix, &iy, &iz);
14681 }
14682
14683
14684
14685 eps = 1.;
14686 L1:
14687 eps /= 2.;
14688 d__1 = eps + 1.;
14689 if (store_(&d__1) > 1.) {
14690 goto L1;
14691 }
14692 eps *= 2.;
14693 tol = eps * 4.;
14694
14695
14696
14697
14698 L2:
14699 lp = lend[n0];
14700 nl = list[lp];
14701 lp = lptr[lp];
14702 nf = list[lp];
14703 n1 = nf;
14704
14705
14706
14707
14708 if (nl > 0) {
14709
14710
14711
14712 L3:
14713 if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14714 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14715 -1e-10) {
14716 lp = lptr[lp];
14717 n1 = list[lp];
14718 if (n1 == nl) {
14719 goto L6;
14720 }
14721 goto L3;
14722 }
14723 } else {
14724
14725
14726
14727 nl = -nl;
14728 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14729 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14730 -1e-10) {
14731
14732
14733
14734 n1 = n0;
14735 n2 = nf;
14736 goto L9;
14737 }
14738 if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14739 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14740 -1e-10) {
14741
14742
14743
14744 n1 = nl;
14745 n2 = n0;
14746 goto L9;
14747 }
14748 }
14749
14750
14751
14752
14753 L4:
14754 lp = lptr[lp];
14755 n2 = (i__1 = list[lp], abs(i__1));
14756 if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14757 n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14758 goto L7;
14759 }
14760 n1 = n2;
14761 if (n1 != nl) {
14762 goto L4;
14763 }
14764 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14765 nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14766 goto L6;
14767 }
14768
14769
14770
14771
14772 d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14773 if (store_(&d__2) < 1. - eps * 4.) {
14774
14775
14776
14777
14778
14779 L5:
14780 if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14781 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14782 -1e-10) {
14783 lp = lptr[lp];
14784 n1 = (i__1 = list[lp], abs(i__1));
14785 if (n1 == nl) {
14786 goto L14;
14787 }
14788 goto L5;
14789 }
14790 }
14791
14792
14793
14794
14795 n0 = n1;
14796 goto L2;
14797
14798
14799
14800 L6:
14801 n2 = nf;
14802
14803
14804
14805
14806
14807 L7:
14808 n3 = n0;
14809 n1s = n1;
14810 n2s = n2;
14811
14812
14813
14814 L8:
14815
14816 *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14817 x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14818 if (*b3 < -1e-10) {
14819
14820
14821
14822
14823 lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14824 if (list[lp] < 0) {
14825 goto L9;
14826 }
14827 lp = lptr[lp];
14828 n4 = (i__1 = list[lp], abs(i__1));
14829
14830
14831
14832 if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14833 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14834 -1e-10) {
14835 n3 = n2;
14836 n2 = n4;
14837 n1s = n1;
14838 if (n2 != n2s && n2 != n0) {
14839 goto L8;
14840 }
14841 } else {
14842 n3 = n1;
14843 n1 = n4;
14844 n2s = n2;
14845 if (n1 != n1s && n1 != n0) {
14846 goto L8;
14847 }
14848 }
14849
14850
14851
14852
14853
14854 n0 = jrand_(n, &ix, &iy, &iz);
14855 goto L2;
14856 }
14857
14858
14859
14860
14861 if (*b3 >= eps) {
14862
14863
14864
14865 *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14866 - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14867 *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14868 - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14869 if (*b1 < -tol || *b2 < -tol) {
14870
14871
14872
14873 n0 = jrand_(n, &ix, &iy, &iz);
14874 goto L2;
14875 }
14876 } else {
14877
14878
14879
14880
14881 *b3 = 0.;
14882 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14883 ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14884 ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14885 *b1 = ptn1 - s12 * ptn2;
14886 *b2 = ptn2 - s12 * ptn1;
14887 if (*b1 < -tol || *b2 < -tol) {
14888
14889
14890
14891 n0 = jrand_(n, &ix, &iy, &iz);
14892 goto L2;
14893 }
14894 }
14895
14896
14897
14898 *i1 = n1;
14899 *i2 = n2;
14900 *i3 = n3;
14901 if (*b1 < 0.f) {
14902 *b1 = 0.f;
14903 }
14904 if (*b2 < 0.f) {
14905 *b2 = 0.f;
14906 }
14907 return 0;
14908
14909
14910
14911
14912
14913 L9:
14914 n1s = n1;
14915 n2s = n2;
14916 nl = 0;
14917
14918
14919
14920 L10:
14921
14922 lp = lend[n2];
14923 lp = lptr[lp];
14924 next = list[lp];
14925 if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14926 - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14927 >= -1e-10) {
14928
14929
14930
14931
14932 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14933 q[0] = x[n1] - s12 * x[n2];
14934 q[1] = y[n1] - s12 * y[n2];
14935 q[2] = z__[n1] - s12 * z__[n2];
14936 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14937 goto L11;
14938 }
14939 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14940 goto L11;
14941 }
14942
14943
14944
14945
14946 nl = n2;
14947 }
14948
14949
14950
14951 n1 = n2;
14952 n2 = next;
14953 if (n2 != n1s) {
14954 goto L10;
14955 }
14956
14957
14958
14959 *i1 = n1s;
14960 *i2 = n1s;
14961 *i3 = 0;
14962 return 0;
14963
14964
14965
14966 L11:
14967 nf = n2;
14968 if (nl == 0) {
14969
14970
14971
14972
14973 n2 = n2s;
14974 n1 = n1s;
14975
14976
14977
14978 L12:
14979 lp = lend[n1];
14980 next = -list[lp];
14981 if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14982 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14983 y[next]) >= -1e-10) {
14984
14985
14986
14987
14988 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14989 q[0] = x[n2] - s12 * x[n1];
14990 q[1] = y[n2] - s12 * y[n1];
14991 q[2] = z__[n2] - s12 * z__[n1];
14992 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14993 goto L13;
14994 }
14995 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14996 goto L13;
14997 }
14998
14999
15000
15001
15002 nf = n1;
15003 }
15004
15005
15006
15007 n2 = n1;
15008 n1 = next;
15009 if (n1 != n1s) {
15010 goto L12;
15011 }
15012
15013
15014
15015 *i1 = n1;
15016 *i2 = n1;
15017 *i3 = 0;
15018 return 0;
15019
15020
15021
15022 L13:
15023 nl = n1;
15024 }
15025
15026
15027
15028 *i1 = nf;
15029 *i2 = nl;
15030 *i3 = 0;
15031 return 0;
15032
15033
15034
15035 L14:
15036 *i1 = 0;
15037 *i2 = 0;
15038 *i3 = 0;
15039 return 0;
15040 }
15041
15042 int trlist_(int *n, int *list, int *lptr,
15043 int *lend, int *nrow, int *nt, int *ltri, int *
15044 ier)
15045 {
15046
15047 int ltri_dim1, ltri_offset, i__1, i__2;
15048
15049
15050 static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
15051 lpl, isv;
15052 static long int arcs;
15053 static int lpln1;
15054
15055
15056
15057
15058
15059
15060
15061
15062
15063
15064
15065
15066
15067
15068
15069
15070
15071
15072
15073
15074
15075
15076
15077
15078
15079
15080
15081
15082
15083
15084
15085
15086
15087
15088
15089
15090
15091
15092
15093
15094
15095
15096
15097
15098
15099
15100
15101
15102
15103
15104
15105
15106
15107
15108
15109
15110
15111
15112
15113
15114
15115
15116
15117
15118
15119
15120
15121
15122
15123
15124
15125
15126
15127
15128
15129
15130
15131
15132
15133
15134
15135
15136
15137
15138
15139
15140
15141
15142
15143
15144
15145
15146
15147
15148
15149
15150
15151
15152
15153
15154
15155
15156
15157
15158 --lend;
15159 --list;
15160 --lptr;
15161 ltri_dim1 = *nrow;
15162 ltri_offset = 1 + ltri_dim1;
15163 ltri -= ltri_offset;
15164
15165
15166 if (*n < 3 || (*nrow != 6 && *nrow != 9)) {
15167 goto L11;
15168 }
15169
15170
15171
15172
15173
15174
15175
15176
15177 arcs = *nrow == 9;
15178 ka = 0;
15179 kt = 0;
15180 nm2 = *n - 2;
15181
15182
15183
15184 i__1 = nm2;
15185 for (n1 = 1; n1 <= i__1; ++n1) {
15186
15187
15188
15189
15190 lpln1 = lend[n1];
15191 lp2 = lpln1;
15192 L1:
15193 lp2 = lptr[lp2];
15194 n2 = list[lp2];
15195 lp = lptr[lp2];
15196 n3 = (i__2 = list[lp], abs(i__2));
15197 if (n2 < n1 || n3 < n1) {
15198 goto L8;
15199 }
15200
15201
15202
15203 ++kt;
15204 ltri[kt * ltri_dim1 + 1] = n1;
15205 ltri[kt * ltri_dim1 + 2] = n2;
15206 ltri[kt * ltri_dim1 + 3] = n3;
15207
15208
15209
15210
15211 for (i__ = 1; i__ <= 3; ++i__) {
15212 if (i__ == 1) {
15213 i1 = n3;
15214 i2 = n2;
15215 } else if (i__ == 2) {
15216 i1 = n1;
15217 i2 = n3;
15218 } else {
15219 i1 = n2;
15220 i2 = n1;
15221 }
15222
15223
15224
15225
15226 lpl = lend[i1];
15227 lp = lptr[lpl];
15228 L2:
15229 if (list[lp] == i2) {
15230 goto L3;
15231 }
15232 lp = lptr[lp];
15233 if (lp != lpl) {
15234 goto L2;
15235 }
15236
15237
15238
15239
15240
15241 if ((i__2 = list[lp], abs(i__2)) != i2) {
15242 goto L12;
15243 }
15244 kn = 0;
15245 if (list[lp] < 0) {
15246 goto L6;
15247 }
15248
15249
15250
15251
15252 L3:
15253 lp = lptr[lp];
15254 i3 = (i__2 = list[lp], abs(i__2));
15255
15256
15257
15258
15259
15260 if (i1 < i2 && i1 < i3) {
15261 j = 3;
15262 } else if (i2 < i3) {
15263 j = 2;
15264 isv = i1;
15265 i1 = i2;
15266 i2 = i3;
15267 i3 = isv;
15268 } else {
15269 j = 1;
15270 isv = i1;
15271 i1 = i3;
15272 i3 = i2;
15273 i2 = isv;
15274 }
15275
15276
15277
15278 if (i1 > n1) {
15279 goto L7;
15280 }
15281
15282
15283
15284
15285 for (kn = kt - 1; kn >= 1; --kn) {
15286 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15287 == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15288 goto L5;
15289 }
15290
15291 }
15292 goto L7;
15293
15294
15295
15296 L5:
15297 ltri[j + 3 + kn * ltri_dim1] = kt;
15298
15299
15300
15301 L6:
15302 ltri[i__ + 3 + kt * ltri_dim1] = kn;
15303 if (arcs) {
15304 ++ka;
15305 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15306 if (kn != 0) {
15307 ltri[j + 6 + kn * ltri_dim1] = ka;
15308 }
15309 }
15310 L7:
15311 ;
15312 }
15313
15314
15315
15316 L8:
15317 if (lp2 != lpln1) {
15318 goto L1;
15319 }
15320
15321 }
15322
15323
15324
15325 *nt = kt;
15326 *ier = 0;
15327 return 0;
15328
15329
15330
15331 L11:
15332 *nt = 0;
15333 *ier = 1;
15334 return 0;
15335
15336
15337
15338
15339 L12:
15340 *nt = 0;
15341 *ier = 2;
15342 return 0;
15343 }
15344
15345 int trlprt_(int *n, double *x, double *y,
15346 double *z__, int *iflag, int *nrow, int *nt, int *
15347 ltri, int *lout)
15348 {
15349
15350
15351 static int nmax = 9999;
15352 static int nlmax = 58;
15353
15354
15355 int ltri_dim1, ltri_offset, i__1;
15356
15357
15358 static int i__, k, na, nb, nl, lun;
15359
15360
15361
15362
15363
15364
15365
15366
15367
15368
15369
15370
15371
15372
15373
15374
15375
15376
15377
15378
15379
15380
15381
15382
15383
15384
15385
15386
15387
15388
15389
15390
15391
15392
15393
15394
15395
15396
15397
15398
15399
15400
15401
15402
15403
15404
15405
15406
15407
15408
15409
15410
15411
15412
15413
15414
15415
15416
15417
15418
15419
15420
15421
15422
15423
15424
15425
15426
15427
15428
15429
15430
15431
15432 --z__;
15433 --y;
15434 --x;
15435 ltri_dim1 = *nrow;
15436 ltri_offset = 1 + ltri_dim1;
15437 ltri -= ltri_offset;
15438
15439
15440
15441
15442
15443
15444
15445
15446
15447
15448
15449
15450
15451
15452
15453
15454 lun = *lout;
15455 if (lun < 0 || lun > 99) {
15456 lun = 6;
15457 }
15458
15459
15460
15461
15462 nl = 3;
15463 if (*n < 3 || *n > nmax || (*nrow != 6 && *nrow != 9) || *nt < 1 || *nt >
15464 nmax) {
15465
15466
15467
15468
15469 return 0;
15470 }
15471 if (*iflag == 0) {
15472
15473
15474
15475
15476 nl = 6;
15477 i__1 = *n;
15478 for (i__ = 1; i__ <= i__1; ++i__) {
15479 if (nl >= nlmax) {
15480
15481 nl = 0;
15482 }
15483
15484 ++nl;
15485
15486 }
15487 } else if (*iflag > 0) {
15488
15489
15490
15491
15492 nl = 6;
15493 i__1 = *n;
15494 for (i__ = 1; i__ <= i__1; ++i__) {
15495 if (nl >= nlmax) {
15496
15497 nl = 0;
15498 }
15499
15500 ++nl;
15501
15502 }
15503 }
15504
15505
15506
15507 if (nl > nlmax / 2) {
15508
15509 nl = 0;
15510 }
15511 if (*nrow == 6) {
15512
15513 } else {
15514
15515 }
15516 nl += 5;
15517 i__1 = *nt;
15518 for (k = 1; k <= i__1; ++k) {
15519 if (nl >= nlmax) {
15520
15521 nl = 0;
15522 }
15523
15524 ++nl;
15525
15526 }
15527
15528
15529
15530
15531 nb = (*n << 1) - *nt - 2;
15532 if (nb < 3) {
15533 nb = 0;
15534 na = *n * 3 - 6;
15535 } else {
15536 na = *nt + *n - 1;
15537 }
15538
15539 return 0;
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 int trmesh_(int *n, double *x, double *y,
15566 double *z__, int *list, int *lptr, int *lend, int
15567 *lnew, int *near__, int *next, double *dist, int *ier)
15568 {
15569
15570 int i__1, i__2;
15571
15572
15573 static double d__;
15574 static int i__, j, k;
15575 static double d1, d2, d3;
15576 static int i0, lp, nn, lpl;
15577 long int left_(double *, double *, double *, double
15578 *, double *, double *, double *, double *,
15579 double *);
15580 static int nexti;
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
15657
15658
15659
15660
15661
15662
15663
15664
15665
15666
15667
15668
15669
15670
15671
15672
15673
15674
15675
15676
15677
15678
15679
15680
15681
15682
15683
15684
15685
15686
15687
15688
15689
15690
15691
15692
15693
15694
15695
15696
15697
15698
15699
15700
15701
15702
15703
15704
15705
15706
15707
15708
15709
15710
15711
15712
15713
15714
15715
15716
15717
15718
15719
15720
15721
15722
15723
15724
15725
15726
15727
15728
15729
15730
15731
15732
15733
15734
15735
15736
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
15766
15767
15768
15769
15770
15771
15772
15773
15774
15775
15776
15777
15778
15779
15780
15781
15782
15783
15784
15785
15786
15787
15788
15789
15790
15791
15792
15793
15794
15795
15796
15797
15798
15799
15800
15801
15802
15803
15804
15805
15806
15807 --dist;
15808 --next;
15809 --near__;
15810 --lend;
15811 --z__;
15812 --y;
15813 --x;
15814 --list;
15815 --lptr;
15816
15817
15818 nn = *n;
15819 if (nn < 3) {
15820 *ier = -1;
15821 return 0;
15822 }
15823
15824
15825
15826 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15827 z__[3])) {
15828
15829
15830
15831 list[1] = 3;
15832 lptr[1] = 2;
15833 list[2] = -2;
15834 lptr[2] = 1;
15835 lend[1] = 2;
15836
15837 list[3] = 1;
15838 lptr[3] = 4;
15839 list[4] = -3;
15840 lptr[4] = 3;
15841 lend[2] = 4;
15842
15843 list[5] = 2;
15844 lptr[5] = 6;
15845 list[6] = -1;
15846 lptr[6] = 5;
15847 lend[3] = 6;
15848
15849 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15850 y[3], &z__[3])) {
15851
15852
15853
15854
15855
15856 list[1] = 2;
15857 lptr[1] = 2;
15858 list[2] = -3;
15859 lptr[2] = 1;
15860 lend[1] = 2;
15861
15862 list[3] = 3;
15863 lptr[3] = 4;
15864 list[4] = -1;
15865 lptr[4] = 3;
15866 lend[2] = 4;
15867
15868 list[5] = 1;
15869 lptr[5] = 6;
15870 list[6] = -2;
15871 lptr[6] = 5;
15872 lend[3] = 6;
15873
15874 } else {
15875
15876
15877
15878 *ier = -2;
15879 return 0;
15880 }
15881
15882
15883
15884 *lnew = 7;
15885 if (nn == 3) {
15886 *ier = 0;
15887 return 0;
15888 }
15889
15890
15891
15892
15893
15894
15895
15896
15897
15898
15899
15900
15901
15902
15903
15904
15905
15906
15907
15908
15909
15910
15911
15912
15913
15914
15915
15916 near__[1] = 0;
15917 near__[2] = 0;
15918 near__[3] = 0;
15919 for (k = nn; k >= 4; --k) {
15920 d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15921 d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15922 d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15923 if (d1 <= d2 && d1 <= d3) {
15924 near__[k] = 1;
15925 dist[k] = d1;
15926 next[k] = near__[1];
15927 near__[1] = k;
15928 } else if (d2 <= d1 && d2 <= d3) {
15929 near__[k] = 2;
15930 dist[k] = d2;
15931 next[k] = near__[2];
15932 near__[2] = k;
15933 } else {
15934 near__[k] = 3;
15935 dist[k] = d3;
15936 next[k] = near__[3];
15937 near__[3] = k;
15938 }
15939
15940 }
15941
15942
15943
15944 i__1 = nn;
15945 for (k = 4; k <= i__1; ++k) {
15946 addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15947 lend[1], lnew, ier);
15948 if (*ier != 0) {
15949 return 0;
15950 }
15951
15952
15953
15954
15955 i__ = near__[k];
15956 if (near__[i__] == k) {
15957 near__[i__] = next[k];
15958 } else {
15959 i__ = near__[i__];
15960 L2:
15961 i0 = i__;
15962 i__ = next[i0];
15963 if (i__ != k) {
15964 goto L2;
15965 }
15966 next[i0] = next[k];
15967 }
15968 near__[k] = 0;
15969
15970
15971
15972 lpl = lend[k];
15973 lp = lpl;
15974 L3:
15975 lp = lptr[lp];
15976 j = (i__2 = list[lp], abs(i__2));
15977
15978
15979
15980
15981
15982
15983
15984 i__ = near__[j];
15985 L4:
15986 if (i__ == 0) {
15987 goto L5;
15988 }
15989 nexti = next[i__];
15990
15991
15992
15993
15994 d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15995 if (d__ < dist[i__]) {
15996
15997
15998
15999
16000
16001 near__[i__] = k;
16002 dist[i__] = d__;
16003 if (i__ == near__[j]) {
16004 near__[j] = nexti;
16005 } else {
16006 next[i0] = nexti;
16007 }
16008 next[i__] = near__[k];
16009 near__[k] = i__;
16010 } else {
16011 i0 = i__;
16012 }
16013
16014
16015
16016 i__ = nexti;
16017 goto L4;
16018
16019
16020
16021 L5:
16022 if (lp != lpl) {
16023 goto L3;
16024 }
16025
16026 }
16027 return 0;
16028 }
16029
16030 int trplot_(int *lun, double *pltsiz, double *
16031 elat, double *elon, double *a, int *n, double *x,
16032 double *y, double *z__, int *list, int *lptr, int
16033 *lend, char *, long int *numbr, int *ier, short )
16034 {
16035
16036
16037 static long int annot = TRUE_;
16038 static double fsizn = 10.;
16039 static double fsizt = 16.;
16040 static double tol = .5;
16041
16042
16043 int i__1, i__2;
16044 double d__1;
16045
16046
16047
16048
16049
16050
16051
16052 static double t;
16053 static int n0, n1;
16054 static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
16055 static int ir, lp;
16056 static double ex, ey, ez, wr, tx, ty;
16057 static int lpl;
16058 static double wrs;
16059 static int ipx1, ipx2, ipy1, ipy2, nseg;
16060 int drwarc_(int *, double *, double *,
16061 double *, int *);
16062
16063
16064
16065
16066
16067
16068
16069
16070
16071
16072
16073
16074
16075
16076
16077
16078
16079
16080
16081
16082
16083
16084
16085
16086
16087
16088
16089
16090
16091
16092
16093
16094
16095
16096
16097
16098
16099
16100
16101
16102
16103
16104
16105
16106
16107
16108
16109
16110
16111
16112
16113
16114
16115
16116
16117
16118
16119
16120
16121
16122
16123
16124
16125
16126
16127
16128
16129
16130
16131
16132
16133
16134
16135
16136
16137
16138
16139
16140
16141
16142
16143
16144
16145
16146
16147
16148
16149
16150
16151
16152
16153
16154
16155
16156
16157
16158
16159
16160
16161
16162
16163
16164
16165
16166
16167
16168 --lend;
16169 --z__;
16170 --y;
16171 --x;
16172 --list;
16173 --lptr;
16174
16175
16176
16177
16178
16179
16180
16181
16182
16183
16184
16185
16186
16187
16188
16189
16190
16191
16192
16193
16194
16195
16196
16197
16198
16199
16200
16201
16202
16203
16204
16205
16206
16207
16208
16209
16210
16211
16212
16213
16214
16215
16216
16217
16218
16219
16220
16221
16222
16223
16224
16225
16226
16227
16228
16229 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16230 goto L11;
16231 }
16232 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16233 goto L12;
16234 }
16235
16236
16237
16238
16239 cf = atan(1.) / 45.;
16240 wr = sin(cf * *a);
16241 wrs = wr * wr;
16242
16243
16244
16245
16246
16247
16248
16249
16250
16251
16252 d__1 = *pltsiz * 36.;
16253 ir = i_dnnt(&d__1);
16254 ipx1 = 306 - ir;
16255 ipx2 = ir + 306;
16256 ipy1 = 396 - ir;
16257 ipy2 = ir + 396;
16258
16259
16260
16261
16262
16263
16264
16265
16266
16267
16268
16269
16270
16271
16272 d__1 = (double) ir * .88;
16273 ir = i_dnnt(&d__1);
16274 ipx1 = 306 - ir;
16275 ipx2 = ir + 306;
16276 ipy1 = 396 - ir;
16277 ipy2 = ir + 396;
16278
16279
16280
16281
16282 t = 2.;
16283
16284
16285
16286
16287
16288
16289
16290
16291
16292
16293 sf = (double) ir / wr;
16294 tx = ipx1 + sf * wr;
16295 ty = ipy1 + sf * wr;
16296
16297
16298
16299
16300
16301
16302
16303
16304 t = 1. / sf;
16305
16306
16307
16308
16309
16310
16311
16312
16313
16314
16315
16316
16317
16318
16319
16320
16321
16322
16323
16324
16325
16326
16327
16328 t = cf * *elon;
16329 ct = cos(cf * *elat);
16330 ex = ct * cos(t);
16331 ey = ct * sin(t);
16332 ez = sin(cf * *elat);
16333 if (ct != 0.) {
16334 r11 = -ey / ct;
16335 r12 = ex / ct;
16336 } else {
16337 r11 = 0.;
16338 r12 = 1.;
16339 }
16340 r21 = -ez * r12;
16341 r22 = ez * r11;
16342 r23 = ct;
16343
16344
16345
16346
16347 i__1 = *n;
16348 for (n0 = 1; n0 <= i__1; ++n0) {
16349 p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16350 if (p0[2] < 0.) {
16351 goto L3;
16352 }
16353 p0[0] = r11 * x[n0] + r12 * y[n0];
16354 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16355 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16356 goto L3;
16357 }
16358 lpl = lend[n0];
16359 lp = lpl;
16360
16361
16362
16363
16364 L1:
16365 lp = lptr[lp];
16366 n1 = (i__2 = list[lp], abs(i__2));
16367 p1[0] = r11 * x[n1] + r12 * y[n1];
16368 p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16369 p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16370 if (p1[2] < 0.) {
16371
16372
16373
16374
16375
16376 p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16377 p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16378 t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16379 p1[0] /= t;
16380 p1[1] /= t;
16381 }
16382
16383
16384
16385
16386 if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16387 goto L2;
16388 }
16389
16390
16391
16392
16393 if (p1[2] < 0.) {
16394 p1[2] = 0.;
16395 }
16396 d__1 = tol / sf;
16397 drwarc_(lun, p0, p1, &d__1, &nseg);
16398
16399
16400
16401 L2:
16402 if (lp != lpl) {
16403 goto L1;
16404 }
16405 L3:
16406 ;
16407 }
16408
16409
16410
16411
16412
16413
16414
16415 if (*numbr) {
16416
16417
16418
16419
16420
16421 t = fsizn / sf;
16422
16423
16424
16425
16426
16427
16428
16429 i__1 = *n;
16430 for (n0 = 1; n0 <= i__1; ++n0) {
16431 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16432 goto L4;
16433 }
16434 p0[0] = r11 * x[n0] + r12 * y[n0];
16435 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16436 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16437 goto L4;
16438 }
16439
16440
16441
16442
16443
16444
16445
16446
16447
16448 L4:
16449 ;
16450 }
16451 }
16452
16453
16454
16455
16456 t = fsizt / sf;
16457
16458
16459
16460
16461 p0[1] = wr + t * 3.;
16462
16463
16464
16465
16466
16467 if (annot) {
16468
16469
16470
16471 p0[0] = -wr;
16472 p0[1] = -wr - 50. / sf;
16473
16474
16475 p0[1] -= t * 2.;
16476
16477
16478
16479
16480
16481 }
16482
16483
16484
16485
16486
16487
16488
16489
16490
16491
16492
16493
16494
16495
16496
16497
16498
16499
16500 *ier = 0;
16501 return 0;
16502
16503
16504
16505 L11:
16506 *ier = 1;
16507 return 0;
16508
16509
16510
16511 L12:
16512 *ier = 2;
16513 return 0;
16514
16515
16516
16517
16518 *ier = 3;
16519 return 0;
16520 }
16521
16522 int trprnt_(int *n, double *x, double *y,
16523 double *z__, int *iflag, int *list, int *lptr,
16524 int *lend, int *lout)
16525 {
16526
16527
16528 static int nmax = 9999;
16529 static int nlmax = 58;
16530
16531
16532 int i__1;
16533
16534
16535 static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16536 400];
16537
16538
16539
16540
16541
16542
16543
16544
16545
16546
16547
16548
16549
16550
16551
16552
16553
16554
16555
16556
16557
16558
16559
16560
16561
16562
16563
16564
16565
16566
16567
16568
16569
16570
16571
16572
16573
16574
16575
16576
16577
16578
16579
16580
16581
16582
16583
16584
16585
16586
16587
16588
16589
16590
16591
16592
16593
16594
16595
16596
16597
16598
16599 --lend;
16600 --z__;
16601 --y;
16602 --x;
16603 --list;
16604 --lptr;
16605
16606
16607
16608
16609
16610
16611
16612
16613
16614
16615
16616
16617
16618
16619
16620
16621
16622
16623
16624
16625
16626
16627
16628
16629
16630
16631
16632 nn = *n;
16633 lun = *lout;
16634 if (lun < 0 || lun > 99) {
16635 lun = 6;
16636 }
16637
16638
16639
16640
16641 if (nn < 3 || nn > nmax) {
16642
16643
16644
16645
16646 return 0;
16647 }
16648
16649
16650
16651
16652 nl = 6;
16653 nb = 0;
16654 if (*iflag < 0) {
16655
16656
16657
16658
16659
16660 i__1 = nn;
16661 for (node = 1; node <= i__1; ++node) {
16662 lpl = lend[node];
16663 lp = lpl;
16664 k = 0;
16665
16666 L1:
16667 ++k;
16668 lp = lptr[lp];
16669 nd = list[lp];
16670 nabor[k - 1] = nd;
16671 if (lp != lpl) {
16672 goto L1;
16673 }
16674 if (nd <= 0) {
16675
16676
16677
16678
16679
16680 nabor[k - 1] = -nd;
16681 ++k;
16682 nabor[k - 1] = 0;
16683 ++nb;
16684 }
16685
16686
16687
16688 inc = (k - 1) / 14 + 2;
16689 nl += inc;
16690 if (nl > nlmax) {
16691
16692 nl = inc;
16693 }
16694
16695
16696
16697
16698 }
16699 } else if (*iflag > 0) {
16700
16701
16702
16703
16704 i__1 = nn;
16705 for (node = 1; node <= i__1; ++node) {
16706 lpl = lend[node];
16707 lp = lpl;
16708 k = 0;
16709
16710 L3:
16711 ++k;
16712 lp = lptr[lp];
16713 nd = list[lp];
16714 nabor[k - 1] = nd;
16715 if (lp != lpl) {
16716 goto L3;
16717 }
16718 if (nd <= 0) {
16719
16720
16721
16722 nabor[k - 1] = -nd;
16723 ++k;
16724 nabor[k - 1] = 0;
16725 ++nb;
16726 }
16727
16728
16729
16730 inc = (k - 1) / 8 + 2;
16731 nl += inc;
16732 if (nl > nlmax) {
16733
16734 nl = inc;
16735 }
16736
16737
16738
16739
16740
16741 }
16742 } else {
16743
16744
16745
16746
16747 i__1 = nn;
16748 for (node = 1; node <= i__1; ++node) {
16749 lpl = lend[node];
16750 lp = lpl;
16751 k = 0;
16752
16753 L5:
16754 ++k;
16755 lp = lptr[lp];
16756 nd = list[lp];
16757 nabor[k - 1] = nd;
16758 if (lp != lpl) {
16759 goto L5;
16760 }
16761 if (nd <= 0) {
16762
16763
16764
16765 nabor[k - 1] = -nd;
16766 ++k;
16767 nabor[k - 1] = 0;
16768 ++nb;
16769 }
16770
16771
16772
16773 inc = (k - 1) / 5 + 2;
16774 nl += inc;
16775 if (nl > nlmax) {
16776
16777 nl = inc;
16778 }
16779
16780
16781
16782
16783
16784 }
16785 }
16786
16787
16788
16789
16790 if (nb != 0) {
16791 na = nn * 3 - nb - 3;
16792 nt = (nn << 1) - nb - 2;
16793 } else {
16794 na = nn * 3 - 6;
16795 nt = (nn << 1) - 4;
16796 }
16797
16798 return 0;
16799
16800
16801
16802
16803
16804
16805
16806
16807
16808
16809
16810
16811
16812
16813
16814
16815
16816
16817
16818
16819 }
16820
16821 int vrplot_(int *lun, double *pltsiz, double *
16822 elat, double *elon, double *a, int *n, double *x,
16823 double *y, double *z__, int *nt, int *listc, int *
16824 lptr, int *lend, double *xc, double *yc, double *zc,
16825 char *, long int *numbr, int *ier, short)
16826 {
16827
16828
16829 static long int annot = TRUE_;
16830 static double fsizn = 10.;
16831 static double fsizt = 16.;
16832 static double tol = .5;
16833
16834
16835 int i__1;
16836 double d__1;
16837
16838
16839
16840
16841
16842
16843
16844 static double t;
16845 static int n0;
16846 static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16847 sf;
16848 static int ir, lp;
16849 static double ex, ey, ez, wr, tx, ty;
16850 static long int in1, in2;
16851 static int kv1, kv2, lpl;
16852 static double wrs;
16853 static int ipx1, ipx2, ipy1, ipy2, nseg;
16854 int drwarc_(int *, double *, double *,
16855 double *, int *);
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
16915
16916
16917
16918
16919
16920
16921
16922
16923
16924
16925
16926
16927
16928
16929
16930
16931
16932
16933
16934
16935
16936
16937
16938
16939
16940
16941
16942
16943
16944
16945
16946
16947
16948
16949
16950
16951
16952
16953
16954
16955
16956
16957
16958
16959
16960
16961
16962
16963
16964
16965
16966
16967
16968
16969
16970
16971
16972
16973
16974
16975
16976
16977
16978
16979
16980
16981
16982
16983
16984
16985
16986
16987
16988
16989
16990
16991
16992
16993
16994
16995
16996
16997
16998 --lend;
16999 --z__;
17000 --y;
17001 --x;
17002 --zc;
17003 --yc;
17004 --xc;
17005 --listc;
17006 --lptr;
17007
17008
17009
17010
17011
17012
17013
17014
17015
17016
17017
17018
17019
17020
17021
17022
17023
17024
17025
17026
17027
17028
17029
17030
17031
17032
17033
17034
17035
17036
17037
17038
17039
17040
17041
17042
17043
17044
17045
17046
17047
17048
17049
17050
17051
17052
17053
17054
17055
17056
17057
17058
17059
17060
17061
17062
17063
17064
17065 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
17066 nt != 2 * *n - 4) {
17067 goto L11;
17068 }
17069 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
17070 goto L12;
17071 }
17072
17073
17074
17075
17076 cf = atan(1.) / 45.;
17077 wr = sin(cf * *a);
17078 wrs = wr * wr;
17079
17080
17081
17082
17083
17084
17085
17086
17087
17088
17089 d__1 = *pltsiz * 36.;
17090 ir = i_dnnt(&d__1);
17091 ipx1 = 306 - ir;
17092 ipx2 = ir + 306;
17093 ipy1 = 396 - ir;
17094 ipy2 = ir + 396;
17095
17096
17097
17098
17099
17100
17101
17102
17103
17104
17105
17106
17107
17108 d__1 = (double) ir * .88;
17109 ir = i_dnnt(&d__1);
17110 ipx1 = 306 - ir;
17111 ipx2 = ir + 306;
17112 ipy1 = 396 - ir;
17113 ipy2 = ir + 396;
17114
17115
17116
17117
17118 t = 2.;
17119
17120
17121
17122
17123
17124
17125
17126
17127
17128
17129 sf = (double) ir / wr;
17130 tx = ipx1 + sf * wr;
17131 ty = ipy1 + sf * wr;
17132
17133
17134
17135
17136
17137
17138
17139
17140 t = 1. / sf;
17141
17142
17143
17144
17145
17146
17147
17148
17149
17150
17151
17152
17153
17154
17155
17156
17157
17158
17159
17160
17161
17162
17163
17164 t = cf * *elon;
17165 ct = cos(cf * *elat);
17166 ex = ct * cos(t);
17167 ey = ct * sin(t);
17168 ez = sin(cf * *elat);
17169 if (ct != 0.) {
17170 r11 = -ey / ct;
17171 r12 = ex / ct;
17172 } else {
17173 r11 = 0.;
17174 r12 = 1.;
17175 }
17176 r21 = -ez * r12;
17177 r22 = ez * r11;
17178 r23 = ct;
17179
17180
17181
17182
17183 i__1 = *n;
17184 for (n0 = 1; n0 <= i__1; ++n0) {
17185 lpl = lend[n0];
17186
17187
17188
17189
17190 kv2 = listc[lpl];
17191 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17192 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17193 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17194
17195
17196
17197 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17198
17199
17200
17201
17202 lp = lpl;
17203 L1:
17204 lp = lptr[lp];
17205 kv1 = kv2;
17206 p1[0] = p2[0];
17207 p1[1] = p2[1];
17208 p1[2] = p2[2];
17209 in1 = in2;
17210 kv2 = listc[lp];
17211
17212
17213
17214 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17215 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17216 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17217 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17218
17219
17220
17221
17222
17223 if (! in1 || (in2 && kv2 <= kv1)) {
17224 goto L2;
17225 }
17226 if (p2[2] < 0.) {
17227
17228
17229
17230
17231
17232 p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17233 p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17234 t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17235 p2[0] /= t;
17236 p2[1] /= t;
17237 }
17238
17239
17240
17241
17242 if (p2[2] < 0.) {
17243 p2[2] = 0.f;
17244 }
17245 d__1 = tol / sf;
17246 drwarc_(lun, p1, p2, &d__1, &nseg);
17247
17248
17249
17250 L2:
17251 if (lp != lpl) {
17252 goto L1;
17253 }
17254
17255 }
17256
17257
17258
17259
17260
17261
17262
17263 if (*numbr) {
17264
17265
17266
17267
17268
17269 t = fsizn / sf;
17270
17271
17272
17273
17274
17275
17276
17277 i__1 = *n;
17278 for (n0 = 1; n0 <= i__1; ++n0) {
17279 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17280 goto L4;
17281 }
17282 x0 = r11 * x[n0] + r12 * y[n0];
17283 y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17284 if (x0 * x0 + y0 * y0 > wrs) {
17285 goto L4;
17286 }
17287
17288
17289
17290
17291
17292
17293
17294
17295 L4:
17296 ;
17297 }
17298 }
17299
17300
17301
17302
17303 t = fsizt / sf;
17304
17305
17306
17307
17308 y0 = wr + t * 3.;
17309
17310
17311
17312
17313
17314 if (annot) {
17315
17316
17317
17318 x0 = -wr;
17319 y0 = -wr - 50. / sf;
17320
17321
17322 y0 -= t * 2.;
17323
17324
17325
17326
17327
17328 }
17329
17330
17331
17332
17333
17334
17335
17336
17337
17338
17339
17340
17341
17342
17343
17344
17345
17346
17347 *ier = 0;
17348 return 0;
17349
17350
17351
17352 L11:
17353 *ier = 1;
17354 return 0;
17355
17356
17357
17358 L12:
17359 *ier = 2;
17360 return 0;
17361
17362
17363
17364
17365 *ier = 3;
17366 return 0;
17367 }
17368
17369 int random_(int *ix, int *iy, int *iz,
17370 double *rannum)
17371 {
17372 static double x;
17373
17374
17375
17376
17377
17378
17379
17380
17381
17382
17383
17384
17385
17386
17387
17388 *ix = *ix * 171 % 30269;
17389 *iy = *iy * 172 % 30307;
17390 *iz = *iz * 170 % 30323;
17391 x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17392 double) (*iz) / 30323.;
17393 *rannum = x - (int) x;
17394 return 0;
17395 }
17396
17397 #undef TRUE_
17398 #undef FALSE_
17399 #undef abs
17400
17401
17402
17403
17404
17405
17406
17407
17408
17409 EMData* Util::mult_scalar(EMData* img, float scalar)
17410 {
17411 ENTERFUNC;
17412
17413 if (!img) {
17414 throw NullPointerException("NULL input image");
17415 }
17416
17417
17418 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17419 size_t size = (size_t)nx*ny*nz;
17420 EMData * img2 = img->copy_head();
17421 float *img_ptr =img->get_data();
17422 float *img2_ptr = img2->get_data();
17423 for (size_t i=0;i<size;++i)img2_ptr[i] = img_ptr[i]*scalar;
17424 img2->update();
17425
17426 if(img->is_complex()) {
17427 img2->set_complex(true);
17428 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17429 }
17430 EXITFUNC;
17431 return img2;
17432 }
17433
17434 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17435 {
17436 ENTERFUNC;
17437
17438 if (!img) {
17439 throw NullPointerException("NULL input image");
17440 }
17441
17442
17443 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17444 size_t size = (size_t)nx*ny*nz;
17445 EMData * img2 = img->copy_head();
17446 float *img_ptr =img->get_data();
17447 float *img2_ptr = img2->get_data();
17448 float *img1_ptr = img1->get_data();
17449 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17450 img2->update();
17451 if(img->is_complex()) {
17452 img2->set_complex(true);
17453 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17454 }
17455
17456 EXITFUNC;
17457 return img2;
17458 }
17459
17460 EMData* Util::addn_img(EMData* img, EMData* img1)
17461 {
17462 ENTERFUNC;
17463
17464 if (!img) {
17465 throw NullPointerException("NULL input image");
17466 }
17467
17468
17469 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17470 size_t size = (size_t)nx*ny*nz;
17471 EMData * img2 = img->copy_head();
17472 float *img_ptr =img->get_data();
17473 float *img2_ptr = img2->get_data();
17474 float *img1_ptr = img1->get_data();
17475 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17476 img2->update();
17477 if(img->is_complex()) {
17478 img2->set_complex(true);
17479 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17480 }
17481
17482 EXITFUNC;
17483 return img2;
17484 }
17485
17486 EMData* Util::subn_img(EMData* img, EMData* img1)
17487 {
17488 ENTERFUNC;
17489
17490 if (!img) {
17491 throw NullPointerException("NULL input image");
17492 }
17493
17494
17495 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17496 size_t size = (size_t)nx*ny*nz;
17497 EMData * img2 = img->copy_head();
17498 float *img_ptr =img->get_data();
17499 float *img2_ptr = img2->get_data();
17500 float *img1_ptr = img1->get_data();
17501 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17502 img2->update();
17503 if(img->is_complex()) {
17504 img2->set_complex(true);
17505 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17506 }
17507
17508 EXITFUNC;
17509 return img2;
17510 }
17511
17512 EMData* Util::muln_img(EMData* img, EMData* img1)
17513 {
17514 ENTERFUNC;
17515
17516 if (!img) {
17517 throw NullPointerException("NULL input image");
17518 }
17519
17520
17521 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17522 size_t size = (size_t)nx*ny*nz;
17523 EMData * img2 = img->copy_head();
17524 float *img_ptr =img->get_data();
17525 float *img2_ptr = img2->get_data();
17526 float *img1_ptr = img1->get_data();
17527 if(img->is_complex()) {
17528 for (size_t i=0; i<size; i+=2) {
17529 img2_ptr[i] = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17530 img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17531 }
17532 img2->set_complex(true);
17533 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17534 } else {
17535 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17536 img2->update();
17537 }
17538
17539 EXITFUNC;
17540 return img2;
17541 }
17542
17543 EMData* Util::divn_img(EMData* img, EMData* img1)
17544 {
17545 ENTERFUNC;
17546
17547 if (!img) {
17548 throw NullPointerException("NULL input image");
17549 }
17550
17551
17552 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17553 size_t size = (size_t)nx*ny*nz;
17554 EMData * img2 = img->copy_head();
17555 float *img_ptr =img->get_data();
17556 float *img2_ptr = img2->get_data();
17557 float *img1_ptr = img1->get_data();
17558 if(img->is_complex()) {
17559 float sq2;
17560 for (size_t i=0; i<size; i+=2) {
17561 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17562 img2_ptr[i] = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17563 img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17564 }
17565 img2->set_complex(true);
17566 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17567 } else {
17568 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17569 img2->update();
17570 }
17571
17572 EXITFUNC;
17573 return img2;
17574 }
17575
17576 EMData* Util::divn_filter(EMData* img, EMData* img1)
17577 {
17578 ENTERFUNC;
17579
17580 if (!img) {
17581 throw NullPointerException("NULL input image");
17582 }
17583
17584
17585 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17586 size_t size = (size_t)nx*ny*nz;
17587 EMData * img2 = img->copy_head();
17588 float *img_ptr =img->get_data();
17589 float *img1_ptr = img1->get_data();
17590 float *img2_ptr = img2->get_data();
17591 if(img->is_complex()) {
17592 for (size_t i=0; i<size; i+=2) {
17593 if(img1_ptr[i] > 1.e-10f) {
17594 img2_ptr[i] = img_ptr[i] /img1_ptr[i];
17595 img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17596 } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17597 }
17598 } else throw ImageFormatException("Only Fourier image allowed");
17599
17600 img->update();
17601
17602 EXITFUNC;
17603 return img2;
17604 }
17605
17606 void Util::mul_scalar(EMData* img, float scalar)
17607 {
17608 ENTERFUNC;
17609
17610 if (!img) {
17611 throw NullPointerException("NULL input image");
17612 }
17613
17614
17615 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17616 size_t size = (size_t)nx*ny*nz;
17617 float *img_ptr =img->get_data();
17618 for (size_t i=0;i<size;++i) img_ptr[i] *= scalar;
17619 img->update();
17620
17621 EXITFUNC;
17622 }
17623
17624 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17625 {
17626 ENTERFUNC;
17627
17628 if (!img) {
17629 throw NullPointerException("NULL input image");
17630 }
17631
17632
17633 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17634 size_t size = (size_t)nx*ny*nz;
17635 float *img_ptr =img->get_data();
17636 float *img1_ptr = img1->get_data();
17637 for (size_t i=0;i<size;++i)img_ptr[i] += img1_ptr[i]*scalar;
17638 img1->update();
17639
17640 EXITFUNC;
17641 }
17642
17643 void Util::add_img(EMData* img, EMData* img1)
17644 {
17645 ENTERFUNC;
17646
17647 if (!img) {
17648 throw NullPointerException("NULL input image");
17649 }
17650
17651
17652 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17653 size_t size = (size_t)nx*ny*nz;
17654 float *img_ptr = img->get_data();
17655 float *img1_ptr = img1->get_data();
17656 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i];
17657 img->update();
17658
17659 EXITFUNC;
17660 }
17661
17662 void Util::add_img_abs(EMData* img, EMData* img1)
17663 {
17664 ENTERFUNC;
17665
17666 if (!img) {
17667 throw NullPointerException("NULL input image");
17668 }
17669
17670
17671 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17672 size_t size = (size_t)nx*ny*nz;
17673 float *img_ptr = img->get_data();
17674 float *img1_ptr = img1->get_data();
17675 for (size_t i=0;i<size;++i) img_ptr[i] += abs(img1_ptr[i]);
17676 img->update();
17677
17678 EXITFUNC;
17679 }
17680
17681 void Util::add_img2(EMData* img, EMData* img1)
17682 {
17683 ENTERFUNC;
17684
17685 if (!img) {
17686 throw NullPointerException("NULL input image");
17687 }
17688
17689
17690 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17691 size_t size = (size_t)nx*ny*nz;
17692 float *img_ptr = img->get_data();
17693 float *img1_ptr = img1->get_data();
17694 if(img->is_complex()) {
17695 for (size_t i=0; i<size; i+=2) img_ptr[i] += img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1] ;
17696 } else {
17697 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17698 }
17699 img->update();
17700
17701 EXITFUNC;
17702 }
17703
17704 void Util::sub_img(EMData* img, EMData* img1)
17705 {
17706 ENTERFUNC;
17707
17708 if (!img) {
17709 throw NullPointerException("NULL input image");
17710 }
17711
17712
17713 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17714 size_t size = (size_t)nx*ny*nz;
17715 float *img_ptr = img->get_data();
17716 float *img1_ptr = img1->get_data();
17717 for (size_t i=0;i<size;++i) img_ptr[i] -= img1_ptr[i];
17718 img->update();
17719
17720 EXITFUNC;
17721 }
17722
17723 void Util::mul_img(EMData* img, EMData* img1)
17724 {
17725 ENTERFUNC;
17726
17727 if (!img) {
17728 throw NullPointerException("NULL input image");
17729 }
17730
17731
17732 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17733 size_t size = (size_t)nx*ny*nz;
17734 float *img_ptr = img->get_data();
17735 float *img1_ptr = img1->get_data();
17736 if(img->is_complex()) {
17737 for (size_t i=0; i<size; i+=2) {
17738 float tmp = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17739 img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17740 img_ptr[i] = tmp;
17741
17742 }
17743 } else {
17744 for (size_t i=0;i<size;++i) img_ptr[i] *= img1_ptr[i];
17745 }
17746 img->update();
17747
17748 EXITFUNC;
17749 }
17750
17751 void Util::div_img(EMData* img, EMData* img1)
17752 {
17753 ENTERFUNC;
17754
17755 if (!img) {
17756 throw NullPointerException("NULL input image");
17757 }
17758
17759
17760 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17761 size_t size = (size_t)nx*ny*nz;
17762 float *img_ptr = img->get_data();
17763 float *img1_ptr = img1->get_data();
17764 if(img->is_complex()) {
17765 float sq2;
17766 for (size_t i=0; i<size; i+=2) {
17767 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17768 float tmp = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17769 img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17770 img_ptr[i] = tmp;
17771 }
17772 } else {
17773 for (size_t i=0; i<size; ++i) img_ptr[i] /= img1_ptr[i];
17774 }
17775 img->update();
17776
17777 EXITFUNC;
17778 }
17779
17780 void Util::div_filter(EMData* img, EMData* img1)
17781 {
17782 ENTERFUNC;
17783
17784 if (!img) {
17785 throw NullPointerException("NULL input image");
17786 }
17787
17788
17789 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17790 size_t size = (size_t)nx*ny*nz;
17791 float *img_ptr = img->get_data();
17792 float *img1_ptr = img1->get_data();
17793 if(img->is_complex()) {
17794 for (size_t i=0; i<size; i+=2) {
17795 if(img1_ptr[i] > 1.e-10f) {
17796 img_ptr[i] /= img1_ptr[i];
17797 img_ptr[i+1] /= img1_ptr[i];
17798 } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17799 }
17800 } else throw ImageFormatException("Only Fourier image allowed");
17801
17802 img->update();
17803
17804 EXITFUNC;
17805 }
17806
17807 #define img_ptr(i,j,k) img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*(size_t)nxo]
17808
17809 EMData* Util::pack_complex_to_real(EMData* img)
17810 {
17811 ENTERFUNC;
17812
17813 if (!img) {
17814 throw NullPointerException("NULL input image");
17815 }
17816
17817
17818
17819 int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17820 int nx = nxo - 2 + img->is_fftodd();
17821 int lsd2 = (nx + 2 - nx%2) / 2;
17822 int nyt, nzt;
17823 int nx2 = nx/2;
17824 int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17825 int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17826 int nx2p = nx2+nx%2;
17827 int ny2p = ny2+ny%2;
17828 int nz2p = nz2+nz%2;
17829 EMData& power = *(new EMData());
17830 power.set_size(nx, ny, nz);
17831 power.set_array_offsets(-nx2,-ny2,-nz2);
17832
17833 float *img_ptr = img->get_data();
17834 for (int iz = 1; iz <= nz; iz++) {
17835 int jz=iz-1;
17836 if(jz>=nz2p) jz=jz-nzt;
17837 for (int iy = 1; iy <= ny; iy++) {
17838 int jy=iy-1;
17839 if(jy>=ny2p) jy=jy-nyt;
17840 for (int ix = 1; ix <= lsd2; ix++) {
17841 int jx=ix-1;
17842 if(jx>=nx2p) jx=jx-nx;
17843 power(jx,jy,jz) = img_ptr(ix,iy,iz);
17844 }
17845 }
17846 }
17847
17848 int nzb, nze, nyb, nye, nxb, nxe;
17849 nxb =-nx2+(nx+1)%2;
17850 nxe = nx2-(nx+1)%2;
17851 if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17852 if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17853 for (int iz = nzb; iz <= nze; iz++) {
17854 for (int iy = nyb; iy <= nye; iy++) {
17855 for (int ix = 1; ix <= nxe; ix++) {
17856 power(-ix,-iy,-iz) = power(ix,iy,iz);
17857 }
17858 }
17859 }
17860 if(ny2 != 0) {
17861 if(nz2 != 0) {
17862 if(nz%2 == 0) {
17863 for (int iy = nyb; iy <= nye; iy++) {
17864 for (int ix = nxb; ix <= -1; ix++) {
17865 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17866 }
17867 }
17868 if(ny%2 == 0) {
17869 for (int ix = nxb; ix <= -1; ix++) {
17870 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17871 }
17872 }
17873 }
17874 }
17875 if(ny%2 == 0) {
17876 for (int iz = nzb; iz <= nze; iz++) {
17877 for (int ix = nxb; ix <= -1; ix++) {
17878 power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17879 }
17880 }
17881 }
17882
17883 }
17884 power.update();
17885 power.set_array_offsets(0,0,0);
17886 return &power;
17887 }
17888 #undef img_ptr
17889
17890 float Util::ang_n(float peakp, string mode, int maxrin)
17891 {
17892 if (mode == "f" || mode == "F")
17893 return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17894 else
17895 return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17896 }
17897
17898
17899 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17900 {
17901 float* data = ring->get_data();
17902 float av=0.0;
17903 float sq=0.0;
17904 float nn=0.0;
17905 int nring = numr.size()/3;
17906 for( int i=0; i < nring; ++i )
17907 {
17908 int numr3i = numr[3*i+2];
17909 int numr2i = numr[3*i+1]-1;
17910 float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17911 for( int j=0; j < numr3i; ++j )
17912 {
17913 int jc = numr2i+j;
17914 av += data[jc] * w;
17915 sq += data[jc] * data[jc] * w;
17916 nn += w;
17917 }
17918 }
17919
17920 float avg = av/nn;
17921 float sgm = sqrt( (sq-av*av/nn)/nn );
17922 size_t n = (size_t)ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17923 for( size_t i=0; i < n; ++i )
17924 {
17925 data[i] -= avg;
17926 data[i] /= sgm;
17927 }
17928
17929 ring->update();
17930 }
17931
17932 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17933 float xrng, float yrng, float step, string mode,
17934 vector<int>numr, float cnx, float cny) {
17935
17936
17937
17938
17939
17940
17941
17942
17943
17944
17945
17946
17947 size_t crefim_len = crefim.size();
17948
17949 int ky = int(2*yrng/step+0.5)/2;
17950 int kx = int(2*xrng/step+0.5)/2;
17951 int iref, nref=0, mirror=0;
17952 float iy, ix, sx=0, sy=0;
17953 float peak = -1.0E23f;
17954 float ang=0.0f;
17955 for (int i = -ky; i <= ky; i++) {
17956 iy = i * step ;
17957 for (int j = -kx; j <= kx; j++) {
17958 ix = j*step ;
17959 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17960
17961 Normalize_ring( cimage, numr );
17962
17963 Frngs(cimage, numr);
17964
17965
17966 for ( iref = 0; iref < (int)crefim_len; iref++) {
17967 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17968 double qn = retvals["qn"];
17969 double qm = retvals["qm"];
17970 if(qn >= peak || qm >= peak) {
17971 sx = -ix;
17972 sy = -iy;
17973 nref = iref;
17974 if (qn >= qm) {
17975 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17976 peak = static_cast<float>(qn);
17977 mirror = 0;
17978 } else {
17979 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17980 peak = static_cast<float>(qm);
17981 mirror = 1;
17982 }
17983 }
17984 } delete cimage; cimage = 0;
17985 }
17986 }
17987 float co, so, sxs, sys;
17988 co = static_cast<float>( cos(ang*pi/180.0) );
17989 so = static_cast<float>( -sin(ang*pi/180.0) );
17990 sxs = sx*co - sy*so;
17991 sys = sx*so + sy*co;
17992 vector<float> res;
17993 res.push_back(ang);
17994 res.push_back(sxs);
17995 res.push_back(sys);
17996 res.push_back(static_cast<float>(mirror));
17997 res.push_back(static_cast<float>(nref));
17998 res.push_back(peak);
17999 return res;
18000 }
18001
18002 vector<float> Util::multiref_polar_ali_2d_peaklist(EMData* image, const vector< EMData* >& crefim,
18003 float xrng, float yrng, float step, string mode,
18004 vector<int>numr, float cnx, float cny) {
18005
18006 size_t crefim_len = crefim.size();
18007
18008 int ky = int(2*yrng/step+0.5)/2;
18009 int kx = int(2*xrng/step+0.5)/2;
18010 float iy, ix;
18011 vector<float> peak(crefim_len*5, -1.0e23f);
18012 for (int i = -ky; i <= ky; i++) {
18013 iy = i * step ;
18014 for (int j = -kx; j <= kx; j++) {
18015 ix = j*step ;
18016 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18017 Normalize_ring( cimage, numr );
18018 Frngs(cimage, numr);
18019 for (int iref = 0; iref < (int)crefim_len; iref++) {
18020 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18021 double qn = retvals["qn"];
18022 double qm = retvals["qm"];
18023 if(qn >= peak[iref*5] || qm >= peak[iref*5]) {
18024 if (qn >= qm) {
18025 peak[iref*5] = static_cast<float>(qn);
18026 peak[iref*5+1] = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18027 peak[iref*5+2] = -ix;
18028 peak[iref*5+3] = -iy;
18029 peak[iref*5+4] = 0;
18030 } else {
18031 peak[iref*5] = static_cast<float>(qm);
18032 peak[iref*5+1] = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18033 peak[iref*5+2] = -ix;
18034 peak[iref*5+3] = -iy;
18035 peak[iref*5+4] = 1;
18036 }
18037 }
18038 } delete cimage; cimage = 0;
18039 }
18040 }
18041 for (int iref = 0; iref < (int)crefim_len; iref++) {
18042 float ang = peak[iref*5+1];
18043 float sx = peak[iref*5+2];
18044 float sy = peak[iref*5+3];
18045 float co = cos(ang*pi/180.0);
18046 float so = -sin(ang*pi/180.0);
18047 float sxs = sx*co - sy*so;
18048 float sys = sx*so + sy*co;
18049 peak[iref*5+2] = sxs;
18050 peak[iref*5+3] = sys;
18051 }
18052 return peak;
18053 }
18054
18055 struct peak_table {
18056 float value;
18057 int index;
18058 bool operator<(const peak_table& b) const { return value > b.value; }
18059 };
18060
18061 vector<int> Util::assign_groups(const vector<float>& d, int nref, int nima) {
18062
18063 int kt = nref;
18064 unsigned int maxasi = nima/nref;
18065 vector< vector<int> > id_list;
18066 id_list.resize(nref);
18067 int group, ima;
18068
18069 peak_table* dd = new peak_table[nref*nima];
18070 for (int i=0; i<nref*nima; i++) {
18071 dd[i].value = d[i];
18072 dd[i].index = i;
18073 }
18074 sort(dd, dd+nref*nima);
18075 int begin = 0;
18076
18077 bool* del_row = new bool[nref];
18078 for (int i=0; i<nref; i++) del_row[i] = false;
18079 bool* del_column = new bool[nima];
18080 for (int i=0; i<nima; i++) del_column[i] = false;
18081 while (kt > 0) {
18082 bool flag = true;
18083 while (flag) {
18084 int l = dd[begin].index;
18085 group = l/nima;
18086 ima = l%nima;
18087 if (del_column[ima] || del_row[group]) begin++;
18088 else flag = false;
18089 }
18090
18091 id_list[group].push_back(ima);
18092 if (kt > 1) {
18093 if (id_list[group].size() < maxasi) group = -1;
18094 else kt -= 1;
18095 } else {
18096 if (id_list[group].size() < maxasi+nima%nref) group = -1;
18097 else kt -= 1;
18098 }
18099 del_column[ima] = true;
18100 if (group != -1) {
18101 del_row[group] = true;
18102 }
18103 }
18104
18105 vector<int> id_list_1;
18106 for (int iref=0; iref<nref; iref++)
18107 for (unsigned int im=0; im<maxasi; im++)
18108 id_list_1.push_back(id_list[iref][im]);
18109 for (unsigned int im=maxasi; im<maxasi+nima%nref; im++)
18110 id_list_1.push_back(id_list[group][im]);
18111 id_list_1.push_back(group);
18112
18113 delete[] del_row;
18114 delete[] del_column;
18115 delete[] dd;
18116 return id_list_1;
18117 }
18118
18119
18120 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
18121 float xrng, float yrng, float step, string mode,
18122 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
18123
18124
18125
18126
18127
18128
18129
18130
18131
18132
18133
18134
18135 size_t crefim_len = crefim.size();
18136
18137 int ky = int(2*yrng/step+0.5)/2;
18138 int kx = int(2*xrng/step+0.5)/2;
18139 int iref, nref=0, mirror=0;
18140 float iy, ix, sx=0, sy=0;
18141 float peak = -1.0E23f;
18142 float ang=0.0f;
18143 for (int i = -ky; i <= ky; i++) {
18144 iy = i * step ;
18145 for (int j = -kx; j <= kx; j++) {
18146 ix = j*step ;
18147 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18148
18149 Normalize_ring( cimage, numr );
18150
18151 Frngs(cimage, numr);
18152
18153
18154 for ( iref = 0; iref < (int)crefim_len; iref++) {
18155 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
18156 double qn = retvals["qn"];
18157 double qm = retvals["qm"];
18158 if(qn >= peak || qm >= peak) {
18159 sx = -ix;
18160 sy = -iy;
18161 nref = iref;
18162 if (qn >= qm) {
18163 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18164 peak = static_cast<float>(qn);
18165 mirror = 0;
18166 } else {
18167 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18168 peak = static_cast<float>(qm);
18169 mirror = 1;
18170 }
18171 }
18172 } delete cimage; cimage = 0;
18173 }
18174 }
18175 float co, so, sxs, sys;
18176 co = static_cast<float>( cos(ang*pi/180.0) );
18177 so = static_cast<float>( -sin(ang*pi/180.0) );
18178 sxs = sx*co - sy*so;
18179 sys = sx*so + sy*co;
18180 vector<float> res;
18181 res.push_back(ang);
18182 res.push_back(sxs);
18183 res.push_back(sys);
18184 res.push_back(static_cast<float>(mirror));
18185 res.push_back(static_cast<float>(nref));
18186 res.push_back(peak);
18187 return res;
18188 }
18189
18190 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
18191 float xrng, float yrng, float step, string mode,
18192 vector< int >numr, float cnx, float cny) {
18193
18194
18195
18196
18197
18198
18199
18200
18201
18202
18203
18204 size_t crefim_len = crefim.size();
18205
18206 int ky = int(2*yrng/step+0.5)/2;
18207 int kx = int(2*xrng/step+0.5)/2;
18208 int iref, nref=0;
18209 float iy, ix, sx=0, sy=0;
18210 float peak = -1.0E23f;
18211 float ang=0.0f;
18212 for (int i = -ky; i <= ky; i++) {
18213 iy = i * step ;
18214 for (int j = -kx; j <= kx; j++) {
18215 ix = j*step ;
18216 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18217 Frngs(cimage, numr);
18218
18219
18220 for ( iref = 0; iref < (int)crefim_len; iref++) {
18221 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
18222 double qn = retvals["qn"];
18223 if(qn >= peak) {
18224 sx = -ix;
18225 sy = -iy;
18226 nref = iref;
18227 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18228 peak = static_cast<float>(qn);
18229 }
18230 } delete cimage; cimage = 0;
18231 }
18232 }
18233 float co, so, sxs, sys;
18234 co = static_cast<float>( cos(ang*pi/180.0) );
18235 so = static_cast<float>( -sin(ang*pi/180.0) );
18236 sxs = sx*co - sy*so;
18237 sys = sx*so + sy*co;
18238 vector<float> res;
18239 res.push_back(ang);
18240 res.push_back(sxs);
18241 res.push_back(sys);
18242 res.push_back(static_cast<float>(nref));
18243 res.push_back(peak);
18244 return res;
18245 }
18246
18247 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
18248 float xrng, float yrng, float step, float ant, string mode,
18249 vector<int>numr, float cnx, float cny) {
18250
18251
18252
18253
18254
18255
18256
18257
18258
18259
18260
18261 size_t crefim_len = crefim.size();
18262 const float qv = static_cast<float>( pi/180.0 );
18263
18264 Transform * t = image->get_attr("xform.projection");
18265 Dict d = t->get_params("spider");
18266 if(t) {delete t; t=0;}
18267 float phi = d["phi"];
18268 float theta = d["theta"];
18269 int ky = int(2*yrng/step+0.5)/2;
18270 int kx = int(2*xrng/step+0.5)/2;
18271 int iref, nref=0, mirror=0;
18272 float iy, ix, sx=0, sy=0;
18273 float peak = -1.0E23f;
18274 float ang=0.0f;
18275 float imn1 = sin(theta*qv)*cos(phi*qv);
18276 float imn2 = sin(theta*qv)*sin(phi*qv);
18277 float imn3 = cos(theta*qv);
18278 vector<float> n1(crefim_len);
18279 vector<float> n2(crefim_len);
18280 vector<float> n3(crefim_len);
18281 for ( iref = 0; iref < (int)crefim_len; iref++) {
18282 n1[iref] = crefim[iref]->get_attr("n1");
18283 n2[iref] = crefim[iref]->get_attr("n2");
18284 n3[iref] = crefim[iref]->get_attr("n3");
18285 }
18286 for (int i = -ky; i <= ky; i++) {
18287 iy = i * step ;
18288 for (int j = -kx; j <= kx; j++) {
18289 ix = j*step;
18290 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18291
18292 Normalize_ring( cimage, numr );
18293
18294 Frngs(cimage, numr);
18295
18296
18297 for ( iref = 0; iref < (int)crefim_len; iref++) {
18298 if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18299 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18300 double qn = retvals["qn"];
18301 double qm = retvals["qm"];
18302 if(qn >= peak || qm >= peak) {
18303 sx = -ix;
18304 sy = -iy;
18305 nref = iref;
18306 if (qn >= qm) {
18307 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18308 peak = static_cast<float>( qn );
18309 mirror = 0;
18310 } else {
18311 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18312 peak = static_cast<float>( qm );
18313 mirror = 1;
18314 }
18315 }
18316 }
18317 } delete cimage; cimage = 0;
18318 }
18319 }
18320 float co, so, sxs, sys;
18321 if(peak == -1.0E23) {
18322 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18323 nref = -1;
18324 } else {
18325 co = cos(ang*qv);
18326 so = -sin(ang*qv);
18327 sxs = sx*co - sy*so;
18328 sys = sx*so + sy*co;
18329 }
18330 vector<float> res;
18331 res.push_back(ang);
18332 res.push_back(sxs);
18333 res.push_back(sys);
18334 res.push_back(static_cast<float>(mirror));
18335 res.push_back(static_cast<float>(nref));
18336 res.push_back(peak);
18337 return res;
18338 }
18339
18340 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18341 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18342 vector<int>numr, float cnx, float cny) {
18343
18344
18345
18346
18347
18348
18349
18350
18351
18352
18353
18354 size_t crefim_len = crefim.size();
18355 const float qv = static_cast<float>(pi/180.0);
18356
18357 Transform* t = image->get_attr("xform.projection");
18358 Dict d = t->get_params("spider");
18359 if(t) {delete t; t=0;}
18360 float phi = d["phi"];
18361 float theta = d["theta"];
18362 float psi = d["psi"];
18363 int ky = int(2*yrng/step+0.5)/2;
18364 int kx = int(2*xrng/step+0.5)/2;
18365 int iref, nref = 0, mirror = 0;
18366 float iy, ix, sx = 0, sy = 0;
18367 float peak = -1.0E23f;
18368 float ang = 0.0f;
18369 float imn1 = sin(theta*qv)*cos(phi*qv);
18370 float imn2 = sin(theta*qv)*sin(phi*qv);
18371 float imn3 = cos(theta*qv);
18372 vector<float> n1(crefim_len);
18373 vector<float> n2(crefim_len);
18374 vector<float> n3(crefim_len);
18375 for (iref = 0; iref < (int)crefim_len; iref++) {
18376 n1[iref] = crefim[iref]->get_attr("n1");
18377 n2[iref] = crefim[iref]->get_attr("n2");
18378 n3[iref] = crefim[iref]->get_attr("n3");
18379 }
18380 bool nomirror = (theta<90.0) || ((theta==90.0) && (psi<psi_max));
18381 if (!nomirror) {
18382 phi = fmod(phi+540.0f, 360.0f);
18383 theta = 180-theta;
18384 psi = fmod(540.0f-psi, 360.0f);
18385 }
18386 for (int i = -ky; i <= ky; i++) {
18387 iy = i * step ;
18388 for (int j = -kx; j <= kx; j++) {
18389 ix = j*step;
18390 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18391
18392 Normalize_ring(cimage, numr);
18393
18394 Frngs(cimage, numr);
18395
18396
18397 for (iref = 0; iref < (int)crefim_len; iref++) {
18398 if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18399 if (nomirror) {
18400 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 0);
18401 double qn = retvals["qn"];
18402 if (qn >= peak) {
18403 sx = -ix;
18404 sy = -iy;
18405 nref = iref;
18406 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18407 peak = static_cast<float>(qn);
18408 mirror = 0;
18409 }
18410 } else {
18411 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 1);
18412 double qn = retvals["qn"];
18413 if (qn >= peak) {
18414 sx = -ix;
18415 sy = -iy;
18416 nref = iref;
18417 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18418 peak = static_cast<float>(qn);
18419 mirror = 1;
18420 }
18421 }
18422 }
18423 } delete cimage; cimage = 0;
18424 }
18425 }
18426 float co, so, sxs, sys;
18427 if(peak == -1.0E23) {
18428 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18429 nref = -1;
18430 } else {
18431 co = cos(ang*qv);
18432 so = -sin(ang*qv);
18433 sxs = sx*co - sy*so;
18434 sys = sx*so + sy*co;
18435 }
18436 vector<float> res;
18437 res.push_back(ang);
18438 res.push_back(sxs);
18439 res.push_back(sys);
18440 res.push_back(static_cast<float>(mirror));
18441 res.push_back(static_cast<float>(nref));
18442 res.push_back(peak);
18443 return res;
18444 }
18445
18446
18447 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18448 float xrng, float yrng, float step, float psi_max, string mode,
18449 vector<int>numr, float cnx, float cny, int ynumber) {
18450
18451 size_t crefim_len = crefim.size();
18452
18453 int iref, nref=0, mirror=0;
18454 float iy, ix, sx=0, sy=0;
18455 float peak = -1.0E23f;
18456 float ang=0.0f;
18457 int kx = int(2*xrng/step+0.5)/2;
18458
18459 if(ynumber==-1) {
18460 int ky = int(2*yrng/step+0.5)/2;
18461 for (int i = -ky; i <= ky; i++) {
18462 iy = i * step ;
18463 for (int j = -kx; j <= kx; j++) {
18464 ix = j*step ;
18465 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18466
18467 Normalize_ring( cimage, numr );
18468
18469 Frngs(cimage, numr);
18470
18471
18472 for ( iref = 0; iref < (int)crefim_len; iref++) {
18473 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18474 double qn = retvals["qn"];
18475 double qm = retvals["qm"];
18476 if(qn >= peak || qm >= peak) {
18477 sx = -ix;
18478 sy = -iy;
18479 nref = iref;
18480 if (qn >= qm) {
18481 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18482 peak = static_cast<float>(qn);
18483 mirror = 0;
18484 } else {
18485 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18486 peak = static_cast<float>(qm);
18487 mirror = 1;
18488 }
18489 }
18490 }
18491 delete cimage; cimage = 0;
18492 }
18493 }
18494 }
18495
18496 else if(ynumber==0) {
18497 sy = 0.0f;
18498 for (int j = -kx; j <= kx; j++) {
18499 ix = j*step ;
18500 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18501
18502 Normalize_ring( cimage, numr );
18503
18504 Frngs(cimage, numr);
18505
18506
18507 for ( iref = 0; iref < (int)crefim_len; iref++) {
18508 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18509 double qn = retvals["qn"];
18510 double qm = retvals["qm"];
18511 if(qn >= peak || qm >= peak) {
18512 sx = -ix;
18513 nref = iref;
18514 if (qn >= qm) {
18515 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18516 peak = static_cast<float>(qn);
18517 mirror = 0;
18518 } else {
18519 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18520 peak = static_cast<float>(qm);
18521 mirror = 1;
18522 }
18523 }
18524 }
18525 delete cimage; cimage = 0;
18526 }
18527 } else {
18528 int ky = int(ynumber/2);
18529 float stepy=2*yrng/ynumber;
18530
18531 for (int i = -ky+1; i <= ky; i++) {
18532 iy = i * stepy ;
18533 for (int j = -kx; j <= kx; j++) {
18534 ix = j*step ;
18535 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18536
18537 Normalize_ring( cimage, numr );
18538
18539 Frngs(cimage, numr);
18540
18541
18542 for ( iref = 0; iref < (int)crefim_len; iref++) {
18543 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18544 double qn = retvals["qn"];
18545 double qm = retvals["qm"];
18546 if(qn >= peak || qm >= peak) {
18547 sx = -ix;
18548 sy = -iy;
18549 nref = iref;
18550 if (qn >= qm) {
18551 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18552 peak = static_cast<float>(qn);
18553 mirror = 0;
18554 } else {
18555 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18556 peak = static_cast<float>(qm);
18557 mirror = 1;
18558 }
18559 }
18560 }
18561 delete cimage; cimage = 0;
18562 }
18563 }
18564 }
18565 float co, so, sxs, sys;
18566 co = static_cast<float>( cos(ang*pi/180.0) );
18567 so = static_cast<float>( -sin(ang*pi/180.0) );
18568 sxs = sx*co - sy*so;
18569 sys = sx*so + sy*co;
18570 vector<float> res;
18571 res.push_back(ang);
18572 res.push_back(sxs);
18573 res.push_back(sys);
18574 res.push_back(static_cast<float>(mirror));
18575 res.push_back(static_cast<float>(nref));
18576 res.push_back(peak);
18577 return res;
18578 }
18579
18580 vector<float> Util::multiref_polar_ali_helical_local(EMData* image, const vector< EMData* >& crefim,
18581 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18582 vector<int>numr, float cnx, float cny, int ynumber) {
18583
18584 size_t crefim_len = crefim.size();
18585
18586 int iref, nref=-1, mirror=0;
18587 float iy, ix, sx=0, sy=0;
18588 float peak = -1.0E23f;
18589 float ang=0.0f;
18590 const float qv = static_cast<float>( pi/180.0 );
18591 Transform * t = image->get_attr("xform.projection");
18592 Dict d = t->get_params("spider");
18593 if(t) {delete t; t=0;}
18594 float phi = d["phi"];
18595 float theta = d["theta"];
18596 float imn1 = sin(theta*qv)*cos(phi*qv);
18597 float imn2 = sin(theta*qv)*sin(phi*qv);
18598 float imn3 = cos(theta*qv);
18599 vector<float> n1(crefim_len);
18600 vector<float> n2(crefim_len);
18601 vector<float> n3(crefim_len);
18602 for ( iref = 0; iref < (int)crefim_len; iref++) {
18603 n1[iref] = crefim[iref]->get_attr("n1");
18604 n2[iref] = crefim[iref]->get_attr("n2");
18605 n3[iref] = crefim[iref]->get_attr("n3");
18606 }
18607
18608 int kx = int(2*xrng/step+0.5)/2;
18609
18610 if(ynumber==-1) {
18611 int ky = int(2*yrng/step+0.5)/2;
18612 for (int i = -ky; i <= ky; i++) {
18613 iy = i * step ;
18614 for (int j = -kx; j <= kx; j++) {
18615 ix = j*step ;
18616 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18617
18618 Normalize_ring( cimage, numr );
18619
18620 Frngs(cimage, numr);
18621
18622
18623 for ( iref = 0; iref < (int)crefim_len; iref++) {
18624 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18625 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18626 double qn = retvals["qn"];
18627 double qm = retvals["qm"];
18628 if(qn >= peak || qm >= peak) {
18629 sx = -ix;
18630 sy = -iy;
18631 nref = iref;
18632 if (qn >= qm) {
18633 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18634 peak = static_cast<float>(qn);
18635 mirror = 0;
18636 } else {
18637 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18638 peak = static_cast<float>(qm);
18639 mirror = 1;
18640 }
18641 }
18642 }
18643 }
18644 delete cimage; cimage = 0;
18645 }
18646 }
18647 }
18648
18649 else if(ynumber==0) {
18650 sy = 0.0f;
18651 for (int j = -kx; j <= kx; j++) {
18652 ix = j*step ;
18653 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18654
18655 Normalize_ring( cimage, numr );
18656
18657 Frngs(cimage, numr);
18658
18659
18660 for ( iref = 0; iref < (int)crefim_len; iref++) {
18661 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18662 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18663 double qn = retvals["qn"];
18664 double qm = retvals["qm"];
18665 if(qn >= peak || qm >= peak) {
18666 sx = -ix;
18667 nref = iref;
18668 if (qn >= qm) {
18669 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18670 peak = static_cast<float>(qn);
18671 mirror = 0;
18672 } else {
18673 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18674 peak = static_cast<float>(qm);
18675 mirror = 1;
18676 }
18677 }
18678 }
18679 }
18680 delete cimage; cimage = 0;
18681 }
18682 } else {
18683 int ky = int(ynumber/2);
18684 float stepy=2*yrng/ynumber;
18685
18686 for (int i = -ky+1; i <= ky; i++) {
18687 iy = i * stepy ;
18688 for (int j = -kx; j <= kx; j++) {
18689 ix = j*step ;
18690 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18691
18692 Normalize_ring( cimage, numr );
18693
18694 Frngs(cimage, numr);
18695
18696
18697 for ( iref = 0; iref < (int)crefim_len; iref++) {
18698 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18699 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18700 double qn = retvals["qn"];
18701 double qm = retvals["qm"];
18702 if(qn >= peak || qm >= peak) {
18703 sx = -ix;
18704 sy = -iy;
18705 nref = iref;
18706 if (qn >= qm) {
18707 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18708 peak = static_cast<float>(qn);
18709 mirror = 0;
18710 } else {
18711 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18712 peak = static_cast<float>(qm);
18713 mirror = 1;
18714 }
18715 }
18716 }
18717 }
18718 delete cimage; cimage = 0;
18719 }
18720 }
18721 }
18722 float co, so, sxs, sys;
18723 co = static_cast<float>( cos(ang*pi/180.0) );
18724 so = static_cast<float>( -sin(ang*pi/180.0) );
18725 sxs = sx*co - sy*so;
18726 sys = sx*so + sy*co;
18727 vector<float> res;
18728 res.push_back(ang);
18729 res.push_back(sxs);
18730 res.push_back(sys);
18731 res.push_back(static_cast<float>(mirror));
18732 res.push_back(static_cast<float>(nref));
18733 res.push_back(peak);
18734 return res;
18735 }
18736
18737 vector<float> Util::multiref_polar_ali_helical_90(EMData* image, const vector< EMData* >& crefim,
18738 float xrng, float yrng, float step, float psi_max, string mode,
18739 vector<int>numr, float cnx, float cny, int ynumber) {
18740
18741 size_t crefim_len = crefim.size();
18742
18743 int iref, nref=0, mirror=0;
18744 float iy, ix, sx=0, sy=0;
18745 float peak = -1.0E23f;
18746 float ang=0.0f;
18747 int kx = int(2*xrng/step+0.5)/2;
18748
18749 if(ynumber==-1) {
18750 int ky = int(2*yrng/step+0.5)/2;
18751 for (int i = -ky; i <= ky; i++) {
18752 iy = i * step ;
18753 for (int j = -kx; j <= kx; j++) {
18754 ix = j*step ;
18755 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18756
18757 Normalize_ring( cimage, numr );
18758
18759 Frngs(cimage, numr);
18760
18761
18762 for ( iref = 0; iref < (int)crefim_len; iref++) {
18763 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18764 double qn = retvals["qn"];
18765 if( qn >= peak) {
18766 sx = -ix;
18767 sy = -iy;
18768 nref = iref;
18769 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18770 peak = static_cast<float>(qn);
18771 mirror = 0;
18772 }
18773 }
18774 delete cimage; cimage = 0;
18775 }
18776 }
18777 }
18778
18779 else if(ynumber==0) {
18780 sy = 0.0f;
18781 for (int j = -kx; j <= kx; j++) {
18782 ix = j*step ;
18783 iy = 0.0f ;
18784 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18785
18786 Normalize_ring( cimage, numr );
18787
18788 Frngs(cimage, numr);
18789
18790
18791 for ( iref = 0; iref < (int)crefim_len; iref++) {
18792 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18793 double qn = retvals["qn"];
18794 if( qn >= peak ) {
18795 sx = -ix;
18796 nref = iref;
18797 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18798 peak = static_cast<float>(qn);
18799 mirror = 0;
18800 }
18801 }
18802 delete cimage; cimage = 0;
18803 }
18804 } else {
18805 int ky = int(ynumber/2);
18806 float stepy=2*yrng/ynumber;
18807
18808 for (int i = -ky+1; i <= ky; i++) {
18809 iy = i * stepy ;
18810 for (int j = -kx; j <= kx; j++) {
18811 ix = j*step ;
18812 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18813
18814 Normalize_ring( cimage, numr );
18815
18816 Frngs(cimage, numr);
18817
18818
18819 for ( iref = 0; iref < (int)crefim_len; iref++) {
18820 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18821 double qn = retvals["qn"];
18822 if( qn >= peak) {
18823 sx = -ix;
18824 sy = -iy;
18825 nref = iref;
18826 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18827 peak = static_cast<float>(qn);
18828 mirror = 0;
18829 }
18830 }
18831 delete cimage; cimage = 0;
18832 }
18833 }
18834 }
18835 float co, so, sxs, sys;
18836 co = static_cast<float>( cos(ang*pi/180.0) );
18837 so = static_cast<float>( -sin(ang*pi/180.0) );
18838 sxs = sx*co - sy*so;
18839 sys = sx*so + sy*co;
18840 vector<float> res;
18841 res.push_back(ang);
18842 res.push_back(sxs);
18843 res.push_back(sys);
18844 res.push_back(static_cast<float>(mirror));
18845 res.push_back(static_cast<float>(nref));
18846 res.push_back(peak);
18847 return res;
18848 }
18849
18850
18851
18852 vector<float> Util::multiref_polar_ali_helical_90_local(EMData* image, const vector< EMData* >& crefim,
18853 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18854 vector<int>numr, float cnx, float cny, int ynumber) {
18855
18856 size_t crefim_len = crefim.size();
18857 const float qv = static_cast<float>( pi/180.0 );
18858 Transform * t = image->get_attr("xform.projection");
18859 Dict d = t->get_params("spider");
18860 if(t) {delete t; t=0;}
18861 float phi = d["phi"];
18862 float theta = d["theta"];
18863 float imn1 = sin(theta*qv)*cos(phi*qv);
18864 float imn2 = sin(theta*qv)*sin(phi*qv);
18865 float imn3 = cos(theta*qv);
18866 vector<float> n1(crefim_len);
18867 vector<float> n2(crefim_len);
18868 vector<float> n3(crefim_len);
18869 int iref, nref=-1, mirror=0;
18870 float iy, ix, sx=0, sy=0;
18871 float peak = -1.0E23f;
18872 float ang=0.0f;
18873 int kx = int(2*xrng/step+0.5)/2;
18874
18875 for ( iref = 0; iref < (int)crefim_len; iref++) {
18876 n1[iref] = crefim[iref]->get_attr("n1");
18877 n2[iref] = crefim[iref]->get_attr("n2");
18878 n3[iref] = crefim[iref]->get_attr("n3");
18879 }
18880
18881
18882 if(ynumber==-1) {
18883 int ky = int(2*yrng/step+0.5)/2;
18884 for (int i = -ky; i <= ky; i++) {
18885 iy = i * step ;
18886 for (int j = -kx; j <= kx; j++) {
18887 ix = j*step ;
18888 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18889
18890 Normalize_ring( cimage, numr );
18891
18892 Frngs(cimage, numr);
18893
18894
18895 for ( iref = 0; iref < (int)crefim_len; iref++) {
18896 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18897 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18898 double qn = retvals["qn"];
18899 if( qn >= peak) {
18900 sx = -ix;
18901 sy = -iy;
18902 nref = iref;
18903 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18904 peak = static_cast<float>(qn);
18905 mirror = 0;
18906 }
18907 }
18908 }
18909 delete cimage; cimage = 0;
18910 }
18911 }
18912 }
18913
18914 else if(ynumber==0) {
18915 sy = 0.0f;
18916 for (int j = -kx; j <= kx; j++) {
18917 ix = j*step ;
18918 iy = 0.0f ;
18919 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18920
18921 Normalize_ring( cimage, numr );
18922
18923 Frngs(cimage, numr);
18924
18925
18926 for ( iref = 0; iref < (int)crefim_len; iref++) {
18927 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18928 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18929 double qn = retvals["qn"];
18930 if( qn >= peak ) {
18931 sx = -ix;
18932 nref = iref;
18933 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18934 peak = static_cast<float>(qn);
18935 mirror = 0;
18936 }
18937 }
18938 }
18939 delete cimage; cimage = 0;
18940 }
18941 } else {
18942 int ky = int(ynumber/2);
18943 float stepy=2*yrng/ynumber;
18944
18945 for (int i = -ky+1; i <= ky; i++) {
18946 iy = i * stepy ;
18947 for (int j = -kx; j <= kx; j++) {
18948 ix = j*step ;
18949 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18950
18951 Normalize_ring( cimage, numr );
18952
18953 Frngs(cimage, numr);
18954
18955
18956 for ( iref = 0; iref < (int)crefim_len; iref++) {
18957 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18958 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18959 double qn = retvals["qn"];
18960 if( qn >= peak) {
18961 sx = -ix;
18962 sy = -iy;
18963 nref = iref;
18964 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18965 peak = static_cast<float>(qn);
18966 mirror = 0;
18967 }
18968 }
18969 }
18970 delete cimage; cimage = 0;
18971 }
18972 }
18973 }
18974 float co, so, sxs, sys;
18975 co = static_cast<float>( cos(ang*pi/180.0) );
18976 so = static_cast<float>( -sin(ang*pi/180.0) );
18977 sxs = sx*co - sy*so;
18978 sys = sx*so + sy*co;
18979 vector<float> res;
18980 res.push_back(ang);
18981 res.push_back(sxs);
18982 res.push_back(sys);
18983 res.push_back(static_cast<float>(mirror));
18984 res.push_back(static_cast<float>(nref));
18985 res.push_back(peak);
18986 return res;
18987 }
18988
18989
18990 void Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
18991 float xrng, float yrng, float step, string mode,
18992 vector< int >numr, float cnx, float cny,
18993 EMData *peaks, EMData *peakm) {
18994
18995 int maxrin = numr[numr.size()-1];
18996
18997 int ky = int(2*yrng/step+0.5)/2;
18998 int kx = int(2*xrng/step+0.5)/2;
18999
19000 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19001 float *p_ccf1ds = peaks->get_data();
19002
19003 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19004 float *p_ccf1dm = peakm->get_data();
19005
19006 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19007 p_ccf1ds[i] = -1.e20f;
19008 p_ccf1dm[i] = -1.e20f;
19009 }
19010
19011 for (int i = -ky; i <= ky; i++) {
19012 float iy = i * step;
19013 for (int j = -kx; j <= kx; j++) {
19014 float ix = j*step;
19015 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19016 Frngs(cimage, numr);
19017 Crosrng_msg_vec(crefim, cimage, numr,
19018 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19019 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19020 delete cimage; cimage = 0;
19021 }
19022 }
19023 return;
19024 }
19025
19026 void Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
19027 float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
19028 EMData *peaks_compress, EMData *peakm_compress) {
19029
19030 int maxrin = numr[numr.size()-1];
19031
19032 int ky = int(2*yrng/step+0.5)/2;
19033 int kx = int(2*xrng/step+0.5)/2;
19034
19035 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19036 float *p_ccf1ds = peaks->get_data();
19037
19038 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19039 float *p_ccf1dm = peakm->get_data();
19040
19041 peaks_compress->set_size(maxrin, 1, 1);
19042 float *p_ccf1ds_compress = peaks_compress->get_data();
19043
19044 peakm_compress->set_size(maxrin, 1, 1);
19045 float *p_ccf1dm_compress = peakm_compress->get_data();
19046
19047 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19048 p_ccf1ds[i] = -1.e20f;
19049 p_ccf1dm[i] = -1.e20f;
19050 }
19051
19052 for (int i = -ky; i <= ky; i++) {
19053 float iy = i * step;
19054 for (int j = -kx; j <= kx; j++) {
19055 float ix = j*step;
19056 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19057 Frngs(cimage, numr);
19058 Crosrng_msg_vec(crefim, cimage, numr,
19059 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19060 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19061 delete cimage; cimage = 0;
19062 }
19063 }
19064 for (int x=0; x<maxrin; x++) {
19065 float maxs = -1.0e22f;
19066 float maxm = -1.0e22f;
19067 for (int i=1; i<=2*ky+1; i++) {
19068 for (int j=1; j<=2*kx+1; j++) {
19069 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
19070 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
19071 }
19072 }
19073 p_ccf1ds_compress[x] = maxs;
19074 p_ccf1dm_compress[x] = maxm;
19075 }
19076 return;
19077 }
19078
19079 struct ccf_point
19080 {
19081 float value;
19082 int i;
19083 int j;
19084 int k;
19085 int mirror;
19086 };
19087
19088
19089 struct ccf_value
19090 {
19091 bool operator()( const ccf_point& a, const ccf_point& b )
19092 {
19093 return a.value > b.value;
19094 }
19095 };
19096
19097
19098 vector<float> Util::ali2d_ccf_list(EMData* image, EMData* crefim,
19099 float xrng, float yrng, float step, string mode,
19100 vector< int >numr, float cnx, float cny, double T) {
19101
19102 int maxrin = numr[numr.size()-1];
19103
19104 int ky = int(2*yrng/step+0.5)/2;
19105 int kx = int(2*xrng/step+0.5)/2;
19106
19107 float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
19108 float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
19109 int vol = maxrin*(2*kx+1)*(2*ky+1);
19110 vector<ccf_point> ccf(2*vol);
19111 ccf_point temp;
19112
19113 int index = 0;
19114 for (int i = -ky; i <= ky; i++) {
19115 float iy = i * step;
19116 for (int j = -kx; j <= kx; j++) {
19117 float ix = j*step;
19118 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19119 Frngs(cimage, numr);
19120 Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
19121 for (int k=0; k<maxrin; k++) {
19122 temp.value = p_ccf1ds[k];
19123 temp.i = k;
19124 temp.j = j;
19125 temp.k = i;
19126 temp.mirror = 0;
19127 ccf[index] = temp;
19128 index++;
19129 temp.value = p_ccf1dm[k];
19130 temp.mirror = 1;
19131 ccf[index] = temp;
19132 index++;
19133 }
19134 delete cimage; cimage = 0;
19135 }
19136 }
19137
19138 delete p_ccf1ds;
19139 delete p_ccf1dm;
19140 std::sort(ccf.begin(), ccf.end(), ccf_value());
19141
19142 double qt = (double)ccf[0].value;
19143 vector <double> p(2*vol), cp(2*vol);
19144
19145 double sump = 0.0;
19146 for (int i=0; i<2*vol; i++) {
19147 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
19148 sump += p[i];
19149 }
19150 for (int i=0; i<2*vol; i++) {
19151 p[i] /= sump;
19152 }
19153 for (int i=1; i<2*vol; i++) {
19154 p[i] += p[i-1];
19155 }
19156 p[2*vol-1] = 2.0;
19157
19158 float t = get_frand(0.0f, 1.0f);
19159 int select = 0;
19160 while (p[select] < t) select += 1;
19161
19162 vector<float> a(6);
19163 a[0] = ccf[select].value;
19164 a[1] = (float)ccf[select].i;
19165 a[2] = (float)ccf[select].j;
19166 a[3] = (float)ccf[select].k;
19167 a[4] = (float)ccf[select].mirror;
19168 a[5] = (float)select;
19169 return a;
19170 }
19171
19172
19173
19174
19175
19176
19177
19178
19179
19180
19181
19182
19183
19184
19185
19186
19187
19188
19189
19190
19191
19192
19193
19194
19195
19196
19197
19198
19199
19200
19201
19202
19203
19204
19205
19206
19207
19208
19209
19210
19211
19212
19213
19214
19215
19216
19217
19218
19219
19220
19221
19222
19223
19224
19225
19226
19227
19228
19229
19230
19231
19232
19233
19234
19235
19236
19237
19238
19239 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19240
19241 EMData *rot;
19242
19243 const int nmax=3, mmax=3;
19244 char task[60], csave[60];
19245 long int lsave[4];
19246 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19247 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];
19248 long int SIXTY=60;
19249
19250
19251 iprint = -1;
19252
19253
19254 factr=1.0e1;
19255 pgtol=1.0e-5;
19256
19257
19258
19259
19260 n=3;
19261 m=3;
19262
19263
19264
19265
19266
19267 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19268 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19269 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19270
19271
19272
19273
19274 strcpy(task,"START");
19275 for (int i=5;i<60;i++) task[i]=' ';
19276
19277
19278
19279 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19280
19281
19282
19283 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19284
19285 if (strncmp(task,"FG",2)==0) {
19286
19287
19288
19289
19290 rot = new EMData();
19291 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
19292 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19293
19294 delete rot;
19295
19296
19297 float dt = 1.0e-3f;
19298 rot = new EMData();
19299 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
19300 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19301
19302 g[0] = (f1-f)/dt;
19303 delete rot;
19304
19305 dt = 1.0e-2f;
19306 rot = new EMData();
19307 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
19308 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19309
19310 g[1] = (f2-f)/dt;
19311 delete rot;
19312
19313 rot = new EMData();
19314 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
19315 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19316
19317 g[2] = (f3-f)/dt;
19318 delete rot;
19319 }
19320
19321
19322 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19323
19324 }
19325
19326
19327 vector<float> res;
19328 res.push_back(static_cast<float>(x[0]));
19329 res.push_back(static_cast<float>(x[1]));
19330 res.push_back(static_cast<float>(x[2]));
19331
19332 return res;
19333 }
19334
19335 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19336
19337 EMData *rot;
19338
19339 const int nmax=3, mmax=3;
19340 char task[60], csave[60];
19341 long int lsave[4];
19342 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19343 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];
19344 long int SIXTY=60;
19345
19346
19347 iprint = -1;
19348
19349
19350 factr=1.0e1;
19351 pgtol=1.0e-5;
19352
19353
19354
19355
19356 n=3;
19357 m=3;
19358
19359
19360
19361
19362
19363 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19364 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19365 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19366
19367
19368
19369
19370 strcpy(task,"START");
19371 for (int i=5;i<60;i++) task[i]=' ';
19372
19373
19374
19375 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19376
19377
19378
19379 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19380
19381 if (strncmp(task,"FG",2)==0) {
19382
19383
19384
19385
19386 rot = new EMData();
19387 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19388 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19389
19390 delete rot;
19391
19392
19393 float dt = 1.0e-3f;
19394 rot = new EMData();
19395 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19396 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19397
19398 g[0] = (f1-f)/dt;
19399 delete rot;
19400
19401 rot = new EMData();
19402 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
19403 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19404
19405 g[1] = (f2-f)/dt;
19406 delete rot;
19407
19408 rot = new EMData();
19409 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
19410 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19411
19412 g[2] = (f3-f)/dt;
19413 delete rot;
19414 }
19415
19416
19417 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19418
19419 }
19420
19421
19422 vector<float> res;
19423 res.push_back(static_cast<float>(x[0]));
19424 res.push_back(static_cast<float>(x[1]));
19425 res.push_back(static_cast<float>(x[2]));
19426
19427 return res;
19428 }
19429
19430 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) {
19431
19432 EMData *proj, *proj2;
19433
19434 const int nmax=5, mmax=5;
19435 char task[60], csave[60];
19436 long int lsave[4];
19437 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19438 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];
19439 long int SIXTY=60;
19440
19441
19442 iprint = -1;
19443
19444
19445 factr=1.0e1;
19446 pgtol=1.0e-5;
19447
19448
19449
19450
19451 n=5;
19452 m=5;
19453
19454
19455
19456
19457
19458 x[0] = phi; nbd[0] = 2; l[0] = phi-2.0; u[0] = phi+2.0;
19459 x[1] = theta; nbd[1] = 2; l[1] = theta-2.0; u[1] = theta+2.0;
19460 x[2] = psi; nbd[2] = 2; l[2] = psi-2.0; u[2] = psi+2.0;
19461 x[3] = sxs; nbd[3] = 2; l[3] = sxs-2.0; u[3] = sxs+2.0;
19462 x[4] = sys; nbd[4] = 2; l[4] = sys-2.0; u[4] = sys+2.0;
19463
19464
19465
19466
19467 strcpy(task,"START");
19468 for (int i=5;i<60;i++) task[i]=' ';
19469
19470
19471
19472 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19473 int step = 1;
19474
19475
19476 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19477
19478 if (strncmp(task,"FG",2)==0) {
19479
19480
19481
19482
19483 proj = new EMData();
19484 proj2 = new EMData();
19485 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19486 proj->fft_shuffle();
19487 proj->center_origin_fft();
19488 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19489 proj->do_ift_inplace();
19490 int M = proj->get_ysize()/2;
19491 proj2 = proj->window_center(M);
19492 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19493
19494 delete proj;
19495 delete proj2;
19496
19497
19498 float dt = 1.0e-3f;
19499 proj = new EMData();
19500 proj2 = new EMData();
19501 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
19502 proj->fft_shuffle();
19503 proj->center_origin_fft();
19504 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19505 proj->do_ift_inplace();
19506 proj2 = proj->window_center(M);
19507 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19508
19509 delete proj;
19510 delete proj2;
19511 g[0] = (ft-f)/dt;
19512
19513 proj = new EMData();
19514 proj2 = new EMData();
19515 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
19516 proj->fft_shuffle();
19517 proj->center_origin_fft();
19518 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19519 proj->do_ift_inplace();
19520 proj2 = proj->window_center(M);
19521 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19522
19523 delete proj;
19524 delete proj2;
19525 g[1] = (ft-f)/dt;
19526
19527 proj = new EMData();
19528 proj2 = new EMData();
19529 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
19530 proj->fft_shuffle();
19531 proj->center_origin_fft();
19532 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19533 proj->do_ift_inplace();
19534 proj2 = proj->window_center(M);
19535 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19536
19537 delete proj;
19538 delete proj2;
19539 g[2] = (ft-f)/dt;
19540
19541 proj = new EMData();
19542 proj2 = new EMData();
19543 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19544 proj->fft_shuffle();
19545 proj->center_origin_fft();
19546 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
19547 proj->do_ift_inplace();
19548 proj2 = proj->window_center(M);
19549 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19550
19551 delete proj;
19552 delete proj2;
19553 g[3] = (ft-f)/dt;
19554
19555 proj = new EMData();
19556 proj2 = new EMData();
19557 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19558 proj->fft_shuffle();
19559 proj->center_origin_fft();
19560 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
19561 proj->do_ift_inplace();
19562 proj2 = proj->window_center(M);
19563 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19564
19565 delete proj;
19566 delete proj2;
19567 g[4] = (ft-f)/dt;
19568 }
19569
19570
19571 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19572 step++;
19573 }
19574
19575
19576 vector<float> res;
19577 res.push_back(static_cast<float>(x[0]));
19578 res.push_back(static_cast<float>(x[1]));
19579 res.push_back(static_cast<float>(x[2]));
19580 res.push_back(static_cast<float>(x[3]));
19581 res.push_back(static_cast<float>(x[4]));
19582
19583 return res;
19584 }
19585
19586
19587 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19588
19589 double x[4];
19590 int n;
19591 int l = 3;
19592 int m = 200;
19593 double e = 1e-9;
19594 double step = 0.01;
19595 float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
19596
19597 x[1] = ang;
19598 x[2] = sxs;
19599 x[3] = sys;
19600
19601 Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);
19602
19603
19604 vector<float> res;
19605 res.push_back(static_cast<float>(x[1]));
19606 res.push_back(static_cast<float>(x[2]));
19607 res.push_back(static_cast<float>(x[3]));
19608 res.push_back(static_cast<float>(n));
19609 return res;
19610 }
19611
19612 vector<float> Util::multi_align_error(vector<float> args, vector<float> all_ali_params, int d) {
19613
19614 const int nmax=args.size(), mmax=nmax;
19615 char task[60], csave[60];
19616 long int lsave[4];
19617 long int n, m, iprint, isave[44];
19618 long int* nbd = new long int[nmax];
19619 long int* iwa = new long int[3*nmax];
19620 double f, factr, pgtol;
19621 double* x = new double[nmax];
19622 double* l = new double[nmax];
19623 double* u = new double[nmax];
19624 double* g = new double[nmax];
19625 double dsave[29];
19626 double* wa = new double[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
19627 long int SIXTY=60;
19628
19629 int num_ali = nmax/3+1;
19630 int nima = all_ali_params.size()/(num_ali*4);
19631
19632
19633 iprint = -1;
19634
19635
19636 factr=1.0e1;
19637 pgtol=1.0e-9;
19638
19639
19640
19641
19642 n=nmax;
19643 m=mmax;
19644
19645
19646
19647
19648
19649 for (int i=0; i<nmax; i++) {
19650 x[i] = args[i];
19651 nbd[i] = 0;
19652 }
19653
19654
19655
19656 strcpy(task,"START");
19657 for (int i=5;i<60;i++) task[i]=' ';
19658
19659
19660
19661 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19662 int step = 1;
19663
19664
19665 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19666
19667 if (strncmp(task,"FG",2)==0) {
19668
19669
19670
19671
19672 f = multi_align_error_func(x, all_ali_params, nima, num_ali, d);
19673
19674
19675 multi_align_error_dfunc(x, all_ali_params, nima, num_ali, g, d);
19676
19677 }
19678
19679 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19680 step++;
19681 }
19682
19683
19684 vector<float> res;
19685 for (int i=0; i<nmax; i++) res.push_back(static_cast<float>(x[i]));
19686 res.push_back(static_cast<float>(f));
19687
19688 delete[] nbd;
19689 delete[] iwa;
19690 delete[] x;
19691 delete[] l;
19692 delete[] u;
19693 delete[] g;
19694 delete[] wa;
19695
19696 return res;
19697
19698 }
19699
19700 double Util::multi_align_error_func(double* x, vector<float> all_ali_params, int nima, int num_ali, int d) {
19701
19702 vector<double> sqr_pixel_error = multi_align_error_func2(x, all_ali_params, nima, num_ali, d);
19703 double sum_sqr_pixel_error = 0.0;
19704 for (int i=0; i<nima; i++) sum_sqr_pixel_error += sqr_pixel_error[i];
19705 return sum_sqr_pixel_error/static_cast<float>(nima);
19706 }
19707
19708
19709 vector<double> Util::multi_align_error_func2(double* x, vector<float> ali_params, int nima, int num_ali, int d) {
19710
19711 double* args = new double[num_ali*3];
19712 for (int i=0; i<3*num_ali-3; i++) args[i] = x[i];
19713 args[3*num_ali-3] = 0.0;
19714 args[3*num_ali-2] = 0.0;
19715 args[3*num_ali-1] = 0.0;
19716 double* cosa = new double[num_ali];
19717 double* sina = new double[num_ali];
19718 for (int i=0; i<num_ali; i++) {
19719 cosa[i] = cos(args[i*3]*M_PI/180.0);
19720 sina[i] = sin(args[i*3]*M_PI/180.0);
19721 }
19722 double* sx = new double[num_ali];
19723 double* sy = new double[num_ali];
19724
19725 vector<double> sqr_pixel_error(nima);
19726
19727 for (int i=0; i<nima; i++) {
19728 double sum_cosa = 0.0;
19729 double sum_sina = 0.0;
19730 for (int j=0; j<num_ali; j++) {
19731 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19732 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19733 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19734 sx[j] = args[j*3+1] + ali_params[j*nima*4+i*4+1]*cosa[j] - ali_params[j*nima*4+i*4+2]*sina[j];
19735 sy[j] = args[j*3+2] + ali_params[j*nima*4+i*4+1]*sina[j] + ali_params[j*nima*4+i*4+2]*cosa[j];
19736 } else {
19737 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19738 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19739 sx[j] = -args[j*3+1] + ali_params[j*nima*4+i*4+1]*cosa[j] + ali_params[j*nima*4+i*4+2]*sina[j];
19740 sy[j] = args[j*3+2] - ali_params[j*nima*4+i*4+1]*sina[j] + ali_params[j*nima*4+i*4+2]*cosa[j];
19741 }
19742 }
19743 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
19744 sum_cosa /= P;
19745 sum_sina /= P;
19746 sqr_pixel_error[i] = d*d/4.0*(1.0-P/num_ali)+var(sx, num_ali)+var(sy, num_ali);
19747 }
19748
19749 delete[] args;
19750 delete[] cosa;
19751 delete[] sina;
19752 delete[] sx;
19753 delete[] sy;
19754
19755 return sqr_pixel_error;
19756 }
19757
19758 void Util::multi_align_error_dfunc(double* x, vector<float> ali_params, int nima, int num_ali, double* g, int d) {
19759
19760 for (int i=0; i<num_ali*3-3; i++) g[i] = 0.0;
19761
19762 double* args = new double[num_ali*3];
19763 for (int i=0; i<3*num_ali-3; i++) args[i] = x[i];
19764 args[3*num_ali-3] = 0.0;
19765 args[3*num_ali-2] = 0.0;
19766 args[3*num_ali-1] = 0.0;
19767 double* cosa = new double[num_ali];
19768 double* sina = new double[num_ali];
19769 for (int i=0; i<num_ali; i++) {
19770 cosa[i] = cos(args[i*3]*M_PI/180.0);
19771 sina[i] = sin(args[i*3]*M_PI/180.0);
19772 }
19773 double* sx = new double[num_ali];
19774 double* sy = new double[num_ali];
19775
19776 vector<float> sqr_pixel_error(nima);
19777
19778 for (int i=0; i<nima; i++) {
19779 double sum_cosa = 0.0;
19780 double sum_sina = 0.0;
19781 for (int j=0; j<num_ali; j++) {
19782 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19783 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19784 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19785 sx[j] = args[j*3+1] + ali_params[j*nima*4+i*4+1]*cosa[j] - ali_params[j*nima*4+i*4+2]*sina[j];
19786 sy[j] = args[j*3+2] + ali_params[j*nima*4+i*4+1]*sina[j] + ali_params[j*nima*4+i*4+2]*cosa[j];
19787 } else {
19788 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19789 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19790 sx[j] = -args[j*3+1] + ali_params[j*nima*4+i*4+1]*cosa[j] + ali_params[j*nima*4+i*4+2]*sina[j];
19791 sy[j] = args[j*3+2] - ali_params[j*nima*4+i*4+1]*sina[j] + ali_params[j*nima*4+i*4+2]*cosa[j];
19792 }
19793 }
19794 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
19795 sum_cosa /= P;
19796 sum_sina /= P;
19797 for (int j=0; j<num_ali-1; j++) {
19798 double dx = 2.0*(sx[j]-mean(sx, num_ali));
19799 double dy = 2.0*(sy[j]-mean(sy, num_ali));
19800 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19801 g[j*3] += (d*d/4.0*(sum_cosa*sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) -
19802 sum_sina*cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
19803 dx*(-ali_params[j*nima*4+i*4+1]*sina[j]-ali_params[j*nima*4+i*4+2]*cosa[j])+
19804 dy*( ali_params[j*nima*4+i*4+1]*cosa[j]-ali_params[j*nima*4+i*4+2]*sina[j]))*M_PI/180.0;
19805 g[j*3+1] += dx;
19806 g[j*3+2] += dy;
19807 } else {
19808 g[j*3] += (d*d/4.0*(-sum_cosa*sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) +
19809 sum_sina*cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
19810 dx*(-ali_params[j*nima*4+i*4+1]*sina[j]+ali_params[j*nima*4+i*4+2]*cosa[j])+
19811 dy*(-ali_params[j*nima*4+i*4+1]*cosa[j]-ali_params[j*nima*4+i*4+2]*sina[j]))*M_PI/180.0;
19812 g[j*3+1] += -dx;
19813 g[j*3+2] += dy;
19814 }
19815 }
19816 }
19817
19818 for (int i=0; i<3*num_ali-3; i++) g[i] /= (num_ali*nima);
19819
19820 delete[] args;
19821 delete[] cosa;
19822 delete[] sina;
19823 delete[] sx;
19824 delete[] sy;
19825 }
19826
19827 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
19828
19829 EMData *rot= new EMData();
19830 float ccc;
19831
19832 rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
19833 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19834 delete rot;
19835 return ccc;
19836 }
19837
19838 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19839
19840 double x[4];
19841 int n;
19842 int l = 3;
19843 int m = 200;
19844 double e = 1e-9;
19845 double step = 0.001;
19846 float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
19847
19848 x[1] = ang;
19849 x[2] = sxs;
19850 x[3] = sys;
19851
19852 Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);
19853
19854
19855 vector<float> res;
19856 res.push_back(static_cast<float>(x[1]));
19857 res.push_back(static_cast<float>(x[2]));
19858 res.push_back(static_cast<float>(x[3]));
19859 res.push_back(static_cast<float>(n));
19860 return res;
19861 }
19862
19863
19864 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
19865
19866 EMData *rot= new EMData();
19867 float ccc;
19868
19869 rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
19870 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19871 delete rot;
19872 return ccc;
19873 }
19874
19875 #define img_ptr(i,j,k) img_ptr[i+(j+(k*ny))*(size_t)nx]
19876 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*(size_t)nx]
19877 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
19878 {
19879 ENTERFUNC;
19880
19881 if (!img) {
19882 throw NullPointerException("NULL input image");
19883 }
19884
19885 int newx, newy, newz;
19886 bool keep_going;
19887 cout << " entered " <<endl;
19888 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
19889
19890 EMData * img2 = new EMData();
19891 img2->set_size(nx,ny,nz);
19892 img2->to_zero();
19893 float *img_ptr =img->get_data();
19894 float *img2_ptr = img2->get_data();
19895 int r2 = ro*ro;
19896 int r3 = r2*ro;
19897 int ri2 = ri*ri;
19898 int ri3 = ri2*ri;
19899
19900 int n2 = nx/2;
19901
19902 for (int k=-n2; k<=n2; k++) {
19903 float z2 = static_cast<float>(k*k);
19904 for (int j=-n2; j<=n2; j++) {
19905 float y2 = z2 + j*j;
19906 if(y2 <= r2) {
19907
19908
19909 for (int i=-n2; i<=n2; i++) {
19910 float x2 = y2 + i*i;
19911 if(x2 <= r3) {
19912
19913 int ib = i+n2; int jb = j+n2; int kb = k+n2;
19914 if(x2 >= ri3) {
19915
19916 if(img_ptr(ib,jb,kb) == 1.0f) {
19917
19918 if(Util::get_frand(0.0f, 1.0f) > qprob){
19919 img2_ptr(ib,jb,kb) = 0.0f;
19920 keep_going = true;
19921
19922 while(keep_going) {
19923 newx = Util::get_irand(-ro,ro);
19924 newy = Util::get_irand(-ro,ro);
19925 newz = Util::get_irand(-ro,ro);
19926 if(newx*newx+newy*newy+newz*newz <= r3) {
19927 newx += n2; newy += n2; newz += n2;
19928 if( img_ptr(newx,newy,newz) == 0.0f) {
19929 img2_ptr(newx,newy,newz) = 1.0f;
19930 keep_going = false; }
19931 }
19932 }
19933 } else img2_ptr(ib,jb,kb) = 1.0f;
19934 }
19935 } else {
19936
19937 if(img_ptr(ib,jb,kb) == 1.0) {
19938 if(Util::get_frand(0.0f,1.0f) > qprob) {
19939
19940 float numn = -1.0f;
19941 for (newz = -1; newz <= 1; newz++)
19942 for (newy = -1; newy <= 1; newy++)
19943 for (newx = -1; newx <= 1; newx++)
19944 numn += img_ptr(ib+newx,jb+newy,kb+newz);
19945 img2_ptr(ib,jb,kb) = 0.0;
19946 if(numn == 26.0f) {
19947
19948 keep_going = true;
19949 while(keep_going) {
19950 newx = Util::get_irand(-ro,ro);
19951 newy = Util::get_irand(-ro,ro);
19952 newz = Util::get_irand(-ro,ro);
19953 if(newx*newx+newy*newy+newz*newz < r3) {
19954 newx += n2; newy += n2; newz += n2;
19955 if( img_ptr(newx,newy,newz) == 0.0f) {
19956 if(newx*newx+newy*newy+newz*newz < r3) {
19957 if(newx*newx+newy*newy+newz*newz < r3) {
19958 newx += n2; newy += n2; newz += n2;
19959 if( img_ptr(newx,newy,newz) == 0.0f) {
19960 img2_ptr(newx,newy,newz) = 1.0f;
19961 keep_going = false; }
19962 }
19963 }
19964 }
19965 }
19966 }
19967 } else if(numn == 25.0f) {
19968
19969 for (newz = -1; newz <= 1; newz++) {
19970 for (newy = -1; newy <= 1; newy++) {
19971 for (newx = -1; newx <= 1; newx++) {
19972 if( newx != 0 && newy != 0 && newz != 0) {
19973 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
19974 img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
19975 }
19976 }
19977 }
19978 }
19979 }
19980 } else {
19981
19982 keep_going = true;
19983 while(keep_going) {
19984 newx = Util::get_irand(-1,1);
19985 newy = Util::get_irand(-1,1);
19986 newz = Util::get_irand(-1,1);
19987 if(newx != 0 && newy != 0 && newz != 0) {
19988 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
19989 img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;
19990 keep_going = false;
19991 }
19992 }
19993 }
19994 }
19995 } else img2_ptr(ib,jb,kb) = 1.0f;
19996 }
19997 }
19998 }
19999 }
20000 }
20001 }
20002 }
20003
20004 img2->update();
20005
20006 EXITFUNC;
20007 return img2;
20008 }
20009 #undef img_ptr
20010 #undef img2_ptr
20011
20012 struct point3d_t
20013 {
20014 point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
20015
20016 int x;
20017 int y;
20018 int z;
20019 };
20020
20021
20022 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
20023 {
20024 int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
20025 int noff = 6;
20026
20027 int nx = visited->get_xsize();
20028 int ny = visited->get_ysize();
20029 int nz = visited->get_zsize();
20030
20031 vector< point3d_t > pts;
20032 pts.push_back( point3d_t(ix, iy, iz) );
20033 visited->set_value_at( ix, iy, iz, (float)grpid );
20034
20035 int start = 0;
20036 int end = pts.size();
20037
20038 while( end > start ) {
20039 for(int i=start; i < end; ++i ) {
20040 int ix = pts[i].x;
20041 int iy = pts[i].y;
20042 int iz = pts[i].z;
20043
20044 for( int j=0; j < noff; ++j ) {
20045 int jx = ix + offs[j][0];
20046 int jy = iy + offs[j][1];
20047 int jz = iz + offs[j][2];
20048
20049 if( jx < 0 || jx >= nx ) continue;
20050 if( jy < 0 || jy >= ny ) continue;
20051 if( jz < 0 || jz >= nz ) continue;
20052
20053
20054 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
20055 pts.push_back( point3d_t(jx, jy, jz) );
20056 visited->set_value_at( jx, jy, jz, (float)grpid );
20057 }
20058
20059 }
20060 }
20061
20062 start = end;
20063 end = pts.size();
20064 }
20065 return pts.size();
20066 }
20067
20068
20069 EMData* Util::get_biggest_cluster( EMData* mg )
20070 {
20071 int nx = mg->get_xsize();
20072 int ny = mg->get_ysize();
20073 int nz = mg->get_zsize();
20074
20075 EMData* visited = new EMData();
20076 visited->set_size( nx, ny, nz );
20077 visited->to_zero();
20078 int grpid = 0;
20079 int maxgrp = 0;
20080 int maxsize = 0;
20081 for( int iz=0; iz < nz; ++iz ) {
20082 for( int iy=0; iy < ny; ++iy ) {
20083 for( int ix=0; ix < nx; ++ix ) {
20084 if( (*mg)(ix, iy, iz)==0.0 ) continue;
20085
20086 if( (*visited)(ix, iy, iz) > 0.0 ) {
20087
20088 continue;
20089 }
20090
20091 grpid++;
20092 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
20093 if( grpsize > maxsize ) {
20094 maxsize = grpsize;
20095 maxgrp = grpid;
20096 }
20097 }
20098 }
20099 }
20100
20101 Assert( maxgrp > 0 );
20102
20103 int npoint = 0;
20104 EMData* result = new EMData();
20105 result->set_size( nx, ny, nz );
20106 result->to_zero();
20107
20108 for( int iz=0; iz < nz; ++iz ) {
20109 for( int iy=0; iy < ny; ++iy ) {
20110 for( int ix=0; ix < nx; ++ix ) {
20111 if( (*visited)(ix, iy, iz)==maxgrp ) {
20112 (*result)(ix,iy,iz) = 1.0;
20113 npoint++;
20114 }
20115 }
20116 }
20117 }
20118
20119 Assert( npoint==maxsize );
20120 delete visited;
20121 return result;
20122
20123 }
20124
20125 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)
20126 {
20127 int ix, iy, iz;
20128 int i, j, k;
20129 int nr2, nl2;
20130 float az, ak;
20131 float scx, scy, scz;
20132 int offset = 2 - nx%2;
20133 int lsm = nx + offset;
20134 EMData* ctf_img1 = new EMData();
20135 ctf_img1->set_size(lsm, ny, nz);
20136 float freq = 1.0f/(2.0f*ps);
20137 scx = 2.0f/float(nx);
20138 if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20139 if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20140 nr2 = ny/2 ;
20141 nl2 = nz/2 ;
20142 float pihalf = M_PI/2.0f;
20143 for ( k=0; k<nz;k++) {
20144 iz = k; if(k>nl2) iz=k-nz;
20145 float oz2 = iz*scz*iz*scz;
20146 for ( j=0; j<ny;j++) {
20147 iy = j; if(j>nr2) iy=j - ny;
20148 float oy = iy*scy;
20149 float oy2 = oy*oy;
20150 for ( i=0; i<lsm/2; i++) {
20151 ix=i;
20152 if( dza == 0.0f) {
20153 ak=pow(ix*ix*scx*scx + oy2 + oz2, 0.5f)*freq;
20154 (*ctf_img1) (i*2,j,k) = Util::tf(dz, ak, voltage, cs, wgh, b_factor, sign);
20155 } else {
20156 float ox = ix*scx;
20157 ak=pow(ox*ox + oy2 + oz2, 0.5f)*freq;
20158 az = atan2(oy, ox);
20159 float dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f-pihalf));
20160 (*ctf_img1) (i*2,j,k) = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
20161 }
20162
20163 }
20164 }
20165 }
20166 ctf_img1->update();
20167 ctf_img1->set_complex(true);
20168 ctf_img1->set_ri(true);
20169
20170
20171 if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
20172 return ctf_img1;
20173 }
20174
20175
20176
20177
20178
20179
20180
20181
20182
20183
20184
20185
20186
20187
20188
20189
20190
20191
20192
20193
20194
20195
20196
20197
20198
20199
20200
20201
20202
20203
20204
20205
20206
20207
20208
20209
20210
20211
20212
20213
20214
20215
20216
20217
20218
20219
20220
20221
20222
20223
20224
20225
20226
20227
20228
20229
20230
20231
20232
20233
20234
20235
20236
20237
20238
20239
20240
20241
20242
20243
20244
20245
20246
20247
20248
20249
20250
20251
20252
20253
20254
20255
20256
20257
20258
20259
20260
20261 #define cent(i) out[i+N]
20262 #define assign(i) out[i]
20263 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
20264 int nx = d->get_xsize();
20265 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20266 vector<float> out(N+K+2);
20267 if(N*(N-1)/2 != nx) {
20268
20269 return out;}
20270
20271 for(int i=0; i<N; i++) assign(i) = float(i);
20272
20273 for(int i=0; i<N; i++) {
20274 int j = Util::get_irand(0,N-1);
20275 float temp = assign(i);
20276 assign(i) = assign(j);
20277 assign(j) = temp;
20278 }
20279 for(int k=0; k<K; k++) cent(k) = float(assign(k));
20280
20281
20282 for(int i=0; i<N; i++) assign(i) = 0.0f;
20283 float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
20284 bool change = true;
20285 int it = -1;
20286 int ct = -1;
20287 while ((change && disp < dispold) || ct > 0) {
20288
20289 change = false;
20290 dispold = disp;
20291 it++;
20292
20293
20294 disp = 0.0f;
20295 ct = 0;
20296 for(int i=0; i<N; i++) {
20297 qm = 1.0e23f;
20298 for(int k=0; k<K; k++) {
20299 if(float(i) == cent(k)) {
20300 qm = 0.0f;
20301 na = (float)k;
20302 } else {
20303 float dt = (*d)(mono(i,int(cent(k))));
20304 if(dt < qm) {
20305 qm = dt;
20306 na = (float)k;
20307 }
20308 }
20309 }
20310
20311
20312
20313 if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
20314 na = (float)(Util::get_irand(0, K));
20315 qm = (*d)(mono(i,int(na)));
20316 ct++;
20317 }
20318
20319 disp += qm;
20320
20321 if(na != assign(i)) {
20322 assign(i) = na;
20323 change = true;
20324 }
20325 }
20326
20327
20328 T = T*F;
20329
20330
20331
20332
20333
20334 for(int k=0; k<K; k++) {
20335 qm = 1.0e23f;
20336 for(int i=0; i<N; i++) {
20337 if(assign(i) == float(k)) {
20338 float q = 0.0;
20339 for(int j=0; j<N; j++) {
20340 if(assign(j) == float(k)) {
20341
20342 if(i != j) q += (*d)(mono(i,j));
20343
20344 }
20345 }
20346 if(q < qm) {
20347
20348 qm = q;
20349 cent(k) = float(i);
20350 }
20351 }
20352 }
20353 }
20354
20355 }
20356 out[N+K] = disp;
20357 out[N+K+1] = float(it);
20358 return out;
20359 }
20360 #undef cent
20361 #undef assign
20362
20363
20364
20365
20366
20367
20368
20369
20370
20371
20372
20373
20374
20375
20376
20377
20378
20379
20380
20381
20382
20383
20384
20385
20386
20387
20388
20389
20390
20391
20392
20393
20394
20395
20396
20397
20398
20399
20400
20401
20402
20403
20404
20405
20406
20407
20408
20409
20410
20411
20412
20413
20414
20415
20416
20417
20418
20419
20420
20421
20422
20423
20424
20425
20426
20427
20428
20429
20430
20431
20432
20433
20434
20435
20436
20437
20438
20439
20440
20441
20442
20443
20444
20445
20446
20447
20448
20449
20450
20451
20452
20453
20454
20455
20456
20457
20458
20459
20460
20461
20462
20463
20464
20465
20466
20467
20468
20469
20470
20471 vector<float> Util::cluster_equalsize(EMData* d) {
20472
20473 int nx = d->get_xsize();
20474 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20475 int K = N/2;
20476 vector<float> group(N);
20477 if(N*(N-1)/2 != nx) {
20478
20479 return group;}
20480
20481 bool * active = new bool[N];
20482 for(int i=0; i<N; i++) active[i] = true;
20483
20484 float dm, qd;
20485 int ppi = 0, ppj = 0;
20486 for(int k=0; k<K; k++) {
20487
20488
20489 dm = 1.0e23f;
20490 for(int i=1; i<N; i++) {
20491 if(active[i]) {
20492 for(int j=0; j<i; j++) {
20493 if(active[j]) {
20494 qd = (*d)(i*(i - 1)/2 + j);
20495 if(qd < dm) {
20496 dm = qd;
20497 ppi = i;
20498 ppj = j;
20499 }
20500 }
20501 }
20502 }
20503 }
20504 group[2*k] = float(ppi);
20505 group[1+2*k] = float(ppj);
20506 active[ppi] = false;
20507 active[ppj] = false;
20508 }
20509
20510 delete [] active;
20511 active = NULL;
20512 return group;
20513 }
20514
20515
20516
20517
20518
20519
20520
20521
20522
20523
20524
20525
20526
20527
20528
20529
20530
20531
20532
20533
20534
20535
20536
20537
20538
20539
20540
20541
20542
20543
20544
20545
20546
20547
20548
20549
20550
20551
20552
20553
20554
20555
20556
20557
20558
20559
20560
20561
20562
20563
20564
20565
20566
20567
20568 #define data(i,j) group[i*ny+j]
20569 vector<float> Util::vareas(EMData* d) {
20570 const float step=0.001f;
20571 int ny = d->get_ysize();
20572
20573
20574
20575 vector<float> group(2*ny);
20576 for(int i=0; i<2*ny; i++) group[i] = 0.0f;
20577 int K = int(1.0f/step) +1;
20578 int hit = 0;
20579 for(int kx=0; kx<=K; kx++) {
20580 float tx = kx*step;
20581 for(int ky=0; ky<=K; ky++) {
20582 float ty = ky*step;
20583 float dm = 1.0e23f;
20584 for(int i=0; i<ny; i++) {
20585 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
20586 if( qd < dm) {
20587 dm = qd;
20588 hit = i;
20589 }
20590 }
20591 data(0,hit) += 1.0f;
20592 if(kx == 0 || ky == 0 || kx == K || ky == K) data(1,hit) = 1.0f;
20593 }
20594 }
20595 return group;
20596 }
20597 #undef data
20598
20599 EMData* Util::get_slice(EMData *vol, int dim, int index) {
20600
20601 int nx = vol->get_xsize();
20602 int ny = vol->get_ysize();
20603 int nz = vol->get_zsize();
20604 float *vol_data = vol->get_data();
20605 int new_nx, new_ny;
20606
20607 if (nz == 1)
20608 throw ImageDimensionException("Error: Input must be a 3-D object");
20609 if ((dim < 1) || (dim > 3))
20610 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
20611 if (((dim == 1) && (index < 0 || index > nx-1)) ||
20612 ((dim == 1) && (index < 0 || index > nx-1)) ||
20613 ((dim == 1) && (index < 0 || index > nx-1)))
20614 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
20615
20616 if (dim == 1) {
20617 new_nx = ny;
20618 new_ny = nz;
20619 } else if (dim == 2) {
20620 new_nx = nx;
20621 new_ny = nz;
20622 } else {
20623 new_nx = nx;
20624 new_ny = ny;
20625 }
20626
20627 EMData *slice = new EMData();
20628 slice->set_size(new_nx, new_ny, 1);
20629 float *slice_data = slice->get_data();
20630
20631 if (dim == 1) {
20632 for (int x=0; x<new_nx; x++)
20633 for (int y=0; y<new_ny; y++)
20634 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
20635 } else if (dim == 2) {
20636 for (int x=0; x<new_nx; x++)
20637 for (int y=0; y<new_ny; y++)
20638 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
20639 } else {
20640 for (int x=0; x<new_nx; x++)
20641 for (int y=0; y<new_ny; y++)
20642 slice_data[y*new_nx+x] = vol_data[((size_t)index*ny+y)*nx+x];
20643 }
20644
20645 return slice;
20646 }
20647
20648 void Util::image_mutation(EMData *img, float mutation_rate) {
20649 int nx = img->get_xsize();
20650 float min = img->get_attr("minimum");
20651 float max = img->get_attr("maximum");
20652 float* img_data = img->get_data();
20653 array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
20654 return;
20655 }
20656
20657
20658 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20659
20660 if (is_mirror != 0) {
20661 for (int i=0; i<len_list; i++) {
20662 int r = rand()%10000;
20663 float f = r/10000.0f;
20664 if (f < mutation_rate) list[i] = 1-list[i];
20665 }
20666 } else {
20667 map<int, vector<int> > graycode;
20668 map<vector<int>, int> rev_graycode;
20669 vector <int> gray;
20670
20671 int K=1;
20672 for (int i=0; i<L; i++) K*=2;
20673
20674 for (int k=0; k<K; k++) {
20675 int shift = 0;
20676 vector <int> gray;
20677 for (int i=L-1; i>-1; i--) {
20678 int t = ((k>>i)%2-shift)%2;
20679 gray.push_back(t);
20680 shift += t-2;
20681 }
20682 graycode[k] = gray;
20683 rev_graycode[gray] = k;
20684 }
20685
20686 float gap = (K-1)/(max_val-min_val);
20687 for (int i=0; i<len_list; i++) {
20688 float val = list[i];
20689 if (val < min_val) { val = min_val; }
20690 else if (val > max_val) { val = max_val; }
20691 int k = int((val-min_val)*gap+0.5);
20692 vector<int> gray = graycode[k];
20693 bool changed = false;
20694 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20695 int r = rand()%10000;
20696 float f = r/10000.0f;
20697 if (f < mutation_rate) {
20698 *p = 1-*p;
20699 changed = true;
20700 }
20701 }
20702 if (changed) {
20703 k = rev_graycode[gray];
20704 list[i] = k/gap+min_val;
20705 }
20706 }
20707 }
20708
20709 }
20710
20711 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20712
20713 if (is_mirror != 0) {
20714 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20715 int r = rand()%10000;
20716 float f = r/10000.0f;
20717 if (f < mutation_rate) *q = 1-*q;
20718 }
20719 } else {
20720 map<int, vector<int> > graycode;
20721 map<vector<int>, int> rev_graycode;
20722 vector <int> gray;
20723
20724 int K=1;
20725 for (int i=0; i<L; i++) K*=2;
20726
20727 for (int k=0; k<K; k++) {
20728 int shift = 0;
20729 vector <int> gray;
20730 for (int i=L-1; i>-1; i--) {
20731 int t = ((k>>i)%2-shift)%2;
20732 gray.push_back(t);
20733 shift += t-2;
20734 }
20735 graycode[k] = gray;
20736 rev_graycode[gray] = k;
20737 }
20738
20739 float gap = (K-1)/(max_val-min_val);
20740 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20741 float val = *q;
20742 if (val < min_val) { val = min_val; }
20743 else if (val > max_val) { val = max_val; }
20744 int k = int((val-min_val)*gap+0.5);
20745 vector<int> gray = graycode[k];
20746 bool changed = false;
20747 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20748 int r = rand()%10000;
20749 float f = r/10000.0f;
20750 if (f < mutation_rate) {
20751 *p = 1-*p;
20752 changed = true;
20753 }
20754 }
20755 if (changed) {
20756 k = rev_graycode[gray];
20757 *q = k/gap+min_val;
20758 }
20759 }
20760 }
20761 return list;
20762 }
20763
20764
20765 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
20766
20767 int total_cost = *output;
20768 int num_matches = *(output+1);
20769
20770 int cost=0;
20771 int* intx;
20772 int intx_size;
20773 int* intx_next(0);
20774 int intx_next_size = 0;
20775 int curclass;
20776 int curclass_size;
20777
20778 for(int i = 0; i < num_matches; i++){
20779 curclass = *(output+2+ i*nParts);
20780
20781 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
20782 *(argParts + Indices[curclass]+1) = -5;
20783
20784 curclass_size = *(dimClasses+curclass)-2;
20785 intx = new int[curclass_size];
20786 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
20787 intx_size = curclass_size;
20788
20789 for (int j=1; j < nParts; j++){
20790 curclass = *(output+2+ i*nParts+j);
20791 if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
20792 *(argParts + Indices[j*K+curclass]+1)=-5;
20793
20794 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);
20795 intx_next = new int[intx_next_size];
20796 Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
20797 delete[] intx;
20798 intx=intx_next;
20799 intx_size= intx_next_size;
20800 }
20801 delete[] intx_next;
20802
20803 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
20804
20805 cost = cost + intx_next_size;
20806 }
20807
20808 if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
20809
20810 return 1;
20811
20812 }
20813
20814
20815
20816
20817
20818
20819 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int* costlist, int J){
20820
20821
20822 bool flag = 0;
20823 int nintx;
20824 int* dummy(0);
20825
20826 int* curbranch = new int[nParts];
20827
20828
20829 for(int jit= 0; jit< J; jit++) *(costlist+jit) = 0;
20830
20831
20832 for(int a=0; a<K; a++)
20833 {
20834
20835
20836 if (*(argParts + Indices[a] + 1) < 1) continue;
20837 if (*(dimClasses + a)-2 <= T) continue;
20838
20839
20840
20841 for( int i=1; i < nParts; i++){
20842 flag = 0;
20843 for(int j=0; j < K; j++){
20844 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
20845 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);
20846 if (nintx > T) flag=1;
20847 else *(argParts + Indices[i*K+j] + 1) =-4;
20848 }
20849 if (flag==0) {break;}
20850 }
20851
20852
20853 *curbranch = a;
20854
20855 if (flag > 0)
20856 Util::explore2(argParts, Indices, dimClasses, nParts, K, T, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2,
20857 *(dimClasses+a)-2,0, J, matchlist, costlist, curbranch);
20858
20859
20860 for( int i=1; i < nParts; i++){
20861 for(int j=0; j < K; j++){
20862 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
20863
20864 }
20865 }
20866 }
20867
20868 delete[] curbranch;
20869 }
20870
20871
20872 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){
20873
20874
20875 int* curintx2(0);
20876 int nintx = size_curintx;
20877
20878
20879
20880 if (depth >0){
20881 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
20882 if (nintx <= T) return;
20883 }
20884
20885
20886 if (depth == (nParts-1)) {
20887
20888 int replace = 0;
20889 int ind_smallest = -1;
20890 int smallest_cost = -1;
20891
20892 for (int jit = 0; jit < J; jit++){
20893 if (*(costlist+jit) < nintx){
20894 replace = 1;
20895 if (ind_smallest == -1) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20896 if (*(costlist+jit) < smallest_cost) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20897 }
20898 }
20899
20900 if (replace > 0){
20901
20902 *(costlist + ind_smallest) = nintx;
20903 for (int xit = 0; xit < nParts; xit++)
20904 *(matchlist + ind_smallest*nParts + xit) = *(curbranch+xit);
20905
20906 }
20907
20908 return;
20909 }
20910
20911
20912
20913
20914 if (depth > 0){
20915 curintx2 = new int[nintx];
20916 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
20917 }
20918
20919 if (depth == 0){
20920
20921 curintx2 = new int[size_curintx];
20922 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
20923 }
20924
20925
20926
20927 depth=depth+1;
20928
20929 for (int i=0; i < K; i++){
20930
20931 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue;
20932 size_next = (*(dimClasses + depth*K+i ))-2;
20933 if (size_next <= T) continue;
20934 *(curbranch+depth) = i;
20935 Util::explore2(argParts,Indices, dimClasses, nParts, K, T, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth,J, matchlist,
20936 costlist, curbranch);
20937
20938 }
20939
20940 delete[] curintx2;
20941 }
20942
20943 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
20944
20945
20946
20947
20948
20949
20950
20951
20952
20953
20954 int* dummy(0);
20955 int* cref;
20956 int cref_size;
20957 int* ccomp;
20958 int ccomp_size;
20959 int nintx;
20960 for (int i=0; i < nParts; i++){
20961 for (int j =0; j < K; j++){
20962
20963
20964 cref = Parts[i][j];
20965 cref_size = dimClasses[i*K+cref[0]]-2;
20966
20967
20968 if (cref_size <= T){
20969 cref[0] = -1;
20970 continue;
20971 }
20972 bool done = 0;
20973 for (int a = 0; a < nParts; a++){
20974 if (a == i) continue;
20975 bool hasActive=0;
20976 for (unsigned int b=0; b < Parts[a].size(); b++){
20977
20978
20979 ccomp = Parts[a][b];
20980 ccomp_size= dimClasses[a*K+ccomp[0]]-2;
20981 nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
20982
20983
20984 if (nintx <= T)
20985 ccomp[1] = 0;
20986 else{
20987 ccomp[1] = 1;
20988 hasActive=1;
20989 }
20990 }
20991
20992 if (hasActive < 1){
20993 done=1;
20994 break;
20995 }
20996
20997 }
20998
20999 if (done > 0){
21000
21001
21002 cref[0] = -1;
21003 continue;
21004 }
21005
21006
21007
21008
21009
21010
21011
21012
21013
21014
21015 bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
21016
21017 if (found<1){
21018
21019 cref[0] = -1;
21020 }
21021 }
21022
21023
21024
21025 for (int d = K-1; d > -1; d--){
21026 if (Parts[i][d][0] < 0) Parts[i].erase(Parts[i].begin()+d);
21027 }
21028
21029 }
21030
21031
21032
21033
21034
21035 }
21036
21037
21038 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) {
21039
21040
21041 if (size_next <= T) return 0;
21042
21043
21044 int* curintx2(0);
21045 int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
21046 if (nintx <= T) return 0;
21047
21048 int old_depth=depth;
21049 if (depth == partref) depth = depth + 1;
21050 if (depth == nParts && old_depth>0) return 1;
21051
21052
21053
21054 curintx2 = new int[nintx];
21055 Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
21056
21057
21058
21059
21060 bool gt_thresh;
21061 int num_classes = Parts[depth].size();
21062
21063 for (int i=0; i < num_classes; i++){
21064 if (Parts[depth][i][1] < 1) continue;
21065 size_next = dimClasses[depth*K + Parts[depth][i][0] ]-2;
21066 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
21067 if (gt_thresh) { delete[] curintx2; return 1; }
21068 }
21069 delete[] curintx2;
21070 return 0;
21071 }
21072
21073
21074
21075
21076 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int n_guesses, int LARGEST_CLASS, int J,
21077 int max_branching, float stmult, int branchfunc, int LIM) {
21078
21079
21080
21081
21082
21083 int* Indices = new int[nParts*K];
21084 int ind_c = 0;
21085 for (int i=0; i < nParts; i++){
21086 for(int j = 0; j < K; j++){
21087 Indices[i*K + j] = ind_c;
21088 ind_c = ind_c + dimClasses[i*K + j];
21089 }
21090 }
21091
21092
21093
21094
21095 vector <vector <int*> > Parts(nParts,vector<int*>(K));
21096 ind_c = 0;
21097 int argParts_size=0;
21098 for (int i=0; i < nParts; i++){
21099 for(int j = 0; j < K; j++){
21100 Parts[i][j] = argParts + ind_c;
21101 ind_c = ind_c + dimClasses[i*K + j];
21102 argParts_size = argParts_size + dimClasses[i*K + j];
21103 }
21104 }
21105
21106
21107
21108
21109
21110
21111 Util::initial_prune(Parts, dimClasses, nParts, K, T);
21112 for(int i = 0; i < nParts; i++){
21113 for(int j=0; j < K; j++){
21114 argParts[Indices[i*K + j]+1] = -1;
21115 }
21116 }
21117
21118 int num_classes;
21119 int old_index;
21120 for(int i=0; i<nParts; i++){
21121 num_classes = Parts[i].size();
21122 for (int j=0; j < num_classes; j++){
21123 old_index = Parts[i][j][0];
21124
21125 argParts[Indices[i*K + old_index]+1] = 1;
21126 }
21127 }
21128
21129
21130
21131
21132
21133 int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T, 0, n_guesses, LARGEST_CLASS, J, max_branching, stmult, branchfunc, LIM);
21134
21135
21136
21137
21138 bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
21139
21140 delete[] Indices;
21141
21142
21143 if (correct < 1){
21144 cout << "something is wrong with output of branchMPI!\n";
21145 vector<int> ret(1);
21146 ret[0] = -1;
21147 if (output != 0) { delete[] output; output = 0; }
21148 return ret;
21149 }
21150
21151
21152
21153
21154
21155 int output_size = 2 + output[1] * nParts;
21156 vector<int> ret(output_size);
21157 for (int i = 0; i < output_size; i++) {
21158 ret[i]= output[i];
21159 }
21160 if (output != 0) { delete[] output; output = 0; }
21161 return ret;
21162
21163 }
21164
21165
21166 int branch_all=0;
21167 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int curlevel,int n_guesses, int
21168 LARGEST_CLASS, int J, int max_branching, float stmult, int branchfunc, int LIM) {
21169
21170
21171
21172 if (1 == 0){
21173 cout <<"begin test search2\n";
21174 int* matchlist = new int[J*nParts];
21175 int* costlist = new int[J];
21176 for (int jit = 0; jit < nParts; jit++) *(costlist+jit) = 0;
21177 Util::search2(argParts,Indices, dimClasses, nParts, K, T, matchlist,costlist,J);
21178
21179 for (int jit = 0; jit < J; jit++) {
21180 cout << *(costlist +jit)<<": ";
21181 for (int yit = 0; yit < nParts; yit++)
21182 cout << *(matchlist + jit*nParts + yit)<<",";
21183 cout <<"\n";
21184
21185 }
21186 cout <<"end test search2\n";
21187 int* output = new int[1];
21188 output[0] = 1;
21189 delete [] matchlist;
21190 delete [] costlist;
21191 return output;
21192 }
21193
21194
21195
21196 if (curlevel > K -1){
21197 int* output = new int[2];
21198 output[0] = 0;
21199 output[1] = 0;
21200 return output;
21201 }
21202
21203
21204
21205 int* matchlist = new int[J*nParts];
21206 int* costlist = new int[J];
21207 Util::search2(argParts, Indices, dimClasses, nParts, K, T, matchlist, costlist, J);
21208
21209
21210
21211
21212
21213
21214 for (int jit = 0; jit < J ; jit++){
21215
21216 if (costlist[jit] > T) break;
21217 if (jit == J-1){
21218 int* output = new int[2];
21219 output[0] = 0;
21220 output[1] = 0;
21221 delete[] matchlist;
21222 delete[] costlist;
21223 return output;
21224 }
21225 }
21226
21227
21228
21229
21230 if (curlevel==0) branch_all = 0;
21231
21232 int nBranches = -1;
21233
21234 if (branchfunc == 0)
21235 nBranches = branch_factor_0(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM);
21236
21237 if (branchfunc == 2)
21238 nBranches = branch_factor_2(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM);
21239
21240 if (branchfunc == 3)
21241 nBranches = branch_factor_3(costlist,matchlist,J, T, nParts, curlevel, max_branching, K, LIM);
21242
21243 if (branchfunc == 4)
21244 nBranches = branch_factor_4(costlist,matchlist,J, T, nParts, curlevel, max_branching, stmult);
21245
21246 int* newcostlist= new int[nBranches];
21247 int* newmatchlist = new int[nBranches*nParts];
21248 for (int i=0; i<nBranches; i++){
21249 newcostlist[i] = costlist[i];
21250 for (int j=0; j< nParts; j++)
21251 newmatchlist[i*nParts + j] = matchlist[i*nParts + j];
21252 }
21253
21254 delete[] costlist;
21255 delete[] matchlist;
21256
21257
21258 int* output = new int[2+K*nParts];
21259 output[0] = 0;
21260 output[1] = 0;
21261
21262 int old_index;
21263 int totalcost;
21264 int nmatches;
21265
21266
21267 for(int i=0; i < nBranches ; i++){
21268
21269
21270
21271
21272
21273
21274
21275 for(int j=0; j < nParts; j++){
21276
21277 old_index = newmatchlist[i*nParts + j];
21278 argParts[Indices[j*K+old_index] + 1] = -2;
21279 }
21280
21281
21282 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, curlevel+1, n_guesses, LARGEST_CLASS,
21283 J, max_branching, stmult,branchfunc, LIM);
21284
21285
21286 totalcost = newcostlist[i] + ret[0];
21287
21288
21289
21290
21291
21292 if (totalcost > output[0])
21293 {
21294 nmatches = 1 + ret[1];
21295
21296
21297 output[0] = totalcost;
21298 output[1] = nmatches;
21299 int nret = 2+(nmatches-1)*nParts;
21300 for(int iret=2; iret < nret; iret++) output[iret] = ret[iret];
21301 for(int imax=0; imax < nParts; imax++) output[nret+imax] = newmatchlist[i*nParts + imax];
21302 }
21303
21304
21305 delete[] ret;
21306
21307
21308
21309 for(int j=0; j < nParts; j++){
21310 old_index = newmatchlist[i*nParts + j];
21311 argParts[Indices[j*K+old_index] + 1] = 1;
21312 }
21313
21314 }
21315
21316 delete[] newmatchlist;
21317 delete[] newcostlist;
21318
21319 return output;
21320 }
21321
21322 int* costlist_global;
21323
21324 bool jiafunc(int i, int j){
21325 return (costlist_global[j] < costlist_global[i]) ;
21326
21327 }
21328
21329
21330
21331
21332
21333 int Util::branch_factor_2(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21334
21335 int ntot=0;
21336 for (int jit=0; jit < J; jit++){
21337 if (*(costlist+jit) > T) ntot++;
21338 }
21339
21340 int cur;
21341
21342 int* indx = new int[J];
21343 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21344 vector<int> myindx (indx, indx+J);
21345 vector<int>::iterator it;
21346 costlist_global=costlist;
21347 sort(myindx.begin(), myindx.end(), jiafunc);
21348
21349
21350 int* templist = new int[J];
21351 int* temp2list = new int[J*nParts];
21352 int next = 0;
21353
21354 for (it=myindx.begin(); it!=myindx.end();++it){
21355 cur = *(costlist + *it);
21356 if (cur > T){
21357
21358 templist[next] = cur;
21359 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21360 next = next + 1;
21361 }
21362 }
21363
21364 for (int jit=0; jit < ntot; jit++){
21365 *(costlist+jit)=*(templist + jit);
21366
21367 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21368 }
21369
21370
21371 delete [] indx;
21372
21373
21374
21375
21376
21377
21378 int B=1;
21379 int B_init=B;
21380 int infeasible=0;
21381
21382 for (int i=B_init; i<ntot; i++){
21383 if (i==ntot) continue;
21384
21385
21386 infeasible = 0;
21387 if (LIM < 0) LIM = B;
21388 for (int j=0; j<B; j++){
21389
21390 for (int vit=0; vit<nParts; vit++){
21391 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21392 }
21393 if (infeasible >= LIM) break;
21394 }
21395
21396 if (infeasible >= LIM){
21397 *(costlist+B)=*(templist+i);
21398 for (int vit=0; vit < nParts; vit++)
21399 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21400 B=B+1;
21401 }
21402 }
21403
21404 delete [] templist;
21405 delete [] temp2list;
21406
21407
21408 if (branch_all < max_branching){
21409 if (B>1)
21410 {branch_all = branch_all + B -1 ; }
21411 }
21412 else B=1;
21413
21414 return B;
21415
21416
21417 }
21418
21419
21420
21421 int Util::branch_factor_3(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int, int LIM){
21422
21423 int ntot=0;
21424 for (int jit=0; jit < J; jit++){
21425 if (*(costlist+jit) > T) ntot++;
21426 }
21427
21428 int cur;
21429
21430 int* indx = new int[J];
21431 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21432 vector<int> myindx (indx, indx+J);
21433 vector<int>::iterator it;
21434 costlist_global=costlist;
21435 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21436
21437
21438 int* templist = new int[J];
21439 int* temp2list = new int[J*nParts];
21440 int next = 0;
21441
21442 for (it=myindx.begin(); it!=myindx.end();++it){
21443 cur = *(costlist + *it);
21444 if (cur > T){
21445
21446 templist[next] = cur;
21447 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21448 next = next + 1;
21449 }
21450 }
21451
21452 for (int jit=0; jit < ntot; jit++){
21453 *(costlist+jit)=*(templist + jit);
21454
21455 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21456 }
21457
21458
21459 delete [] indx;
21460
21461
21462
21463
21464
21465
21466 int B=1;
21467 int B_init=B;
21468 int infeasible=0;
21469
21470
21471 if (LIM < 0) LIM = ntot-1;
21472 for (int i=B_init; i<ntot; i++){
21473 if (i==ntot) continue;
21474
21475
21476 infeasible = 0;
21477
21478 for (int j=0; j<ntot; j++){
21479 if (j == i) continue;
21480 for (int vit=0; vit<nParts; vit++){
21481 if (temp2list[i*nParts+vit] == temp2list[j*nParts+vit]) {infeasible++; break;}
21482 }
21483 if (infeasible >= LIM) break;
21484 }
21485
21486 if (infeasible >= LIM){
21487 *(costlist+B)=*(templist+i);
21488 for (int vit=0; vit < nParts; vit++)
21489 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21490 B=B+1;
21491 }
21492 }
21493
21494 delete [] templist;
21495 delete [] temp2list;
21496
21497
21498
21499 if (branch_all < max_branching){
21500 if (B>1)
21501 {branch_all = branch_all + B-1;}
21502 }
21503 else B=1;
21504
21505 return B;
21506
21507
21508 }
21509
21510
21511
21512
21513
21514 int Util::branch_factor_4(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, float stmult){
21515 int sum=0;
21516 float average =0;
21517 int ntot=0;
21518 for (int jit=0; jit < J; jit++){
21519 if (*(costlist+jit) > T) {ntot++; sum = sum +*(costlist+jit);}
21520 }
21521 average = ((float)sum)/((float)ntot);
21522 int cur;
21523
21524 int* indx = new int[J];
21525 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21526 vector<int> myindx (indx, indx+J);
21527 vector<int>::iterator it;
21528 costlist_global=costlist;
21529 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21530
21531
21532 int* templist = new int[J];
21533 int* temp2list = new int[J*nParts];
21534 int next = 0;
21535
21536 for (it=myindx.begin(); it!=myindx.end();++it){
21537 cur = *(costlist + *it);
21538 if (cur > T){
21539
21540 templist[next] = cur;
21541 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21542 next = next + 1;
21543 }
21544 }
21545
21546 for (int jit=0; jit < ntot; jit++){
21547 *(costlist+jit)=*(templist + jit);
21548
21549 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21550 }
21551
21552
21553 delete [] indx;
21554 delete [] templist;
21555 delete [] temp2list;
21556
21557 if (ntot == 1) return 1;
21558
21559
21560
21561 float sq_sum=0.0;
21562
21563 for (int i=0; i< ntot; i++){
21564 sq_sum = sq_sum + (float) pow((float) *(costlist+i) - average, (float)2.0);
21565
21566 }
21567
21568
21569 float variance = sq_sum/ntot;
21570 float stdev = (float)pow((float)variance,(float)0.5);
21571
21572
21573
21574 int B=1;
21575 int largest = *costlist;
21576
21577 for (int i=1; i<ntot; i++){
21578 int cur = *(costlist+i);
21579 if (largest-cur < (float)(stdev*stmult)) B++;
21580 else break;
21581
21582 }
21583
21584 if (branch_all < max_branching){
21585 if (B>1)
21586 {branch_all = branch_all + B-1;}
21587 }
21588 else B=1;
21589
21590 return B;
21591
21592
21593 }
21594
21595 int Util::branch_factor_0(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21596
21597 int ntot=0;
21598 for (int jit=0; jit < J; jit++){
21599 if (*(costlist+jit) > T) ntot++;
21600 }
21601
21602 int cur;
21603
21604 int* indx = new int[J];
21605 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21606 vector<int> myindx (indx, indx+J);
21607 vector<int>::iterator it;
21608 costlist_global=costlist;
21609 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21610
21611
21612 int* templist = new int[J];
21613 int* temp2list = new int[J*nParts];
21614 int next = 0;
21615
21616 for (it=myindx.begin(); it!=myindx.end();++it){
21617 cur = *(costlist + *it);
21618 if (cur > T){
21619
21620 templist[next] = cur;
21621 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21622 next = next + 1;
21623 }
21624 }
21625
21626 for (int jit=0; jit < ntot; jit++){
21627 *(costlist+jit)=*(templist + jit);
21628
21629 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21630 }
21631
21632
21633 for (int jit=1; jit < ntot; jit++){
21634
21635 if ((costlist[jit] == costlist[0]) && costlist[jit] > T){
21636
21637 for (int vit=0; vit < nParts; vit++){
21638 if ( matchlist[jit*nParts + vit] > matchlist[vit])
21639 break;
21640 if ( matchlist[jit*nParts + vit] == matchlist[vit])
21641 continue;
21642 if ( matchlist[jit*nParts + vit] < matchlist[vit])
21643 {
21644
21645 for (int swp=0; swp < nParts; swp++){
21646 int tmp = matchlist[swp];
21647 matchlist[swp]= matchlist[jit*nParts + swp];
21648 matchlist[jit*nParts + swp] = tmp;
21649 }
21650 break;
21651
21652 }
21653 }
21654 }
21655
21656 }
21657
21658
21659 delete [] indx;
21660
21661
21662
21663
21664
21665
21666 int B=1;
21667 int B_init=B;
21668 int infeasible=0;
21669
21670 for (int i=B_init; i<ntot; i++){
21671 if (i==ntot) continue;
21672
21673
21674 infeasible = 0;
21675 if (LIM < 0) LIM = B;
21676 for (int j=0; j<B; j++){
21677
21678 for (int vit=0; vit<nParts; vit++){
21679 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21680 }
21681 if (infeasible >= LIM) break;
21682 }
21683
21684 if (infeasible >= LIM){
21685 *(costlist+B)=*(templist+i);
21686 for (int vit=0; vit < nParts; vit++)
21687 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21688 B=B+1;
21689 }
21690 }
21691
21692 delete [] templist;
21693 delete [] temp2list;
21694
21695
21696 if (branch_all < max_branching){
21697 if (B>1)
21698 {branch_all = branch_all + B -1 ; }
21699 }
21700 else B=1;
21701
21702 return B;
21703
21704
21705 }