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 #ifdef _WIN32
00036 #pragma warning(disable:4819)
00037 #include <malloc.h>
00038 #endif //_WIN32
00039
00040 #include <cstring>
00041 #include <ctime>
00042 #include <iostream>
00043 #include <boost/shared_ptr.hpp>
00044 #include <cstdio>
00045 #include <cstdlib>
00046 #include <cassert>
00047 #include "emdata.h"
00048 #include "util.h"
00049 #include "fundamentals.h"
00050 #include "lapackblas.h"
00051 #include "lbfgsb.h"
00052 using namespace EMAN;
00053 #include "steepest.h"
00054 #include "emassert.h"
00055 #include "randnum.h"
00056
00057 #include <gsl/gsl_sf_bessel.h>
00058 #include <gsl/gsl_sf_bessel.h>
00059 #include <cmath>
00060
00061 using namespace std;
00062 using std::complex;
00063
00064
00065 int circum_(double *, double *, double *, double *, int *);
00066 long int left_(double *, double *, double *, double *, double *, double *, double *, double *, double *);
00067 int addnod_(int *, int *, double *, double *, double *, int *, int *, int *, int *, int *);
00068
00069 vector<float> Util::infomask(EMData* Vol, EMData* mask, bool flip = false)
00070
00071
00072 {
00073 ENTERFUNC;
00074 vector<float> stats;
00075 float *Volptr, *maskptr,MAX,MIN;
00076 long double Sum1,Sum2;
00077 long count;
00078
00079 MAX = -FLT_MAX;
00080 MIN = FLT_MAX;
00081 count = 0L;
00082 Sum1 = 0.0L;
00083 Sum2 = 0.0L;
00084
00085 if (mask == NULL) {
00086
00087 stats.push_back(Vol->get_attr("mean"));
00088 stats.push_back(Vol->get_attr("sigma"));
00089 stats.push_back(Vol->get_attr("minimum"));
00090 stats.push_back(Vol->get_attr("maximum"));
00091 return stats;
00092 }
00093
00094
00095
00096 size_t nx = Vol->get_xsize();
00097 size_t ny = Vol->get_ysize();
00098 size_t nz = Vol->get_zsize();
00099
00100 size_t mask_nx = mask->get_xsize();
00101 size_t mask_ny = mask->get_ysize();
00102 size_t mask_nz = mask->get_zsize();
00103
00104 if (nx != mask_nx || ny != mask_ny || nz != mask_nz )
00105 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118 Volptr = Vol->get_data();
00119 maskptr = mask->get_data();
00120
00121 for (size_t i = 0; i < (size_t)nx*ny*nz; ++i) {
00122 if ((maskptr[i]>0.5f) == flip) {
00123 Sum1 += Volptr[i];
00124 Sum2 += Volptr[i]*double(Volptr[i]);
00125 MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00126 MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00127 count++;
00128 }
00129 }
00130
00131 if (count == 0) {
00132 LOGERR("Invalid mask");
00133 throw ImageFormatException( "Invalid mask");
00134 }
00135
00136 float avg = static_cast<float>(Sum1/count);
00137 float sig = static_cast<float>(sqrt((Sum2 - Sum1*Sum1/count)/(count-1)));
00138
00139 stats.push_back(avg);
00140 stats.push_back(sig);
00141 stats.push_back(MIN);
00142 stats.push_back(MAX);
00143
00144 return stats;
00145 }
00146
00147
00148
00149
00150 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00151 {
00152 ENTERFUNC;
00153
00154 if (!EMUtil::is_same_size(V1, V2)) {
00155 LOGERR("images not same size");
00156 throw ImageFormatException( "images not same size");
00157 }
00158
00159 size_t nx = V1->get_xsize();
00160 size_t ny = V1->get_ysize();
00161 size_t nz = V1->get_zsize();
00162 size_t size = (size_t)nx*ny*nz;
00163
00164 EMData *BD = new EMData();
00165 BD->set_size(nx, ny, nz);
00166
00167 float *params = new float[2];
00168
00169 float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00170 long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00171 int nvox = 0L;
00172
00173 V1ptr = V1->get_data();
00174 V2ptr = V2->get_data();
00175 BDptr = BD->get_data();
00176
00177
00178 if(!mask){
00179 EMData * Mask = new EMData();
00180 Mask->set_size(nx,ny,nz);
00181 Mask->to_one();
00182 MASKptr = Mask->get_data();
00183 } else {
00184 if (!EMUtil::is_same_size(V1, mask)) {
00185 LOGERR("mask not same size");
00186 throw ImageFormatException( "mask not same size");
00187 }
00188
00189 MASKptr = mask->get_data();
00190 }
00191
00192
00193
00194
00195
00196 for (size_t i = 0L;i < size; i++) {
00197 if (MASKptr[i]>0.5f) {
00198 S1 += V1ptr[i]*V2ptr[i];
00199 S2 += V1ptr[i]*V1ptr[i];
00200 S3 += V2ptr[i];
00201 S4 += V1ptr[i];
00202 nvox ++;
00203 }
00204 }
00205
00206 if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00207 A =1.0f ;
00208 } else {
00209 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00210 }
00211 B = static_cast<float> (A*S4 - S3)/nvox;
00212
00213
00214
00215 for (size_t i = 0L;i < size; i++) {
00216 if (MASKptr[i]>0.5f) {
00217 BDptr[i] = A*V1ptr[i] - B - V2ptr[i];
00218 } else {
00219 BDptr[i] = 0.f;
00220 }
00221 }
00222
00223 BD->update();
00224
00225 params[0] = A;
00226 params[1] = B;
00227
00228 Dict BDnParams;
00229 BDnParams["imdiff"] = BD;
00230 BDnParams["A"] = params[0];
00231 BDnParams["B"] = params[1];
00232
00233 EXITFUNC;
00234 return BDnParams;
00235 }
00236
00237
00238
00239
00240
00241 EMData *Util::TwoDTestFunc(int Size, float p, float q, float a, float b, int flag, float alphaDeg)
00242 {
00243 ENTERFUNC;
00244 int Mid= (Size+1)/2;
00245
00246 if (flag==0) {
00247 EMData* ImBW = new EMData();
00248 ImBW->set_size(Size,Size,1);
00249 ImBW->to_zero();
00250
00251 float tempIm;
00252 float x,y;
00253
00254 for (int ix=(1-Mid); ix<Mid; ix++){
00255 for (int iy=(1-Mid); iy<Mid; iy++){
00256 x = (float)ix;
00257 y = (float)iy;
00258 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)) );
00259 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00260 }
00261 }
00262 ImBW->update();
00263 ImBW->set_complex(false);
00264 ImBW->set_ri(true);
00265
00266 return ImBW;
00267 }
00268 else if (flag==1) {
00269 EMData* ImBWFFT = new EMData();
00270 ImBWFFT ->set_size(2*Size,Size,1);
00271 ImBWFFT ->to_zero();
00272
00273 float r,s;
00274
00275 for (int ir=(1-Mid); ir<Mid; ir++){
00276 for (int is=(1-Mid); is<Mid; is++){
00277 r = (float)ir;
00278 s = (float)is;
00279 (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00280 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00281 }
00282 }
00283 ImBWFFT->update();
00284 ImBWFFT->set_complex(true);
00285 ImBWFFT->set_ri(true);
00286 ImBWFFT->set_shuffled(true);
00287 ImBWFFT->set_fftodd(true);
00288
00289 return ImBWFFT;
00290 }
00291 else if (flag==2 || flag==3) {
00292 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00293 float C=cos(alpha);
00294 float S=sin(alpha);
00295 float D= sqrt(S*S*b*b + C*C*a*a);
00296
00297
00298 float P = p * C *a*a/D ;
00299 float Q = q * S *b*b/D ;
00300
00301 if (flag==2) {
00302 EMData* pofalpha = new EMData();
00303 pofalpha ->set_size(Size,1,1);
00304 pofalpha ->to_zero();
00305
00306 float Norm0 = D*(float)sqrt(2*pi);
00307 float Norm1 = exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00308 float Norm2 = exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00309 float sD;
00310
00311 for (int is=(1-Mid); is<Mid; is++){
00312 sD = is/D ;
00313 (*pofalpha)(is+Mid-1) = Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00314 + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00315 }
00316 pofalpha-> update();
00317 pofalpha-> set_complex(false);
00318 pofalpha-> set_ri(true);
00319
00320 return pofalpha;
00321 }
00322 if (flag==3) {
00323 float vD;
00324
00325 EMData* pofalphak = new EMData();
00326 pofalphak ->set_size(2*Size,1,1);
00327 pofalphak ->to_zero();
00328
00329 for (int iv=(1-Mid); iv<Mid; iv++){
00330 vD = iv*D ;
00331 (*pofalphak)(2*(iv+Mid-1)) = exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00332 }
00333 pofalphak-> update();
00334 pofalphak-> set_complex(false);
00335 pofalphak-> set_ri(true);
00336
00337 return pofalphak;
00338 }
00339 }
00340 else if (flag==4) {
00341 cout <<" FH under construction";
00342 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00343 EMData* TryFH= OutFT -> real2FH(4.0);
00344 return TryFH;
00345 } else {
00346 cout <<" flag must be 0,1,2,3, or 4";
00347 }
00348
00349 EXITFUNC;
00350 return 0;
00351 }
00352
00353
00354 void Util::spline_mat(float *x, float *y, int n, float *xq, float *yq, int m)
00355 {
00356
00357 float x0= x[0];
00358 float x1= x[1];
00359 float x2= x[2];
00360 float y0= y[0];
00361 float y1= y[1];
00362 float y2= y[2];
00363 float yp1 = (y1-y0)/(x1-x0) + (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1) ;
00364 float xn = x[n];
00365 float xnm1= x[n-1];
00366 float xnm2= x[n-2];
00367 float yn = y[n];
00368 float ynm1= y[n-1];
00369 float ynm2= y[n-2];
00370 float ypn= (yn-ynm1)/(xn-xnm1) + (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00371 float *y2d = new float[n];
00372 Util::spline(x,y,n,yp1,ypn,y2d);
00373 Util::splint(x,y,y2d,n,xq,yq,m);
00374 delete [] y2d;
00375 return;
00376 }
00377
00378
00379 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2)
00380 {
00381 int i,k;
00382 float p, qn, sig, un, *u;
00383 u = new float[n-1];
00384
00385 if (yp1 > .99e30){
00386 y2[0]=u[0]=0.0;
00387 } else {
00388 y2[0]=-.5f;
00389 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00390 }
00391
00392 for (i=1; i < n-1; i++) {
00393 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00394 p = sig*y2[i-1] + 2.0f;
00395 y2[i] = (sig-1.0f)/p;
00396 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) - (y[i] - y[i-1] )/(x[i] -x[i-1]);
00397 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00398 }
00399
00400 if (ypn>.99e30){
00401 qn=0; un=0;
00402 } else {
00403 qn= .5f;
00404 un= (3.0f/(x[n-1] -x[n-2])) * (ypn - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00405 }
00406 y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00407 for (k=n-2; k>=0; k--){
00408 y2[k]=y2[k]*y2[k+1]+u[k];
00409 }
00410 delete [] u;
00411 }
00412
00413
00414 void Util::splint( float *xa, float *ya, float *y2a, int n, float *xq, float *yq, int m)
00415 {
00416 int klo, khi, k;
00417 float h, b, a;
00418
00419
00420 for (int j=0; j<m;j++){
00421 klo=0;
00422 khi=n-1;
00423 while (khi-klo >1) {
00424 k=(khi+klo) >>1;
00425 if (xa[k]>xq[j]){ khi=k;}
00426 else { klo=k;}
00427 }
00428 h=xa[khi]- xa[klo];
00429 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00430 a =(xa[khi]-xq[j])/h;
00431 b=(xq[j]-xa[klo])/h;
00432 yq[j]=a*ya[klo] + b*ya[khi]
00433 + ((a*a*a-a)*y2a[klo]
00434 +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00435 }
00436
00437 }
00438
00439
00440 void Util::Radialize(int *PermMatTr, float *kValsSorted,
00441 float *weightofkValsSorted, int Size, int *SizeReturned)
00442 {
00443 int iMax = (int) floor( (Size-1.0)/2 +.01);
00444 int CountMax = (iMax+2)*(iMax+1)/2;
00445 int Count=-1;
00446 float *kVals = new float[CountMax];
00447 float *weightMat = new float[CountMax];
00448 int *PermMat = new int[CountMax];
00449 SizeReturned[0] = CountMax;
00450
00451
00452 for (int jkx=0; jkx< iMax+1; jkx++) {
00453 for (int jky=0; jky< jkx+1; jky++) {
00454 Count++;
00455 kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00456 weightMat[Count]= 1.0;
00457 if (jkx!=0) { weightMat[Count] *=2;}
00458 if (jky!=0) { weightMat[Count] *=2;}
00459 if (jkx!=jky){ weightMat[Count] *=2;}
00460 PermMat[Count]=Count+1;
00461 }
00462 }
00463
00464 int lkVals = Count+1;
00465
00466
00467 sort_mat(&kVals[0],&kVals[Count],
00468 &PermMat[0], &PermMat[Count]);
00469
00470 fflush(stdout);
00471
00472 int newInd;
00473
00474 for (int iP=0; iP < lkVals ; iP++ ) {
00475 newInd = PermMat[iP];
00476 PermMatTr[newInd-1] = iP+1;
00477 }
00478
00479
00480
00481 int CountA=-1;
00482 int CountB=-1;
00483
00484 while (CountB< (CountMax-1)) {
00485 CountA++;
00486 CountB++;
00487
00488 kValsSorted[CountA] = kVals[CountB] ;
00489 if (CountB<(CountMax-1) ) {
00490 while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001 ) {
00491 SizeReturned[0]--;
00492 for (int iP=0; iP < lkVals; iP++){
00493
00494 if (PermMatTr[iP]>CountA+1) {
00495 PermMatTr[iP]--;
00496 }
00497 }
00498 CountB++;
00499 }
00500 }
00501 }
00502
00503
00504 for (int CountD=0; CountD < CountMax; CountD++) {
00505 newInd = PermMatTr[CountD];
00506 weightofkValsSorted[newInd-1] += weightMat[CountD];
00507 }
00508
00509 }
00510
00511
00512 vector<float>
00513 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00514 {
00515 vector<float> angles;
00516 float psi = 0.0;
00517 if ((0.0 == t1 && 0.0 == t2)||(t1 >= t2)) {
00518 t1 = 0.0f;
00519 t2 = 90.0f;
00520 }
00521 if ((0.0 == p1 && 0.0 == p2)||(p1 >= p2)) {
00522 p1 = 0.0f;
00523 p2 = 359.9f;
00524 }
00525 bool skip = ((t1 < 90.0) && (90.0 == t2) && (0.0 == p1) && (p2 > 180.0));
00526 for (float theta = t1; theta <= t2; theta += delta) {
00527 float detphi;
00528 int lt;
00529 if ((0.0 == theta)||(180.0 == theta)) {
00530 detphi = 360.0f;
00531 lt = 1;
00532 } else {
00533 detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00534 lt = int((p2 - p1)/detphi)-1;
00535 if (lt < 1) lt = 1;
00536 detphi = (p2 - p1)/lt;
00537 }
00538 for (int i = 0; i < lt; i++) {
00539 float phi = p1 + i*detphi;
00540 if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00541 angles.push_back(phi);
00542 angles.push_back(theta);
00543 angles.push_back(psi);
00544 }
00545 }
00546 return angles;
00547 }
00548
00549
00550 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
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
00651
00652 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00653 {
00654
00655
00656 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00657 float quadri;
00658 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00659
00660 x = xx;
00661 y = yy;
00662
00663
00664 while ( x < 1.0 ) x += nxdata;
00665 while ( x >= (float)(nxdata+1) ) x -= nxdata;
00666 while ( y < 1.0 ) y += nydata;
00667 while ( y >= (float)(nydata+1) ) y -= nydata;
00668
00669 i = (int) x;
00670 j = (int) y;
00671
00672 dx0 = x - i;
00673 dy0 = y - j;
00674
00675 ip1 = i + 1;
00676 im1 = i - 1;
00677 jp1 = j + 1;
00678 jm1 = j - 1;
00679
00680 if (ip1 > nxdata) ip1 -= nxdata;
00681 if (im1 < 1) im1 += nxdata;
00682 if (jp1 > nydata) jp1 -= nydata;
00683 if (jm1 < 1) jm1 += nydata;
00684
00685 f0 = fdata(i,j);
00686 c1 = fdata(ip1,j) - f0;
00687 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00688 c3 = fdata(i,jp1) - f0;
00689 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00690
00691 dxb = dx0 - 1;
00692 dyb = dy0 - 1;
00693
00694
00695 if (dx0 >= 0) hxc = 1; else hxc = -1;
00696 if (dy0 >= 0) hyc = 1; else hyc = -1;
00697
00698 ic = i + hxc;
00699 jc = j + hyc;
00700
00701 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00702 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00703
00704 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00705 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00706
00707
00708 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00709
00710 return quadri;
00711 }
00712
00713 #undef fdata
00714
00715 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00716 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00717 {
00718
00719
00720 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00721 float quadri;
00722 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00723
00724 x = xx;
00725 y = yy;
00726
00727
00728 if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00729 x = (float)xnew;
00730 y = (float)ynew;
00731 }
00732
00733
00734 i = (int) x;
00735 j = (int) y;
00736
00737 dx0 = x - i;
00738 dy0 = y - j;
00739
00740 ip1 = i + 1;
00741 im1 = i - 1;
00742 jp1 = j + 1;
00743 jm1 = j - 1;
00744
00745 if (ip1 > nxdata) ip1 -= nxdata;
00746 if (im1 < 1) im1 += nxdata;
00747 if (jp1 > nydata) jp1 -= nydata;
00748 if (jm1 < 1) jm1 += nydata;
00749
00750 f0 = fdata(i,j);
00751 c1 = fdata(ip1,j) - f0;
00752 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00753 c3 = fdata(i,jp1) - f0;
00754 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00755
00756 dxb = dx0 - 1;
00757 dyb = dy0 - 1;
00758
00759
00760 if (dx0 >= 0) hxc = 1; else hxc = -1;
00761 if (dy0 >= 0) hyc = 1; else hyc = -1;
00762
00763 ic = i + hxc;
00764 jc = j + hyc;
00765
00766 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00767 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00768
00769 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00770 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00771
00772
00773 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00774
00775 return quadri;
00776 }
00777
00778 #undef fdata
00779
00780
00781 float Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00782 int K = kb.get_window_size();
00783 int kbmin = -K/2;
00784 int kbmax = -kbmin;
00785 int kbc = kbmax+1;
00786
00787 float pixel =0.0f;
00788 float w=0.0f;
00789
00790 delx = restrict1(delx, nx);
00791 int inxold = int(round(delx));
00792 if ( ny < 2 ) {
00793 float tablex1 = kb.i0win_tab(delx-inxold+3);
00794 float tablex2 = kb.i0win_tab(delx-inxold+2);
00795 float tablex3 = kb.i0win_tab(delx-inxold+1);
00796 float tablex4 = kb.i0win_tab(delx-inxold);
00797 float tablex5 = kb.i0win_tab(delx-inxold-1);
00798 float tablex6 = kb.i0win_tab(delx-inxold-2);
00799 float tablex7 = kb.i0win_tab(delx-inxold-3);
00800
00801 int x1, x2, x3, x4, x5, x6, x7;
00802
00803 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
00804 x1 = (inxold-3+nx)%nx;
00805 x2 = (inxold-2+nx)%nx;
00806 x3 = (inxold-1+nx)%nx;
00807 x4 = (inxold +nx)%nx;
00808 x5 = (inxold+1+nx)%nx;
00809 x6 = (inxold+2+nx)%nx;
00810 x7 = (inxold+3+nx)%nx;
00811 } else {
00812 x1 = inxold-3;
00813 x2 = inxold-2;
00814 x3 = inxold-1;
00815 x4 = inxold;
00816 x5 = inxold+1;
00817 x6 = inxold+2;
00818 x7 = inxold+3;
00819 }
00820
00821 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00822 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00823 data[x7]*tablex7 ;
00824
00825 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00826 } else if ( nz < 2 ) {
00827 dely = restrict1(dely, ny);
00828 int inyold = int(round(dely));
00829 float tablex1 = kb.i0win_tab(delx-inxold+3);
00830 float tablex2 = kb.i0win_tab(delx-inxold+2);
00831 float tablex3 = kb.i0win_tab(delx-inxold+1);
00832 float tablex4 = kb.i0win_tab(delx-inxold);
00833 float tablex5 = kb.i0win_tab(delx-inxold-1);
00834 float tablex6 = kb.i0win_tab(delx-inxold-2);
00835 float tablex7 = kb.i0win_tab(delx-inxold-3);
00836
00837 float tabley1 = kb.i0win_tab(dely-inyold+3);
00838 float tabley2 = kb.i0win_tab(dely-inyold+2);
00839 float tabley3 = kb.i0win_tab(dely-inyold+1);
00840 float tabley4 = kb.i0win_tab(dely-inyold);
00841 float tabley5 = kb.i0win_tab(dely-inyold-1);
00842 float tabley6 = kb.i0win_tab(dely-inyold-2);
00843 float tabley7 = kb.i0win_tab(dely-inyold-3);
00844
00845 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00846
00847 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
00848 x1 = (inxold-3+nx)%nx;
00849 x2 = (inxold-2+nx)%nx;
00850 x3 = (inxold-1+nx)%nx;
00851 x4 = (inxold +nx)%nx;
00852 x5 = (inxold+1+nx)%nx;
00853 x6 = (inxold+2+nx)%nx;
00854 x7 = (inxold+3+nx)%nx;
00855
00856 y1 = ((inyold-3+ny)%ny)*nx;
00857 y2 = ((inyold-2+ny)%ny)*nx;
00858 y3 = ((inyold-1+ny)%ny)*nx;
00859 y4 = ((inyold +ny)%ny)*nx;
00860 y5 = ((inyold+1+ny)%ny)*nx;
00861 y6 = ((inyold+2+ny)%ny)*nx;
00862 y7 = ((inyold+3+ny)%ny)*nx;
00863 } else {
00864 x1 = inxold-3;
00865 x2 = inxold-2;
00866 x3 = inxold-1;
00867 x4 = inxold;
00868 x5 = inxold+1;
00869 x6 = inxold+2;
00870 x7 = inxold+3;
00871
00872 y1 = (inyold-3)*nx;
00873 y2 = (inyold-2)*nx;
00874 y3 = (inyold-1)*nx;
00875 y4 = inyold*nx;
00876 y5 = (inyold+1)*nx;
00877 y6 = (inyold+2)*nx;
00878 y7 = (inyold+3)*nx;
00879 }
00880
00881 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00882 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00883 data[x7+y1]*tablex7 ) * tabley1 +
00884 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00885 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00886 data[x7+y2]*tablex7 ) * tabley2 +
00887 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00888 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00889 data[x7+y3]*tablex7 ) * tabley3 +
00890 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00891 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00892 data[x7+y4]*tablex7 ) * tabley4 +
00893 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00894 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00895 data[x7+y5]*tablex7 ) * tabley5 +
00896 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00897 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00898 data[x7+y6]*tablex7 ) * tabley6 +
00899 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00900 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00901 data[x7+y7]*tablex7 ) * tabley7;
00902
00903 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00904 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00905 } else {
00906 dely = restrict1(dely, ny);
00907 int inyold = int(Util::round(dely));
00908 delz = restrict1(delz, nz);
00909 int inzold = int(Util::round(delz));
00910
00911 float tablex1 = kb.i0win_tab(delx-inxold+3);
00912 float tablex2 = kb.i0win_tab(delx-inxold+2);
00913 float tablex3 = kb.i0win_tab(delx-inxold+1);
00914 float tablex4 = kb.i0win_tab(delx-inxold);
00915 float tablex5 = kb.i0win_tab(delx-inxold-1);
00916 float tablex6 = kb.i0win_tab(delx-inxold-2);
00917 float tablex7 = kb.i0win_tab(delx-inxold-3);
00918
00919 float tabley1 = kb.i0win_tab(dely-inyold+3);
00920 float tabley2 = kb.i0win_tab(dely-inyold+2);
00921 float tabley3 = kb.i0win_tab(dely-inyold+1);
00922 float tabley4 = kb.i0win_tab(dely-inyold);
00923 float tabley5 = kb.i0win_tab(dely-inyold-1);
00924 float tabley6 = kb.i0win_tab(dely-inyold-2);
00925 float tabley7 = kb.i0win_tab(dely-inyold-3);
00926
00927 float tablez1 = kb.i0win_tab(delz-inzold+3);
00928 float tablez2 = kb.i0win_tab(delz-inzold+2);
00929 float tablez3 = kb.i0win_tab(delz-inzold+1);
00930 float tablez4 = kb.i0win_tab(delz-inzold);
00931 float tablez5 = kb.i0win_tab(delz-inzold-1);
00932 float tablez6 = kb.i0win_tab(delz-inzold-2);
00933 float tablez7 = kb.i0win_tab(delz-inzold-3);
00934
00935 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00936
00937 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
00938 x1 = (inxold-3+nx)%nx;
00939 x2 = (inxold-2+nx)%nx;
00940 x3 = (inxold-1+nx)%nx;
00941 x4 = (inxold +nx)%nx;
00942 x5 = (inxold+1+nx)%nx;
00943 x6 = (inxold+2+nx)%nx;
00944 x7 = (inxold+3+nx)%nx;
00945
00946 y1 = ((inyold-3+ny)%ny)*nx;
00947 y2 = ((inyold-2+ny)%ny)*nx;
00948 y3 = ((inyold-1+ny)%ny)*nx;
00949 y4 = ((inyold +ny)%ny)*nx;
00950 y5 = ((inyold+1+ny)%ny)*nx;
00951 y6 = ((inyold+2+ny)%ny)*nx;
00952 y7 = ((inyold+3+ny)%ny)*nx;
00953
00954 z1 = ((inzold-3+nz)%nz)*nx*ny;
00955 z2 = ((inzold-2+nz)%nz)*nx*ny;
00956 z3 = ((inzold-1+nz)%nz)*nx*ny;
00957 z4 = ((inzold +nz)%nz)*nx*ny;
00958 z5 = ((inzold+1+nz)%nz)*nx*ny;
00959 z6 = ((inzold+2+nz)%nz)*nx*ny;
00960 z7 = ((inzold+3+nz)%nz)*nx*ny;
00961 } else {
00962 x1 = inxold-3;
00963 x2 = inxold-2;
00964 x3 = inxold-1;
00965 x4 = inxold;
00966 x5 = inxold+1;
00967 x6 = inxold+2;
00968 x7 = inxold+3;
00969
00970 y1 = (inyold-3)*nx;
00971 y2 = (inyold-2)*nx;
00972 y3 = (inyold-1)*nx;
00973 y4 = inyold*nx;
00974 y5 = (inyold+1)*nx;
00975 y6 = (inyold+2)*nx;
00976 y7 = (inyold+3)*nx;
00977
00978 z1 = (inzold-3)*nx*ny;
00979 z2 = (inzold-2)*nx*ny;
00980 z3 = (inzold-1)*nx*ny;
00981 z4 = inzold*nx*ny;
00982 z5 = (inzold+1)*nx*ny;
00983 z6 = (inzold+2)*nx*ny;
00984 z7 = (inzold+3)*nx*ny;
00985 }
00986
00987 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00988 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00989 data[x7+y1+z1]*tablex7 ) * tabley1 +
00990 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00991 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00992 data[x7+y2+z1]*tablex7 ) * tabley2 +
00993 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00994 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00995 data[x7+y3+z1]*tablex7 ) * tabley3 +
00996 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00997 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00998 data[x7+y4+z1]*tablex7 ) * tabley4 +
00999 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01000 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01001 data[x7+y5+z1]*tablex7 ) * tabley5 +
01002 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01003 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01004 data[x7+y6+z1]*tablex7 ) * tabley6 +
01005 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01006 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01007 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01008 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01009 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01010 data[x7+y1+z2]*tablex7 ) * tabley1 +
01011 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01012 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01013 data[x7+y2+z2]*tablex7 ) * tabley2 +
01014 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01015 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01016 data[x7+y3+z2]*tablex7 ) * tabley3 +
01017 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01018 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01019 data[x7+y4+z2]*tablex7 ) * tabley4 +
01020 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01021 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01022 data[x7+y5+z2]*tablex7 ) * tabley5 +
01023 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01024 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01025 data[x7+y6+z2]*tablex7 ) * tabley6 +
01026 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01027 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01028 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01029 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01030 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01031 data[x7+y1+z3]*tablex7 ) * tabley1 +
01032 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01033 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01034 data[x7+y2+z3]*tablex7 ) * tabley2 +
01035 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01036 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01037 data[x7+y3+z3]*tablex7 ) * tabley3 +
01038 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01039 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01040 data[x7+y4+z3]*tablex7 ) * tabley4 +
01041 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01042 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01043 data[x7+y5+z3]*tablex7 ) * tabley5 +
01044 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01045 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01046 data[x7+y6+z3]*tablex7 ) * tabley6 +
01047 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01048 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01049 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01050 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01051 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01052 data[x7+y1+z4]*tablex7 ) * tabley1 +
01053 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01054 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01055 data[x7+y2+z4]*tablex7 ) * tabley2 +
01056 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01057 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01058 data[x7+y3+z4]*tablex7 ) * tabley3 +
01059 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01060 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01061 data[x7+y4+z4]*tablex7 ) * tabley4 +
01062 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01063 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01064 data[x7+y5+z4]*tablex7 ) * tabley5 +
01065 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01066 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01067 data[x7+y6+z4]*tablex7 ) * tabley6 +
01068 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01069 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01070 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01071 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01072 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01073 data[x7+y1+z5]*tablex7 ) * tabley1 +
01074 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01075 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01076 data[x7+y2+z5]*tablex7 ) * tabley2 +
01077 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01078 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01079 data[x7+y3+z5]*tablex7 ) * tabley3 +
01080 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01081 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01082 data[x7+y4+z5]*tablex7 ) * tabley4 +
01083 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01084 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01085 data[x7+y5+z5]*tablex7 ) * tabley5 +
01086 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01087 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01088 data[x7+y6+z5]*tablex7 ) * tabley6 +
01089 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01090 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01091 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01092 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01093 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01094 data[x7+y1+z6]*tablex7 ) * tabley1 +
01095 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01096 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01097 data[x7+y2+z6]*tablex7 ) * tabley2 +
01098 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01099 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01100 data[x7+y3+z6]*tablex7 ) * tabley3 +
01101 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01102 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01103 data[x7+y4+z6]*tablex7 ) * tabley4 +
01104 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01105 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01106 data[x7+y5+z6]*tablex7 ) * tabley5 +
01107 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01108 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01109 data[x7+y6+z6]*tablex7 ) * tabley6 +
01110 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01111 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01112 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01113 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01114 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01115 data[x7+y1+z7]*tablex7 ) * tabley1 +
01116 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01117 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01118 data[x7+y2+z7]*tablex7 ) * tabley2 +
01119 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01120 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01121 data[x7+y3+z7]*tablex7 ) * tabley3 +
01122 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01123 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01124 data[x7+y4+z7]*tablex7 ) * tabley4 +
01125 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01126 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01127 data[x7+y5+z7]*tablex7 ) * tabley5 +
01128 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01129 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01130 data[x7+y6+z7]*tablex7 ) * tabley6 +
01131 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01132 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01133 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01134
01135 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01136 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01137 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01138 }
01139 return pixel/w;
01140 }
01141
01142 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) {
01143 int K = kb.get_window_size();
01144 int kbmin = -K/2;
01145 int kbmax = -kbmin;
01146 int kbc = kbmax+1;
01147
01148 float pixel =0.0f;
01149 float w=0.0f;
01150
01151 float argdelx = delx;
01152 delx = restrict1(delx, nx);
01153 int inxold = int(round(delx));
01154 if ( ny < 2 ) {
01155 float tablex1 = kb.i0win_tab(delx-inxold+3);
01156 float tablex2 = kb.i0win_tab(delx-inxold+2);
01157 float tablex3 = kb.i0win_tab(delx-inxold+1);
01158 float tablex4 = kb.i0win_tab(delx-inxold);
01159 float tablex5 = kb.i0win_tab(delx-inxold-1);
01160 float tablex6 = kb.i0win_tab(delx-inxold-2);
01161 float tablex7 = kb.i0win_tab(delx-inxold-3);
01162
01163 int x1, x2, x3, x4, x5, x6, x7;
01164
01165 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
01166 x1 = (inxold-3+nx)%nx;
01167 x2 = (inxold-2+nx)%nx;
01168 x3 = (inxold-1+nx)%nx;
01169 x4 = (inxold +nx)%nx;
01170 x5 = (inxold+1+nx)%nx;
01171 x6 = (inxold+2+nx)%nx;
01172 x7 = (inxold+3+nx)%nx;
01173 } else {
01174 x1 = inxold-3;
01175 x2 = inxold-2;
01176 x3 = inxold-1;
01177 x4 = inxold;
01178 x5 = inxold+1;
01179 x6 = inxold+2;
01180 x7 = inxold+3;
01181 }
01182
01183 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01184 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01185 data[x7]*tablex7 ;
01186
01187 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01188 } else if ( nz < 2 ) {
01189
01190 delx = argdelx;
01191
01192 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01193 delx = (float)xnew*2.0f;
01194 dely = (float)ynew*2.0f;
01195 }
01196
01197 int inxold = int(round(delx));
01198 int inyold = int(round(dely));
01199
01200 float tablex1 = kb.i0win_tab(delx-inxold+3);
01201 float tablex2 = kb.i0win_tab(delx-inxold+2);
01202 float tablex3 = kb.i0win_tab(delx-inxold+1);
01203 float tablex4 = kb.i0win_tab(delx-inxold);
01204 float tablex5 = kb.i0win_tab(delx-inxold-1);
01205 float tablex6 = kb.i0win_tab(delx-inxold-2);
01206 float tablex7 = kb.i0win_tab(delx-inxold-3);
01207
01208 float tabley1 = kb.i0win_tab(dely-inyold+3);
01209 float tabley2 = kb.i0win_tab(dely-inyold+2);
01210 float tabley3 = kb.i0win_tab(dely-inyold+1);
01211 float tabley4 = kb.i0win_tab(dely-inyold);
01212 float tabley5 = kb.i0win_tab(dely-inyold-1);
01213 float tabley6 = kb.i0win_tab(dely-inyold-2);
01214 float tabley7 = kb.i0win_tab(dely-inyold-3);
01215
01216 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01217
01218 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
01219 x1 = (inxold-3+nx)%nx;
01220 x2 = (inxold-2+nx)%nx;
01221 x3 = (inxold-1+nx)%nx;
01222 x4 = (inxold +nx)%nx;
01223 x5 = (inxold+1+nx)%nx;
01224 x6 = (inxold+2+nx)%nx;
01225 x7 = (inxold+3+nx)%nx;
01226
01227 y1 = ((inyold-3+ny)%ny)*nx;
01228 y2 = ((inyold-2+ny)%ny)*nx;
01229 y3 = ((inyold-1+ny)%ny)*nx;
01230 y4 = ((inyold +ny)%ny)*nx;
01231 y5 = ((inyold+1+ny)%ny)*nx;
01232 y6 = ((inyold+2+ny)%ny)*nx;
01233 y7 = ((inyold+3+ny)%ny)*nx;
01234 } else {
01235 x1 = inxold-3;
01236 x2 = inxold-2;
01237 x3 = inxold-1;
01238 x4 = inxold;
01239 x5 = inxold+1;
01240 x6 = inxold+2;
01241 x7 = inxold+3;
01242
01243 y1 = (inyold-3)*nx;
01244 y2 = (inyold-2)*nx;
01245 y3 = (inyold-1)*nx;
01246 y4 = inyold*nx;
01247 y5 = (inyold+1)*nx;
01248 y6 = (inyold+2)*nx;
01249 y7 = (inyold+3)*nx;
01250 }
01251
01252 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01253 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01254 data[x7+y1]*tablex7 ) * tabley1 +
01255 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01256 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01257 data[x7+y2]*tablex7 ) * tabley2 +
01258 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01259 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01260 data[x7+y3]*tablex7 ) * tabley3 +
01261 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01262 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01263 data[x7+y4]*tablex7 ) * tabley4 +
01264 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01265 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01266 data[x7+y5]*tablex7 ) * tabley5 +
01267 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01268 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01269 data[x7+y6]*tablex7 ) * tabley6 +
01270 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01271 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01272 data[x7+y7]*tablex7 ) * tabley7;
01273
01274 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01275 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01276 } else {
01277 dely = restrict1(dely, ny);
01278 int inyold = int(Util::round(dely));
01279 delz = restrict1(delz, nz);
01280 int inzold = int(Util::round(delz));
01281
01282 float tablex1 = kb.i0win_tab(delx-inxold+3);
01283 float tablex2 = kb.i0win_tab(delx-inxold+2);
01284 float tablex3 = kb.i0win_tab(delx-inxold+1);
01285 float tablex4 = kb.i0win_tab(delx-inxold);
01286 float tablex5 = kb.i0win_tab(delx-inxold-1);
01287 float tablex6 = kb.i0win_tab(delx-inxold-2);
01288 float tablex7 = kb.i0win_tab(delx-inxold-3);
01289
01290 float tabley1 = kb.i0win_tab(dely-inyold+3);
01291 float tabley2 = kb.i0win_tab(dely-inyold+2);
01292 float tabley3 = kb.i0win_tab(dely-inyold+1);
01293 float tabley4 = kb.i0win_tab(dely-inyold);
01294 float tabley5 = kb.i0win_tab(dely-inyold-1);
01295 float tabley6 = kb.i0win_tab(dely-inyold-2);
01296 float tabley7 = kb.i0win_tab(dely-inyold-3);
01297
01298 float tablez1 = kb.i0win_tab(delz-inzold+3);
01299 float tablez2 = kb.i0win_tab(delz-inzold+2);
01300 float tablez3 = kb.i0win_tab(delz-inzold+1);
01301 float tablez4 = kb.i0win_tab(delz-inzold);
01302 float tablez5 = kb.i0win_tab(delz-inzold-1);
01303 float tablez6 = kb.i0win_tab(delz-inzold-2);
01304 float tablez7 = kb.i0win_tab(delz-inzold-3);
01305
01306 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01307
01308 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
01309 x1 = (inxold-3+nx)%nx;
01310 x2 = (inxold-2+nx)%nx;
01311 x3 = (inxold-1+nx)%nx;
01312 x4 = (inxold +nx)%nx;
01313 x5 = (inxold+1+nx)%nx;
01314 x6 = (inxold+2+nx)%nx;
01315 x7 = (inxold+3+nx)%nx;
01316
01317 y1 = ((inyold-3+ny)%ny)*nx;
01318 y2 = ((inyold-2+ny)%ny)*nx;
01319 y3 = ((inyold-1+ny)%ny)*nx;
01320 y4 = ((inyold +ny)%ny)*nx;
01321 y5 = ((inyold+1+ny)%ny)*nx;
01322 y6 = ((inyold+2+ny)%ny)*nx;
01323 y7 = ((inyold+3+ny)%ny)*nx;
01324
01325 z1 = ((inzold-3+nz)%nz)*nx*ny;
01326 z2 = ((inzold-2+nz)%nz)*nx*ny;
01327 z3 = ((inzold-1+nz)%nz)*nx*ny;
01328 z4 = ((inzold +nz)%nz)*nx*ny;
01329 z5 = ((inzold+1+nz)%nz)*nx*ny;
01330 z6 = ((inzold+2+nz)%nz)*nx*ny;
01331 z7 = ((inzold+3+nz)%nz)*nx*ny;
01332 } else {
01333 x1 = inxold-3;
01334 x2 = inxold-2;
01335 x3 = inxold-1;
01336 x4 = inxold;
01337 x5 = inxold+1;
01338 x6 = inxold+2;
01339 x7 = inxold+3;
01340
01341 y1 = (inyold-3)*nx;
01342 y2 = (inyold-2)*nx;
01343 y3 = (inyold-1)*nx;
01344 y4 = inyold*nx;
01345 y5 = (inyold+1)*nx;
01346 y6 = (inyold+2)*nx;
01347 y7 = (inyold+3)*nx;
01348
01349 z1 = (inzold-3)*nx*ny;
01350 z2 = (inzold-2)*nx*ny;
01351 z3 = (inzold-1)*nx*ny;
01352 z4 = inzold*nx*ny;
01353 z5 = (inzold+1)*nx*ny;
01354 z6 = (inzold+2)*nx*ny;
01355 z7 = (inzold+3)*nx*ny;
01356 }
01357
01358 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01359 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01360 data[x7+y1+z1]*tablex7 ) * tabley1 +
01361 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01362 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01363 data[x7+y2+z1]*tablex7 ) * tabley2 +
01364 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01365 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01366 data[x7+y3+z1]*tablex7 ) * tabley3 +
01367 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01368 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01369 data[x7+y4+z1]*tablex7 ) * tabley4 +
01370 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01371 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01372 data[x7+y5+z1]*tablex7 ) * tabley5 +
01373 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01374 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01375 data[x7+y6+z1]*tablex7 ) * tabley6 +
01376 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01377 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01378 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01379 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01380 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01381 data[x7+y1+z2]*tablex7 ) * tabley1 +
01382 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01383 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01384 data[x7+y2+z2]*tablex7 ) * tabley2 +
01385 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01386 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01387 data[x7+y3+z2]*tablex7 ) * tabley3 +
01388 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01389 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01390 data[x7+y4+z2]*tablex7 ) * tabley4 +
01391 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01392 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01393 data[x7+y5+z2]*tablex7 ) * tabley5 +
01394 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01395 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01396 data[x7+y6+z2]*tablex7 ) * tabley6 +
01397 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01398 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01399 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01400 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01401 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01402 data[x7+y1+z3]*tablex7 ) * tabley1 +
01403 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01404 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01405 data[x7+y2+z3]*tablex7 ) * tabley2 +
01406 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01407 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01408 data[x7+y3+z3]*tablex7 ) * tabley3 +
01409 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01410 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01411 data[x7+y4+z3]*tablex7 ) * tabley4 +
01412 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01413 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01414 data[x7+y5+z3]*tablex7 ) * tabley5 +
01415 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01416 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01417 data[x7+y6+z3]*tablex7 ) * tabley6 +
01418 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01419 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01420 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01421 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01422 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01423 data[x7+y1+z4]*tablex7 ) * tabley1 +
01424 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01425 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01426 data[x7+y2+z4]*tablex7 ) * tabley2 +
01427 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01428 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01429 data[x7+y3+z4]*tablex7 ) * tabley3 +
01430 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01431 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01432 data[x7+y4+z4]*tablex7 ) * tabley4 +
01433 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01434 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01435 data[x7+y5+z4]*tablex7 ) * tabley5 +
01436 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01437 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01438 data[x7+y6+z4]*tablex7 ) * tabley6 +
01439 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01440 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01441 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01442 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01443 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01444 data[x7+y1+z5]*tablex7 ) * tabley1 +
01445 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01446 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01447 data[x7+y2+z5]*tablex7 ) * tabley2 +
01448 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01449 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01450 data[x7+y3+z5]*tablex7 ) * tabley3 +
01451 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01452 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01453 data[x7+y4+z5]*tablex7 ) * tabley4 +
01454 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01455 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01456 data[x7+y5+z5]*tablex7 ) * tabley5 +
01457 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01458 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01459 data[x7+y6+z5]*tablex7 ) * tabley6 +
01460 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01461 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01462 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01463 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01464 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01465 data[x7+y1+z6]*tablex7 ) * tabley1 +
01466 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01467 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01468 data[x7+y2+z6]*tablex7 ) * tabley2 +
01469 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01470 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01471 data[x7+y3+z6]*tablex7 ) * tabley3 +
01472 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01473 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01474 data[x7+y4+z6]*tablex7 ) * tabley4 +
01475 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01476 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01477 data[x7+y5+z6]*tablex7 ) * tabley5 +
01478 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01479 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01480 data[x7+y6+z6]*tablex7 ) * tabley6 +
01481 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01482 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01483 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01484 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01485 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01486 data[x7+y1+z7]*tablex7 ) * tabley1 +
01487 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01488 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01489 data[x7+y2+z7]*tablex7 ) * tabley2 +
01490 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01491 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01492 data[x7+y3+z7]*tablex7 ) * tabley3 +
01493 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01494 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01495 data[x7+y4+z7]*tablex7 ) * tabley4 +
01496 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01497 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01498 data[x7+y5+z7]*tablex7 ) * tabley5 +
01499 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01500 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01501 data[x7+y6+z7]*tablex7 ) * tabley6 +
01502 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01503 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01504 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01505
01506 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01507 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01508 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01509 }
01510 return pixel/w;
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
01690
01691 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01692
01693 int nxreal = nx - 2;
01694 if (nxreal != ny)
01695 throw ImageDimensionException("extractpoint requires ny == nx");
01696 int nhalf = nxreal/2;
01697 bool flip = (nuxnew < 0.f);
01698 if (flip) {
01699 nuxnew *= -1;
01700 nuynew *= -1;
01701 }
01702 if (nuynew >= nhalf-0.5) {
01703 nuynew -= nxreal;
01704 } else if (nuynew < -nhalf-0.5) {
01705 nuynew += nxreal;
01706 }
01707
01708
01709
01710
01711 int ixn = int(Util::round(nuxnew));
01712 int iyn = int(Util::round(nuynew));
01713
01714
01715 static float wy[7];
01716 static float wx[7];
01717
01718 float iynn = nuynew - iyn;
01719 wy[0] = kb.i0win_tab(iynn+3);
01720 wy[1] = kb.i0win_tab(iynn+2);
01721 wy[2] = kb.i0win_tab(iynn+1);
01722 wy[3] = kb.i0win_tab(iynn);
01723 wy[4] = kb.i0win_tab(iynn-1);
01724 wy[5] = kb.i0win_tab(iynn-2);
01725 wy[6] = kb.i0win_tab(iynn-3);
01726
01727 float ixnn = nuxnew - ixn;
01728 wx[0] = kb.i0win_tab(ixnn+3);
01729 wx[1] = kb.i0win_tab(ixnn+2);
01730 wx[2] = kb.i0win_tab(ixnn+1);
01731 wx[3] = kb.i0win_tab(ixnn);
01732 wx[4] = kb.i0win_tab(ixnn-1);
01733 wx[5] = kb.i0win_tab(ixnn-2);
01734 wx[6] = kb.i0win_tab(ixnn-3);
01735
01736 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]);
01737
01738 complex<float> result(0.f,0.f);
01739 if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01740
01741 for (int iy = 0; iy < 7; iy++) {
01742 int iyp = iyn + iy - 3 ;
01743 for (int ix = 0; ix < 7; ix++) {
01744 int ixp = ixn + ix - 3;
01745 float w = wx[ix]*wy[iy];
01746 complex<float> val = fimage->cmplx(ixp,iyp);
01747 result += val*w;
01748 }
01749 }
01750 } else {
01751
01752 for (int iy = 0; iy < 7; iy++) {
01753 int iyp = iyn + iy - 3;
01754 for (int ix = 0; ix < 7; ix++) {
01755 int ixp = ixn + ix - 3;
01756 bool mirror = false;
01757 int ixt = ixp, iyt = iyp;
01758 if (ixt < 0) {
01759 ixt = -ixt;
01760 iyt = -iyt;
01761 mirror = !mirror;
01762 }
01763 if (ixt > nhalf) {
01764 ixt = nxreal - ixt;
01765 iyt = -iyt;
01766 mirror = !mirror;
01767 }
01768 if (iyt > nhalf-1) iyt -= nxreal;
01769 if (iyt < -nhalf) iyt += nxreal;
01770 float w = wx[ix]*wy[iy];
01771 complex<float> val = fimage->cmplx(ixt,iyt);
01772 if (mirror) result += conj(val)*w;
01773 else result += val*w;
01774 }
01775 }
01776 }
01777 if (flip) result = conj(result)/wsum;
01778 else result /= wsum;
01779 return result;
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
01909
01910 float Util::triquad(float R, float S, float T, float* fdata)
01911 {
01912
01913 const float C2 = 0.5f;
01914 const float C4 = 0.25f;
01915 const float C8 = 0.125f;
01916
01917 float RS = R * S;
01918 float ST = S * T;
01919 float RT = R * T;
01920 float RST = R * ST;
01921
01922 float RSQ = 1-R*R;
01923 float SSQ = 1-S*S;
01924 float TSQ = 1-T*T;
01925
01926 float RM1 = (1-R);
01927 float SM1 = (1-S);
01928 float TM1 = (1-T);
01929
01930 float RP1 = (1+R);
01931 float SP1 = (1+S);
01932 float TP1 = (1+T);
01933
01934 float triquad =
01935 (-C8) * RST * RM1 * SM1 * TM1 * fdata[0] +
01936 ( C4) * ST * RSQ * SM1 * TM1 * fdata[1] +
01937 ( C8) * RST * RP1 * SM1 * TM1 * fdata[2] +
01938 ( C4) * RT * RM1 * SSQ * TM1 * fdata[3] +
01939 (-C2) * T * RSQ * SSQ * TM1 * fdata[4] +
01940 (-C4) * RT * RP1 * SSQ * TM1 * fdata[5] +
01941 ( C8) * RST * RM1 * SP1 * TM1 * fdata[6] +
01942 (-C4) * ST * RSQ * SP1 * TM1 * fdata[7] +
01943 (-C8) * RST * RP1 * SP1 * TM1 * fdata[8] +
01944
01945 ( C4) * RS * RM1 * SM1 * TSQ * fdata[9] +
01946 (-C2) * S * RSQ * SM1 * TSQ * fdata[10] +
01947 (-C4) * RS * RP1 * SM1 * TSQ * fdata[11] +
01948 (-C2) * R * RM1 * SSQ * TSQ * fdata[12] +
01949 RSQ * SSQ * TSQ * fdata[13] +
01950 ( C2) * R * RP1 * SSQ * TSQ * fdata[14] +
01951 (-C4) * RS * RM1 * SP1 * TSQ * fdata[15] +
01952 ( C2) * S * RSQ * SP1 * TSQ * fdata[16] +
01953 ( C4) * RS * RP1 * SP1 * TSQ * fdata[17] +
01954
01955 ( C8) * RST * RM1 * SM1 * TP1 * fdata[18] +
01956 (-C4) * ST * RSQ * SM1 * TP1 * fdata[19] +
01957 (-C8) * RST * RP1 * SM1 * TP1 * fdata[20] +
01958 (-C4) * RT * RM1 * SSQ * TP1 * fdata[21] +
01959 ( C2) * T * RSQ * SSQ * TP1 * fdata[22] +
01960 ( C4) * RT * RP1 * SSQ * TP1 * fdata[23] +
01961 (-C8) * RST * RM1 * SP1 * TP1 * fdata[24] +
01962 ( C4) * ST * RSQ * SP1 * TP1 * fdata[25] +
01963 ( C8) * RST * RP1 * SP1 * TP1 * fdata[26] ;
01964 return triquad;
01965 }
01966
01967 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01968 : M(M_), fc(fc_), ntable(ntable_) {
01969
01970 build_sBtable();
01971 }
01972
01973 void Util::sincBlackman::build_sBtable() {
01974 sBtable.resize(ntable+1);
01975 int ltab = int(round(float(ntable)/1.25f));
01976 int M2 = M/2;
01977 fltb = float(ltab)/M2;
01978 for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01979 float x = 1.0e-7f;
01980 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)));
01981 for (int i=1; i <= ltab; i++) {
01982 x = float(i)/fltb;
01983 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)));
01984
01985 }
01986 }
01987
01988 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01989 int N_, float vtable_, int ntable_)
01990 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01991 ntable(ntable_) {
01992
01993 if (0.f == v) v = float(K)/2;
01994 if (0.f == vtable) vtable = v;
01995 alphar = alpha*r;
01996 fac = static_cast<float>(twopi)*alphar*v;
01997 vadjust = 1.0f*v;
01998 facadj = static_cast<float>(twopi)*alphar*vadjust;
01999 build_I0table();
02000 }
02001
02002 float Util::KaiserBessel::i0win(float x) const {
02003 float val0 = float(gsl_sf_bessel_I0(facadj));
02004 float absx = fabs(x);
02005 if (absx > vadjust) return 0.f;
02006 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02007 float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02008 return res;
02009 }
02010
02011 void Util::KaiserBessel::build_I0table() {
02012 i0table.resize(ntable+1);
02013 int ltab = int(round(float(ntable)/1.25f));
02014 fltb = float(ltab)/(K/2);
02015 float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02016 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02017 for (int i=0; i <= ltab; i++) {
02018 float s = float(i)/fltb/N;
02019 if (s < vadjust) {
02020 float rt = sqrt(1.f - pow(s/vadjust, 2));
02021 i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02022 } else {
02023 i0table[i] = 0.f;
02024 }
02025
02026 }
02027 }
02028
02029 float Util::KaiserBessel::I0table_maxerror() {
02030 float maxdiff = 0.f;
02031 for (int i = 1; i <= ntable; i++) {
02032 float diff = fabs(i0table[i] - i0table[i-1]);
02033 if (diff > maxdiff) maxdiff = diff;
02034 }
02035 return maxdiff;
02036 }
02037
02038 float Util::KaiserBessel::sinhwin(float x) const {
02039 float val0 = sinh(fac)/fac;
02040 float absx = fabs(x);
02041 if (0.0 == x) {
02042 float res = 1.0f;
02043 return res;
02044 } else if (absx == alphar) {
02045 return 1.0f/val0;
02046 } else if (absx < alphar) {
02047 float rt = sqrt(1.0f - pow((x/alphar), 2));
02048 float facrt = fac*rt;
02049 float res = (sinh(facrt)/facrt)/val0;
02050 return res;
02051 } else {
02052 float rt = sqrt(pow((x/alphar),2) - 1.f);
02053 float facrt = fac*rt;
02054 float res = (sin(facrt)/facrt)/val0;
02055 return res;
02056 }
02057 }
02058
02059 float Util::FakeKaiserBessel::i0win(float x) const {
02060 float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02061 float absx = fabs(x);
02062 if (absx > vadjust) return 0.f;
02063 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02064 float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02065 return res;
02066 }
02067
02068 void Util::FakeKaiserBessel::build_I0table() {
02069 i0table.resize(ntable+1);
02070 int ltab = int(round(float(ntable)/1.1f));
02071 fltb = float(ltab)/(K/2);
02072 float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02073 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02074 for (int i=0; i <= ltab; i++) {
02075 float s = float(i)/fltb/N;
02076 if (s < vadjust) {
02077 float rt = sqrt(1.f - pow(s/vadjust, 2));
02078 i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02079 } else {
02080 i0table[i] = 0.f;
02081 }
02082 }
02083 }
02084
02085 float Util::FakeKaiserBessel::sinhwin(float x) const {
02086 float val0 = sinh(fac)/fac;
02087 float absx = fabs(x);
02088 if (0.0 == x) {
02089 float res = 1.0f;
02090 return res;
02091 } else if (absx == alphar) {
02092 return 1.0f/val0;
02093 } else if (absx < alphar) {
02094 float rt = sqrt(1.0f - pow((x/alphar), 2));
02095 float facrt = fac*rt;
02096 float res = (sinh(facrt)/facrt)/val0;
02097 return res;
02098 } else {
02099 float rt = sqrt(pow((x/alphar),2) - 1.f);
02100 float facrt = fac*rt;
02101 float res = (sin(facrt)/facrt)/val0;
02102 return res;
02103 }
02104 }
02105
02106 #if 0 // 1-st order KB window
02107 float Util::FakeKaiserBessel::sinhwin(float x) const {
02108
02109 float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02110 float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02111 float absx = fabs(x);
02112 if (0.0 == x) {
02113
02114 float res = val0;
02115 return res;
02116 } else if (absx == alphar) {
02117
02118 return prefix;
02119 } else if (absx < alphar) {
02120 float rt = sqrt(1.0f - pow((x/alphar), 2));
02121
02122 float facrt = facadj*rt;
02123
02124 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02125 return res;
02126 } else {
02127 float rt = sqrt(pow((x/alphar),2) - 1.f);
02128
02129 float facrt = facadj*rt;
02130
02131 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02132 return res;
02133 }
02134 }
02135 #endif // 0
02136
02137
02138
02139 #define circ(i) circ[i-1]
02140 #define numr(i,j) numr[(j-1)*3 + i-1]
02141 #define xim(i,j) xim[(j-1)*nsam + i-1]
02142
02143 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02144 int nsam = image->get_xsize();
02145 int nrow = image->get_ysize();
02146 int nring = numr.size()/3;
02147 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02148 EMData* out = new EMData();
02149 out->set_size(lcirc,1,1);
02150 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02151 float *xim = image->get_data();
02152 float *circ = out->get_data();
02153
02154
02155
02156
02157
02158
02159
02160
02161
02162
02163
02164
02165
02166
02167
02168
02169 double dfi, dpi;
02170 int ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02171 float yq, xold, yold, fi, x, y;
02172
02173 ns2 = nsam/2+1;
02174 nr2 = nrow/2+1;
02175 dpi = 2.0*atan(1.0);
02176
02177 for (i=1;i<=nring;i++) {
02178
02179 inr = numr(1,i);
02180 yq = static_cast<float>(inr);
02181 l = numr(3,i);
02182 if (mode == 'h' || mode == 'H') lt = l/2;
02183 else lt = l/4;
02184
02185 nsim = lt-1;
02186 dfi = dpi/(nsim+1);
02187 kcirc = numr(2,i);
02188 xold = 0.0f;
02189 yold = static_cast<float>(inr);
02190 circ(kcirc) = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02191 xold = static_cast<float>(inr);
02192 yold = 0.0f;
02193 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02194
02195 if (mode == 'f' || mode == 'F') {
02196 xold = 0.0f;
02197 yold = static_cast<float>(-inr);
02198 circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02199 xold = static_cast<float>(-inr);
02200 yold = 0.0f;
02201 circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02202 }
02203
02204 for (j=1;j<=nsim;j++) {
02205 fi = static_cast<float>(dfi*j);
02206 x = sin(fi)*yq;
02207 y = cos(fi)*yq;
02208 xold = x;
02209 yold = y;
02210 circ(j+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02211 xold = y;
02212 yold = -x;
02213 circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02214
02215 if (mode == 'f' || mode == 'F') {
02216 xold = -x;
02217 yold = -y;
02218 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02219 xold = -y;
02220 yold = x;
02221 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02222 }
02223 }
02224 }
02225 return out;
02226 }
02227
02228 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02229 int nsam = image->get_xsize();
02230 int nrow = image->get_ysize();
02231 int nring = numr.size()/3;
02232 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02233 EMData* out = new EMData();
02234 out->set_size(lcirc,1,1);
02235 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02236 float *xim = image->get_data();
02237 float *circ = out->get_data();
02238 double dpi, dfi;
02239 int it, jt, inr, l, nsim, kcirc, lt;
02240 float xold, yold, fi, x, y;
02241
02242
02243
02244 dpi = 2*atan(1.0);
02245 for (it=1; it<=nring; it++) {
02246
02247 inr = numr(1,it);
02248
02249
02250
02251
02252 l = numr(3,it);
02253 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02254 else lt = l / 4;
02255
02256 nsim = lt - 1;
02257 dfi = dpi / (nsim+1);
02258 kcirc = numr(2,it);
02259 xold = 0.0f+cns2;
02260 yold = inr+cnr2;
02261
02262 Assert( kcirc <= lcirc );
02263 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02264
02265 xold = inr+cns2;
02266 yold = 0.0f+cnr2;
02267 Assert( lt+kcirc <= lcirc );
02268 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02269
02270 if ( mode == 'f' || mode == 'F' ) {
02271 xold = 0.0f+cns2;
02272 yold = -inr+cnr2;
02273 Assert( lt+lt+kcirc <= lcirc );
02274 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02275
02276 xold = -inr+cns2;
02277 yold = 0.0f+cnr2;
02278 Assert(lt+lt+lt+kcirc <= lcirc );
02279 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02280 }
02281
02282 for (jt=1; jt<=nsim; jt++) {
02283 fi = static_cast<float>(dfi * jt);
02284 x = sin(fi) * inr;
02285 y = cos(fi) * inr;
02286
02287 xold = x+cns2;
02288 yold = y+cnr2;
02289
02290 Assert( jt+kcirc <= lcirc );
02291 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02292
02293 xold = y+cns2;
02294 yold = -x+cnr2;
02295
02296 Assert( jt+lt+kcirc <= lcirc );
02297 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02298
02299 if ( mode == 'f' || mode == 'F' ) {
02300 xold = -x+cns2;
02301 yold = -y+cnr2;
02302
02303 Assert( jt+lt+lt+kcirc <= lcirc );
02304 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02305
02306 xold = -y+cns2;
02307 yold = x+cnr2;
02308
02309 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02310 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02311 }
02312 }
02313 }
02314 return out;
02315 }
02316
02317 float Util::bilinear(float xold, float yold, int nsam, int, float* xim)
02318 {
02319
02320
02321
02322
02323 float bilinear;
02324 int ixold, iyold;
02325
02326
02327
02328
02329
02330
02331
02332
02333
02334
02335
02336
02337
02338
02339
02340
02341
02342
02343
02344
02345
02346
02347
02348 float xdif, ydif;
02349
02350 ixold = (int) xold;
02351 iyold = (int) yold;
02352 ydif = yold - iyold;
02353
02354
02355
02356
02357
02358 xdif = xold - ixold;
02359 bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02360 xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02361 ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02362
02363 return bilinear;
02364 }
02365
02366 void Util::alrl_ms(float *xim, int nsam, int nrow, float cns2, float cnr2,
02367 int *numr, float *circ, int , int nring, char mode) {
02368 double dpi, dfi;
02369 int it, jt, inr, l, nsim, kcirc, lt;
02370 float xold, yold, fi, x, y;
02371
02372
02373
02374
02375 dpi = 2*atan(1.0);
02376 for (it=1; it<=nring; it++) {
02377
02378 inr = numr(1,it);
02379
02380 l = numr(3,it);
02381 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02382 else lt = l / 4;
02383
02384 nsim = lt - 1;
02385 dfi = dpi / (nsim+1);
02386 kcirc = numr(2,it);
02387
02388
02389 xold = 0.0f+cns2;
02390 yold = inr+cnr2;
02391
02392 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02393
02394 xold = inr+cns2;
02395 yold = 0.0f+cnr2;
02396 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02397
02398 if ( mode == 'f' || mode == 'F' ) {
02399 xold = 0.0f+cns2;
02400 yold = -inr+cnr2;
02401 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02402
02403 xold = -inr+cns2;
02404 yold = 0.0f+cnr2;
02405 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02406 }
02407
02408 for (jt=1; jt<=nsim; jt++) {
02409 fi = static_cast<float>(dfi * jt);
02410 x = sin(fi) * inr;
02411 y = cos(fi) * inr;
02412
02413 xold = x+cns2;
02414 yold = y+cnr2;
02415 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02416
02417 xold = y+cns2;
02418 yold = -x+cnr2;
02419 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02420
02421 if ( mode == 'f' || mode == 'F' ) {
02422 xold = -x+cns2;
02423 yold = -y+cnr2;
02424 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02425
02426 xold = -y+cns2;
02427 yold = x+cnr2;
02428 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
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
02508
02509 #undef xim
02510
02511 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02512
02513 int nring = numr.size()/3;
02514 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02515 EMData* out = new EMData();
02516 out->set_size(lcirc,1,1);
02517 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02518 float *circ = out->get_data();
02519 float *fimage = image->get_data();
02520 int nx = image->get_xsize();
02521 int ny = image->get_ysize();
02522 int nz = image->get_zsize();
02523 double dpi, dfi;
02524 int it, jt, inr, l, nsim, kcirc, lt;
02525 float yq, xold, yold, fi, x, y;
02526
02527
02528
02529
02530 dpi = 2*atan(1.0);
02531 for (it=1;it<=nring;it++) {
02532
02533 inr = numr(1,it);
02534 yq = static_cast<float>(inr);
02535
02536 l = numr(3,it);
02537 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02538 else lt = l / 4;
02539
02540 nsim = lt - 1;
02541 dfi = dpi / (nsim+1);
02542 kcirc = numr(2,it);
02543 xold = 0.0f;
02544 yold = static_cast<float>(inr);
02545 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02546
02547
02548 xold = static_cast<float>(inr);
02549 yold = 0.0f;
02550 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02551
02552
02553 if ( mode == 'f' || mode == 'F' ) {
02554 xold = 0.0f;
02555 yold = static_cast<float>(-inr);
02556 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02557
02558
02559 xold = static_cast<float>(-inr);
02560 yold = 0.0f;
02561 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);
02562
02563 }
02564
02565 for (jt=1;jt<=nsim;jt++) {
02566 fi = static_cast<float>(dfi * jt);
02567 x = sin(fi) * yq;
02568 y = cos(fi) * yq;
02569
02570 xold = x;
02571 yold = y;
02572 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02573
02574
02575 xold = y;
02576 yold = -x;
02577 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02578
02579
02580 if ( mode == 'f' || mode == 'F' ) {
02581 xold = -x;
02582 yold = -y;
02583 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);
02584
02585
02586 xold = -y;
02587 yold = x;
02588 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);
02589
02590 }
02591 }
02592 }
02593 return out;
02594 }
02595
02596
02597
02598
02599
02600
02601
02602
02603
02604
02605
02606
02607
02608
02609
02610
02611
02612
02613
02614
02615
02616
02617
02618
02619 #define tab1(i) tab1[i-1]
02620 #define xcmplx(i,j) xcmplx [(j-1)*2 + i-1]
02621 #define br(i) br[i-1]
02622 #define bi(i) bi[i-1]
02623
02624 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02625 {
02626 double rni,sgn,tr1,tr2,ti1,ti2;
02627 double cc,c,ss,s,t,x2,x3,x4,x5;
02628 int b3,b4,b5,b6,b7,b56;
02629 int n, k, l, j, i, ix0, ix1, status=0;
02630
02631 const double tab1[] = {
02632 9.58737990959775e-5,
02633 1.91747597310703e-4,
02634 3.83495187571395e-4,
02635 7.66990318742704e-4,
02636 1.53398018628476e-3,
02637 3.06795676296598e-3,
02638 6.13588464915449e-3,
02639 1.22715382857199e-2,
02640 2.45412285229123e-2,
02641 4.90676743274181e-2,
02642 9.80171403295604e-2,
02643 1.95090322016128e-1,
02644 3.82683432365090e-1,
02645 7.07106781186546e-1,
02646 1.00000000000000,
02647 };
02648
02649 n=(int)pow(2.0f,ln);
02650
02651 k=abs(ks);
02652 l=16-ln;
02653 b3=n*k;
02654 b6=b3;
02655 b7=k;
02656 if (ks > 0) {
02657 sgn=1.0f;
02658 } else {
02659 sgn=-1.0f;
02660 rni=1.0f/(float)(n);
02661 j=1;
02662 for (i=1; i<=n; i++) {
02663 br(j)=br(j)*rni;
02664 bi(j)=bi(j)*rni;
02665 j=j+k;
02666 }
02667 }
02668
02669 L12:
02670 b6=b6/2;
02671 b5=b6;
02672 b4=2*b6;
02673 b56=b5-b6;
02674
02675 L14:
02676 tr1=br(b5+1);
02677 ti1=bi(b5+1);
02678 tr2=br(b56+1);
02679 ti2=bi(b56+1);
02680
02681 br(b5+1)=tr2-tr1;
02682 bi(b5+1)=ti2-ti1;
02683 br(b56+1)=tr1+tr2;
02684 bi(b56+1)=ti1+ti2;
02685
02686 b5=b5+b4;
02687 b56=b5-b6;
02688 if ( b5 <= b3 ) goto L14;
02689 if ( b6 == b7 ) goto L20;
02690
02691 b4=b7;
02692 cc=2.0f*pow(tab1(l),2);
02693 c=1.0f-cc;
02694 l++;
02695 ss=sgn*tab1(l);
02696 s=ss;
02697
02698 L16:
02699 b5=b6+b4;
02700 b4=2*b6;
02701 b56=b5-b6;
02702
02703 L18:
02704 tr1=br(b5+1);
02705 ti1=bi(b5+1);
02706 tr2=br(b56+1);
02707 ti2=bi(b56+1);
02708 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02709 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02710 br(b56+1)=tr1+tr2;
02711 bi(b56+1)=ti1+ti2;
02712
02713 b5=b5+b4;
02714 b56=b5-b6;
02715 if ( b5 <= b3 ) goto L18;
02716 b4=b5-b6;
02717 b5=b4-b3;
02718 c=-c;
02719 b4=b6-b5;
02720 if ( b5 < b4 ) goto L16;
02721 b4=b4+b7;
02722 if ( b4 >= b5 ) goto L12;
02723
02724 t=c-cc*c-ss*s;
02725 s=s+ss*c-cc*s;
02726 c=t;
02727 goto L16;
02728
02729 L20:
02730 ix0=b3/2;
02731 b3=b3-b7;
02732 b4=0;
02733 b5=0;
02734 b6=ix0;
02735 ix1=0;
02736 if (b6 == b7) goto EXIT;
02737
02738 L22:
02739 b4=b3-b4;
02740 b5=b3-b5;
02741 x2=br(b4+1);
02742 x3=br(b5+1);
02743 x4=bi(b4+1);
02744 x5=bi(b5+1);
02745 br(b4+1)=x3;
02746 br(b5+1)=x2;
02747 bi(b4+1)=x5;
02748 bi(b5+1)=x4;
02749 if(b6 < b4) goto L22;
02750
02751 L24:
02752 b4=b4+b7;
02753 b5=b6+b5;
02754 x2=br(b4+1);
02755 x3=br(b5+1);
02756 x4=bi(b4+1);
02757 x5=bi(b5+1);
02758 br(b4+1)=x3;
02759 br(b5+1)=x2;
02760 bi(b4+1)=x5;
02761 bi(b5+1)=x4;
02762 ix0=b6;
02763
02764 L26:
02765 ix0=ix0/2;
02766 ix1=ix1-ix0;
02767 if( ix1 >= 0) goto L26;
02768
02769 ix0=2*ix0;
02770 b4=b4+b7;
02771 ix1=ix1+ix0;
02772 b5=ix1;
02773 if ( b5 >= b4) goto L22;
02774 if ( b4 < b6) goto L24;
02775
02776 EXIT:
02777 status = 0;
02778 }
02779
02780
02781 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02782 {
02783
02784
02785 int b3,b4,b5,b6,b7,b56;
02786 int n, k, l, j, i, ix0, ix1;
02787 float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02788 int status=0;
02789
02790 const float tab1[] = {
02791 9.58737990959775e-5f,
02792 1.91747597310703e-4f,
02793 3.83495187571395e-4f,
02794 7.66990318742704e-4f,
02795 1.53398018628476e-3f,
02796 3.06795676296598e-3f,
02797 6.13588464915449e-3f,
02798 1.22715382857199e-2f,
02799 2.45412285229123e-2f,
02800 4.90676743274181e-2f,
02801 9.80171403295604e-2f,
02802 1.95090322016128e-1f,
02803 3.82683432365090e-1f,
02804 7.07106781186546e-1f,
02805 1.00000000000000f,
02806 };
02807
02808 n=(int)pow(2.0f,ln);
02809
02810 k=abs(ks);
02811 l=16-ln;
02812 b3=n*k;
02813 b6=b3;
02814 b7=k;
02815 if( ks > 0 ) {
02816 sgn=1.0f;
02817 } else {
02818 sgn=-1.0f;
02819 rni=1.0f/(float)n;
02820 j=1;
02821 for (i=1; i<=n; i++) {
02822 br(j)=br(j)*rni;
02823 bi(j)=bi(j)*rni;
02824 j=j+k;
02825 }
02826 }
02827 L12:
02828 b6=b6/2;
02829 b5=b6;
02830 b4=2*b6;
02831 b56=b5-b6;
02832 L14:
02833 tr1=br(b5+1);
02834 ti1=bi(b5+1);
02835
02836 tr2=br(b56+1);
02837 ti2=bi(b56+1);
02838
02839 br(b5+1)=tr2-tr1;
02840 bi(b5+1)=ti2-ti1;
02841 br(b56+1)=tr1+tr2;
02842 bi(b56+1)=ti1+ti2;
02843
02844 b5=b5+b4;
02845 b56=b5-b6;
02846 if ( b5 <= b3 ) goto L14;
02847 if ( b6 == b7 ) goto L20;
02848
02849 b4=b7;
02850 cc=2.0f*pow(tab1(l),2);
02851 c=1.0f-cc;
02852 l++;
02853 ss=sgn*tab1(l);
02854 s=ss;
02855 L16:
02856 b5=b6+b4;
02857 b4=2*b6;
02858 b56=b5-b6;
02859 L18:
02860 tr1=br(b5+1);
02861 ti1=bi(b5+1);
02862 tr2=br(b56+1);
02863 ti2=bi(b56+1);
02864 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02865 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02866 br(b56+1)=tr1+tr2;
02867 bi(b56+1)=ti1+ti2;
02868
02869 b5=b5+b4;
02870 b56=b5-b6;
02871 if(b5 <= b3) goto L18;
02872 b4=b5-b6;
02873 b5=b4-b3;
02874 c=-c;
02875 b4=b6-b5;
02876 if(b5 < b4) goto L16;
02877 b4=b4+b7;
02878 if(b4 >= b5) goto L12;
02879
02880 t=c-cc*c-ss*s;
02881 s=s+ss*c-cc*s;
02882 c=t;
02883 goto L16;
02884 L20:
02885 ix0=b3/2;
02886 b3=b3-b7;
02887 b4=0;
02888 b5=0;
02889 b6=ix0;
02890 ix1=0;
02891 if ( b6 == b7) goto EXIT;
02892 L22:
02893 b4=b3-b4;
02894 b5=b3-b5;
02895 x2=br(b4+1);
02896 x3=br(b5+1);
02897 x4=bi(b4+1);
02898 x5=bi(b5+1);
02899 br(b4+1)=x3;
02900 br(b5+1)=x2;
02901 bi(b4+1)=x5;
02902 bi(b5+1)=x4;
02903 if (b6 < b4) goto L22;
02904 L24:
02905 b4=b4+b7;
02906 b5=b6+b5;
02907 x2=br(b4+1);
02908 x3=br(b5+1);
02909 x4=bi(b4+1);
02910 x5=bi(b5+1);
02911 br(b4+1)=x3;
02912 br(b5+1)=x2;
02913 bi(b4+1)=x5;
02914 bi(b5+1)=x4;
02915 ix0=b6;
02916 L26:
02917 ix0=ix0/2;
02918 ix1=ix1-ix0;
02919 if(ix1 >= 0) goto L26;
02920
02921 ix0=2*ix0;
02922 b4=b4+b7;
02923 ix1=ix1+ix0;
02924 b5=ix1;
02925 if (b5 >= b4) goto L22;
02926 if (b4 < b6) goto L24;
02927 EXIT:
02928 status = 0;
02929 }
02930
02931 void Util::fftr_q(float *xcmplx, int nv)
02932 {
02933
02934
02935 int nu, inv, nu1, n, isub, n2, i1, i2, i;
02936 float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02937
02938 const float tab1[] = {
02939 9.58737990959775e-5f,
02940 1.91747597310703e-4f,
02941 3.83495187571395e-4f,
02942 7.66990318742704e-4f,
02943 1.53398018628476e-3f,
02944 3.06795676296598e-3f,
02945 6.13588464915449e-3f,
02946 1.22715382857199e-2f,
02947 2.45412285229123e-2f,
02948 4.90676743274181e-2f,
02949 9.80171403295604e-2f,
02950 1.95090322016128e-1f,
02951 3.82683432365090e-1f,
02952 7.07106781186546e-1f,
02953 1.00000000000000f,
02954 };
02955
02956 nu=abs(nv);
02957 inv=nv/nu;
02958 nu1=nu-1;
02959 n=(int)pow(2.f,nu1);
02960 isub=16-nu1;
02961
02962 ss=-tab1(isub);
02963 cc=-2.0f*pow(tab1(isub-1),2.f);
02964 c=1.0f;
02965 s=0.0f;
02966 n2=n/2;
02967 if ( inv > 0) {
02968 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02969 tr=xcmplx(1,1);
02970 ti=xcmplx(2,1);
02971 xcmplx(1,1)=tr+ti;
02972 xcmplx(2,1)=tr-ti;
02973 for (i=1;i<=n2;i++) {
02974 i1=i+1;
02975 i2=n-i+1;
02976 tr1=xcmplx(1,i1);
02977 tr2=xcmplx(1,i2);
02978 ti1=xcmplx(2,i1);
02979 ti2=xcmplx(2,i2);
02980 t=(cc*c-ss*s)+c;
02981 s=(cc*s+ss*c)+s;
02982 c=t;
02983 xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02984 xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02985 xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02986 xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02987 }
02988 } else {
02989 tr=xcmplx(1,1);
02990 ti=xcmplx(2,1);
02991 xcmplx(1,1)=0.5f*(tr+ti);
02992 xcmplx(2,1)=0.5f*(tr-ti);
02993 for (i=1; i<=n2; i++) {
02994 i1=i+1;
02995 i2=n-i+1;
02996 tr1=xcmplx(1,i1);
02997 tr2=xcmplx(1,i2);
02998 ti1=xcmplx(2,i1);
02999 ti2=xcmplx(2,i2);
03000 t=(cc*c-ss*s)+c;
03001 s=(cc*s+ss*c)+s;
03002 c=t;
03003 xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03004 xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03005 xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03006 xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03007 }
03008 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03009 }
03010 }
03011
03012
03013 void Util::fftr_d(double *xcmplx, int nv)
03014 {
03015
03016 int i1, i2, nu, inv, nu1, n, isub, n2, i;
03017 double tr1,tr2,ti1,ti2,tr,ti;
03018 double cc,c,ss,s,t;
03019 const double tab1[] = {
03020 9.58737990959775e-5,
03021 1.91747597310703e-4,
03022 3.83495187571395e-4,
03023 7.66990318742704e-4,
03024 1.53398018628476e-3,
03025 3.06795676296598e-3,
03026 6.13588464915449e-3,
03027 1.22715382857199e-2,
03028 2.45412285229123e-2,
03029 4.90676743274181e-2,
03030 9.80171403295604e-2,
03031 1.95090322016128e-1,
03032 3.82683432365090e-1,
03033 7.07106781186546e-1,
03034 1.00000000000000,
03035 };
03036
03037 nu=abs(nv);
03038 inv=nv/nu;
03039 nu1=nu-1;
03040 n=(int)pow(2.0f,nu1);
03041 isub=16-nu1;
03042 ss=-tab1(isub);
03043 cc=-2.0*pow(tab1(isub-1),2);
03044 c=1.0f;
03045 s=0.0f;
03046 n2=n/2;
03047
03048 if ( inv > 0 ) {
03049 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03050 tr=xcmplx(1,1);
03051 ti=xcmplx(2,1);
03052 xcmplx(1,1)=tr+ti;
03053 xcmplx(2,1)=tr-ti;
03054 for (i=1;i<=n2;i++) {
03055 i1=i+1;
03056 i2=n-i+1;
03057 tr1=xcmplx(1,i1);
03058 tr2=xcmplx(1,i2);
03059 ti1=xcmplx(2,i1);
03060 ti2=xcmplx(2,i2);
03061 t=(cc*c-ss*s)+c;
03062 s=(cc*s+ss*c)+s;
03063 c=t;
03064 xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03065 xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03066 xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03067 xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03068 }
03069 } else {
03070 tr=xcmplx(1,1);
03071 ti=xcmplx(2,1);
03072 xcmplx(1,1)=0.5*(tr+ti);
03073 xcmplx(2,1)=0.5*(tr-ti);
03074 for (i=1; i<=n2; i++) {
03075 i1=i+1;
03076 i2=n-i+1;
03077 tr1=xcmplx(1,i1);
03078 tr2=xcmplx(1,i2);
03079 ti1=xcmplx(2,i1);
03080 ti2=xcmplx(2,i2);
03081 t=(cc*c-ss*s)+c;
03082 s=(cc*s+ss*c)+s;
03083 c=t;
03084 xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03085 xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03086 xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03087 xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03088 }
03089 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03090 }
03091 }
03092 #undef tab1
03093 #undef xcmplx
03094 #undef br
03095 #undef bi
03096
03097
03098 void Util::Frngs(EMData* circp, vector<int> numr){
03099 int nring = numr.size()/3;
03100 float *circ = circp->get_data();
03101 int i, l;
03102 for (i=1; i<=nring;i++) {
03103
03104 #ifdef _WIN32
03105 l = (int)( log((float)numr(3,i))/log(2.0f) );
03106 #else
03107 l=(int)(log2(numr(3,i)));
03108 #endif //_WIN32
03109
03110 fftr_q(&circ(numr(2,i)),l);
03111 }
03112 }
03113
03114 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03115 int nring = numr.size()/3;
03116 float *circ = circp->get_data();
03117 int i, l;
03118 for (i=1; i<=nring;i++) {
03119
03120 #ifdef _WIN32
03121 l = (int)( log((float)numr(3,i))/log(2.0f) );
03122 #else
03123 l=(int)(log2(numr(3,i)));
03124 #endif //_WIN32
03125
03126 fftr_q(&circ(numr(2,i)),-l);
03127 }
03128 }
03129 #undef circ
03130
03131 void Util::Applyws(EMData* circp, vector<int> numr, vector<float> wr)
03132 {
03133
03134
03135 const int nring = numr.size() / 3;
03136 const int maxrin = numr.back();
03137 float *circ = circp->get_data();
03138 for (int i = 0; i < nring; ++i) {
03139 const int numr3i = numr[2+i*3];
03140 const int numr2i = numr[1+i*3]-1;
03141 const float w = wr[i];
03142 circ[numr2i] *= w;
03143 if (numr3i == maxrin) circ[numr2i+1] *= w;
03144 else circ[numr2i+1] *= 0.5*w;
03145 for (int j = 2+numr2i; j < numr3i+numr2i; ++j) circ[j] *= w;
03146 }
03147 }
03148
03149 #define b(i) b[i-1]
03150 void Util::prb1d(double *b, int npoint, float *pos) {
03151 double c2,c3;
03152 int nhalf;
03153
03154 nhalf = npoint/2 + 1;
03155 *pos = 0.0;
03156
03157 if (npoint == 7) {
03158 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03159 - 6.*b(6) + 31.*b(7);
03160 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03161 }
03162 else if (npoint == 5) {
03163 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03164 + 46.*b(5) ) / (-70.);
03165 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03166 }
03167 else if (npoint == 3) {
03168 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03169 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03170 }
03171
03172 else {
03173 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03174 - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03175 + 1092.*b(9) ) / (-4620.);
03176 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03177 - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03178 }
03179 if (c3 != 0.0) *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03180 }
03181 #undef b
03182
03183 #define circ1(i) circ1[i-1]
03184 #define circ2(i) circ2[i-1]
03185 #define t(i) t[i-1]
03186 #define q(i) q[i-1]
03187 #define b(i) b[i-1]
03188 #define t7(i) t7[i-1]
03189 Dict Util::Crosrng_e(EMData* circ1p, EMData* circ2p, vector<int> numr, int neg) {
03190
03191 int nring = numr.size()/3;
03192
03193 int maxrin = numr[numr.size()-1];
03194 double qn; float tot;
03195 float *circ1 = circ1p->get_data();
03196 float *circ2 = circ2p->get_data();
03197
03198
03199
03200
03201
03202
03203
03204
03205
03206
03207
03208 float *t;
03209 double t7[7], *q;
03210 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03211 float pos;
03212
03213 #ifdef _WIN32
03214 ip = -(int)(log((float)maxrin)/log(2.0f));
03215 #else
03216 ip = -(int) (log2(maxrin));
03217 #endif //_WIN32
03218
03219 q = (double*)calloc(maxrin, sizeof(double));
03220 t = (float*)calloc(maxrin, sizeof(float));
03221
03222
03223 for (i=1; i<=nring; i++) {
03224 numr3i = numr(3,i);
03225 numr2i = numr(2,i);
03226
03227 t(1) = (circ1(numr2i)) * circ2(numr2i);
03228
03229 if (numr3i != maxrin) {
03230
03231 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03232 t(2) = 0.0;
03233
03234 if (neg) {
03235
03236 for (j=3;j<=numr3i;j=j+2) {
03237 jc = j+numr2i-1;
03238 t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03239 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03240 }
03241 } else {
03242 for (j=3;j<=numr3i;j=j+2) {
03243 jc = j+numr2i-1;
03244 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03245 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03246 }
03247 }
03248 for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03249 } else {
03250 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03251 if (neg) {
03252
03253 for (j=3;j<=maxrin;j=j+2) {
03254 jc = j+numr2i-1;
03255 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03256 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03257 }
03258 } else {
03259 for (j=3;j<=maxrin;j=j+2) {
03260 jc = j+numr2i-1;
03261 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03262 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03263 }
03264 }
03265 for (j = 1; j <= maxrin; j++) q(j) += t(j);
03266 }
03267 }
03268
03269 fftr_d(q,ip);
03270
03271 qn = -1.0e20;
03272 for (j=1;j<=maxrin;j++) {
03273 if (q(j) >= qn) {
03274 qn = q(j); jtot = j;
03275 }
03276 }
03277
03278 for (k=-3; k<=3; k++) {
03279 j = (jtot+k+maxrin-1)%maxrin + 1;
03280 t7(k+4) = q(j);
03281 }
03282
03283 prb1d(t7,7,&pos);
03284
03285 tot = (float)jtot + pos;
03286
03287 if (q) free(q);
03288 if (t) free(t);
03289
03290 Dict retvals;
03291 retvals["qn"] = qn;
03292 retvals["tot"] = tot;
03293 return retvals;
03294 }
03295
03296 Dict Util::Crosrng_ew(EMData* circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03297
03298 int nring = numr.size()/3;
03299
03300 int maxrin = numr[numr.size()-1];
03301 double qn; float tot;
03302 float *circ1 = circ1p->get_data();
03303 float *circ2 = circ2p->get_data();
03304
03305
03306
03307
03308
03309
03310
03311
03312
03313
03314
03315 float *t;
03316 double t7[7], *q;
03317 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03318 float pos;
03319
03320 #ifdef _WIN32
03321 ip = -(int)(log((float)maxrin)/log(2.0f));
03322 #else
03323 ip = -(int) (log2(maxrin));
03324 #endif //_WIN32
03325
03326 q = (double*)calloc(maxrin, sizeof(double));
03327 t = (float*)calloc(maxrin, sizeof(float));
03328
03329
03330 for (i=1;i<=nring;i++) {
03331 numr3i = numr(3,i);
03332 numr2i = numr(2,i);
03333
03334 t(1) = circ1(numr2i) * circ2(numr2i);
03335
03336 if (numr3i != maxrin) {
03337
03338 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03339 t(2) = 0.0;
03340
03341 if (neg) {
03342
03343 for (j=3; j<=numr3i; j=j+2) {
03344 jc = j+numr2i-1;
03345 t(j) = (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03346 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03347 }
03348 } else {
03349 for (j=3; j<=numr3i; j=j+2) {
03350 jc = j+numr2i-1;
03351 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03352 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03353 }
03354 }
03355 for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03356 } else {
03357 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03358 if (neg) {
03359
03360 for (j=3; j<=maxrin; j=j+2) {
03361 jc = j+numr2i-1;
03362 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03363 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03364 }
03365 } else {
03366 for (j=3; j<=maxrin; j=j+2) {
03367 jc = j+numr2i-1;
03368 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03369 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03370 }
03371 }
03372 for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03373 }
03374 }
03375
03376 fftr_d(q,ip);
03377
03378 qn = -1.0e20;
03379 for (j=1;j<=maxrin;j++) {
03380
03381 if (q(j) >= qn) {
03382 qn = q(j);
03383 jtot = j;
03384 }
03385 }
03386
03387 for (k=-3; k<=3; k++) {
03388 j = (jtot+k+maxrin-1)%maxrin + 1;
03389 t7(k+4) = q(j);
03390 }
03391
03392 prb1d(t7,7,&pos);
03393
03394 tot = (float)jtot + pos;
03395
03396
03397 if (t) free(t);
03398
03399 Dict retvals;
03400
03401
03402 retvals["qn"] = qn;
03403 retvals["tot"] = tot;
03404
03405 if (q) free(q);
03406
03407 return retvals;
03408 }
03409
03410 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03411 int nring = numr.size()/3;
03412
03413 int maxrin = numr[numr.size()-1];
03414 double qn; float tot; double qm; float tmt;
03415 float *circ1 = circ1p->get_data();
03416 float *circ2 = circ2p->get_data();
03417
03418
03419
03420
03421
03422
03423
03424
03425
03426
03427
03428
03429 double *t, *q, t7[7];
03430
03431 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03432 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03433
03434 qn = 0.0f;
03435 qm = 0.0f;
03436 tot = 0.0f;
03437 tmt = 0.0f;
03438 #ifdef _WIN32
03439 ip = -(int)(log((float)maxrin)/log(2.0f));
03440 #else
03441 ip = -(int)(log2(maxrin));
03442 #endif //_WIN32
03443
03444
03445
03446
03447
03448 q = (double*)calloc(maxrin,sizeof(double));
03449
03450
03451
03452 t = (double*)calloc(maxrin,sizeof(double));
03453
03454
03455 for (i=1; i<=nring; i++) {
03456
03457 numr3i = numr(3,i);
03458 numr2i = numr(2,i);
03459
03460 t1 = circ1(numr2i) * circ2(numr2i);
03461 q(1) += t1;
03462 t(1) += t1;
03463
03464 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03465 if (numr3i == maxrin) {
03466 q(2) += t1;
03467 t(2) += t1;
03468 } else {
03469 q(numr3i+1) += t1;
03470 t(numr3i+1) += t1;
03471 }
03472
03473 for (j=3; j<=numr3i; j += 2) {
03474 jc = j+numr2i-1;
03475
03476
03477
03478
03479
03480
03481
03482
03483 c1 = circ1(jc);
03484 c2 = circ1(jc+1);
03485 d1 = circ2(jc);
03486 d2 = circ2(jc+1);
03487
03488 t1 = c1 * d1;
03489 t2 = c2 * d2;
03490 t3 = c1 * d2;
03491 t4 = c2 * d1;
03492
03493 q(j) += t1 + t2;
03494 q(j+1) += -t3 + t4;
03495 t(j) += t1 - t2;
03496 t(j+1) += -t3 - t4;
03497 }
03498 }
03499
03500 fftr_d(q,ip);
03501
03502 qn = -1.0e20;
03503 for (j=1; j<=maxrin; j++) {
03504 if (q(j) >= qn) {
03505 qn = q(j);
03506 jtot = j;
03507 }
03508 }
03509
03510 for (k=-3; k<=3; k++) {
03511 j = ((jtot+k+maxrin-1)%maxrin)+1;
03512 t7(k+4) = q(j);
03513 }
03514
03515
03516 prb1d(t7,7,&pos);
03517 tot = (float)(jtot)+pos;
03518
03519
03520
03521
03522 fftr_d(t,ip);
03523
03524
03525 qm = -1.0e20;
03526 for (j=1; j<=maxrin;j++) {
03527 if ( t(j) >= qm ) {
03528 qm = t(j);
03529 jtot = j;
03530 }
03531 }
03532
03533 for (k=-3; k<=3; k++) {
03534 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03535 t7(k+4) = t(j);
03536 }
03537
03538
03539
03540 prb1d(t7,7,&pos);
03541 tmt = float(jtot) + pos;
03542
03543
03544
03545 free(t);
03546 free(q);
03547
03548 Dict retvals;
03549 retvals["qn"] = qn;
03550 retvals["tot"] = tot;
03551 retvals["qm"] = qm;
03552 retvals["tmt"] = tmt;
03553 return retvals;
03554 }
03555
03556 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03557 int nring = numr.size()/3;
03558
03559 int maxrin = numr[numr.size()-1];
03560 double qn; float tot; double qm; float tmt;
03561 float *circ1 = circ1p->get_data();
03562 float *circ2 = circ2p->get_data();
03563
03564
03565
03566
03567
03568
03569
03570
03571
03572
03573
03574
03575 double *t, *q;
03576
03577 int ip, jc, numr3i, numr2i, i, j, jtot = 0;
03578 float t1, t2, t3, t4, c1, c2, d1, d2;
03579
03580 qn = 0.0f;
03581 qm = 0.0f;
03582 tot = 0.0f;
03583 tmt = 0.0f;
03584 #ifdef _WIN32
03585 ip = -(int)(log((float)maxrin)/log(2.0f));
03586 #else
03587 ip = -(int)(log2(maxrin));
03588 #endif //_WIN32
03589
03590
03591
03592
03593
03594 q = (double*)calloc(maxrin,sizeof(double));
03595
03596
03597
03598 t = (double*)calloc(maxrin,sizeof(double));
03599
03600
03601 for (i=1; i<=nring; i++) {
03602
03603 numr3i = numr(3,i);
03604 numr2i = numr(2,i);
03605
03606 t1 = circ1(numr2i) * circ2(numr2i);
03607 q(1) += t1;
03608 t(1) += t1;
03609
03610 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03611 if (numr3i == maxrin) {
03612 q(2) += t1;
03613 t(2) += t1;
03614 } else {
03615 q(numr3i+1) += t1;
03616 t(numr3i+1) += t1;
03617 }
03618
03619 for (j=3; j<=numr3i; j += 2) {
03620 jc = j+numr2i-1;
03621
03622
03623
03624
03625
03626
03627
03628
03629 c1 = circ1(jc);
03630 c2 = circ1(jc+1);
03631 d1 = circ2(jc);
03632 d2 = circ2(jc+1);
03633
03634 t1 = c1 * d1;
03635 t2 = c2 * d2;
03636 t3 = c1 * d2;
03637 t4 = c2 * d1;
03638
03639 q(j) += t1 + t2;
03640 q(j+1) += -t3 + t4;
03641 t(j) += t1 - t2;
03642 t(j+1) += -t3 - t4;
03643 }
03644 }
03645
03646 fftr_d(q,ip);
03647
03648 qn = -1.0e20;
03649
03650 int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03651 int jstep = static_cast<int>(delta/360.0*maxrin);
03652 if (jstep < 1) { jstep = 1; }
03653
03654 for (j=jstart; j<=maxrin; j+=jstep) {
03655 if (q(j) >= qn) {
03656 qn = q(j);
03657 jtot = j;
03658 }
03659 }
03660
03661
03662
03663
03664
03665
03666
03667
03668
03669
03670 tot = (float)(jtot);
03671
03672
03673 fftr_d(t,ip);
03674
03675
03676 qm = -1.0e20;
03677 for (j=jstart; j<=maxrin;j+=jstep) {
03678 if ( t(j) >= qm ) {
03679 qm = t(j);
03680 jtot = j;
03681 }
03682 }
03683
03684
03685
03686
03687
03688
03689
03690
03691
03692
03693
03694 tmt = float(jtot);
03695
03696 free(t);
03697 free(q);
03698
03699 Dict retvals;
03700 retvals["qn"] = qn;
03701 retvals["tot"] = tot;
03702 retvals["qm"] = qm;
03703 retvals["tmt"] = tmt;
03704 return retvals;
03705 }
03706
03707
03708 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag, float psi_max) {
03709
03710
03711 int nring = numr.size()/3;
03712 int maxrin = numr[numr.size()-1];
03713 double qn; float tot;
03714 float *circ1 = circ1p->get_data();
03715 float *circ2 = circ2p->get_data();
03716
03717 double *q;
03718
03719 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03720 float c1, c2, d1, d2;
03721
03722 qn = 0.0f;
03723 tot = 0.0f;
03724 #ifdef _WIN32
03725 ip = -(int)(log((float)maxrin)/log(2.0f));
03726 #else
03727 ip = -(int)(log2(maxrin));
03728 #endif //_WIN32
03729
03730
03731
03732
03733 q = (double*)calloc(maxrin,sizeof(double));
03734 int neg = 1-2*flag;
03735
03736
03737 for (i=1; i<=nring; i++) {
03738
03739 numr3i = numr(3,i);
03740 numr2i = numr(2,i);
03741
03742 q(1) += circ1(numr2i) * circ2(numr2i);
03743
03744 float t1 = circ1(numr2i+1) * circ2(numr2i+1);
03745 if (numr3i == maxrin) q(2) += t1;
03746 else q(numr3i+1) += t1;
03747
03748 for (j=3; j<=numr3i; j += 2) {
03749 jc = j+numr2i-1;
03750
03751
03752
03753
03754
03755 c1 = circ1(jc);
03756 c2 = circ1(jc+1);
03757 d1 = circ2(jc);
03758 d2 = circ2(jc+1);
03759
03760 q(j) += c1 * d1 + c2 * d2*neg;
03761 q(j+1) += -c1 * d2 + c2 * d1*neg;
03762 }
03763 }
03764
03765 fftr_d(q,ip);
03766
03767 qn = -1.0e20;
03768
03769 int psi_pos = int(psi/360.0*maxrin+0.5) + 1;
03770 const int psi_range = int(psi_max/360.0*maxrin + 0.5);
03771
03772 for (k=-psi_range; k<=psi_range; k++) {
03773 j = ( k + psi_pos + maxrin - 1)%maxrin+1;
03774 if (q(j) >= qn) {
03775 qn = q(j);
03776 jtot = j;
03777 }
03778 }
03779
03780 tot = (float)(jtot);
03781 free(q);
03782
03783 Dict retvals;
03784 retvals["qn"] = qn;
03785 retvals["tot"] = tot;
03786 return retvals;
03787 }
03788
03789 Dict Util::Crosrng_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, float psi_max) {
03790
03791
03792 int nring = numr.size()/3;
03793 int maxrin = numr[numr.size()-1];
03794 double qn; float tot; double qm; float tmt;
03795 float *circ1 = circ1p->get_data();
03796 float *circ2 = circ2p->get_data();
03797
03798 double *t, *q;
03799
03800 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03801 float t1, t2, t3, t4, c1, c2, d1, d2;
03802
03803 qn = 0.0f;
03804 qm = 0.0f;
03805 tot = 0.0f;
03806 tmt = 0.0f;
03807 #ifdef _WIN32
03808 ip = -(int)(log((float)maxrin)/log(2.0f));
03809 #else
03810 ip = -(int)(log2(maxrin));
03811 #endif //_WIN32
03812
03813
03814
03815
03816 q = (double*)calloc(maxrin,sizeof(double));
03817
03818
03819
03820 t = (double*)calloc(maxrin,sizeof(double));
03821
03822
03823
03824 for (i=1; i<=nring; i++) {
03825
03826 numr3i = numr(3,i);
03827 numr2i = numr(2,i);
03828
03829 t1 = circ1(numr2i) * circ2(numr2i);
03830 q(1) += t1;
03831 t(1) += t1;
03832
03833 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03834 if (numr3i == maxrin) {
03835 q(2) += t1;
03836 t(2) += t1;
03837 } else {
03838 q(numr3i+1) += t1;
03839 t(numr3i+1) += t1;
03840 }
03841
03842 for (j=3; j<=numr3i; j += 2) {
03843 jc = j+numr2i-1;
03844
03845
03846
03847
03848
03849 c1 = circ1(jc);
03850 c2 = circ1(jc+1);
03851 d1 = circ2(jc);
03852 d2 = circ2(jc+1);
03853
03854 t1 = c1 * d1;
03855 t3 = c1 * d2;
03856 t2 = c2 * d2;
03857 t4 = c2 * d1;
03858
03859 q(j) += t1 + t2;
03860 q(j+1) += -t3 + t4;
03861 t(j) += t1 - t2;
03862 t(j+1) += -t3 - t4;
03863 }
03864 }
03865
03866 fftr_d(q,ip);
03867
03868 qn = -1.0e20;
03869
03870 int psi_pos = int(psi/360.0*maxrin+0.5) + 1;
03871 const int psi_range = int(psi_max/360.0*maxrin + 0.5);
03872
03873 for (k=-psi_range; k<=psi_range; k++) {
03874 j = (k+psi_pos+maxrin-1)%maxrin+1;
03875 if (q(j) >= qn) {
03876 qn = q(j);
03877 jtot = j;
03878 }
03879 }
03880
03881 tot = (float)(jtot);
03882 free(q);
03883
03884
03885 fftr_d(t,ip);
03886
03887 qm = -1.0e20;
03888
03889 for (k=-psi_range; k<=psi_range; k++) {
03890 j = (k+psi_pos+maxrin-1)%maxrin+1;
03891 if (t(j) >= qm) {
03892 qm = t(j);
03893 jtot = j;
03894 }
03895 }
03896
03897 tmt = (float)(jtot);
03898 free(t);
03899
03900 Dict retvals;
03901 retvals["qn"] = qn;
03902 retvals["tot"] = tot;
03903 retvals["qm"] = qm;
03904 retvals["tmt"] = tmt;
03905 return retvals;
03906 }
03907
03908 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03909 int nring = numr.size()/3;
03910
03911 int maxrin = numr[numr.size()-1];
03912 double qn; float tot;
03913 float *circ1 = circ1p->get_data();
03914 float *circ2 = circ2p->get_data();
03915
03916
03917
03918
03919
03920
03921
03922
03923
03924
03925
03926
03927 double *q, t7[7];
03928
03929 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03930 float c1, c2, d1, d2, pos;
03931
03932 qn = 0.0;
03933 tot = 0.0;
03934 #ifdef _WIN32
03935 ip = -(int)(log((float)maxrin)/log(2.0f));
03936 #else
03937 ip = -(int)(log2(maxrin));
03938 #endif //_WIN32
03939
03940
03941
03942
03943
03944 q = (double*)calloc(maxrin,sizeof(double));
03945
03946
03947 for (i=1; i<=nring; i++) {
03948
03949 numr3i = numr(3,i);
03950 numr2i = numr(2,i);
03951
03952 q(1) += circ1(numr2i) * circ2(numr2i);
03953
03954 if (numr3i == maxrin) q(2) += circ1(numr2i+1) * circ2(numr2i+1);
03955 else q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
03956
03957 for (j=3; j<=numr3i; j += 2) {
03958 jc = j+numr2i-1;
03959
03960
03961
03962
03963
03964 c1 = circ1(jc);
03965 c2 = circ1(jc+1);
03966 d1 = circ2(jc);
03967 d2 = circ2(jc+1);
03968
03969 q(j) += c1 * d1 + c2 * d2;
03970 q(j+1) += -c1 * d2 + c2 * d1;
03971 }
03972 }
03973
03974 fftr_d(q,ip);
03975
03976 qn = -1.0e20;
03977 for (j=1; j<=maxrin; j++) {
03978 if (q(j) >= qn) {
03979 qn = q(j);
03980 jtot = j;
03981 }
03982 }
03983
03984 for (k=-3; k<=3; k++) {
03985 j = ((jtot+k+maxrin-1)%maxrin)+1;
03986 t7(k+4) = q(j);
03987 }
03988
03989
03990 prb1d(t7,7,&pos);
03991 tot = (float)(jtot)+pos;
03992
03993
03994
03995 free(q);
03996
03997 Dict retvals;
03998 retvals["qn"] = qn;
03999 retvals["tot"] = tot;
04000 return retvals;
04001 }
04002
04003 #define dout(i,j) dout[i+maxrin*j]
04004 #define circ1b(i) circ1b[i-1]
04005 #define circ2b(i) circ2b[i-1]
04006
04007 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04008
04009
04010
04011 int ip, jc, numr3i, numr2i, i, j;
04012 float t1, t2, t3, t4, c1, c2, d1, d2;
04013
04014 int nring = numr.size()/3;
04015
04016 int maxrin = numr[numr.size()-1];
04017
04018 float* circ1b = circ1->get_data();
04019 float* circ2b = circ2->get_data();
04020
04021
04022 double *t, *q;
04023
04024 q = (double*)calloc(maxrin,sizeof(double));
04025 t = (double*)calloc(maxrin,sizeof(double));
04026
04027 #ifdef _WIN32
04028 ip = -(int)(log((float)maxrin)/log(2.0f));
04029 #else
04030 ip = -(int)(log2(maxrin));
04031 #endif //_WIN32
04032
04033
04034
04035
04036
04037
04038
04039 for (i=1; i<=nring; i++) {
04040
04041 numr3i = numr(3,i);
04042 numr2i = numr(2,i);
04043
04044 t1 = circ1b(numr2i) * circ2b(numr2i);
04045 q(1) += t1;
04046 t(1) += t1;
04047
04048 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04049 if (numr3i == maxrin) {
04050 q(2) += t1;
04051 t(2) += t1;
04052 } else {
04053 q(numr3i+1) += t1;
04054 t(numr3i+1) += t1;
04055 }
04056
04057 for (j=3; j<=numr3i; j=j+2) {
04058 jc = j+numr2i-1;
04059
04060 c1 = circ1b(jc);
04061 c2 = circ1b(jc+1);
04062 d1 = circ2b(jc);
04063 d2 = circ2b(jc+1);
04064
04065 t1 = c1 * d1;
04066 t3 = c1 * d2;
04067 t2 = c2 * d2;
04068 t4 = c2 * d1;
04069
04070 q(j) += t1 + t2;
04071 q(j+1) += -t3 + t4;
04072 t(j) += t1 - t2;
04073 t(j+1) += -t3 - t4;
04074 }
04075 }
04076
04077
04078 fftr_d(q,ip);
04079
04080
04081 fftr_d(t,ip);
04082
04083 EMData* out = new EMData();
04084 out->set_size(maxrin,2,1);
04085 float *dout = out->get_data();
04086 for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04087
04088
04089
04090 free(t);
04091 free(q);
04092 return out;
04093 }
04094
04095
04096 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04097
04098 int maxrin = numr[numr.size()-1];
04099
04100 vector<float> r(2*maxrin);
04101
04102 Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04103
04104 return r;
04105 }
04106
04107 #define dout(i,j) dout[i+maxrin*j]
04108 #define circ1b(i) circ1b[i-1]
04109 #define circ2b(i) circ2b[i-1]
04110
04111 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04112
04113
04114
04115 int ip, jc, numr3i, numr2i, i, j;
04116 float t1, t2, t3, t4, c1, c2, d1, d2;
04117
04118 int nring = numr.size()/3;
04119
04120 int maxrin = numr[numr.size()-1];
04121
04122 float* circ1b = circ1->get_data();
04123 float* circ2b = circ2->get_data();
04124
04125 #ifdef _WIN32
04126 ip = -(int)(log((float)maxrin)/log(2.0f));
04127 #else
04128 ip = -(int)(log2(maxrin));
04129 #endif //_WIN32
04130 for (int i=1; i<=maxrin; i++) {q(i) = 0.0f; t(i) = 0.0f;}
04131
04132
04133
04134
04135
04136 for (i=1; i<=nring; i++) {
04137
04138 numr3i = numr(3,i);
04139 numr2i = numr(2,i);
04140
04141 t1 = circ1b(numr2i) * circ2b(numr2i);
04142 q(1) += t1;
04143 t(1) += t1;
04144
04145 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04146 if (numr3i == maxrin) {
04147 q(2) += t1;
04148 t(2) += t1;
04149 } else {
04150 q(numr3i+1) += t1;
04151 t(numr3i+1) += t1;
04152 }
04153
04154 for (j=3; j<=numr3i; j=j+2) {
04155 jc = j+numr2i-1;
04156
04157 c1 = circ1b(jc);
04158 c2 = circ1b(jc+1);
04159 d1 = circ2b(jc);
04160 d2 = circ2b(jc+1);
04161
04162 t1 = c1 * d1;
04163 t3 = c1 * d2;
04164 t2 = c2 * d2;
04165 t4 = c2 * d1;
04166
04167 q(j) += t1 + t2;
04168 q(j+1) += -t3 + t4;
04169 t(j) += t1 - t2;
04170 t(j+1) += -t3 - t4;
04171 }
04172 }
04173
04174 fftr_q(q,ip);
04175
04176
04177
04178 fftr_q(t,ip);
04179 }
04180
04181
04182
04183 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04184 {
04185
04186 int ip, jc, numr3i, numr2i, i, j;
04187 float c1, c2, d1, d2;
04188
04189 int nring = numr.size()/3;
04190 int maxrin = numr[numr.size()-1];
04191
04192 float* circ1b = circ1->get_data();
04193 float* circ2b = circ2->get_data();
04194
04195 double *q;
04196
04197 q = (double*)calloc(maxrin,sizeof(double));
04198
04199 #ifdef _WIN32
04200 ip = -(int)(log((float)maxrin)/log(2.0f));
04201 #else
04202 ip = -(int)(log2(maxrin));
04203 #endif //_WIN32
04204
04205
04206
04207 for (i=1;i<=nring;i++) {
04208
04209 numr3i = numr(3,i);
04210 numr2i = numr(2,i);
04211
04212 q(1) += circ1b(numr2i) * circ2b(numr2i);
04213
04214 if (numr3i == maxrin) q(2) += circ1b(numr2i+1) * circ2b(numr2i+1);
04215 else q(numr3i+1) += circ1b(numr2i+1) * circ2b(numr2i+1);
04216
04217 for (j=3;j<=numr3i;j=j+2) {
04218 jc = j+numr2i-1;
04219
04220 c1 = circ1b(jc);
04221 c2 = circ1b(jc+1);
04222 d1 = circ2b(jc);
04223 d2 = circ2b(jc+1);
04224
04225 q(j) += c1 * d1 + c2 * d2;
04226 q(j+1) += -c1 * d2 + c2 * d1;
04227 }
04228 }
04229
04230
04231 fftr_d(q,ip);
04232
04233 EMData* out = new EMData();
04234 out->set_size(maxrin,1,1);
04235 float *dout = out->get_data();
04236 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04237 free(q);
04238 return out;
04239
04240 }
04241
04242
04243 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04244 {
04245
04246 int ip, jc, numr3i, numr2i, i, j;
04247 float c1, c2, d1, d2;
04248
04249 int nring = numr.size()/3;
04250 int maxrin = numr[numr.size()-1];
04251
04252 float* circ1b = circ1->get_data();
04253 float* circ2b = circ2->get_data();
04254
04255 double *t;
04256
04257 t = (double*)calloc(maxrin,sizeof(double));
04258
04259 #ifdef _WIN32
04260 ip = -(int)(log((float)maxrin)/log(2.0f));
04261 #else
04262 ip = -(int)(log2(maxrin));
04263 #endif //_WIN32
04264
04265
04266
04267 for (i=1;i<=nring;i++) {
04268
04269 numr3i = numr(3,i);
04270 numr2i = numr(2,i);
04271 t(1) += circ1b(numr2i) * circ2b(numr2i);
04272
04273 if (numr3i == maxrin) t(2) += circ1b(numr2i+1) * circ2b(numr2i+1);
04274 else t(numr3i+1) += circ1b(numr2i+1) * circ2b(numr2i+1);
04275
04276 for (j=3;j<=numr3i;j=j+2) {
04277 jc = j+numr2i-1;
04278
04279 c1 = circ1b(jc);
04280 c2 = circ1b(jc+1);
04281 d1 = circ2b(jc);
04282 d2 = circ2b(jc+1);
04283
04284 t(j) += c1 * d1 - c2 * d2;
04285 t(j+1) += -c1 * d2 - c2 * d1;
04286 }
04287 }
04288
04289
04290 fftr_d(t,ip);
04291
04292 EMData* out = new EMData();
04293 out->set_size(maxrin,1,1);
04294 float *dout = out->get_data();
04295 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04296 free(t);
04297 return out;
04298
04299 }
04300
04301 #undef circ1b
04302 #undef circ2b
04303 #undef dout
04304
04305 #undef circ1
04306 #undef circ2
04307 #undef t
04308 #undef q
04309 #undef b
04310 #undef t7
04311
04312
04313 #define QUADPI 3.141592653589793238462643383279502884197
04314 #define PI2 2*QUADPI
04315
04316 float Util::ener(EMData* ave, vector<int> numr) {
04317 ENTERFUNC;
04318 long double ener,en;
04319
04320 int nring = numr.size()/3;
04321 float *aveptr = ave->get_data();
04322
04323 ener = 0.0;
04324 for (int i=1; i<=nring; i++) {
04325 int numr3i = numr(3,i);
04326 int np = numr(2,i)-1;
04327 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04328 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04329 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04330 ener += en/numr3i;
04331 }
04332 EXITFUNC;
04333 return static_cast<float>(ener);
04334 }
04335
04336 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04337 ENTERFUNC;
04338 long double ener, en;
04339 float arg, cs, si;
04340
04341 int nima = data.size();
04342 int nring = numr.size()/3;
04343 int maxrin = numr(3,nring);
04344
04345 ener = 0.0;
04346 for (int i=1; i<=nring; i++) {
04347 int numr3i = numr(3,i);
04348 int np = numr(2,i)-1;
04349 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04350 float temp1 = 0.0, temp2 = 0.0;
04351 for (int kk=0; kk<nima; kk++) {
04352 float *ptr = data[kk]->get_data();
04353 temp1 += ptr[np];
04354 temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04355 }
04356 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04357 for (int j=2; j<numr3i; j+=2) {
04358 float tempr = 0.0, tempi = 0.0;
04359 for (int kk=0; kk<nima; kk++) {
04360 float *ptr = data[kk]->get_data();
04361 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04362 cs = cos(arg);
04363 si = sin(arg);
04364 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04365 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04366 }
04367 en += tq*(tempr*tempr+tempi*tempi);
04368 }
04369 ener += en/numr3i;
04370 }
04371 EXITFUNC;
04372 return static_cast<float>(ener);
04373 }
04374
04375 void Util::update_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04376 int nring = numr.size()/3;
04377 float *ave = avep->get_data();
04378 float *dat = datp->get_data();
04379 int i, j, numr3i, np;
04380 float arg, cs, si;
04381 int maxrin = numr(3,nring);
04382 if(mirror == 1) {
04383 for (i=1; i<=nring; i++) {
04384 numr3i = numr(3,i);
04385 np = numr(2,i)-1;
04386 ave[np] += dat[np];
04387 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04388 for (j=2; j<numr3i; j=j+2) {
04389 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04390 cs = cos(arg);
04391 si = sin(arg);
04392
04393 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04394 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04395 }
04396 }
04397 } else {
04398 for (i=1; i<=nring; i++) {
04399 numr3i = numr(3,i);
04400 np = numr(2,i)-1;
04401 ave[np] += dat[np];
04402 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04403 for (j=2; j<numr3i; j=j+2) {
04404 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04405 cs = cos(arg);
04406 si = sin(arg);
04407
04408 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04409 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04410 }
04411 }
04412 }
04413 avep->update();
04414 EXITFUNC;
04415 }
04416
04417 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04418 int nring = numr.size()/3;
04419 float *ave = avep->get_data();
04420 float *dat = datp->get_data();
04421 int i, j, numr3i, np;
04422 float arg, cs, si;
04423 int maxrin = numr(3,nring);
04424 if(mirror == 1) {
04425 for (i=1; i<=nring; i++) {
04426 numr3i = numr(3,i);
04427 np = numr(2,i)-1;
04428 ave[np] -= dat[np];
04429 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04430 for (j=2; j<numr3i; j=j+2) {
04431 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04432 cs = cos(arg);
04433 si = sin(arg);
04434
04435 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04436 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04437 }
04438 }
04439 } else {
04440 for (i=1; i<=nring; i++) {
04441 numr3i = numr(3,i);
04442 np = numr(2,i)-1;
04443 ave[np] -= dat[np];
04444 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04445 for (j=2; j<numr3i; j=j+2) {
04446 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04447 cs = cos(arg);
04448 si = sin(arg);
04449
04450 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04451 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04452 }
04453 }
04454 }
04455 avep->update();
04456 EXITFUNC;
04457 }
04458
04459
04460 #undef QUADPI
04461 #undef PI2
04462
04463 #undef numr
04464 #undef circ
04465
04466
04467 #define QUADPI 3.141592653589793238462643383279502884197
04468 #define PI2 QUADPI*2
04469 #define deg_rad QUADPI/180.0
04470 #define rad_deg 180.0/QUADPI
04471
04472 struct ori_t
04473 {
04474 int iphi;
04475 int itht;
04476 int id;
04477 };
04478
04479
04480 struct cmpang
04481 {
04482 bool operator()( const ori_t& a, const ori_t& b )
04483 {
04484 if( a.itht != b.itht )
04485 {
04486 return a.itht < b.itht;
04487 }
04488
04489 return a.iphi < b.iphi;
04490 }
04491 };
04492
04493
04494 vector<double> Util::cml_weights(const vector<float>& cml){
04495 static const int NBIN = 100;
04496 int nline=cml.size()/2;
04497 vector<double> weights(nline);
04498
04499 vector<ori_t> angs(nline);
04500 for( int i=0; i < nline; ++i ) {
04501 angs[i].iphi = int( NBIN*cml[2*i] );
04502 angs[i].itht = int( NBIN*cml[2*i+1] );
04503 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04504 angs[i].id = i;
04505 }
04506
04507
04508
04509 std::sort( angs.begin(), angs.end(), cmpang() );
04510
04511 vector<float> newphi;
04512 vector<float> newtht;
04513 vector< vector<int> > indices;
04514
04515 int curt_iphi = -1;
04516 int curt_itht = -1;
04517 for(unsigned int i=0 ;i < angs.size(); ++i ) {
04518 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04519 Assert( indices.size() > 0 );
04520 indices.back().push_back(angs[i].id);
04521 } else {
04522 curt_iphi = angs[i].iphi;
04523 curt_itht = angs[i].itht;
04524
04525 newphi.push_back( float(curt_iphi)/NBIN );
04526 newtht.push_back( float(curt_itht)/NBIN );
04527 indices.push_back( vector<int>(1,angs[i].id) );
04528 }
04529 }
04530
04531
04532
04533
04534 int num_agl = newphi.size();
04535
04536 if(num_agl>2) {
04537 vector<double> w=Util::vrdg(newphi, newtht);
04538
04539 Assert( w.size()==newphi.size() );
04540 Assert( indices.size()==newphi.size() );
04541
04542 for(unsigned int i=0; i < newphi.size(); ++i ) {
04543
04544
04545
04546
04547
04548
04549
04550
04551 for(unsigned int j=0; j < indices[i].size(); ++j ) {
04552 int id = indices[i][j];
04553 weights[id] = w[i]/indices[i].size();
04554
04555 }
04556
04557
04558
04559 }
04560 } else {
04561 cout<<"warning in Util.cml_weights"<<endl;
04562 double val = PI2/float(nline);
04563 for(int i=0; i<nline; i++) weights[i]=val;
04564 }
04565
04566 return weights;
04567
04568 }
04569
04570
04571
04572
04573
04574 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04575 {
04576 int i;
04577 int nx=img->get_xsize();
04578 float *img_ptr = img->get_data();
04579 float *line_ptr = line->get_data();
04580 for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04581 img->update();
04582 }
04583
04584 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04585 int j;
04586 int nx = sino->get_xsize();
04587 int i = nx * pos_line;
04588 float r1, r2;
04589 float *line_ptr = line->get_data();
04590 float *sino_ptr = sino->get_data();
04591 for (j=ilf;j<=ihf; j += 2) {
04592 r1 = line_ptr[j];
04593 r2 = line_ptr[j + 1];
04594 sino_ptr[i + j - ilf] = r1;
04595 sino_ptr[i + j - ilf + 1] = r2;
04596 sino_ptr[i + nx * nblines + j - ilf] = r1;
04597 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04598 }
04599 sino->update();
04600 }
04601
04602 vector<double> Util::cml_init_rot(vector<float> Ori){
04603 int nb_ori = Ori.size() / 4;
04604 int i, ind;
04605 float ph, th, ps;
04606 double cph, cth, cps, sph, sth, sps;
04607 vector<double> Rot(nb_ori*9);
04608 for (i=0; i<nb_ori; ++i){
04609 ind = i*4;
04610
04611 ph = Ori[ind+2]-90;
04612 th = Ori[ind+1];
04613 ps = Ori[ind]+90;
04614 ph *= deg_rad;
04615 th *= deg_rad;
04616 ps *= deg_rad;
04617
04618 cph = cos(ph);
04619 cth = cos(th);
04620 cps = cos(ps);
04621 sph = sin(ph);
04622 sth = sin(th);
04623 sps = sin(ps);
04624
04625 ind = i*9;
04626 Rot[ind] = cph*cps-cth*sps*sph;
04627 Rot[ind+1] = cph*sps+cth*cps*sph;
04628 Rot[ind+2] = sth*sph;
04629 Rot[ind+3] = -sph*cps-cth*sps*cph;
04630 Rot[ind+4] = -sph*sps+cth*cps*cph;
04631 Rot[ind+5] = sth*cph;
04632 Rot[ind+6] = sth*sps;
04633 Rot[ind+7] = -sth*cps;
04634 Rot[ind+8] = cth;
04635 }
04636
04637 return Rot;
04638 }
04639
04640 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04641 float ph, ps;
04642 double cph, cth, cps, sph, sth, sps;
04643 int ind = iprj*9;
04644
04645 ph = nps-90;
04646 ps = nph+90;
04647 ph *= deg_rad;
04648 th *= deg_rad;
04649 ps *= deg_rad;
04650
04651 cph = cos(ph);
04652 cth = cos(th);
04653 cps = cos(ps);
04654 sph = sin(ph);
04655 sth = sin(th);
04656 sps = sin(ps);
04657
04658 Rot[ind] = (float)(cph*cps-cth*sps*sph);
04659 Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04660 Rot[ind+2] = (float)(sth*sph);
04661 Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04662 Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04663 Rot[ind+5] = (float)(sth*cph);
04664 Rot[ind+6] = (float)(sth*sps);
04665 Rot[ind+7] = (float)(-sth*cps);
04666 Rot[ind+8] = (float)(cth);
04667
04668 return Rot;
04669 }
04670
04671 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04672 vector<int> com(2*(n_prj - 1));
04673 int a = i_prj*9;
04674 int i, b, c;
04675 int n1=0, n2=0;
04676 float vmax = 1 - 1.0e-6f;
04677 double r11, r12, r13, r23, r31, r32, r33;
04678
04679 c = 0;
04680 for (i=0; i<n_prj; ++i){
04681 if (i!=i_prj){
04682 b = i*9;
04683
04684 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04685 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04686 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04687 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04688 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04689 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04690 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04691 if (r33 > vmax) {
04692 n2 = 270;
04693 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04694 }
04695 else if (r33 < -vmax) {
04696 n2 = 270;
04697 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04698 } else {
04699 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04700 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04701 if (n1 < 0) {n1 += 360;}
04702 if (n2 <= 0) {n2 = abs(n2);}
04703 else {n2 = 360 - n2;}
04704 }
04705
04706 if (n1 >= 360){n1 = n1 % 360;}
04707 if (n2 >= 360){n2 = n2 % 360;}
04708
04709
04710 b = c*2;
04711 com[b] = n1;
04712 com[b+1] = n2;
04713 ++c;
04714 }
04715 }
04716
04717 return com;
04718
04719 }
04720
04721 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int, int n_lines) {
04722 vector<int> com(2*n_lines);
04723 int a=0, b, c, l;
04724 int n1=0, n2=0, mem=-1;
04725 float vmax = 1 - 1.0e-6f;
04726 double r11, r12, r13, r23, r31, r32, r33;
04727 c = 0;
04728 for (l=0; l<n_lines; ++l){
04729 c = 2*l;
04730 if (seq[c]!=mem){
04731 mem = seq[c];
04732 a = seq[c]*9;
04733 }
04734 b = seq[c+1]*9;
04735
04736
04737 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04738 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04739 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04740 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04741 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04742 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04743 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04744 if (r33 > vmax) {
04745 n2 = 270;
04746 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04747 }
04748 else if (r33 < -vmax) {
04749 n2 = 270;
04750 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04751 } else {
04752 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04753 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04754 if (n1 < 0) {n1 += 360;}
04755 if (n2 <= 0) {n2 = abs(n2);}
04756 else {n2 = 360 - n2;}
04757 }
04758 if (n1 >= 360){n1 = n1 % 360;}
04759 if (n2 >= 360){n2 = n2 % 360;}
04760
04761
04762 com[c] = n1;
04763 com[c+1] = n2;
04764 }
04765
04766 return com;
04767
04768 }
04769
04770 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int, int nlines){
04771
04772 vector<double> cml(2*nlines);
04773 float ph1, th1;
04774 float ph2, th2;
04775 double nx, ny, nz;
04776 double norm;
04777 double sth1=0, sph1=0, cth1=0, cph1=0;
04778 double sth2, sph2, cth2, cph2;
04779 int l, ind, c;
04780 int mem = -1;
04781 for (l=0; l<nlines; ++l){
04782 c = 2*l;
04783 if (seq[c]!=mem){
04784 mem = seq[c];
04785 ind = 4*seq[c];
04786 ph1 = Ori[ind]*deg_rad;
04787 th1 = Ori[ind+1]*deg_rad;
04788 sth1 = sin(th1);
04789 sph1 = sin(ph1);
04790 cth1 = cos(th1);
04791 cph1 = cos(ph1);
04792 }
04793 ind = 4*seq[c+1];
04794 ph2 = Ori[ind]*deg_rad;
04795 th2 = Ori[ind+1]*deg_rad;
04796 sth2 = sin(th2);
04797 cth2 = cos(th2);
04798 sph2 = sin(ph2);
04799 cph2 = cos(ph2);
04800
04801 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04802 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04803 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04804 norm = sqrt(nx*nx+ny*ny+nz*nz);
04805 nx /= norm;
04806 ny /= norm;
04807 nz /= norm;
04808
04809 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04810
04811 cml[c+1] = acos(nz);
04812 if (cml[c+1] == 0) {cml[c] = 0;}
04813 else {
04814 cml[c+1] *= rad_deg;
04815 if (cml[c+1] > 89.99) {cml[c+1] = 89.99;}
04816 cml[c] = rad_deg * atan2(nx, ny);
04817 cml[c] = fmod(360 + cml[c], 360);
04818
04819 }
04820 }
04821
04822 return cml;
04823 }
04824
04825 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04826 double res = 0;
04827 double buf = 0;
04828 float* line_1;
04829 float* line_2;
04830 int i, n, ind;
04831 int lnlen = data[0]->get_xsize();
04832 for (n=0; n<n_lines; ++n) {
04833 ind = n*2;
04834 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04835 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04836 buf = 0;
04837 for (i=0; i<lnlen; ++i) {
04838 buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04839 }
04840 res += buf * weights[n];
04841 }
04842
04843 return res;
04844
04845 }
04846
04847 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04848 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04849
04850
04851
04852 vector<double> res(2);
04853 int lnlen = data[0]->get_xsize();
04854 int end = 2*(n_prj-1);
04855 double disc, buf, bdisc, tmp;
04856 int n, i, ipsi, ind, bipsi, c;
04857 float* line_1;
04858 float* line_2;
04859 bdisc = 1.0e6;
04860 bipsi = -1;
04861
04862 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04863
04864 disc = 0;
04865 c = 0;
04866 for (n=0; n<n_prj; ++n) {
04867 if(n!=iprj) {
04868 ind = 2*c;
04869 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04870 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04871 buf = 0;
04872 for (i=0; i<lnlen; ++i) {
04873 tmp = line_1[i]-line_2[i];
04874 buf += tmp*tmp;
04875 }
04876 disc += buf * weights[iw[c]];
04877 ++c;
04878 }
04879 }
04880
04881 if (disc <= bdisc) {
04882 bdisc = disc;
04883 bipsi = ipsi;
04884 }
04885
04886 for (i=0; i<end; i+=2){
04887 com[i] += d_psi;
04888 if (com[i] >= n_psi) com[i] = com[i] - n_psi;
04889 }
04890 }
04891 res[0] = bdisc;
04892 res[1] = float(bipsi);
04893
04894 return res;
04895 }
04896
04897 vector<double> Util::cml_spin_psi_now(const vector<EMData*>& data, vector<int> com, \
04898 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04899
04900
04901
04902 vector<double> res(2);
04903 int lnlen = data[0]->get_xsize();
04904 int end = 2*(n_prj-1);
04905 double disc, buf, bdisc, tmp;
04906 int n, i, ipsi, ind, bipsi, c;
04907 float* line_1;
04908 float* line_2;
04909 bdisc = 1.0e6;
04910 bipsi = -1;
04911
04912 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04913
04914 disc = 0;
04915 c = 0;
04916 for (n=0; n<n_prj; ++n) {
04917 if(n!=iprj) {
04918 ind = 2*c;
04919 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04920 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04921 buf = 0;
04922 for (i=0; i<lnlen; ++i) {
04923 tmp = line_1[i]-line_2[i];
04924 buf += tmp*tmp;
04925 }
04926 disc += buf;
04927 ++c;
04928 }
04929 }
04930
04931 if (disc <= bdisc) {
04932 bdisc = disc;
04933 bipsi = ipsi;
04934 }
04935
04936 for (i=0; i<end; i+=2){
04937 com[i] += d_psi;
04938 if (com[i] >= n_psi) com[i] = com[i] - n_psi;
04939 }
04940 }
04941 res[0] = bdisc;
04942 res[1] = float(bipsi);
04943
04944 return res;
04945 }
04946
04947 #undef QUADPI
04948 #undef PI2
04949 #undef deg_rad
04950 #undef rad_deg
04951
04952
04953
04954
04955
04956
04957 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
04958 ENTERFUNC;
04959
04960 int nima = data.size();
04961 vector<float> res(nima);
04962 double result = 0.;
04963 double valmin = 1.0e20;
04964 int valpos = -1;
04965
04966 for (int kk=0; kk<nima; kk++){
04967 result = 0;
04968
04969 float *y_data = data[kk]->get_data();
04970 float *x_data = image->get_data();
04971 long totsize = image->get_xsize()*image->get_ysize();
04972 for (long i = 0; i < totsize; i++) {
04973 double temp = x_data[i]- y_data[i];
04974 result += temp*temp;
04975 }
04976 result /= totsize;
04977 res[kk] = (float)result;
04978
04979 if(result<valmin) {valmin = result; valpos = kk;}
04980
04981 }
04982
04983 Dict retvals;
04984 retvals["dist"] = res;
04985 retvals["pos"] = valpos;
04986
04987 EXITFUNC;
04988 return retvals;
04989
04990 }
04991
04992 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
04993 ENTERFUNC;
04994
04995 int nima = data.size();
04996 vector<float> res(nima);
04997 double result = 0.;
04998 double valmin = 1.0e20;
04999 int valpos = -1;
05000
05001 for (int kk=0; kk<nima; kk++){
05002 result = 0;
05003
05004
05005 float *y_data = data[kk]->get_data();
05006 float *x_data = image->get_data();
05007
05008
05009 int nx = data[kk]->get_xsize();
05010 int ny = data[kk]->get_ysize();
05011 nx = (nx - 2 + data[kk]->is_fftodd());
05012 int lsd2 = (nx + 2 - nx%2) ;
05013
05014 int ixb = 2*((nx+1)%2);
05015 int iyb = ny%2;
05016 int iz = 0;
05017
05018 for ( int iy = 0; iy <= ny-1; iy++) {
05019 for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05020 int ii = ix + (iy + iz * ny)* lsd2;
05021 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05022 }
05023 }
05024 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05025 int ii = (iy + iz * ny)* lsd2;
05026 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05027 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05028 }
05029 if(nx%2 == 0) {
05030 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05031 int ii = lsd2 - 2 + (iy + iz * ny)* lsd2;
05032 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05033 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05034 }
05035
05036 }
05037 result *= 2;
05038 result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05039 if(ny%2 == 0) {
05040 int ii = (ny/2 + iz * ny)* lsd2;
05041 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05042 }
05043 if(nx%2 == 0) {
05044 int ii = lsd2 - 2 + (0 + iz * ny)* lsd2;
05045 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05046 if(ny%2 == 0) {
05047 int ii = lsd2 - 2 +(ny/2 + iz * ny)* lsd2;
05048 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05049 }
05050 }
05051
05052 result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05053 res[kk] = (float)result;
05054
05055 if(result<valmin) {valmin = result; valpos = kk;}
05056
05057 }
05058
05059 Dict retvals;
05060 retvals["dist"] = res;
05061 retvals["pos"] = valpos;
05062
05063 EXITFUNC;
05064 return retvals;
05065 }
05066
05067 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05068 long int d2 = group2[s2 - 1] - group2[0];
05069 long int p2 = 0;
05070 long int i1 = 0;
05071 long int i2 = 0;
05072 long int max = 0;
05073 long int cont = 0;
05074 long int i = 0;
05075 int stop1 = 0;
05076 int stop2 = 0;
05077
05078 for (i=0; i<s1; i++) {
05079 p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05080 if (p2 >= s2) {p2 = s2 - 1;}
05081 i1 = p2;
05082 i2 = p2;
05083 max = s2;
05084 if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05085
05086 stop1 = 0;
05087 stop2 = 0;
05088 while (max--) {
05089 if (group1[i] == group2[i1]) {
05090 if (flag) {stb[cont] = group1[i];}
05091 cont++;
05092 break;
05093 }
05094 if (group2[i1] < group1[i]) {stop1=1;}
05095 if (group1[i] == group2[i2]) {
05096 if (flag) {stb[cont] = group1[i];}
05097 cont++;
05098 break;
05099 }
05100 if (group2[i2] > group1[i]) {stop2=1;}
05101
05102
05103 if (stop1 & stop2) {break;}
05104 i1--;
05105 i2++;
05106 if (i1 < 0) {i1 = 0;}
05107 if (i2 >= s2) {i2 = s2 - 1;}
05108 }
05109
05110 }
05111
05112 return cont;
05113 }
05114
05115
05116
05117 #define old_ptr(i,j,k) old_ptr[i+(j+(k*ny))*(size_t)nx]
05118 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*(size_t)new_nx]
05119 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05120 {
05121
05122 if (!img) {
05123 throw NullPointerException("NULL input image");
05124 }
05125
05126
05127
05128 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05129
05130
05131
05132
05133 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)
05134 {
05135 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05136 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05137 }
05138
05139
05140
05141
05142 int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05143
05144
05145
05146
05147 int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05148 int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05149 int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05150 if(r1>1){r1=1;}
05151 if(r2>1){r2=1;}
05152 if(r3>1){r3=1;}
05153 int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05154
05155
05156
05157 EMData* img2 = new EMData();
05158 img2->set_size(new_nx,new_ny,new_nz);
05159 float *new_ptr = img2->get_data();
05160 float *old_ptr = img->get_data();
05161 int iptr, jptr, kptr = 0;
05162 for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05163 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05164 for (int i=new_st_x; i<nx; i+=x_step) {
05165 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05166 iptr++;}
05167 jptr++;}
05168 kptr++;}
05169 img2->update();
05170 return img2;
05171 }
05172 #undef old_ptr
05173 #undef new_ptr
05174
05175 #define inp(i,j,k) inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*(size_t)nx]
05176 #define outp(i,j,k) outp[i+(j+(k*new_ny))*(size_t)new_nx]
05177 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05178 {
05179
05180 if (!img) throw NullPointerException("NULL input image");
05181
05182
05183
05184 int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05185
05186
05187
05188 if(new_nx>nx || new_ny>ny || new_nz>nz)
05189 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size. 1");
05190 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)
05191 throw ImageDimensionException("The offset inconsistent with the input image size. 2");
05192 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))))
05193 throw ImageDimensionException("The offset inconsistent with the input image size. 3");
05194
05195
05196
05197 int new_st_x = nx/2-new_nx/2 + x_offset,
05198 new_st_y = ny/2-new_ny/2 + y_offset,
05199 new_st_z = nz/2-new_nz/2 + z_offset;
05200
05201
05202
05203 if (new_st_x<0 || new_st_y<0 || new_st_z<0)
05204 throw ImageDimensionException("The offset inconsistent with the input image size. 4");
05205
05206
05207 EMData* wind = img->copy_empty_head();
05208 wind->set_size(new_nx, new_ny, new_nz);
05209 float *outp=wind->get_data();
05210 float *inp=img->get_data();
05211
05212 for (int k=0; k<new_nz; k++)
05213 for(int j=0; j<new_ny; j++)
05214 for(int i=0; i<new_nx; i++)
05215 outp(i,j,k) = inp(i,j,k);
05216 wind->update();
05217 return wind;
05218 }
05219 #undef inp
05220 #undef outp
05221
05222 #define inp(i,j,k) inp[i+(j+(k*ny))*(size_t)nx]
05223 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*(size_t)new_nx]
05224 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)
05225 {
05226
05227 if (!img) throw NullPointerException("NULL input image");
05228
05229
05230
05231 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05232
05233
05234
05235 if(new_nx<nx || new_ny<ny || new_nz<nz)
05236 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05237 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)
05238 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05239 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))))
05240 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05241
05242
05243 EMData* pading = img->copy_head();
05244 pading->set_size(new_nx, new_ny, new_nz);
05245 float *inp = img->get_data();
05246 float *outp = pading->get_data();
05247
05248
05249
05250
05251 float background;
05252
05253 if (strcmp(params,"average")==0) background = img->get_attr("mean");
05254 else if (strcmp(params,"circumference")==0) {
05255 float sum1=0.0f;
05256 size_t cnt=0;
05257 for(int i=0;i<nx;i++) {
05258 sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05259 cnt+=2;
05260 }
05261 if(nz-1 == 0) {
05262 for (int j=1;j<ny-1;j++) {
05263 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05264 cnt+=2;
05265 }
05266 } else {
05267 for (int k=1;k<nz-1;k++) {
05268 for (int j=1;j<ny-1;j++) {
05269 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05270 cnt+=2;
05271 }
05272 }
05273 }
05274 background = sum1/cnt;
05275 } else {
05276 background = static_cast<float>( atof( params ) );
05277 }
05278
05279
05280
05281 int new_st_x=0,new_st_y=0,new_st_z=0;
05282 for (int k=0;k<new_nz;k++)
05283 for(int j=0;j<new_ny;j++)
05284 for (int i=0;i<new_nx;i++)
05285 outp(i,j,k)=background;
05286
05287
05288
05289 new_st_x=int((new_nx/2-nx/2) + x_offset);
05290 new_st_y=int((new_ny/2-ny/2) + y_offset);
05291 new_st_z=int((new_nz/2-nz/2) + z_offset);
05292
05293
05294 for (int k=0;k<nz;k++)
05295 for(int j=0;j<ny;j++)
05296 for(int i=0;i<nx;i++)
05297 outp(i,j,k)=inp(i,j,k);
05298 pading->update();
05299 return pading;
05300 }
05301 #undef inp
05302 #undef outp
05303
05304
05305 void Util::colreverse(float* beg, float* end, int nx) {
05306 float* tmp = new float[nx];
05307 int n = (end - beg)/nx;
05308 int nhalf = n/2;
05309 for (int i = 0; i < nhalf; i++) {
05310
05311 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05312 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05313 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05314 }
05315 delete[] tmp;
05316 }
05317
05318 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05319 {
05320 int nxy = nx*ny;
05321 colreverse(beg, end, nxy);
05322 }
05323
05324
05325 void Util::cyclicshift(EMData *image, Dict params) {
05326
05327 if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05328
05329 int dx = params["dx"];
05330 int dy = params["dy"];
05331 int dz = params["dz"];
05332
05333
05334 int nx = image->get_xsize();
05335 dx %= nx;
05336 if (dx < 0) dx += nx;
05337 int ny = image->get_ysize();
05338 dy %= ny;
05339 if (dy < 0) dy += ny;
05340 int nz = image->get_zsize();
05341 dz %= nz;
05342 if (dz < 0) dz += nz;
05343
05344 int mx = -(dx - nx);
05345 int my = -(dy - ny);
05346 int mz = -(dz - nz);
05347
05348 float* data = image->get_data();
05349
05350 if (mx != 0) {
05351 for (int iz = 0; iz < nz; iz++)
05352 for (int iy = 0; iy < ny; iy++) {
05353
05354 size_t offset = nx*iy + (size_t)nx*ny*iz;
05355 reverse(&data[offset],&data[offset+mx]);
05356 reverse(&data[offset+mx],&data[offset+nx]);
05357 reverse(&data[offset],&data[offset+nx]);
05358 }
05359 }
05360
05361 if (my != 0) {
05362 for (int iz = 0; iz < nz; iz++) {
05363 size_t offset = (size_t)nx*ny*iz;
05364 colreverse(&data[offset], &data[offset + my*nx], nx);
05365 colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05366 colreverse(&data[offset], &data[offset + ny*nx], nx);
05367 }
05368 }
05369 if (mz != 0) {
05370 slicereverse(&data[0], &data[(size_t)mz*ny*nx], nx, ny);
05371 slicereverse(&data[mz*ny*nx], &data[(size_t)nz*ny*nx], nx, ny);
05372 slicereverse(&data[0], &data[(size_t)nz*ny*nx], nx ,ny);
05373 }
05374 image->update();
05375 }
05376
05377
05378
05379
05380 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05381 {
05382 if (image->is_complex())
05383 throw ImageFormatException("Cannot do histogram on Fourier image");
05384
05385 float *imageptr=0, *maskptr=0;
05386 int nx=image->get_xsize();
05387 int ny=image->get_ysize();
05388 int nz=image->get_zsize();
05389
05390 if(mask != NULL){
05391 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05392 throw ImageDimensionException("The size of mask image should be of same size as the input image");
05393 maskptr =mask->get_data();
05394 }
05395 if( nbins == 0) nbins = nx;
05396 vector <float> freq(2*nbins, 0.0);
05397
05398 imageptr=image->get_data();
05399 if( hmin == hmax ) {
05400 if(mask == NULL) {
05401 hmax = image->get_attr("maximum");
05402 hmin = image->get_attr("minimum");
05403 } else {
05404 bool First = true;
05405 for (size_t i = 0;i < (size_t)nx*ny*nz; i++) {
05406 if (maskptr[i]>=0.5f) {
05407 if(First) {
05408 hmax = imageptr[i];
05409 hmin = imageptr[i];
05410 First = false;
05411 } else {
05412 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05413 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05414 }
05415 }
05416 }
05417 }
05418 }
05419 float hdiff = hmax - hmin;
05420 float ff = (nbins-1)/hdiff;
05421 for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05422 if(mask == NULL) {
05423 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05424 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05425 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05426 }
05427 } else {
05428 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05429 if(maskptr[i] >= 0.5) {
05430 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05431 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05432 }
05433 }
05434 }
05435 return freq;
05436 }
05437
05438 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05439 {
05440
05441 if (img->is_complex() || ref->is_complex())
05442 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05443
05444 if(mask != NULL){
05445 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05446 throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05447
05448
05449
05450 size_t size_ref = ((size_t)(ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05451 size_t size_img = ((size_t)(img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05452
05453
05454
05455 float *ref_ptr = ref->get_data();
05456 float ref_h_min = ref->get_attr("minimum");
05457 float ref_h_max = ref->get_attr("maximum");
05458 float ref_h_avg = ref->get_attr("mean");
05459 float ref_h_sig = ref->get_attr("sigma");
05460
05461
05462
05463 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05464
05465 vector<float> img_data = Util::infomask(img, mask);
05466 float img_avg = img_data[0];
05467 float img_sig = img_data[1];
05468
05469
05470 int cnt=0;
05471 for(size_t i=0;i<size_img;++i)
05472 if (mask_ptr[i]>0.5f)
05473 cnt++;
05474
05475
05476
05477 float ref_h_diff = ref_h_max - ref_h_min;
05478
05479 #ifdef _WIN32
05480 int hist_len = _cpp_min((unsigned long)size_ref/16,_cpp_min((unsigned long)size_img/16,256lu));
05481 #else
05482 int hist_len = std::min((unsigned long)size_ref/16,std::min((unsigned long)size_img/16,256lu));
05483 #endif //_WIN32
05484
05485 float *ref_freq_bin = new float[3*hist_len];
05486
05487
05488 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05489
05490 for (size_t i = 0;i < size_ref;++i) {
05491 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05492 ref_freq_bin[L]++;
05493 }
05494 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05495
05496
05497 float A = ref_h_sig/img_sig;
05498 float B = ref_h_avg - (A*img_avg);
05499
05500 vector<float> args;
05501 args.push_back(A);
05502 args.push_back(B);
05503
05504 vector<float> scale;
05505 scale.push_back(1.e-7f*A);
05506 scale.push_back(-1.e-7f*B);
05507
05508 vector<float> ref_freq_hist;
05509 for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05510
05511 vector<float> data;
05512 data.push_back(ref_h_diff);
05513 data.push_back(ref_h_min);
05514
05515 Dict parameter;
05516
05517
05518 parameter["args"] = args;
05519 parameter["scale"]= scale;
05520 parameter["data"] = data;
05521 parameter["ref_freq_bin"] = ref_freq_hist;
05522 parameter["size_img"]=(double)size_img;
05523 parameter["hist_len"]=hist_len;
05524
05525
05526 return parameter;
05527 }
05528
05529
05530 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)
05531 {
05532 float *img_ptr = img->get_data();
05533 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05534
05535 int *img_freq_bin = new int[3*hist_len];
05536 for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05537 for(size_t i = 0;i < size_img;++i) {
05538 if(mask_ptr[i] > 0.5f) {
05539 float img_xn = img_ptr[i]*PA + PB;
05540 int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05541 if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05542 }
05543 }
05544 int freq_hist = 0;
05545
05546 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);
05547 freq_hist = (-freq_hist);
05548 return static_cast<float>(freq_hist);
05549 }
05550
05551 #define QUADPI 3.141592653589793238462643383279502884197
05552 #define DGR_TO_RAD QUADPI/180
05553 #define DM(I) DM [I-1]
05554 #define SS(I) SS [I-1]
05555 Dict Util::CANG(float PHI,float THETA,float PSI)
05556 {
05557 double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05558 vector<float> DM,SS;
05559
05560 for(int i =0;i<9;i++) DM.push_back(0);
05561
05562 for(int i =0;i<6;i++) SS.push_back(0);
05563
05564 CPHI = cos(double(PHI)*DGR_TO_RAD);
05565 SPHI = sin(double(PHI)*DGR_TO_RAD);
05566 CTHE = cos(double(THETA)*DGR_TO_RAD);
05567 STHE = sin(double(THETA)*DGR_TO_RAD);
05568 CPSI = cos(double(PSI)*DGR_TO_RAD);
05569 SPSI = sin(double(PSI)*DGR_TO_RAD);
05570
05571 SS(1) = float(CPHI);
05572 SS(2) = float(SPHI);
05573 SS(3) = float(CTHE);
05574 SS(4) = float(STHE);
05575 SS(5) = float(CPSI);
05576 SS(6) = float(SPSI);
05577
05578 DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05579 DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05580 DM(3) = float(-STHE*CPSI);
05581 DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05582 DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05583 DM(6) = float(STHE*SPSI);
05584 DM(7) = float(STHE*CPHI);
05585 DM(8) = float(STHE*SPHI);
05586 DM(9) = float(CTHE);
05587
05588 Dict DMnSS;
05589 DMnSS["DM"] = DM;
05590 DMnSS["SS"] = SS;
05591
05592 return(DMnSS);
05593 }
05594 #undef SS
05595 #undef DM
05596 #undef QUADPI
05597 #undef DGR_TO_RAD
05598
05599 struct t_BPCQ_line{
05600 int rX;
05601 int offset;
05602 float xbb;
05603 float ybb;
05604 };
05605
05606 void Util::BPCQ( EMData *B, EMData *CUBE, const int radius )
05607 {
05608 if (B->is_complex()) {
05609 B->do_ift_inplace();
05610 B->depad();
05611 }
05612
05613 const Transform * transform = B->get_attr("xform.projection");
05614 Dict transform_params = transform->get_params("spider");
05615
05616
05617 std::vector<float> DM = transform->get_matrix();
05618 DM[3+0] = DM[4+0];
05619 DM[3+1] = DM[4+1];
05620 DM[3+2] = DM[4+2];
05621
05622 delete transform;
05623
05624 const int NSAM = B->get_xsize();
05625 const int NROW = B->get_ysize();
05626
05627
05628 t_BPCQ_line * lines_to_process;
05629 #ifdef _WIN32
05630 if ( (lines_to_process = (t_BPCQ_line *)_aligned_malloc( 4*radius*radius*sizeof(t_BPCQ_line), 256 )) == NULL )
05631 #else
05632 if ( posix_memalign( reinterpret_cast<void**>(&lines_to_process), 256, 4*radius*radius*sizeof(t_BPCQ_line) ) != 0 )
05633 #endif //_WIN32
05634 {
05635 throw std::bad_alloc();
05636 }
05637 t_BPCQ_line * first_free_line = lines_to_process;
05638
05639
05640 {
05641
05642 const float x_shift_plus_center = float(NSAM/2 +1) + float(transform_params[ "tx" ]);
05643 const float y_shift_plus_center = float(NROW/2 +1) + float(transform_params[ "ty" ]);
05644
05645 const int sizeX = CUBE->get_xsize();
05646 const int sizeY = CUBE->get_ysize();
05647 const int sizeZ = CUBE->get_zsize();
05648
05649 const int centerX = sizeX / 2;
05650 const int centerY = sizeY / 2;
05651 const int centerZ = sizeZ / 2;
05652
05653 const int minRZ = (centerZ >= radius) ? (-radius) : (-centerZ);
05654 const int maxRZ = (sizeZ > centerZ+radius) ? (radius) : (sizeZ-centerZ-1);
05655
05656 for ( int rZ=minRZ; rZ<=maxRZ; ++rZ ) {
05657 for ( int rY=-radius; rY<=radius; ++rY ) {
05658 const int sqRX = radius*radius - rZ*rZ - rY*rY;
05659 if (sqRX >= 0) {
05660 #ifdef _WIN32
05661 first_free_line->rX = static_cast<int>( floor(sqrtf(sqRX)+0.5) );
05662 #else
05663 first_free_line->rX = static_cast<int>( roundf(sqrtf(sqRX)) );
05664 #endif //_WIN32
05665 first_free_line->offset = sizeX*( centerY+rY + sizeY*(centerZ+rZ) ) + centerX - first_free_line->rX;
05666 first_free_line->xbb = rZ*DM[2] + rY*DM[1] + x_shift_plus_center;
05667 first_free_line->ybb = rZ*DM[5] + rY*DM[4] + y_shift_plus_center;
05668 ++first_free_line;
05669 }
05670 }
05671 }
05672 }
05673
05674 const float * const Bptr = B->get_data();
05675 float * const CUBE_begin = CUBE->get_data();
05676
05677
05678
05679 for ( t_BPCQ_line * iLine = lines_to_process; iLine < first_free_line; ++iLine ) {
05680 const int rX_first = -(iLine->rX);
05681 const int rX_last = iLine->rX;
05682 float *CUBE_ptr = CUBE_begin + iLine->offset;
05683 for (int rX=rX_first; rX<=rX_last; ++rX, ++CUBE_ptr) {
05684 const float XB = rX * DM[0] + iLine->xbb;
05685 const float YB = rX * DM[3] + iLine->ybb;
05686 const int IQX = int(XB);
05687 const int IQY = int(YB);
05688 if ( IQX < 1 || IQX >= NSAM || IQY < 1 || IQY >= NROW ) continue;
05689 const float DIPX = XB-IQX;
05690 const float DIPY = YB-IQY;
05691 const float b00 = Bptr[IQX-1+((IQY-1)*NSAM)];
05692 const float b01 = Bptr[IQX-1+((IQY-0)*NSAM)];
05693 const float b10 = Bptr[IQX-0+((IQY-1)*NSAM)];
05694 const float b11 = Bptr[IQX-0+((IQY-0)*NSAM)];
05695 *(CUBE_ptr) = *(CUBE_ptr) + b00 + DIPY*(b01-b00) + DIPX*(b10-b00+DIPY*(b11-b10-b01+b00));
05696 }
05697 }
05698
05699 free(lines_to_process);
05700 }
05701
05702 #define W(i,j) Wptr [i+(j)*Wnx]
05703 #define PROJ(i,j) PROJptr [i+(j)*NNNN]
05704 #define SS(i,j) SS [i+(j)*6]
05705
05706 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05707 {
05708
05709 --K;
05710
05711 int NSAM = PROJ->get_xsize();
05712 int NROW = PROJ->get_ysize();
05713
05714 if (PROJ->is_fftpadded()) {
05715 NSAM -= (PROJ->is_fftodd()) ? (1) : (2);
05716 }
05717
05718 const int ntotal = NSAM*NROW;
05719 const float q = 2.0f;
05720 const float qt = 8.0f/q;
05721
05722 const int ipad = 1;
05723 NSAM *= ipad;
05724 NROW *= ipad;
05725 const int NNNN = NSAM+2-(NSAM%2);
05726 const int NX2 = NSAM/2;
05727 const int NR2 = NROW/2;
05728
05729 const int NANG = int(SS.size())/6;
05730
05731 EMData* W = new EMData();
05732 const int Wnx = NNNN/2;
05733 W->set_size(Wnx,NROW,1);
05734 W->to_zero();
05735 float *Wptr = W->get_data();
05736
05737 for (int L=0; L<NANG; L++) {
05738 const float tmp1 = SS(2,K)*SS(3,L)*(SS(0,K)*SS(0,L) + SS(1,K)*SS(1,L)) - SS(2,L)*SS(3,K);
05739 const float tmp2 = SS(3,L)*( SS(0,K)*SS(1,L) - SS(0,L)*SS(1,K) );
05740 float OX = SS(5,K)*tmp2 + SS(4,K)*tmp1;
05741 float OY = SS(4,K)*tmp2 - SS(5,K)*tmp1;
05742 if(OX < 0.0f) {
05743 OX = -OX;
05744 OY = -OY;
05745 }
05746
05747 if( OX > 1.0e-6f || fabs(OY) > 1.0e-6f ) {
05748 for (int J=0; J<NROW; ++J) {
05749 const float JY_OY = (J > NR2) ? ((J-NROW)*OY) : (J*OY);
05750 int xma = NX2;
05751 int xmi = 0;
05752 const float fxma = ( q-JY_OY) / OX;
05753 const float fxmi = (-q-JY_OY) / OX;
05754 if (fxma < xmi || fxmi > xma ) {
05755 continue;
05756 }
05757 if (fxma < xma) {
05758 xma = static_cast<int>(fxma+0.5f);
05759 }
05760 if (fxmi > xmi) {
05761 xmi = static_cast<int>(fxmi+0.5f);
05762 }
05763 for( int I=xmi; I<=xma; ++I ) {
05764 const float Y = I*OX + JY_OY;
05765 W(I,J) += exp(-qt*Y*Y);
05766 }
05767 }
05768 } else {
05769 for (int J=0; J<NROW; ++J) {
05770 for (int I=0; I<NNNN/2; ++I) {
05771 W(I,J) += 1.0f;
05772 }
05773 }
05774 }
05775 }
05776
05777 EMData* proj_in = PROJ;
05778
05779 const bool realOnInput = PROJ->is_real();
05780 if (realOnInput) {
05781
05782 PROJ = PROJ->norm_pad( false, ipad);
05783 PROJ->do_fft_inplace();
05784 PROJ->update();
05785 }
05786 float * PROJptr = PROJ->get_data();
05787
05788 const float osnr = 1.0f/SNR;
05789 const float WNRMinv = 1.0f/W(0,0);
05790 for (int J=0; J<NROW; ++J) {
05791 float sy = (J > NR2) ? (J - NROW) : (J);
05792 sy /= NROW;
05793 sy *= sy;
05794 for (int I=0; I<NNNN; I+=2) {
05795 const int KX = I/2;
05796 const float temp = W(KX,J)*WNRMinv;
05797 float WW = temp/(temp*temp + osnr);
05798
05799 const float sx = float(KX) / NSAM;
05800 WW *= exp(qt*(sy + sx*sx));
05801 PROJ(I,J) *= WW;
05802 PROJ(I+1,J) *= WW;
05803 }
05804 }
05805 delete W; W = 0;
05806
05807 PROJ->do_ift_inplace();
05808 PROJ->depad();
05809
05810 if (realOnInput) {
05811
05812 float* data_src = PROJ->get_data();
05813 float* data_dst = proj_in->get_data();
05814 memcpy( data_dst, data_src, ntotal * sizeof(float) );
05815 delete PROJ;
05816 }
05817
05818 proj_in->update();
05819 }
05820
05821
05822
05823
05824
05825
05826
05827
05828
05829
05830
05831
05832
05833
05834
05835
05836
05837
05838
05839
05840
05841
05842
05843
05844
05845
05846
05847
05848
05849
05850
05851
05852
05853
05854
05855
05856
05857
05858
05859
05860
05861
05862
05863
05864
05865
05866
05867
05868
05869
05870
05871
05872
05873
05874
05875
05876
05877
05878
05879
05880
05881
05882
05883
05884
05885
05886
05887
05888
05889
05890
05891
05892
05893
05894
05895
05896
05897
05898
05899 #undef PROJ
05900 #undef W
05901 #undef SS
05902
05903 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
05904 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
05905 #define SS(I,J) SS [I-1 + (J-1)*6]
05906 #define RI(i,j) RI [(i-1) + ((j-1)*3)]
05907 #define CC(i) CC [i-1]
05908 #define CP(i) CP [i-1]
05909 #define VP(i) VP [i-1]
05910 #define VV(i) VV [i-1]
05911 #define AMAX1(i,j) i>j?i:j
05912 #define AMIN1(i,j) i<j?i:j
05913 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
05914 {
05915 float rad2deg =(180.0f/3.1415926f);
05916 float deg2rad = (3.1415926f/180.0f);
05917
05918 int NSAM,NROW,NNNN,NR2,NANG,L,JY;
05919
05920 NSAM = PROJ->get_xsize();
05921 NROW = PROJ->get_ysize();
05922
05923 if (PROJ->is_fftpadded()) {
05924 NSAM -= (PROJ->is_fftodd()) ? (1) : (2);
05925 }
05926
05927 NNNN = NSAM+2-(NSAM%2);
05928 NR2 = NROW/2;
05929 NANG = int(SS.size())/6;
05930
05931 float RI[9];
05932 RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
05933 RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
05934 RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
05935 RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
05936 RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
05937 RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
05938 RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
05939 RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
05940 RI(3,3)=SS(3,NUMP);
05941
05942 float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
05943
05944 EMData* W = new EMData();
05945 int Wnx = NNNN/2;
05946 W->set_size(NNNN/2,NROW,1);
05947 W->to_one();
05948 float *Wptr = W->get_data();
05949
05950 float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
05951
05952 for (L=1; L<=NANG; L++) {
05953 if (L != NUMP) {
05954 CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
05955 CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
05956 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);
05957
05958 TMP = sqrt(CC(1)*CC(1) + CC(2)*CC(2) + CC(3)*CC(3));
05959 CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
05960 ALPHA=rad2deg*float(asin(CCN));
05961 if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
05962 if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
05963 if(ALPHA<1.0E-6) {
05964 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
05965 } else {
05966 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
05967 CC(1) = CC(1)/CCN;CC(2) = CC(2)/CCN;CC(3) = CC(3)/CCN;
05968 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
05969 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
05970 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
05971 CP(1) = 0.0;CP(2) = 0.0;
05972 VP(1) = 0.0;VP(2) = 0.0;
05973
05974 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
05975 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
05976 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
05977 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
05978
05979 TMP = CP(1)*VP(2)-CP(2)*VP(1);
05980
05981
05982 TMP = AMAX1(1.0E-4f,fabs(TMP));
05983 float tmpinv = 1.0f/TMP;
05984 for(int J=1;J<=NROW;J++) {
05985 JY = (J-1);
05986 if (JY>NR2) JY=JY-NROW;
05987 for(int I=1;I<=NNNN/2;I++) {
05988 FV = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
05989 RT = 1.0f-FV/FM;
05990 W(I,J) += ((RT>0.0f)*RT);
05991 }
05992 }
05993 }
05994 }
05995 }
05996
05997 EMData* proj_in = PROJ;
05998 const bool realOnInput = PROJ->is_real();
05999
06000 if (realOnInput) {
06001
06002 PROJ = PROJ->norm_pad( false, 1 );
06003 PROJ->do_fft_inplace();
06004 PROJ->update();
06005 }
06006 float *PROJptr = PROJ->get_data();
06007
06008 int KX;
06009 float WW;
06010 for(int J=1; J<=NROW; J++)
06011 for(int I=1; I<=NNNN; I+=2) {
06012 KX = (I+1)/2;
06013 WW = 1.0f/W(KX,J);
06014 PROJ(I,J) = PROJ(I,J)*WW;
06015 PROJ(I+1,J) = PROJ(I+1,J)*WW;
06016 }
06017 delete W; W = 0;
06018 PROJ->do_ift_inplace();
06019 PROJ->depad();
06020
06021 if (realOnInput) {
06022
06023 float* data_src = PROJ->get_data();
06024 float* data_dst = proj_in->get_data();
06025 int ntotal = NSAM*NROW;
06026 memcpy( data_dst, data_src, ntotal * sizeof(float) );
06027 delete PROJ;
06028 }
06029
06030 proj_in->update();
06031 }
06032 #undef AMAX1
06033 #undef AMIN1
06034 #undef RI
06035 #undef CC
06036 #undef CP
06037 #undef VV
06038 #undef VP
06039 #undef W
06040 #undef SS
06041 #undef PROJ
06042
06043 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06044 {
06045 float cst = cs*1.0e7f;
06046
06047 wgh /= 100.0;
06048 float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06049 float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06050 float ak2 = ak*ak;
06051 float g1 = dzz*1.0e4f*lambda*ak2;
06052 float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06053
06054 float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06055 if(b_factor != 0.0f) ctfv *= exp(-b_factor*ak2/4.0f);
06056
06057 return ctfv;
06058 }
06059
06060 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06061 {
06062
06063
06064
06065 int nx = image->get_xsize();
06066 int ny = image->get_ysize();
06067 int nz = image->get_zsize();
06068
06069
06070
06071 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06072 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06073
06074 size_t i, size = (size_t)nx*ny*nz;
06075
06076 float* img_ptr = image->get_data();
06077 float* mask_ptr = mask->get_data();
06078
06079 int ln=0;
06080 for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06081
06082 EMData* new_image = new EMData();
06083 new_image->set_size(ln,1,1);
06084 float *new_ptr = new_image->get_data();
06085
06086 ln=-1;
06087 for(i = 0;i < size;i++){
06088 if(mask_ptr[i] > 0.5f) {
06089 ln++;
06090 new_ptr[ln]=img_ptr[i];
06091 }
06092 }
06093
06094 return new_image;
06095 }
06096
06097 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06098 {
06099
06100
06101
06102 if(mask == NULL)
06103 throw ImageDimensionException("The mask cannot be an null image");
06104
06105
06106
06107
06108 int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06109
06110 size_t i,size = (size_t)nx*ny*nz;
06111
06112 EMData *new_image = new EMData();
06113 new_image->set_size(nx,ny,nz);
06114 float *new_ptr = new_image->get_data();
06115 float *mask_ptr = mask->get_data();
06116 float *img_ptr = image->get_data();
06117 int count = 0;
06118 float sum_under_mask = 0.0 ;
06119 for(i = 0;i < size;i++){
06120 if(mask_ptr[i] > 0.5f){
06121 new_ptr[i] = img_ptr[count];
06122 sum_under_mask += img_ptr[count];
06123 count++;
06124 if( count > image->get_xsize() ) {
06125 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06126 }
06127 }
06128 }
06129
06130 if( count > image->get_xsize() ) {
06131 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06132 }
06133
06134 float avg_under_mask = sum_under_mask / count;
06135 for(i = 0;i < size;i++) {
06136 if(mask_ptr[i] <= 0.5f) new_ptr[i] = avg_under_mask;
06137 }
06138 new_image->update();
06139 return new_image;
06140 }
06141
06142
06143
06144 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06145 {
06146 vector<float>new_peak;
06147 int n1=peak1.size()/3;
06148 float p_size2=p_size*p_size;
06149 for (int i=0;i<n1;++i) {
06150 vector<float>::iterator it2= peak1.begin()+3*i;
06151 bool push_back1=true;
06152 int n2=peak2.size()/3;
06153
06154
06155 if(n2 ==0) {
06156 new_peak.push_back(*it2);
06157 new_peak.push_back(*(it2+1));
06158 new_peak.push_back(*(it2+2));
06159 } else {
06160 int j=0;
06161 while (j< n2-1 ) {
06162 vector<float>::iterator it3= peak2.begin()+3*j;
06163 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06164 if(d2< p_size2 ) {
06165 if( (*it2)<(*it3) ) {
06166 new_peak.push_back(*it3);
06167 new_peak.push_back(*(it3+1));
06168 new_peak.push_back(*(it3+2));
06169 peak2.erase(it3);
06170 peak2.erase(it3);
06171 peak2.erase(it3);
06172 push_back1=false;
06173 } else {
06174 peak2.erase(it3);
06175 peak2.erase(it3);
06176 peak2.erase(it3);
06177 }
06178 } else j=j+1;
06179 n2=peak2.size()/3;
06180 }
06181 if(push_back1) {
06182 new_peak.push_back(*it2);
06183 new_peak.push_back(*(it2+1));
06184 new_peak.push_back(*(it2+2));
06185 }
06186 }
06187 }
06188 return new_peak;
06189 }
06190
06191 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06192 {
06193
06194
06195
06196
06197
06198 ENTERFUNC;
06199
06200 int i;
06201
06202
06203 for ( i = 0 ; i < n * n ; i++ ) eigvec[i] = covmat[i];
06204
06205 char NEEDV = 'V';
06206 char UPLO = 'U';
06207 int lwork = -1;
06208 int info = 0;
06209 float *work, wsize;
06210
06211
06212 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06213 lwork = (int)wsize;
06214
06215 work = (float *)calloc(lwork, sizeof(float));
06216
06217 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06218 free(work);
06219 EXITFUNC;
06220 return info;
06221 }
06222
06223 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06224 {
06225
06226 ENTERFUNC;
06227 int len = covmatpy.size();
06228 float *eigvec;
06229 float *eigval;
06230 float *covmat;
06231 int status = 0;
06232 eigval = (float*)calloc(ncov,sizeof(float));
06233 eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06234 covmat = (float*)calloc(ncov*ncov, sizeof(float));
06235
06236 const float *covmat_ptr;
06237 covmat_ptr = &covmatpy[0];
06238 for(int i=0;i<len;i++){
06239 covmat[i] = covmat_ptr[i];
06240 }
06241
06242 status = Util::coveig(ncov, covmat, eigval, eigvec);
06243
06244 vector<float> eigval_py(ncov);
06245 const float *eigval_ptr;
06246 eigval_ptr = &eigval[0];
06247 for(int i=0;i<ncov;i++){
06248 eigval_py[i] = eigval_ptr[i];
06249 }
06250
06251 vector<float> eigvec_py(ncov*ncov);
06252 const float *eigvec_ptr;
06253 eigvec_ptr = &eigvec[0];
06254 for(int i=0;i<ncov*ncov;i++){
06255 eigvec_py[i] = eigvec_ptr[i];
06256 }
06257
06258 Dict res;
06259 res["eigval"] = eigval_py;
06260 res["eigvec"] = eigvec_py;
06261
06262 EXITFUNC;
06263 return res;
06264 }
06265
06266 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06267 {
06268 int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06269
06270 k=(int)pw.size();
06271 l=0;
06272 m=k;
06273 n2=n+2;
06274 n1=n+1;
06275 klmd=k+l+m;
06276 klm2d= k+l+m+2;
06277 nklmd=k+l+m+n;
06278 n2d=n+2;
06279
06280 n_larg=klmd*2;
06281 klm2d=n_larg+klm2d;
06282 klmd=n_larg+klmd;
06283 nklmd=n_larg+nklmd;
06284 int size_q=klm2d*n2d;
06285 int size_cu=nklmd*2;
06286 static int i__;
06287
06288 double *q ;
06289 double *x ;
06290 double *res;
06291 double *cu;
06292 float *q2;
06293 float *pw_;
06294 long int *iu;
06295 double *s;
06296 q = (double*)calloc(size_q,sizeof(double));
06297 x = (double*)calloc(n2d,sizeof(double));
06298 res = (double*)calloc(klmd,sizeof(double));
06299 cu = (double*)calloc(size_cu,sizeof(double));
06300 s = (double*)calloc(klmd,sizeof(double));
06301 q2 = (float*)calloc(size_q,sizeof(float));
06302 iu = (long int*)calloc(size_cu,sizeof(long int));
06303 pw_ = (float*)calloc(k,sizeof(float));
06304
06305 for( i__ =0; i__<k; ++i__) pw_[i__] = log(pw[i__]);
06306 long int l_k=k;
06307 long int l_n=n;
06308 long int l_iswi=iswi;
06309 vector<float> cl1_res;
06310 cl1_res = Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06311 free(q);
06312 free(x);
06313 free(res);
06314 free(s);
06315 free(cu);
06316 free(q2);
06317 free(iu);
06318 free(pw_);
06319 return cl1_res;
06320 }
06321 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)
06322 {
06323 long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06324 float r__1;
06325 int tmp__i;
06326 long int i__, j;
06327 --s;
06328 --res;
06329 iu -= 3;
06330 cu -= 3;
06331 --x;
06332 long int klm2d;
06333 klm2d= *k+*k+2;
06334 klm2d=klm2d+klm2d;
06335 q_dim1 = klm2d;
06336 q_offset = 1 + q_dim1;
06337 q -= q_offset;
06338 q2_dim1 = klm2d;
06339 q2_offset = 1 + q2_dim1;
06340 q2 -= q2_offset;
06341 i__2=0;
06342 i__1 = *n - 1;
06343 tmp__i=0;
06344 for (j = 1; j <= i__1; ++j) {
06345 i__2 = *k;
06346 tmp__i+=1;
06347 for (i__ = 1; i__ <= i__2; ++i__) {
06348 r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06349 q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06350 }
06351 }
06352 for (i__ = 1; i__ <= i__2; ++i__)
06353 { q2[i__ + *n * q2_dim1] = 1.f;
06354 q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06355 }
06356 vector<float> fit_res;
06357 fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06358 return fit_res;
06359 }
06360 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)
06361 {
06362
06363 long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06364
06365
06366 long int i__, j, m, n1, ii, jj;
06367 double tmp;
06368 vector<float> p;
06369 --x;
06370 q_dim1 = *klm2d;
06371 q_offset = 1 + q_dim1;
06372 q -= q_offset;
06373 q1_dim1 = *klm2d;
06374 q1_offset = 1 + q1_dim1;
06375 q1 -= q1_offset;
06376 --s;
06377 --res;
06378 iu -= 3;
06379 cu -= 3;
06380
06381
06382 long int l = 0;
06383
06384
06385 m = *ks;
06386 n1 = *n + 1;
06387 if (*iswi == 1) {
06388 i__1 = n1;
06389 for (jj = 1; jj <= i__1; ++jj) {
06390 i__2 = *ks;
06391 for (ii = 1; ii <= i__2; ++ii) {
06392
06393
06394 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06395 ;
06396 }
06397 }
06398 } else if (*iswi == 2) {
06399 i__1 = *ks;
06400 for (ii = 1; ii <= i__1; ++ii) {
06401 i__2 = n1;
06402 for (jj = 1; jj <= i__2; ++jj) {
06403 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06404 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06405 }
06406 }
06407 } else if (*iswi == 3) {
06408 l = 2;
06409 i__1 = n1;
06410 for (jj = 1; jj <= i__1; ++jj) {
06411 i__2 = *ks + 2;
06412 for (ii = 1; ii <= i__2; ++ii) {
06413 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06414 }
06415 i__2 = *ks;
06416 for (ii = 1; ii <= i__2; ++ii) {
06417 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06418 }
06419 }
06420 } else if (*iswi == 4) {
06421 l = 2;
06422 i__1 = n1;
06423 for (jj = 1; jj <= i__1; ++jj) {
06424 i__2 = *ks + 2;
06425 for (ii = 1; ii <= i__2; ++ii) {
06426 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06427 }
06428 i__2 = *ks;
06429 for (ii = 1; ii <= i__2; ++ii) {
06430 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06431 }
06432 }
06433 } else if (*iswi == 5) {
06434 l = 1;
06435 i__1 = n1;
06436 for (jj = 1; jj <= i__1; ++jj) {
06437 i__2 = *ks + 1;
06438 for (ii = 1; ii <= i__2; ++ii) {
06439 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06440 }
06441 i__2 = *ks;
06442 for (ii = 1; ii <= i__2; ++ii) {
06443 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06444 }
06445 }
06446 } else if (*iswi == 6) {
06447 l = 1;
06448 i__1 = n1;
06449 for (jj = 1; jj <= i__1; ++jj) {
06450 i__2 = *ks + 1;
06451 for (ii = 1; ii <= i__2; ++ii) {
06452 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06453 }
06454 i__2 = *ks;
06455 for (ii = 1; ii <= i__2; ++ii) {
06456 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06457 }
06458 }
06459 } else if (*iswi == 7) {
06460 l = 3;
06461 i__1 = n1;
06462 for (jj = 1; jj <= i__1; ++jj) {
06463 i__2 = *ks + 3;
06464 for (ii = 1; ii <= i__2; ++ii) {
06465 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06466 }
06467 i__2 = *ks;
06468 for (ii = 1; ii <= i__2; ++ii) {
06469 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06470 }
06471 }
06472 } else if (*iswi == 8) {
06473 l = 4;
06474 i__1 = n1;
06475 for (jj = 1; jj <= i__1; ++jj) {
06476 i__2 = *ks + 4;
06477 for (ii = 1; ii <= i__2; ++ii) {
06478 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06479 }
06480 i__2 = *ks;
06481 for (ii = 1; ii <= i__2; ++ii) {
06482 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06483 }
06484 }
06485 }
06486
06487 Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06488 i__1 = *ks;
06489 int tmp__j=0;
06490 for (i__ = 1; i__ <= i__1; ++i__) {
06491 tmp = 0.f;
06492 i__2 = *n - 1;
06493 for (j = 1; j <= i__2; ++j) {
06494 tmp__j=j;
06495 tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06496 }
06497 tmp += x[*n];
06498 p.push_back(static_cast<float>(exp(tmp)));
06499 p.push_back(q1[i__ + q1_dim1]);
06500 }
06501 i__2=*n;
06502 for (i__=1;i__<=i__2;++i__)
06503 { p.push_back(static_cast<float>(x[i__]));}
06504 return p;
06505 }
06506 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06507 double *q, double *x, double *res, double *cu, long int *iu, double *s)
06508 {
06509
06510 long int q_dim1, q_offset, i__1, i__2;
06511 double d__1;
06512
06513 static long int i__, j;
06514 static double z__;
06515 static long int n1, n2, ia, ii, kk, in, nk, js;
06516 static double sn, zu, zv;
06517 static long int nk1, klm, nkl, jmn, jpn;
06518 static double cuv;
06519 static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06520 static float xmin;
06521 static double xmax;
06522 static long int iout;
06523 static double xsum;
06524 static long int iineg, maxit;
06525 static double toler;
06526 static float error;
06527 static double pivot;
06528 static long int kforce, iphase;
06529 static double tpivot;
06530
06531 --s;
06532 --res;
06533 iu -= 3;
06534 cu -= 3;
06535 --x;
06536 q_dim1 = *klm2d;
06537 q_offset = 1 + q_dim1;
06538 q -= q_offset;
06539
06540
06541 maxit = 500;
06542 kode = 0;
06543 toler = 1e-4f;
06544 iter = 0;
06545 n1 = *n + 1;
06546 n2 = *n + 2;
06547 nk = *n + *k;
06548 nk1 = nk + 1;
06549 nkl = nk + *l;
06550 nkl1 = nkl + 1;
06551 klm = *k + *l + *m;
06552 klm1 = klm + 1;
06553 klm2 = klm + 2;
06554 nklm = *n + klm;
06555 kforce = 1;
06556 iter = 0;
06557 js = 1;
06558 ia = 0;
06559
06560 i__1 = *n;
06561 for (j = 1; j <= i__1; ++j) {
06562 q[klm2 + j * q_dim1] = (double) j;
06563
06564 }
06565 i__1 = klm;
06566 for (i__ = 1; i__ <= i__1; ++i__) {
06567 q[i__ + n2 * q_dim1] = (double) (*n + i__);
06568 if (q[i__ + n1 * q_dim1] >= 0.f) {
06569 goto L30;
06570 }
06571 i__2 = n2;
06572 for (j = 1; j <= i__2; ++j) {
06573 q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06574
06575 }
06576 L30:
06577 ;
06578 }
06579
06580 iphase = 2;
06581 i__1 = nklm;
06582 for (j = 1; j <= i__1; ++j) {
06583 cu[(j << 1) + 1] = 0.f;
06584 cu[(j << 1) + 2] = 0.f;
06585 iu[(j << 1) + 1] = 0;
06586 iu[(j << 1) + 2] = 0;
06587
06588 }
06589 if (*l == 0) {
06590 goto L60;
06591 }
06592 i__1 = nkl;
06593 for (j = nk1; j <= i__1; ++j) {
06594 cu[(j << 1) + 1] = 1.f;
06595 cu[(j << 1) + 2] = 1.f;
06596 iu[(j << 1) + 1] = 1;
06597 iu[(j << 1) + 2] = 1;
06598
06599 }
06600 iphase = 1;
06601 L60:
06602 if (*m == 0) {
06603 goto L80;
06604 }
06605 i__1 = nklm;
06606 for (j = nkl1; j <= i__1; ++j) {
06607 cu[(j << 1) + 2] = 1.f;
06608 iu[(j << 1) + 2] = 1;
06609 jmn = j - *n;
06610 if (q[jmn + n2 * q_dim1] < 0.f) {
06611 iphase = 1;
06612 }
06613
06614 }
06615 L80:
06616 if (kode == 0) {
06617 goto L150;
06618 }
06619 i__1 = *n;
06620 for (j = 1; j <= i__1; ++j) {
06621 if ((d__1 = x[j]) < 0.) {
06622 goto L90;
06623 } else if (d__1 == 0) {
06624 goto L110;
06625 } else {
06626 goto L100;
06627 }
06628 L90:
06629 cu[(j << 1) + 1] = 1.f;
06630 iu[(j << 1) + 1] = 1;
06631 goto L110;
06632 L100:
06633 cu[(j << 1) + 2] = 1.f;
06634 iu[(j << 1) + 2] = 1;
06635 L110:
06636 ;
06637 }
06638 i__1 = *k;
06639 for (j = 1; j <= i__1; ++j) {
06640 jpn = j + *n;
06641 if ((d__1 = res[j]) < 0.) {
06642 goto L120;
06643 } else if (d__1 == 0) {
06644 goto L140;
06645 } else {
06646 goto L130;
06647 }
06648 L120:
06649 cu[(jpn << 1) + 1] = 1.f;
06650 iu[(jpn << 1) + 1] = 1;
06651 if (q[j + n2 * q_dim1] > 0.f) {
06652 iphase = 1;
06653 }
06654 goto L140;
06655 L130:
06656 cu[(jpn << 1) + 2] = 1.f;
06657 iu[(jpn << 1) + 2] = 1;
06658 if (q[j + n2 * q_dim1] < 0.f) {
06659 iphase = 1;
06660 }
06661 L140:
06662 ;
06663 }
06664 L150:
06665 if (iphase == 2) {
06666 goto L500;
06667 }
06668
06669 L160:
06670 i__1 = n1;
06671 for (j = js; j <= i__1; ++j) {
06672 xsum = 0.;
06673 i__2 = klm;
06674 for (i__ = 1; i__ <= i__2; ++i__) {
06675 ii = (long int) q[i__ + n2 * q_dim1];
06676 if (ii < 0) {
06677 goto L170;
06678 }
06679 z__ = cu[(ii << 1) + 1];
06680 goto L180;
06681 L170:
06682 iineg = -ii;
06683 z__ = cu[(iineg << 1) + 2];
06684 L180:
06685 xsum += q[i__ + j * q_dim1] * z__;
06686
06687
06688 }
06689 q[klm1 + j * q_dim1] = xsum;
06690
06691 }
06692 i__1 = *n;
06693 for (j = js; j <= i__1; ++j) {
06694 ii = (long int) q[klm2 + j * q_dim1];
06695 if (ii < 0) {
06696 goto L210;
06697 }
06698 z__ = cu[(ii << 1) + 1];
06699 goto L220;
06700 L210:
06701 iineg = -ii;
06702 z__ = cu[(iineg << 1) + 2];
06703 L220:
06704 q[klm1 + j * q_dim1] -= z__;
06705
06706 }
06707
06708 L240:
06709 xmax = 0.f;
06710 if (js > *n) {
06711 goto L490;
06712 }
06713 i__1 = *n;
06714 for (j = js; j <= i__1; ++j) {
06715 zu = q[klm1 + j * q_dim1];
06716 ii = (long int) q[klm2 + j * q_dim1];
06717 if (ii > 0) {
06718 goto L250;
06719 }
06720 ii = -ii;
06721 zv = zu;
06722 zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06723 goto L260;
06724 L250:
06725 zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06726 L260:
06727 if (kforce == 1 && ii > *n) {
06728 goto L280;
06729 }
06730 if (iu[(ii << 1) + 1] == 1) {
06731 goto L270;
06732 }
06733 if (zu <= xmax) {
06734 goto L270;
06735 }
06736 xmax = zu;
06737 in = j;
06738 L270:
06739 if (iu[(ii << 1) + 2] == 1) {
06740 goto L280;
06741 }
06742 if (zv <= xmax) {
06743 goto L280;
06744 }
06745 xmax = zv;
06746 in = j;
06747 L280:
06748 ;
06749 }
06750 if (xmax <= toler) {
06751 goto L490;
06752 }
06753 if (q[klm1 + in * q_dim1] == xmax) {
06754 goto L300;
06755 }
06756 i__1 = klm2;
06757 for (i__ = 1; i__ <= i__1; ++i__) {
06758 q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06759
06760 }
06761 q[klm1 + in * q_dim1] = xmax;
06762
06763 L300:
06764 if (iphase == 1 || ia == 0) {
06765 goto L330;
06766 }
06767 xmax = 0.f;
06768 i__1 = ia;
06769 for (i__ = 1; i__ <= i__1; ++i__) {
06770 z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06771 if (z__ <= xmax) {
06772 goto L310;
06773 }
06774 xmax = z__;
06775 iout = i__;
06776 L310:
06777 ;
06778 }
06779 if (xmax <= toler) {
06780 goto L330;
06781 }
06782 i__1 = n2;
06783 for (j = 1; j <= i__1; ++j) {
06784 z__ = q[ia + j * q_dim1];
06785 q[ia + j * q_dim1] = q[iout + j * q_dim1];
06786 q[iout + j * q_dim1] = z__;
06787
06788 }
06789 iout = ia;
06790 --ia;
06791 pivot = q[iout + in * q_dim1];
06792 goto L420;
06793 L330:
06794 kk = 0;
06795 i__1 = klm;
06796 for (i__ = 1; i__ <= i__1; ++i__) {
06797 z__ = q[i__ + in * q_dim1];
06798 if (z__ <= toler) {
06799 goto L340;
06800 }
06801 ++kk;
06802 res[kk] = q[i__ + n1 * q_dim1] / z__;
06803 s[kk] = (double) i__;
06804 L340:
06805 ;
06806 }
06807 L350:
06808 if (kk > 0) {
06809 goto L360;
06810 }
06811 kode = 2;
06812 goto L590;
06813 L360:
06814 xmin = static_cast<float>( res[1] );
06815 iout = (long int) s[1];
06816 j = 1;
06817 if (kk == 1) {
06818 goto L380;
06819 }
06820 i__1 = kk;
06821 for (i__ = 2; i__ <= i__1; ++i__) {
06822 if (res[i__] >= xmin) {
06823 goto L370;
06824 }
06825 j = i__;
06826 xmin = static_cast<float>( res[i__] );
06827 iout = (long int) s[i__];
06828 L370:
06829 ;
06830 }
06831 res[j] = res[kk];
06832 s[j] = s[kk];
06833 L380:
06834 --kk;
06835 pivot = q[iout + in * q_dim1];
06836 ii = (long int) q[iout + n2 * q_dim1];
06837 if (iphase == 1) {
06838 goto L400;
06839 }
06840 if (ii < 0) {
06841 goto L390;
06842 }
06843 if (iu[(ii << 1) + 2] == 1) {
06844 goto L420;
06845 }
06846 goto L400;
06847 L390:
06848 iineg = -ii;
06849 if (iu[(iineg << 1) + 1] == 1) {
06850 goto L420;
06851 }
06852
06853 L400:
06854 ii = abs(ii);
06855 cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06856 if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06857 goto L420;
06858 }
06859
06860 i__1 = n1;
06861 for (j = js; j <= i__1; ++j) {
06862 z__ = q[iout + j * q_dim1];
06863 q[klm1 + j * q_dim1] -= z__ * cuv;
06864 q[iout + j * q_dim1] = -z__;
06865
06866 }
06867 q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06868 goto L350;
06869
06870 L420:
06871 if (iter < maxit) {
06872 goto L430;
06873 }
06874 kode = 3;
06875 goto L590;
06876 L430:
06877 ++iter;
06878 i__1 = n1;
06879 for (j = js; j <= i__1; ++j) {
06880 if (j != in) {
06881 q[iout + j * q_dim1] /= pivot;
06882 }
06883
06884 }
06885
06886
06887
06888
06889
06890
06891
06892
06893 i__1 = n1;
06894 for (j = js; j <= i__1; ++j) {
06895 if (j == in) {
06896 goto L460;
06897 }
06898 z__ = -q[iout + j * q_dim1];
06899 i__2 = klm1;
06900 for (i__ = 1; i__ <= i__2; ++i__) {
06901 if (i__ != iout) {
06902 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06903 }
06904
06905 }
06906 L460:
06907 ;
06908 }
06909 tpivot = -pivot;
06910 i__1 = klm1;
06911 for (i__ = 1; i__ <= i__1; ++i__) {
06912 if (i__ != iout) {
06913 q[i__ + in * q_dim1] /= tpivot;
06914 }
06915
06916 }
06917 q[iout + in * q_dim1] = 1.f / pivot;
06918 z__ = q[iout + n2 * q_dim1];
06919 q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
06920 q[klm2 + in * q_dim1] = z__;
06921 ii = (long int) abs(z__);
06922 if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
06923 goto L240;
06924 }
06925 i__1 = klm2;
06926 for (i__ = 1; i__ <= i__1; ++i__) {
06927 z__ = q[i__ + in * q_dim1];
06928 q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
06929 q[i__ + js * q_dim1] = z__;
06930
06931 }
06932 ++js;
06933 goto L240;
06934
06935 L490:
06936 if (kforce == 0) {
06937 goto L580;
06938 }
06939 if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
06940 goto L500;
06941 }
06942 kforce = 0;
06943 goto L240;
06944
06945 L500:
06946 iphase = 2;
06947 i__1 = nklm;
06948 for (j = 1; j <= i__1; ++j) {
06949 cu[(j << 1) + 1] = 0.f;
06950 cu[(j << 1) + 2] = 0.f;
06951
06952 }
06953 i__1 = nk;
06954 for (j = n1; j <= i__1; ++j) {
06955 cu[(j << 1) + 1] = 1.f;
06956 cu[(j << 1) + 2] = 1.f;
06957
06958 }
06959 i__1 = klm;
06960 for (i__ = 1; i__ <= i__1; ++i__) {
06961 ii = (long int) q[i__ + n2 * q_dim1];
06962 if (ii > 0) {
06963 goto L530;
06964 }
06965 ii = -ii;
06966 if (iu[(ii << 1) + 2] == 0) {
06967 goto L560;
06968 }
06969 cu[(ii << 1) + 2] = 0.f;
06970 goto L540;
06971 L530:
06972 if (iu[(ii << 1) + 1] == 0) {
06973 goto L560;
06974 }
06975 cu[(ii << 1) + 1] = 0.f;
06976 L540:
06977 ++ia;
06978 i__2 = n2;
06979 for (j = 1; j <= i__2; ++j) {
06980 z__ = q[ia + j * q_dim1];
06981 q[ia + j * q_dim1] = q[i__ + j * q_dim1];
06982 q[i__ + j * q_dim1] = z__;
06983
06984 }
06985 L560:
06986 ;
06987 }
06988 goto L160;
06989 L570:
06990 if (q[klm1 + n1 * q_dim1] <= toler) {
06991 goto L500;
06992 }
06993 kode = 1;
06994 goto L590;
06995 L580:
06996 if (iphase == 1) {
06997 goto L570;
06998 }
06999
07000 kode = 0;
07001 L590:
07002 xsum = 0.;
07003 i__1 = *n;
07004 for (j = 1; j <= i__1; ++j) {
07005 x[j] = 0.f;
07006
07007 }
07008 i__1 = klm;
07009 for (i__ = 1; i__ <= i__1; ++i__) {
07010 res[i__] = 0.f;
07011
07012 }
07013 i__1 = klm;
07014 for (i__ = 1; i__ <= i__1; ++i__) {
07015 ii = (long int) q[i__ + n2 * q_dim1];
07016 sn = 1.f;
07017 if (ii > 0) {
07018 goto L620;
07019 }
07020 ii = -ii;
07021 sn = -1.f;
07022 L620:
07023 if (ii > *n) {
07024 goto L630;
07025 }
07026 x[ii] = sn * q[i__ + n1 * q_dim1];
07027 goto L640;
07028 L630:
07029 iimn = ii - *n;
07030 res[iimn] = sn * q[i__ + n1 * q_dim1];
07031 if (ii >= n1 && ii <= nk) {
07032 xsum += q[i__ + n1 * q_dim1];
07033 }
07034 L640:
07035 ;
07036 }
07037 error = (float)xsum;
07038 return;
07039 }
07040
07041 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07042 {
07043 int j,d;
07044 EMData * e = new EMData();
07045 float *eptr, *imgptr;
07046 imgptr = img->get_data();
07047 float SSE = 0.f;
07048 for (j = 0 ; j < N ; j++) {
07049 e->read_image(images,S[j]);
07050 eptr = e->get_data();
07051 for (d = 0; d < size; d++) {
07052 SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07053 }
07054 delete e;
07055 return SSE;
07056 }
07057
07058
07059 #define mymax(x,y) (((x)>(y))?(x):(y))
07060 #define mymin(x,y) (((x)<(y))?(x):(y))
07061 #define sign(x,y) (((((y)>0)?(1):(-1))*(y!=0))*(x))
07062
07063
07064 #define quadpi 3.141592653589793238462643383279502884197
07065 #define dgr_to_rad quadpi/180
07066 #define deg_to_rad quadpi/180
07067 #define rad_to_deg 180/quadpi
07068 #define rad_to_dgr 180/quadpi
07069 #define TRUE 1
07070 #define FALSE 0
07071
07072
07073 #define theta(i) theta [i-1]
07074 #define phi(i) phi [i-1]
07075 #define weight(i) weight [i-1]
07076 #define lband(i) lband [i-1]
07077 #define ts(i) ts [i-1]
07078 #define thetast(i) thetast [i-1]
07079 #define key(i) key [i-1]
07080
07081
07082 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07083 {
07084
07085 ENTERFUNC;
07086
07087 if ( th.size() != ph.size() ) {
07088 LOGERR("images not same size");
07089 throw ImageFormatException( "images not same size");
07090 }
07091
07092
07093 srand(10);
07094
07095 int i,*key;
07096 int len = th.size();
07097 double *theta,*phi,*weight;
07098 theta = (double*) calloc(len,sizeof(double));
07099 phi = (double*) calloc(len,sizeof(double));
07100 weight = (double*) calloc(len,sizeof(double));
07101 key = (int*) calloc(len,sizeof(int));
07102 const float *thptr, *phptr;
07103
07104 thptr = &th[0];
07105 phptr = &ph[0];
07106 for(i=1;i<=len;i++){
07107 key(i) = i;
07108 weight(i) = 0.0;
07109 }
07110
07111 for(i = 0;i<len;i++){
07112 theta[i] = thptr[i];
07113 phi[i] = phptr[i];
07114 }
07115
07116
07117 Util::hsortd(theta, phi, key, len, 1);
07118
07119
07120 Util::voronoi(phi, theta, weight, len);
07121
07122
07123 Util::hsortd(weight, weight, key, len, 2);
07124
07125 free(theta);
07126 free(phi);
07127 free(key);
07128 vector<double> wt;
07129 double count = 0;
07130 for(i=1; i<= len; i++)
07131 {
07132 wt.push_back(weight(i));
07133 count += weight(i);
07134 }
07135
07136
07137
07138
07139
07140
07141 free(weight);
07142
07143 EXITFUNC;
07144 return wt;
07145
07146 }
07147
07148 struct tmpstruct{
07149 double theta1,phi1;
07150 int key1;
07151 };
07152
07153 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07154 {
07155 ENTERFUNC;
07156 vector<tmpstruct> tmp(len);
07157 int i;
07158 for(i = 1;i<=len;i++)
07159 {
07160 tmp[i-1].theta1 = theta(i);
07161 tmp[i-1].phi1 = phi(i);
07162 tmp[i-1].key1 = key(i);
07163 }
07164
07165 if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07166 if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07167
07168 for(i = 1;i<=len;i++)
07169 {
07170 theta(i) = tmp[i-1].theta1;
07171 phi(i) = tmp[i-1].phi1;
07172 key(i) = tmp[i-1].key1;
07173 }
07174 EXITFUNC;
07175 }
07176
07177 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07178 {
07179 return(tmp1.theta1 < tmp2.theta1);
07180 }
07181
07182 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07183 {
07184 return(tmp1.key1 < tmp2.key1);
07185 }
07186
07187
07188
07189
07190
07191
07192
07193
07194
07195
07196
07197
07198
07199
07200
07201
07202
07203
07204
07205
07206
07207
07208
07209
07210
07211
07212
07213
07214
07215
07216
07217
07218
07219
07220
07221
07222
07223
07224
07225
07226
07227
07228
07229
07230
07231
07232
07233
07234
07235
07236
07237
07238
07239
07240
07241
07242
07243
07244
07245
07246
07247
07248
07249
07250
07251
07252
07253
07254
07255
07256
07257
07258
07259
07260
07261
07262
07263
07264
07265
07266
07267
07268
07269
07270
07271
07272
07273
07274
07275
07276
07277
07278
07279
07280
07281
07282
07283
07284
07285
07286
07287
07288
07289
07290
07291
07292
07293
07294
07295
07296
07297
07298
07299
07300
07301
07302
07303
07304
07305
07306
07307
07308
07309
07310
07311
07312
07313
07314
07315
07316
07317
07318
07319
07320
07321
07322
07323
07324
07325
07326
07327
07328
07329
07330
07331
07332
07333
07334
07335
07336
07337
07338
07339
07340
07341
07342
07343
07344
07345
07346
07347
07348
07349
07350
07351
07352
07353
07354
07355
07356
07357
07358
07359
07360
07361
07362
07363
07364
07365
07366
07367
07368
07369
07370
07371
07372
07373
07374
07375
07376
07377
07378
07379
07380
07381
07382
07383
07384
07385
07386
07387
07388
07389
07390
07391
07392
07393
07394
07395
07396
07397
07398
07399
07400
07401
07402
07403
07404
07405
07406
07407
07408
07409
07410
07411
07412
07413
07414
07415
07416
07417
07418
07419
07420
07421
07422
07423
07424
07425
07426
07427
07428
07429
07430
07431
07432
07433
07434
07435
07436
07437
07438
07439
07440
07441
07442
07443
07444
07445
07446
07447
07448
07449
07450
07451
07452
07453
07454
07455
07456
07457
07458
07459
07460
07461
07462
07463
07464
07465
07466
07467
07468
07469
07470
07471
07472
07473
07474
07475
07476
07477
07478
07479
07480
07481
07482
07483
07484
07485
07486
07487
07488
07489
07490
07491
07492
07493
07494
07495
07496
07497
07498
07499
07500
07501
07502
07503
07504
07505 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07506 {
07507
07508 ENTERFUNC;
07509
07510 int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07511 int nt6, n, ier, nout, lnew, mdup, nd;
07512 int i,k,mt,status;
07513
07514
07515 double *ds, *x, *y, *z;
07516 double tol = 1.0e-8;
07517 double dtol = 15;
07518 double a;
07519
07520
07521
07522
07523
07524
07525
07526
07527
07528 n = nt + nt;
07529
07530 nt6 = n*6;
07531
07532 list = (int*)calloc(nt6,sizeof(int));
07533 lptr = (int*)calloc(nt6,sizeof(int));
07534 lend = (int*)calloc(n ,sizeof(int));
07535 iwk = (int*)calloc(n ,sizeof(int));
07536 good = (int*)calloc(n ,sizeof(int));
07537 key = (int*)calloc(n ,sizeof(int));
07538 indx = (int*)calloc(n ,sizeof(int));
07539 lcnt = (int*)calloc(n ,sizeof(int));
07540
07541 ds = (double*) calloc(n,sizeof(double));
07542 x = (double*) calloc(n,sizeof(double));
07543 y = (double*) calloc(n,sizeof(double));
07544 z = (double*) calloc(n,sizeof(double));
07545
07546 if (list == NULL ||
07547 lptr == NULL ||
07548 lend == NULL ||
07549 iwk == NULL ||
07550 good == NULL ||
07551 key == NULL ||
07552 indx == NULL ||
07553 lcnt == NULL ||
07554 x == NULL ||
07555 y == NULL ||
07556 z == NULL ||
07557 ds == NULL) {
07558 printf("memory allocation failure!\n");
07559 exit(1);
07560 }
07561
07562 bool colinear=true;
07563 while(colinear)
07564 {
07565
07566 L1:
07567 for(i = 0; i<nt; i++){
07568 x[i] = theta[i];
07569 y[i] = phi[i];
07570 x[nt+i] = 180.0 - x[i];
07571 y[nt+i] = 180.0 + y[i];
07572 }
07573
07574 Util::disorder2(x, y, key, n);
07575
07576
07577 double val;
07578 for(k=0; k<2; k++){
07579 for(i=k+1; i<3; i++){
07580 val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07581 if( val < dtol) {
07582 goto L1;
07583 }
07584 }
07585 }
07586
07587 Util::ang_to_xyz(x, y, z, n);
07588
07589
07590 bool dupnode=true;
07591 dupnode=true;
07592 while(dupnode)
07593 {
07594 for(k=0; k<2; k++){
07595 for(i=k+1; i<3; i++){
07596 if( x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07597 Util::flip23(x, y, z, key, k, n);
07598 continue;
07599 }
07600 }
07601 }
07602 dupnode = false;
07603 }
07604
07605
07606 ier = 0;
07607
07608 status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07609
07610 if (status != 0) {
07611 printf(" error in trmsh3 \n");
07612 exit(1);
07613 }
07614
07615 if (ier > 0) {
07616 printf("*** Error in TRMESH: duplicate nodes encountered ***\n");
07617 exit(1);
07618 }
07619
07620 mdup=n-nout;
07621 if (ier == -2) {
07622
07623 }
07624 else
07625 {
07626 colinear=false;
07627 }
07628 }
07629
07630
07631 Assert( ier != -2 );
07632
07633
07634 nd=0;
07635 for (k=1; k<=n; k++){
07636 if (indx[k-1]>0) {
07637 nd++;
07638 good[nd-1]=k;
07639 }
07640 }
07641
07642
07643
07644
07645 for(i = 1; i<=nout; i++) {
07646 k=good[i-1];
07647
07648 if (key[k-1] <= nt) {
07649
07650 a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07651 if (ier != 0){
07652
07653
07654 printf(" *** error in areav: ier = %d ***\n", ier);
07655 weight[key[k-1]-1] =-1.0;
07656 } else {
07657
07658 weight[key[k-1]-1]=a/lcnt[i-1];
07659 }
07660 }
07661 }
07662
07663
07664
07665 for(i = 1; i<=n; i++){
07666 mt =- indx[i-1];
07667 if (mt>0){
07668 k = good[mt-1];
07669
07670
07671
07672 if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07673 }
07674 }
07675
07676 free(list);
07677 free(lend);
07678 free(iwk);
07679 free(good);
07680 free(key);
07681 free(lptr);
07682 free(indx);
07683 free(lcnt);
07684 free(ds);
07685 free(x);
07686 free(y);
07687 free(z);
07688
07689
07690 EXITFUNC;
07691 }
07692
07693 void Util::disorder2(double *x,double *y, int *key, int len)
07694 {
07695 ENTERFUNC;
07696 int k, i;
07697 for(i=0; i<len; i++) key[i]=i+1;
07698
07699 for(i = 0; i<len;i++){
07700 k = rand()%len;
07701 std::swap(key[k], key[i]);
07702 std::swap(x[k], x[i]);
07703 std::swap(y[k], y[i]);
07704 }
07705 EXITFUNC;
07706 }
07707
07708 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07709 {
07710 ENTERFUNC;
07711 double costheta,sintheta,cosphi,sinphi;
07712 for(int i = 0; i<len; i++) {
07713 cosphi = cos(y[i]*dgr_to_rad);
07714 sinphi = sin(y[i]*dgr_to_rad);
07715 if(fabs(x[i]-90.0)< 1.0e-5){
07716 x[i] = cosphi;
07717 y[i] = sinphi;
07718 z[i] = 0.0;
07719 } else {
07720 costheta = cos(x[i]*dgr_to_rad);
07721 sintheta = sin(x[i]*dgr_to_rad);
07722 x[i] = cosphi*sintheta;
07723 y[i] = sinphi*sintheta;
07724 z[i] = costheta;
07725 }
07726 }
07727 EXITFUNC;
07728 }
07729
07730 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07731 {
07732 ENTERFUNC;
07733 int i = k;
07734 while( i == k ) i = rand()%len;
07735 std::swap(key[i], key[k]);
07736 std::swap(x[i], x[k]);
07737 std::swap(y[i], y[k]);
07738 std::swap(z[i], z[k]);
07739 EXITFUNC;
07740 }
07741
07742
07743 #undef mymax
07744 #undef mymin
07745 #undef sign
07746 #undef quadpi
07747 #undef dgr_to_rad
07748 #undef deg_to_rad
07749 #undef rad_to_deg
07750 #undef rad_to_dgr
07751 #undef TRUE
07752 #undef FALSE
07753 #undef theta
07754 #undef phi
07755 #undef weight
07756 #undef lband
07757 #undef ts
07758 #undef thetast
07759 #undef key
07760
07761
07762
07763
07764
07765
07766
07767
07768
07769
07770
07771 #define TRUE_ (1)
07772 #define FALSE_ (0)
07773 #define abs(x) ((x) >= 0 ? (x) : -(x))
07774
07775 struct stcom_{
07776 double y;
07777 };
07778 stcom_ stcom_1;
07779 #ifdef KR_headers
07780 double floor();
07781 int i_dnnt(x) double *x;
07782 #else
07783 int i_dnnt(double *x)
07784 #endif
07785 {
07786 return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07787 }
07788
07789
07790
07791
07792
07793 int Util::trmsh3_(int *n0, double *tol, double *x,
07794 double *y, double *z__, int *n, int *list, int *
07795 lptr, int *lend, int *lnew, int *indx, int *lcnt,
07796 int *near__, int *next, double *dist, int *ier)
07797 {
07798
07799 int i__1, i__2;
07800
07801
07802 static double d__;
07803 static int i__, j;
07804 static double d1, d2, d3;
07805 static int i0, lp, kt, ku, lpl, nku;
07806 static int nexti;
07807
07808
07809
07810
07811
07812
07813
07814
07815
07816
07817
07818
07819
07820
07821
07822
07823
07824
07825
07826
07827
07828
07829
07830
07831
07832
07833
07834
07835
07836
07837
07838
07839
07840
07841
07842
07843
07844
07845
07846
07847
07848
07849
07850
07851
07852
07853
07854
07855
07856
07857
07858
07859
07860
07861
07862
07863
07864
07865
07866
07867
07868
07869
07870
07871
07872
07873
07874
07875
07876
07877
07878
07879
07880
07881
07882
07883
07884
07885
07886
07887
07888
07889
07890
07891
07892
07893
07894
07895
07896
07897
07898
07899
07900
07901
07902
07903
07904
07905
07906
07907
07908
07909
07910
07911
07912
07913
07914
07915
07916
07917
07918
07919
07920
07921
07922
07923
07924
07925
07926
07927
07928
07929
07930
07931
07932
07933
07934
07935
07936
07937
07938
07939
07940
07941
07942
07943
07944
07945
07946
07947
07948
07949
07950
07951
07952
07953
07954
07955
07956
07957
07958 --dist;
07959 --next;
07960 --near__;
07961 --indx;
07962 --lend;
07963 --z__;
07964 --y;
07965 --x;
07966 --list;
07967 --lptr;
07968 --lcnt;
07969
07970
07971 if (*n0 < 3) {
07972 *n = 0;
07973 *ier = -1;
07974 return 0;
07975 }
07976
07977
07978
07979 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
07980 z__[3])) {
07981
07982
07983
07984 list[1] = 3;
07985 lptr[1] = 2;
07986 list[2] = -2;
07987 lptr[2] = 1;
07988 lend[1] = 2;
07989
07990 list[3] = 1;
07991 lptr[3] = 4;
07992 list[4] = -3;
07993 lptr[4] = 3;
07994 lend[2] = 4;
07995
07996 list[5] = 2;
07997 lptr[5] = 6;
07998 list[6] = -1;
07999 lptr[6] = 5;
08000 lend[3] = 6;
08001
08002 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
08003 y[3], &z__[3])) {
08004
08005
08006
08007
08008
08009 list[1] = 2;
08010 lptr[1] = 2;
08011 list[2] = -3;
08012 lptr[2] = 1;
08013 lend[1] = 2;
08014
08015 list[3] = 3;
08016 lptr[3] = 4;
08017 list[4] = -1;
08018 lptr[4] = 3;
08019 lend[2] = 4;
08020
08021 list[5] = 1;
08022 lptr[5] = 6;
08023 list[6] = -2;
08024 lptr[6] = 5;
08025 lend[3] = 6;
08026
08027
08028 } else {
08029
08030
08031
08032 *n = 0;
08033 *ier = -2;
08034 return 0;
08035 }
08036
08037
08038
08039
08040
08041 *lnew = 7;
08042 indx[1] = 1;
08043 indx[2] = 2;
08044 indx[3] = 3;
08045 lcnt[1] = 1;
08046 lcnt[2] = 1;
08047 lcnt[3] = 1;
08048 if (*n0 == 3) {
08049 *n = 3;
08050 *ier = 0;
08051 return 0;
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 near__[1] = 0;
08081 near__[2] = 0;
08082 near__[3] = 0;
08083 for (ku = *n0; ku >= 4; --ku) {
08084 d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08085 d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08086 d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08087 if (d1 <= d2 && d1 <= d3) {
08088 near__[ku] = 1;
08089 dist[ku] = d1;
08090 next[ku] = near__[1];
08091 near__[1] = ku;
08092 } else if (d2 <= d1 && d2 <= d3) {
08093 near__[ku] = 2;
08094 dist[ku] = d2;
08095 next[ku] = near__[2];
08096 near__[2] = ku;
08097 } else {
08098 near__[ku] = 3;
08099 dist[ku] = d3;
08100 next[ku] = near__[3];
08101 near__[3] = ku;
08102 }
08103
08104 }
08105
08106
08107
08108
08109 kt = 3;
08110 i__1 = *n0;
08111 for (ku = 4; ku <= i__1; ++ku) {
08112 nku = near__[ku];
08113
08114
08115
08116 i__ = nku;
08117 if (near__[i__] == ku) {
08118 near__[i__] = next[ku];
08119 } else {
08120 i__ = near__[i__];
08121 L2:
08122 i0 = i__;
08123 i__ = next[i0];
08124 if (i__ != ku) {
08125 goto L2;
08126 }
08127 next[i0] = next[ku];
08128 }
08129 near__[ku] = 0;
08130
08131
08132
08133 if (dist[ku] <= *tol - 1.) {
08134 indx[ku] = -nku;
08135 ++lcnt[nku];
08136 goto L6;
08137 }
08138
08139
08140
08141 ++kt;
08142 x[kt] = x[ku];
08143 y[kt] = y[ku];
08144 z__[kt] = z__[ku];
08145 indx[ku] = kt;
08146 lcnt[kt] = 1;
08147 addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08148 , lnew, ier);
08149 if (*ier != 0) {
08150 *n = 0;
08151 *ier = -3;
08152 return 0;
08153 }
08154
08155
08156
08157 lpl = lend[kt];
08158 lp = lpl;
08159 L3:
08160 lp = lptr[lp];
08161 j = (i__2 = list[lp], abs(i__2));
08162
08163
08164
08165
08166
08167
08168
08169 i__ = near__[j];
08170 L4:
08171 if (i__ == 0) {
08172 goto L5;
08173 }
08174 nexti = next[i__];
08175
08176
08177
08178
08179 d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08180 if (d__ < dist[i__]) {
08181
08182
08183
08184
08185
08186 near__[i__] = kt;
08187 dist[i__] = d__;
08188 if (i__ == near__[j]) {
08189 near__[j] = nexti;
08190 } else {
08191 next[i0] = nexti;
08192 }
08193 next[i__] = near__[kt];
08194 near__[kt] = i__;
08195 } else {
08196 i0 = i__;
08197 }
08198
08199
08200
08201 i__ = nexti;
08202 goto L4;
08203
08204
08205
08206 L5:
08207 if (lp != lpl) {
08208 goto L3;
08209 }
08210 L6:
08211 ;
08212 }
08213 *n = kt;
08214 *ier = 0;
08215 return 0;
08216 }
08217
08218
08219 int addnod_(int *nst, int *k, double *x,
08220 double *y, double *z__, int *list, int *lptr, int
08221 *lend, int *lnew, int *ier)
08222 {
08223
08224
08225 static double tol = 0.;
08226
08227
08228 int i__1;
08229
08230
08231 static int l;
08232 static double p[3], b1, b2, b3;
08233 static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08234 int swap_(int *, int *, int *,
08235 int *, int *, int *, int *, int *);
08236 static int lpo1s;
08237 int bdyadd_(int *, int *, int *,
08238 int *, int *, int *, int *), intadd_(int *,
08239 int *, int *, int *, int *, int *, int *,
08240 int *), trfind_(int *, double *, int *,
08241 double *, double *, double *, int *, int *,
08242 int *, double *, double *, double *, int *,
08243 int *, int *), covsph_(int *, int *, int *,
08244 int *, int *, int *);
08245 int lstptr_(int *, int *, int *, int *);
08246 long int swptst_(int *, int *, int *, int *,
08247 double *, double *, double *);
08248
08249
08250
08251
08252
08253
08254
08255
08256
08257
08258
08259
08260
08261
08262
08263
08264
08265
08266
08267
08268
08269
08270
08271
08272
08273
08274
08275
08276
08277
08278
08279
08280
08281
08282
08283
08284
08285
08286
08287
08288
08289
08290
08291
08292
08293
08294
08295
08296
08297
08298
08299
08300
08301
08302
08303
08304
08305
08306
08307
08308
08309
08310
08311
08312
08313
08314
08315
08316
08317
08318
08319
08320
08321
08322
08323
08324
08325
08326
08327
08328
08329
08330
08331
08332
08333
08334
08335
08336
08337
08338
08339
08340
08341
08342
08343
08344
08345
08346
08347
08348
08349
08350 --lend;
08351 --z__;
08352 --y;
08353 --x;
08354 --list;
08355 --lptr;
08356
08357
08358
08359 kk = *k;
08360 if (kk < 4) {
08361 goto L3;
08362 }
08363
08364
08365 km1 = kk - 1;
08366 ist = *nst;
08367 if (ist < 1) {
08368 ist = km1;
08369 }
08370 p[0] = x[kk];
08371 p[1] = y[kk];
08372 p[2] = z__[kk];
08373
08374
08375
08376
08377 trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08378 , &b1, &b2, &b3, &i1, &i2, &i3);
08379
08380
08381
08382 if (i1 == 0) {
08383 goto L4;
08384 }
08385 l = i1;
08386 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08387 goto L5;
08388 }
08389 l = i2;
08390 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08391 goto L5;
08392 }
08393 if (i3 != 0) {
08394 l = i3;
08395 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08396 goto L5;
08397 }
08398 intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08399 } else {
08400 if (i1 != i2) {
08401 bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08402 } else {
08403 covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08404 }
08405 }
08406 *ier = 0;
08407
08408
08409
08410 lp = lend[kk];
08411 lpf = lptr[lp];
08412 io2 = list[lpf];
08413 lpo1 = lptr[lpf];
08414 io1 = (i__1 = list[lpo1], abs(i__1));
08415
08416
08417
08418 L1:
08419 lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08420 if (list[lp] < 0) {
08421 goto L2;
08422 }
08423 lp = lptr[lp];
08424 in1 = (i__1 = list[lp], abs(i__1));
08425
08426
08427
08428
08429 lpo1s = lpo1;
08430 if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08431 goto L2;
08432 }
08433 swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08434 if (lpo1 == 0) {
08435
08436
08437
08438
08439
08440
08441 lpo1 = lpo1s;
08442 goto L2;
08443 }
08444 io1 = in1;
08445 goto L1;
08446
08447
08448
08449
08450 L2:
08451 if (lpo1 == lpf || list[lpo1] < 0) {
08452 return 0;
08453 }
08454 io2 = io1;
08455 lpo1 = lptr[lpo1];
08456 io1 = (i__1 = list[lpo1], abs(i__1));
08457 goto L1;
08458
08459
08460
08461 L3:
08462 *ier = -1;
08463 return 0;
08464
08465
08466
08467 L4:
08468 *ier = -2;
08469 return 0;
08470
08471
08472
08473 L5:
08474 *ier = l;
08475 return 0;
08476 }
08477
08478 double angle_(double *v1, double *v2, double *v3)
08479 {
08480
08481 double ret_val;
08482
08483
08484
08485
08486
08487 static double a;
08488 static int i__;
08489 static double ca, s21, s23, u21[3], u23[3];
08490
08491
08492
08493
08494
08495
08496
08497
08498
08499
08500
08501
08502
08503
08504
08505
08506
08507
08508
08509
08510
08511
08512
08513
08514
08515
08516
08517
08518
08519
08520
08521
08522
08523
08524
08525
08526
08527
08528
08529
08530
08531
08532
08533
08534
08535
08536
08537
08538
08539
08540
08541
08542
08543
08544
08545
08546
08547
08548
08549 --v3;
08550 --v2;
08551 --v1;
08552
08553
08554 u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08555 u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08556 u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08557
08558 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08559 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08560 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08561
08562
08563
08564 s21 = 0.;
08565 s23 = 0.;
08566 for (i__ = 1; i__ <= 3; ++i__) {
08567 s21 += u21[i__ - 1] * u21[i__ - 1];
08568 s23 += u23[i__ - 1] * u23[i__ - 1];
08569
08570 }
08571
08572
08573
08574
08575 if (s21 == 0. || s23 == 0.) {
08576 ret_val = 0.;
08577 return ret_val;
08578 }
08579 s21 = sqrt(s21);
08580 s23 = sqrt(s23);
08581 for (i__ = 1; i__ <= 3; ++i__) {
08582 u21[i__ - 1] /= s21;
08583 u23[i__ - 1] /= s23;
08584
08585 }
08586
08587
08588
08589
08590
08591 ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08592 if (ca < -1.) {
08593 ca = -1.;
08594 }
08595 if (ca > 1.) {
08596 ca = 1.;
08597 }
08598 a = acos(ca);
08599
08600
08601
08602
08603 if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08604 , &v3[3])) {
08605 a = acos(-1.) * 2. - a;
08606 }
08607 ret_val = a;
08608 return ret_val;
08609 }
08610
08611 double areas_(double *v1, double *v2, double *v3)
08612 {
08613
08614 double ret_val;
08615
08616
08617
08618
08619
08620 static int i__;
08621 static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08622 ca2, ca3;
08623
08624
08625
08626
08627
08628
08629
08630
08631
08632
08633
08634
08635
08636
08637
08638
08639
08640
08641
08642
08643
08644
08645
08646
08647
08648
08649
08650
08651
08652
08653
08654
08655
08656
08657
08658
08659
08660
08661
08662
08663
08664
08665
08666
08667
08668
08669
08670
08671
08672
08673
08674
08675
08676 --v3;
08677 --v2;
08678 --v1;
08679
08680
08681 u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08682 u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08683 u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08684
08685 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08686 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08687 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08688
08689 u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08690 u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08691 u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08692
08693
08694
08695 s12 = 0.;
08696 s23 = 0.;
08697 s31 = 0.;
08698 for (i__ = 1; i__ <= 3; ++i__) {
08699 s12 += u12[i__ - 1] * u12[i__ - 1];
08700 s23 += u23[i__ - 1] * u23[i__ - 1];
08701 s31 += u31[i__ - 1] * u31[i__ - 1];
08702
08703 }
08704
08705
08706
08707
08708 if (s12 == 0. || s23 == 0. || s31 == 0.) {
08709 ret_val = 0.;
08710 return ret_val;
08711 }
08712 s12 = sqrt(s12);
08713 s23 = sqrt(s23);
08714 s31 = sqrt(s31);
08715 for (i__ = 1; i__ <= 3; ++i__) {
08716 u12[i__ - 1] /= s12;
08717 u23[i__ - 1] /= s23;
08718 u31[i__ - 1] /= s31;
08719
08720 }
08721
08722
08723
08724
08725
08726
08727
08728 ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08729 ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08730 ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08731 if (ca1 < -1.) {
08732 ca1 = -1.;
08733 }
08734 if (ca1 > 1.) {
08735 ca1 = 1.;
08736 }
08737 if (ca2 < -1.) {
08738 ca2 = -1.;
08739 }
08740 if (ca2 > 1.) {
08741 ca2 = 1.;
08742 }
08743 if (ca3 < -1.) {
08744 ca3 = -1.;
08745 }
08746 if (ca3 > 1.) {
08747 ca3 = 1.;
08748 }
08749 a1 = acos(ca1);
08750 a2 = acos(ca2);
08751 a3 = acos(ca3);
08752
08753
08754
08755 ret_val = a1 + a2 + a3 - acos(-1.);
08756 if (ret_val < 0.) {
08757 ret_val = 0.;
08758 }
08759 return ret_val;
08760 }
08761
08762
08763
08764 double Util::areav_(int *k, int *n, double *x, double *y,
08765 double *z__, int *list, int *lptr, int *lend, int
08766 *ier)
08767 {
08768
08769
08770 static double amax = 6.28;
08771
08772
08773 double ret_val;
08774
08775
08776 static double a, c0[3], c2[3], c3[3];
08777 static int n1, n2, n3;
08778 static double v1[3], v2[3], v3[3];
08779 static int lp, lpl, ierr;
08780 static double asum;
08781 static long int first;
08782
08783
08784
08785
08786
08787
08788
08789
08790
08791
08792
08793
08794
08795
08796
08797
08798
08799
08800
08801
08802
08803
08804
08805
08806
08807
08808
08809
08810
08811
08812
08813
08814
08815
08816
08817
08818
08819
08820
08821
08822
08823
08824
08825
08826
08827
08828
08829
08830
08831
08832
08833
08834
08835
08836
08837
08838
08839
08840 --lend;
08841 --z__;
08842 --y;
08843 --x;
08844 --list;
08845 --lptr;
08846
08847
08848
08849
08850
08851 if (*k < 1 || *k > *n || *n <= 3) {
08852 goto L11;
08853 }
08854
08855
08856
08857
08858
08859 n1 = *k;
08860 v1[0] = x[n1];
08861 v1[1] = y[n1];
08862 v1[2] = z__[n1];
08863 lpl = lend[n1];
08864 n3 = list[lpl];
08865 if (n3 < 0) {
08866 goto L12;
08867 }
08868 lp = lpl;
08869 first = TRUE_;
08870 asum = 0.;
08871
08872
08873
08874 L1:
08875 n2 = n3;
08876 lp = lptr[lp];
08877 n3 = list[lp];
08878 v2[0] = x[n2];
08879 v2[1] = y[n2];
08880 v2[2] = z__[n2];
08881 v3[0] = x[n3];
08882 v3[1] = y[n3];
08883 v3[2] = z__[n3];
08884 if (first) {
08885
08886
08887
08888
08889 circum_(v1, v2, v3, c3, &ierr);
08890 if (ierr != 0) {
08891 goto L13;
08892 }
08893 c0[0] = c3[0];
08894 c0[1] = c3[1];
08895 c0[2] = c3[2];
08896 first = FALSE_;
08897 } else {
08898
08899
08900
08901
08902 c2[0] = c3[0];
08903 c2[1] = c3[1];
08904 c2[2] = c3[2];
08905 circum_(v1, v2, v3, c3, &ierr);
08906 if (ierr != 0) {
08907 goto L13;
08908 }
08909 a = areas_(v1, c2, c3);
08910 if (a > amax) {
08911 goto L14;
08912 }
08913 asum += a;
08914 }
08915
08916
08917
08918 if (lp != lpl) {
08919 goto L1;
08920 }
08921
08922
08923
08924 a = areas_(v1, c3, c0);
08925 if (a > amax) {
08926 goto L14;
08927 }
08928 asum += a;
08929
08930
08931
08932 *ier = 0;
08933 ret_val = asum;
08934 return ret_val;
08935
08936
08937
08938 L11:
08939 *ier = 1;
08940 ret_val = 0.;
08941 return ret_val;
08942
08943
08944
08945 L12:
08946 *ier = 2;
08947 ret_val = 0.;
08948 return ret_val;
08949
08950
08951
08952 L13:
08953 *ier = 3;
08954 ret_val = 0.;
08955 return ret_val;
08956
08957
08958
08959 L14:
08960 *ier = 4;
08961 ret_val = 0.;
08962 return ret_val;
08963 }
08964
08965 double areav_new__(int *k, int *n, double *x, double *y,
08966 double *z__, int *list, int *lptr, int *lend, int
08967 *ier)
08968 {
08969
08970 double ret_val = 0;
08971
08972
08973
08974
08975
08976 static int m;
08977 static double c1[3], c2[3], c3[3];
08978 static int n1, n2, n3;
08979 static double v1[3], v2[3], v3[3];
08980 static int lp;
08981 static double c1s[3], c2s[3];
08982 static int lpl, ierr;
08983 static double asum;
08984 double angle_(double *, double *, double *);
08985 static float areav;
08986
08987
08988
08989
08990
08991
08992
08993
08994
08995
08996
08997
08998
08999
09000
09001
09002
09003
09004
09005
09006
09007
09008
09009
09010
09011
09012
09013
09014
09015
09016
09017
09018
09019
09020
09021
09022
09023
09024
09025
09026
09027
09028
09029
09030
09031
09032
09033
09034
09035
09036
09037
09038
09039
09040
09041
09042
09043
09044
09045
09046
09047 --lend;
09048 --z__;
09049 --y;
09050 --x;
09051 --list;
09052 --lptr;
09053
09054
09055 if (*k < 1 || *k > *n || *n <= 3) {
09056 goto L11;
09057 }
09058
09059
09060
09061
09062
09063 n1 = *k;
09064 v1[0] = x[n1];
09065 v1[1] = y[n1];
09066 v1[2] = z__[n1];
09067 lpl = lend[n1];
09068 n3 = list[lpl];
09069 if (n3 < 0) {
09070 goto L12;
09071 }
09072 lp = lpl;
09073 m = 0;
09074 asum = 0.;
09075
09076
09077
09078 L1:
09079 ++m;
09080 n2 = n3;
09081 lp = lptr[lp];
09082 n3 = list[lp];
09083 v2[0] = x[n2];
09084 v2[1] = y[n2];
09085 v2[2] = z__[n2];
09086 v3[0] = x[n3];
09087 v3[1] = y[n3];
09088 v3[2] = z__[n3];
09089 if (m == 1) {
09090
09091
09092
09093
09094 circum_(v1, v2, v3, c2, &ierr);
09095 if (ierr != 0) {
09096 goto L13;
09097 }
09098 c1s[0] = c2[0];
09099 c1s[1] = c2[1];
09100 c1s[2] = c2[2];
09101 } else if (m == 2) {
09102
09103
09104
09105
09106 circum_(v1, v2, v3, c3, &ierr);
09107 if (ierr != 0) {
09108 goto L13;
09109 }
09110 c2s[0] = c3[0];
09111 c2s[1] = c3[1];
09112 c2s[2] = c3[2];
09113 } else {
09114
09115
09116
09117
09118
09119 c1[0] = c2[0];
09120 c1[1] = c2[1];
09121 c1[2] = c2[2];
09122 c2[0] = c3[0];
09123 c2[1] = c3[1];
09124 c2[2] = c3[2];
09125 circum_(v1, v2, v3, c3, &ierr);
09126 if (ierr != 0) {
09127 goto L13;
09128 }
09129 asum += angle_(c1, c2, c3);
09130 }
09131
09132
09133
09134 if (lp != lpl) {
09135 goto L1;
09136 }
09137
09138
09139
09140
09141 asum += angle_(c2, c3, c1s);
09142
09143
09144
09145
09146 asum += angle_(c3, c1s, c2s);
09147
09148
09149
09150 *ier = 0;
09151 ret_val = asum - (double) (m - 2) * acos(-1.);
09152 return ret_val;
09153
09154
09155
09156 L11:
09157 *ier = 1;
09158 areav = 0.f;
09159 return ret_val;
09160
09161
09162
09163 L12:
09164 *ier = 2;
09165 areav = 0.f;
09166 return ret_val;
09167
09168
09169
09170 L13:
09171 *ier = 3;
09172 areav = 0.f;
09173 return ret_val;
09174 }
09175
09176 int bdyadd_(int *kk, int *i1, int *i2, int *
09177 list, int *lptr, int *lend, int *lnew)
09178 {
09179 static int k, n1, n2, lp, lsav, nsav, next;
09180 int insert_(int *, int *, int *,
09181 int *, int *);
09182
09183
09184
09185
09186
09187
09188
09189
09190
09191
09192
09193
09194
09195
09196
09197
09198
09199
09200
09201
09202
09203
09204
09205
09206
09207
09208
09209
09210
09211
09212
09213
09214
09215
09216
09217
09218
09219
09220
09221
09222
09223
09224
09225
09226
09227
09228
09229
09230
09231
09232
09233
09234
09235
09236
09237
09238
09239
09240
09241
09242
09243
09244
09245
09246 --lend;
09247 --lptr;
09248 --list;
09249
09250
09251 k = *kk;
09252 n1 = *i1;
09253 n2 = *i2;
09254
09255
09256
09257 lp = lend[n1];
09258 lsav = lptr[lp];
09259 lptr[lp] = *lnew;
09260 list[*lnew] = -k;
09261 lptr[*lnew] = lsav;
09262 lend[n1] = *lnew;
09263 ++(*lnew);
09264 next = -list[lp];
09265 list[lp] = next;
09266 nsav = next;
09267
09268
09269
09270
09271 L1:
09272 lp = lend[next];
09273 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09274 if (next == n2) {
09275 goto L2;
09276 }
09277 next = -list[lp];
09278 list[lp] = next;
09279 goto L1;
09280
09281
09282
09283
09284 L2:
09285 lsav = *lnew;
09286 list[*lnew] = n1;
09287 lptr[*lnew] = *lnew + 1;
09288 ++(*lnew);
09289 next = nsav;
09290
09291 L3:
09292 if (next == n2) {
09293 goto L4;
09294 }
09295 list[*lnew] = next;
09296 lptr[*lnew] = *lnew + 1;
09297 ++(*lnew);
09298 lp = lend[next];
09299 next = list[lp];
09300 goto L3;
09301
09302 L4:
09303 list[*lnew] = -n2;
09304 lptr[*lnew] = lsav;
09305 lend[k] = *lnew;
09306 ++(*lnew);
09307 return 0;
09308 }
09309
09310 int bnodes_(int *n, int *list, int *lptr,
09311 int *lend, int *nodes, int *nb, int *na, int *nt)
09312 {
09313
09314 int i__1;
09315
09316
09317 static int k, n0, lp, nn, nst;
09318
09319
09320
09321
09322
09323
09324
09325
09326
09327
09328
09329
09330
09331
09332
09333
09334
09335
09336
09337
09338
09339
09340
09341
09342
09343
09344
09345
09346
09347
09348
09349
09350
09351
09352
09353
09354
09355
09356
09357
09358
09359
09360
09361
09362
09363
09364
09365
09366
09367
09368
09369
09370
09371
09372
09373
09374
09375
09376
09377
09378 --lend;
09379 --list;
09380 --lptr;
09381 --nodes;
09382
09383
09384 nn = *n;
09385
09386
09387
09388 i__1 = nn;
09389 for (nst = 1; nst <= i__1; ++nst) {
09390 lp = lend[nst];
09391 if (list[lp] < 0) {
09392 goto L2;
09393 }
09394
09395 }
09396
09397
09398
09399 *nb = 0;
09400 *na = (nn - 2) * 3;
09401 *nt = nn - (2<<1);
09402 return 0;
09403
09404
09405
09406
09407 L2:
09408 nodes[1] = nst;
09409 k = 1;
09410 n0 = nst;
09411
09412
09413
09414 L3:
09415 lp = lend[n0];
09416 lp = lptr[lp];
09417 n0 = list[lp];
09418 if (n0 == nst) {
09419 goto L4;
09420 }
09421 ++k;
09422 nodes[k] = n0;
09423 goto L3;
09424
09425
09426
09427 L4:
09428 *nb = k;
09429 *nt = (*n << 1) - *nb - 2;
09430 *na = *nt + *n - 1;
09431 return 0;
09432 }
09433
09434 int circle_(int *k, double *xc, double *yc,
09435 int *ier)
09436 {
09437
09438 int i__1;
09439
09440
09441
09442
09443
09444 static double a, c__;
09445 static int i__;
09446 static double s;
09447 static int k2, k3;
09448 static double x0, y0;
09449 static int kk, np1;
09450
09451
09452
09453
09454
09455
09456
09457
09458
09459
09460
09461
09462
09463
09464
09465
09466
09467
09468
09469
09470
09471
09472
09473
09474
09475
09476
09477
09478
09479
09480
09481
09482
09483
09484
09485
09486
09487
09488
09489
09490
09491
09492
09493
09494
09495
09496
09497
09498
09499
09500
09501
09502
09503
09504
09505
09506
09507
09508
09509
09510
09511
09512
09513
09514 --yc;
09515 --xc;
09516
09517
09518 kk = *k;
09519 k2 = kk << 1;
09520 k3 = kk * 3;
09521 np1 = (kk << 2) + 1;
09522
09523
09524
09525
09526 if (kk < 1) {
09527 goto L2;
09528 }
09529 a = atan(1.) * 2. / (double) kk;
09530 c__ = cos(a);
09531 s = sin(a);
09532 x0 = 1.;
09533 y0 = 0.;
09534
09535
09536
09537
09538
09539 i__1 = kk;
09540 for (i__ = 1; i__ <= i__1; ++i__) {
09541 xc[i__] = x0;
09542 yc[i__] = y0;
09543 xc[i__ + kk] = -y0;
09544 yc[i__ + kk] = x0;
09545 xc[i__ + k2] = -x0;
09546 yc[i__ + k2] = -y0;
09547 xc[i__ + k3] = y0;
09548 yc[i__ + k3] = -x0;
09549
09550
09551
09552 x0 = c__ * x0 - s * y0;
09553 y0 = s * x0 + c__ * y0;
09554
09555 }
09556
09557
09558
09559
09560 xc[np1] = xc[1];
09561 yc[np1] = yc[1];
09562 *ier = 0;
09563 return 0;
09564
09565
09566
09567 L2:
09568 *ier = 1;
09569 return 0;
09570 }
09571
09572 int circum_(double *v1, double *v2, double *v3,
09573 double *c__, int *ier)
09574 {
09575
09576
09577
09578
09579 static int i__;
09580 static double e1[3], e2[3], cu[3], cnorm;
09581
09582
09583
09584
09585
09586
09587
09588
09589
09590
09591
09592
09593
09594
09595
09596
09597
09598
09599
09600
09601
09602
09603
09604
09605
09606
09607
09608
09609
09610
09611
09612
09613
09614
09615
09616
09617
09618
09619
09620
09621
09622
09623
09624
09625
09626
09627
09628
09629
09630
09631
09632
09633
09634
09635
09636
09637 --c__;
09638 --v3;
09639 --v2;
09640 --v1;
09641
09642
09643 for (i__ = 1; i__ <= 3; ++i__) {
09644 e1[i__ - 1] = v2[i__] - v1[i__];
09645 e2[i__ - 1] = v3[i__] - v1[i__];
09646
09647 }
09648
09649
09650
09651 cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09652 cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09653 cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09654 cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09655
09656
09657
09658
09659 if (cnorm != 0.) {
09660
09661
09662
09663 cnorm = sqrt(cnorm);
09664 for (i__ = 1; i__ <= 3; ++i__) {
09665 c__[i__] = cu[i__ - 1] / cnorm;
09666
09667 }
09668
09669
09670
09671
09672
09673
09674 if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09675 c__[1] = -c__[1];
09676 c__[2] = -c__[2];
09677 c__[3] = -c__[3];
09678 }
09679 *ier = 0;
09680 } else {
09681
09682
09683
09684 *ier = 1;
09685 }
09686 return 0;
09687 }
09688
09689 int covsph_(int *kk, int *n0, int *list, int
09690 *lptr, int *lend, int *lnew)
09691 {
09692 static int k, lp, nst, lsav, next;
09693 int insert_(int *, int *, int *,
09694 int *, int *);
09695
09696
09697
09698
09699
09700
09701
09702
09703
09704
09705
09706
09707
09708
09709
09710
09711
09712
09713
09714
09715
09716
09717
09718
09719
09720
09721
09722
09723
09724
09725
09726
09727
09728
09729
09730
09731
09732
09733
09734
09735
09736
09737
09738
09739
09740
09741
09742
09743
09744
09745
09746
09747
09748
09749
09750
09751
09752 --lend;
09753 --lptr;
09754 --list;
09755
09756
09757 k = *kk;
09758 nst = *n0;
09759
09760
09761
09762
09763
09764 next = nst;
09765 L1:
09766 lp = lend[next];
09767 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09768 next = -list[lp];
09769 list[lp] = next;
09770 if (next != nst) {
09771 goto L1;
09772 }
09773
09774
09775
09776
09777 lsav = *lnew;
09778 L2:
09779 lp = lend[next];
09780 list[*lnew] = next;
09781 lptr[*lnew] = *lnew + 1;
09782 ++(*lnew);
09783 next = list[lp];
09784 if (next != nst) {
09785 goto L2;
09786 }
09787
09788 lptr[*lnew - 1] = lsav;
09789 lend[k] = *lnew - 1;
09790 return 0;
09791 }
09792
09793
09794 int crlist_(int *n, int *ncol, double *x,
09795 double *y, double *z__, int *list, int *lend, int
09796 *lptr, int *lnew, int *ltri, int *listc, int *nb,
09797 double *xc, double *yc, double *zc, double *rc,
09798 int *ier)
09799 {
09800
09801 int i__1, i__2;
09802
09803
09804
09805
09806
09807 static double c__[3], t;
09808 static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09809 static double v1[3], v2[3], v3[3];
09810 static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09811 lpn;
09812 static long int swp;
09813 static int ierr;
09814 int lstptr_(int *, int *, int *, int *);
09815 long int swptst_(int *, int *, int *, int *,
09816 double *, double *, double *);
09817
09818
09819
09820
09821
09822
09823
09824
09825
09826
09827
09828
09829
09830
09831
09832
09833
09834
09835
09836
09837
09838
09839
09840
09841
09842
09843
09844
09845
09846
09847
09848
09849
09850
09851
09852
09853
09854
09855
09856
09857
09858
09859
09860
09861
09862
09863
09864
09865
09866
09867
09868
09869
09870
09871
09872
09873
09874
09875
09876
09877
09878
09879
09880
09881
09882
09883
09884
09885
09886
09887
09888
09889
09890
09891
09892
09893
09894
09895
09896
09897
09898
09899
09900
09901
09902
09903
09904
09905
09906
09907
09908
09909
09910
09911
09912
09913
09914
09915
09916
09917
09918
09919
09920
09921
09922
09923
09924
09925
09926
09927
09928
09929
09930
09931
09932
09933
09934
09935
09936
09937
09938
09939
09940
09941
09942
09943
09944
09945
09946
09947
09948
09949
09950
09951
09952
09953
09954
09955
09956
09957
09958
09959
09960
09961
09962
09963
09964
09965
09966
09967
09968
09969
09970
09971
09972
09973
09974
09975
09976
09977
09978
09979
09980
09981
09982
09983
09984
09985
09986
09987
09988
09989
09990
09991
09992
09993 --lend;
09994 --z__;
09995 --y;
09996 --x;
09997 ltri -= 7;
09998 --list;
09999 --lptr;
10000 --listc;
10001 --xc;
10002 --yc;
10003 --zc;
10004 --rc;
10005
10006
10007 nn = *n;
10008 *nb = 0;
10009 nt = 0;
10010 if (nn < 3) {
10011 goto L21;
10012 }
10013
10014
10015
10016 i__1 = nn;
10017 for (n1 = 1; n1 <= i__1; ++n1) {
10018 lp = lend[n1];
10019 if (list[lp] < 0) {
10020 goto L2;
10021 }
10022
10023 }
10024
10025
10026
10027 goto L9;
10028
10029
10030
10031
10032
10033
10034
10035
10036 L2:
10037 n2 = -list[lp];
10038 lp = lptr[lp];
10039 n3 = list[lp];
10040
10041
10042
10043
10044
10045
10046 L3:
10047 ++nt;
10048 if (nt <= *ncol) {
10049 ltri[nt * 6 + 1] = n1;
10050 ltri[nt * 6 + 2] = n2;
10051 ltri[nt * 6 + 3] = n3;
10052 ltri[nt * 6 + 4] = nt + 1;
10053 ltri[nt * 6 + 5] = nt - 1;
10054 ltri[nt * 6 + 6] = 0;
10055 }
10056 n1 = n2;
10057 lp = lend[n1];
10058 n2 = -list[lp];
10059 if (n2 != n3) {
10060 goto L3;
10061 }
10062
10063 *nb = nt + 2;
10064 if (*ncol < nt) {
10065 goto L22;
10066 }
10067 ltri[nt * 6 + 4] = 0;
10068 if (nt == 1) {
10069 goto L7;
10070 }
10071
10072
10073
10074
10075
10076
10077
10078 L4:
10079 swp = FALSE_;
10080 i__1 = nt - 1;
10081 for (kt1 = 1; kt1 <= i__1; ++kt1) {
10082 for (i3 = 1; i3 <= 3; ++i3) {
10083 kt2 = ltri[i3 + 3 + kt1 * 6];
10084 if (kt2 <= kt1) {
10085 goto L5;
10086 }
10087
10088
10089
10090
10091 if (i3 == 1) {
10092 i1 = 2;
10093 i2 = 3;
10094 } else if (i3 == 2) {
10095 i1 = 3;
10096 i2 = 1;
10097 } else {
10098 i1 = 1;
10099 i2 = 2;
10100 }
10101 n1 = ltri[i1 + kt1 * 6];
10102 n2 = ltri[i2 + kt1 * 6];
10103 n3 = ltri[i3 + kt1 * 6];
10104
10105
10106
10107
10108 if (ltri[kt2 * 6 + 4] == kt1) {
10109 i4 = 1;
10110 } else if (ltri[kt2 * 6 + 5] == kt1) {
10111 i4 = 2;
10112 } else {
10113 i4 = 3;
10114 }
10115 n4 = ltri[i4 + kt2 * 6];
10116
10117
10118
10119
10120
10121 if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10122 goto L5;
10123 }
10124
10125
10126
10127
10128 swp = TRUE_;
10129 kt11 = ltri[i1 + 3 + kt1 * 6];
10130 kt12 = ltri[i2 + 3 + kt1 * 6];
10131 if (i4 == 1) {
10132 i2 = 2;
10133 i1 = 3;
10134 } else if (i4 == 2) {
10135 i2 = 3;
10136 i1 = 1;
10137 } else {
10138 i2 = 1;
10139 i1 = 2;
10140 }
10141 kt21 = ltri[i1 + 3 + kt2 * 6];
10142 kt22 = ltri[i2 + 3 + kt2 * 6];
10143 ltri[kt1 * 6 + 1] = n4;
10144 ltri[kt1 * 6 + 2] = n3;
10145 ltri[kt1 * 6 + 3] = n1;
10146 ltri[kt1 * 6 + 4] = kt12;
10147 ltri[kt1 * 6 + 5] = kt22;
10148 ltri[kt1 * 6 + 6] = kt2;
10149 ltri[kt2 * 6 + 1] = n3;
10150 ltri[kt2 * 6 + 2] = n4;
10151 ltri[kt2 * 6 + 3] = n2;
10152 ltri[kt2 * 6 + 4] = kt21;
10153 ltri[kt2 * 6 + 5] = kt11;
10154 ltri[kt2 * 6 + 6] = kt1;
10155
10156
10157
10158 if (kt11 != 0) {
10159 i4 = 4;
10160 if (ltri[kt11 * 6 + 4] != kt1) {
10161 i4 = 5;
10162 if (ltri[kt11 * 6 + 5] != kt1) {
10163 i4 = 6;
10164 }
10165 }
10166 ltri[i4 + kt11 * 6] = kt2;
10167 }
10168 if (kt22 != 0) {
10169 i4 = 4;
10170 if (ltri[kt22 * 6 + 4] != kt2) {
10171 i4 = 5;
10172 if (ltri[kt22 * 6 + 5] != kt2) {
10173 i4 = 6;
10174 }
10175 }
10176 ltri[i4 + kt22 * 6] = kt1;
10177 }
10178 L5:
10179 ;
10180 }
10181
10182 }
10183 if (swp) {
10184 goto L4;
10185 }
10186
10187
10188
10189
10190 L7:
10191 i__1 = nt;
10192 for (kt = 1; kt <= i__1; ++kt) {
10193 n1 = ltri[kt * 6 + 1];
10194 n2 = ltri[kt * 6 + 2];
10195 n3 = ltri[kt * 6 + 3];
10196 v1[0] = x[n1];
10197 v1[1] = y[n1];
10198 v1[2] = z__[n1];
10199 v2[0] = x[n2];
10200 v2[1] = y[n2];
10201 v2[2] = z__[n2];
10202 v3[0] = x[n3];
10203 v3[1] = y[n3];
10204 v3[2] = z__[n3];
10205 circum_(v2, v1, v3, c__, &ierr);
10206 if (ierr != 0) {
10207 goto L23;
10208 }
10209
10210
10211
10212
10213 xc[kt] = -c__[0];
10214 yc[kt] = -c__[1];
10215 zc[kt] = -c__[2];
10216 t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10217 if (t < -1.) {
10218 t = -1.;
10219 }
10220 if (t > 1.) {
10221 t = 1.;
10222 }
10223 rc[kt] = acos(t);
10224
10225 }
10226
10227
10228
10229
10230
10231
10232 L9:
10233 kt = nt;
10234
10235
10236
10237 nm2 = nn - 2;
10238 i__1 = nm2;
10239 for (n1 = 1; n1 <= i__1; ++n1) {
10240 lpl = lend[n1];
10241 lp = lpl;
10242 n3 = list[lp];
10243
10244
10245
10246
10247 L10:
10248 lp = lptr[lp];
10249 n2 = n3;
10250 n3 = (i__2 = list[lp], abs(i__2));
10251 if (n2 <= n1 || n3 <= n1) {
10252 goto L11;
10253 }
10254 ++kt;
10255
10256
10257
10258 v1[0] = x[n1];
10259 v1[1] = y[n1];
10260 v1[2] = z__[n1];
10261 v2[0] = x[n2];
10262 v2[1] = y[n2];
10263 v2[2] = z__[n2];
10264 v3[0] = x[n3];
10265 v3[1] = y[n3];
10266 v3[2] = z__[n3];
10267 circum_(v1, v2, v3, c__, &ierr);
10268 if (ierr != 0) {
10269 goto L23;
10270 }
10271
10272
10273
10274 xc[kt] = c__[0];
10275 yc[kt] = c__[1];
10276 zc[kt] = c__[2];
10277 t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10278 if (t < -1.) {
10279 t = -1.;
10280 }
10281 if (t > 1.) {
10282 t = 1.;
10283 }
10284 rc[kt] = acos(t);
10285
10286
10287
10288
10289
10290 lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10291 listc[lpn] = kt;
10292 lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10293 listc[lpn] = kt;
10294 lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10295 listc[lpn] = kt;
10296 L11:
10297 if (lp != lpl) {
10298 goto L10;
10299 }
10300
10301 }
10302 if (nt == 0) {
10303 goto L20;
10304 }
10305
10306
10307
10308
10309
10310
10311 kt1 = 0;
10312 L13:
10313 ++kt1;
10314 if (ltri[kt1 * 6 + 4] == 0) {
10315 i1 = 2;
10316 i2 = 3;
10317 i3 = 1;
10318 goto L14;
10319 } else if (ltri[kt1 * 6 + 5] == 0) {
10320 i1 = 3;
10321 i2 = 1;
10322 i3 = 2;
10323 goto L14;
10324 } else if (ltri[kt1 * 6 + 6] == 0) {
10325 i1 = 1;
10326 i2 = 2;
10327 i3 = 3;
10328 goto L14;
10329 }
10330 goto L13;
10331 L14:
10332 n1 = ltri[i1 + kt1 * 6];
10333 n0 = n1;
10334
10335
10336
10337
10338
10339
10340
10341
10342
10343 L15:
10344 lp = lend[n1];
10345 lpn = lptr[lp];
10346 listc[lp] = kt1;
10347
10348
10349
10350 L16:
10351 kt2 = ltri[i2 + 3 + kt1 * 6];
10352 if (kt2 != 0) {
10353
10354
10355
10356 lptr[lp] = *lnew;
10357 lp = *lnew;
10358 listc[lp] = kt2;
10359 ++(*lnew);
10360
10361
10362
10363
10364 kt1 = kt2;
10365 if (ltri[kt1 * 6 + 1] == n1) {
10366 i1 = 1;
10367 i2 = 2;
10368 i3 = 3;
10369 } else if (ltri[kt1 * 6 + 2] == n1) {
10370 i1 = 2;
10371 i2 = 3;
10372 i3 = 1;
10373 } else {
10374 i1 = 3;
10375 i2 = 1;
10376 i3 = 2;
10377 }
10378 goto L16;
10379 }
10380
10381
10382
10383
10384
10385
10386
10387 lptr[lp] = lpn;
10388 n1 = ltri[i3 + kt1 * 6];
10389 if (n1 != n0) {
10390 i4 = i3;
10391 i3 = i2;
10392 i2 = i1;
10393 i1 = i4;
10394 goto L15;
10395 }
10396
10397
10398
10399 L20:
10400 *ier = 0;
10401 return 0;
10402
10403
10404
10405 L21:
10406 *ier = 1;
10407 return 0;
10408
10409
10410
10411 L22:
10412 *ier = 2;
10413 return 0;
10414
10415
10416
10417 L23:
10418 *ier = 3;
10419 return 0;
10420 }
10421
10422 int delarc_(int *n, int *io1, int *io2, int *
10423 list, int *lptr, int *lend, int *lnew, int *ier)
10424 {
10425
10426 int i__1;
10427
10428
10429 static int n1, n2, n3, lp, lph, lpl;
10430 int delnb_(int *, int *, int *,
10431 int *, int *, int *, int *, int *);
10432 int lstptr_(int *, int *, int *, int *);
10433
10434
10435
10436
10437
10438
10439
10440
10441
10442
10443
10444
10445
10446
10447
10448
10449
10450
10451
10452
10453
10454
10455
10456
10457
10458
10459
10460
10461
10462
10463
10464
10465
10466
10467
10468
10469
10470
10471
10472
10473
10474
10475
10476
10477
10478
10479
10480
10481
10482
10483
10484
10485
10486
10487
10488
10489
10490
10491
10492
10493
10494
10495
10496
10497
10498
10499
10500
10501
10502
10503
10504
10505
10506 --lend;
10507 --list;
10508 --lptr;
10509
10510
10511 n1 = *io1;
10512 n2 = *io2;
10513
10514
10515
10516
10517
10518 if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10519 *ier = 1;
10520 return 0;
10521 }
10522
10523 lpl = lend[n2];
10524 if (-list[lpl] != n1) {
10525 n1 = n2;
10526 n2 = *io1;
10527 lpl = lend[n2];
10528 if (-list[lpl] != n1) {
10529 *ier = 2;
10530 return 0;
10531 }
10532 }
10533
10534
10535
10536
10537
10538 lpl = lend[n1];
10539 lp = lptr[lpl];
10540 lp = lptr[lp];
10541 n3 = (i__1 = list[lp], abs(i__1));
10542 lpl = lend[n3];
10543 if (list[lpl] <= 0) {
10544 *ier = 3;
10545 return 0;
10546 }
10547
10548
10549
10550
10551
10552
10553 delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10554 if (lph < 0) {
10555 *ier = 4;
10556 return 0;
10557 }
10558
10559
10560
10561
10562 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10563
10564
10565
10566
10567 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10568 lend[n3] = lp;
10569 list[lp] = -n1;
10570
10571
10572
10573 *ier = 0;
10574 return 0;
10575 }
10576
10577 int delnb_(int *n0, int *nb, int *n, int *
10578 list, int *lptr, int *lend, int *lnew, int *lph)
10579 {
10580
10581 int i__1;
10582
10583
10584 static int i__, lp, nn, lpb, lpl, lpp, lnw;
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 --lend;
10657 --list;
10658 --lptr;
10659
10660
10661 nn = *n;
10662
10663
10664
10665 if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10666 *lph = -1;
10667 return 0;
10668 }
10669
10670
10671
10672
10673
10674
10675
10676 lpl = lend[*n0];
10677 lpp = lpl;
10678 lpb = lptr[lpp];
10679 L1:
10680 if (list[lpb] == *nb) {
10681 goto L2;
10682 }
10683 lpp = lpb;
10684 lpb = lptr[lpp];
10685 if (lpb != lpl) {
10686 goto L1;
10687 }
10688
10689
10690
10691 if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10692 *lph = -2;
10693 return 0;
10694 }
10695
10696
10697
10698
10699
10700 lend[*n0] = lpp;
10701 lp = lend[*nb];
10702 if (list[lp] < 0) {
10703 list[lpp] = -list[lpp];
10704 }
10705 goto L3;
10706
10707
10708
10709
10710
10711 L2:
10712 lp = lend[*nb];
10713 if (list[lp] < 0 && list[lpl] > 0) {
10714 lend[*n0] = lpp;
10715 list[lpp] = -list[lpp];
10716 }
10717
10718
10719
10720
10721 L3:
10722 lptr[lpp] = lptr[lpb];
10723 lnw = *lnew - 1;
10724 list[lpb] = list[lnw];
10725 lptr[lpb] = lptr[lnw];
10726 for (i__ = nn; i__ >= 1; --i__) {
10727 if (lend[i__] == lnw) {
10728 lend[i__] = lpb;
10729 goto L5;
10730 }
10731
10732 }
10733
10734 L5:
10735 i__1 = lnw - 1;
10736 for (i__ = 1; i__ <= i__1; ++i__) {
10737 if (lptr[i__] == lnw) {
10738 lptr[i__] = lpb;
10739 }
10740
10741 }
10742
10743
10744
10745 *lnew = lnw;
10746 *lph = lpb;
10747 return 0;
10748 }
10749
10750 int delnod_(int *k, int *n, double *x,
10751 double *y, double *z__, int *list, int *lptr, int
10752 *lend, int *lnew, int *lwk, int *iwk, int *ier)
10753 {
10754
10755 int i__1;
10756
10757
10758 static int i__, j, n1, n2;
10759 static double x1, x2, y1, y2, z1, z2;
10760 static int nl, lp, nn, nr;
10761 static double xl, yl, zl, xr, yr, zr;
10762 static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10763 static long int bdry;
10764 static int ierr, lwkl;
10765 int swap_(int *, int *, int *,
10766 int *, int *, int *, int *, int *), delnb_(
10767 int *, int *, int *, int *, int *, int *,
10768 int *, int *);
10769 int nbcnt_(int *, int *);
10770 int optim_(double *, double *, double
10771 *, int *, int *, int *, int *, int *, int
10772 *, int *);
10773 static int nfrst;
10774 int lstptr_(int *, int *, int *, int *);
10775
10776
10777
10778
10779
10780
10781
10782
10783
10784
10785
10786
10787
10788
10789
10790
10791
10792
10793
10794
10795
10796
10797
10798
10799
10800
10801
10802
10803
10804
10805
10806
10807
10808
10809
10810
10811
10812
10813
10814
10815
10816
10817
10818
10819
10820
10821
10822
10823
10824
10825
10826
10827
10828
10829
10830
10831
10832
10833
10834
10835
10836
10837
10838
10839
10840
10841
10842
10843
10844
10845
10846
10847
10848
10849
10850
10851
10852
10853
10854
10855
10856
10857
10858
10859
10860
10861
10862
10863
10864
10865
10866
10867
10868
10869
10870
10871
10872
10873
10874
10875
10876
10877
10878
10879
10880
10881
10882
10883
10884
10885
10886
10887
10888
10889
10890
10891
10892
10893
10894
10895
10896
10897
10898
10899
10900
10901
10902
10903
10904
10905
10906
10907
10908
10909
10910
10911
10912
10913
10914
10915
10916
10917
10918 iwk -= 3;
10919 --lend;
10920 --lptr;
10921 --list;
10922 --z__;
10923 --y;
10924 --x;
10925
10926
10927 n1 = *k;
10928 nn = *n;
10929 if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
10930 goto L21;
10931 }
10932 lpl = lend[n1];
10933 lpf = lptr[lpl];
10934 nnb = nbcnt_(&lpl, &lptr[1]);
10935 bdry = list[lpl] < 0;
10936 if (bdry) {
10937 ++nnb;
10938 }
10939 if (nnb < 3) {
10940 goto L23;
10941 }
10942 lwkl = *lwk;
10943 *lwk = nnb - 3;
10944 if (lwkl < *lwk) {
10945 goto L22;
10946 }
10947 iwl = 0;
10948 if (nnb == 3) {
10949 goto L3;
10950 }
10951
10952
10953
10954
10955
10956
10957
10958 x1 = x[n1];
10959 y1 = y[n1];
10960 z1 = z__[n1];
10961 nfrst = list[lpf];
10962 nr = nfrst;
10963 xr = x[nr];
10964 yr = y[nr];
10965 zr = z__[nr];
10966 lp = lptr[lpf];
10967 n2 = list[lp];
10968 x2 = x[n2];
10969 y2 = y[n2];
10970 z2 = z__[n2];
10971 lp = lptr[lp];
10972
10973
10974
10975 L1:
10976 nl = (i__1 = list[lp], abs(i__1));
10977 if (nl == nfrst && bdry) {
10978 goto L3;
10979 }
10980 xl = x[nl];
10981 yl = y[nl];
10982 zl = z__[nl];
10983
10984
10985
10986
10987
10988
10989 lpl2 = lend[n2];
10990 if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
10991 list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
10992 z2)))) {
10993
10994
10995
10996 nr = n2;
10997 xr = x2;
10998 yr = y2;
10999 zr = z2;
11000 goto L2;
11001 }
11002
11003
11004
11005
11006
11007
11008
11009
11010 swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11011 if (lp21 == 0) {
11012 nr = n2;
11013 xr = x2;
11014 yr = y2;
11015 zr = z2;
11016 goto L2;
11017 }
11018 ++iwl;
11019 if (nl <= n1) {
11020 iwk[(iwl << 1) + 1] = nl;
11021 } else {
11022 iwk[(iwl << 1) + 1] = nl - 1;
11023 }
11024 if (nr <= n1) {
11025 iwk[(iwl << 1) + 2] = nr;
11026 } else {
11027 iwk[(iwl << 1) + 2] = nr - 1;
11028 }
11029
11030
11031
11032 lpl = lend[n1];
11033 --nnb;
11034 if (nnb == 3) {
11035 goto L3;
11036 }
11037 lpf = lptr[lpl];
11038 nfrst = list[lpf];
11039 lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11040 if (nr == nfrst) {
11041 goto L2;
11042 }
11043
11044
11045
11046
11047
11048
11049
11050 n2 = nr;
11051 x2 = xr;
11052 y2 = yr;
11053 z2 = zr;
11054 lp21 = lptr[lp21];
11055 lp21 = lptr[lp21];
11056 nr = (i__1 = list[lp21], abs(i__1));
11057 xr = x[nr];
11058 yr = y[nr];
11059 zr = z__[nr];
11060 goto L1;
11061
11062
11063
11064 L2:
11065 if (n2 == nfrst) {
11066 goto L3;
11067 }
11068 n2 = nl;
11069 x2 = xl;
11070 y2 = yl;
11071 z2 = zl;
11072 lp = lptr[lp];
11073 goto L1;
11074
11075
11076
11077
11078
11079
11080
11081
11082
11083
11084 L3:
11085 if (! bdry) {
11086 if (nnb > 3) {
11087 bdry = TRUE_;
11088 } else {
11089 lpf = lptr[lpl];
11090 nr = list[lpf];
11091 lp = lptr[lpf];
11092 n2 = list[lp];
11093 nl = list[lpl];
11094 bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11095 x[n2], &y[n2], &z__[n2]);
11096 }
11097 if (bdry) {
11098
11099
11100
11101
11102
11103
11104 i__1 = nn;
11105 for (i__ = 1; i__ <= i__1; ++i__) {
11106 if (list[lend[i__]] < 0) {
11107 bdry = FALSE_;
11108 goto L5;
11109 }
11110
11111 }
11112 list[lpl] = -list[lpl];
11113 ++nnb;
11114 }
11115 }
11116 L5:
11117 if (! bdry && nnb > 3) {
11118 goto L24;
11119 }
11120
11121
11122
11123
11124 lp = lpl;
11125 lnw = *lnew;
11126
11127
11128
11129 L6:
11130 lp = lptr[lp];
11131 n2 = (i__1 = list[lp], abs(i__1));
11132 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11133 if (lph < 0) {
11134 goto L23;
11135 }
11136
11137
11138
11139 if (lpl == lnw) {
11140 lpl = lph;
11141 }
11142 if (lp == lnw) {
11143 lp = lph;
11144 }
11145 if (lp != lpl) {
11146 goto L6;
11147 }
11148
11149
11150
11151
11152
11153 --nn;
11154 if (n1 > nn) {
11155 goto L9;
11156 }
11157 i__1 = nn;
11158 for (i__ = n1; i__ <= i__1; ++i__) {
11159 x[i__] = x[i__ + 1];
11160 y[i__] = y[i__ + 1];
11161 z__[i__] = z__[i__ + 1];
11162 lend[i__] = lend[i__ + 1];
11163
11164 }
11165
11166 i__1 = lnw - 1;
11167 for (i__ = 1; i__ <= i__1; ++i__) {
11168 if (list[i__] > n1) {
11169 --list[i__];
11170 }
11171 if (list[i__] < -n1) {
11172 ++list[i__];
11173 }
11174
11175 }
11176
11177
11178
11179
11180
11181
11182
11183
11184
11185
11186
11187 L9:
11188 if (bdry) {
11189 --nnb;
11190 }
11191 lpn = lpl;
11192 i__1 = nnb;
11193 for (j = 1; j <= i__1; ++j) {
11194 --lnw;
11195 lp = lpn;
11196 lpn = lptr[lp];
11197 list[lp] = list[lnw];
11198 lptr[lp] = lptr[lnw];
11199 if (lptr[lpn] == lnw) {
11200 lptr[lpn] = lp;
11201 }
11202 if (lpn == lnw) {
11203 lpn = lp;
11204 }
11205 for (i__ = nn; i__ >= 1; --i__) {
11206 if (lend[i__] == lnw) {
11207 lend[i__] = lp;
11208 goto L11;
11209 }
11210
11211 }
11212
11213 L11:
11214 for (i__ = lnw - 1; i__ >= 1; --i__) {
11215 if (lptr[i__] == lnw) {
11216 lptr[i__] = lp;
11217 }
11218
11219 }
11220
11221 }
11222
11223
11224
11225
11226
11227 *n = nn;
11228 *lnew = lnw;
11229 if (iwl > 0) {
11230 nit = iwl << 2;
11231 optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11232 nit, &iwk[3], &ierr);
11233 if (ierr != 0 && ierr != 1) {
11234 goto L25;
11235 }
11236 if (ierr == 1) {
11237 goto L26;
11238 }
11239 }
11240
11241
11242
11243 *ier = 0;
11244 return 0;
11245
11246
11247
11248 L21:
11249 *ier = 1;
11250 return 0;
11251
11252
11253
11254 L22:
11255 *ier = 2;
11256 return 0;
11257
11258
11259
11260
11261 L23:
11262 *ier = 3;
11263 return 0;
11264
11265
11266
11267 L24:
11268 *ier = 4;
11269 return 0;
11270
11271
11272
11273 L25:
11274 *ier = 5;
11275
11276
11277
11278 return 0;
11279
11280
11281
11282 L26:
11283 *ier = 6;
11284 return 0;
11285 }
11286
11287 int drwarc_(int *, double *p, double *q,
11288 double *tol, int *nseg)
11289 {
11290
11291 int i__1;
11292 double d__1;
11293
11294
11295
11296
11297
11298 static int i__, k;
11299 static double s, p1[3], p2[3], u1, u2, v1, v2;
11300 static int na;
11301 static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11302
11303
11304
11305
11306
11307
11308
11309
11310
11311
11312
11313
11314
11315
11316
11317
11318
11319
11320
11321
11322
11323
11324
11325
11326
11327
11328
11329
11330
11331
11332
11333
11334
11335
11336
11337
11338
11339
11340
11341
11342
11343
11344
11345
11346
11347
11348
11349
11350
11351
11352
11353
11354
11355
11356
11357
11358
11359
11360
11361
11362
11363
11364
11365
11366
11367
11368
11369
11370
11371
11372
11373
11374
11375
11376
11377
11378
11379 --q;
11380 --p;
11381
11382
11383 enrm = 0.;
11384 for (i__ = 1; i__ <= 3; ++i__) {
11385 pm[i__ - 1] = p[i__] + q[i__];
11386 enrm += pm[i__ - 1] * pm[i__ - 1];
11387
11388 }
11389 if (enrm == 0.) {
11390 goto L5;
11391 }
11392 enrm = sqrt(enrm);
11393 pm[0] /= enrm;
11394 pm[1] /= enrm;
11395 pm[2] /= enrm;
11396
11397
11398
11399
11400 u1 = p[1];
11401 v1 = p[2];
11402 u2 = q[1];
11403 v2 = q[2];
11404 um = pm[0];
11405 vm = pm[1];
11406
11407
11408
11409
11410
11411
11412 du = u2 - u1;
11413 dv = v2 - v1;
11414 enrm = du * du + dv * dv;
11415 if (enrm == 0.) {
11416 goto L5;
11417 }
11418 err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11419
11420
11421
11422
11423
11424 na = (int) (err / *tol + 1.);
11425
11426
11427
11428
11429
11430 s = 1. / (double) na;
11431 for (i__ = 1; i__ <= 3; ++i__) {
11432 dp[i__ - 1] = s * (q[i__] - p[i__]);
11433 pm[i__ - 1] = p[i__];
11434 p1[i__ - 1] = p[i__];
11435
11436 }
11437
11438
11439
11440
11441 i__1 = na - 1;
11442 for (k = 1; k <= i__1; ++k) {
11443 enrm = 0.;
11444 for (i__ = 1; i__ <= 3; ++i__) {
11445 pm[i__ - 1] += dp[i__ - 1];
11446 enrm += pm[i__ - 1] * pm[i__ - 1];
11447
11448 }
11449 if (enrm == 0.) {
11450 goto L5;
11451 }
11452 enrm = sqrt(enrm);
11453 p2[0] = pm[0] / enrm;
11454 p2[1] = pm[1] / enrm;
11455 p2[2] = pm[2] / enrm;
11456
11457
11458 p1[0] = p2[0];
11459 p1[1] = p2[1];
11460 p1[2] = p2[2];
11461
11462 }
11463
11464
11465
11466
11467 *nseg = na;
11468 return 0;
11469
11470
11471
11472 L5:
11473 *nseg = 0;
11474 return 0;
11475 }
11476
11477 int edge_(int *in1, int *in2, double *x,
11478 double *y, double *z__, int *lwk, int *iwk, int *
11479 list, int *lptr, int *lend, int *ier)
11480 {
11481
11482 int i__1;
11483
11484
11485 static int i__, n0, n1, n2;
11486 static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11487 static int nl, lp, nr;
11488 static double dp12;
11489 static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11490 static double dp1l, dp2l, dp1r, dp2r;
11491 static int ierr;
11492 int swap_(int *, int *, int *,
11493 int *, int *, int *, int *, int *);
11494 static int next, iwcp1, n1lst, iwend;
11495 int optim_(double *, double *, double
11496 *, int *, int *, int *, int *, int *, int
11497 *, int *);
11498 static int n1frst;
11499
11500
11501
11502
11503
11504
11505
11506
11507
11508
11509
11510
11511
11512
11513
11514
11515
11516
11517
11518
11519
11520
11521
11522
11523
11524
11525
11526
11527
11528
11529
11530
11531
11532
11533
11534
11535
11536
11537
11538
11539
11540
11541
11542
11543
11544
11545
11546
11547
11548
11549
11550
11551
11552
11553
11554
11555
11556
11557
11558
11559
11560
11561
11562
11563
11564
11565
11566
11567
11568
11569
11570
11571
11572
11573
11574
11575
11576
11577
11578
11579
11580
11581
11582
11583
11584
11585
11586
11587
11588
11589
11590
11591
11592
11593
11594
11595
11596
11597
11598
11599
11600
11601
11602
11603
11604
11605
11606
11607
11608
11609
11610
11611
11612
11613
11614
11615
11616
11617
11618
11619
11620
11621
11622
11623
11624
11625
11626
11627
11628
11629
11630
11631
11632
11633
11634
11635
11636 --lend;
11637 --lptr;
11638 --list;
11639 iwk -= 3;
11640 --z__;
11641 --y;
11642 --x;
11643
11644
11645 n1 = *in1;
11646 n2 = *in2;
11647 iwend = *lwk;
11648 if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11649 goto L31;
11650 }
11651
11652
11653
11654
11655 lpl = lend[n1];
11656 n0 = (i__1 = list[lpl], abs(i__1));
11657 lp = lpl;
11658 L1:
11659 if (n0 == n2) {
11660 goto L30;
11661 }
11662 lp = lptr[lp];
11663 n0 = list[lp];
11664 if (lp != lpl) {
11665 goto L1;
11666 }
11667
11668
11669
11670 iwl = 0;
11671 nit = 0;
11672
11673
11674
11675 L2:
11676 x1 = x[n1];
11677 y1 = y[n1];
11678 z1 = z__[n1];
11679 x2 = x[n2];
11680 y2 = y[n2];
11681 z2 = z__[n2];
11682
11683
11684
11685
11686
11687
11688
11689
11690
11691
11692 lpl = lend[n1];
11693 n1lst = list[lpl];
11694 lp = lptr[lpl];
11695 n1frst = list[lp];
11696 nl = n1frst;
11697 if (n1lst < 0) {
11698 goto L4;
11699 }
11700
11701
11702
11703
11704 L3:
11705 if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11706 goto L4;
11707 }
11708 lp = lptr[lp];
11709 nl = list[lp];
11710 if (nl != n1frst) {
11711 goto L3;
11712 }
11713
11714
11715
11716 goto L5;
11717
11718
11719
11720
11721 L4:
11722 nr = nl;
11723 lp = lptr[lp];
11724 nl = (i__1 = list[lp], abs(i__1));
11725 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11726
11727
11728
11729
11730
11731 dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11732 dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11733 dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11734 dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11735 dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11736 if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11737 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11738 goto L6;
11739 }
11740
11741
11742
11743
11744
11745 if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11746 goto L5;
11747 }
11748 }
11749
11750
11751
11752 if (nl != n1frst) {
11753 goto L4;
11754 }
11755
11756
11757
11758
11759
11760
11761
11762 L5:
11763 if (nit > 0) {
11764 goto L33;
11765 }
11766 nit = 1;
11767 n1 = n2;
11768 n2 = *in1;
11769 goto L2;
11770
11771
11772
11773
11774 L6:
11775 ++iwl;
11776 if (iwl > iwend) {
11777 goto L32;
11778 }
11779 iwk[(iwl << 1) + 1] = nl;
11780 iwk[(iwl << 1) + 2] = nr;
11781
11782
11783
11784 lpl = lend[nl];
11785 lp = lptr[lpl];
11786
11787
11788
11789
11790 L7:
11791 if (list[lp] == nr) {
11792 goto L8;
11793 }
11794 lp = lptr[lp];
11795 if (lp != lpl) {
11796 goto L7;
11797 }
11798
11799
11800
11801
11802 if (list[lp] != nr) {
11803 goto L33;
11804 }
11805
11806
11807
11808
11809 L8:
11810 lp = lptr[lp];
11811 next = (i__1 = list[lp], abs(i__1));
11812 if (next == n2) {
11813 goto L9;
11814 }
11815
11816
11817
11818 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11819 nl = next;
11820 } else {
11821 nr = next;
11822 }
11823 goto L6;
11824
11825
11826
11827
11828 L9:
11829 *lwk = iwl;
11830 iwend = iwl;
11831
11832
11833
11834
11835
11836
11837
11838
11839 iwf = 1;
11840
11841
11842
11843
11844
11845 L10:
11846 lft = 0;
11847 n0 = n1;
11848 x0 = x1;
11849 y0 = y1;
11850 z0 = z1;
11851 nl = iwk[(iwf << 1) + 1];
11852 nr = iwk[(iwf << 1) + 2];
11853 iwc = iwf;
11854
11855
11856
11857
11858 L11:
11859 if (iwc == iwl) {
11860 goto L21;
11861 }
11862 iwcp1 = iwc + 1;
11863 next = iwk[(iwcp1 << 1) + 1];
11864 if (next != nl) {
11865 goto L16;
11866 }
11867 next = iwk[(iwcp1 << 1) + 2];
11868
11869
11870
11871
11872 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11873 z__[next])) {
11874 goto L14;
11875 }
11876 if (lft >= 0) {
11877 goto L12;
11878 }
11879 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11880 z__[next])) {
11881 goto L14;
11882 }
11883
11884
11885
11886 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11887 iwk[(iwc << 1) + 1] = n0;
11888 iwk[(iwc << 1) + 2] = next;
11889 goto L15;
11890
11891
11892
11893
11894
11895 L12:
11896 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11897 i__1 = iwl;
11898 for (i__ = iwcp1; i__ <= i__1; ++i__) {
11899 iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11900 iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11901
11902 }
11903 iwk[(iwl << 1) + 1] = n0;
11904 iwk[(iwl << 1) + 2] = next;
11905 --iwl;
11906 nr = next;
11907 goto L11;
11908
11909
11910
11911 L14:
11912 n0 = nr;
11913 x0 = x[n0];
11914 y0 = y[n0];
11915 z0 = z__[n0];
11916 lft = 1;
11917
11918
11919
11920 L15:
11921 nr = next;
11922 ++iwc;
11923 goto L11;
11924
11925
11926
11927
11928 L16:
11929 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11930 z__[next])) {
11931 goto L19;
11932 }
11933 if (lft <= 0) {
11934 goto L17;
11935 }
11936 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11937 z__[next])) {
11938 goto L19;
11939 }
11940
11941
11942
11943 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11944 iwk[(iwc << 1) + 1] = next;
11945 iwk[(iwc << 1) + 2] = n0;
11946 goto L20;
11947
11948
11949
11950
11951
11952 L17:
11953 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11954 i__1 = iwf;
11955 for (i__ = iwc - 1; i__ >= i__1; --i__) {
11956 iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11957 iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11958
11959 }
11960 iwk[(iwf << 1) + 1] = n0;
11961 iwk[(iwf << 1) + 2] = next;
11962 ++iwf;
11963 goto L20;
11964
11965
11966
11967 L19:
11968 n0 = nl;
11969 x0 = x[n0];
11970 y0 = y[n0];
11971 z0 = z__[n0];
11972 lft = -1;
11973
11974
11975
11976 L20:
11977 nl = next;
11978 ++iwc;
11979 goto L11;
11980
11981
11982
11983 L21:
11984 if (n0 == n1) {
11985 goto L24;
11986 }
11987 if (lft < 0) {
11988 goto L22;
11989 }
11990
11991
11992
11993 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
11994 goto L10;
11995 }
11996
11997
11998
11999
12000 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12001 iwk[(iwl << 1) + 1] = n0;
12002 iwk[(iwl << 1) + 2] = n2;
12003 --iwl;
12004 goto L10;
12005
12006
12007
12008 L22:
12009 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12010 goto L10;
12011 }
12012
12013
12014
12015
12016 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12017 i__ = iwl;
12018 L23:
12019 iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12020 iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12021 --i__;
12022 if (i__ > iwf) {
12023 goto L23;
12024 }
12025 iwk[(iwf << 1) + 1] = n0;
12026 iwk[(iwf << 1) + 2] = n2;
12027 ++iwf;
12028 goto L10;
12029
12030
12031
12032
12033 L24:
12034 swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12035 iwk[(iwc << 1) + 1] = 0;
12036 iwk[(iwc << 1) + 2] = 0;
12037
12038
12039
12040 *ier = 0;
12041 if (iwc > 1) {
12042
12043
12044
12045 nit = iwc - (1<<2);
12046 i__1 = iwc - 1;
12047 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12048 nit, &iwk[3], &ierr);
12049 if (ierr != 0 && ierr != 1) {
12050 goto L34;
12051 }
12052 if (ierr == 1) {
12053 *ier = 5;
12054 }
12055 }
12056 if (iwc < iwend) {
12057
12058
12059
12060 nit = iwend - (iwc<<2);
12061 i__1 = iwend - iwc;
12062 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12063 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12064 if (ierr != 0 && ierr != 1) {
12065 goto L34;
12066 }
12067 if (ierr == 1) {
12068 goto L35;
12069 }
12070 }
12071 if (*ier == 5) {
12072 goto L35;
12073 }
12074
12075
12076
12077 return 0;
12078
12079
12080
12081 L30:
12082 *ier = 0;
12083 return 0;
12084
12085
12086
12087 L31:
12088 *ier = 1;
12089 return 0;
12090
12091
12092
12093 L32:
12094 *ier = 2;
12095 return 0;
12096
12097
12098
12099
12100 L33:
12101 *ier = 3;
12102
12103
12104
12105
12106 return 0;
12107
12108
12109
12110 L34:
12111 *ier = 4;
12112
12113
12114
12115 return 0;
12116
12117
12118
12119 L35:
12120 *ier = 5;
12121 return 0;
12122 }
12123
12124 int getnp_(double *x, double *y, double *z__,
12125 int *list, int *lptr, int *lend, int *l, int *
12126 npts, double *df, int *ier)
12127 {
12128
12129 int i__1, i__2;
12130
12131
12132 static int i__, n1;
12133 static double x1, y1, z1;
12134 static int nb, ni, lp, np, lm1;
12135 static double dnb, dnp;
12136 static int lpl;
12137
12138
12139
12140
12141
12142
12143
12144
12145
12146
12147
12148
12149
12150
12151
12152
12153
12154
12155
12156
12157
12158
12159
12160
12161
12162
12163
12164
12165
12166
12167
12168
12169
12170
12171
12172
12173
12174
12175
12176
12177
12178
12179
12180
12181
12182
12183
12184
12185
12186
12187
12188
12189
12190
12191
12192
12193
12194
12195
12196
12197
12198
12199
12200
12201
12202
12203
12204
12205
12206
12207
12208
12209
12210
12211
12212
12213
12214
12215
12216 --x;
12217 --y;
12218 --z__;
12219 --list;
12220 --lptr;
12221 --lend;
12222 --npts;
12223
12224
12225 lm1 = *l - 1;
12226 if (lm1 < 1) {
12227 goto L6;
12228 }
12229 *ier = 0;
12230
12231
12232
12233 n1 = npts[1];
12234 x1 = x[n1];
12235 y1 = y[n1];
12236 z1 = z__[n1];
12237 i__1 = lm1;
12238 for (i__ = 1; i__ <= i__1; ++i__) {
12239 ni = npts[i__];
12240 lend[ni] = -lend[ni];
12241
12242 }
12243
12244
12245
12246
12247
12248 dnp = 2.;
12249
12250
12251
12252 i__1 = lm1;
12253 for (i__ = 1; i__ <= i__1; ++i__) {
12254 ni = npts[i__];
12255 lpl = -lend[ni];
12256 lp = lpl;
12257
12258
12259
12260 L2:
12261 nb = (i__2 = list[lp], abs(i__2));
12262 if (lend[nb] < 0) {
12263 goto L3;
12264 }
12265
12266
12267
12268
12269 dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12270 if (dnb >= dnp) {
12271 goto L3;
12272 }
12273 np = nb;
12274 dnp = dnb;
12275 L3:
12276 lp = lptr[lp];
12277 if (lp != lpl) {
12278 goto L2;
12279 }
12280
12281 }
12282 npts[*l] = np;
12283 *df = dnp;
12284
12285
12286
12287 i__1 = lm1;
12288 for (i__ = 1; i__ <= i__1; ++i__) {
12289 ni = npts[i__];
12290 lend[ni] = -lend[ni];
12291
12292 }
12293 return 0;
12294
12295
12296
12297 L6:
12298 *ier = 1;
12299 return 0;
12300 }
12301
12302 int insert_(int *k, int *lp, int *list, int *
12303 lptr, int *lnew)
12304 {
12305 static int lsav;
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 --lptr;
12350 --list;
12351
12352
12353 lsav = lptr[*lp];
12354 lptr[*lp] = *lnew;
12355 list[*lnew] = *k;
12356 lptr[*lnew] = lsav;
12357 ++(*lnew);
12358 return 0;
12359 }
12360
12361 long int inside_(double *p, int *lv, double *xv, double *yv,
12362 double *zv, int *nv, int *listv, int *ier)
12363 {
12364
12365
12366 static double eps = .001;
12367
12368
12369 int i__1;
12370 long int ret_val = 0;
12371
12372
12373
12374
12375
12376 static double b[3], d__;
12377 static int k, n;
12378 static double q[3];
12379 static int i1, i2, k0;
12380 static double v1[3], v2[3], cn[3], bp, bq;
12381 static int ni;
12382 static double pn[3], qn[3], vn[3];
12383 static int imx;
12384 static long int lft1, lft2, even;
12385 static int ierr;
12386 static long int pinr, qinr;
12387 static double qnrm, vnrm;
12388 int intrsc_(double *, double *,
12389 double *, double *, int *);
12390
12391
12392
12393
12394
12395
12396
12397
12398
12399
12400
12401
12402
12403
12404
12405
12406
12407
12408
12409
12410
12411
12412
12413
12414
12415
12416
12417
12418
12419
12420
12421
12422
12423
12424
12425
12426
12427
12428
12429
12430
12431
12432
12433
12434
12435
12436
12437
12438
12439
12440
12441
12442
12443
12444
12445
12446
12447
12448
12449
12450
12451
12452
12453
12454
12455
12456
12457
12458
12459
12460
12461
12462
12463
12464
12465
12466
12467
12468
12469
12470
12471
12472
12473
12474
12475
12476
12477
12478
12479
12480
12481
12482
12483
12484
12485
12486
12487
12488
12489
12490
12491
12492
12493
12494
12495
12496
12497
12498
12499
12500
12501
12502
12503
12504
12505
12506
12507
12508
12509
12510
12511
12512
12513
12514
12515
12516
12517
12518
12519
12520
12521
12522
12523
12524
12525
12526
12527
12528
12529
12530
12531
12532
12533
12534
12535
12536
12537
12538
12539
12540
12541
12542 --p;
12543 --zv;
12544 --yv;
12545 --xv;
12546 --listv;
12547
12548
12549
12550
12551
12552
12553 imx = *lv;
12554 n = *nv;
12555 if (n < 3 || n > imx) {
12556 goto L11;
12557 }
12558 k0 = 0;
12559 i1 = listv[1];
12560 if (i1 < 1 || i1 > imx) {
12561 goto L12;
12562 }
12563
12564
12565
12566
12567
12568 L1:
12569 ++k0;
12570 if (k0 > n) {
12571 goto L14;
12572 }
12573 i1 = listv[k0];
12574 if (k0 < n) {
12575 i2 = listv[k0 + 1];
12576 } else {
12577 i2 = listv[1];
12578 }
12579 if (i2 < 1 || i2 > imx) {
12580 goto L12;
12581 }
12582 vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12583 vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12584 vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12585 vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12586 if (vnrm == 0.) {
12587 goto L1;
12588 }
12589 q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12590 q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12591 q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12592 qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12593 q[0] /= qnrm;
12594 q[1] /= qnrm;
12595 q[2] /= qnrm;
12596
12597
12598
12599 cn[0] = q[1] * p[3] - q[2] * p[2];
12600 cn[1] = q[2] * p[1] - q[0] * p[3];
12601 cn[2] = q[0] * p[2] - q[1] * p[1];
12602 if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12603 goto L1;
12604 }
12605 pn[0] = p[2] * cn[2] - p[3] * cn[1];
12606 pn[1] = p[3] * cn[0] - p[1] * cn[2];
12607 pn[2] = p[1] * cn[1] - p[2] * cn[0];
12608 qn[0] = cn[1] * q[2] - cn[2] * q[1];
12609 qn[1] = cn[2] * q[0] - cn[0] * q[2];
12610 qn[2] = cn[0] * q[1] - cn[1] * q[0];
12611
12612
12613
12614 ni = 0;
12615 even = TRUE_;
12616 bp = -2.;
12617 bq = -2.;
12618 pinr = TRUE_;
12619 qinr = TRUE_;
12620 i2 = listv[n];
12621 if (i2 < 1 || i2 > imx) {
12622 goto L12;
12623 }
12624 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12625
12626
12627
12628 i__1 = n;
12629 for (k = 1; k <= i__1; ++k) {
12630 i1 = i2;
12631 lft1 = lft2;
12632 i2 = listv[k];
12633 if (i2 < 1 || i2 > imx) {
12634 goto L12;
12635 }
12636 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12637 if (lft1 == lft2) {
12638 goto L2;
12639 }
12640
12641
12642
12643
12644 ++ni;
12645 v1[0] = xv[i1];
12646 v1[1] = yv[i1];
12647 v1[2] = zv[i1];
12648 v2[0] = xv[i2];
12649 v2[1] = yv[i2];
12650 v2[2] = zv[i2];
12651 intrsc_(v1, v2, cn, b, &ierr);
12652
12653
12654
12655
12656
12657 if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12658 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12659
12660
12661
12662 even = ! even;
12663 d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12664 if (d__ > bq) {
12665 bq = d__;
12666 qinr = lft2;
12667 }
12668 d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12669 if (d__ > bp) {
12670 bp = d__;
12671 pinr = lft1;
12672 }
12673 }
12674 L2:
12675 ;
12676 }
12677
12678
12679
12680
12681 if (ni != ni / 2 << 1 || ! qinr) {
12682 goto L1;
12683 }
12684
12685
12686
12687 if (pinr != even) {
12688 goto L13;
12689 }
12690
12691
12692
12693 *ier = 0;
12694 ret_val = even;
12695 return ret_val;
12696
12697
12698
12699 L11:
12700 *ier = 1;
12701 return ret_val;
12702
12703
12704
12705 L12:
12706 *ier = 2;
12707 return ret_val;
12708
12709
12710
12711 L13:
12712 *ier = 3;
12713 return ret_val;
12714
12715
12716
12717 L14:
12718 *ier = 4;
12719 return ret_val;
12720 }
12721
12722 int intadd_(int *kk, int *i1, int *i2, int *
12723 i3, int *list, int *lptr, int *lend, int *lnew)
12724 {
12725 static int k, n1, n2, n3, lp;
12726 int insert_(int *, int *, int *,
12727 int *, int *);
12728 int lstptr_(int *, int *, int *, int *);
12729
12730
12731
12732
12733
12734
12735
12736
12737
12738
12739
12740
12741
12742
12743
12744
12745
12746
12747
12748
12749
12750
12751
12752
12753
12754
12755
12756
12757
12758
12759
12760
12761
12762
12763
12764
12765
12766
12767
12768
12769
12770
12771
12772
12773
12774
12775
12776
12777
12778
12779
12780
12781
12782
12783
12784
12785
12786 --lend;
12787 --lptr;
12788 --list;
12789
12790
12791 k = *kk;
12792
12793
12794
12795 n1 = *i1;
12796 n2 = *i2;
12797 n3 = *i3;
12798
12799
12800
12801 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12802 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12803 lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12804 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12805 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12806 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12807
12808
12809
12810 list[*lnew] = n1;
12811 list[*lnew + 1] = n2;
12812 list[*lnew + 2] = n3;
12813 lptr[*lnew] = *lnew + 1;
12814 lptr[*lnew + 1] = *lnew + 2;
12815 lptr[*lnew + 2] = *lnew;
12816 lend[k] = *lnew + 2;
12817 *lnew += 3;
12818 return 0;
12819 }
12820
12821 int intrsc_(double *p1, double *p2, double *cn,
12822 double *p, int *ier)
12823 {
12824
12825
12826
12827
12828 static int i__;
12829 static double t, d1, d2, pp[3], ppn;
12830
12831
12832
12833
12834
12835
12836
12837
12838
12839
12840
12841
12842
12843
12844
12845
12846
12847
12848
12849
12850
12851
12852
12853
12854
12855
12856
12857
12858
12859
12860
12861
12862
12863
12864
12865
12866
12867
12868
12869
12870
12871
12872
12873
12874
12875
12876
12877
12878
12879
12880
12881
12882
12883
12884
12885
12886
12887
12888
12889
12890
12891
12892
12893
12894
12895
12896
12897
12898
12899 --p;
12900 --cn;
12901 --p2;
12902 --p1;
12903
12904
12905 d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
12906 d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
12907
12908 if (d1 == d2) {
12909 *ier = 1;
12910 return 0;
12911 }
12912
12913
12914
12915 t = d1 / (d1 - d2);
12916 ppn = 0.;
12917 for (i__ = 1; i__ <= 3; ++i__) {
12918 pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
12919 ppn += pp[i__ - 1] * pp[i__ - 1];
12920
12921 }
12922
12923
12924
12925 if (ppn == 0.) {
12926 *ier = 2;
12927 return 0;
12928 }
12929 ppn = sqrt(ppn);
12930
12931
12932
12933 for (i__ = 1; i__ <= 3; ++i__) {
12934 p[i__] = pp[i__ - 1] / ppn;
12935
12936 }
12937 *ier = 0;
12938 return 0;
12939 }
12940
12941 int jrand_(int *n, int *ix, int *iy, int *iz)
12942 {
12943
12944 int ret_val;
12945
12946
12947 static float u, x;
12948
12949
12950
12951
12952
12953
12954
12955
12956
12957
12958
12959
12960
12961
12962
12963
12964
12965
12966
12967
12968
12969
12970
12971
12972
12973
12974
12975
12976
12977
12978
12979
12980
12981
12982
12983
12984
12985
12986
12987
12988
12989
12990
12991
12992
12993
12994
12995
12996
12997
12998
12999
13000
13001 *ix = *ix * 171 % 30269;
13002 *iy = *iy * 172 % 30307;
13003 *iz = *iz * 170 % 30323;
13004 x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13005 30323.f;
13006 u = x - (int) x;
13007 ret_val = (int) ((float) (*n) * u + 1.f);
13008 return ret_val;
13009 }
13010
13011 long int left_(double *x1, double *y1, double *z1, double *x2,
13012 double *y2, double *z2, double *x0, double *y0,
13013 double *z0)
13014 {
13015
13016 long int ret_val;
13017
13018
13019
13020
13021
13022
13023
13024
13025
13026
13027
13028
13029
13030
13031
13032
13033
13034
13035
13036
13037
13038
13039
13040
13041
13042
13043
13044
13045
13046
13047
13048
13049
13050
13051
13052
13053
13054
13055 ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13056 *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13057
13058
13059 return ret_val;
13060 }
13061
13062 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13063 {
13064
13065 int ret_val;
13066
13067
13068 static int nd, lp;
13069
13070
13071
13072
13073
13074
13075
13076
13077
13078
13079
13080
13081
13082
13083
13084
13085
13086
13087
13088
13089
13090
13091
13092
13093
13094
13095
13096
13097
13098
13099
13100
13101
13102
13103
13104
13105
13106
13107
13108
13109
13110
13111
13112
13113
13114
13115
13116 --lptr;
13117 --list;
13118
13119
13120 lp = lptr[*lpl];
13121 L1:
13122 nd = list[lp];
13123 if (nd == *nb) {
13124 goto L2;
13125 }
13126 lp = lptr[lp];
13127 if (lp != *lpl) {
13128 goto L1;
13129 }
13130
13131 L2:
13132 ret_val = lp;
13133 return ret_val;
13134 }
13135
13136 int nbcnt_(int *lpl, int *lptr)
13137 {
13138
13139 int ret_val;
13140
13141
13142 static int k, lp;
13143
13144
13145
13146
13147
13148
13149
13150
13151
13152
13153
13154
13155
13156
13157
13158
13159
13160
13161
13162
13163
13164
13165
13166
13167
13168
13169
13170
13171
13172
13173
13174
13175
13176
13177
13178
13179
13180
13181
13182
13183
13184
13185 --lptr;
13186
13187
13188 lp = *lpl;
13189 k = 1;
13190
13191 L1:
13192 lp = lptr[lp];
13193 if (lp == *lpl) {
13194 goto L2;
13195 }
13196 ++k;
13197 goto L1;
13198
13199 L2:
13200 ret_val = k;
13201 return ret_val;
13202 }
13203
13204 int nearnd_(double *p, int *ist, int *n, double *x,
13205 double *y, double *z__, int *list, int *lptr, int
13206 *lend, double *al)
13207 {
13208
13209 int ret_val, i__1;
13210
13211
13212
13213
13214
13215 static int l;
13216 static double b1, b2, b3;
13217 static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13218 static double ds1;
13219 static int lp1, lp2;
13220 static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13221 static int lpl;
13222 static double dsr;
13223 static int nst, listp[25], lptrp[25];
13224 int trfind_(int *, double *, int *,
13225 double *, double *, double *, int *, int *,
13226 int *, double *, double *, double *, int *,
13227 int *, int *);
13228 int lstptr_(int *, int *, int *, int *);
13229
13230
13231
13232
13233
13234
13235
13236
13237
13238
13239
13240
13241
13242
13243
13244
13245
13246
13247
13248
13249
13250
13251
13252
13253
13254
13255
13256
13257
13258
13259
13260
13261
13262
13263
13264
13265
13266
13267
13268
13269
13270
13271
13272
13273
13274
13275
13276
13277
13278
13279
13280
13281
13282
13283
13284
13285
13286
13287
13288
13289
13290
13291
13292
13293
13294
13295
13296
13297
13298
13299
13300
13301
13302
13303
13304
13305
13306
13307
13308
13309
13310
13311
13312
13313
13314
13315
13316
13317
13318
13319
13320
13321
13322
13323
13324
13325 --p;
13326 --lend;
13327 --z__;
13328 --y;
13329 --x;
13330 --list;
13331 --lptr;
13332
13333
13334 nn = *n;
13335 if (nn < 3) {
13336 goto L6;
13337 }
13338 nst = *ist;
13339 if (nst < 1 || nst > nn) {
13340 nst = 1;
13341 }
13342
13343
13344
13345
13346
13347 trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13348 1], &b1, &b2, &b3, &i1, &i2, &i3);
13349
13350
13351
13352 if (i1 == 0) {
13353 goto L6;
13354 }
13355
13356
13357
13358
13359
13360
13361
13362 if (i3 != 0) {
13363 listp[0] = i1;
13364 lptrp[0] = 2;
13365 listp[1] = i2;
13366 lptrp[1] = 3;
13367 listp[2] = i3;
13368 lptrp[2] = 1;
13369 l = 3;
13370 } else {
13371 n1 = i1;
13372 l = 1;
13373 lp1 = 2;
13374 listp[l - 1] = n1;
13375 lptrp[l - 1] = lp1;
13376
13377
13378
13379
13380 L1:
13381 lpl = lend[n1];
13382 n1 = -list[lpl];
13383 l = lp1;
13384 lp1 = l + 1;
13385 listp[l - 1] = n1;
13386 lptrp[l - 1] = lp1;
13387 if (n1 != i2 && lp1 < 25) {
13388 goto L1;
13389 }
13390 l = lp1;
13391 listp[l - 1] = 0;
13392 lptrp[l - 1] = 1;
13393 }
13394
13395
13396
13397
13398
13399
13400 lp2 = 1;
13401 n2 = i1;
13402 lp1 = lptrp[0];
13403 n1 = listp[lp1 - 1];
13404
13405
13406
13407 L2:
13408 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13409 if (list[lp] < 0) {
13410 goto L3;
13411 }
13412 lp = lptr[lp];
13413 n3 = (i__1 = list[lp], abs(i__1));
13414
13415
13416
13417 if (l == 25) {
13418 goto L4;
13419 }
13420 dx1 = x[n1] - p[1];
13421 dy1 = y[n1] - p[2];
13422 dz1 = z__[n1] - p[3];
13423
13424 dx2 = x[n2] - p[1];
13425 dy2 = y[n2] - p[2];
13426 dz2 = z__[n2] - p[3];
13427
13428 dx3 = x[n3] - p[1];
13429 dy3 = y[n3] - p[2];
13430 dz3 = z__[n3] - p[3];
13431 if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13432 (dx2 * dy1 - dx1 * dy2) <= 0.) {
13433 goto L3;
13434 }
13435
13436
13437
13438
13439 ++l;
13440 lptrp[lp2 - 1] = l;
13441 listp[l - 1] = n3;
13442 lptrp[l - 1] = lp1;
13443 lp1 = l;
13444 n1 = n3;
13445 goto L2;
13446
13447
13448
13449
13450 L3:
13451 if (lp1 == 1) {
13452 goto L4;
13453 }
13454 lp2 = lp1;
13455 n2 = n1;
13456 lp1 = lptrp[lp1 - 1];
13457 n1 = listp[lp1 - 1];
13458 if (n1 == 0) {
13459 goto L4;
13460 }
13461 goto L2;
13462
13463
13464
13465
13466
13467 L4:
13468 nr = i1;
13469 dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13470 i__1 = l;
13471 for (lp = 2; lp <= i__1; ++lp) {
13472 n1 = listp[lp - 1];
13473 if (n1 == 0) {
13474 goto L5;
13475 }
13476 ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13477 if (ds1 < dsr) {
13478 nr = n1;
13479 dsr = ds1;
13480 }
13481 L5:
13482 ;
13483 }
13484 dsr = -dsr;
13485 if (dsr > 1.) {
13486 dsr = 1.;
13487 }
13488 *al = acos(dsr);
13489 ret_val = nr;
13490 return ret_val;
13491
13492
13493
13494 L6:
13495 ret_val = 0;
13496 return ret_val;
13497 }
13498
13499 int optim_(double *x, double *y, double *z__,
13500 int *na, int *list, int *lptr, int *lend, int *
13501 nit, int *iwk, int *ier)
13502 {
13503
13504 int i__1, i__2;
13505
13506
13507 static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13508 static long int swp;
13509 static int iter;
13510 int swap_(int *, int *, int *,
13511 int *, int *, int *, int *, int *);
13512 static int maxit;
13513 long int swptst_(int *, int *, int *, int *,
13514 double *, double *, double *);
13515
13516
13517
13518
13519
13520
13521
13522
13523
13524
13525
13526
13527
13528
13529
13530
13531
13532
13533
13534
13535
13536
13537
13538
13539
13540
13541
13542
13543
13544
13545
13546
13547
13548
13549
13550
13551
13552
13553
13554
13555
13556
13557
13558
13559
13560
13561
13562
13563
13564
13565
13566
13567
13568
13569
13570
13571
13572
13573
13574
13575
13576
13577
13578
13579
13580
13581
13582
13583
13584
13585
13586
13587
13588
13589
13590
13591
13592
13593
13594
13595
13596
13597
13598
13599
13600
13601
13602
13603
13604
13605
13606
13607
13608
13609
13610
13611 --x;
13612 --y;
13613 --z__;
13614 iwk -= 3;
13615 --list;
13616 --lptr;
13617 --lend;
13618
13619
13620 nna = *na;
13621 maxit = *nit;
13622 if (nna < 0 || maxit < 1) {
13623 goto L7;
13624 }
13625
13626
13627
13628 iter = 0;
13629 if (nna == 0) {
13630 goto L5;
13631 }
13632
13633
13634
13635
13636 L1:
13637 if (iter == maxit) {
13638 goto L6;
13639 }
13640 ++iter;
13641 swp = FALSE_;
13642
13643
13644
13645 i__1 = nna;
13646 for (i__ = 1; i__ <= i__1; ++i__) {
13647 io1 = iwk[(i__ << 1) + 1];
13648 io2 = iwk[(i__ << 1) + 2];
13649
13650
13651
13652
13653
13654
13655
13656
13657 lpl = lend[io1];
13658 lpp = lpl;
13659 lp = lptr[lpp];
13660 L2:
13661 if (list[lp] == io2) {
13662 goto L3;
13663 }
13664 lpp = lp;
13665 lp = lptr[lpp];
13666 if (lp != lpl) {
13667 goto L2;
13668 }
13669
13670
13671
13672
13673
13674 if ((i__2 = list[lp], abs(i__2)) != io2) {
13675 goto L8;
13676 }
13677 if (list[lp] < 0) {
13678 goto L4;
13679 }
13680
13681
13682
13683
13684 L3:
13685 n2 = list[lpp];
13686 if (n2 < 0) {
13687 goto L4;
13688 }
13689 lp = lptr[lp];
13690 n1 = (i__2 = list[lp], abs(i__2));
13691
13692
13693
13694 if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13695 goto L4;
13696 }
13697 swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13698 if (lp21 == 0) {
13699 goto L9;
13700 }
13701 swp = TRUE_;
13702 iwk[(i__ << 1) + 1] = n1;
13703 iwk[(i__ << 1) + 2] = n2;
13704 L4:
13705 ;
13706 }
13707 if (swp) {
13708 goto L1;
13709 }
13710
13711
13712
13713 L5:
13714 *nit = iter;
13715 *ier = 0;
13716 return 0;
13717
13718
13719
13720 L6:
13721 *nit = maxit;
13722 *ier = 1;
13723 return 0;
13724
13725
13726
13727 L7:
13728 *nit = 0;
13729 *ier = 2;
13730 return 0;
13731
13732
13733
13734 L8:
13735 *nit = iter;
13736 *ier = 3;
13737 return 0;
13738
13739
13740
13741 L9:
13742 *nit = iter;
13743 *ier = 4;
13744 return 0;
13745 }
13746
13747 int projct_(double *px, double *py, double *pz,
13748 double *ox, double *oy, double *oz, double *ex,
13749 double *ey, double *ez, double *vx, double *vy,
13750 double *vz, long int *init, double *x, double *y,
13751 double *z__, int *ier)
13752 {
13753
13754
13755
13756
13757 static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13758 oes, xoe, yoe, zoe, xep, yep, zep;
13759
13760
13761
13762
13763
13764
13765
13766
13767
13768
13769
13770
13771
13772
13773
13774
13775
13776
13777
13778
13779
13780
13781
13782
13783
13784
13785
13786
13787
13788
13789
13790
13791
13792
13793
13794
13795
13796
13797
13798
13799
13800
13801
13802
13803
13804
13805
13806
13807
13808
13809
13810
13811
13812
13813
13814
13815
13816
13817
13818
13819
13820
13821
13822
13823
13824
13825
13826
13827
13828
13829
13830
13831
13832
13833
13834
13835
13836
13837
13838
13839
13840
13841
13842
13843
13844
13845
13846
13847
13848
13849
13850
13851
13852
13853
13854
13855
13856
13857
13858
13859
13860
13861
13862
13863
13864
13865
13866
13867
13868
13869
13870
13871
13872
13873 if (*init) {
13874
13875
13876
13877
13878
13879
13880
13881
13882 xe = *ex;
13883 ye = *ey;
13884 ze = *ez;
13885 xoe = xe - *ox;
13886 yoe = ye - *oy;
13887 zoe = ze - *oz;
13888 oes = xoe * xoe + yoe * yoe + zoe * zoe;
13889 if (oes == 0.) {
13890 goto L1;
13891 }
13892
13893
13894
13895 s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
13896 xv = *vx - s * xoe;
13897 yv = *vy - s * yoe;
13898 zv = *vz - s * zoe;
13899
13900
13901
13902 sc = xv * xv + yv * yv + zv * zv;
13903 if (sc == 0.) {
13904 goto L2;
13905 }
13906 sc = 1. / sqrt(sc);
13907 xv = sc * xv;
13908 yv = sc * yv;
13909 zv = sc * zv;
13910
13911
13912
13913 xh = yv * zoe - yoe * zv;
13914 yh = xoe * zv - xv * zoe;
13915 zh = xv * yoe - xoe * yv;
13916 sc = sqrt(xh * xh + yh * yh + zh * zh);
13917 if (sc == 0.) {
13918 goto L2;
13919 }
13920 sc = 1. / sc;
13921 xh = sc * xh;
13922 yh = sc * yh;
13923 zh = sc * zh;
13924 }
13925
13926
13927
13928
13929
13930
13931 xep = *px - xe;
13932 yep = *py - ye;
13933 zep = *pz - ze;
13934 s = xoe * xep + yoe * yep + zoe * zep;
13935 if (s >= 0.) {
13936 goto L1;
13937 }
13938 s = oes / s;
13939 xw = xoe - s * xep;
13940 yw = yoe - s * yep;
13941 zw = zoe - s * zep;
13942
13943
13944
13945
13946 *x = xw * xh + yw * yh + zw * zh;
13947 *y = xw * xv + yw * yv + zw * zv;
13948 *z__ = s + 1.;
13949 *init = FALSE_;
13950 *ier = 0;
13951 return 0;
13952
13953
13954
13955 L1:
13956 *ier = 1;
13957 return 0;
13958
13959
13960
13961 L2:
13962 *ier = 2;
13963 return 0;
13964 }
13965
13966 int scoord_(double *px, double *py, double *pz,
13967 double *plat, double *plon, double *pnrm)
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 *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14012 if (*px != 0. || *py != 0.) {
14013 *plon = atan2(*py, *px);
14014 } else {
14015 *plon = 0.;
14016 }
14017 if (*pnrm != 0.) {
14018 *plat = asin(*pz / *pnrm);
14019 } else {
14020 *plat = 0.;
14021 }
14022 return 0;
14023 }
14024
14025 double store_(double *x)
14026 {
14027
14028 double ret_val;
14029
14030
14031
14032
14033
14034
14035
14036
14037
14038
14039
14040
14041
14042
14043
14044
14045
14046
14047
14048
14049
14050
14051
14052
14053
14054
14055
14056
14057
14058
14059
14060
14061
14062
14063 stcom_1.y = *x;
14064 ret_val = stcom_1.y;
14065 return ret_val;
14066 }
14067
14068 int swap_(int *in1, int *in2, int *io1, int *
14069 io2, int *list, int *lptr, int *lend, int *lp21)
14070 {
14071
14072 int i__1;
14073
14074
14075 static int lp, lph, lpsav;
14076 int lstptr_(int *, int *, int *, int *);
14077
14078
14079
14080
14081
14082
14083
14084
14085
14086
14087
14088
14089
14090
14091
14092
14093
14094
14095
14096
14097
14098
14099
14100
14101
14102
14103
14104
14105
14106
14107
14108
14109
14110
14111
14112
14113
14114
14115
14116
14117
14118
14119
14120
14121
14122
14123
14124
14125
14126
14127
14128
14129
14130
14131
14132
14133
14134
14135
14136
14137 --lend;
14138 --lptr;
14139 --list;
14140
14141
14142 lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14143 if ((i__1 = list[lp], abs(i__1)) == *in2) {
14144 *lp21 = 0;
14145 return 0;
14146 }
14147
14148
14149
14150 lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14151 lph = lptr[lp];
14152 lptr[lp] = lptr[lph];
14153
14154
14155
14156
14157 if (lend[*io1] == lph) {
14158 lend[*io1] = lp;
14159 }
14160
14161
14162
14163
14164 lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14165 lpsav = lptr[lp];
14166 lptr[lp] = lph;
14167 list[lph] = *in2;
14168 lptr[lph] = lpsav;
14169
14170
14171
14172 lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14173 lph = lptr[lp];
14174 lptr[lp] = lptr[lph];
14175
14176
14177
14178
14179 if (lend[*io2] == lph) {
14180 lend[*io2] = lp;
14181 }
14182
14183
14184
14185 lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14186 lpsav = lptr[lp];
14187 lptr[lp] = lph;
14188 list[lph] = *in1;
14189 lptr[lph] = lpsav;
14190 *lp21 = lph;
14191 return 0;
14192 }
14193
14194 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14195 double *x, double *y, double *z__)
14196 {
14197
14198 long int ret_val;
14199
14200
14201 static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14202
14203
14204
14205
14206
14207
14208
14209
14210
14211
14212
14213
14214
14215
14216
14217
14218
14219
14220
14221
14222
14223
14224
14225
14226
14227
14228
14229
14230
14231
14232
14233
14234
14235
14236
14237
14238
14239
14240
14241
14242
14243
14244
14245
14246
14247
14248
14249
14250
14251
14252
14253
14254
14255
14256
14257
14258 --z__;
14259 --y;
14260 --x;
14261
14262
14263 x4 = x[*n4];
14264 y4 = y[*n4];
14265 z4 = z__[*n4];
14266 dx1 = x[*n1] - x4;
14267 dx2 = x[*n2] - x4;
14268 dx3 = x[*n3] - x4;
14269 dy1 = y[*n1] - y4;
14270 dy2 = y[*n2] - y4;
14271 dy3 = y[*n3] - y4;
14272 dz1 = z__[*n1] - z4;
14273 dz2 = z__[*n2] - z4;
14274 dz3 = z__[*n3] - z4;
14275
14276
14277
14278
14279
14280 ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14281 dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14282 return ret_val;
14283 }
14284
14285 int trans_(int *n, double *rlat, double *rlon,
14286 double *x, double *y, double *z__)
14287 {
14288
14289 int i__1;
14290
14291
14292
14293
14294
14295 static int i__, nn;
14296 static double phi, theta, cosphi;
14297
14298
14299
14300
14301
14302
14303
14304
14305
14306
14307
14308
14309
14310
14311
14312
14313
14314
14315
14316
14317
14318
14319
14320
14321
14322
14323
14324
14325
14326
14327
14328
14329
14330
14331
14332
14333
14334
14335
14336
14337
14338
14339
14340
14341
14342
14343
14344
14345
14346
14347
14348
14349
14350
14351 --z__;
14352 --y;
14353 --x;
14354 --rlon;
14355 --rlat;
14356
14357
14358 nn = *n;
14359 i__1 = nn;
14360 for (i__ = 1; i__ <= i__1; ++i__) {
14361 phi = rlat[i__];
14362 theta = rlon[i__];
14363 cosphi = cos(phi);
14364 x[i__] = cosphi * cos(theta);
14365 y[i__] = cosphi * sin(theta);
14366 z__[i__] = sin(phi);
14367
14368 }
14369 return 0;
14370 }
14371
14372 int trfind_(int *nst, double *p, int *n,
14373 double *x, double *y, double *z__, int *list, int
14374 *lptr, int *lend, double *b1, double *b2, double *b3,
14375 int *i1, int *i2, int *i3)
14376 {
14377
14378
14379 static int ix = 1;
14380 static int iy = 2;
14381 static int iz = 3;
14382
14383
14384 int i__1;
14385 double d__1, d__2;
14386
14387
14388 static double q[3];
14389 static int n0, n1, n2, n3, n4, nf;
14390 static double s12;
14391 static int nl, lp;
14392 static double xp, yp, zp;
14393 static int n1s, n2s;
14394 static double eps, tol, ptn1, ptn2;
14395 static int next;
14396 int jrand_(int *, int *, int *, int *);
14397 double store_(double *);
14398 int lstptr_(int *, int *, int *, int *);
14399
14400
14401
14402
14403
14404
14405
14406
14407
14408
14409
14410
14411
14412
14413
14414
14415
14416
14417
14418
14419
14420
14421
14422
14423
14424
14425
14426
14427
14428
14429
14430
14431
14432
14433
14434
14435
14436
14437
14438
14439
14440
14441
14442
14443
14444
14445
14446
14447
14448
14449
14450
14451
14452
14453
14454
14455
14456
14457
14458
14459
14460
14461
14462
14463
14464
14465
14466
14467
14468 --p;
14469 --lend;
14470 --z__;
14471 --y;
14472 --x;
14473 --list;
14474 --lptr;
14475
14476
14477
14478
14479
14480
14481
14482
14483
14484
14485
14486
14487
14488
14489
14490
14491
14492
14493
14494
14495
14496
14497
14498
14499
14500
14501
14502
14503
14504
14505
14506
14507
14508
14509
14510
14511
14512
14513
14514
14515
14516
14517
14518
14519
14520
14521
14522
14523 xp = p[1];
14524 yp = p[2];
14525 zp = p[3];
14526 n0 = *nst;
14527 if (n0 < 1 || n0 > *n) {
14528 n0 = jrand_(n, &ix, &iy, &iz);
14529 }
14530
14531
14532
14533 eps = 1.;
14534 L1:
14535 eps /= 2.;
14536 d__1 = eps + 1.;
14537 if (store_(&d__1) > 1.) {
14538 goto L1;
14539 }
14540 eps *= 2.;
14541 tol = eps * 4.;
14542
14543
14544
14545
14546 L2:
14547 lp = lend[n0];
14548 nl = list[lp];
14549 lp = lptr[lp];
14550 nf = list[lp];
14551 n1 = nf;
14552
14553
14554
14555
14556 if (nl > 0) {
14557
14558
14559
14560 L3:
14561 if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14562 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14563 -1e-10) {
14564 lp = lptr[lp];
14565 n1 = list[lp];
14566 if (n1 == nl) {
14567 goto L6;
14568 }
14569 goto L3;
14570 }
14571 } else {
14572
14573
14574
14575 nl = -nl;
14576 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14577 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14578 -1e-10) {
14579
14580
14581
14582 n1 = n0;
14583 n2 = nf;
14584 goto L9;
14585 }
14586 if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14587 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14588 -1e-10) {
14589
14590
14591
14592 n1 = nl;
14593 n2 = n0;
14594 goto L9;
14595 }
14596 }
14597
14598
14599
14600
14601 L4:
14602 lp = lptr[lp];
14603 n2 = (i__1 = list[lp], abs(i__1));
14604 if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14605 n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14606 goto L7;
14607 }
14608 n1 = n2;
14609 if (n1 != nl) {
14610 goto L4;
14611 }
14612 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14613 nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14614 goto L6;
14615 }
14616
14617
14618
14619
14620 d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14621 if (store_(&d__2) < 1. - eps * 4.) {
14622
14623
14624
14625
14626
14627 L5:
14628 if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14629 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14630 -1e-10) {
14631 lp = lptr[lp];
14632 n1 = (i__1 = list[lp], abs(i__1));
14633 if (n1 == nl) {
14634 goto L14;
14635 }
14636 goto L5;
14637 }
14638 }
14639
14640
14641
14642
14643 n0 = n1;
14644 goto L2;
14645
14646
14647
14648 L6:
14649 n2 = nf;
14650
14651
14652
14653
14654
14655 L7:
14656 n3 = n0;
14657 n1s = n1;
14658 n2s = n2;
14659
14660
14661
14662 L8:
14663
14664 *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14665 x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14666 if (*b3 < -1e-10) {
14667
14668
14669
14670
14671 lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14672 if (list[lp] < 0) {
14673 goto L9;
14674 }
14675 lp = lptr[lp];
14676 n4 = (i__1 = list[lp], abs(i__1));
14677
14678
14679
14680 if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14681 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14682 -1e-10) {
14683 n3 = n2;
14684 n2 = n4;
14685 n1s = n1;
14686 if (n2 != n2s && n2 != n0) {
14687 goto L8;
14688 }
14689 } else {
14690 n3 = n1;
14691 n1 = n4;
14692 n2s = n2;
14693 if (n1 != n1s && n1 != n0) {
14694 goto L8;
14695 }
14696 }
14697
14698
14699
14700
14701
14702 n0 = jrand_(n, &ix, &iy, &iz);
14703 goto L2;
14704 }
14705
14706
14707
14708
14709 if (*b3 >= eps) {
14710
14711
14712
14713 *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14714 - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14715 *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14716 - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14717 if (*b1 < -tol || *b2 < -tol) {
14718
14719
14720
14721 n0 = jrand_(n, &ix, &iy, &iz);
14722 goto L2;
14723 }
14724 } else {
14725
14726
14727
14728
14729 *b3 = 0.;
14730 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14731 ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14732 ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14733 *b1 = ptn1 - s12 * ptn2;
14734 *b2 = ptn2 - s12 * ptn1;
14735 if (*b1 < -tol || *b2 < -tol) {
14736
14737
14738
14739 n0 = jrand_(n, &ix, &iy, &iz);
14740 goto L2;
14741 }
14742 }
14743
14744
14745
14746 *i1 = n1;
14747 *i2 = n2;
14748 *i3 = n3;
14749 if (*b1 < 0.f) {
14750 *b1 = 0.f;
14751 }
14752 if (*b2 < 0.f) {
14753 *b2 = 0.f;
14754 }
14755 return 0;
14756
14757
14758
14759
14760
14761 L9:
14762 n1s = n1;
14763 n2s = n2;
14764 nl = 0;
14765
14766
14767
14768 L10:
14769
14770 lp = lend[n2];
14771 lp = lptr[lp];
14772 next = list[lp];
14773 if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14774 - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14775 >= -1e-10) {
14776
14777
14778
14779
14780 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14781 q[0] = x[n1] - s12 * x[n2];
14782 q[1] = y[n1] - s12 * y[n2];
14783 q[2] = z__[n1] - s12 * z__[n2];
14784 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14785 goto L11;
14786 }
14787 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14788 goto L11;
14789 }
14790
14791
14792
14793
14794 nl = n2;
14795 }
14796
14797
14798
14799 n1 = n2;
14800 n2 = next;
14801 if (n2 != n1s) {
14802 goto L10;
14803 }
14804
14805
14806
14807 *i1 = n1s;
14808 *i2 = n1s;
14809 *i3 = 0;
14810 return 0;
14811
14812
14813
14814 L11:
14815 nf = n2;
14816 if (nl == 0) {
14817
14818
14819
14820
14821 n2 = n2s;
14822 n1 = n1s;
14823
14824
14825
14826 L12:
14827 lp = lend[n1];
14828 next = -list[lp];
14829 if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14830 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14831 y[next]) >= -1e-10) {
14832
14833
14834
14835
14836 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14837 q[0] = x[n2] - s12 * x[n1];
14838 q[1] = y[n2] - s12 * y[n1];
14839 q[2] = z__[n2] - s12 * z__[n1];
14840 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14841 goto L13;
14842 }
14843 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14844 goto L13;
14845 }
14846
14847
14848
14849
14850 nf = n1;
14851 }
14852
14853
14854
14855 n2 = n1;
14856 n1 = next;
14857 if (n1 != n1s) {
14858 goto L12;
14859 }
14860
14861
14862
14863 *i1 = n1;
14864 *i2 = n1;
14865 *i3 = 0;
14866 return 0;
14867
14868
14869
14870 L13:
14871 nl = n1;
14872 }
14873
14874
14875
14876 *i1 = nf;
14877 *i2 = nl;
14878 *i3 = 0;
14879 return 0;
14880
14881
14882
14883 L14:
14884 *i1 = 0;
14885 *i2 = 0;
14886 *i3 = 0;
14887 return 0;
14888 }
14889
14890 int trlist_(int *n, int *list, int *lptr,
14891 int *lend, int *nrow, int *nt, int *ltri, int *
14892 ier)
14893 {
14894
14895 int ltri_dim1, ltri_offset, i__1, i__2;
14896
14897
14898 static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
14899 lpl, isv;
14900 static long int arcs;
14901 static int lpln1;
14902
14903
14904
14905
14906
14907
14908
14909
14910
14911
14912
14913
14914
14915
14916
14917
14918
14919
14920
14921
14922
14923
14924
14925
14926
14927
14928
14929
14930
14931
14932
14933
14934
14935
14936
14937
14938
14939
14940
14941
14942
14943
14944
14945
14946
14947
14948
14949
14950
14951
14952
14953
14954
14955
14956
14957
14958
14959
14960
14961
14962
14963
14964
14965
14966
14967
14968
14969
14970
14971
14972
14973
14974
14975
14976
14977
14978
14979
14980
14981
14982
14983
14984
14985
14986
14987
14988
14989
14990
14991
14992
14993
14994
14995
14996
14997
14998
14999
15000
15001
15002
15003
15004
15005
15006 --lend;
15007 --list;
15008 --lptr;
15009 ltri_dim1 = *nrow;
15010 ltri_offset = 1 + ltri_dim1;
15011 ltri -= ltri_offset;
15012
15013
15014 if (*n < 3 || (*nrow != 6 && *nrow != 9)) {
15015 goto L11;
15016 }
15017
15018
15019
15020
15021
15022
15023
15024
15025 arcs = *nrow == 9;
15026 ka = 0;
15027 kt = 0;
15028 nm2 = *n - 2;
15029
15030
15031
15032 i__1 = nm2;
15033 for (n1 = 1; n1 <= i__1; ++n1) {
15034
15035
15036
15037
15038 lpln1 = lend[n1];
15039 lp2 = lpln1;
15040 L1:
15041 lp2 = lptr[lp2];
15042 n2 = list[lp2];
15043 lp = lptr[lp2];
15044 n3 = (i__2 = list[lp], abs(i__2));
15045 if (n2 < n1 || n3 < n1) {
15046 goto L8;
15047 }
15048
15049
15050
15051 ++kt;
15052 ltri[kt * ltri_dim1 + 1] = n1;
15053 ltri[kt * ltri_dim1 + 2] = n2;
15054 ltri[kt * ltri_dim1 + 3] = n3;
15055
15056
15057
15058
15059 for (i__ = 1; i__ <= 3; ++i__) {
15060 if (i__ == 1) {
15061 i1 = n3;
15062 i2 = n2;
15063 } else if (i__ == 2) {
15064 i1 = n1;
15065 i2 = n3;
15066 } else {
15067 i1 = n2;
15068 i2 = n1;
15069 }
15070
15071
15072
15073
15074 lpl = lend[i1];
15075 lp = lptr[lpl];
15076 L2:
15077 if (list[lp] == i2) {
15078 goto L3;
15079 }
15080 lp = lptr[lp];
15081 if (lp != lpl) {
15082 goto L2;
15083 }
15084
15085
15086
15087
15088
15089 if ((i__2 = list[lp], abs(i__2)) != i2) {
15090 goto L12;
15091 }
15092 kn = 0;
15093 if (list[lp] < 0) {
15094 goto L6;
15095 }
15096
15097
15098
15099
15100 L3:
15101 lp = lptr[lp];
15102 i3 = (i__2 = list[lp], abs(i__2));
15103
15104
15105
15106
15107
15108 if (i1 < i2 && i1 < i3) {
15109 j = 3;
15110 } else if (i2 < i3) {
15111 j = 2;
15112 isv = i1;
15113 i1 = i2;
15114 i2 = i3;
15115 i3 = isv;
15116 } else {
15117 j = 1;
15118 isv = i1;
15119 i1 = i3;
15120 i3 = i2;
15121 i2 = isv;
15122 }
15123
15124
15125
15126 if (i1 > n1) {
15127 goto L7;
15128 }
15129
15130
15131
15132
15133 for (kn = kt - 1; kn >= 1; --kn) {
15134 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15135 == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15136 goto L5;
15137 }
15138
15139 }
15140 goto L7;
15141
15142
15143
15144 L5:
15145 ltri[j + 3 + kn * ltri_dim1] = kt;
15146
15147
15148
15149 L6:
15150 ltri[i__ + 3 + kt * ltri_dim1] = kn;
15151 if (arcs) {
15152 ++ka;
15153 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15154 if (kn != 0) {
15155 ltri[j + 6 + kn * ltri_dim1] = ka;
15156 }
15157 }
15158 L7:
15159 ;
15160 }
15161
15162
15163
15164 L8:
15165 if (lp2 != lpln1) {
15166 goto L1;
15167 }
15168
15169 }
15170
15171
15172
15173 *nt = kt;
15174 *ier = 0;
15175 return 0;
15176
15177
15178
15179 L11:
15180 *nt = 0;
15181 *ier = 1;
15182 return 0;
15183
15184
15185
15186
15187 L12:
15188 *nt = 0;
15189 *ier = 2;
15190 return 0;
15191 }
15192
15193 int trlprt_(int *n, double *x, double *y,
15194 double *z__, int *iflag, int *nrow, int *nt, int *
15195 ltri, int *lout)
15196 {
15197
15198
15199 static int nmax = 9999;
15200 static int nlmax = 58;
15201
15202
15203 int ltri_dim1, ltri_offset, i__1;
15204
15205
15206 static int i__, k, na, nb, nl, lun;
15207
15208
15209
15210
15211
15212
15213
15214
15215
15216
15217
15218
15219
15220
15221
15222
15223
15224
15225
15226
15227
15228
15229
15230
15231
15232
15233
15234
15235
15236
15237
15238
15239
15240
15241
15242
15243
15244
15245
15246
15247
15248
15249
15250
15251
15252
15253
15254
15255
15256
15257
15258
15259
15260
15261
15262
15263
15264
15265
15266
15267
15268
15269
15270
15271
15272
15273
15274
15275
15276
15277
15278
15279
15280 --z__;
15281 --y;
15282 --x;
15283 ltri_dim1 = *nrow;
15284 ltri_offset = 1 + ltri_dim1;
15285 ltri -= ltri_offset;
15286
15287
15288
15289
15290
15291
15292
15293
15294
15295
15296
15297
15298
15299
15300
15301
15302 lun = *lout;
15303 if (lun < 0 || lun > 99) {
15304 lun = 6;
15305 }
15306
15307
15308
15309
15310 nl = 3;
15311 if (*n < 3 || *n > nmax || (*nrow != 6 && *nrow != 9) || *nt < 1 || *nt >
15312 nmax) {
15313
15314
15315
15316
15317 return 0;
15318 }
15319 if (*iflag == 0) {
15320
15321
15322
15323
15324 nl = 6;
15325 i__1 = *n;
15326 for (i__ = 1; i__ <= i__1; ++i__) {
15327 if (nl >= nlmax) {
15328
15329 nl = 0;
15330 }
15331
15332 ++nl;
15333
15334 }
15335 } else if (*iflag > 0) {
15336
15337
15338
15339
15340 nl = 6;
15341 i__1 = *n;
15342 for (i__ = 1; i__ <= i__1; ++i__) {
15343 if (nl >= nlmax) {
15344
15345 nl = 0;
15346 }
15347
15348 ++nl;
15349
15350 }
15351 }
15352
15353
15354
15355 if (nl > nlmax / 2) {
15356
15357 nl = 0;
15358 }
15359 if (*nrow == 6) {
15360
15361 } else {
15362
15363 }
15364 nl += 5;
15365 i__1 = *nt;
15366 for (k = 1; k <= i__1; ++k) {
15367 if (nl >= nlmax) {
15368
15369 nl = 0;
15370 }
15371
15372 ++nl;
15373
15374 }
15375
15376
15377
15378
15379 nb = (*n << 1) - *nt - 2;
15380 if (nb < 3) {
15381 nb = 0;
15382 na = *n * 3 - 6;
15383 } else {
15384 na = *nt + *n - 1;
15385 }
15386
15387 return 0;
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 int trmesh_(int *n, double *x, double *y,
15414 double *z__, int *list, int *lptr, int *lend, int
15415 *lnew, int *near__, int *next, double *dist, int *ier)
15416 {
15417
15418 int i__1, i__2;
15419
15420
15421 static double d__;
15422 static int i__, j, k;
15423 static double d1, d2, d3;
15424 static int i0, lp, nn, lpl;
15425 long int left_(double *, double *, double *, double
15426 *, double *, double *, double *, double *,
15427 double *);
15428 static int nexti;
15429
15430
15431
15432
15433
15434
15435
15436
15437
15438
15439
15440
15441
15442
15443
15444
15445
15446
15447
15448
15449
15450
15451
15452
15453
15454
15455
15456
15457
15458
15459
15460
15461
15462
15463
15464
15465
15466
15467
15468
15469
15470
15471
15472
15473
15474
15475
15476
15477
15478
15479
15480
15481
15482
15483
15484
15485
15486
15487
15488
15489
15490
15491
15492
15493
15494
15495
15496
15497
15498
15499
15500
15501
15502
15503
15504
15505
15506
15507
15508
15509
15510
15511
15512
15513
15514
15515
15516
15517
15518
15519
15520
15521
15522
15523
15524
15525
15526
15527
15528
15529
15530
15531
15532
15533
15534
15535
15536
15537
15538
15539
15540
15541
15542
15543
15544
15545
15546
15547
15548
15549
15550
15551
15552
15553
15554
15555
15556
15557
15558
15559
15560
15561
15562
15563
15564
15565
15566
15567
15568
15569
15570
15571
15572
15573
15574
15575
15576
15577
15578
15579
15580
15581
15582
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 --dist;
15656 --next;
15657 --near__;
15658 --lend;
15659 --z__;
15660 --y;
15661 --x;
15662 --list;
15663 --lptr;
15664
15665
15666 nn = *n;
15667 if (nn < 3) {
15668 *ier = -1;
15669 return 0;
15670 }
15671
15672
15673
15674 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15675 z__[3])) {
15676
15677
15678
15679 list[1] = 3;
15680 lptr[1] = 2;
15681 list[2] = -2;
15682 lptr[2] = 1;
15683 lend[1] = 2;
15684
15685 list[3] = 1;
15686 lptr[3] = 4;
15687 list[4] = -3;
15688 lptr[4] = 3;
15689 lend[2] = 4;
15690
15691 list[5] = 2;
15692 lptr[5] = 6;
15693 list[6] = -1;
15694 lptr[6] = 5;
15695 lend[3] = 6;
15696
15697 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15698 y[3], &z__[3])) {
15699
15700
15701
15702
15703
15704 list[1] = 2;
15705 lptr[1] = 2;
15706 list[2] = -3;
15707 lptr[2] = 1;
15708 lend[1] = 2;
15709
15710 list[3] = 3;
15711 lptr[3] = 4;
15712 list[4] = -1;
15713 lptr[4] = 3;
15714 lend[2] = 4;
15715
15716 list[5] = 1;
15717 lptr[5] = 6;
15718 list[6] = -2;
15719 lptr[6] = 5;
15720 lend[3] = 6;
15721
15722 } else {
15723
15724
15725
15726 *ier = -2;
15727 return 0;
15728 }
15729
15730
15731
15732 *lnew = 7;
15733 if (nn == 3) {
15734 *ier = 0;
15735 return 0;
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 near__[1] = 0;
15765 near__[2] = 0;
15766 near__[3] = 0;
15767 for (k = nn; k >= 4; --k) {
15768 d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15769 d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15770 d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15771 if (d1 <= d2 && d1 <= d3) {
15772 near__[k] = 1;
15773 dist[k] = d1;
15774 next[k] = near__[1];
15775 near__[1] = k;
15776 } else if (d2 <= d1 && d2 <= d3) {
15777 near__[k] = 2;
15778 dist[k] = d2;
15779 next[k] = near__[2];
15780 near__[2] = k;
15781 } else {
15782 near__[k] = 3;
15783 dist[k] = d3;
15784 next[k] = near__[3];
15785 near__[3] = k;
15786 }
15787
15788 }
15789
15790
15791
15792 i__1 = nn;
15793 for (k = 4; k <= i__1; ++k) {
15794 addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15795 lend[1], lnew, ier);
15796 if (*ier != 0) {
15797 return 0;
15798 }
15799
15800
15801
15802
15803 i__ = near__[k];
15804 if (near__[i__] == k) {
15805 near__[i__] = next[k];
15806 } else {
15807 i__ = near__[i__];
15808 L2:
15809 i0 = i__;
15810 i__ = next[i0];
15811 if (i__ != k) {
15812 goto L2;
15813 }
15814 next[i0] = next[k];
15815 }
15816 near__[k] = 0;
15817
15818
15819
15820 lpl = lend[k];
15821 lp = lpl;
15822 L3:
15823 lp = lptr[lp];
15824 j = (i__2 = list[lp], abs(i__2));
15825
15826
15827
15828
15829
15830
15831
15832 i__ = near__[j];
15833 L4:
15834 if (i__ == 0) {
15835 goto L5;
15836 }
15837 nexti = next[i__];
15838
15839
15840
15841
15842 d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15843 if (d__ < dist[i__]) {
15844
15845
15846
15847
15848
15849 near__[i__] = k;
15850 dist[i__] = d__;
15851 if (i__ == near__[j]) {
15852 near__[j] = nexti;
15853 } else {
15854 next[i0] = nexti;
15855 }
15856 next[i__] = near__[k];
15857 near__[k] = i__;
15858 } else {
15859 i0 = i__;
15860 }
15861
15862
15863
15864 i__ = nexti;
15865 goto L4;
15866
15867
15868
15869 L5:
15870 if (lp != lpl) {
15871 goto L3;
15872 }
15873
15874 }
15875 return 0;
15876 }
15877
15878 int trplot_(int *lun, double *pltsiz, double *
15879 elat, double *elon, double *a, int *n, double *x,
15880 double *y, double *z__, int *list, int *lptr, int
15881 *lend, char *, long int *numbr, int *ier, short )
15882 {
15883
15884
15885 static long int annot = TRUE_;
15886 static double fsizn = 10.;
15887 static double fsizt = 16.;
15888 static double tol = .5;
15889
15890
15891 int i__1, i__2;
15892 double d__1;
15893
15894
15895
15896
15897
15898
15899
15900 static double t;
15901 static int n0, n1;
15902 static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
15903 static int ir, lp;
15904 static double ex, ey, ez, wr, tx, ty;
15905 static int lpl;
15906 static double wrs;
15907 static int ipx1, ipx2, ipy1, ipy2, nseg;
15908 int drwarc_(int *, double *, double *,
15909 double *, int *);
15910
15911
15912
15913
15914
15915
15916
15917
15918
15919
15920
15921
15922
15923
15924
15925
15926
15927
15928
15929
15930
15931
15932
15933
15934
15935
15936
15937
15938
15939
15940
15941
15942
15943
15944
15945
15946
15947
15948
15949
15950
15951
15952
15953
15954
15955
15956
15957
15958
15959
15960
15961
15962
15963
15964
15965
15966
15967
15968
15969
15970
15971
15972
15973
15974
15975
15976
15977
15978
15979
15980
15981
15982
15983
15984
15985
15986
15987
15988
15989
15990
15991
15992
15993
15994
15995
15996
15997
15998
15999
16000
16001
16002
16003
16004
16005
16006
16007
16008
16009
16010
16011
16012
16013
16014
16015
16016 --lend;
16017 --z__;
16018 --y;
16019 --x;
16020 --list;
16021 --lptr;
16022
16023
16024
16025
16026
16027
16028
16029
16030
16031
16032
16033
16034
16035
16036
16037
16038
16039
16040
16041
16042
16043
16044
16045
16046
16047
16048
16049
16050
16051
16052
16053
16054
16055
16056
16057
16058
16059
16060
16061
16062
16063
16064
16065
16066
16067
16068
16069
16070
16071
16072
16073
16074
16075
16076
16077 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16078 goto L11;
16079 }
16080 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16081 goto L12;
16082 }
16083
16084
16085
16086
16087 cf = atan(1.) / 45.;
16088 wr = sin(cf * *a);
16089 wrs = wr * wr;
16090
16091
16092
16093
16094
16095
16096
16097
16098
16099
16100 d__1 = *pltsiz * 36.;
16101 ir = i_dnnt(&d__1);
16102 ipx1 = 306 - ir;
16103 ipx2 = ir + 306;
16104 ipy1 = 396 - ir;
16105 ipy2 = ir + 396;
16106
16107
16108
16109
16110
16111
16112
16113
16114
16115
16116
16117
16118
16119
16120 d__1 = (double) ir * .88;
16121 ir = i_dnnt(&d__1);
16122 ipx1 = 306 - ir;
16123 ipx2 = ir + 306;
16124 ipy1 = 396 - ir;
16125 ipy2 = ir + 396;
16126
16127
16128
16129
16130 t = 2.;
16131
16132
16133
16134
16135
16136
16137
16138
16139
16140
16141 sf = (double) ir / wr;
16142 tx = ipx1 + sf * wr;
16143 ty = ipy1 + sf * wr;
16144
16145
16146
16147
16148
16149
16150
16151
16152 t = 1. / sf;
16153
16154
16155
16156
16157
16158
16159
16160
16161
16162
16163
16164
16165
16166
16167
16168
16169
16170
16171
16172
16173
16174
16175
16176 t = cf * *elon;
16177 ct = cos(cf * *elat);
16178 ex = ct * cos(t);
16179 ey = ct * sin(t);
16180 ez = sin(cf * *elat);
16181 if (ct != 0.) {
16182 r11 = -ey / ct;
16183 r12 = ex / ct;
16184 } else {
16185 r11 = 0.;
16186 r12 = 1.;
16187 }
16188 r21 = -ez * r12;
16189 r22 = ez * r11;
16190 r23 = ct;
16191
16192
16193
16194
16195 i__1 = *n;
16196 for (n0 = 1; n0 <= i__1; ++n0) {
16197 p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16198 if (p0[2] < 0.) {
16199 goto L3;
16200 }
16201 p0[0] = r11 * x[n0] + r12 * y[n0];
16202 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16203 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16204 goto L3;
16205 }
16206 lpl = lend[n0];
16207 lp = lpl;
16208
16209
16210
16211
16212 L1:
16213 lp = lptr[lp];
16214 n1 = (i__2 = list[lp], abs(i__2));
16215 p1[0] = r11 * x[n1] + r12 * y[n1];
16216 p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16217 p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16218 if (p1[2] < 0.) {
16219
16220
16221
16222
16223
16224 p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16225 p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16226 t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16227 p1[0] /= t;
16228 p1[1] /= t;
16229 }
16230
16231
16232
16233
16234 if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16235 goto L2;
16236 }
16237
16238
16239
16240
16241 if (p1[2] < 0.) {
16242 p1[2] = 0.;
16243 }
16244 d__1 = tol / sf;
16245 drwarc_(lun, p0, p1, &d__1, &nseg);
16246
16247
16248
16249 L2:
16250 if (lp != lpl) {
16251 goto L1;
16252 }
16253 L3:
16254 ;
16255 }
16256
16257
16258
16259
16260
16261
16262
16263 if (*numbr) {
16264
16265
16266
16267
16268
16269 t = fsizn / sf;
16270
16271
16272
16273
16274
16275
16276
16277 i__1 = *n;
16278 for (n0 = 1; n0 <= i__1; ++n0) {
16279 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16280 goto L4;
16281 }
16282 p0[0] = r11 * x[n0] + r12 * y[n0];
16283 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16284 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16285 goto L4;
16286 }
16287
16288
16289
16290
16291
16292
16293
16294
16295
16296 L4:
16297 ;
16298 }
16299 }
16300
16301
16302
16303
16304 t = fsizt / sf;
16305
16306
16307
16308
16309 p0[1] = wr + t * 3.;
16310
16311
16312
16313
16314
16315 if (annot) {
16316
16317
16318
16319 p0[0] = -wr;
16320 p0[1] = -wr - 50. / sf;
16321
16322
16323 p0[1] -= t * 2.;
16324
16325
16326
16327
16328
16329 }
16330
16331
16332
16333
16334
16335
16336
16337
16338
16339
16340
16341
16342
16343
16344
16345
16346
16347
16348 *ier = 0;
16349 return 0;
16350
16351
16352
16353 L11:
16354 *ier = 1;
16355 return 0;
16356
16357
16358
16359 L12:
16360 *ier = 2;
16361 return 0;
16362
16363
16364
16365
16366 *ier = 3;
16367 return 0;
16368 }
16369
16370 int trprnt_(int *n, double *x, double *y,
16371 double *z__, int *iflag, int *list, int *lptr,
16372 int *lend, int *lout)
16373 {
16374
16375
16376 static int nmax = 9999;
16377 static int nlmax = 58;
16378
16379
16380 int i__1;
16381
16382
16383 static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16384 400];
16385
16386
16387
16388
16389
16390
16391
16392
16393
16394
16395
16396
16397
16398
16399
16400
16401
16402
16403
16404
16405
16406
16407
16408
16409
16410
16411
16412
16413
16414
16415
16416
16417
16418
16419
16420
16421
16422
16423
16424
16425
16426
16427
16428
16429
16430
16431
16432
16433
16434
16435
16436
16437
16438
16439
16440
16441
16442
16443
16444
16445
16446
16447 --lend;
16448 --z__;
16449 --y;
16450 --x;
16451 --list;
16452 --lptr;
16453
16454
16455
16456
16457
16458
16459
16460
16461
16462
16463
16464
16465
16466
16467
16468
16469
16470
16471
16472
16473
16474
16475
16476
16477
16478
16479
16480 nn = *n;
16481 lun = *lout;
16482 if (lun < 0 || lun > 99) {
16483 lun = 6;
16484 }
16485
16486
16487
16488
16489 if (nn < 3 || nn > nmax) {
16490
16491
16492
16493
16494 return 0;
16495 }
16496
16497
16498
16499
16500 nl = 6;
16501 nb = 0;
16502 if (*iflag < 0) {
16503
16504
16505
16506
16507
16508 i__1 = nn;
16509 for (node = 1; node <= i__1; ++node) {
16510 lpl = lend[node];
16511 lp = lpl;
16512 k = 0;
16513
16514 L1:
16515 ++k;
16516 lp = lptr[lp];
16517 nd = list[lp];
16518 nabor[k - 1] = nd;
16519 if (lp != lpl) {
16520 goto L1;
16521 }
16522 if (nd <= 0) {
16523
16524
16525
16526
16527
16528 nabor[k - 1] = -nd;
16529 ++k;
16530 nabor[k - 1] = 0;
16531 ++nb;
16532 }
16533
16534
16535
16536 inc = (k - 1) / 14 + 2;
16537 nl += inc;
16538 if (nl > nlmax) {
16539
16540 nl = inc;
16541 }
16542
16543
16544
16545
16546 }
16547 } else if (*iflag > 0) {
16548
16549
16550
16551
16552 i__1 = nn;
16553 for (node = 1; node <= i__1; ++node) {
16554 lpl = lend[node];
16555 lp = lpl;
16556 k = 0;
16557
16558 L3:
16559 ++k;
16560 lp = lptr[lp];
16561 nd = list[lp];
16562 nabor[k - 1] = nd;
16563 if (lp != lpl) {
16564 goto L3;
16565 }
16566 if (nd <= 0) {
16567
16568
16569
16570 nabor[k - 1] = -nd;
16571 ++k;
16572 nabor[k - 1] = 0;
16573 ++nb;
16574 }
16575
16576
16577
16578 inc = (k - 1) / 8 + 2;
16579 nl += inc;
16580 if (nl > nlmax) {
16581
16582 nl = inc;
16583 }
16584
16585
16586
16587
16588
16589 }
16590 } else {
16591
16592
16593
16594
16595 i__1 = nn;
16596 for (node = 1; node <= i__1; ++node) {
16597 lpl = lend[node];
16598 lp = lpl;
16599 k = 0;
16600
16601 L5:
16602 ++k;
16603 lp = lptr[lp];
16604 nd = list[lp];
16605 nabor[k - 1] = nd;
16606 if (lp != lpl) {
16607 goto L5;
16608 }
16609 if (nd <= 0) {
16610
16611
16612
16613 nabor[k - 1] = -nd;
16614 ++k;
16615 nabor[k - 1] = 0;
16616 ++nb;
16617 }
16618
16619
16620
16621 inc = (k - 1) / 5 + 2;
16622 nl += inc;
16623 if (nl > nlmax) {
16624
16625 nl = inc;
16626 }
16627
16628
16629
16630
16631
16632 }
16633 }
16634
16635
16636
16637
16638 if (nb != 0) {
16639 na = nn * 3 - nb - 3;
16640 nt = (nn << 1) - nb - 2;
16641 } else {
16642 na = nn * 3 - 6;
16643 nt = (nn << 1) - 4;
16644 }
16645
16646 return 0;
16647
16648
16649
16650
16651
16652
16653
16654
16655
16656
16657
16658
16659
16660
16661
16662
16663
16664
16665
16666
16667 }
16668
16669 int vrplot_(int *lun, double *pltsiz, double *
16670 elat, double *elon, double *a, int *n, double *x,
16671 double *y, double *z__, int *nt, int *listc, int *
16672 lptr, int *lend, double *xc, double *yc, double *zc,
16673 char *, long int *numbr, int *ier, short)
16674 {
16675
16676
16677 static long int annot = TRUE_;
16678 static double fsizn = 10.;
16679 static double fsizt = 16.;
16680 static double tol = .5;
16681
16682
16683 int i__1;
16684 double d__1;
16685
16686
16687
16688
16689
16690
16691
16692 static double t;
16693 static int n0;
16694 static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16695 sf;
16696 static int ir, lp;
16697 static double ex, ey, ez, wr, tx, ty;
16698 static long int in1, in2;
16699 static int kv1, kv2, lpl;
16700 static double wrs;
16701 static int ipx1, ipx2, ipy1, ipy2, nseg;
16702 int drwarc_(int *, double *, double *,
16703 double *, int *);
16704
16705
16706
16707
16708
16709
16710
16711
16712
16713
16714
16715
16716
16717
16718
16719
16720
16721
16722
16723
16724
16725
16726
16727
16728
16729
16730
16731
16732
16733
16734
16735
16736
16737
16738
16739
16740
16741
16742
16743
16744
16745
16746
16747
16748
16749
16750
16751
16752
16753
16754
16755
16756
16757
16758
16759
16760
16761
16762
16763
16764
16765
16766
16767
16768
16769
16770
16771
16772
16773
16774
16775
16776
16777
16778
16779
16780
16781
16782
16783
16784
16785
16786
16787
16788
16789
16790
16791
16792
16793
16794
16795
16796
16797
16798
16799
16800
16801
16802
16803
16804
16805
16806
16807
16808
16809
16810
16811
16812
16813
16814
16815
16816
16817
16818
16819
16820
16821
16822
16823
16824
16825
16826
16827
16828
16829
16830
16831
16832
16833
16834
16835
16836
16837
16838
16839
16840
16841
16842
16843
16844
16845
16846 --lend;
16847 --z__;
16848 --y;
16849 --x;
16850 --zc;
16851 --yc;
16852 --xc;
16853 --listc;
16854 --lptr;
16855
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 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
16914 nt != 2 * *n - 4) {
16915 goto L11;
16916 }
16917 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16918 goto L12;
16919 }
16920
16921
16922
16923
16924 cf = atan(1.) / 45.;
16925 wr = sin(cf * *a);
16926 wrs = wr * wr;
16927
16928
16929
16930
16931
16932
16933
16934
16935
16936
16937 d__1 = *pltsiz * 36.;
16938 ir = i_dnnt(&d__1);
16939 ipx1 = 306 - ir;
16940 ipx2 = ir + 306;
16941 ipy1 = 396 - ir;
16942 ipy2 = ir + 396;
16943
16944
16945
16946
16947
16948
16949
16950
16951
16952
16953
16954
16955
16956 d__1 = (double) ir * .88;
16957 ir = i_dnnt(&d__1);
16958 ipx1 = 306 - ir;
16959 ipx2 = ir + 306;
16960 ipy1 = 396 - ir;
16961 ipy2 = ir + 396;
16962
16963
16964
16965
16966 t = 2.;
16967
16968
16969
16970
16971
16972
16973
16974
16975
16976
16977 sf = (double) ir / wr;
16978 tx = ipx1 + sf * wr;
16979 ty = ipy1 + sf * wr;
16980
16981
16982
16983
16984
16985
16986
16987
16988 t = 1. / sf;
16989
16990
16991
16992
16993
16994
16995
16996
16997
16998
16999
17000
17001
17002
17003
17004
17005
17006
17007
17008
17009
17010
17011
17012 t = cf * *elon;
17013 ct = cos(cf * *elat);
17014 ex = ct * cos(t);
17015 ey = ct * sin(t);
17016 ez = sin(cf * *elat);
17017 if (ct != 0.) {
17018 r11 = -ey / ct;
17019 r12 = ex / ct;
17020 } else {
17021 r11 = 0.;
17022 r12 = 1.;
17023 }
17024 r21 = -ez * r12;
17025 r22 = ez * r11;
17026 r23 = ct;
17027
17028
17029
17030
17031 i__1 = *n;
17032 for (n0 = 1; n0 <= i__1; ++n0) {
17033 lpl = lend[n0];
17034
17035
17036
17037
17038 kv2 = listc[lpl];
17039 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17040 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17041 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17042
17043
17044
17045 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17046
17047
17048
17049
17050 lp = lpl;
17051 L1:
17052 lp = lptr[lp];
17053 kv1 = kv2;
17054 p1[0] = p2[0];
17055 p1[1] = p2[1];
17056 p1[2] = p2[2];
17057 in1 = in2;
17058 kv2 = listc[lp];
17059
17060
17061
17062 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17063 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17064 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17065 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17066
17067
17068
17069
17070
17071 if (! in1 || (in2 && kv2 <= kv1)) {
17072 goto L2;
17073 }
17074 if (p2[2] < 0.) {
17075
17076
17077
17078
17079
17080 p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17081 p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17082 t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17083 p2[0] /= t;
17084 p2[1] /= t;
17085 }
17086
17087
17088
17089
17090 if (p2[2] < 0.) {
17091 p2[2] = 0.f;
17092 }
17093 d__1 = tol / sf;
17094 drwarc_(lun, p1, p2, &d__1, &nseg);
17095
17096
17097
17098 L2:
17099 if (lp != lpl) {
17100 goto L1;
17101 }
17102
17103 }
17104
17105
17106
17107
17108
17109
17110
17111 if (*numbr) {
17112
17113
17114
17115
17116
17117 t = fsizn / sf;
17118
17119
17120
17121
17122
17123
17124
17125 i__1 = *n;
17126 for (n0 = 1; n0 <= i__1; ++n0) {
17127 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17128 goto L4;
17129 }
17130 x0 = r11 * x[n0] + r12 * y[n0];
17131 y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17132 if (x0 * x0 + y0 * y0 > wrs) {
17133 goto L4;
17134 }
17135
17136
17137
17138
17139
17140
17141
17142
17143 L4:
17144 ;
17145 }
17146 }
17147
17148
17149
17150
17151 t = fsizt / sf;
17152
17153
17154
17155
17156 y0 = wr + t * 3.;
17157
17158
17159
17160
17161
17162 if (annot) {
17163
17164
17165
17166 x0 = -wr;
17167 y0 = -wr - 50. / sf;
17168
17169
17170 y0 -= t * 2.;
17171
17172
17173
17174
17175
17176 }
17177
17178
17179
17180
17181
17182
17183
17184
17185
17186
17187
17188
17189
17190
17191
17192
17193
17194
17195 *ier = 0;
17196 return 0;
17197
17198
17199
17200 L11:
17201 *ier = 1;
17202 return 0;
17203
17204
17205
17206 L12:
17207 *ier = 2;
17208 return 0;
17209
17210
17211
17212
17213 *ier = 3;
17214 return 0;
17215 }
17216
17217 int random_(int *ix, int *iy, int *iz,
17218 double *rannum)
17219 {
17220 static double x;
17221
17222
17223
17224
17225
17226
17227
17228
17229
17230
17231
17232
17233
17234
17235
17236 *ix = *ix * 171 % 30269;
17237 *iy = *iy * 172 % 30307;
17238 *iz = *iz * 170 % 30323;
17239 x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17240 double) (*iz) / 30323.;
17241 *rannum = x - (int) x;
17242 return 0;
17243 }
17244
17245 #undef TRUE_
17246 #undef FALSE_
17247 #undef abs
17248
17249
17250
17251
17252
17253
17254
17255
17256
17257 EMData* Util::mult_scalar(EMData* img, float scalar)
17258 {
17259 ENTERFUNC;
17260
17261 if (!img) {
17262 throw NullPointerException("NULL input image");
17263 }
17264
17265
17266 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17267 size_t size = (size_t)nx*ny*nz;
17268 EMData * img2 = img->copy_head();
17269 float *img_ptr = img->get_data();
17270 float *img2_ptr = img2->get_data();
17271 for (size_t i=0;i<size;++i)img2_ptr[i] = img_ptr[i]*scalar;
17272 img2->update();
17273
17274 if(img->is_complex()) {
17275 img2->set_complex(true);
17276 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17277 }
17278 EXITFUNC;
17279 return img2;
17280 }
17281
17282 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17283 {
17284 ENTERFUNC;
17285
17286 if (!img) {
17287 throw NullPointerException("NULL input image");
17288 }
17289
17290
17291 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17292 size_t size = (size_t)nx*ny*nz;
17293 EMData * img2 = img->copy_head();
17294 float *img_ptr = img->get_data();
17295 float *img2_ptr = img2->get_data();
17296 float *img1_ptr = img1->get_data();
17297 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17298 img2->update();
17299 if(img->is_complex()) {
17300 img2->set_complex(true);
17301 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17302 }
17303
17304 EXITFUNC;
17305 return img2;
17306 }
17307
17308 EMData* Util::addn_img(EMData* img, EMData* img1)
17309 {
17310 ENTERFUNC;
17311
17312 if (!img) {
17313 throw NullPointerException("NULL input image");
17314 }
17315
17316
17317 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17318 size_t size = (size_t)nx*ny*nz;
17319 EMData * img2 = img->copy_head();
17320 float *img_ptr = img->get_data();
17321 float *img2_ptr = img2->get_data();
17322 float *img1_ptr = img1->get_data();
17323 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17324 img2->update();
17325 if(img->is_complex()) {
17326 img2->set_complex(true);
17327 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17328 }
17329
17330 EXITFUNC;
17331 return img2;
17332 }
17333
17334 EMData* Util::subn_img(EMData* img, EMData* img1)
17335 {
17336 ENTERFUNC;
17337
17338 if (!img) {
17339 throw NullPointerException("NULL input image");
17340 }
17341
17342
17343 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17344 size_t size = (size_t)nx*ny*nz;
17345 EMData * img2 = img->copy_head();
17346 float *img_ptr = img->get_data();
17347 float *img2_ptr = img2->get_data();
17348 float *img1_ptr = img1->get_data();
17349 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17350 img2->update();
17351 if(img->is_complex()) {
17352 img2->set_complex(true);
17353 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17354 }
17355
17356 EXITFUNC;
17357 return img2;
17358 }
17359
17360 EMData* Util::muln_img(EMData* img, EMData* img1)
17361 {
17362 ENTERFUNC;
17363
17364 if (!img) {
17365 throw NullPointerException("NULL input image");
17366 }
17367
17368
17369 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17370 size_t size = (size_t)nx*ny*nz;
17371 EMData * img2 = img->copy_head();
17372 float *img_ptr = img->get_data();
17373 float *img2_ptr = img2->get_data();
17374 float *img1_ptr = img1->get_data();
17375 if(img->is_complex()) {
17376 for (size_t i=0; i<size; i+=2) {
17377 img2_ptr[i] = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17378 img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17379 }
17380 img2->set_complex(true);
17381 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17382 } else {
17383 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17384 img2->update();
17385 }
17386
17387 EXITFUNC;
17388 return img2;
17389 }
17390
17391 EMData* Util::divn_img(EMData* img, EMData* img1)
17392 {
17393 ENTERFUNC;
17394
17395 if (!img) {
17396 throw NullPointerException("NULL input image");
17397 }
17398
17399
17400 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17401 size_t size = (size_t)nx*ny*nz;
17402 EMData * img2 = img->copy_head();
17403 float *img_ptr = img->get_data();
17404 float *img2_ptr = img2->get_data();
17405 float *img1_ptr = img1->get_data();
17406 if(img->is_complex()) {
17407 float sq2;
17408 for (size_t i=0; i<size; i+=2) {
17409 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17410 img2_ptr[i] = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17411 img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17412 }
17413 img2->set_complex(true);
17414 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17415 } else {
17416 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17417 img2->update();
17418 }
17419
17420 EXITFUNC;
17421 return img2;
17422 }
17423
17424 EMData* Util::divn_filter(EMData* img, EMData* img1)
17425 {
17426 ENTERFUNC;
17427
17428 if (!img) {
17429 throw NullPointerException("NULL input image");
17430 }
17431
17432
17433 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17434 size_t size = (size_t)nx*ny*nz;
17435 EMData * img2 = img->copy_head();
17436 float *img_ptr = img->get_data();
17437 float *img1_ptr = img1->get_data();
17438 float *img2_ptr = img2->get_data();
17439 if(img->is_complex()) {
17440 for (size_t i=0; i<size; i+=2) {
17441 if(img1_ptr[i] > 1.e-10f) {
17442 img2_ptr[i] = img_ptr[i] /img1_ptr[i];
17443 img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17444 } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17445 }
17446 } else throw ImageFormatException("Only Fourier image allowed");
17447
17448 img->update();
17449
17450 EXITFUNC;
17451 return img2;
17452 }
17453
17454 void Util::mul_scalar(EMData* img, float scalar)
17455 {
17456 ENTERFUNC;
17457
17458 if (!img) {
17459 throw NullPointerException("NULL input image");
17460 }
17461
17462
17463 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17464 size_t size = (size_t)nx*ny*nz;
17465 float *img_ptr =img->get_data();
17466 for (size_t i=0;i<size;++i) img_ptr[i] *= scalar;
17467 img->update();
17468
17469 EXITFUNC;
17470 }
17471
17472 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17473 {
17474 ENTERFUNC;
17475
17476 if (!img) {
17477 throw NullPointerException("NULL input image");
17478 }
17479
17480
17481 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17482 size_t size = (size_t)nx*ny*nz;
17483 float *img_ptr = img->get_data();
17484 float *img1_ptr = img1->get_data();
17485 for (size_t i=0;i<size;++i)img_ptr[i] += img1_ptr[i]*scalar;
17486 img1->update();
17487
17488 EXITFUNC;
17489 }
17490
17491 void Util::add_img(EMData* img, EMData* img1)
17492 {
17493 ENTERFUNC;
17494
17495 if (!img || !img1) {
17496 throw NullPointerException("NULL input image");
17497 }
17498
17499
17500 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17501 size_t size = (size_t)nx*ny*nz;
17502 float *img_ptr = img->get_data();
17503 float *img1_ptr = img1->get_data();
17504 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i];
17505 img->update();
17506
17507 EXITFUNC;
17508 }
17509
17510 void Util::add_img_abs(EMData* img, EMData* img1)
17511 {
17512 ENTERFUNC;
17513
17514 if (!img) {
17515 throw NullPointerException("NULL input image");
17516 }
17517
17518
17519 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17520 size_t size = (size_t)nx*ny*nz;
17521 float *img_ptr = img->get_data();
17522 float *img1_ptr = img1->get_data();
17523 for (size_t i=0;i<size;++i) img_ptr[i] += abs(img1_ptr[i]);
17524 img->update();
17525
17526 EXITFUNC;
17527 }
17528
17529 void Util::add_img2(EMData* img, EMData* img1)
17530 {
17531 ENTERFUNC;
17532
17533 if (!img) {
17534 throw NullPointerException("NULL input image");
17535 }
17536
17537
17538 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17539 size_t size = (size_t)nx*ny*nz;
17540 float *img_ptr = img->get_data();
17541 float *img1_ptr = img1->get_data();
17542 if(img->is_complex()) {
17543 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] ;
17544 } else {
17545 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17546 }
17547 img->update();
17548
17549 EXITFUNC;
17550 }
17551
17552 void Util::sub_img(EMData* img, EMData* img1)
17553 {
17554 ENTERFUNC;
17555
17556 if (!img) {
17557 throw NullPointerException("NULL input image");
17558 }
17559
17560
17561 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17562 size_t size = (size_t)nx*ny*nz;
17563 float *img_ptr = img->get_data();
17564 float *img1_ptr = img1->get_data();
17565 for (size_t i=0;i<size;++i) img_ptr[i] -= img1_ptr[i];
17566 img->update();
17567
17568 EXITFUNC;
17569 }
17570
17571 void Util::mul_img(EMData* img, EMData* img1)
17572 {
17573 ENTERFUNC;
17574
17575 if (!img) {
17576 throw NullPointerException("NULL input image");
17577 }
17578
17579
17580 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17581 size_t size = (size_t)nx*ny*nz;
17582 float *img_ptr = img->get_data();
17583 float *img1_ptr = img1->get_data();
17584 if(img->is_complex()) {
17585 for (size_t i=0; i<size; i+=2) {
17586 float tmp = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17587 img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17588 img_ptr[i] = tmp;
17589
17590 }
17591 } else {
17592 for (size_t i=0;i<size;++i) img_ptr[i] *= img1_ptr[i];
17593 }
17594 img->update();
17595
17596 EXITFUNC;
17597 }
17598
17599 void Util::div_img(EMData* img, EMData* img1)
17600 {
17601 ENTERFUNC;
17602
17603 if (!img) {
17604 throw NullPointerException("NULL input image");
17605 }
17606
17607
17608 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17609 size_t size = (size_t)nx*ny*nz;
17610 float *img_ptr = img->get_data();
17611 float *img1_ptr = img1->get_data();
17612 if(img->is_complex()) {
17613 float sq2;
17614 for (size_t i=0; i<size; i+=2) {
17615 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17616 float tmp = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17617 img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17618 img_ptr[i] = tmp;
17619 }
17620 } else {
17621 for (size_t i=0; i<size; ++i) img_ptr[i] /= img1_ptr[i];
17622 }
17623 img->update();
17624
17625 EXITFUNC;
17626 }
17627
17628 void Util::div_filter(EMData* img, EMData* img1)
17629 {
17630 ENTERFUNC;
17631
17632 if (!img) {
17633 throw NullPointerException("NULL input image");
17634 }
17635
17636
17637 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17638 size_t size = (size_t)nx*ny*nz;
17639 float *img_ptr = img->get_data();
17640 float *img1_ptr = img1->get_data();
17641 if(img->is_complex()) {
17642 for (size_t i=0; i<size; i+=2) {
17643 if(img1_ptr[i] > 1.e-10f) {
17644 img_ptr[i] /= img1_ptr[i];
17645 img_ptr[i+1] /= img1_ptr[i];
17646 } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17647 }
17648 } else throw ImageFormatException("Only Fourier image allowed");
17649
17650 img->update();
17651
17652 EXITFUNC;
17653 }
17654
17655 #define img_ptr(i,j,k) img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*(size_t)nxo]
17656
17657 EMData* Util::pack_complex_to_real(EMData* img)
17658 {
17659 ENTERFUNC;
17660
17661 if (!img) {
17662 throw NullPointerException("NULL input image");
17663 }
17664
17665
17666
17667 int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17668 int nx = nxo - 2 + img->is_fftodd();
17669 int lsd2 = (nx + 2 - nx%2) / 2;
17670 int nyt, nzt;
17671 int nx2 = nx/2;
17672 int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17673 int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17674 int nx2p = nx2+nx%2;
17675 int ny2p = ny2+ny%2;
17676 int nz2p = nz2+nz%2;
17677 EMData& power = *(new EMData());
17678 power.set_size(nx, ny, nz);
17679 power.set_array_offsets(-nx2,-ny2,-nz2);
17680
17681 float *img_ptr = img->get_data();
17682 for (int iz = 1; iz <= nz; iz++) {
17683 int jz=iz-1;
17684 if(jz>=nz2p) jz=jz-nzt;
17685 for (int iy = 1; iy <= ny; iy++) {
17686 int jy=iy-1;
17687 if(jy>=ny2p) jy=jy-nyt;
17688 for (int ix = 1; ix <= lsd2; ix++) {
17689 int jx=ix-1;
17690 if(jx>=nx2p) jx=jx-nx;
17691 power(jx,jy,jz) = img_ptr(ix,iy,iz);
17692 }
17693 }
17694 }
17695
17696 int nzb, nze, nyb, nye, nxb, nxe;
17697 nxb =-nx2+(nx+1)%2;
17698 nxe = nx2-(nx+1)%2;
17699 if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17700 if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17701 for (int iz = nzb; iz <= nze; iz++) {
17702 for (int iy = nyb; iy <= nye; iy++) {
17703 for (int ix = 1; ix <= nxe; ix++) {
17704 power(-ix,-iy,-iz) = power(ix,iy,iz);
17705 }
17706 }
17707 }
17708 if(ny2 != 0) {
17709 if(nz2 != 0) {
17710 if(nz%2 == 0) {
17711 for (int iy = nyb; iy <= nye; iy++) {
17712 for (int ix = nxb; ix <= -1; ix++) {
17713 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17714 }
17715 }
17716 if(ny%2 == 0) {
17717 for (int ix = nxb; ix <= -1; ix++) {
17718 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17719 }
17720 }
17721 }
17722 }
17723 if(ny%2 == 0) {
17724 for (int iz = nzb; iz <= nze; iz++) {
17725 for (int ix = nxb; ix <= -1; ix++) {
17726 power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17727 }
17728 }
17729 }
17730
17731 }
17732 power.update();
17733 power.set_array_offsets(0,0,0);
17734 return &power;
17735 }
17736 #undef img_ptr
17737
17738 float Util::ang_n(float peakp, string mode, int maxrin)
17739 {
17740 if (mode == "f" || mode == "F")
17741 return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17742 else
17743 return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17744 }
17745
17746
17747 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17748 {
17749 float* data = ring->get_data();
17750 float av=0.0;
17751 float sq=0.0;
17752 float nn=0.0;
17753 int nring = numr.size()/3;
17754 for( int i=0; i < nring; ++i )
17755 {
17756 int numr3i = numr[3*i+2];
17757 int numr2i = numr[3*i+1]-1;
17758 float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17759 for( int j=0; j < numr3i; ++j )
17760 {
17761 int jc = numr2i+j;
17762 av += data[jc] * w;
17763 sq += data[jc] * data[jc] * w;
17764 nn += w;
17765 }
17766 }
17767
17768 float avg = av/nn;
17769 float sgm = sqrt( (sq-av*av/nn)/nn );
17770 size_t n = (size_t)ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17771 for( size_t i=0; i < n; ++i )
17772 {
17773 data[i] -= avg;
17774 data[i] /= sgm;
17775 }
17776
17777 ring->update();
17778 }
17779
17780 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17781 float xrng, float yrng, float step, string mode,
17782 vector<int>numr, float cnx, float cny) {
17783
17784
17785
17786
17787
17788
17789
17790
17791
17792
17793
17794
17795 size_t crefim_len = crefim.size();
17796
17797 int ky = int(2*yrng/step+0.5)/2;
17798 int kx = int(2*xrng/step+0.5)/2;
17799 int iref, nref=0, mirror=0;
17800 float iy, ix, sx=0, sy=0;
17801 float peak = -1.0E23f;
17802 float ang=0.0f;
17803 for (int i = -ky; i <= ky; i++) {
17804 iy = i * step ;
17805 for (int j = -kx; j <= kx; j++) {
17806 ix = j*step ;
17807 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17808
17809 Normalize_ring( cimage, numr );
17810
17811 Frngs(cimage, numr);
17812
17813
17814 for ( iref = 0; iref < (int)crefim_len; iref++) {
17815 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17816 double qn = retvals["qn"];
17817 double qm = retvals["qm"];
17818 if(qn >= peak || qm >= peak) {
17819 sx = -ix;
17820 sy = -iy;
17821 nref = iref;
17822 if (qn >= qm) {
17823 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17824 peak = static_cast<float>(qn);
17825 mirror = 0;
17826 } else {
17827 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17828 peak = static_cast<float>(qm);
17829 mirror = 1;
17830 }
17831 }
17832 } delete cimage; cimage = 0;
17833 }
17834 }
17835 float co, so, sxs, sys;
17836 co = static_cast<float>( cos(ang*pi/180.0) );
17837 so = static_cast<float>( -sin(ang*pi/180.0) );
17838 sxs = sx*co - sy*so;
17839 sys = sx*so + sy*co;
17840 vector<float> res;
17841 res.push_back(ang);
17842 res.push_back(sxs);
17843 res.push_back(sys);
17844 res.push_back(static_cast<float>(mirror));
17845 res.push_back(static_cast<float>(nref));
17846 res.push_back(peak);
17847 return res;
17848 }
17849
17850 vector<float> Util::multiref_polar_ali_2d_peaklist(EMData* image, const vector< EMData* >& crefim,
17851 float xrng, float yrng, float step, string mode,
17852 vector<int>numr, float cnx, float cny) {
17853
17854 size_t crefim_len = crefim.size();
17855
17856 int ky = int(2*yrng/step+0.5)/2;
17857 int kx = int(2*xrng/step+0.5)/2;
17858 float iy, ix;
17859 vector<float> peak(crefim_len*5, -1.0e23f);
17860 for (int i = -ky; i <= ky; i++) {
17861 iy = i * step ;
17862 for (int j = -kx; j <= kx; j++) {
17863 ix = j*step ;
17864 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17865 Normalize_ring( cimage, numr );
17866 Frngs(cimage, numr);
17867 for (int iref = 0; iref < (int)crefim_len; iref++) {
17868 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17869 double qn = retvals["qn"];
17870 double qm = retvals["qm"];
17871 if(qn >= peak[iref*5] || qm >= peak[iref*5]) {
17872 if (qn >= qm) {
17873 peak[iref*5] = static_cast<float>(qn);
17874 peak[iref*5+1] = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17875 peak[iref*5+2] = -ix;
17876 peak[iref*5+3] = -iy;
17877 peak[iref*5+4] = 0;
17878 } else {
17879 peak[iref*5] = static_cast<float>(qm);
17880 peak[iref*5+1] = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17881 peak[iref*5+2] = -ix;
17882 peak[iref*5+3] = -iy;
17883 peak[iref*5+4] = 1;
17884 }
17885 }
17886 } delete cimage; cimage = 0;
17887 }
17888 }
17889 for (int iref = 0; iref < (int)crefim_len; iref++) {
17890 float ang = peak[iref*5+1];
17891 float sx = peak[iref*5+2];
17892 float sy = peak[iref*5+3];
17893 float co = cos(ang*pi/180.0);
17894 float so = -sin(ang*pi/180.0);
17895 float sxs = sx*co - sy*so;
17896 float sys = sx*so + sy*co;
17897 peak[iref*5+2] = sxs;
17898 peak[iref*5+3] = sys;
17899 }
17900 return peak;
17901 }
17902
17903 struct assign_groups_comparator {
17904 const float * values;
17905 bool operator() (int i,int j) { return (values[i] > values[j]); }
17906 assign_groups_comparator(const float * v) : values(v) {}
17907 };
17908
17909 vector<int> Util::assign_groups(std::string matrix_address, int nref, int nima)
17910 {
17911 const float * matrix;
17912 {
17913 size_t addr = 0;
17914 for ( std::string::const_iterator i = matrix_address.begin(); i != matrix_address.end(); ++i ) {
17915 int digit = *i - '0';
17916 addr *= 10;
17917 addr += digit;
17918 }
17919 matrix = reinterpret_cast<float*>(addr);
17920 }
17921
17922 int kt = nref;
17923 unsigned int maxasi = nima/nref;
17924 vector< vector<int> > id_list(nref);
17925 int group, ima;
17926
17927 {
17928 int begin = 0;
17929
17930 std::vector<int> dd(nref*nima);
17931 for (int i=0; i<nref*nima; i++) {
17932 dd[i] = i;
17933 }
17934 assign_groups_comparator comparator(matrix);
17935 sort(dd.begin(), dd.end(), comparator);
17936
17937 std::vector<bool> del_row(nref, false);
17938 std::vector<bool> del_column(nima, false);
17939 while (kt > 0) {
17940 bool flag = true;
17941 while (flag) {
17942 int l = dd[begin];
17943 group = l/nima;
17944 ima = l%nima;
17945 if (del_column[ima] || del_row[group]) begin++;
17946 else flag = false;
17947 }
17948
17949 id_list[group].push_back(ima);
17950 if (kt > 1) {
17951 if (id_list[group].size() < maxasi) group = -1;
17952 else kt -= 1;
17953 } else {
17954 if (id_list[group].size() < maxasi+nima%nref) group = -1;
17955 else kt -= 1;
17956 }
17957 del_column[ima] = true;
17958 if (group != -1) {
17959 del_row[group] = true;
17960 }
17961 }
17962 }
17963
17964 vector<int> id_list_1;
17965 for (int iref=0; iref<nref; iref++)
17966 for (unsigned int im=0; im<maxasi; im++)
17967 id_list_1.push_back(id_list[iref][im]);
17968 for (unsigned int im=maxasi; im<maxasi+nima%nref; im++)
17969 id_list_1.push_back(id_list[group][im]);
17970 id_list_1.push_back(group);
17971
17972 return id_list_1;
17973 }
17974
17975 int Util::nearest_ang(const vector<float>& vecref, float x, float y, float z) {
17976 float best_v = -1.0f;
17977 int best_i = -1;
17978
17979 for (unsigned int i=0; i<vecref.size()/3; i++) {
17980 float v = abs(vecref[i*3]*x+vecref[i*3+1]*y+vecref[i*3+2]*z);
17981 if (v > best_v) {
17982 best_v = v;
17983 best_i = i;
17984 }
17985 }
17986 return best_i;
17987 }
17988
17989 struct d_ang {
17990 float d;
17991 int i;
17992 int mirror;
17993 d_ang(float _d, int _i, int _m):d(_d), i(_i), mirror(_m) {}
17994 bool operator<(const d_ang& a) const { return d < a.d || (d == a.d && i < a.i); }
17995 };
17996
17997 vector<int> Util::assign_projangles(const vector<float>& projangles, const vector<float>& refangles) {
17998 int nref = refangles.size()/2;
17999 int nproj = projangles.size()/2;
18000 vector<int> asg(nproj);
18001 vector<float> vecref(nref*3);
18002 for (int i=0; i<nref; i++)
18003 getvec(refangles[i*2], refangles[i*2+1], vecref[i*3], vecref[i*3+1], vecref[i*3+2]);
18004 for (int i=0; i<nproj; i++) {
18005 float x, y, z;
18006 getvec(projangles[i*2], projangles[i*2+1], x, y, z);
18007 asg[i] = nearest_ang(vecref, x, y, z);
18008 }
18009 return asg;
18010 }
18011
18012
18013 vector<int> Util::nearestk_to_refdir(const vector<float>& projangles, const vector<float>& refangles, const int howmany) {
18014 int nref = refangles.size()/2;
18015 int nproj = projangles.size()/2;
18016 vector<int> asg(howmany*nref);
18017 vector<float> vecproj(nproj*3);
18018 for (int i=0; i<nproj; i++)
18019 getvec(projangles[i*2], projangles[i*2+1], vecproj[i*3], vecproj[i*3+1], vecproj[i*3+2]);
18020
18021
18022 vector<bool> taken(nproj);
18023 for (int k=0; k<nref; k++) {
18024 for (int i=0; i<nproj; i++) taken[i] = true;
18025 float x, y, z;
18026 getvec(refangles[k*2], refangles[k*2+1], x, y, z);
18027 for (int h=0; h<howmany; h++) {
18028 float best_v = -1.0f;
18029 int best_i = -1;
18030 for (int i=0; i<nproj; i++) {
18031 if( taken[i] ) {
18032 float v = abs(vecproj[i*3]*x+vecproj[i*3+1]*y+vecproj[i*3+2]*z);
18033 if (v > best_v) {
18034 best_v = v;
18035 best_i = i;
18036 }
18037 }
18038 }
18039 asg[k*howmany + h] = best_i;
18040 taken[best_i] = false;
18041 }
18042 }
18043 return asg;
18044 }
18045
18046
18047 vector<int> Util::group_proj_by_phitheta(const vector<float>& projangles, const vector<float>& ref_ang, const int img_per_grp) {
18048 float c = 100.0;
18049 int L = max(100, img_per_grp);
18050 int N = projangles.size()/2;
18051
18052 int sz = ref_ang.size();
18053 int nref1 = ref_ang[sz-4];
18054 int nref2 = ref_ang[sz-3];
18055 int nref3 = ref_ang[sz-2];
18056 int nref4 = ref_ang[sz-1];
18057 int nref;
18058
18059 set<int> pt;
18060 for (int i=0; i<N; i++) pt.insert(i);
18061 vector<float> v(N*3, 0.0f);
18062 for (int i=0; i<N; i++)
18063 getvec(projangles[i*2], projangles[i*2+1], v[i*3], v[i*3+1], v[i*3+2], 1);
18064
18065 int previous_group = -1;
18066 int previous_zone = 5;
18067 int max_group = 0;
18068 vector<float> ref_ang_list;
18069 vector<float> diff_table;
18070 map<int, int> diff_table_index;
18071 vector<int> proj_list;
18072 vector<int> sg;
18073 vector<int> remain_index;
18074 vector<int> asg;
18075 int mirror;
18076 for (int grp=0; grp<N/img_per_grp; grp++) {
18077 int N_remain = N-grp*img_per_grp;
18078 assert(N_remain == static_cast<int>(pt.size()));
18079 if (N_remain >= nref4*L) {
18080 if (previous_zone > 4) {
18081 ref_ang_list.resize(nref4*2);
18082 for (int i=0; i<nref4*2; i++) ref_ang_list[i] = ref_ang[(nref1+nref2+nref3)*2+i];
18083 nref = nref4;
18084 previous_group = -1;
18085 previous_zone = 4;
18086 }
18087 } else if (N_remain >= nref3*L) {
18088 if (previous_zone > 3) {
18089 ref_ang_list.resize(nref3*2);
18090 for (int i=0; i<nref3*2; i++) ref_ang_list[i] = ref_ang[(nref1+nref2)*2+i];
18091 nref = nref3;
18092 previous_group = -1;
18093 previous_zone = 3;
18094 }
18095 } else if (N_remain >= nref2*L) {
18096 if (previous_zone > 2) {
18097 ref_ang_list.resize(nref2*2);
18098 for (int i=0; i<nref2*2; i++) ref_ang_list[i] = ref_ang[nref1*2+i];
18099 nref = nref2;
18100 previous_group = -1;
18101 previous_zone = 2;
18102 }
18103 } else if (N_remain >= nref1*L) {
18104 if (previous_zone > 1) {
18105 ref_ang_list.resize(nref1*2);
18106 for (int i=0; i<nref1*2; i++) ref_ang_list[i] = ref_ang[i];
18107 nref = nref1;
18108 previous_group = -1;
18109 previous_zone = 1;
18110 }
18111 } else if (previous_zone > 0) {
18112 previous_group = -1;
18113 previous_zone = 0;
18114 }
18115
18116 vector<int> index;
18117 if (N_remain >= nref1*L) {
18118 if (previous_group == -1) {
18119 vector<float> proj_ang_list(N_remain*2, 0.0f);
18120 remain_index.resize(N_remain);
18121 int l = 0;
18122 for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si) {
18123 remain_index[l] = (*si);
18124 proj_ang_list[l*2] = projangles[(*si)*2];
18125 proj_ang_list[l*2+1] = projangles[(*si)*2+1];
18126 l++;
18127 }
18128 assert(N_remain == l);
18129 asg = assign_projangles(proj_ang_list, ref_ang_list);
18130 sg.resize(nref);
18131 for (int i=0; i<nref; i++) sg[i] = 0;
18132 for (int i=0; i<N_remain; i++) sg[asg[i]]++;
18133 }
18134 int max_group_size = 0;
18135 for (int i=0; i<nref; i++)
18136 if (sg[i] > max_group_size) { max_group_size = sg[i]; max_group = i; }
18137 for (unsigned int i=0; i<remain_index.size(); i++)
18138 if (asg[i] == max_group) index.push_back(remain_index[i]);
18139 } else {
18140 for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si)
18141 index.push_back(*si);
18142 max_group = 0;
18143 }
18144
18145 int Nn = index.size();
18146 if (max_group != previous_group) {
18147 diff_table.resize(Nn*Nn);
18148 diff_table_index.clear();
18149 for (int i=0; i<Nn-1; i++)
18150 for (int j=i+1; j<Nn; j++) {
18151 float diff = ang_diff(v[index[i]*3], v[index[i]*3+1], v[index[i]*3+2], v[index[j]*3], v[index[j]*3+1], v[index[j]*3+2], mirror);
18152 float q = exp(-c*pow(diff/180.0f*static_cast<float>(M_PI), 2.0f));
18153 diff_table[i*Nn+j] = q;
18154 diff_table[j*Nn+i] = q;
18155 }
18156 for (int i=0; i<Nn; i++) {
18157 diff_table[i*Nn+i] = 0.0f;
18158 diff_table_index[index[i]] = i;
18159 }
18160 previous_group = max_group;
18161 }
18162
18163 int diff_table_size = static_cast<int>(sqrt((float)diff_table.size())+0.5f);
18164 float max_density = -1;
18165 int max_density_i = -1;
18166 for (int i=0; i<Nn; i++) {
18167 float s = 0.0f;
18168 int z = diff_table_index[index[i]];
18169 for (int j=0; j<diff_table_size; j++) s += diff_table[z*diff_table_size+j];
18170 if (s > max_density) {
18171 max_density = s;
18172 max_density_i = i;
18173 }
18174 }
18175
18176 vector<d_ang> dang(Nn, d_ang(0.0, 0, 0));
18177 for (int i=0; i<Nn; i++) {
18178 dang[i].d = ang_diff(v[index[i]*3], v[index[i]*3+1], v[index[i]*3+2],
18179 v[index[max_density_i]*3], v[index[max_density_i]*3+1], v[index[max_density_i]*3+2], mirror);
18180 dang[i].mirror = mirror;
18181 dang[i].i = i;
18182 }
18183 dang[max_density_i].d = -1;
18184 sort(dang.begin(), dang.end());
18185
18186 for (int i=0; i<img_per_grp; i++) {
18187 int idd = index[dang[i].i];
18188 mirror = dang[i].mirror;
18189 for (unsigned int j=0; j<remain_index.size(); j++)
18190 if (idd == remain_index[j]) asg[j] = -1;
18191 for (int j=0; j<diff_table_size; j++) {
18192 diff_table[diff_table_index[idd]*diff_table_size+j] = 0.0f;
18193 diff_table[diff_table_index[idd]+diff_table_size*j] = 0.0f;
18194 }
18195 proj_list.push_back(mirror*idd);
18196 pt.erase(idd);
18197 }
18198 sg[max_group] -= img_per_grp;
18199 }
18200 for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si) {
18201 proj_list.push_back(*si);
18202 }
18203 return proj_list;
18204 }
18205
18206 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
18207 float xrng, float yrng, float step, string mode,
18208 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
18209
18210
18211
18212
18213
18214
18215
18216
18217
18218
18219
18220
18221 size_t crefim_len = crefim.size();
18222
18223 int ky = int(2*yrng/step+0.5)/2;
18224 int kx = int(2*xrng/step+0.5)/2;
18225 int iref, nref=0, mirror=0;
18226 float iy, ix, sx=0, sy=0;
18227 float peak = -1.0E23f;
18228 float ang=0.0f;
18229 for (int i = -ky; i <= ky; i++) {
18230 iy = i * step ;
18231 for (int j = -kx; j <= kx; j++) {
18232 ix = j*step ;
18233 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18234
18235 Normalize_ring( cimage, numr );
18236
18237 Frngs(cimage, numr);
18238
18239
18240 for ( iref = 0; iref < (int)crefim_len; iref++) {
18241 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
18242 double qn = retvals["qn"];
18243 double qm = retvals["qm"];
18244 if(qn >= peak || qm >= peak) {
18245 sx = -ix;
18246 sy = -iy;
18247 nref = iref;
18248 if (qn >= qm) {
18249 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18250 peak = static_cast<float>(qn);
18251 mirror = 0;
18252 } else {
18253 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18254 peak = static_cast<float>(qm);
18255 mirror = 1;
18256 }
18257 }
18258 } delete cimage; cimage = 0;
18259 }
18260 }
18261 float co, so, sxs, sys;
18262 co = static_cast<float>( cos(ang*pi/180.0) );
18263 so = static_cast<float>( -sin(ang*pi/180.0) );
18264 sxs = sx*co - sy*so;
18265 sys = sx*so + sy*co;
18266 vector<float> res;
18267 res.push_back(ang);
18268 res.push_back(sxs);
18269 res.push_back(sys);
18270 res.push_back(static_cast<float>(mirror));
18271 res.push_back(static_cast<float>(nref));
18272 res.push_back(peak);
18273 return res;
18274 }
18275
18276 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
18277 float xrng, float yrng, float step, string mode,
18278 vector< int >numr, float cnx, float cny) {
18279
18280
18281
18282
18283
18284
18285
18286
18287
18288
18289
18290 size_t crefim_len = crefim.size();
18291
18292 int ky = int(2*yrng/step+0.5)/2;
18293 int kx = int(2*xrng/step+0.5)/2;
18294 int iref, nref=0;
18295 float iy, ix, sx=0, sy=0;
18296 float peak = -1.0E23f;
18297 float ang=0.0f;
18298 for (int i = -ky; i <= ky; i++) {
18299 iy = i * step ;
18300 for (int j = -kx; j <= kx; j++) {
18301 ix = j*step ;
18302 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18303 Frngs(cimage, numr);
18304
18305
18306 for ( iref = 0; iref < (int)crefim_len; iref++) {
18307 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
18308 double qn = retvals["qn"];
18309 if(qn >= peak) {
18310 sx = -ix;
18311 sy = -iy;
18312 nref = iref;
18313 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18314 peak = static_cast<float>(qn);
18315 }
18316 } delete cimage; cimage = 0;
18317 }
18318 }
18319 float co, so, sxs, sys;
18320 co = static_cast<float>( cos(ang*pi/180.0) );
18321 so = static_cast<float>( -sin(ang*pi/180.0) );
18322 sxs = sx*co - sy*so;
18323 sys = sx*so + sy*co;
18324 vector<float> res;
18325 res.push_back(ang);
18326 res.push_back(sxs);
18327 res.push_back(sys);
18328 res.push_back(static_cast<float>(nref));
18329 res.push_back(peak);
18330 return res;
18331 }
18332
18333 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
18334 float xrng, float yrng, float step, float ant, string mode,
18335 vector<int>numr, float cnx, float cny) {
18336
18337
18338
18339
18340
18341
18342
18343
18344
18345
18346
18347 size_t crefim_len = crefim.size();
18348 const float qv = static_cast<float>( pi/180.0 );
18349
18350 Transform * t = image->get_attr("xform.projection");
18351 Dict d = t->get_params("spider");
18352 if(t) {delete t; t=0;}
18353 float phi = d["phi"];
18354 float theta = d["theta"];
18355 int ky = int(2*yrng/step+0.5)/2;
18356 int kx = int(2*xrng/step+0.5)/2;
18357 int iref, nref=0, mirror=0;
18358 float iy, ix, sx=0, sy=0;
18359 float peak = -1.0E23f;
18360 float ang=0.0f;
18361 float imn1 = sin(theta*qv)*cos(phi*qv);
18362 float imn2 = sin(theta*qv)*sin(phi*qv);
18363 float imn3 = cos(theta*qv);
18364 vector<float> n1(crefim_len);
18365 vector<float> n2(crefim_len);
18366 vector<float> n3(crefim_len);
18367 for ( iref = 0; iref < (int)crefim_len; iref++) {
18368 n1[iref] = crefim[iref]->get_attr("n1");
18369 n2[iref] = crefim[iref]->get_attr("n2");
18370 n3[iref] = crefim[iref]->get_attr("n3");
18371 }
18372 for (int i = -ky; i <= ky; i++) {
18373 iy = i * step ;
18374 for (int j = -kx; j <= kx; j++) {
18375 ix = j*step;
18376 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18377 Normalize_ring( cimage, numr );
18378 Frngs(cimage, numr);
18379
18380
18381 for ( iref = 0; iref < (int)crefim_len; iref++) {
18382 if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18383 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18384 double qn = retvals["qn"];
18385 double qm = retvals["qm"];
18386 if(qn >= peak || qm >= peak) {
18387 sx = -ix;
18388 sy = -iy;
18389 nref = iref;
18390 if (qn >= qm) {
18391 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18392 peak = static_cast<float>( qn );
18393 mirror = 0;
18394 } else {
18395 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18396 peak = static_cast<float>( qm );
18397 mirror = 1;
18398 }
18399 }
18400 }
18401 } delete cimage; cimage = 0;
18402 }
18403 }
18404 float co, so, sxs, sys;
18405 if(peak == -1.0E23) {
18406 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18407 nref = -1;
18408 } else {
18409 co = cos(ang*qv);
18410 so = -sin(ang*qv);
18411 sxs = sx*co - sy*so;
18412 sys = sx*so + sy*co;
18413 }
18414 vector<float> res;
18415 res.push_back(ang);
18416 res.push_back(sxs);
18417 res.push_back(sys);
18418 res.push_back(static_cast<float>(mirror));
18419 res.push_back(static_cast<float>(nref));
18420 res.push_back(peak);
18421 return res;
18422 }
18423
18424 vector<float> Util::hans(EMData* image, const vector< EMData* >& crefim,
18425 float xrng, float yrng, float step, float ant, string mode,
18426 vector<int>numr, float cnx, float cny) {
18427
18428
18429
18430
18431
18432
18433
18434
18435
18436
18437
18438 size_t crefim_len = crefim.size();
18439 const float qv = static_cast<float>( pi/180.0 );
18440
18441
18442 float previousmax = image->get_attr("previousmax");
18443
18444
18445
18446
18447 int ky = int(2*yrng/step+0.5)/2;
18448 int kx = int(2*xrng/step+0.5)/2;
18449 int iref, nref=0, mirror=0;
18450 float iy, ix, sx=0, sy=0;
18451 float peak = previousmax;
18452 float ang=0.0f;
18453
18454 vector< vector<EMData*> > cimages( 2*ky+1, vector<EMData*>(2*kx+1) );
18455
18456 for (int i = -ky; i <= ky; i++) {
18457 iy = i * step ;
18458 for (int j = -kx; j <= kx; j++) {
18459 ix = j*step;
18460 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18461 Normalize_ring( cimage, numr );
18462 Frngs(cimage, numr);
18463 cimages[i+ky][j+kx] = cimage;
18464 }
18465 }
18466
18467 vector<unsigned> listr(crefim_len);
18468 for (unsigned i = 0; i < crefim_len; ++i) listr[i] = i;
18469 for (unsigned i = 0; i < crefim_len; ++i) {
18470 unsigned r = Util::get_irand(0,crefim_len-1);
18471 swap( listr[r], listr[i] );
18472 }
18473
18474 bool found_better = false;
18475 for ( int tiref = 0; (tiref < (int)crefim_len) && (! found_better); tiref++) {
18476 iref = listr[tiref];
18477 float best_for_ref = -1.0E23f;
18478 for (int i = -ky; i <= ky; i++) {
18479 iy = i * step ;
18480 for (int j = -kx; j <= kx; j++) {
18481 ix = j*step;
18482 EMData* cimage = cimages[i+ky][j+kx];
18483 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18484 double qn = retvals["qn"];
18485 double qm = retvals["qm"];
18486 if (qn >= best_for_ref || qm >= best_for_ref) {
18487 sx = -ix;
18488 sy = -iy;
18489 nref = iref;
18490 if (qn >= qm) {
18491 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18492 peak = static_cast<float>( qn );
18493 mirror = 0;
18494 } else {
18495 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18496 peak = static_cast<float>( qm );
18497 mirror = 1;
18498 }
18499 best_for_ref = peak;
18500
18501 }
18502 }
18503 }
18504 found_better = (best_for_ref >= previousmax);
18505 }
18506
18507 for (unsigned i = 0; i < cimages.size(); ++i) {
18508 for (unsigned j = 0; j < cimages[i].size(); ++j) {
18509 delete cimages[i][j];
18510 cimages[i][j] = NULL;
18511 }
18512 }
18513
18514 float sxs, sys;
18515 if (found_better) {
18516 float co = cos(ang*qv);
18517 float so = -sin(ang*qv);
18518 sxs = sx*co - sy*so;
18519 sys = sx*so + sy*co;
18520 image->set_attr("previousmax",peak);
18521 } else {
18522 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18523 nref = -1;
18524 peak = previousmax;
18525 }
18526 vector<float> res;
18527 res.push_back(ang);
18528 res.push_back(sxs);
18529 res.push_back(sys);
18530 res.push_back(static_cast<float>(mirror));
18531 res.push_back(static_cast<float>(nref));
18532 res.push_back(peak);
18533 return res;
18534 }
18535
18536
18537 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18538 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18539 vector<int>numr, float cnx, float cny) {
18540
18541
18542
18543
18544
18545
18546
18547
18548
18549
18550
18551 size_t crefim_len = crefim.size();
18552 const float qv = static_cast<float>(pi/180.0);
18553
18554 Transform* t = image->get_attr("xform.projection");
18555 Dict d = t->get_params("spider");
18556 if(t) {delete t; t=0;}
18557 float phi = d["phi"];
18558 float theta = d["theta"];
18559 float psi = d["psi"];
18560 int ky = int(2*yrng/step+0.5)/2;
18561 int kx = int(2*xrng/step+0.5)/2;
18562 int iref, nref = 0, mirror = 0;
18563 float iy, ix, sx = 0, sy = 0;
18564 float peak = -1.0E23f;
18565 float ang = 0.0f;
18566 float imn1 = sin(theta*qv)*cos(phi*qv);
18567 float imn2 = sin(theta*qv)*sin(phi*qv);
18568 float imn3 = cos(theta*qv);
18569 vector<float> n1(crefim_len);
18570 vector<float> n2(crefim_len);
18571 vector<float> n3(crefim_len);
18572 for (iref = 0; iref < (int)crefim_len; iref++) {
18573 n1[iref] = crefim[iref]->get_attr("n1");
18574 n2[iref] = crefim[iref]->get_attr("n2");
18575 n3[iref] = crefim[iref]->get_attr("n3");
18576 }
18577 bool nomirror = (theta<90.0) || (theta==90.0);
18578 if (!nomirror) {
18579 phi = fmod(phi+540.0f, 360.0f);
18580 theta = 180-theta;
18581 psi = fmod(540.0f-psi, 360.0f);
18582 } else { psi = fmod(360.0f-psi, 360.0f); }
18583 for (int i = -ky; i <= ky; i++) {
18584 iy = i * step ;
18585 for (int j = -kx; j <= kx; j++) {
18586 ix = j*step;
18587 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18588
18589 Normalize_ring(cimage, numr);
18590
18591 Frngs(cimage, numr);
18592
18593
18594 for (iref = 0; iref < (int)crefim_len; iref++) {
18595 if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18596 float refpsi = crefim[iref]->get_attr("psi");
18597 if (nomirror) {
18598 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, fmod(360.0+psi+refpsi, 360.0), 0, psi_max);
18599 double qn = retvals["qn"];
18600 if (qn >= peak) {
18601 sx = -ix;
18602 sy = -iy;
18603 nref = iref;
18604 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18605 peak = static_cast<float>(qn);
18606 mirror = 0;
18607 }
18608 } else {
18609 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, fmod(360.0+psi-refpsi, 360.0), 1, psi_max);
18610 double qn = retvals["qn"];
18611 if (qn >= peak) {
18612 sx = -ix;
18613 sy = -iy;
18614 nref = iref;
18615 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18616 peak = static_cast<float>(qn);
18617 mirror = 1;
18618 }
18619 }
18620 }
18621 } delete cimage; cimage = 0;
18622 }
18623 }
18624 float co, so, sxs, sys;
18625 if(peak == -1.0E23) {
18626 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18627 nref = -1;
18628 } else {
18629 co = cos(ang*qv);
18630 so = -sin(ang*qv);
18631 sxs = sx*co - sy*so;
18632 sys = sx*so + sy*co;
18633 }
18634 vector<float> res;
18635 res.push_back(ang);
18636 res.push_back(sxs);
18637 res.push_back(sys);
18638 res.push_back(static_cast<float>(mirror));
18639 res.push_back(static_cast<float>(nref));
18640 res.push_back(peak);
18641 return res;
18642 }
18643
18644
18645 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18646 float xrng, float yrng, float step, float psi_max, string mode,
18647 vector<int>numr, float cnx, float cny, int ynumber) {
18648
18649 size_t crefim_len = crefim.size();
18650
18651 int iref, nref=0, mirror=0;
18652 float iy, ix, sx=0, sy=0;
18653 float peak = -1.0E23f;
18654 float ang=0.0f;
18655 int kx = int(2*xrng/step+0.5)/2;
18656
18657
18658
18659 int ky;
18660 float stepy;
18661 int kystart;
18662
18663 if (ynumber == -1) {
18664 ky = int(2*yrng/step+0.5)/2;
18665 stepy = step;
18666 kystart = -ky;
18667 } else if(ynumber == 0) {
18668 ky = 0;
18669 stepy = 0.0f;
18670 kystart = ky;
18671 } else {
18672 ky = int(ynumber/2);
18673 stepy=2*yrng/ynumber;
18674 kystart = -ky + 1;
18675 }
18676
18677 for (int i = kystart; i <= ky; i++) {
18678 iy = i * stepy ;
18679 for (int j = -kx; j <= kx; j++) {
18680 ix = j*step ;
18681 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18682
18683 Normalize_ring( cimage, numr );
18684
18685 Frngs(cimage, numr);
18686
18687
18688 for ( iref = 0; iref < (int)crefim_len; iref++) {
18689 Dict retvals_0 = Crosrng_psi(crefim[iref], cimage, numr, 0, psi_max);
18690 Dict retvals_180 = Crosrng_psi(crefim[iref], cimage, numr, 180, psi_max);
18691 double qn_0 = retvals_0["qn"];
18692 double qn_180 = retvals_180["qn"];
18693 double qm_0 = retvals_0["qm"];
18694 double qm_180 = retvals_180["qm"];
18695 double qn;
18696 double qm;
18697 bool qn_is_zero = false;
18698 bool qm_is_zero = false;
18699
18700 if (qn_0 >= qn_180) {
18701 qn = qn_0;
18702 qn_is_zero = true;
18703 } else {
18704 qn = qn_180;
18705 qn_is_zero = false;
18706 }
18707
18708 if (qm_0 >= qm_180) {
18709 qm = qm_0;
18710 qm_is_zero = true;
18711 } else {
18712 qm = qm_180;
18713 qm_is_zero = false;
18714 }
18715
18716 if(qn >= peak || qm >= peak) {
18717 sx = -ix;
18718 sy = -iy;
18719 nref = iref;
18720 if (qn >= qm) {
18721 if (qn_is_zero) ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
18722 else ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
18723 peak = static_cast<float>(qn);
18724 mirror = 0;
18725 } else {
18726 if (qm_is_zero) ang = ang_n(retvals_0["tmt"], mode, numr[numr.size()-1]);
18727 else ang = ang_n(retvals_180["tmt"], mode, numr[numr.size()-1]);
18728 peak = static_cast<float>(qm);
18729 mirror = 1;
18730 }
18731 }
18732 }
18733 delete cimage; cimage = 0;
18734 }
18735 }
18736 float co, so, sxs, sys;
18737 co = static_cast<float>( cos(ang*pi/180.0) );
18738 so = static_cast<float>( -sin(ang*pi/180.0) );
18739 sxs = sx*co - sy*so;
18740 sys = sx*so + sy*co;
18741 vector<float> res;
18742 res.push_back(ang);
18743 res.push_back(sxs);
18744 res.push_back(sys);
18745 res.push_back(static_cast<float>(mirror));
18746 res.push_back(static_cast<float>(nref));
18747 res.push_back(peak);
18748 return res;
18749 }
18750
18751 vector<float> Util::multiref_polar_ali_helical_local(EMData* image, const vector< EMData* >& crefim,
18752 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18753 vector<int>numr, float cnx, float cny, int ynumber, float yrnglocal) {
18754
18755 size_t crefim_len = crefim.size();
18756
18757 int iref, nref=-1, mirror=0;
18758 float iy, ix, sx=0, sy=0;
18759 float peak = -1.0E23f;
18760 float ang=0.0f;
18761 const float qv = static_cast<float>( pi/180.0 );
18762 Transform * t = image->get_attr("xform.projection");
18763 Dict d = t->get_params("spider");
18764 if(t) {delete t; t=0;}
18765 float phi = d["phi"];
18766 float theta = d["theta"];
18767 float psi = d["psi"];
18768 float imn1 = sin(theta*qv)*cos(phi*qv);
18769 float imn2 = sin(theta*qv)*sin(phi*qv);
18770 float imn3 = cos(theta*qv);
18771 vector<float> n1(crefim_len);
18772 vector<float> n2(crefim_len);
18773 vector<float> n3(crefim_len);
18774 for ( iref = 0; iref < (int)crefim_len; iref++) {
18775 n1[iref] = crefim[iref]->get_attr("n1");
18776 n2[iref] = crefim[iref]->get_attr("n2");
18777 n3[iref] = crefim[iref]->get_attr("n3");
18778 }
18779 float nbrinp;
18780 float nbrinp_mirror;
18781 bool use_ref;
18782 bool use_ref_mirror;
18783 int kx = int(2*xrng/step+0.5)/2;
18784 int ky;
18785 float stepy;
18786
18787 if (ynumber == 0) {
18788 ky = 0;
18789 }
18790 else {
18791
18792 if (ynumber > 0) stepy=2*yrng/ynumber;
18793 else if (ynumber == -1) stepy = step;
18794
18795 if (yrnglocal >= 0.0) {
18796 ky = int(yrnglocal/stepy);
18797 }
18798 else {
18799 if (ynumber > 0) {
18800 ky = int(ynumber/2);
18801 }
18802 else{
18803 ky = int(2*yrng/stepy+0.5)/2;
18804 }
18805
18806 }
18807 }
18808 for (int i = -ky; 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 Normalize_ring( cimage, numr );
18814
18815 Frngs(cimage, numr);
18816
18817
18818 for ( iref = 0; iref < (int)crefim_len; iref++) {
18819
18820 use_ref = false;
18821 use_ref_mirror = false;
18822
18823
18824 nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18825 if (nbrinp >= ant) use_ref = true;
18826
18827
18828 nbrinp_mirror = n3[iref]*imn3 - n1[iref]*imn1 - n2[iref]*imn2;
18829 if (nbrinp_mirror >= ant) use_ref_mirror = true;
18830 if(use_ref || use_ref_mirror){
18831
18832 Dict retvals;
18833 Dict retvals_mirror;
18834 if (use_ref_mirror == true) {
18835 if ((psi-90.0f) < 90.0f) retvals_mirror = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
18836 else retvals_mirror = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max);
18837 }
18838 if (use_ref == true) {
18839 if ((psi-90.0f) < 90.0f) retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18840 else retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18841 }
18842 double qn = retvals["qn"];
18843 double qm = retvals_mirror["qn"];
18844
18845 if(qn >= peak || qm >= peak) {
18846 sx = -ix;
18847 sy = -iy;
18848 nref = iref;
18849 if (qn >= qm){
18850 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18851 mirror = 0;
18852 peak = static_cast<float>(qn);
18853 }
18854 else{
18855 ang = ang_n(retvals_mirror["tot"], mode, numr[numr.size()-1]);
18856 mirror = 1;
18857 peak = static_cast<float>(qm);
18858 }
18859
18860 }
18861 }
18862 }
18863 delete cimage; cimage = 0;
18864 }
18865 }
18866
18867 float co, so, sxs, sys;
18868 co = static_cast<float>( cos(ang*pi/180.0) );
18869 so = static_cast<float>( -sin(ang*pi/180.0) );
18870 sxs = sx*co - sy*so;
18871 sys = sx*so + sy*co;
18872 vector<float> res;
18873 res.push_back(ang);
18874 res.push_back(sxs);
18875 res.push_back(sys);
18876 res.push_back(static_cast<float>(mirror));
18877 res.push_back(static_cast<float>(nref));
18878 res.push_back(peak);
18879 return res;
18880 }
18881
18882
18883 vector<float> Util::multiref_polar_ali_helical_90(EMData* image, const vector< EMData* >& crefim,
18884 float xrng, float yrng, float step, float psi_max, string mode,
18885 vector<int>numr, float cnx, float cny, int ynumber) {
18886
18887 size_t crefim_len = crefim.size();
18888
18889 int iref, nref=0, mirror=0;
18890 float iy, ix, sx=0, sy=0;
18891 float peak = -1.0E23f;
18892 float ang=0.0f;
18893 int kx = int(2*xrng/step+0.5)/2;
18894
18895
18896 int ky;
18897 float stepy;
18898 int kystart;
18899
18900 if (ynumber == -1) {
18901 ky = int(2*yrng/step+0.5)/2;
18902 stepy = step;
18903 kystart = -ky;
18904 } else if(ynumber == 0) {
18905 ky = 0;
18906 stepy = 0.0f;
18907 kystart = ky;
18908 } else {
18909 ky = int(ynumber/2);
18910 stepy=2*yrng/ynumber;
18911 kystart = -ky + 1;
18912 }
18913
18914
18915 for (int i = kystart; i <= ky; i++) {
18916 iy = i * stepy ;
18917 for (int j = -kx; j <= kx; j++) {
18918 ix = j*step ;
18919 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, 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 Dict retvals_0 = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18928 Dict retvals_180 = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18929 double qn_0 = retvals_0["qn"];
18930 double qn_180 = retvals_180["qn"];
18931 double qn;
18932 bool qn_is_zero = false;
18933
18934 if (qn_0 >= qn_180){
18935 qn = qn_0;
18936 qn_is_zero = true;
18937 }
18938 else{
18939 qn = qn_180;
18940 qn_is_zero = false;
18941 }
18942
18943 if(qn >= peak) {
18944 sx = -ix;
18945 sy = -iy;
18946 nref = iref;
18947
18948 if (qn_is_zero){
18949 ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
18950 }
18951 else{
18952 ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
18953 }
18954 peak = static_cast<float>(qn);
18955 mirror = 0;
18956
18957 }
18958 }
18959 delete cimage; cimage = 0;
18960 }
18961 }
18962 float co, so, sxs, sys;
18963 co = static_cast<float>( cos(ang*pi/180.0) );
18964 so = static_cast<float>( -sin(ang*pi/180.0) );
18965 sxs = sx*co - sy*so;
18966 sys = sx*so + sy*co;
18967 vector<float> res;
18968 res.push_back(ang);
18969 res.push_back(sxs);
18970 res.push_back(sys);
18971 res.push_back(static_cast<float>(mirror));
18972 res.push_back(static_cast<float>(nref));
18973 res.push_back(peak);
18974 return res;
18975 }
18976
18977 vector<float> Util::multiref_polar_ali_helical_90_local(EMData* image, const vector< EMData* >& crefim,
18978 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18979 vector<int>numr, float cnx, float cny, int ynumber, float yrnglocal) {
18980
18981 size_t crefim_len = crefim.size();
18982 const float qv = static_cast<float>( pi/180.0 );
18983 Transform * t = image->get_attr("xform.projection");
18984 Dict d = t->get_params("spider");
18985 if(t) {delete t; t=0;}
18986 float phi = d["phi"];
18987 float theta = d["theta"];
18988 float psi = d["psi"];
18989 float imn1 = sin(theta*qv)*cos(phi*qv);
18990 float imn2 = sin(theta*qv)*sin(phi*qv);
18991 float imn3 = cos(theta*qv);
18992 vector<float> n1(crefim_len);
18993 vector<float> n2(crefim_len);
18994 vector<float> n3(crefim_len);
18995 int iref, nref=-1, mirror=0;
18996 float iy, ix, sx=0, sy=0;
18997 float peak = -1.0E23f;
18998 float ang = 0.0f;
18999 int kx = int(2*xrng/step+0.5)/2;
19000
19001 for ( iref = 0; iref < (int)crefim_len; iref++) {
19002 n1[iref] = crefim[iref]->get_attr("n1");
19003 n2[iref] = crefim[iref]->get_attr("n2");
19004 n3[iref] = crefim[iref]->get_attr("n3");
19005 }
19006
19007 float stepy;
19008 int ky;
19009
19010 if (ynumber == 0) {
19011 ky = 0;
19012 }
19013 else {
19014
19015 if (ynumber > 0) stepy=2*yrng/ynumber;
19016 else if (ynumber == -1) stepy = step;
19017
19018 if (yrnglocal >= 0.0) {
19019 ky = int(yrnglocal/stepy);
19020 }
19021 else {
19022 if (ynumber > 0) {
19023 ky = int(ynumber/2);
19024 }
19025 else{
19026 ky = int(2*yrng/stepy+0.5)/2;
19027 }
19028
19029 }
19030 }
19031
19032 for (int i = -ky; i <= ky; i++) {
19033 iy = i * stepy ;
19034 for (int j = -kx; j <= kx; j++) {
19035 ix = j*step ;
19036 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19037
19038 Normalize_ring( cimage, numr );
19039
19040 Frngs(cimage, numr);
19041
19042
19043 for ( iref = 0; iref < (int)crefim_len; iref++) {
19044 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
19045 Dict retvals;
19046 if ((psi-90.0f) < 90.0f) retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
19047 else retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
19048
19049 double qn = retvals["qn"];
19050 if( qn >= peak) {
19051 sx = -ix;
19052 sy = -iy;
19053 nref = iref;
19054 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19055 peak = static_cast<float>(qn);
19056 mirror = 0;
19057 }
19058 }
19059 }
19060 delete cimage; cimage = 0;
19061 }
19062 }
19063
19064 float co, so, sxs, sys;
19065 co = static_cast<float>( cos(ang*pi/180.0) );
19066 so = static_cast<float>( -sin(ang*pi/180.0) );
19067 sxs = sx*co - sy*so;
19068 sys = sx*so + sy*co;
19069 vector<float> res;
19070 res.push_back(ang);
19071 res.push_back(sxs);
19072 res.push_back(sys);
19073 res.push_back(static_cast<float>(mirror));
19074 res.push_back(static_cast<float>(nref));
19075 res.push_back(peak);
19076 return res;
19077 }
19078
19079
19080 void Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
19081 float xrng, float yrng, float step, string mode,
19082 vector< int >numr, float cnx, float cny,
19083 EMData *peaks, EMData *peakm) {
19084
19085 int maxrin = numr[numr.size()-1];
19086
19087 int ky = int(2*yrng/step+0.5)/2;
19088 int kx = int(2*xrng/step+0.5)/2;
19089
19090 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19091 float *p_ccf1ds = peaks->get_data();
19092
19093 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19094 float *p_ccf1dm = peakm->get_data();
19095
19096 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19097 p_ccf1ds[i] = -1.e20f;
19098 p_ccf1dm[i] = -1.e20f;
19099 }
19100
19101 for (int i = -ky; i <= ky; i++) {
19102 float iy = i * step;
19103 for (int j = -kx; j <= kx; j++) {
19104 float ix = j*step;
19105 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19106 Frngs(cimage, numr);
19107 Crosrng_msg_vec(crefim, cimage, numr,
19108 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19109 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19110 delete cimage; cimage = 0;
19111 }
19112 }
19113 return;
19114 }
19115
19116 void Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
19117 float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
19118 EMData *peaks_compress, EMData *peakm_compress) {
19119
19120 int maxrin = numr[numr.size()-1];
19121
19122 int ky = int(2*yrng/step+0.5)/2;
19123 int kx = int(2*xrng/step+0.5)/2;
19124
19125 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19126 float *p_ccf1ds = peaks->get_data();
19127
19128 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19129 float *p_ccf1dm = peakm->get_data();
19130
19131 peaks_compress->set_size(maxrin, 1, 1);
19132 float *p_ccf1ds_compress = peaks_compress->get_data();
19133
19134 peakm_compress->set_size(maxrin, 1, 1);
19135 float *p_ccf1dm_compress = peakm_compress->get_data();
19136
19137 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19138 p_ccf1ds[i] = -1.e20f;
19139 p_ccf1dm[i] = -1.e20f;
19140 }
19141
19142 for (int i = -ky; i <= ky; i++) {
19143 float iy = i * step;
19144 for (int j = -kx; j <= kx; j++) {
19145 float ix = j*step;
19146 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19147 Frngs(cimage, numr);
19148 Crosrng_msg_vec(crefim, cimage, numr,
19149 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19150 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19151 delete cimage; cimage = 0;
19152 }
19153 }
19154 for (int x=0; x<maxrin; x++) {
19155 float maxs = -1.0e22f;
19156 float maxm = -1.0e22f;
19157 for (int i=1; i<=2*ky+1; i++) {
19158 for (int j=1; j<=2*kx+1; j++) {
19159 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
19160 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
19161 }
19162 }
19163 p_ccf1ds_compress[x] = maxs;
19164 p_ccf1dm_compress[x] = maxm;
19165 }
19166 return;
19167 }
19168
19169 struct ccf_point
19170 {
19171 float value;
19172 int i;
19173 int j;
19174 int k;
19175 int mirror;
19176 };
19177
19178
19179 struct ccf_value
19180 {
19181 bool operator()( const ccf_point& a, const ccf_point& b )
19182 {
19183 return a.value > b.value;
19184 }
19185 };
19186
19187
19188 vector<float> Util::ali2d_ccf_list(EMData* image, EMData* crefim,
19189 float xrng, float yrng, float step, string mode,
19190 vector< int >numr, float cnx, float cny, double T) {
19191
19192 int maxrin = numr[numr.size()-1];
19193
19194 int ky = int(2*yrng/step+0.5)/2;
19195 int kx = int(2*xrng/step+0.5)/2;
19196
19197 float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
19198 float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
19199 int vol = maxrin*(2*kx+1)*(2*ky+1);
19200 vector<ccf_point> ccf(2*vol);
19201 ccf_point temp;
19202
19203 int index = 0;
19204 for (int i = -ky; i <= ky; i++) {
19205 float iy = i * step;
19206 for (int j = -kx; j <= kx; j++) {
19207 float ix = j*step;
19208 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19209 Frngs(cimage, numr);
19210 Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
19211 for (int k=0; k<maxrin; k++) {
19212 temp.value = p_ccf1ds[k];
19213 temp.i = k;
19214 temp.j = j;
19215 temp.k = i;
19216 temp.mirror = 0;
19217 ccf[index] = temp;
19218 index++;
19219 temp.value = p_ccf1dm[k];
19220 temp.mirror = 1;
19221 ccf[index] = temp;
19222 index++;
19223 }
19224 delete cimage; cimage = 0;
19225 }
19226 }
19227
19228 delete p_ccf1ds;
19229 delete p_ccf1dm;
19230 std::sort(ccf.begin(), ccf.end(), ccf_value());
19231
19232 double qt = (double)ccf[0].value;
19233 vector <double> p(2*vol), cp(2*vol);
19234
19235 double sump = 0.0;
19236 for (int i=0; i<2*vol; i++) {
19237 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
19238 sump += p[i];
19239 }
19240 for (int i=0; i<2*vol; i++) {
19241 p[i] /= sump;
19242 }
19243 for (int i=1; i<2*vol; i++) {
19244 p[i] += p[i-1];
19245 }
19246 p[2*vol-1] = 2.0;
19247
19248 float t = get_frand(0.0f, 1.0f);
19249 int select = 0;
19250 while (p[select] < t) select += 1;
19251
19252 vector<float> a(6);
19253 a[0] = ccf[select].value;
19254 a[1] = (float)ccf[select].i;
19255 a[2] = (float)ccf[select].j;
19256 a[3] = (float)ccf[select].k;
19257 a[4] = (float)ccf[select].mirror;
19258 a[5] = (float)select;
19259 return a;
19260 }
19261
19262
19263
19264
19265
19266
19267
19268
19269
19270
19271
19272
19273
19274
19275
19276
19277
19278
19279
19280
19281
19282
19283
19284
19285
19286
19287
19288
19289
19290
19291
19292
19293
19294
19295
19296
19297
19298
19299
19300
19301
19302
19303
19304
19305
19306
19307
19308
19309
19310
19311
19312
19313
19314
19315
19316
19317
19318
19319
19320
19321
19322
19323
19324
19325
19326
19327
19328
19329 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19330
19331 EMData *rot;
19332
19333 const int nmax=3, mmax=3;
19334 char task[60], csave[60];
19335 long int lsave[4];
19336 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19337 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];
19338 long int SIXTY=60;
19339
19340
19341 iprint = -1;
19342
19343
19344 factr=1.0e1;
19345 pgtol=1.0e-5;
19346
19347
19348
19349
19350 n=3;
19351 m=3;
19352
19353
19354
19355
19356
19357 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19358 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19359 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19360
19361
19362
19363
19364 strcpy(task,"START");
19365 for (int i=5;i<60;i++) task[i]=' ';
19366
19367
19368
19369 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19370
19371
19372
19373 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19374
19375 if (strncmp(task,"FG",2)==0) {
19376
19377
19378
19379
19380 rot = new EMData();
19381 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
19382 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19383
19384 delete rot;
19385
19386
19387 float dt = 1.0e-3f;
19388 rot = new EMData();
19389 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
19390 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19391
19392 g[0] = (f1-f)/dt;
19393 delete rot;
19394
19395 dt = 1.0e-2f;
19396 rot = new EMData();
19397 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
19398 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19399
19400 g[1] = (f2-f)/dt;
19401 delete rot;
19402
19403 rot = new EMData();
19404 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
19405 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19406
19407 g[2] = (f3-f)/dt;
19408 delete rot;
19409 }
19410
19411
19412 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19413
19414 }
19415
19416
19417 vector<float> res;
19418 res.push_back(static_cast<float>(x[0]));
19419 res.push_back(static_cast<float>(x[1]));
19420 res.push_back(static_cast<float>(x[2]));
19421
19422 return res;
19423 }
19424
19425 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19426
19427 EMData *rot;
19428
19429 const int nmax=3, mmax=3;
19430 char task[60], csave[60];
19431 long int lsave[4];
19432 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19433 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];
19434 long int SIXTY=60;
19435
19436
19437 iprint = -1;
19438
19439
19440 factr=1.0e1;
19441 pgtol=1.0e-5;
19442
19443
19444
19445
19446 n=3;
19447 m=3;
19448
19449
19450
19451
19452
19453 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19454 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19455 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19456
19457
19458
19459
19460 strcpy(task,"START");
19461 for (int i=5;i<60;i++) task[i]=' ';
19462
19463
19464
19465 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19466
19467
19468
19469 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19470
19471 if (strncmp(task,"FG",2)==0) {
19472
19473
19474
19475
19476 rot = new EMData();
19477 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19478 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19479
19480 delete rot;
19481
19482
19483 float dt = 1.0e-3f;
19484 rot = new EMData();
19485 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19486 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19487
19488 g[0] = (f1-f)/dt;
19489 delete rot;
19490
19491 rot = new EMData();
19492 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
19493 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19494
19495 g[1] = (f2-f)/dt;
19496 delete rot;
19497
19498 rot = new EMData();
19499 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
19500 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19501
19502 g[2] = (f3-f)/dt;
19503 delete rot;
19504 }
19505
19506
19507 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19508
19509 }
19510
19511
19512 vector<float> res;
19513 res.push_back(static_cast<float>(x[0]));
19514 res.push_back(static_cast<float>(x[1]));
19515 res.push_back(static_cast<float>(x[2]));
19516
19517 return res;
19518 }
19519
19520 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) {
19521
19522 EMData *proj, *proj2;
19523
19524 const int nmax=5, mmax=5;
19525 char task[60], csave[60];
19526 long int lsave[4];
19527 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19528 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];
19529 long int SIXTY=60;
19530
19531
19532 iprint = -1;
19533
19534
19535 factr=1.0e1;
19536 pgtol=1.0e-5;
19537
19538
19539
19540
19541 n=5;
19542 m=5;
19543
19544
19545
19546
19547
19548 x[0] = phi; nbd[0] = 2; l[0] = phi-2.0; u[0] = phi+2.0;
19549 x[1] = theta; nbd[1] = 2; l[1] = theta-2.0; u[1] = theta+2.0;
19550 x[2] = psi; nbd[2] = 2; l[2] = psi-2.0; u[2] = psi+2.0;
19551 x[3] = sxs; nbd[3] = 2; l[3] = sxs-2.0; u[3] = sxs+2.0;
19552 x[4] = sys; nbd[4] = 2; l[4] = sys-2.0; u[4] = sys+2.0;
19553
19554
19555
19556
19557 strcpy(task,"START");
19558 for (int i=5;i<60;i++) task[i]=' ';
19559
19560
19561
19562 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19563 int step = 1;
19564
19565
19566 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19567
19568 if (strncmp(task,"FG",2)==0) {
19569
19570
19571
19572
19573 proj = new EMData();
19574 proj2 = new EMData();
19575 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19576 proj->fft_shuffle();
19577 proj->center_origin_fft();
19578 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19579 proj->do_ift_inplace();
19580 int M = proj->get_ysize()/2;
19581 proj2 = proj->window_center(M);
19582 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19583
19584 delete proj;
19585 delete proj2;
19586
19587
19588 float dt = 1.0e-3f;
19589 proj = new EMData();
19590 proj2 = new EMData();
19591 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
19592 proj->fft_shuffle();
19593 proj->center_origin_fft();
19594 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19595 proj->do_ift_inplace();
19596 proj2 = proj->window_center(M);
19597 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19598
19599 delete proj;
19600 delete proj2;
19601 g[0] = (ft-f)/dt;
19602
19603 proj = new EMData();
19604 proj2 = new EMData();
19605 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
19606 proj->fft_shuffle();
19607 proj->center_origin_fft();
19608 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19609 proj->do_ift_inplace();
19610 proj2 = proj->window_center(M);
19611 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19612
19613 delete proj;
19614 delete proj2;
19615 g[1] = (ft-f)/dt;
19616
19617 proj = new EMData();
19618 proj2 = new EMData();
19619 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
19620 proj->fft_shuffle();
19621 proj->center_origin_fft();
19622 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19623 proj->do_ift_inplace();
19624 proj2 = proj->window_center(M);
19625 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19626
19627 delete proj;
19628 delete proj2;
19629 g[2] = (ft-f)/dt;
19630
19631 proj = new EMData();
19632 proj2 = new EMData();
19633 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19634 proj->fft_shuffle();
19635 proj->center_origin_fft();
19636 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
19637 proj->do_ift_inplace();
19638 proj2 = proj->window_center(M);
19639 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19640
19641 delete proj;
19642 delete proj2;
19643 g[3] = (ft-f)/dt;
19644
19645 proj = new EMData();
19646 proj2 = new EMData();
19647 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19648 proj->fft_shuffle();
19649 proj->center_origin_fft();
19650 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
19651 proj->do_ift_inplace();
19652 proj2 = proj->window_center(M);
19653 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19654
19655 delete proj;
19656 delete proj2;
19657 g[4] = (ft-f)/dt;
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 step++;
19663 }
19664
19665
19666 vector<float> res;
19667 res.push_back(static_cast<float>(x[0]));
19668 res.push_back(static_cast<float>(x[1]));
19669 res.push_back(static_cast<float>(x[2]));
19670 res.push_back(static_cast<float>(x[3]));
19671 res.push_back(static_cast<float>(x[4]));
19672
19673 return res;
19674 }
19675
19676
19677 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19678
19679 double x[4];
19680 int n;
19681 int l = 3;
19682 int m = 200;
19683 double e = 1e-9;
19684 double step = 0.01;
19685 float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
19686
19687 x[1] = ang;
19688 x[2] = sxs;
19689 x[3] = sys;
19690
19691 Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);
19692
19693
19694 vector<float> res;
19695 res.push_back(static_cast<float>(x[1]));
19696 res.push_back(static_cast<float>(x[2]));
19697 res.push_back(static_cast<float>(x[3]));
19698 res.push_back(static_cast<float>(n));
19699 return res;
19700 }
19701
19702
19703
19704
19705
19706
19707
19708 vector<float> Util::multi_align_error(vector<float> args, vector<float> all_ali_params, int d) {
19709
19710 const int nmax=args.size(), mmax=nmax;
19711 char task[60], csave[60];
19712 long int lsave[4];
19713 long int n, m, iprint, isave[44];
19714 long int* nbd = new long int[nmax];
19715 long int* iwa = new long int[3*nmax];
19716 double f, factr, pgtol;
19717 double* x = new double[nmax];
19718 double* l = new double[nmax];
19719 double* u = new double[nmax];
19720 double* g = new double[nmax];
19721 double dsave[29];
19722 double* wa = new double[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
19723 long int SIXTY=60;
19724
19725 int num_ali = nmax/3+1;
19726 int nima = all_ali_params.size()/(num_ali*4);
19727
19728
19729 iprint = -1;
19730
19731
19732 factr=1.0e1;
19733 pgtol=1.0e-9;
19734
19735
19736
19737
19738 n=nmax;
19739 m=mmax;
19740
19741
19742
19743
19744
19745 for (int i=0; i<nmax; i++) {
19746 x[i] = args[i];
19747 nbd[i] = 0;
19748 }
19749
19750
19751
19752 strcpy(task,"START");
19753 for (int i=5;i<60;i++) task[i]=' ';
19754
19755
19756
19757 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19758 int step = 1;
19759
19760
19761 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19762
19763 if (strncmp(task,"FG",2)==0) {
19764
19765
19766
19767
19768 f = multi_align_error_func(x, all_ali_params, nima, num_ali, d);
19769
19770
19771 multi_align_error_dfunc(x, all_ali_params, nima, num_ali, g, d);
19772
19773 }
19774
19775 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19776 step++;
19777 }
19778
19779
19780 vector<float> res;
19781 for (int i=0; i<nmax; i++) res.push_back(static_cast<float>(x[i]));
19782 res.push_back(static_cast<float>(f));
19783
19784 delete[] nbd;
19785 delete[] iwa;
19786 delete[] x;
19787 delete[] l;
19788 delete[] u;
19789 delete[] g;
19790 delete[] wa;
19791
19792 return res;
19793
19794 }
19795
19796 double Util::multi_align_error_func(double* x, vector<float> all_ali_params, int nima, int num_ali, int d)
19797 {
19798 vector<double> sqr_pixel_error = multi_align_error_func2(x, all_ali_params, nima, num_ali, d);
19799 double sum_sqr_pixel_error = 0.0;
19800 for (int i=0; i<nima; i++) {
19801 sum_sqr_pixel_error += sqr_pixel_error[i];
19802 }
19803 return ( sum_sqr_pixel_error / nima );
19804 }
19805
19806
19807 vector<double> Util::multi_align_error_func2(double* x, vector<float> ali_params, int nima, int num_ali, int d)
19808 {
19809 double* args = new double[num_ali*3];
19810 for (int i=0; i<3*num_ali-3; i++) args[i] = x[i];
19811 args[3*num_ali-3] = 0.0;
19812 args[3*num_ali-2] = 0.0;
19813 args[3*num_ali-1] = 0.0;
19814 double* cosa = new double[num_ali];
19815 double* sina = new double[num_ali];
19816 for (int i=0; i<num_ali; i++) {
19817 cosa[i] = cos(args[i*3]*M_PI/180.0);
19818 sina[i] = sin(args[i*3]*M_PI/180.0);
19819 }
19820 double* sx = new double[num_ali];
19821 double* sy = new double[num_ali];
19822
19823 vector<double> sqr_pixel_error(nima);
19824
19825 for (int i=0; i<nima; i++) {
19826 double sum_cosa = 0.0;
19827 double sum_sina = 0.0;
19828 for (int j=0; j<num_ali; j++) {
19829 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19830 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19831 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19832 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];
19833 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];
19834 } else {
19835 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19836 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19837 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];
19838 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];
19839 }
19840 }
19841 double sqrtP = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
19842 sqr_pixel_error[i] = d*d/4.0*(1.0-sqrtP/num_ali)+var(sx, num_ali)+var(sy, num_ali);
19843 }
19844
19845 delete[] args;
19846 delete[] cosa;
19847 delete[] sina;
19848 delete[] sx;
19849 delete[] sy;
19850
19851 return sqr_pixel_error;
19852 }
19853
19854 void Util::multi_align_error_dfunc(double* x, vector<float> ali_params, int nima, int num_ali, double* g, int d) {
19855
19856 for (int i=0; i<num_ali*3-3; i++) g[i] = 0.0;
19857
19858 double* args = new double[num_ali*3];
19859 for (int i=0; i<3*num_ali-3; i++) args[i] = x[i];
19860 args[3*num_ali-3] = 0.0;
19861 args[3*num_ali-2] = 0.0;
19862 args[3*num_ali-1] = 0.0;
19863 double* cosa = new double[num_ali];
19864 double* sina = new double[num_ali];
19865 for (int i=0; i<num_ali; i++) {
19866 cosa[i] = cos(args[i*3]*M_PI/180.0);
19867 sina[i] = sin(args[i*3]*M_PI/180.0);
19868 }
19869 double* sx = new double[num_ali];
19870 double* sy = new double[num_ali];
19871
19872 vector<float> sqr_pixel_error(nima);
19873
19874 for (int i=0; i<nima; i++) {
19875 double sum_cosa = 0.0;
19876 double sum_sina = 0.0;
19877 for (int j=0; j<num_ali; j++) {
19878 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19879 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19880 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19881 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];
19882 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];
19883 } else {
19884 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19885 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19886 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];
19887 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];
19888 }
19889 }
19890 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
19891 sum_cosa /= P;
19892 sum_sina /= P;
19893 for (int j=0; j<num_ali-1; j++) {
19894 double dx = 2.0*(sx[j]-mean(sx, num_ali));
19895 double dy = 2.0*(sy[j]-mean(sy, num_ali));
19896 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19897 g[j*3] += (d*d/4.0*(sum_cosa*sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) -
19898 sum_sina*cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
19899 dx*(-ali_params[j*nima*4+i*4+1]*sina[j]+ali_params[j*nima*4+i*4+2]*cosa[j])+
19900 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;
19901 g[j*3+1] += dx;
19902 g[j*3+2] += dy;
19903 } else {
19904 g[j*3] += (d*d/4.0*(-sum_cosa*sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) +
19905 sum_sina*cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
19906 dx*( ali_params[j*nima*4+i*4+1]*sina[j]+ali_params[j*nima*4+i*4+2]*cosa[j])+
19907 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;
19908 g[j*3+1] += -dx;
19909 g[j*3+2] += dy;
19910 }
19911 }
19912 }
19913
19914 for (int i=0; i<3*num_ali-3; i++) g[i] /= (num_ali*nima);
19915
19916 delete[] args;
19917 delete[] cosa;
19918 delete[] sina;
19919 delete[] sx;
19920 delete[] sy;
19921 }
19922
19923 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
19924
19925 EMData *rot= new EMData();
19926 float ccc;
19927
19928 rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
19929 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19930 delete rot;
19931 return ccc;
19932 }
19933
19934 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19935
19936 double x[4];
19937 int n;
19938 int l = 3;
19939 int m = 200;
19940 double e = 1e-9;
19941 double step = 0.001;
19942 float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
19943
19944 x[1] = ang;
19945 x[2] = sxs;
19946 x[3] = sys;
19947
19948 Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);
19949
19950
19951 vector<float> res;
19952 res.push_back(static_cast<float>(x[1]));
19953 res.push_back(static_cast<float>(x[2]));
19954 res.push_back(static_cast<float>(x[3]));
19955 res.push_back(static_cast<float>(n));
19956 return res;
19957 }
19958
19959
19960 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
19961
19962 EMData *rot= new EMData();
19963 float ccc;
19964
19965 rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
19966 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19967 delete rot;
19968 return ccc;
19969 }
19970
19971 #define img_ptr(i,j,k) img_ptr[i+(j+(k*ny))*(size_t)nx]
19972 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*(size_t)nx]
19973 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
19974 {
19975 ENTERFUNC;
19976
19977 if (!img) {
19978 throw NullPointerException("NULL input image");
19979 }
19980 cout <<" VERSION 05/20/2013 3:00pm"<<endl;
19981 int newx, newy, newz;
19982 bool keep_going;
19983 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
19984
19985 EMData * img2 = new EMData();
19986 img2->set_size(nx,ny,nz);
19987 img2->to_zero();
19988 float *img_ptr =img->get_data();
19989 float *img2_ptr = img2->get_data();
19990 int r2 = ro*ro;
19991 int r3 = r2*ro;
19992 int ri2 = ri*ri;
19993 int ri3 = ri2*ri;
19994
19995 int n2 = nx/2;
19996
19997 for (int k=-n2; k<=n2; k++) {
19998 float z2 = static_cast<float>(k*k);
19999 for (int j=-n2; j<=n2; j++) {
20000 float y2 = z2 + j*j;
20001 if(y2 <= r2) {
20002
20003
20004 for (int i=-n2; i<=n2; i++) {
20005 float x2 = y2 + i*i;
20006 if(x2 <= r3) {
20007
20008 int ib = i+n2; int jb = j+n2; int kb = k+n2;
20009 if(x2 >= ri3) {
20010
20011 if(img_ptr(ib,jb,kb) == 1.0f) {
20012
20013 if(Util::get_frand(0.0f, 1.0f) > qprob){
20014 img2_ptr(ib,jb,kb) = 0.0f;
20015 keep_going = true;
20016
20017 while(keep_going) {
20018 newx = Util::get_irand(-ro,ro);
20019 newy = Util::get_irand(-ro,ro);
20020 newz = Util::get_irand(-ro,ro);
20021 if(newx*newx+newy*newy+newz*newz <= r3) {
20022 newx += n2; newy += n2; newz += n2;
20023 if( img_ptr(newx,newy,newz) == 0.0f) {
20024 img2_ptr(newx,newy,newz) = 1.0f;
20025 keep_going = false; }
20026 }
20027 }
20028 } else img2_ptr(ib,jb,kb) = 1.0f;
20029 }
20030 } else {
20031
20032 if(img_ptr(ib,jb,kb) == 1.0) {
20033 if(Util::get_frand(0.0f,1.0f) > qprob) {
20034
20035 float numn = -1.0f;
20036 for (newz = -1; newz <= 1; newz++)
20037 for (newy = -1; newy <= 1; newy++)
20038 for (newx = -1; newx <= 1; newx++)
20039 numn += img_ptr(ib+newx,jb+newy,kb+newz);
20040 img2_ptr(ib,jb,kb) = 0.0;
20041 if(numn == 26.0f) {
20042
20043 keep_going = true;
20044 while(keep_going) {
20045 newx = Util::get_irand(-ro,ro);
20046 newy = Util::get_irand(-ro,ro);
20047 newz = Util::get_irand(-ro,ro);
20048 if(newx*newx+newy*newy+newz*newz < r3) {
20049 newx += n2; newy += n2; newz += n2;
20050 if( img_ptr(newx,newy,newz) == 0.0f) {
20051 if(newx*newx+newy*newy+newz*newz < r3) {
20052 if(newx*newx+newy*newy+newz*newz < r3) {
20053 newx += n2; newy += n2; newz += n2;
20054 if( img_ptr(newx,newy,newz) == 0.0f) {
20055 img2_ptr(newx,newy,newz) = 1.0f;
20056 keep_going = false; }
20057 }
20058 }
20059 }
20060 }
20061 }
20062 } else if(numn == 25.0f) {
20063
20064 for (newz = -1; newz <= 1; newz++) {
20065 for (newy = -1; newy <= 1; newy++) {
20066 for (newx = -1; newx <= 1; newx++) {
20067 if( newx != 0 && newy != 0 && newz != 0) {
20068 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
20069 img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
20070 }
20071 }
20072 }
20073 }
20074 }
20075 } else {
20076
20077 keep_going = true;
20078 while(keep_going) {
20079 newx = Util::get_irand(-1,1);
20080 newy = Util::get_irand(-1,1);
20081 newz = Util::get_irand(-1,1);
20082 if(newx != 0 && newy != 0 && newz != 0) {
20083 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
20084 img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;
20085 keep_going = false;
20086 }
20087 }
20088 }
20089 }
20090 } else img2_ptr(ib,jb,kb) = 1.0f;
20091 }
20092 }
20093 }
20094 }
20095 }
20096 }
20097 }
20098
20099 img2->update();
20100
20101 EXITFUNC;
20102 return img2;
20103 }
20104 #undef img_ptr
20105 #undef img2_ptr
20106
20107 struct point3d_t
20108 {
20109 point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
20110
20111 int x;
20112 int y;
20113 int z;
20114 };
20115
20116
20117 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
20118 {
20119 int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
20120 int noff = 6;
20121
20122 int nx = visited->get_xsize();
20123 int ny = visited->get_ysize();
20124 int nz = visited->get_zsize();
20125
20126 vector< point3d_t > pts;
20127 pts.push_back( point3d_t(ix, iy, iz) );
20128 visited->set_value_at( ix, iy, iz, (float)grpid );
20129
20130 int start = 0;
20131 int end = pts.size();
20132
20133 while( end > start ) {
20134 for(int i=start; i < end; ++i ) {
20135 int ix = pts[i].x;
20136 int iy = pts[i].y;
20137 int iz = pts[i].z;
20138
20139 for( int j=0; j < noff; ++j ) {
20140 int jx = ix + offs[j][0];
20141 int jy = iy + offs[j][1];
20142 int jz = iz + offs[j][2];
20143
20144 if( jx < 0 || jx >= nx ) continue;
20145 if( jy < 0 || jy >= ny ) continue;
20146 if( jz < 0 || jz >= nz ) continue;
20147
20148
20149 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
20150 pts.push_back( point3d_t(jx, jy, jz) );
20151 visited->set_value_at( jx, jy, jz, (float)grpid );
20152 }
20153
20154 }
20155 }
20156
20157 start = end;
20158 end = pts.size();
20159 }
20160 return pts.size();
20161 }
20162
20163
20164 EMData* Util::get_biggest_cluster( EMData* mg )
20165 {
20166 int nx = mg->get_xsize();
20167 int ny = mg->get_ysize();
20168 int nz = mg->get_zsize();
20169
20170 EMData* visited = new EMData();
20171 visited->set_size( nx, ny, nz );
20172 visited->to_zero();
20173 int grpid = 0;
20174 int maxgrp = 0;
20175 int maxsize = 0;
20176 for( int iz=0; iz < nz; ++iz ) {
20177 for( int iy=0; iy < ny; ++iy ) {
20178 for( int ix=0; ix < nx; ++ix ) {
20179 if( (*mg)(ix, iy, iz)==0.0 ) continue;
20180
20181 if( (*visited)(ix, iy, iz) > 0.0 ) {
20182
20183 continue;
20184 }
20185
20186 grpid++;
20187 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
20188 if( grpsize > maxsize ) {
20189 maxsize = grpsize;
20190 maxgrp = grpid;
20191 }
20192 }
20193 }
20194 }
20195
20196 Assert( maxgrp > 0 );
20197
20198 int npoint = 0;
20199 EMData* result = new EMData();
20200 result->set_size( nx, ny, nz );
20201 result->to_zero();
20202
20203 for( int iz=0; iz < nz; ++iz ) {
20204 for( int iy=0; iy < ny; ++iy ) {
20205 for( int ix=0; ix < nx; ++ix ) {
20206 if( (*visited)(ix, iy, iz)==maxgrp ) {
20207 (*result)(ix,iy,iz) = 1.0;
20208 npoint++;
20209 }
20210 }
20211 }
20212 }
20213
20214 Assert( npoint==maxsize );
20215 delete visited;
20216 return result;
20217
20218 }
20219
20220 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)
20221 {
20222 int ix, iy, iz;
20223 int i, j, k;
20224 int nr2, nl2;
20225 float ak;
20226 float scx, scy, scz;
20227 int offset = 2 - nx%2;
20228 int lsm = nx + offset;
20229 EMData* ctf_img1 = new EMData();
20230 ctf_img1->set_size(lsm, ny, nz);
20231 float freq = 1.0f/(2.0f*ps);
20232 scx = 2.0f/float(nx);
20233 if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20234 if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20235 nr2 = ny/2 ;
20236 nl2 = nz/2 ;
20237 for ( k=0; k<nz;k++) {
20238 iz = k; if(k>nl2) iz=k-nz;
20239 float oz2 = iz*scz*iz*scz;
20240 for ( j=0; j<ny;j++) {
20241 iy = j; if(j>nr2) iy=j - ny;
20242 float oy = iy*scy;
20243 float oy2 = oy*oy;
20244 for ( i=0; i<lsm/2; i++) {
20245 ix=i;
20246 if( dza == 0.0f) {
20247 ak=pow(ix*ix*scx*scx + oy2 + oz2, 0.5f)*freq;
20248 (*ctf_img1) (i*2,j,k) = Util::tf(dz, ak, voltage, cs, wgh, b_factor, sign);
20249 } else {
20250 float ox = ix*scx;
20251 ak=pow(ox*ox + oy2 + oz2, 0.5f)*freq;
20252 float dzz = dz - dza/2.0f*sin(2*(atan2(oy, ox)+azz*M_PI/180.0f));
20253 (*ctf_img1) (i*2,j,k) = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
20254 }
20255
20256 }
20257 }
20258 }
20259 ctf_img1->update();
20260 ctf_img1->set_complex(true);
20261 ctf_img1->set_ri(true);
20262
20263
20264 if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
20265 return ctf_img1;
20266 }
20267
20268
20269
20270 EMData* Util::ctf_rimg(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)
20271 {
20272 int ix, iy, iz;
20273 int i, j, k;
20274 float ak;
20275 float scx, scy, scz;
20276 EMData* ctf_img1 = new EMData();
20277 ctf_img1->set_size(nx, ny, nz);
20278 float freq = 1.0f/(2.0f*ps);
20279 scx = 2.0f/float(nx);
20280 if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20281 if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20282 int ns2 = nx/2 ;
20283 int nr2 = ny/2 ;
20284 int nl2 = nz/2 ;
20285 int nod = nx%2 ;
20286 int nok = ny%2 ;
20287 int noz = nz%2 ;
20288 for ( k=0; k<nz;k++) {
20289 iz = k - nl2;
20290 int kz = (nz - k - noz)%nz;
20291 float oz2 = iz*scz*iz*scz;
20292 for ( j=0; j<ny;j++) {
20293 iy = j - nr2;
20294 int jy = (ny - j - nok)%ny;
20295 float oy = iy*scy;
20296 float oy2 = oy*oy;
20297 for ( i=0; i<=ns2; i++) {
20298 ix = i - ns2;
20299 if( dza == 0.0f) {
20300 ak=pow(ix*ix*scx*scx + oy2 + oz2, 0.5f)*freq;
20301 (*ctf_img1) (i,j,k) = Util::tf(dz, ak, voltage, cs, wgh, b_factor, sign);
20302 } else {
20303 float ox = ix*scx;
20304 ak=pow(ox*ox + oy2 + oz2, 0.5f)*freq;
20305 float dzz = dz - dza/2.0f*sin(2*(atan2(oy, ox)+azz*M_PI/180.0f));
20306 (*ctf_img1) (i,j,k) = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
20307 }
20308 ix = nx - i - nod;
20309 if(ix<nx) (*ctf_img1) (ix,jy,kz) = (*ctf_img1) (i,j,k);
20310 }
20311 }
20312 }
20313 ctf_img1->update();
20314 return ctf_img1;
20315 }
20316
20317
20318
20319
20320 EMData* Util::ctf2_rimg(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)
20321 {
20322 int ix, iy, iz;
20323 int i, j, k;
20324 float ak;
20325 float scx, scy, scz;
20326 EMData* ctf_img1 = new EMData();
20327 ctf_img1->set_size(nx, ny, nz);
20328 float freq = 1.0f/(2.0f*ps);
20329 scx = 2.0f/float(nx);
20330 if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20331 if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20332 int ns2 = nx/2 ;
20333 int nr2 = ny/2 ;
20334 int nl2 = nz/2 ;
20335 int nod = nx%2 ;
20336 int nok = ny%2 ;
20337 int noz = nz%2 ;
20338 for ( k=0; k<nz;k++) {
20339 iz = k - nl2;
20340 int kz = (nz - k - noz)%nz;
20341 float oz2 = iz*scz*iz*scz;
20342 for ( j=0; j<ny;j++) {
20343 iy = j - nr2;
20344 int jy = (ny - j - nok)%ny;
20345 float oy = iy*scy;
20346 float oy2 = oy*oy;
20347 for ( i=0; i<=ns2; i++) {
20348 ix = i - ns2;
20349 if( dza == 0.0f) {
20350 ak=pow(ix*ix*scx*scx + oy2 + oz2, 0.5f)*freq;
20351 (*ctf_img1) (i,j,k) = pow(Util::tf(dz, ak, voltage, cs, wgh, b_factor, sign),2);
20352 } else {
20353 float ox = ix*scx;
20354 ak=pow(ox*ox + oy2 + oz2, 0.5f)*freq;
20355
20356
20357 float dzz = dz - dza/2.0f*sin(2*(atan2(oy, ox)+azz*M_PI/180.0f));
20358 (*ctf_img1) (i,j,k) = pow(Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign),2);
20359 }
20360 ix = nx - i - nod;
20361 if(ix<nx) (*ctf_img1) (ix,jy,kz) = (*ctf_img1) (i,j,k);
20362 }
20363 }
20364 }
20365 ctf_img1->update();
20366 return ctf_img1;
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 #define cent(i) out[i+N]
20459 #define assign(i) out[i]
20460 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
20461 int nx = d->get_xsize();
20462 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20463 vector<float> out(N+K+2);
20464 if(N*(N-1)/2 != nx) {
20465
20466 return out;}
20467
20468 for(int i=0; i<N; i++) assign(i) = float(i);
20469
20470 for(int i=0; i<N; i++) {
20471 int j = Util::get_irand(0,N-1);
20472 float temp = assign(i);
20473 assign(i) = assign(j);
20474 assign(j) = temp;
20475 }
20476 for(int k=0; k<K; k++) cent(k) = float(assign(k));
20477
20478
20479 for(int i=0; i<N; i++) assign(i) = 0.0f;
20480 float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
20481 bool change = true;
20482 int it = -1;
20483 int ct = -1;
20484 while ((change && disp < dispold) || ct > 0) {
20485
20486 change = false;
20487 dispold = disp;
20488 it++;
20489
20490
20491 disp = 0.0f;
20492 ct = 0;
20493 for(int i=0; i<N; i++) {
20494 qm = 1.0e23f;
20495 for(int k=0; k<K; k++) {
20496 if(float(i) == cent(k)) {
20497 qm = 0.0f;
20498 na = (float)k;
20499 } else {
20500 float dt = (*d)(mono(i,int(cent(k))));
20501 if(dt < qm) {
20502 qm = dt;
20503 na = (float)k;
20504 }
20505 }
20506 }
20507
20508
20509
20510 if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
20511 na = (float)(Util::get_irand(0, K));
20512 qm = (*d)(mono(i,int(na)));
20513 ct++;
20514 }
20515
20516 disp += qm;
20517
20518 if(na != assign(i)) {
20519 assign(i) = na;
20520 change = true;
20521 }
20522 }
20523
20524
20525 T = T*F;
20526
20527
20528
20529
20530
20531 for(int k=0; k<K; k++) {
20532 qm = 1.0e23f;
20533 for(int i=0; i<N; i++) {
20534 if(assign(i) == float(k)) {
20535 float q = 0.0;
20536 for(int j=0; j<N; j++) {
20537 if(assign(j) == float(k)) {
20538
20539 if(i != j) q += (*d)(mono(i,j));
20540
20541 }
20542 }
20543 if(q < qm) {
20544
20545 qm = q;
20546 cent(k) = float(i);
20547 }
20548 }
20549 }
20550 }
20551
20552 }
20553 out[N+K] = disp;
20554 out[N+K+1] = float(it);
20555 return out;
20556 }
20557 #undef cent
20558 #undef assign
20559
20560
20561
20562
20563
20564
20565
20566
20567
20568
20569
20570
20571
20572
20573
20574
20575
20576
20577
20578
20579
20580
20581
20582
20583
20584
20585
20586
20587
20588
20589
20590
20591
20592
20593
20594
20595
20596
20597
20598
20599
20600
20601
20602
20603
20604
20605
20606
20607
20608
20609
20610
20611
20612
20613
20614
20615
20616
20617
20618
20619
20620
20621
20622
20623
20624
20625
20626
20627
20628
20629
20630
20631
20632
20633
20634
20635
20636
20637
20638
20639
20640
20641
20642
20643
20644
20645
20646
20647
20648
20649
20650
20651
20652
20653
20654
20655
20656
20657
20658
20659
20660
20661
20662
20663
20664
20665
20666
20667
20668 vector<float> Util::cluster_equalsize(EMData* d) {
20669
20670 int nx = d->get_xsize();
20671 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20672 int K = N/2;
20673 vector<float> group(N);
20674 if(N*(N-1)/2 != nx) {
20675
20676 return group;}
20677
20678 bool * active = new bool[N];
20679 for(int i=0; i<N; i++) active[i] = true;
20680
20681 float dm, qd;
20682 int ppi = 0, ppj = 0;
20683 for(int k=0; k<K; k++) {
20684
20685
20686 dm = 1.0e23f;
20687 for(int i=1; i<N; i++) {
20688 if(active[i]) {
20689 for(int j=0; j<i; j++) {
20690 if(active[j]) {
20691 qd = (*d)(i*(i - 1)/2 + j);
20692 if(qd < dm) {
20693 dm = qd;
20694 ppi = i;
20695 ppj = j;
20696 }
20697 }
20698 }
20699 }
20700 }
20701 group[2*k] = float(ppi);
20702 group[1+2*k] = float(ppj);
20703 active[ppi] = false;
20704 active[ppj] = false;
20705 }
20706
20707 delete [] active;
20708 active = NULL;
20709 return group;
20710 }
20711
20712
20713
20714
20715
20716
20717
20718
20719
20720
20721
20722
20723
20724
20725
20726
20727
20728
20729
20730
20731
20732
20733
20734
20735
20736
20737
20738
20739
20740
20741
20742
20743
20744
20745
20746
20747
20748
20749
20750
20751
20752
20753
20754
20755
20756
20757
20758
20759
20760
20761
20762
20763
20764
20765 #define data(i,j) group[i*ny+j]
20766 vector<float> Util::vareas(EMData* d) {
20767 const float step=0.001f;
20768 int ny = d->get_ysize();
20769
20770
20771
20772 vector<float> group(2*ny);
20773 for(int i=0; i<2*ny; i++) group[i] = 0.0f;
20774 int K = int(1.0f/step) +1;
20775 int hit = 0;
20776 for(int kx=0; kx<=K; kx++) {
20777 float tx = kx*step;
20778 for(int ky=0; ky<=K; ky++) {
20779 float ty = ky*step;
20780 float dm = 1.0e23f;
20781 for(int i=0; i<ny; i++) {
20782 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
20783 if( qd < dm) {
20784 dm = qd;
20785 hit = i;
20786 }
20787 }
20788 data(0,hit) += 1.0f;
20789 if(kx == 0 || ky == 0 || kx == K || ky == K) data(1,hit) = 1.0f;
20790 }
20791 }
20792 return group;
20793 }
20794 #undef data
20795
20796 EMData* Util::get_slice(EMData *vol, int dim, int index) {
20797
20798 int nx = vol->get_xsize();
20799 int ny = vol->get_ysize();
20800 int nz = vol->get_zsize();
20801 float *vol_data = vol->get_data();
20802 int new_nx, new_ny;
20803
20804 if (nz == 1)
20805 throw ImageDimensionException("Error: Input must be a 3-D object");
20806 if ((dim < 1) || (dim > 3))
20807 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
20808 if (((dim == 1) && (index < 0 || index > nx-1)) ||
20809 ((dim == 1) && (index < 0 || index > nx-1)) ||
20810 ((dim == 1) && (index < 0 || index > nx-1)))
20811 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
20812
20813 if (dim == 1) {
20814 new_nx = ny;
20815 new_ny = nz;
20816 } else if (dim == 2) {
20817 new_nx = nx;
20818 new_ny = nz;
20819 } else {
20820 new_nx = nx;
20821 new_ny = ny;
20822 }
20823
20824 EMData *slice = new EMData();
20825 slice->set_size(new_nx, new_ny, 1);
20826 float *slice_data = slice->get_data();
20827
20828 if (dim == 1) {
20829 for (int x=0; x<new_nx; x++)
20830 for (int y=0; y<new_ny; y++)
20831 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
20832 } else if (dim == 2) {
20833 for (int x=0; x<new_nx; x++)
20834 for (int y=0; y<new_ny; y++)
20835 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
20836 } else {
20837 for (int x=0; x<new_nx; x++)
20838 for (int y=0; y<new_ny; y++)
20839 slice_data[y*new_nx+x] = vol_data[((size_t)index*ny+y)*nx+x];
20840 }
20841
20842 return slice;
20843 }
20844
20845 void Util::image_mutation(EMData *img, float mutation_rate) {
20846 int nx = img->get_xsize();
20847 float min = img->get_attr("minimum");
20848 float max = img->get_attr("maximum");
20849 float* img_data = img->get_data();
20850 array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
20851 return;
20852 }
20853
20854
20855 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20856
20857 if (is_mirror != 0) {
20858 for (int i=0; i<len_list; i++) {
20859 int r = rand()%10000;
20860 float f = r/10000.0f;
20861 if (f < mutation_rate) list[i] = 1-list[i];
20862 }
20863 } else {
20864 map<int, vector<int> > graycode;
20865 map<vector<int>, int> rev_graycode;
20866 vector <int> gray;
20867
20868 int K=1;
20869 for (int i=0; i<L; i++) K*=2;
20870
20871 for (int k=0; k<K; k++) {
20872 int shift = 0;
20873 vector <int> gray;
20874 for (int i=L-1; i>-1; i--) {
20875 int t = ((k>>i)%2-shift)%2;
20876 gray.push_back(t);
20877 shift += t-2;
20878 }
20879 graycode[k] = gray;
20880 rev_graycode[gray] = k;
20881 }
20882
20883 float gap = (K-1)/(max_val-min_val);
20884 for (int i=0; i<len_list; i++) {
20885 float val = list[i];
20886 if (val < min_val) { val = min_val; }
20887 else if (val > max_val) { val = max_val; }
20888 int k = int((val-min_val)*gap+0.5);
20889 vector<int> gray = graycode[k];
20890 bool changed = false;
20891 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20892 int r = rand()%10000;
20893 float f = r/10000.0f;
20894 if (f < mutation_rate) {
20895 *p = 1-*p;
20896 changed = true;
20897 }
20898 }
20899 if (changed) {
20900 k = rev_graycode[gray];
20901 list[i] = k/gap+min_val;
20902 }
20903 }
20904 }
20905
20906 }
20907
20908 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20909
20910 if (is_mirror != 0) {
20911 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20912 int r = rand()%10000;
20913 float f = r/10000.0f;
20914 if (f < mutation_rate) *q = 1-*q;
20915 }
20916 } else {
20917 map<int, vector<int> > graycode;
20918 map<vector<int>, int> rev_graycode;
20919 vector <int> gray;
20920
20921 int K=1;
20922 for (int i=0; i<L; i++) K*=2;
20923
20924 for (int k=0; k<K; k++) {
20925 int shift = 0;
20926 vector <int> gray;
20927 for (int i=L-1; i>-1; i--) {
20928 int t = ((k>>i)%2-shift)%2;
20929 gray.push_back(t);
20930 shift += t-2;
20931 }
20932 graycode[k] = gray;
20933 rev_graycode[gray] = k;
20934 }
20935
20936 float gap = (K-1)/(max_val-min_val);
20937 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20938 float val = *q;
20939 if (val < min_val) { val = min_val; }
20940 else if (val > max_val) { val = max_val; }
20941 int k = int((val-min_val)*gap+0.5);
20942 vector<int> gray = graycode[k];
20943 bool changed = false;
20944 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20945 int r = rand()%10000;
20946 float f = r/10000.0f;
20947 if (f < mutation_rate) {
20948 *p = 1-*p;
20949 changed = true;
20950 }
20951 }
20952 if (changed) {
20953 k = rev_graycode[gray];
20954 *q = k/gap+min_val;
20955 }
20956 }
20957 }
20958 return list;
20959 }
20960
20961
20962 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
20963
20964 int total_cost = *output;
20965 int num_matches = *(output+1);
20966
20967 int cost=0;
20968 int* intx;
20969 int intx_size;
20970 int* intx_next(0);
20971 int intx_next_size = 0;
20972 int curclass;
20973 int curclass_size;
20974
20975 for(int i = 0; i < num_matches; i++){
20976 curclass = *(output+2+ i*nParts);
20977
20978 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
20979 *(argParts + Indices[curclass]+1) = -5;
20980
20981 curclass_size = *(dimClasses+curclass)-2;
20982 intx = new int[curclass_size];
20983 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
20984 intx_size = curclass_size;
20985
20986 for (int j=1; j < nParts; j++){
20987 curclass = *(output+2+ i*nParts+j);
20988 if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
20989 *(argParts + Indices[j*K+curclass]+1)=-5;
20990
20991 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);
20992 intx_next = new int[intx_next_size];
20993 Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
20994 delete[] intx;
20995 intx=intx_next;
20996 intx_size= intx_next_size;
20997 }
20998 delete[] intx_next;
20999
21000 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
21001
21002 cost = cost + intx_next_size;
21003 }
21004
21005 if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
21006
21007 return 1;
21008
21009 }
21010
21011
21012
21013
21014
21015
21016 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int* costlist, int J){
21017
21018
21019 bool flag = 0;
21020 int nintx;
21021 int* dummy(0);
21022
21023 int* curbranch = new int[nParts];
21024
21025
21026 for(int jit= 0; jit< J; jit++) *(costlist+jit) = 0;
21027
21028
21029 for(int a=0; a<K; a++)
21030 {
21031
21032
21033 if (*(argParts + Indices[a] + 1) < 1) continue;
21034 if (*(dimClasses + a)-2 <= T) continue;
21035
21036
21037
21038 for( int i=1; i < nParts; i++){
21039 flag = 0;
21040 for(int j=0; j < K; j++){
21041 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
21042 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);
21043 if (nintx > T) flag=1;
21044 else *(argParts + Indices[i*K+j] + 1) =-4;
21045 }
21046 if (flag==0) {break;}
21047 }
21048
21049
21050 *curbranch = a;
21051
21052 if (flag > 0)
21053 Util::explore2(argParts, Indices, dimClasses, nParts, K, T, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2,
21054 *(dimClasses+a)-2,0, J, matchlist, costlist, curbranch);
21055
21056
21057 for( int i=1; i < nParts; i++){
21058 for(int j=0; j < K; j++){
21059 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
21060
21061 }
21062 }
21063 }
21064
21065 delete[] curbranch;
21066 }
21067
21068
21069 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){
21070
21071
21072 int* curintx2(0);
21073 int nintx = size_curintx;
21074
21075
21076
21077 if (depth >0){
21078 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
21079 if (nintx <= T) return;
21080 }
21081
21082
21083 if (depth == (nParts-1)) {
21084
21085 int replace = 0;
21086 int ind_smallest = -1;
21087 int smallest_cost = -1;
21088
21089 for (int jit = 0; jit < J; jit++){
21090 if (*(costlist+jit) < nintx){
21091 replace = 1;
21092 if (ind_smallest == -1) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
21093 if (*(costlist+jit) < smallest_cost) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
21094 }
21095 }
21096
21097 if (replace > 0){
21098
21099 *(costlist + ind_smallest) = nintx;
21100 for (int xit = 0; xit < nParts; xit++)
21101 *(matchlist + ind_smallest*nParts + xit) = *(curbranch+xit);
21102
21103 }
21104
21105 return;
21106 }
21107
21108
21109
21110
21111 if (depth > 0){
21112 curintx2 = new int[nintx];
21113 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
21114 }
21115
21116 if (depth == 0){
21117
21118 curintx2 = new int[size_curintx];
21119 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
21120 }
21121
21122
21123
21124 depth=depth+1;
21125
21126 for (int i=0; i < K; i++){
21127
21128 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue;
21129 size_next = (*(dimClasses + depth*K+i ))-2;
21130 if (size_next <= T) continue;
21131 *(curbranch+depth) = i;
21132 Util::explore2(argParts,Indices, dimClasses, nParts, K, T, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth,J, matchlist,
21133 costlist, curbranch);
21134
21135 }
21136
21137 delete[] curintx2;
21138 }
21139
21140 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
21141
21142
21143
21144
21145
21146
21147
21148
21149
21150
21151 int* dummy(0);
21152 int* cref;
21153 int cref_size;
21154 int* ccomp;
21155 int ccomp_size;
21156 int nintx;
21157 for (int i=0; i < nParts; i++){
21158 for (int j =0; j < K; j++){
21159
21160
21161 cref = Parts[i][j];
21162 cref_size = dimClasses[i*K+cref[0]]-2;
21163
21164
21165 if (cref_size <= T){
21166 cref[0] = -1;
21167 continue;
21168 }
21169 bool done = 0;
21170 for (int a = 0; a < nParts; a++){
21171 if (a == i) continue;
21172 bool hasActive=0;
21173 for (unsigned int b=0; b < Parts[a].size(); b++){
21174
21175
21176 ccomp = Parts[a][b];
21177 ccomp_size= dimClasses[a*K+ccomp[0]]-2;
21178 nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
21179
21180
21181 if (nintx <= T)
21182 ccomp[1] = 0;
21183 else{
21184 ccomp[1] = 1;
21185 hasActive=1;
21186 }
21187 }
21188
21189 if (hasActive < 1){
21190 done=1;
21191 break;
21192 }
21193
21194 }
21195
21196 if (done > 0){
21197
21198
21199 cref[0] = -1;
21200 continue;
21201 }
21202
21203
21204
21205
21206
21207
21208
21209
21210
21211
21212 bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
21213
21214 if (found<1){
21215
21216 cref[0] = -1;
21217 }
21218 }
21219
21220
21221
21222 for (int d = K-1; d > -1; d--){
21223 if (Parts[i][d][0] < 0) Parts[i].erase(Parts[i].begin()+d);
21224 }
21225
21226 }
21227
21228
21229
21230
21231
21232 }
21233
21234
21235 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) {
21236
21237
21238 if (size_next <= T) return 0;
21239
21240
21241 int* curintx2(0);
21242 int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
21243 if (nintx <= T) return 0;
21244
21245 int old_depth=depth;
21246 if (depth == partref) depth = depth + 1;
21247 if (depth == nParts && old_depth>0) return 1;
21248
21249
21250
21251 curintx2 = new int[nintx];
21252 Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
21253
21254
21255
21256
21257 bool gt_thresh;
21258 int num_classes = Parts[depth].size();
21259
21260 for (int i=0; i < num_classes; i++){
21261 if (Parts[depth][i][1] < 1) continue;
21262 size_next = dimClasses[depth*K + Parts[depth][i][0] ]-2;
21263 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
21264 if (gt_thresh) { delete[] curintx2; return 1; }
21265 }
21266 delete[] curintx2;
21267 return 0;
21268 }
21269
21270
21271
21272
21273 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int n_guesses, int LARGEST_CLASS, int J,
21274 int max_branching, float stmult, int branchfunc, int LIM) {
21275
21276
21277
21278
21279
21280 int* Indices = new int[nParts*K];
21281 int ind_c = 0;
21282 for (int i=0; i < nParts; i++){
21283 for(int j = 0; j < K; j++){
21284 Indices[i*K + j] = ind_c;
21285 ind_c = ind_c + dimClasses[i*K + j];
21286 }
21287 }
21288
21289
21290
21291
21292 vector <vector <int*> > Parts(nParts,vector<int*>(K));
21293 ind_c = 0;
21294 int argParts_size=0;
21295 for (int i=0; i < nParts; i++){
21296 for(int j = 0; j < K; j++){
21297 Parts[i][j] = argParts + ind_c;
21298 ind_c = ind_c + dimClasses[i*K + j];
21299 argParts_size = argParts_size + dimClasses[i*K + j];
21300 }
21301 }
21302
21303
21304
21305
21306
21307
21308 Util::initial_prune(Parts, dimClasses, nParts, K, T);
21309 for(int i = 0; i < nParts; i++){
21310 for(int j=0; j < K; j++){
21311 argParts[Indices[i*K + j]+1] = -1;
21312 }
21313 }
21314
21315 int num_classes;
21316 int old_index;
21317 for(int i=0; i<nParts; i++){
21318 num_classes = Parts[i].size();
21319 for (int j=0; j < num_classes; j++){
21320 old_index = Parts[i][j][0];
21321
21322 argParts[Indices[i*K + old_index]+1] = 1;
21323 }
21324 }
21325
21326
21327
21328
21329
21330 int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T, 0, n_guesses, LARGEST_CLASS, J, max_branching, stmult, branchfunc, LIM);
21331
21332
21333
21334
21335 bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
21336
21337 delete[] Indices;
21338
21339
21340 if (correct < 1){
21341 cout << "something is wrong with output of branchMPI!\n";
21342 vector<int> ret(1);
21343 ret[0] = -1;
21344 if (output != 0) { delete[] output; output = 0; }
21345 return ret;
21346 }
21347
21348
21349
21350
21351
21352 int output_size = 2 + output[1] * nParts;
21353 vector<int> ret(output_size);
21354 for (int i = 0; i < output_size; i++) {
21355 ret[i]= output[i];
21356 }
21357 if (output != 0) { delete[] output; output = 0; }
21358 return ret;
21359
21360 }
21361
21362
21363 int branch_all=0;
21364 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int curlevel,int n_guesses, int
21365 LARGEST_CLASS, int J, int max_branching, float stmult, int branchfunc, int LIM) {
21366
21367
21368
21369 if (1 == 0){
21370 cout <<"begin test search2\n";
21371 int* matchlist = new int[J*nParts];
21372 int* costlist = new int[J];
21373 for (int jit = 0; jit < nParts; jit++) *(costlist+jit) = 0;
21374 Util::search2(argParts,Indices, dimClasses, nParts, K, T, matchlist,costlist,J);
21375
21376 for (int jit = 0; jit < J; jit++) {
21377 cout << *(costlist +jit)<<": ";
21378 for (int yit = 0; yit < nParts; yit++)
21379 cout << *(matchlist + jit*nParts + yit)<<",";
21380 cout <<"\n";
21381
21382 }
21383 cout <<"end test search2\n";
21384 int* output = new int[1];
21385 output[0] = 1;
21386 delete [] matchlist;
21387 delete [] costlist;
21388 return output;
21389 }
21390
21391
21392
21393 if (curlevel > K -1){
21394 int* output = new int[2];
21395 output[0] = 0;
21396 output[1] = 0;
21397 return output;
21398 }
21399
21400
21401
21402 int* matchlist = new int[J*nParts];
21403 int* costlist = new int[J];
21404 Util::search2(argParts, Indices, dimClasses, nParts, K, T, matchlist, costlist, J);
21405
21406
21407
21408
21409
21410
21411 for (int jit = 0; jit < J ; jit++){
21412
21413 if (costlist[jit] > T) break;
21414 if (jit == J-1){
21415 int* output = new int[2];
21416 output[0] = 0;
21417 output[1] = 0;
21418 delete[] matchlist;
21419 delete[] costlist;
21420 return output;
21421 }
21422 }
21423
21424
21425
21426
21427 if (curlevel==0) branch_all = 0;
21428
21429 int nBranches = -1;
21430
21431 if (branchfunc == 0)
21432 nBranches = branch_factor_0(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM);
21433
21434 if (branchfunc == 2)
21435 nBranches = branch_factor_2(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM);
21436
21437 if (branchfunc == 3)
21438 nBranches = branch_factor_3(costlist,matchlist,J, T, nParts, curlevel, max_branching, K, LIM);
21439
21440 if (branchfunc == 4)
21441 nBranches = branch_factor_4(costlist,matchlist,J, T, nParts, curlevel, max_branching, stmult);
21442
21443 int* newcostlist= new int[nBranches];
21444 int* newmatchlist = new int[nBranches*nParts];
21445 for (int i=0; i<nBranches; i++){
21446 newcostlist[i] = costlist[i];
21447 for (int j=0; j< nParts; j++)
21448 newmatchlist[i*nParts + j] = matchlist[i*nParts + j];
21449 }
21450
21451 delete[] costlist;
21452 delete[] matchlist;
21453
21454
21455 int* output = new int[2+K*nParts];
21456 output[0] = 0;
21457 output[1] = 0;
21458
21459 int old_index;
21460 int totalcost;
21461 int nmatches;
21462
21463
21464 for(int i=0; i < nBranches ; i++){
21465
21466
21467
21468
21469
21470
21471
21472 for(int j=0; j < nParts; j++){
21473
21474 old_index = newmatchlist[i*nParts + j];
21475 argParts[Indices[j*K+old_index] + 1] = -2;
21476 }
21477
21478
21479 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, curlevel+1, n_guesses, LARGEST_CLASS,
21480 J, max_branching, stmult,branchfunc, LIM);
21481
21482
21483 totalcost = newcostlist[i] + ret[0];
21484
21485
21486
21487
21488
21489 if (totalcost > output[0])
21490 {
21491 nmatches = 1 + ret[1];
21492
21493
21494 output[0] = totalcost;
21495 output[1] = nmatches;
21496 int nret = 2+(nmatches-1)*nParts;
21497 for(int iret=2; iret < nret; iret++) output[iret] = ret[iret];
21498 for(int imax=0; imax < nParts; imax++) output[nret+imax] = newmatchlist[i*nParts + imax];
21499 }
21500
21501
21502 delete[] ret;
21503
21504
21505
21506 for(int j=0; j < nParts; j++){
21507 old_index = newmatchlist[i*nParts + j];
21508 argParts[Indices[j*K+old_index] + 1] = 1;
21509 }
21510
21511 }
21512
21513 delete[] newmatchlist;
21514 delete[] newcostlist;
21515
21516 return output;
21517 }
21518
21519 int* costlist_global;
21520
21521 bool jiafunc(int i, int j){
21522 return (costlist_global[j] < costlist_global[i]) ;
21523
21524 }
21525
21526
21527
21528
21529
21530 int Util::branch_factor_2(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21531
21532 int ntot=0;
21533 for (int jit=0; jit < J; jit++){
21534 if (*(costlist+jit) > T) ntot++;
21535 }
21536
21537 int cur;
21538
21539 int* indx = new int[J];
21540 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21541 vector<int> myindx (indx, indx+J);
21542 vector<int>::iterator it;
21543 costlist_global=costlist;
21544 sort(myindx.begin(), myindx.end(), jiafunc);
21545
21546
21547 int* templist = new int[J];
21548 int* temp2list = new int[J*nParts];
21549 int next = 0;
21550
21551 for (it=myindx.begin(); it!=myindx.end();++it){
21552 cur = *(costlist + *it);
21553 if (cur > T){
21554
21555 templist[next] = cur;
21556 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21557 next = next + 1;
21558 }
21559 }
21560
21561 for (int jit=0; jit < ntot; jit++){
21562 *(costlist+jit)=*(templist + jit);
21563
21564 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21565 }
21566
21567
21568 delete [] indx;
21569
21570
21571
21572
21573
21574
21575 int B=1;
21576 int B_init=B;
21577 int infeasible=0;
21578
21579 for (int i=B_init; i<ntot; i++){
21580 if (i==ntot) continue;
21581
21582
21583 infeasible = 0;
21584 if (LIM < 0) LIM = B;
21585 for (int j=0; j<B; j++){
21586
21587 for (int vit=0; vit<nParts; vit++){
21588 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21589 }
21590 if (infeasible >= LIM) break;
21591 }
21592
21593 if (infeasible >= LIM){
21594 *(costlist+B)=*(templist+i);
21595 for (int vit=0; vit < nParts; vit++)
21596 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21597 B=B+1;
21598 }
21599 }
21600
21601 delete [] templist;
21602 delete [] temp2list;
21603
21604
21605 if (branch_all < max_branching){
21606 if (B>1)
21607 {branch_all = branch_all + B -1 ; }
21608 }
21609 else B=1;
21610
21611 return B;
21612
21613
21614 }
21615
21616
21617
21618 int Util::branch_factor_3(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int, int LIM){
21619
21620 int ntot=0;
21621 for (int jit=0; jit < J; jit++){
21622 if (*(costlist+jit) > T) ntot++;
21623 }
21624
21625 int cur;
21626
21627 int* indx = new int[J];
21628 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21629 vector<int> myindx (indx, indx+J);
21630 vector<int>::iterator it;
21631 costlist_global=costlist;
21632 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21633
21634
21635 int* templist = new int[J];
21636 int* temp2list = new int[J*nParts];
21637 int next = 0;
21638
21639 for (it=myindx.begin(); it!=myindx.end();++it){
21640 cur = *(costlist + *it);
21641 if (cur > T){
21642
21643 templist[next] = cur;
21644 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21645 next = next + 1;
21646 }
21647 }
21648
21649 for (int jit=0; jit < ntot; jit++){
21650 *(costlist+jit)=*(templist + jit);
21651
21652 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21653 }
21654
21655
21656 delete [] indx;
21657
21658
21659
21660
21661
21662
21663 int B=1;
21664 int B_init=B;
21665 int infeasible=0;
21666
21667
21668 if (LIM < 0) LIM = ntot-1;
21669 for (int i=B_init; i<ntot; i++){
21670 if (i==ntot) continue;
21671
21672
21673 infeasible = 0;
21674
21675 for (int j=0; j<ntot; j++){
21676 if (j == i) continue;
21677 for (int vit=0; vit<nParts; vit++){
21678 if (temp2list[i*nParts+vit] == temp2list[j*nParts+vit]) {infeasible++; break;}
21679 }
21680 if (infeasible >= LIM) break;
21681 }
21682
21683 if (infeasible >= LIM){
21684 *(costlist+B)=*(templist+i);
21685 for (int vit=0; vit < nParts; vit++)
21686 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21687 B=B+1;
21688 }
21689 }
21690
21691 delete [] templist;
21692 delete [] temp2list;
21693
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 }
21706
21707
21708
21709
21710
21711 int Util::branch_factor_4(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, float stmult){
21712 int sum=0;
21713 float average =0;
21714 int ntot=0;
21715 for (int jit=0; jit < J; jit++){
21716 if (*(costlist+jit) > T) {ntot++; sum = sum +*(costlist+jit);}
21717 }
21718 average = ((float)sum)/((float)ntot);
21719 int cur;
21720
21721 int* indx = new int[J];
21722 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21723 vector<int> myindx (indx, indx+J);
21724 vector<int>::iterator it;
21725 costlist_global=costlist;
21726 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21727
21728
21729 int* templist = new int[J];
21730 int* temp2list = new int[J*nParts];
21731 int next = 0;
21732
21733 for (it=myindx.begin(); it!=myindx.end();++it){
21734 cur = *(costlist + *it);
21735 if (cur > T){
21736
21737 templist[next] = cur;
21738 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21739 next = next + 1;
21740 }
21741 }
21742
21743 for (int jit=0; jit < ntot; jit++){
21744 *(costlist+jit)=*(templist + jit);
21745
21746 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21747 }
21748
21749
21750 delete [] indx;
21751 delete [] templist;
21752 delete [] temp2list;
21753
21754 if (ntot == 1) return 1;
21755
21756
21757
21758 float sq_sum=0.0;
21759
21760 for (int i=0; i< ntot; i++){
21761 sq_sum = sq_sum + (float) pow((float) *(costlist+i) - average, (float)2.0);
21762
21763 }
21764
21765
21766 float variance = sq_sum/ntot;
21767 float stdev = (float)pow((float)variance,(float)0.5);
21768
21769
21770
21771 int B=1;
21772 int largest = *costlist;
21773
21774 for (int i=1; i<ntot; i++){
21775 int cur = *(costlist+i);
21776 if (largest-cur < (float)(stdev*stmult)) B++;
21777 else break;
21778
21779 }
21780
21781 if (branch_all < max_branching){
21782 if (B>1)
21783 {branch_all = branch_all + B-1;}
21784 }
21785 else B=1;
21786
21787 return B;
21788
21789
21790 }
21791
21792 int Util::branch_factor_0(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21793
21794 int ntot=0;
21795 for (int jit=0; jit < J; jit++){
21796 if (*(costlist+jit) > T) ntot++;
21797 }
21798
21799 int cur;
21800
21801 int* indx = new int[J];
21802 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21803 vector<int> myindx (indx, indx+J);
21804 vector<int>::iterator it;
21805 costlist_global=costlist;
21806 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21807
21808
21809 int* templist = new int[J];
21810 int* temp2list = new int[J*nParts];
21811 int next = 0;
21812
21813 for (it=myindx.begin(); it!=myindx.end();++it){
21814 cur = *(costlist + *it);
21815 if (cur > T){
21816
21817 templist[next] = cur;
21818 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21819 next = next + 1;
21820 }
21821 }
21822
21823 for (int jit=0; jit < ntot; jit++){
21824 *(costlist+jit)=*(templist + jit);
21825
21826 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21827 }
21828
21829
21830 for (int jit=1; jit < ntot; jit++){
21831
21832 if ((costlist[jit] == costlist[0]) && costlist[jit] > T){
21833
21834 for (int vit=0; vit < nParts; vit++){
21835 if ( matchlist[jit*nParts + vit] > matchlist[vit])
21836 break;
21837 if ( matchlist[jit*nParts + vit] == matchlist[vit])
21838 continue;
21839 if ( matchlist[jit*nParts + vit] < matchlist[vit])
21840 {
21841
21842 for (int swp=0; swp < nParts; swp++){
21843 int tmp = matchlist[swp];
21844 matchlist[swp]= matchlist[jit*nParts + swp];
21845 matchlist[jit*nParts + swp] = tmp;
21846 }
21847 break;
21848
21849 }
21850 }
21851 }
21852
21853 }
21854
21855
21856 delete [] indx;
21857
21858
21859
21860
21861
21862
21863 int B=1;
21864 int B_init=B;
21865 int infeasible=0;
21866
21867 for (int i=B_init; i<ntot; i++){
21868 if (i==ntot) continue;
21869
21870
21871 infeasible = 0;
21872 if (LIM < 0) LIM = B;
21873 for (int j=0; j<B; j++){
21874
21875 for (int vit=0; vit<nParts; vit++){
21876 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21877 }
21878 if (infeasible >= LIM) break;
21879 }
21880
21881 if (infeasible >= LIM){
21882 *(costlist+B)=*(templist+i);
21883 for (int vit=0; vit < nParts; vit++)
21884 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21885 B=B+1;
21886 }
21887 }
21888
21889 delete [] templist;
21890 delete [] temp2list;
21891
21892
21893 if (branch_all < max_branching){
21894 if (B>1)
21895 {branch_all = branch_all + B -1 ; }
21896 }
21897 else B=1;
21898
21899 return B;
21900
21901
21902 }
21903
21904
21905 Dict Util::get_transform_params(EMData* image, string xform, string convention)
21906 {
21907 const Transform * transform = image->get_attr(xform);
21908 Dict transform_params = transform->get_params(convention);
21909 delete transform;
21910 return transform_params;
21911 }
21912
21913
21914 static void compose_transform2(float psi1, float sx1, float sy1, float psi2, float sx2, float sy2, float & out_psi, float & out_sx, float & out_sy)
21915 {
21916 Dict t_params;
21917 t_params["type"] = "2D";
21918 t_params["alpha"] = psi1;
21919 t_params["tx"] = sx1;
21920 t_params["ty"] = sy1;
21921 t_params["mirror"] = 0;
21922 t_params["scale"] = 1.0;
21923 Transform t1(t_params);
21924 t_params["alpha"] = psi2;
21925 t_params["tx"] = sx2;
21926 t_params["ty"] = sy2;
21927 Transform t2(t_params);
21928 Transform tt = t2*t1;
21929 Dict d = tt.get_params("2D");
21930 out_psi = d["alpha"];
21931 out_sx = d["tx"];
21932 out_sy = d["ty"];
21933 }
21934
21935 void Util::constrained_helix( vector<EMData*> data, vector<EMData*> fdata, vector<EMData*> refproj, vector<EMData*> rotproj
21936 , vector<float> dp_dphi_rise_delta, vector<int> nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc
21937 , bool FindPsi, float psi_max, vector<EMData*> crefim, vector<int> numr, int maxrin, string mode, int cnx, int cny)
21938 {
21939 if (dp_dphi_rise_delta.size() < 4) {
21940 printf("Not enough parameters (dp_dphi_rise_delta)");
21941 return;
21942 }
21943 if (nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc.size() < 9) {
21944 printf("Not enough parameters (nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc)");
21945 return;
21946 }
21947 float dpsi;
21948
21949 float dphi = dp_dphi_rise_delta[1];
21950 int rise = static_cast<int>(dp_dphi_rise_delta[2] + 0.2);
21951 float delta = dp_dphi_rise_delta[3];
21952 int nphi = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[0];
21953 int phiwobble = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[1];
21954 int range = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[2];
21955 int ywobble = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[3];
21956 bool Dsym = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[4];
21957 int nwx = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[5];
21958 int nwy = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[6];
21959 int nwxc = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[7];
21960 int nwyc = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[8];
21961
21962 const int ndata = data.size();
21963
21964 std::vector< boost::shared_ptr<EMData> > objectsToDelete;
21965
21966 vector<float> c0 = data[0]->get_attr("ptcl_source_coord");
21967 vector< vector<EMData*> > ccfs(ndata, vector<EMData*>(nphi));
21968 vector< vector<EMData*> > ccfr(0);
21969 if (! Dsym) {
21970 ccfr.resize(ndata, vector<EMData*>(nphi));
21971 }
21972 for (int im = 0; im < ndata; ++im) {
21973 for (int iphi = 0; iphi < nphi; ++iphi) {
21974 std::auto_ptr<EMData> corr( correlation( refproj[iphi], fdata[im], CIRCULANT, true) );
21975 ccfs[im][iphi] = Util::window( corr.get(), nwx, nwy);
21976 objectsToDelete.push_back(boost::shared_ptr<EMData>(ccfs[im][iphi]));
21977 if (! Dsym) {
21978 std::auto_ptr<EMData> corr2( correlation( rotproj[iphi], fdata[im], CIRCULANT, true) );
21979 ccfr[im][iphi] = Util::window( corr2.get(), nwx, nwy);
21980 objectsToDelete.push_back(boost::shared_ptr<EMData>(ccfr[im][iphi]));
21981 }
21982 }
21983 }
21984 vector<float> dxshiftlocal(ndata, 0);
21985 vector<float> dyshiftlocal(ndata, 0);
21986 vector<float> dphilocal(ndata, 0);
21987
21988 vector<float> xshiftlocal(ndata, 0);
21989 vector<float> xrshiftlocal(ndata, 0);
21990 vector<float> mxshiftlocal(ndata, 0);
21991 vector<float> yshiftlocal(ndata, 0);
21992 vector<float> yrshiftlocal(ndata, 0);
21993 vector<float> myshiftlocal(ndata, 0);
21994 vector<float> philocal(ndata, 0);
21995 vector<float> phirlocal(ndata, 0);
21996 vector<float> mphilocal(ndata, 0);
21997 float dirma = -1.0e23f;
21998 for (int idir = -1; idir < 2; idir += 2) {
21999 float tmax = -1.0e23f;
22000 float mpsi;
22001 for (int ix = 1; ix < nwx-1; ++ix) {
22002
22003 int six = ix - nwxc;
22004 for (int iy = 1+ywobble; iy < nwy-ywobble-1; ++iy) {
22005 int siy = iy - nwyc;
22006 yshiftlocal[0] = float(iy-nwyc);
22007 yrshiftlocal[0] = float(iy-nwyc);
22008 for (int iphi = 0; iphi < nphi; ++iphi) {
22009 float qphi = iphi*delta;
22010 philocal[0] = qphi;
22011 phirlocal[0] = fmod( 180.0f - qphi + ((int)(fabs((180.0f-qphi)/360.0f))+1)*360.0f , 360.0f );
22012
22013
22014 float mxm = -1.0e23f;
22015 float mxr;
22016 for (int iux = max(1, ix - range); iux < min(nwx - 1, ix+range+1); ++iux) {
22017 float qcf = ccfs[0][iphi]->get_value_at(iux,iy);
22018 if (qcf > mxm) {
22019 mxm = qcf;
22020 xshiftlocal[0] = float(iux-nwxc);
22021 }
22022 }
22023 if (! Dsym) {
22024 mxr = -1.0e23f;
22025 for (int iux = max(1, ix - range); iux < min(nwx - 1, ix+range+1); ++iux) {
22026 float qcf = ccfr[0][iphi]->get_value_at(iux,iy);
22027 if (qcf > mxr) {
22028 mxr = qcf;
22029 xrshiftlocal[0] = float(iux-nwxc);
22030 }
22031 }
22032 }
22033 for ( int im = 1; im < ndata; ++im) {
22034
22035 vector<float> cim = data[im]->get_attr("ptcl_source_coord");
22036 float dst = sqrt( (c0[0] - cim[0])*(c0[0] - cim[0]) + (c0[1] - cim[1])*(c0[1] - cim[1]));
22037
22038 qphi = idir*(dst/rise)*dphi;
22039 float pphi = fmod(philocal[0] + qphi + ((int)(abs(qphi/360.0f))+1)*360.0f , 360.0f);
22040 int pix = six;
22041 int piy = siy;
22042 int xix = pix + nwxc;
22043 int yiy = piy + nwyc;
22044
22045 int fix = int(xix);
22046 float xdif = xix - fix;
22047 float xrem = 1.0f - xdif;
22048 int fiy = int(yiy);
22049 float ydif = yiy - fiy;
22050 float yrem = 1.0f - ydif;
22051 float ciq = -1.0e23f;
22052
22053 qphi = pphi/delta;
22054 int ttphi = (int( qphi + ((int)(abs(qphi/nphi))+1)*nphi+ 0.5))%nphi;
22055 for (int lphi = -phiwobble; lphi < phiwobble+1; ++lphi) {
22056 int tphi = (ttphi+lphi+nphi)%nphi;
22057 for (int iux = max(1, fix - range); iux < min(nwx - 1, fix+range+1); ++iux) {
22058 for (int iuy = max(1, fiy - ywobble); iuy < min(nwy - 1, fiy+ywobble+1); ++iuy) {
22059 float qcf = xrem*yrem*ccfs[im][tphi]->get_value_at(iux,iuy)
22060 + xdif*yrem*ccfs[im][tphi]->get_value_at(iux+1,iuy)
22061 + xrem*ydif*ccfs[im][tphi]->get_value_at(iux,iuy+1)
22062 + xdif*ydif*ccfs[im][tphi]->get_value_at(iux+1,iuy+1);
22063 if (qcf > ciq) {
22064 ciq = qcf;
22065 xshiftlocal[im] = iux + xdif - nwxc;
22066 yshiftlocal[im] = iuy + ydif - nwyc;
22067 philocal[im] = tphi * delta;
22068 }
22069 }
22070 }
22071 }
22072 mxm += ciq;
22073
22074 if (! Dsym) {
22075 qphi = idir*(dst/rise)*dphi;
22076 pphi = fmod(phirlocal[0] + qphi + ((int)(abs(qphi/360.0f))+1)*360.0f, 360.0f);
22077 pix = six;
22078 piy = siy;
22079 xix = pix + nwxc;
22080 yiy = piy + nwyc;
22081 fix = int(xix);
22082 xdif = xix - fix;
22083 xrem = 1.0f - xdif;
22084 fiy = int(yiy);
22085 ydif = yiy - fiy;
22086 yrem = 1.0f - ydif;
22087 ciq = -1.0e23f;
22088
22089 for (int lphi = -phiwobble; lphi < phiwobble+1; ++lphi) {
22090 qphi = lphi*delta;
22091 float qtphi = fmod( pphi + qphi + (int(fabs(qphi/360.0f))+1)*360.0f , 360.0f);
22092 qphi = fmod(540.0f-qtphi, 360.0f) / delta;
22093 int tphi = (int( qphi + (int(fabs(qphi/nphi))+1)*nphi + 0.5))%nphi;
22094 for (int iux = max(1, fix - range); iux < min(nwx - 1, fix+range+1); ++iux) {
22095 for (int iuy = max(1, fiy - ywobble); iuy < min(nwy - 1, fiy+ywobble+1); ++iuy) {
22096 float qcf = xrem*yrem*ccfr[im][tphi]->get_value_at(iux,iuy)
22097 + xdif*yrem*ccfr[im][tphi]->get_value_at(iux+1,iuy)
22098 + xrem*ydif*ccfr[im][tphi]->get_value_at(iux,iuy+1)
22099 + xdif*ydif*ccfr[im][tphi]->get_value_at(iux+1,iuy+1);
22100 if (qcf > ciq) {
22101 ciq = qcf;
22102 xrshiftlocal[im] = iux + xdif - nwxc;
22103 yrshiftlocal[im] = iuy + ydif - nwyc;
22104 phirlocal[im] = int(qtphi/delta+0.5f)*delta;
22105 }
22106 }
22107 }
22108 }
22109 mxr += ciq;
22110 } else {
22111 mxr = mxm-1.e5;
22112 }
22113 }
22114 if ( mxr > mxm ) {
22115 if (mxr > tmax) {
22116 tmax = mxr;
22117 mpsi = 270.0f;
22118 for (int im = 0; im < ndata; ++im) mxshiftlocal[im] = xrshiftlocal[im];
22119 for (int im = 0; im < ndata; ++im) myshiftlocal[im] = yrshiftlocal[im];
22120 for (int im = 0; im < ndata; ++im) mphilocal[im] = fmod(540.0f-phirlocal[im], 360.0f);
22121 }
22122 } else {
22123 if (mxm > tmax) {
22124 tmax = mxm;
22125 mpsi = 90.0f;
22126 for (int im = 0; im < ndata; ++im) mxshiftlocal[im] = xshiftlocal[im];
22127 for (int im = 0; im < ndata; ++im) myshiftlocal[im] = yshiftlocal[im];
22128 for (int im = 0; im < ndata; ++im) mphilocal[im] = philocal[im];
22129 }
22130 }
22131 }
22132 }
22133 }
22134 if (tmax > dirma) {
22135 dirma = tmax;
22136 dpsi = mpsi;
22137 for (int im = 0; im < ndata; ++im) dxshiftlocal[im] = mxshiftlocal[im];
22138 for (int im = 0; im < ndata; ++im) dyshiftlocal[im] = myshiftlocal[im];
22139 for (int im = 0; im < ndata; ++im) dphilocal[im] = mphilocal[im];
22140 }
22141 }
22142
22143 for (int im = 0; im < ndata; ++im) {
22144 float psx = dxshiftlocal[im];
22145 float psy = dyshiftlocal[im];
22146 float pphi = dphilocal[im];
22147 float epsi;
22148 float bestang;
22149 if (FindPsi) {
22150 float qphi = pphi/delta;
22151 int iphi = ( int(qphi + ((int)(abs(qphi/nphi))+1)*nphi + 0.5f))%nphi ;
22152
22153 Dict params = Util::get_transform_params(data[im], "xform.projection", "spider");
22154 float opsi3 = params["psi"];
22155 float opx3 = -static_cast<float>(params["tx"]);
22156 float opy3 = -static_cast<float>(params["ty"]);
22157
22158 float gamma = (abs(opsi3 - 90) < abs(opsi3 - 270)) ? (90) : (270);
22159 float oalpha, osx, osy;
22160 compose_transform2(0, opx3, opy3, gamma-opsi3, 0, 0, oalpha, osx, osy);
22161
22162
22163
22164
22165 float junk, nnsx, nnsy;
22166 compose_transform2(0, psx, psy, -oalpha, 0, 0, junk, nnsx, nnsy);
22167
22168
22169 std::auto_ptr<EMData> cimage( Util::Polar2Dm(data[im], cnx+nnsx, cny+nnsy, numr, mode) );
22170 Util::Frngs(cimage.get(), numr);
22171 std::auto_ptr<EMData> temp( Util::Crosrng_msg_s( cimage.get(), crefim[iphi], numr) );
22172
22173 int ipr = int(psi_max*maxrin/360.0f + 0.5f);
22174 int incpsi = (dpsi == 270.0f) ? (maxrin/2) : (0);
22175 float qn = -1.0e23f;
22176 for (int ips = -ipr; ips < ipr+1; ++ips) {
22177 int tot = (ips + incpsi + maxrin)%maxrin;
22178 float tval = temp->get_value_at(tot);
22179 if (tval > qn) {
22180 qn = tval;
22181 bestang = ang_n(tot+1.0f, mode, maxrin);
22182 }
22183 }
22184
22185 bestang = fmod(bestang - (dpsi-90.0f) + 720.0f, 360.0f);
22186
22187
22188
22189
22190
22191
22192
22193 epsi = fmod(bestang+dpsi, 360.0f);
22194 psx = nnsx;
22195 psy = nnsy;
22196
22197 } else {
22198 epsi = dpsi;
22199 bestang = 0;
22200 }
22201 data[im]->set_attr("bestang",fmod(720.0f-bestang, 360.0f));
22202
22203 Dict t_params;
22204 t_params["type"] = "spider";
22205 t_params["phi"] = pphi;
22206 t_params["theta"] = 90.0f;
22207 t_params["psi"] = epsi;
22208 t_params["tx"] = -psx;
22209 t_params["ty"] = -psy;
22210 Transform t(t_params);
22211 data[im]->set_attr("xform.projection", &t);
22212 }
22213 }
22214
22215 void Util::constrained_helix_test( vector<EMData*> data, vector<EMData*> fdata, vector<EMData*> refproj, vector<EMData*> rotproj
22216 , vector<float> dp_dphi_rise_delta, vector<int> nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc
22217 , bool FindPsi, float psi_max, vector<EMData*> crefim, vector<int> numr, int maxrin, string mode, int cnx, int cny)
22218 {
22219 if (dp_dphi_rise_delta.size() < 4) {
22220 printf("Not enough parameters (dp_dphi_rise_delta)");
22221 return;
22222 }
22223 if (nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc.size() < 9) {
22224 printf("Not enough parameters (nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc)");
22225 return;
22226 }
22227 float dpsi;
22228
22229 float dphi = dp_dphi_rise_delta[1];
22230 int rise = static_cast<int>(dp_dphi_rise_delta[2] + 0.2);
22231 float delta = dp_dphi_rise_delta[3];
22232 int nphi = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[0];
22233 int phiwobble = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[1];
22234 int range = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[2];
22235 int ywobble = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[3];
22236 bool Dsym = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[4];
22237 int nwx = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[5];
22238 int nwy = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[6];
22239 int nwxc = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[7];
22240 int nwyc = nphi_phiwobble_range_ywobble_Dsym_nwx_nwy_nwxc_nwyc[8];
22241
22242 const int ndata = data.size();
22243
22244 std::vector< boost::shared_ptr<EMData> > objectsToDelete;
22245
22246 vector<float> c0 = data[0]->get_attr("ptcl_source_coord");
22247 vector< vector<EMData*> > ccfs(ndata, vector<EMData*>(nphi));
22248 vector< vector<EMData*> > ccfr(0);
22249 if (! Dsym) {
22250 ccfr.resize(ndata, vector<EMData*>(nphi));
22251 }
22252 for (int im = 0; im < ndata; ++im) {
22253 for (int iphi = 0; iphi < nphi; ++iphi) {
22254 std::auto_ptr<EMData> corr( correlation( refproj[iphi], fdata[im], CIRCULANT, true) );
22255 ccfs[im][iphi] = Util::window( corr.get(), nwx, nwy);
22256 objectsToDelete.push_back(boost::shared_ptr<EMData>(ccfs[im][iphi]));
22257 if (! Dsym) {
22258 std::auto_ptr<EMData> corr2( correlation( rotproj[iphi], fdata[im], CIRCULANT, true) );
22259 ccfr[im][iphi] = Util::window( corr2.get(), nwx, nwy);
22260 objectsToDelete.push_back(boost::shared_ptr<EMData>(ccfr[im][iphi]));
22261 }
22262 }
22263 }
22264 vector<float> dxshiftlocal(ndata, 0);
22265 vector<float> dyshiftlocal(ndata, 0);
22266 vector<float> dphilocal(ndata, 0);
22267
22268 vector<float> xshiftlocal(ndata, 0);
22269 vector<float> xrshiftlocal(ndata, 0);
22270 vector<float> mxshiftlocal(ndata, 0);
22271 vector<float> yshiftlocal(ndata, 0);
22272 vector<float> yrshiftlocal(ndata, 0);
22273 vector<float> myshiftlocal(ndata, 0);
22274 vector<float> philocal(ndata, 0);
22275 vector<float> phirlocal(ndata, 0);
22276 vector<float> mphilocal(ndata, 0);
22277 float dirma = -1.0e23f;
22278 for (int yidir = -1; yidir < 2; yidir += 2) {
22279 for (int idir = -1; idir < 2; idir += 2) {
22280 float tmax = -1.0e23f;
22281 float mpsi;
22282 for (int ix = 1; ix < nwx-1; ++ix) {
22283
22284 int six = ix - nwxc;
22285 for (int iy = 1+ywobble; iy < nwy-ywobble-1; ++iy) {
22286 int siy = iy - nwyc;
22287 yshiftlocal[0] = float(iy-nwyc);
22288 yrshiftlocal[0] = float(iy-nwyc);
22289 for (int iphi = 0; iphi < nphi; ++iphi) {
22290 float qphi = iphi*delta;
22291 philocal[0] = qphi;
22292 phirlocal[0] = fmod( 180.0f - qphi + ((int)(fabs((180.0f-qphi)/360.0f))+1)*360.0f , 360.0f );
22293
22294
22295 float mxm = -1.0e23f;
22296 float mxr;
22297 for (int iux = max(1, ix - range); iux < min(nwx - 1, ix+range+1); ++iux) {
22298 float qcf = ccfs[0][iphi]->get_value_at(iux,iy);
22299 if (qcf > mxm) {
22300 mxm = qcf;
22301 xshiftlocal[0] = float(iux-nwxc);
22302 }
22303 }
22304 if (! Dsym) {
22305 mxr = -1.0e23f;
22306 for (int iux = max(1, ix - range); iux < min(nwx - 1, ix+range+1); ++iux) {
22307 float qcf = ccfr[0][iphi]->get_value_at(iux,iy);
22308 if (qcf > mxr) {
22309 mxr = qcf;
22310 xrshiftlocal[0] = float(iux-nwxc);
22311 }
22312 }
22313 }
22314 for ( int im = 1; im < ndata; ++im) {
22315
22316 vector<float> cim = data[im]->get_attr("ptcl_source_coord");
22317 float dst = sqrt( (c0[0] - cim[0])*(c0[0] - cim[0]) + (c0[1] - cim[1])*(c0[1] - cim[1]));
22318
22319 qphi = idir*(dst/rise)*dphi;
22320
22321 int pix = six;
22322
22323 Dict pret = predict(philocal[0], siy, dst, idir, yidir, rise, dphi, true);
22324 float piy = pret["predy"];
22325 float pphi = pret["predphi"];
22326 int xix = pix + nwxc;
22327 float yiy = piy + nwyc;
22328
22329 int fix = int(xix);
22330 float xdif = xix - fix;
22331 float xrem = 1.0f - xdif;
22332 int fiy = int(yiy);
22333 float ydif = yiy - fiy;
22334 float yrem = 1.0f - ydif;
22335 float ciq = -1.0e23f;
22336
22337 qphi = pphi/delta;
22338 int ttphi = (int( qphi + ((int)(abs(qphi/nphi))+1)*nphi+ 0.5))%nphi;
22339 for (int lphi = -phiwobble; lphi < phiwobble+1; ++lphi) {
22340 int tphi = (ttphi+lphi+nphi)%nphi;
22341 for (int iux = max(1, fix - range); iux < min(nwx - 1, fix+range+1); ++iux) {
22342 for (int iuy = max(1, fiy - ywobble); iuy < min(nwy - 1, fiy+ywobble+1); ++iuy) {
22343 float qcf = xrem*yrem*ccfs[im][tphi]->get_value_at(iux,iuy)
22344 + xdif*yrem*ccfs[im][tphi]->get_value_at(iux+1,iuy)
22345 + xrem*ydif*ccfs[im][tphi]->get_value_at(iux,iuy+1)
22346 + xdif*ydif*ccfs[im][tphi]->get_value_at(iux+1,iuy+1);
22347 if (qcf > ciq) {
22348 ciq = qcf;
22349 xshiftlocal[im] = iux + xdif - nwxc;
22350 yshiftlocal[im] = iuy + ydif - nwyc;
22351 philocal[im] = tphi * delta;
22352 }
22353 }
22354 }
22355 }
22356 mxm += ciq;
22357
22358 if (! Dsym) {
22359 qphi = idir*(dst/rise)*dphi;
22360
22361 pix = six;
22362
22363 Dict pret = predict(phirlocal[0], siy, dst, idir, yidir, rise, dphi, true);
22364 piy = pret["predy"];
22365 pphi = pret["predphi"];
22366 xix = pix + nwxc;
22367 yiy = piy + nwyc;
22368 fix = int(xix);
22369 xdif = xix - fix;
22370 xrem = 1.0f - xdif;
22371 fiy = int(yiy);
22372 ydif = yiy - fiy;
22373 yrem = 1.0f - ydif;
22374 ciq = -1.0e23f;
22375
22376 for (int lphi = -phiwobble; lphi < phiwobble+1; ++lphi) {
22377 qphi = lphi*delta;
22378 float qtphi = fmod( pphi + qphi + (int(fabs(qphi/360.0f))+1)*360.0f , 360.0f);
22379 qphi = fmod(540.0f-qtphi, 360.0f) / delta;
22380 int tphi = (int( qphi + (int(fabs(qphi/nphi))+1)*nphi + 0.5))%nphi;
22381 for (int iux = max(1, fix - range); iux < min(nwx - 1, fix+range+1); ++iux) {
22382 for (int iuy = max(1, fiy - ywobble); iuy < min(nwy - 1, fiy+ywobble+1); ++iuy) {
22383 float qcf = xrem*yrem*ccfr[im][tphi]->get_value_at(iux,iuy)
22384 + xdif*yrem*ccfr[im][tphi]->get_value_at(iux+1,iuy)
22385 + xrem*ydif*ccfr[im][tphi]->get_value_at(iux,iuy+1)
22386 + xdif*ydif*ccfr[im][tphi]->get_value_at(iux+1,iuy+1);
22387 if (qcf > ciq) {
22388 ciq = qcf;
22389 xrshiftlocal[im] = iux + xdif - nwxc;
22390 yrshiftlocal[im] = iuy + ydif - nwyc;
22391 phirlocal[im] = int(qtphi/delta+0.5f)*delta;
22392 }
22393 }
22394 }
22395 }
22396 mxr += ciq;
22397 } else {
22398 mxr = mxm-1.e5;
22399 }
22400 }
22401 if ( mxr > mxm ) {
22402 if (mxr > tmax) {
22403 tmax = mxr;
22404 mpsi = 270.0f;
22405 for (int im = 0; im < ndata; ++im) mxshiftlocal[im] = xrshiftlocal[im];
22406 for (int im = 0; im < ndata; ++im) myshiftlocal[im] = yrshiftlocal[im];
22407 for (int im = 0; im < ndata; ++im) mphilocal[im] = fmod(540.0f-phirlocal[im], 360.0f);
22408 }
22409 } else {
22410 if (mxm > tmax) {
22411 tmax = mxm;
22412 mpsi = 90.0f;
22413 for (int im = 0; im < ndata; ++im) mxshiftlocal[im] = xshiftlocal[im];
22414 for (int im = 0; im < ndata; ++im) myshiftlocal[im] = yshiftlocal[im];
22415 for (int im = 0; im < ndata; ++im) mphilocal[im] = philocal[im];
22416 }
22417 }
22418 }
22419 }
22420 }
22421 if (tmax > dirma) {
22422 dirma = tmax;
22423 dpsi = mpsi;
22424 for (int im = 0; im < ndata; ++im) dxshiftlocal[im] = mxshiftlocal[im];
22425 for (int im = 0; im < ndata; ++im) dyshiftlocal[im] = myshiftlocal[im];
22426 for (int im = 0; im < ndata; ++im) dphilocal[im] = mphilocal[im];
22427 }
22428 }
22429 }
22430
22431 for (int im = 0; im < ndata; ++im) {
22432 float psx = dxshiftlocal[im];
22433 float psy = dyshiftlocal[im];
22434 float pphi = dphilocal[im];
22435 float epsi;
22436 float bestang;
22437 if (FindPsi) {
22438 float qphi = pphi/delta;
22439 int iphi = ( int(qphi + ((int)(abs(qphi/nphi))+1)*nphi + 0.5f))%nphi ;
22440
22441 Dict params = Util::get_transform_params(data[im], "xform.projection", "spider");
22442 float opsi3 = params["psi"];
22443 float opx3 = -static_cast<float>(params["tx"]);
22444 float opy3 = -static_cast<float>(params["ty"]);
22445
22446 float gamma = (abs(opsi3 - 90) < abs(opsi3 - 270)) ? (90) : (270);
22447 float oalpha, osx, osy;
22448 compose_transform2(0, opx3, opy3, gamma-opsi3, 0, 0, oalpha, osx, osy);
22449
22450
22451
22452
22453 float junk, nnsx, nnsy;
22454 compose_transform2(0, psx, psy, -oalpha, 0, 0, junk, nnsx, nnsy);
22455
22456
22457 std::auto_ptr<EMData> cimage( Util::Polar2Dm(data[im], cnx+nnsx, cny+nnsy, numr, mode) );
22458 Util::Frngs(cimage.get(), numr);
22459 std::auto_ptr<EMData> temp( Util::Crosrng_msg_s( cimage.get(), crefim[iphi], numr) );
22460
22461 int ipr = int(psi_max*maxrin/360.0f + 0.5f);
22462 int incpsi = (dpsi == 270.0f) ? (maxrin/2) : (0);
22463 float qn = -1.0e23f;
22464 for (int ips = -ipr; ips < ipr+1; ++ips) {
22465 int tot = (ips + incpsi + maxrin)%maxrin;
22466 float tval = temp->get_value_at(tot);
22467 if (tval > qn) {
22468 qn = tval;
22469 bestang = ang_n(tot+1.0f, mode, maxrin);
22470 }
22471 }
22472
22473 bestang = fmod(bestang - (dpsi-90.0f) + 720.0f, 360.0f);
22474
22475
22476
22477
22478
22479
22480
22481 epsi = fmod(bestang+dpsi, 360.0f);
22482 psx = nnsx;
22483 psy = nnsy;
22484
22485 } else {
22486 epsi = dpsi;
22487 bestang = 0;
22488 }
22489 data[im]->set_attr("bestang",fmod(720.0f-bestang, 360.0f));
22490
22491 Dict t_params;
22492 t_params["type"] = "spider";
22493 t_params["phi"] = pphi;
22494 t_params["theta"] = 90.0f;
22495 t_params["psi"] = epsi;
22496 t_params["tx"] = -psx;
22497 t_params["ty"] = -psy;
22498 Transform t(t_params);
22499 data[im]->set_attr("xform.projection", &t);
22500 }
22501 }
22502
22503 Dict Util::predict(float phig, float yg, float dst, float sgn, float ysgn, float dpp, float dphi, bool backpred){
22504
22505 float back = 1.0;
22506 if (!backpred)
22507 back=-1.0;
22508 float predphi;
22509 float predy;
22510 if (fmod(dst, dpp) <= 0.5*dpp) {
22511 predphi = fmod( phig + back*sgn * floor(dst/dpp)* dphi, float(360.0));
22512 predy = yg + back*ysgn*(fmod(dst, dpp));
22513 if (predy > 0)
22514 if (fmod(predy,dpp) > 0.5*dpp){
22515 predy = predy - dpp;
22516 predphi = fmod( (predphi + sgn*dphi),float(360.0));
22517 }
22518 else{
22519 if (fmod(abs(predy), dpp) > 0.5*dpp) {
22520 predy = predy + dpp;
22521 predphi = fmod( (predphi - sgn*dphi), float(360.0));
22522 }
22523 }
22524
22525 }
22526 else{
22527 predphi = fmod ( (phig + back*sgn * (floor(dst/dpp)* dphi + dphi)), float(360.0));
22528 predy = yg + back*ysgn*(fmod(dst,dpp) - dpp) ;
22529 if (predy > 0){
22530 if (fmod(predy, dpp) > 0.5*dpp){
22531 predy = predy - dpp;
22532 predphi = fmod( (predphi + sgn*dphi),float(360.0));
22533 }
22534 }
22535 else{
22536 if (fmod(abs(predy), dpp) > 0.5*dpp){
22537 predy = predy + dpp;
22538 predphi = fmod( (predphi - sgn*dphi), float(360.0));
22539 }
22540 }
22541
22542 }
22543 Dict retvals;
22544 retvals["predy"] = predy;
22545 retvals["predphi"] = predphi;
22546 return retvals;
22547 }