00001
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifdef _WIN32
00037 #pragma warning(disable:4819)
00038 #endif //_WIN32
00039
00040 #include <cstring>
00041 #include <ctime>
00042 #include <iostream>
00043 #include <cstdio>
00044 #include <cstdlib>
00045 #include <boost/format.hpp>
00046 #include "emdata.h"
00047 #include "util.h"
00048 #include "fundamentals.h"
00049 #include "lapackblas.h"
00050 #include "lbfgsb.h"
00051 using namespace EMAN;
00052 #include "steepest.h"
00053 #include "emassert.h"
00054 #include "randnum.h"
00055
00056 #include <gsl/gsl_sf_bessel.h>
00057 #include <gsl/gsl_sf_bessel.h>
00058 #include <cmath>
00059 using namespace std;
00060 using std::complex;
00061
00062
00063 int circum_(double *, double *, double *, double *, int *);
00064 long int left_(double *, double *, double *, double *, double *, double *, double *, double *, double *);
00065 int addnod_(int *, int *, double *, double *, double *, int *, int *, int *, int *, int *);
00066
00067 vector<float> Util::infomask(EMData* Vol, EMData* mask, bool flip = false)
00068
00069
00070 {
00071 ENTERFUNC;
00072 vector<float> stats;
00073 float *Volptr, *maskptr,MAX,MIN;
00074 long double Sum1,Sum2;
00075 long count;
00076
00077 MAX = -FLT_MAX;
00078 MIN = FLT_MAX;
00079 count = 0L;
00080 Sum1 = 0.0L;
00081 Sum2 = 0.0L;
00082
00083 if (mask == NULL) {
00084
00085 stats.push_back(Vol->get_attr("mean"));
00086 stats.push_back(Vol->get_attr("sigma"));
00087 stats.push_back(Vol->get_attr("minimum"));
00088 stats.push_back(Vol->get_attr("maximum"));
00089 return stats;
00090 }
00091
00092
00093
00094 size_t nx = Vol->get_xsize();
00095 size_t ny = Vol->get_ysize();
00096 size_t nz = Vol->get_zsize();
00097
00098 size_t mask_nx = mask->get_xsize();
00099 size_t mask_ny = mask->get_ysize();
00100 size_t mask_nz = mask->get_zsize();
00101
00102 if (nx != mask_nx || ny != mask_ny || nz != mask_nz )
00103 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116 Volptr = Vol->get_data();
00117 maskptr = mask->get_data();
00118
00119 for (size_t i = 0; i < (size_t)nx*ny*nz; ++i) {
00120 if ((maskptr[i]>0.5f) == flip) {
00121 Sum1 += Volptr[i];
00122 Sum2 += Volptr[i]*double(Volptr[i]);
00123 MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00124 MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00125 count++;
00126 }
00127 }
00128
00129 if (count == 0) {
00130 LOGERR("Invalid mask");
00131 throw ImageFormatException( "Invalid mask");
00132 }
00133
00134 float avg = static_cast<float>(Sum1/count);
00135 float sig = static_cast<float>(sqrt((Sum2 - Sum1*Sum1/count)/(count-1)));
00136
00137 stats.push_back(avg);
00138 stats.push_back(sig);
00139 stats.push_back(MIN);
00140 stats.push_back(MAX);
00141
00142 return stats;
00143 }
00144
00145
00146
00147
00148 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00149 {
00150 ENTERFUNC;
00151
00152 if (!EMUtil::is_same_size(V1, V2)) {
00153 LOGERR("images not same size");
00154 throw ImageFormatException( "images not same size");
00155 }
00156
00157 size_t nx = V1->get_xsize();
00158 size_t ny = V1->get_ysize();
00159 size_t nz = V1->get_zsize();
00160 size_t size = (size_t)nx*ny*nz;
00161
00162 EMData *BD = new EMData();
00163 BD->set_size(nx, ny, nz);
00164
00165 float *params = new float[2];
00166
00167 float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00168 long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00169 int nvox = 0L;
00170
00171 V1ptr = V1->get_data();
00172 V2ptr = V2->get_data();
00173 BDptr = BD->get_data();
00174
00175
00176 if(!mask){
00177 EMData * Mask = new EMData();
00178 Mask->set_size(nx,ny,nz);
00179 Mask->to_one();
00180 MASKptr = Mask->get_data();
00181 } else {
00182 if (!EMUtil::is_same_size(V1, mask)) {
00183 LOGERR("mask not same size");
00184 throw ImageFormatException( "mask not same size");
00185 }
00186
00187 MASKptr = mask->get_data();
00188 }
00189
00190
00191
00192
00193
00194 for (size_t i = 0L;i < size; i++) {
00195 if (MASKptr[i]>0.5f) {
00196 S1 += V1ptr[i]*V2ptr[i];
00197 S2 += V1ptr[i]*V1ptr[i];
00198 S3 += V2ptr[i];
00199 S4 += V1ptr[i];
00200 nvox ++;
00201 }
00202 }
00203
00204 if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00205 A =1.0f ;
00206 } else {
00207 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00208 }
00209 B = static_cast<float> (A*S4 - S3)/nvox;
00210
00211
00212
00213 for (size_t i = 0L;i < size; i++) {
00214 if (MASKptr[i]>0.5f) {
00215 BDptr[i] = A*V1ptr[i] - B - V2ptr[i];
00216 } else {
00217 BDptr[i] = 0.f;
00218 }
00219 }
00220
00221 BD->update();
00222
00223 params[0] = A;
00224 params[1] = B;
00225
00226 Dict BDnParams;
00227 BDnParams["imdiff"] = BD;
00228 BDnParams["A"] = params[0];
00229 BDnParams["B"] = params[1];
00230
00231 EXITFUNC;
00232 return BDnParams;
00233 }
00234
00235
00236
00237
00238
00239 EMData *Util::TwoDTestFunc(int Size, float p, float q, float a, float b, int flag, float alphaDeg)
00240 {
00241 ENTERFUNC;
00242 int Mid= (Size+1)/2;
00243
00244 if (flag==0) {
00245 EMData* ImBW = new EMData();
00246 ImBW->set_size(Size,Size,1);
00247 ImBW->to_zero();
00248
00249 float tempIm;
00250 float x,y;
00251
00252 for (int ix=(1-Mid); ix<Mid; ix++){
00253 for (int iy=(1-Mid); iy<Mid; iy++){
00254 x = (float)ix;
00255 y = (float)iy;
00256 tempIm= static_cast<float>( (1/(2*M_PI)) * cos(p*x)* cos(q*y) * exp(-.5*x*x/(a*a))* exp(-.5*y*y/(b*b)) );
00257 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00258 }
00259 }
00260 ImBW->update();
00261 ImBW->set_complex(false);
00262 ImBW->set_ri(true);
00263
00264 return ImBW;
00265 }
00266 else if (flag==1) {
00267 EMData* ImBWFFT = new EMData();
00268 ImBWFFT ->set_size(2*Size,Size,1);
00269 ImBWFFT ->to_zero();
00270
00271 float r,s;
00272
00273 for (int ir=(1-Mid); ir<Mid; ir++){
00274 for (int is=(1-Mid); is<Mid; is++){
00275 r = (float)ir;
00276 s = (float)is;
00277 (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00278 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00279 }
00280 }
00281 ImBWFFT->update();
00282 ImBWFFT->set_complex(true);
00283 ImBWFFT->set_ri(true);
00284 ImBWFFT->set_shuffled(true);
00285 ImBWFFT->set_fftodd(true);
00286
00287 return ImBWFFT;
00288 }
00289 else if (flag==2 || flag==3) {
00290 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00291 float C=cos(alpha);
00292 float S=sin(alpha);
00293 float D= sqrt(S*S*b*b + C*C*a*a);
00294
00295
00296 float P = p * C *a*a/D ;
00297 float Q = q * S *b*b/D ;
00298
00299 if (flag==2) {
00300 EMData* pofalpha = new EMData();
00301 pofalpha ->set_size(Size,1,1);
00302 pofalpha ->to_zero();
00303
00304 float Norm0 = D*(float)sqrt(2*pi);
00305 float Norm1 = exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00306 float Norm2 = exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00307 float sD;
00308
00309 for (int is=(1-Mid); is<Mid; is++){
00310 sD = is/D ;
00311 (*pofalpha)(is+Mid-1) = Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00312 + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00313 }
00314 pofalpha-> update();
00315 pofalpha-> set_complex(false);
00316 pofalpha-> set_ri(true);
00317
00318 return pofalpha;
00319 }
00320 if (flag==3) {
00321 float vD;
00322
00323 EMData* pofalphak = new EMData();
00324 pofalphak ->set_size(2*Size,1,1);
00325 pofalphak ->to_zero();
00326
00327 for (int iv=(1-Mid); iv<Mid; iv++){
00328 vD = iv*D ;
00329 (*pofalphak)(2*(iv+Mid-1)) = exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00330 }
00331 pofalphak-> update();
00332 pofalphak-> set_complex(false);
00333 pofalphak-> set_ri(true);
00334
00335 return pofalphak;
00336 }
00337 }
00338 else if (flag==4) {
00339 cout <<" FH under construction";
00340 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00341 EMData* TryFH= OutFT -> real2FH(4.0);
00342 return TryFH;
00343 } else {
00344 cout <<" flag must be 0,1,2,3, or 4";
00345 }
00346
00347 EXITFUNC;
00348 return 0;
00349 }
00350
00351
00352 void Util::spline_mat(float *x, float *y, int n, float *xq, float *yq, int m)
00353 {
00354
00355 float x0= x[0];
00356 float x1= x[1];
00357 float x2= x[2];
00358 float y0= y[0];
00359 float y1= y[1];
00360 float y2= y[2];
00361 float yp1 = (y1-y0)/(x1-x0) + (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1) ;
00362 float xn = x[n];
00363 float xnm1= x[n-1];
00364 float xnm2= x[n-2];
00365 float yn = y[n];
00366 float ynm1= y[n-1];
00367 float ynm2= y[n-2];
00368 float ypn= (yn-ynm1)/(xn-xnm1) + (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00369 float *y2d = new float[n];
00370 Util::spline(x,y,n,yp1,ypn,y2d);
00371 Util::splint(x,y,y2d,n,xq,yq,m);
00372 delete [] y2d;
00373 return;
00374 }
00375
00376
00377 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2)
00378 {
00379 int i,k;
00380 float p, qn, sig, un, *u;
00381 u = new float[n-1];
00382
00383 if (yp1 > .99e30){
00384 y2[0]=u[0]=0.0;
00385 } else {
00386 y2[0]=-.5f;
00387 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00388 }
00389
00390 for (i=1; i < n-1; i++) {
00391 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00392 p = sig*y2[i-1] + 2.0f;
00393 y2[i] = (sig-1.0f)/p;
00394 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) - (y[i] - y[i-1] )/(x[i] -x[i-1]);
00395 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00396 }
00397
00398 if (ypn>.99e30){
00399 qn=0; un=0;
00400 } else {
00401 qn= .5f;
00402 un= (3.0f/(x[n-1] -x[n-2])) * (ypn - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00403 }
00404 y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00405 for (k=n-2; k>=0; k--){
00406 y2[k]=y2[k]*y2[k+1]+u[k];
00407 }
00408 delete [] u;
00409 }
00410
00411
00412 void Util::splint( float *xa, float *ya, float *y2a, int n, float *xq, float *yq, int m)
00413 {
00414 int klo, khi, k;
00415 float h, b, a;
00416
00417
00418 for (int j=0; j<m;j++){
00419 klo=0;
00420 khi=n-1;
00421 while (khi-klo >1) {
00422 k=(khi+klo) >>1;
00423 if (xa[k]>xq[j]){ khi=k;}
00424 else { klo=k;}
00425 }
00426 h=xa[khi]- xa[klo];
00427 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00428 a =(xa[khi]-xq[j])/h;
00429 b=(xq[j]-xa[klo])/h;
00430 yq[j]=a*ya[klo] + b*ya[khi]
00431 + ((a*a*a-a)*y2a[klo]
00432 +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00433 }
00434
00435 }
00436
00437
00438 void Util::Radialize(int *PermMatTr, float *kValsSorted,
00439 float *weightofkValsSorted, int Size, int *SizeReturned)
00440 {
00441 int iMax = (int) floor( (Size-1.0)/2 +.01);
00442 int CountMax = (iMax+2)*(iMax+1)/2;
00443 int Count=-1;
00444 float *kVals = new float[CountMax];
00445 float *weightMat = new float[CountMax];
00446 int *PermMat = new int[CountMax];
00447 SizeReturned[0] = CountMax;
00448
00449
00450 for (int jkx=0; jkx< iMax+1; jkx++) {
00451 for (int jky=0; jky< jkx+1; jky++) {
00452 Count++;
00453 kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00454 weightMat[Count]= 1.0;
00455 if (jkx!=0) { weightMat[Count] *=2;}
00456 if (jky!=0) { weightMat[Count] *=2;}
00457 if (jkx!=jky){ weightMat[Count] *=2;}
00458 PermMat[Count]=Count+1;
00459 }
00460 }
00461
00462 int lkVals = Count+1;
00463
00464
00465 sort_mat(&kVals[0],&kVals[Count],
00466 &PermMat[0], &PermMat[Count]);
00467
00468 fflush(stdout);
00469
00470 int newInd;
00471
00472 for (int iP=0; iP < lkVals ; iP++ ) {
00473 newInd = PermMat[iP];
00474 PermMatTr[newInd-1] = iP+1;
00475 }
00476
00477
00478
00479 int CountA=-1;
00480 int CountB=-1;
00481
00482 while (CountB< (CountMax-1)) {
00483 CountA++;
00484 CountB++;
00485
00486 kValsSorted[CountA] = kVals[CountB] ;
00487 if (CountB<(CountMax-1) ) {
00488 while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001 ) {
00489 SizeReturned[0]--;
00490 for (int iP=0; iP < lkVals; iP++){
00491
00492 if (PermMatTr[iP]>CountA+1) {
00493 PermMatTr[iP]--;
00494 }
00495 }
00496 CountB++;
00497 }
00498 }
00499 }
00500
00501
00502 for (int CountD=0; CountD < CountMax; CountD++) {
00503 newInd = PermMatTr[CountD];
00504 weightofkValsSorted[newInd-1] += weightMat[CountD];
00505 }
00506
00507 }
00508
00509
00510 vector<float>
00511 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00512 {
00513 vector<float> angles;
00514 float psi = 0.0;
00515 if ((0.0 == t1 && 0.0 == t2)||(t1 >= t2)) {
00516 t1 = 0.0f;
00517 t2 = 90.0f;
00518 }
00519 if ((0.0 == p1 && 0.0 == p2)||(p1 >= p2)) {
00520 p1 = 0.0f;
00521 p2 = 359.9f;
00522 }
00523 bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00524 for (float theta = t1; theta <= t2; theta += delta) {
00525 float detphi;
00526 int lt;
00527 if ((0.0 == theta)||(180.0 == theta)) {
00528 detphi = 360.0f;
00529 lt = 1;
00530 } else {
00531 detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00532 lt = int((p2 - p1)/detphi)-1;
00533 if (lt < 1) lt = 1;
00534 detphi = (p2 - p1)/lt;
00535 }
00536 for (int i = 0; i < lt; i++) {
00537 float phi = p1 + i*detphi;
00538 if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00539 angles.push_back(phi);
00540 angles.push_back(theta);
00541 angles.push_back(psi);
00542 }
00543 }
00544 return angles;
00545 }
00546
00547
00548 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00651 {
00652
00653
00654 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00655 float quadri;
00656 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00657
00658 x = xx;
00659 y = yy;
00660
00661
00662 while ( x < 1.0 ) x += nxdata;
00663 while ( x >= (float)(nxdata+1) ) x -= nxdata;
00664 while ( y < 1.0 ) y += nydata;
00665 while ( y >= (float)(nydata+1) ) y -= nydata;
00666
00667 i = (int) x;
00668 j = (int) y;
00669
00670 dx0 = x - i;
00671 dy0 = y - j;
00672
00673 ip1 = i + 1;
00674 im1 = i - 1;
00675 jp1 = j + 1;
00676 jm1 = j - 1;
00677
00678 if (ip1 > nxdata) ip1 -= nxdata;
00679 if (im1 < 1) im1 += nxdata;
00680 if (jp1 > nydata) jp1 -= nydata;
00681 if (jm1 < 1) jm1 += nydata;
00682
00683 f0 = fdata(i,j);
00684 c1 = fdata(ip1,j) - f0;
00685 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00686 c3 = fdata(i,jp1) - f0;
00687 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00688
00689 dxb = dx0 - 1;
00690 dyb = dy0 - 1;
00691
00692
00693 if (dx0 >= 0) hxc = 1; else hxc = -1;
00694 if (dy0 >= 0) hyc = 1; else hyc = -1;
00695
00696 ic = i + hxc;
00697 jc = j + hyc;
00698
00699 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00700 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00701
00702 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00703 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00704
00705
00706 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00707
00708 return quadri;
00709 }
00710
00711 #undef fdata
00712
00713 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00714 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00715 {
00716
00717
00718 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00719 float quadri;
00720 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00721
00722 x = xx;
00723 y = yy;
00724
00725
00726 if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00727 x = (float)xnew;
00728 y = (float)ynew;
00729 }
00730
00731
00732 i = (int) x;
00733 j = (int) y;
00734
00735 dx0 = x - i;
00736 dy0 = y - j;
00737
00738 ip1 = i + 1;
00739 im1 = i - 1;
00740 jp1 = j + 1;
00741 jm1 = j - 1;
00742
00743 if (ip1 > nxdata) ip1 -= nxdata;
00744 if (im1 < 1) im1 += nxdata;
00745 if (jp1 > nydata) jp1 -= nydata;
00746 if (jm1 < 1) jm1 += nydata;
00747
00748 f0 = fdata(i,j);
00749 c1 = fdata(ip1,j) - f0;
00750 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00751 c3 = fdata(i,jp1) - f0;
00752 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00753
00754 dxb = dx0 - 1;
00755 dyb = dy0 - 1;
00756
00757
00758 if (dx0 >= 0) hxc = 1; else hxc = -1;
00759 if (dy0 >= 0) hyc = 1; else hyc = -1;
00760
00761 ic = i + hxc;
00762 jc = j + hyc;
00763
00764 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00765 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00766
00767 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00768 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00769
00770
00771 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00772
00773 return quadri;
00774 }
00775
00776 #undef fdata
00777
00778
00779 float Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00780 int K = kb.get_window_size();
00781 int kbmin = -K/2;
00782 int kbmax = -kbmin;
00783 int kbc = kbmax+1;
00784
00785 float pixel =0.0f;
00786 float w=0.0f;
00787
00788 delx = restrict1(delx, nx);
00789 int inxold = int(round(delx));
00790 if ( ny < 2 ) {
00791 float tablex1 = kb.i0win_tab(delx-inxold+3);
00792 float tablex2 = kb.i0win_tab(delx-inxold+2);
00793 float tablex3 = kb.i0win_tab(delx-inxold+1);
00794 float tablex4 = kb.i0win_tab(delx-inxold);
00795 float tablex5 = kb.i0win_tab(delx-inxold-1);
00796 float tablex6 = kb.i0win_tab(delx-inxold-2);
00797 float tablex7 = kb.i0win_tab(delx-inxold-3);
00798
00799 int x1, x2, x3, x4, x5, x6, x7;
00800
00801 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
00802 x1 = (inxold-3+nx)%nx;
00803 x2 = (inxold-2+nx)%nx;
00804 x3 = (inxold-1+nx)%nx;
00805 x4 = (inxold +nx)%nx;
00806 x5 = (inxold+1+nx)%nx;
00807 x6 = (inxold+2+nx)%nx;
00808 x7 = (inxold+3+nx)%nx;
00809 } else {
00810 x1 = inxold-3;
00811 x2 = inxold-2;
00812 x3 = inxold-1;
00813 x4 = inxold;
00814 x5 = inxold+1;
00815 x6 = inxold+2;
00816 x7 = inxold+3;
00817 }
00818
00819 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00820 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00821 data[x7]*tablex7 ;
00822
00823 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00824 } else if ( nz < 2 ) {
00825 dely = restrict1(dely, ny);
00826 int inyold = int(round(dely));
00827 float tablex1 = kb.i0win_tab(delx-inxold+3);
00828 float tablex2 = kb.i0win_tab(delx-inxold+2);
00829 float tablex3 = kb.i0win_tab(delx-inxold+1);
00830 float tablex4 = kb.i0win_tab(delx-inxold);
00831 float tablex5 = kb.i0win_tab(delx-inxold-1);
00832 float tablex6 = kb.i0win_tab(delx-inxold-2);
00833 float tablex7 = kb.i0win_tab(delx-inxold-3);
00834
00835 float tabley1 = kb.i0win_tab(dely-inyold+3);
00836 float tabley2 = kb.i0win_tab(dely-inyold+2);
00837 float tabley3 = kb.i0win_tab(dely-inyold+1);
00838 float tabley4 = kb.i0win_tab(dely-inyold);
00839 float tabley5 = kb.i0win_tab(dely-inyold-1);
00840 float tabley6 = kb.i0win_tab(dely-inyold-2);
00841 float tabley7 = kb.i0win_tab(dely-inyold-3);
00842
00843 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00844
00845 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
00846 x1 = (inxold-3+nx)%nx;
00847 x2 = (inxold-2+nx)%nx;
00848 x3 = (inxold-1+nx)%nx;
00849 x4 = (inxold +nx)%nx;
00850 x5 = (inxold+1+nx)%nx;
00851 x6 = (inxold+2+nx)%nx;
00852 x7 = (inxold+3+nx)%nx;
00853
00854 y1 = ((inyold-3+ny)%ny)*nx;
00855 y2 = ((inyold-2+ny)%ny)*nx;
00856 y3 = ((inyold-1+ny)%ny)*nx;
00857 y4 = ((inyold +ny)%ny)*nx;
00858 y5 = ((inyold+1+ny)%ny)*nx;
00859 y6 = ((inyold+2+ny)%ny)*nx;
00860 y7 = ((inyold+3+ny)%ny)*nx;
00861 } else {
00862 x1 = inxold-3;
00863 x2 = inxold-2;
00864 x3 = inxold-1;
00865 x4 = inxold;
00866 x5 = inxold+1;
00867 x6 = inxold+2;
00868 x7 = inxold+3;
00869
00870 y1 = (inyold-3)*nx;
00871 y2 = (inyold-2)*nx;
00872 y3 = (inyold-1)*nx;
00873 y4 = inyold*nx;
00874 y5 = (inyold+1)*nx;
00875 y6 = (inyold+2)*nx;
00876 y7 = (inyold+3)*nx;
00877 }
00878
00879 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00880 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00881 data[x7+y1]*tablex7 ) * tabley1 +
00882 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00883 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00884 data[x7+y2]*tablex7 ) * tabley2 +
00885 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00886 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00887 data[x7+y3]*tablex7 ) * tabley3 +
00888 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00889 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00890 data[x7+y4]*tablex7 ) * tabley4 +
00891 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00892 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00893 data[x7+y5]*tablex7 ) * tabley5 +
00894 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00895 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00896 data[x7+y6]*tablex7 ) * tabley6 +
00897 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00898 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00899 data[x7+y7]*tablex7 ) * tabley7;
00900
00901 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00902 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00903 } else {
00904 dely = restrict1(dely, ny);
00905 int inyold = int(Util::round(dely));
00906 delz = restrict1(delz, nz);
00907 int inzold = int(Util::round(delz));
00908
00909 float tablex1 = kb.i0win_tab(delx-inxold+3);
00910 float tablex2 = kb.i0win_tab(delx-inxold+2);
00911 float tablex3 = kb.i0win_tab(delx-inxold+1);
00912 float tablex4 = kb.i0win_tab(delx-inxold);
00913 float tablex5 = kb.i0win_tab(delx-inxold-1);
00914 float tablex6 = kb.i0win_tab(delx-inxold-2);
00915 float tablex7 = kb.i0win_tab(delx-inxold-3);
00916
00917 float tabley1 = kb.i0win_tab(dely-inyold+3);
00918 float tabley2 = kb.i0win_tab(dely-inyold+2);
00919 float tabley3 = kb.i0win_tab(dely-inyold+1);
00920 float tabley4 = kb.i0win_tab(dely-inyold);
00921 float tabley5 = kb.i0win_tab(dely-inyold-1);
00922 float tabley6 = kb.i0win_tab(dely-inyold-2);
00923 float tabley7 = kb.i0win_tab(dely-inyold-3);
00924
00925 float tablez1 = kb.i0win_tab(delz-inzold+3);
00926 float tablez2 = kb.i0win_tab(delz-inzold+2);
00927 float tablez3 = kb.i0win_tab(delz-inzold+1);
00928 float tablez4 = kb.i0win_tab(delz-inzold);
00929 float tablez5 = kb.i0win_tab(delz-inzold-1);
00930 float tablez6 = kb.i0win_tab(delz-inzold-2);
00931 float tablez7 = kb.i0win_tab(delz-inzold-3);
00932
00933 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00934
00935 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
00936 x1 = (inxold-3+nx)%nx;
00937 x2 = (inxold-2+nx)%nx;
00938 x3 = (inxold-1+nx)%nx;
00939 x4 = (inxold +nx)%nx;
00940 x5 = (inxold+1+nx)%nx;
00941 x6 = (inxold+2+nx)%nx;
00942 x7 = (inxold+3+nx)%nx;
00943
00944 y1 = ((inyold-3+ny)%ny)*nx;
00945 y2 = ((inyold-2+ny)%ny)*nx;
00946 y3 = ((inyold-1+ny)%ny)*nx;
00947 y4 = ((inyold +ny)%ny)*nx;
00948 y5 = ((inyold+1+ny)%ny)*nx;
00949 y6 = ((inyold+2+ny)%ny)*nx;
00950 y7 = ((inyold+3+ny)%ny)*nx;
00951
00952 z1 = ((inzold-3+nz)%nz)*nx*ny;
00953 z2 = ((inzold-2+nz)%nz)*nx*ny;
00954 z3 = ((inzold-1+nz)%nz)*nx*ny;
00955 z4 = ((inzold +nz)%nz)*nx*ny;
00956 z5 = ((inzold+1+nz)%nz)*nx*ny;
00957 z6 = ((inzold+2+nz)%nz)*nx*ny;
00958 z7 = ((inzold+3+nz)%nz)*nx*ny;
00959 } else {
00960 x1 = inxold-3;
00961 x2 = inxold-2;
00962 x3 = inxold-1;
00963 x4 = inxold;
00964 x5 = inxold+1;
00965 x6 = inxold+2;
00966 x7 = inxold+3;
00967
00968 y1 = (inyold-3)*nx;
00969 y2 = (inyold-2)*nx;
00970 y3 = (inyold-1)*nx;
00971 y4 = inyold*nx;
00972 y5 = (inyold+1)*nx;
00973 y6 = (inyold+2)*nx;
00974 y7 = (inyold+3)*nx;
00975
00976 z1 = (inzold-3)*nx*ny;
00977 z2 = (inzold-2)*nx*ny;
00978 z3 = (inzold-1)*nx*ny;
00979 z4 = inzold*nx*ny;
00980 z5 = (inzold+1)*nx*ny;
00981 z6 = (inzold+2)*nx*ny;
00982 z7 = (inzold+3)*nx*ny;
00983 }
00984
00985 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00986 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00987 data[x7+y1+z1]*tablex7 ) * tabley1 +
00988 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00989 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00990 data[x7+y2+z1]*tablex7 ) * tabley2 +
00991 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00992 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00993 data[x7+y3+z1]*tablex7 ) * tabley3 +
00994 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00995 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00996 data[x7+y4+z1]*tablex7 ) * tabley4 +
00997 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
00998 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
00999 data[x7+y5+z1]*tablex7 ) * tabley5 +
01000 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01001 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01002 data[x7+y6+z1]*tablex7 ) * tabley6 +
01003 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01004 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01005 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01006 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01007 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01008 data[x7+y1+z2]*tablex7 ) * tabley1 +
01009 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01010 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01011 data[x7+y2+z2]*tablex7 ) * tabley2 +
01012 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01013 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01014 data[x7+y3+z2]*tablex7 ) * tabley3 +
01015 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01016 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01017 data[x7+y4+z2]*tablex7 ) * tabley4 +
01018 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01019 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01020 data[x7+y5+z2]*tablex7 ) * tabley5 +
01021 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01022 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01023 data[x7+y6+z2]*tablex7 ) * tabley6 +
01024 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01025 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01026 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01027 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01028 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01029 data[x7+y1+z3]*tablex7 ) * tabley1 +
01030 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01031 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01032 data[x7+y2+z3]*tablex7 ) * tabley2 +
01033 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01034 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01035 data[x7+y3+z3]*tablex7 ) * tabley3 +
01036 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01037 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01038 data[x7+y4+z3]*tablex7 ) * tabley4 +
01039 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01040 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01041 data[x7+y5+z3]*tablex7 ) * tabley5 +
01042 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01043 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01044 data[x7+y6+z3]*tablex7 ) * tabley6 +
01045 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01046 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01047 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01048 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01049 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01050 data[x7+y1+z4]*tablex7 ) * tabley1 +
01051 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01052 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01053 data[x7+y2+z4]*tablex7 ) * tabley2 +
01054 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01055 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01056 data[x7+y3+z4]*tablex7 ) * tabley3 +
01057 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01058 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01059 data[x7+y4+z4]*tablex7 ) * tabley4 +
01060 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01061 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01062 data[x7+y5+z4]*tablex7 ) * tabley5 +
01063 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01064 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01065 data[x7+y6+z4]*tablex7 ) * tabley6 +
01066 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01067 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01068 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01069 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01070 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01071 data[x7+y1+z5]*tablex7 ) * tabley1 +
01072 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01073 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01074 data[x7+y2+z5]*tablex7 ) * tabley2 +
01075 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01076 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01077 data[x7+y3+z5]*tablex7 ) * tabley3 +
01078 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01079 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01080 data[x7+y4+z5]*tablex7 ) * tabley4 +
01081 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01082 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01083 data[x7+y5+z5]*tablex7 ) * tabley5 +
01084 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01085 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01086 data[x7+y6+z5]*tablex7 ) * tabley6 +
01087 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01088 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01089 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01090 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01091 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01092 data[x7+y1+z6]*tablex7 ) * tabley1 +
01093 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01094 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01095 data[x7+y2+z6]*tablex7 ) * tabley2 +
01096 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01097 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01098 data[x7+y3+z6]*tablex7 ) * tabley3 +
01099 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01100 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01101 data[x7+y4+z6]*tablex7 ) * tabley4 +
01102 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01103 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01104 data[x7+y5+z6]*tablex7 ) * tabley5 +
01105 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01106 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01107 data[x7+y6+z6]*tablex7 ) * tabley6 +
01108 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01109 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01110 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01111 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01112 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01113 data[x7+y1+z7]*tablex7 ) * tabley1 +
01114 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01115 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01116 data[x7+y2+z7]*tablex7 ) * tabley2 +
01117 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01118 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01119 data[x7+y3+z7]*tablex7 ) * tabley3 +
01120 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01121 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01122 data[x7+y4+z7]*tablex7 ) * tabley4 +
01123 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01124 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01125 data[x7+y5+z7]*tablex7 ) * tabley5 +
01126 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01127 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01128 data[x7+y6+z7]*tablex7 ) * tabley6 +
01129 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01130 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01131 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01132
01133 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01134 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01135 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01136 }
01137 return pixel/w;
01138 }
01139
01140 float Util::get_pixel_conv_new_background(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb, int xnew, int ynew) {
01141 int K = kb.get_window_size();
01142 int kbmin = -K/2;
01143 int kbmax = -kbmin;
01144 int kbc = kbmax+1;
01145
01146 float pixel =0.0f;
01147 float w=0.0f;
01148
01149 float argdelx = delx;
01150 delx = restrict1(delx, nx);
01151 int inxold = int(round(delx));
01152 if ( ny < 2 ) {
01153 float tablex1 = kb.i0win_tab(delx-inxold+3);
01154 float tablex2 = kb.i0win_tab(delx-inxold+2);
01155 float tablex3 = kb.i0win_tab(delx-inxold+1);
01156 float tablex4 = kb.i0win_tab(delx-inxold);
01157 float tablex5 = kb.i0win_tab(delx-inxold-1);
01158 float tablex6 = kb.i0win_tab(delx-inxold-2);
01159 float tablex7 = kb.i0win_tab(delx-inxold-3);
01160
01161 int x1, x2, x3, x4, x5, x6, x7;
01162
01163 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
01164 x1 = (inxold-3+nx)%nx;
01165 x2 = (inxold-2+nx)%nx;
01166 x3 = (inxold-1+nx)%nx;
01167 x4 = (inxold +nx)%nx;
01168 x5 = (inxold+1+nx)%nx;
01169 x6 = (inxold+2+nx)%nx;
01170 x7 = (inxold+3+nx)%nx;
01171 } else {
01172 x1 = inxold-3;
01173 x2 = inxold-2;
01174 x3 = inxold-1;
01175 x4 = inxold;
01176 x5 = inxold+1;
01177 x6 = inxold+2;
01178 x7 = inxold+3;
01179 }
01180
01181 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01182 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01183 data[x7]*tablex7 ;
01184
01185 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01186 } else if ( nz < 2 ) {
01187
01188 delx = argdelx;
01189
01190 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01191 delx = (float)xnew*2.0f;
01192 dely = (float)ynew*2.0f;
01193 }
01194
01195 int inxold = int(round(delx));
01196 int inyold = int(round(dely));
01197
01198 float tablex1 = kb.i0win_tab(delx-inxold+3);
01199 float tablex2 = kb.i0win_tab(delx-inxold+2);
01200 float tablex3 = kb.i0win_tab(delx-inxold+1);
01201 float tablex4 = kb.i0win_tab(delx-inxold);
01202 float tablex5 = kb.i0win_tab(delx-inxold-1);
01203 float tablex6 = kb.i0win_tab(delx-inxold-2);
01204 float tablex7 = kb.i0win_tab(delx-inxold-3);
01205
01206 float tabley1 = kb.i0win_tab(dely-inyold+3);
01207 float tabley2 = kb.i0win_tab(dely-inyold+2);
01208 float tabley3 = kb.i0win_tab(dely-inyold+1);
01209 float tabley4 = kb.i0win_tab(dely-inyold);
01210 float tabley5 = kb.i0win_tab(dely-inyold-1);
01211 float tabley6 = kb.i0win_tab(dely-inyold-2);
01212 float tabley7 = kb.i0win_tab(dely-inyold-3);
01213
01214 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01215
01216 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
01217 x1 = (inxold-3+nx)%nx;
01218 x2 = (inxold-2+nx)%nx;
01219 x3 = (inxold-1+nx)%nx;
01220 x4 = (inxold +nx)%nx;
01221 x5 = (inxold+1+nx)%nx;
01222 x6 = (inxold+2+nx)%nx;
01223 x7 = (inxold+3+nx)%nx;
01224
01225 y1 = ((inyold-3+ny)%ny)*nx;
01226 y2 = ((inyold-2+ny)%ny)*nx;
01227 y3 = ((inyold-1+ny)%ny)*nx;
01228 y4 = ((inyold +ny)%ny)*nx;
01229 y5 = ((inyold+1+ny)%ny)*nx;
01230 y6 = ((inyold+2+ny)%ny)*nx;
01231 y7 = ((inyold+3+ny)%ny)*nx;
01232 } else {
01233 x1 = inxold-3;
01234 x2 = inxold-2;
01235 x3 = inxold-1;
01236 x4 = inxold;
01237 x5 = inxold+1;
01238 x6 = inxold+2;
01239 x7 = inxold+3;
01240
01241 y1 = (inyold-3)*nx;
01242 y2 = (inyold-2)*nx;
01243 y3 = (inyold-1)*nx;
01244 y4 = inyold*nx;
01245 y5 = (inyold+1)*nx;
01246 y6 = (inyold+2)*nx;
01247 y7 = (inyold+3)*nx;
01248 }
01249
01250 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01251 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01252 data[x7+y1]*tablex7 ) * tabley1 +
01253 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01254 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01255 data[x7+y2]*tablex7 ) * tabley2 +
01256 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01257 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01258 data[x7+y3]*tablex7 ) * tabley3 +
01259 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01260 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01261 data[x7+y4]*tablex7 ) * tabley4 +
01262 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01263 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01264 data[x7+y5]*tablex7 ) * tabley5 +
01265 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01266 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01267 data[x7+y6]*tablex7 ) * tabley6 +
01268 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01269 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01270 data[x7+y7]*tablex7 ) * tabley7;
01271
01272 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01273 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01274 } else {
01275 dely = restrict1(dely, ny);
01276 int inyold = int(Util::round(dely));
01277 delz = restrict1(delz, nz);
01278 int inzold = int(Util::round(delz));
01279
01280 float tablex1 = kb.i0win_tab(delx-inxold+3);
01281 float tablex2 = kb.i0win_tab(delx-inxold+2);
01282 float tablex3 = kb.i0win_tab(delx-inxold+1);
01283 float tablex4 = kb.i0win_tab(delx-inxold);
01284 float tablex5 = kb.i0win_tab(delx-inxold-1);
01285 float tablex6 = kb.i0win_tab(delx-inxold-2);
01286 float tablex7 = kb.i0win_tab(delx-inxold-3);
01287
01288 float tabley1 = kb.i0win_tab(dely-inyold+3);
01289 float tabley2 = kb.i0win_tab(dely-inyold+2);
01290 float tabley3 = kb.i0win_tab(dely-inyold+1);
01291 float tabley4 = kb.i0win_tab(dely-inyold);
01292 float tabley5 = kb.i0win_tab(dely-inyold-1);
01293 float tabley6 = kb.i0win_tab(dely-inyold-2);
01294 float tabley7 = kb.i0win_tab(dely-inyold-3);
01295
01296 float tablez1 = kb.i0win_tab(delz-inzold+3);
01297 float tablez2 = kb.i0win_tab(delz-inzold+2);
01298 float tablez3 = kb.i0win_tab(delz-inzold+1);
01299 float tablez4 = kb.i0win_tab(delz-inzold);
01300 float tablez5 = kb.i0win_tab(delz-inzold-1);
01301 float tablez6 = kb.i0win_tab(delz-inzold-2);
01302 float tablez7 = kb.i0win_tab(delz-inzold-3);
01303
01304 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01305
01306 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
01307 x1 = (inxold-3+nx)%nx;
01308 x2 = (inxold-2+nx)%nx;
01309 x3 = (inxold-1+nx)%nx;
01310 x4 = (inxold +nx)%nx;
01311 x5 = (inxold+1+nx)%nx;
01312 x6 = (inxold+2+nx)%nx;
01313 x7 = (inxold+3+nx)%nx;
01314
01315 y1 = ((inyold-3+ny)%ny)*nx;
01316 y2 = ((inyold-2+ny)%ny)*nx;
01317 y3 = ((inyold-1+ny)%ny)*nx;
01318 y4 = ((inyold +ny)%ny)*nx;
01319 y5 = ((inyold+1+ny)%ny)*nx;
01320 y6 = ((inyold+2+ny)%ny)*nx;
01321 y7 = ((inyold+3+ny)%ny)*nx;
01322
01323 z1 = ((inzold-3+nz)%nz)*nx*ny;
01324 z2 = ((inzold-2+nz)%nz)*nx*ny;
01325 z3 = ((inzold-1+nz)%nz)*nx*ny;
01326 z4 = ((inzold +nz)%nz)*nx*ny;
01327 z5 = ((inzold+1+nz)%nz)*nx*ny;
01328 z6 = ((inzold+2+nz)%nz)*nx*ny;
01329 z7 = ((inzold+3+nz)%nz)*nx*ny;
01330 } else {
01331 x1 = inxold-3;
01332 x2 = inxold-2;
01333 x3 = inxold-1;
01334 x4 = inxold;
01335 x5 = inxold+1;
01336 x6 = inxold+2;
01337 x7 = inxold+3;
01338
01339 y1 = (inyold-3)*nx;
01340 y2 = (inyold-2)*nx;
01341 y3 = (inyold-1)*nx;
01342 y4 = inyold*nx;
01343 y5 = (inyold+1)*nx;
01344 y6 = (inyold+2)*nx;
01345 y7 = (inyold+3)*nx;
01346
01347 z1 = (inzold-3)*nx*ny;
01348 z2 = (inzold-2)*nx*ny;
01349 z3 = (inzold-1)*nx*ny;
01350 z4 = inzold*nx*ny;
01351 z5 = (inzold+1)*nx*ny;
01352 z6 = (inzold+2)*nx*ny;
01353 z7 = (inzold+3)*nx*ny;
01354 }
01355
01356 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01357 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01358 data[x7+y1+z1]*tablex7 ) * tabley1 +
01359 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01360 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01361 data[x7+y2+z1]*tablex7 ) * tabley2 +
01362 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01363 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01364 data[x7+y3+z1]*tablex7 ) * tabley3 +
01365 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01366 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01367 data[x7+y4+z1]*tablex7 ) * tabley4 +
01368 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01369 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01370 data[x7+y5+z1]*tablex7 ) * tabley5 +
01371 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01372 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01373 data[x7+y6+z1]*tablex7 ) * tabley6 +
01374 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01375 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01376 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01377 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01378 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01379 data[x7+y1+z2]*tablex7 ) * tabley1 +
01380 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01381 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01382 data[x7+y2+z2]*tablex7 ) * tabley2 +
01383 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01384 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01385 data[x7+y3+z2]*tablex7 ) * tabley3 +
01386 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01387 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01388 data[x7+y4+z2]*tablex7 ) * tabley4 +
01389 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01390 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01391 data[x7+y5+z2]*tablex7 ) * tabley5 +
01392 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01393 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01394 data[x7+y6+z2]*tablex7 ) * tabley6 +
01395 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01396 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01397 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01398 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01399 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01400 data[x7+y1+z3]*tablex7 ) * tabley1 +
01401 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01402 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01403 data[x7+y2+z3]*tablex7 ) * tabley2 +
01404 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01405 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01406 data[x7+y3+z3]*tablex7 ) * tabley3 +
01407 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01408 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01409 data[x7+y4+z3]*tablex7 ) * tabley4 +
01410 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01411 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01412 data[x7+y5+z3]*tablex7 ) * tabley5 +
01413 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01414 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01415 data[x7+y6+z3]*tablex7 ) * tabley6 +
01416 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01417 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01418 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01419 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01420 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01421 data[x7+y1+z4]*tablex7 ) * tabley1 +
01422 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01423 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01424 data[x7+y2+z4]*tablex7 ) * tabley2 +
01425 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01426 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01427 data[x7+y3+z4]*tablex7 ) * tabley3 +
01428 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01429 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01430 data[x7+y4+z4]*tablex7 ) * tabley4 +
01431 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01432 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01433 data[x7+y5+z4]*tablex7 ) * tabley5 +
01434 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01435 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01436 data[x7+y6+z4]*tablex7 ) * tabley6 +
01437 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01438 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01439 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01440 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01441 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01442 data[x7+y1+z5]*tablex7 ) * tabley1 +
01443 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01444 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01445 data[x7+y2+z5]*tablex7 ) * tabley2 +
01446 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01447 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01448 data[x7+y3+z5]*tablex7 ) * tabley3 +
01449 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01450 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01451 data[x7+y4+z5]*tablex7 ) * tabley4 +
01452 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01453 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01454 data[x7+y5+z5]*tablex7 ) * tabley5 +
01455 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01456 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01457 data[x7+y6+z5]*tablex7 ) * tabley6 +
01458 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01459 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01460 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01461 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01462 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01463 data[x7+y1+z6]*tablex7 ) * tabley1 +
01464 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01465 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01466 data[x7+y2+z6]*tablex7 ) * tabley2 +
01467 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01468 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01469 data[x7+y3+z6]*tablex7 ) * tabley3 +
01470 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01471 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01472 data[x7+y4+z6]*tablex7 ) * tabley4 +
01473 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01474 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01475 data[x7+y5+z6]*tablex7 ) * tabley5 +
01476 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01477 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01478 data[x7+y6+z6]*tablex7 ) * tabley6 +
01479 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01480 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01481 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01482 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01483 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01484 data[x7+y1+z7]*tablex7 ) * tabley1 +
01485 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01486 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01487 data[x7+y2+z7]*tablex7 ) * tabley2 +
01488 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01489 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01490 data[x7+y3+z7]*tablex7 ) * tabley3 +
01491 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01492 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01493 data[x7+y4+z7]*tablex7 ) * tabley4 +
01494 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01495 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01496 data[x7+y5+z7]*tablex7 ) * tabley5 +
01497 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01498 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01499 data[x7+y6+z7]*tablex7 ) * tabley6 +
01500 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01501 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01502 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01503
01504 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01505 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01506 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01507 }
01508 return pixel/w;
01509 }
01510
01511
01512
01513
01514
01515
01516
01517
01518
01519
01520
01521
01522
01523
01524
01525
01526
01527
01528
01529
01530
01531
01532
01533
01534
01535
01536
01537
01538
01539
01540
01541
01542
01543
01544
01545
01546
01547
01548
01549
01550
01551
01552
01553
01554
01555
01556
01557
01558
01559
01560
01561
01562
01563
01564
01565
01566
01567
01568
01569
01570
01571
01572
01573
01574
01575
01576
01577
01578
01579
01580
01581
01582
01583
01584
01585
01586
01587
01588
01589
01590
01591
01592
01593
01594
01595
01596
01597
01598
01599
01600
01601
01602
01603
01604
01605
01606
01607
01608
01609
01610
01611
01612
01613
01614
01615
01616
01617
01618
01619
01620
01621
01622
01623
01624
01625
01626
01627
01628
01629
01630
01631
01632
01633
01634
01635
01636
01637
01638
01639
01640
01641
01642
01643
01644
01645
01646
01647
01648
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672
01673
01674
01675
01676
01677
01678
01679
01680
01681
01682
01683
01684
01685
01686
01687
01688
01689 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01690
01691 int nxreal = nx - 2;
01692 if (nxreal != ny)
01693 throw ImageDimensionException("extractpoint requires ny == nx");
01694 int nhalf = nxreal/2;
01695 bool flip = (nuxnew < 0.f);
01696 if (flip) {
01697 nuxnew *= -1;
01698 nuynew *= -1;
01699 }
01700 if (nuynew >= nhalf-0.5) {
01701 nuynew -= nxreal;
01702 } else if (nuynew < -nhalf-0.5) {
01703 nuynew += nxreal;
01704 }
01705
01706
01707
01708
01709 int ixn = int(Util::round(nuxnew));
01710 int iyn = int(Util::round(nuynew));
01711
01712
01713 static float wy[7];
01714 static float wx[7];
01715
01716 float iynn = nuynew - iyn;
01717 wy[0] = kb.i0win_tab(iynn+3);
01718 wy[1] = kb.i0win_tab(iynn+2);
01719 wy[2] = kb.i0win_tab(iynn+1);
01720 wy[3] = kb.i0win_tab(iynn);
01721 wy[4] = kb.i0win_tab(iynn-1);
01722 wy[5] = kb.i0win_tab(iynn-2);
01723 wy[6] = kb.i0win_tab(iynn-3);
01724
01725 float ixnn = nuxnew - ixn;
01726 wx[0] = kb.i0win_tab(ixnn+3);
01727 wx[1] = kb.i0win_tab(ixnn+2);
01728 wx[2] = kb.i0win_tab(ixnn+1);
01729 wx[3] = kb.i0win_tab(ixnn);
01730 wx[4] = kb.i0win_tab(ixnn-1);
01731 wx[5] = kb.i0win_tab(ixnn-2);
01732 wx[6] = kb.i0win_tab(ixnn-3);
01733
01734 float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01735
01736 complex<float> result(0.f,0.f);
01737 if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01738
01739 for (int iy = 0; iy < 7; iy++) {
01740 int iyp = iyn + iy - 3 ;
01741 for (int ix = 0; ix < 7; ix++) {
01742 int ixp = ixn + ix - 3;
01743 float w = wx[ix]*wy[iy];
01744 complex<float> val = fimage->cmplx(ixp,iyp);
01745 result += val*w;
01746 }
01747 }
01748 } else {
01749
01750 for (int iy = 0; iy < 7; iy++) {
01751 int iyp = iyn + iy - 3;
01752 for (int ix = 0; ix < 7; ix++) {
01753 int ixp = ixn + ix - 3;
01754 bool mirror = false;
01755 int ixt = ixp, iyt = iyp;
01756 if (ixt < 0) {
01757 ixt = -ixt;
01758 iyt = -iyt;
01759 mirror = !mirror;
01760 }
01761 if (ixt > nhalf) {
01762 ixt = nxreal - ixt;
01763 iyt = -iyt;
01764 mirror = !mirror;
01765 }
01766 if (iyt > nhalf-1) iyt -= nxreal;
01767 if (iyt < -nhalf) iyt += nxreal;
01768 float w = wx[ix]*wy[iy];
01769 complex<float> val = fimage->cmplx(ixt,iyt);
01770 if (mirror) result += conj(val)*w;
01771 else result += val*w;
01772 }
01773 }
01774 }
01775 if (flip) result = conj(result)/wsum;
01776 else result /= wsum;
01777 return result;
01778 }
01779
01780
01781
01782
01783
01784
01785
01786
01787
01788
01789
01790
01791
01792
01793
01794
01795
01796
01797
01798
01799
01800
01801
01802
01803
01804
01805
01806
01807
01808
01809
01810
01811
01812
01813
01814
01815
01816
01817
01818
01819
01820
01821
01822
01823
01824
01825
01826
01827
01828
01829
01830
01831
01832
01833
01834
01835
01836
01837
01838
01839
01840
01841
01842
01843
01844
01845
01846
01847
01848
01849
01850
01851
01852
01853
01854
01855
01856
01857
01858
01859
01860
01861
01862
01863
01864
01865
01866
01867
01868
01869
01870
01871
01872
01873
01874
01875
01876
01877
01878
01879
01880
01881
01882
01883
01884
01885
01886
01887
01888
01889
01890
01891
01892
01893
01894
01895
01896
01897
01898
01899
01900
01901
01902
01903
01904
01905
01906
01907
01908 float Util::triquad(float R, float S, float T, float* fdata)
01909 {
01910
01911 const float C2 = 0.5f;
01912 const float C4 = 0.25f;
01913 const float C8 = 0.125f;
01914
01915 float RS = R * S;
01916 float ST = S * T;
01917 float RT = R * T;
01918 float RST = R * ST;
01919
01920 float RSQ = 1-R*R;
01921 float SSQ = 1-S*S;
01922 float TSQ = 1-T*T;
01923
01924 float RM1 = (1-R);
01925 float SM1 = (1-S);
01926 float TM1 = (1-T);
01927
01928 float RP1 = (1+R);
01929 float SP1 = (1+S);
01930 float TP1 = (1+T);
01931
01932 float triquad =
01933 (-C8) * RST * RM1 * SM1 * TM1 * fdata[0] +
01934 ( C4) * ST * RSQ * SM1 * TM1 * fdata[1] +
01935 ( C8) * RST * RP1 * SM1 * TM1 * fdata[2] +
01936 ( C4) * RT * RM1 * SSQ * TM1 * fdata[3] +
01937 (-C2) * T * RSQ * SSQ * TM1 * fdata[4] +
01938 (-C4) * RT * RP1 * SSQ * TM1 * fdata[5] +
01939 ( C8) * RST * RM1 * SP1 * TM1 * fdata[6] +
01940 (-C4) * ST * RSQ * SP1 * TM1 * fdata[7] +
01941 (-C8) * RST * RP1 * SP1 * TM1 * fdata[8] +
01942
01943 ( C4) * RS * RM1 * SM1 * TSQ * fdata[9] +
01944 (-C2) * S * RSQ * SM1 * TSQ * fdata[10] +
01945 (-C4) * RS * RP1 * SM1 * TSQ * fdata[11] +
01946 (-C2) * R * RM1 * SSQ * TSQ * fdata[12] +
01947 RSQ * SSQ * TSQ * fdata[13] +
01948 ( C2) * R * RP1 * SSQ * TSQ * fdata[14] +
01949 (-C4) * RS * RM1 * SP1 * TSQ * fdata[15] +
01950 ( C2) * S * RSQ * SP1 * TSQ * fdata[16] +
01951 ( C4) * RS * RP1 * SP1 * TSQ * fdata[17] +
01952
01953 ( C8) * RST * RM1 * SM1 * TP1 * fdata[18] +
01954 (-C4) * ST * RSQ * SM1 * TP1 * fdata[19] +
01955 (-C8) * RST * RP1 * SM1 * TP1 * fdata[20] +
01956 (-C4) * RT * RM1 * SSQ * TP1 * fdata[21] +
01957 ( C2) * T * RSQ * SSQ * TP1 * fdata[22] +
01958 ( C4) * RT * RP1 * SSQ * TP1 * fdata[23] +
01959 (-C8) * RST * RM1 * SP1 * TP1 * fdata[24] +
01960 ( C4) * ST * RSQ * SP1 * TP1 * fdata[25] +
01961 ( C8) * RST * RP1 * SP1 * TP1 * fdata[26] ;
01962 return triquad;
01963 }
01964
01965 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01966 : M(M_), fc(fc_), ntable(ntable_) {
01967
01968 build_sBtable();
01969 }
01970
01971 void Util::sincBlackman::build_sBtable() {
01972 sBtable.resize(ntable+1);
01973 int ltab = int(round(float(ntable)/1.25f));
01974 int M2 = M/2;
01975 fltb = float(ltab)/M2;
01976 for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01977 float x = 1.0e-7f;
01978 sBtable[0] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01979 for (int i=1; i <= ltab; i++) {
01980 x = float(i)/fltb;
01981 sBtable[i] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01982
01983 }
01984 }
01985
01986 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01987 int N_, float vtable_, int ntable_)
01988 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01989 ntable(ntable_) {
01990
01991 if (0.f == v) v = float(K)/2;
01992 if (0.f == vtable) vtable = v;
01993 alphar = alpha*r;
01994 fac = static_cast<float>(twopi)*alphar*v;
01995 vadjust = 1.0f*v;
01996 facadj = static_cast<float>(twopi)*alphar*vadjust;
01997 build_I0table();
01998 }
01999
02000 float Util::KaiserBessel::i0win(float x) const {
02001 float val0 = float(gsl_sf_bessel_I0(facadj));
02002 float absx = fabs(x);
02003 if (absx > vadjust) return 0.f;
02004 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02005 float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02006 return res;
02007 }
02008
02009 void Util::KaiserBessel::build_I0table() {
02010 i0table.resize(ntable+1);
02011 int ltab = int(round(float(ntable)/1.25f));
02012 fltb = float(ltab)/(K/2);
02013 float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02014 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02015 for (int i=0; i <= ltab; i++) {
02016 float s = float(i)/fltb/N;
02017 if (s < vadjust) {
02018 float rt = sqrt(1.f - pow(s/vadjust, 2));
02019 i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02020 } else {
02021 i0table[i] = 0.f;
02022 }
02023
02024 }
02025 }
02026
02027 float Util::KaiserBessel::I0table_maxerror() {
02028 float maxdiff = 0.f;
02029 for (int i = 1; i <= ntable; i++) {
02030 float diff = fabs(i0table[i] - i0table[i-1]);
02031 if (diff > maxdiff) maxdiff = diff;
02032 }
02033 return maxdiff;
02034 }
02035
02036 float Util::KaiserBessel::sinhwin(float x) const {
02037 float val0 = sinh(fac)/fac;
02038 float absx = fabs(x);
02039 if (0.0 == x) {
02040 float res = 1.0f;
02041 return res;
02042 } else if (absx == alphar) {
02043 return 1.0f/val0;
02044 } else if (absx < alphar) {
02045 float rt = sqrt(1.0f - pow((x/alphar), 2));
02046 float facrt = fac*rt;
02047 float res = (sinh(facrt)/facrt)/val0;
02048 return res;
02049 } else {
02050 float rt = sqrt(pow((x/alphar),2) - 1.f);
02051 float facrt = fac*rt;
02052 float res = (sin(facrt)/facrt)/val0;
02053 return res;
02054 }
02055 }
02056
02057 float Util::FakeKaiserBessel::i0win(float x) const {
02058 float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02059 float absx = fabs(x);
02060 if (absx > vadjust) return 0.f;
02061 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02062 float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02063 return res;
02064 }
02065
02066 void Util::FakeKaiserBessel::build_I0table() {
02067 i0table.resize(ntable+1);
02068 int ltab = int(round(float(ntable)/1.1f));
02069 fltb = float(ltab)/(K/2);
02070 float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02071 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02072 for (int i=0; i <= ltab; i++) {
02073 float s = float(i)/fltb/N;
02074 if (s < vadjust) {
02075 float rt = sqrt(1.f - pow(s/vadjust, 2));
02076 i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02077 } else {
02078 i0table[i] = 0.f;
02079 }
02080 }
02081 }
02082
02083 float Util::FakeKaiserBessel::sinhwin(float x) const {
02084 float val0 = sinh(fac)/fac;
02085 float absx = fabs(x);
02086 if (0.0 == x) {
02087 float res = 1.0f;
02088 return res;
02089 } else if (absx == alphar) {
02090 return 1.0f/val0;
02091 } else if (absx < alphar) {
02092 float rt = sqrt(1.0f - pow((x/alphar), 2));
02093 float facrt = fac*rt;
02094 float res = (sinh(facrt)/facrt)/val0;
02095 return res;
02096 } else {
02097 float rt = sqrt(pow((x/alphar),2) - 1.f);
02098 float facrt = fac*rt;
02099 float res = (sin(facrt)/facrt)/val0;
02100 return res;
02101 }
02102 }
02103
02104 #if 0 // 1-st order KB window
02105 float Util::FakeKaiserBessel::sinhwin(float x) const {
02106
02107 float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02108 float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02109 float absx = fabs(x);
02110 if (0.0 == x) {
02111
02112 float res = val0;
02113 return res;
02114 } else if (absx == alphar) {
02115
02116 return prefix;
02117 } else if (absx < alphar) {
02118 float rt = sqrt(1.0f - pow((x/alphar), 2));
02119
02120 float facrt = facadj*rt;
02121
02122 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02123 return res;
02124 } else {
02125 float rt = sqrt(pow((x/alphar),2) - 1.f);
02126
02127 float facrt = facadj*rt;
02128
02129 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02130 return res;
02131 }
02132 }
02133 #endif // 0
02134
02135
02136
02137 #define circ(i) circ[i-1]
02138 #define numr(i,j) numr[(j-1)*3 + i-1]
02139 #define xim(i,j) xim[(j-1)*nsam + i-1]
02140
02141 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02142 int nsam = image->get_xsize();
02143 int nrow = image->get_ysize();
02144 int nring = numr.size()/3;
02145 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02146 EMData* out = new EMData();
02147 out->set_size(lcirc,1,1);
02148 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02149 float *xim = image->get_data();
02150 float *circ = out->get_data();
02151
02152
02153
02154
02155
02156
02157
02158
02159
02160
02161
02162
02163
02164
02165
02166
02167 double dfi, dpi;
02168 int ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02169 float yq, xold, yold, fi, x, y;
02170
02171 ns2 = nsam/2+1;
02172 nr2 = nrow/2+1;
02173 dpi = 2.0*atan(1.0);
02174
02175 for (i=1;i<=nring;i++) {
02176
02177 inr = numr(1,i);
02178 yq = static_cast<float>(inr);
02179 l = numr(3,i);
02180 if (mode == 'h' || mode == 'H') lt = l/2;
02181 else lt = l/4;
02182
02183 nsim = lt-1;
02184 dfi = dpi/(nsim+1);
02185 kcirc = numr(2,i);
02186 xold = 0.0f;
02187 yold = static_cast<float>(inr);
02188 circ(kcirc) = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02189 xold = static_cast<float>(inr);
02190 yold = 0.0f;
02191 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02192
02193 if (mode == 'f' || mode == 'F') {
02194 xold = 0.0f;
02195 yold = static_cast<float>(-inr);
02196 circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02197 xold = static_cast<float>(-inr);
02198 yold = 0.0f;
02199 circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02200 }
02201
02202 for (j=1;j<=nsim;j++) {
02203 fi = static_cast<float>(dfi*j);
02204 x = sin(fi)*yq;
02205 y = cos(fi)*yq;
02206 xold = x;
02207 yold = y;
02208 circ(j+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02209 xold = y;
02210 yold = -x;
02211 circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02212
02213 if (mode == 'f' || mode == 'F') {
02214 xold = -x;
02215 yold = -y;
02216 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02217 xold = -y;
02218 yold = x;
02219 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02220 }
02221 }
02222 }
02223 return out;
02224 }
02225
02226 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02227 int nsam = image->get_xsize();
02228 int nrow = image->get_ysize();
02229 int nring = numr.size()/3;
02230 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02231 EMData* out = new EMData();
02232 out->set_size(lcirc,1,1);
02233 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02234 float *xim = image->get_data();
02235 float *circ = out->get_data();
02236 double dpi, dfi;
02237 int it, jt, inr, l, nsim, kcirc, lt;
02238 float xold, yold, fi, x, y;
02239
02240
02241
02242 dpi = 2*atan(1.0);
02243 for (it=1; it<=nring; it++) {
02244
02245 inr = numr(1,it);
02246
02247
02248
02249
02250 l = numr(3,it);
02251 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02252 else lt = l / 4;
02253
02254 nsim = lt - 1;
02255 dfi = dpi / (nsim+1);
02256 kcirc = numr(2,it);
02257 xold = 0.0f+cns2;
02258 yold = inr+cnr2;
02259
02260 Assert( kcirc <= lcirc );
02261 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02262
02263 xold = inr+cns2;
02264 yold = 0.0f+cnr2;
02265 Assert( lt+kcirc <= lcirc );
02266 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02267
02268 if ( mode == 'f' || mode == 'F' ) {
02269 xold = 0.0f+cns2;
02270 yold = -inr+cnr2;
02271 Assert( lt+lt+kcirc <= lcirc );
02272 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02273
02274 xold = -inr+cns2;
02275 yold = 0.0f+cnr2;
02276 Assert(lt+lt+lt+kcirc <= lcirc );
02277 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02278 }
02279
02280 for (jt=1; jt<=nsim; jt++) {
02281 fi = static_cast<float>(dfi * jt);
02282 x = sin(fi) * inr;
02283 y = cos(fi) * inr;
02284
02285 xold = x+cns2;
02286 yold = y+cnr2;
02287
02288 Assert( jt+kcirc <= lcirc );
02289 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02290
02291 xold = y+cns2;
02292 yold = -x+cnr2;
02293
02294 Assert( jt+lt+kcirc <= lcirc );
02295 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02296
02297 if ( mode == 'f' || mode == 'F' ) {
02298 xold = -x+cns2;
02299 yold = -y+cnr2;
02300
02301 Assert( jt+lt+lt+kcirc <= lcirc );
02302 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02303
02304 xold = -y+cns2;
02305 yold = x+cnr2;
02306
02307 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02308 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02309 }
02310 }
02311 }
02312 return out;
02313 }
02314
02315 float Util::bilinear(float xold, float yold, int nsam, int, float* xim)
02316 {
02317
02318
02319
02320
02321 float bilinear;
02322 int ixold, iyold;
02323
02324
02325
02326
02327
02328
02329
02330
02331
02332
02333
02334
02335
02336
02337
02338
02339
02340
02341
02342
02343
02344
02345
02346 float xdif, ydif;
02347
02348 ixold = (int) xold;
02349 iyold = (int) yold;
02350 ydif = yold - iyold;
02351
02352
02353
02354
02355
02356 xdif = xold - ixold;
02357 bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02358 xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02359 ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02360
02361 return bilinear;
02362 }
02363
02364 void Util::alrl_ms(float *xim, int nsam, int nrow, float cns2, float cnr2,
02365 int *numr, float *circ, int , int nring, char mode) {
02366 double dpi, dfi;
02367 int it, jt, inr, l, nsim, kcirc, lt;
02368 float xold, yold, fi, x, y;
02369
02370
02371
02372
02373 dpi = 2*atan(1.0);
02374 for (it=1; it<=nring; it++) {
02375
02376 inr = numr(1,it);
02377
02378 l = numr(3,it);
02379 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02380 else lt = l / 4;
02381
02382 nsim = lt - 1;
02383 dfi = dpi / (nsim+1);
02384 kcirc = numr(2,it);
02385
02386
02387 xold = 0.0f+cns2;
02388 yold = inr+cnr2;
02389
02390 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02391
02392 xold = inr+cns2;
02393 yold = 0.0f+cnr2;
02394 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02395
02396 if ( mode == 'f' || mode == 'F' ) {
02397 xold = 0.0f+cns2;
02398 yold = -inr+cnr2;
02399 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02400
02401 xold = -inr+cns2;
02402 yold = 0.0f+cnr2;
02403 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02404 }
02405
02406 for (jt=1; jt<=nsim; jt++) {
02407 fi = static_cast<float>(dfi * jt);
02408 x = sin(fi) * inr;
02409 y = cos(fi) * inr;
02410
02411 xold = x+cns2;
02412 yold = y+cnr2;
02413 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02414
02415 xold = y+cns2;
02416 yold = -x+cnr2;
02417 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02418
02419 if ( mode == 'f' || mode == 'F' ) {
02420 xold = -x+cns2;
02421 yold = -y+cnr2;
02422 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02423
02424 xold = -y+cns2;
02425 yold = x+cnr2;
02426 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02427 }
02428 }
02429 }
02430 }
02431
02432
02433
02434
02435
02436
02437
02438
02439
02440
02441
02442
02443
02444
02445
02446
02447
02448
02449
02450
02451
02452
02453
02454
02455
02456
02457
02458
02459
02460
02461
02462
02463
02464
02465
02466
02467
02468
02469
02470
02471
02472
02473
02474
02475
02476
02477
02478
02479
02480
02481
02482
02483
02484
02485
02486
02487
02488
02489
02490
02491
02492
02493
02494
02495
02496
02497
02498
02499
02500
02501
02502
02503
02504
02505
02506
02507 #undef xim
02508
02509 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02510
02511 int nring = numr.size()/3;
02512 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02513 EMData* out = new EMData();
02514 out->set_size(lcirc,1,1);
02515 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02516 float *circ = out->get_data();
02517 float *fimage = image->get_data();
02518 int nx = image->get_xsize();
02519 int ny = image->get_ysize();
02520 int nz = image->get_zsize();
02521 double dpi, dfi;
02522 int it, jt, inr, l, nsim, kcirc, lt;
02523 float yq, xold, yold, fi, x, y;
02524
02525
02526
02527
02528 dpi = 2*atan(1.0);
02529 for (it=1;it<=nring;it++) {
02530
02531 inr = numr(1,it);
02532 yq = static_cast<float>(inr);
02533
02534 l = numr(3,it);
02535 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02536 else lt = l / 4;
02537
02538 nsim = lt - 1;
02539 dfi = dpi / (nsim+1);
02540 kcirc = numr(2,it);
02541 xold = 0.0f;
02542 yold = static_cast<float>(inr);
02543 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02544
02545
02546 xold = static_cast<float>(inr);
02547 yold = 0.0f;
02548 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02549
02550
02551 if ( mode == 'f' || mode == 'F' ) {
02552 xold = 0.0f;
02553 yold = static_cast<float>(-inr);
02554 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02555
02556
02557 xold = static_cast<float>(-inr);
02558 yold = 0.0f;
02559 circ(lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02560
02561 }
02562
02563 for (jt=1;jt<=nsim;jt++) {
02564 fi = static_cast<float>(dfi * jt);
02565 x = sin(fi) * yq;
02566 y = cos(fi) * yq;
02567
02568 xold = x;
02569 yold = y;
02570 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02571
02572
02573 xold = y;
02574 yold = -x;
02575 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02576
02577
02578 if ( mode == 'f' || mode == 'F' ) {
02579 xold = -x;
02580 yold = -y;
02581 circ(jt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02582
02583
02584 xold = -y;
02585 yold = x;
02586 circ(jt+lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02587
02588 }
02589 }
02590 }
02591 return out;
02592 }
02593
02594
02595
02596
02597
02598
02599
02600
02601
02602
02603
02604
02605
02606
02607
02608
02609
02610
02611
02612
02613
02614
02615
02616
02617 #define tab1(i) tab1[i-1]
02618 #define xcmplx(i,j) xcmplx [(j-1)*2 + i-1]
02619 #define br(i) br[i-1]
02620 #define bi(i) bi[i-1]
02621
02622 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02623 {
02624 double rni,sgn,tr1,tr2,ti1,ti2;
02625 double cc,c,ss,s,t,x2,x3,x4,x5;
02626 int b3,b4,b5,b6,b7,b56;
02627 int n, k, l, j, i, ix0, ix1, status=0;
02628
02629 const double tab1[] = {
02630 9.58737990959775e-5,
02631 1.91747597310703e-4,
02632 3.83495187571395e-4,
02633 7.66990318742704e-4,
02634 1.53398018628476e-3,
02635 3.06795676296598e-3,
02636 6.13588464915449e-3,
02637 1.22715382857199e-2,
02638 2.45412285229123e-2,
02639 4.90676743274181e-2,
02640 9.80171403295604e-2,
02641 1.95090322016128e-1,
02642 3.82683432365090e-1,
02643 7.07106781186546e-1,
02644 1.00000000000000,
02645 };
02646
02647 n=(int)pow(2.0f,ln);
02648
02649 k=abs(ks);
02650 l=16-ln;
02651 b3=n*k;
02652 b6=b3;
02653 b7=k;
02654 if (ks > 0) {
02655 sgn=1.0f;
02656 } else {
02657 sgn=-1.0f;
02658 rni=1.0f/(float)(n);
02659 j=1;
02660 for (i=1; i<=n; i++) {
02661 br(j)=br(j)*rni;
02662 bi(j)=bi(j)*rni;
02663 j=j+k;
02664 }
02665 }
02666
02667 L12:
02668 b6=b6/2;
02669 b5=b6;
02670 b4=2*b6;
02671 b56=b5-b6;
02672
02673 L14:
02674 tr1=br(b5+1);
02675 ti1=bi(b5+1);
02676 tr2=br(b56+1);
02677 ti2=bi(b56+1);
02678
02679 br(b5+1)=tr2-tr1;
02680 bi(b5+1)=ti2-ti1;
02681 br(b56+1)=tr1+tr2;
02682 bi(b56+1)=ti1+ti2;
02683
02684 b5=b5+b4;
02685 b56=b5-b6;
02686 if ( b5 <= b3 ) goto L14;
02687 if ( b6 == b7 ) goto L20;
02688
02689 b4=b7;
02690 cc=2.0f*pow(tab1(l),2);
02691 c=1.0f-cc;
02692 l++;
02693 ss=sgn*tab1(l);
02694 s=ss;
02695
02696 L16:
02697 b5=b6+b4;
02698 b4=2*b6;
02699 b56=b5-b6;
02700
02701 L18:
02702 tr1=br(b5+1);
02703 ti1=bi(b5+1);
02704 tr2=br(b56+1);
02705 ti2=bi(b56+1);
02706 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02707 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02708 br(b56+1)=tr1+tr2;
02709 bi(b56+1)=ti1+ti2;
02710
02711 b5=b5+b4;
02712 b56=b5-b6;
02713 if ( b5 <= b3 ) goto L18;
02714 b4=b5-b6;
02715 b5=b4-b3;
02716 c=-c;
02717 b4=b6-b5;
02718 if ( b5 < b4 ) goto L16;
02719 b4=b4+b7;
02720 if ( b4 >= b5 ) goto L12;
02721
02722 t=c-cc*c-ss*s;
02723 s=s+ss*c-cc*s;
02724 c=t;
02725 goto L16;
02726
02727 L20:
02728 ix0=b3/2;
02729 b3=b3-b7;
02730 b4=0;
02731 b5=0;
02732 b6=ix0;
02733 ix1=0;
02734 if (b6 == b7) goto EXIT;
02735
02736 L22:
02737 b4=b3-b4;
02738 b5=b3-b5;
02739 x2=br(b4+1);
02740 x3=br(b5+1);
02741 x4=bi(b4+1);
02742 x5=bi(b5+1);
02743 br(b4+1)=x3;
02744 br(b5+1)=x2;
02745 bi(b4+1)=x5;
02746 bi(b5+1)=x4;
02747 if(b6 < b4) goto L22;
02748
02749 L24:
02750 b4=b4+b7;
02751 b5=b6+b5;
02752 x2=br(b4+1);
02753 x3=br(b5+1);
02754 x4=bi(b4+1);
02755 x5=bi(b5+1);
02756 br(b4+1)=x3;
02757 br(b5+1)=x2;
02758 bi(b4+1)=x5;
02759 bi(b5+1)=x4;
02760 ix0=b6;
02761
02762 L26:
02763 ix0=ix0/2;
02764 ix1=ix1-ix0;
02765 if( ix1 >= 0) goto L26;
02766
02767 ix0=2*ix0;
02768 b4=b4+b7;
02769 ix1=ix1+ix0;
02770 b5=ix1;
02771 if ( b5 >= b4) goto L22;
02772 if ( b4 < b6) goto L24;
02773
02774 EXIT:
02775 status = 0;
02776 }
02777
02778
02779 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02780 {
02781
02782
02783 int b3,b4,b5,b6,b7,b56;
02784 int n, k, l, j, i, ix0, ix1;
02785 float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02786 int status=0;
02787
02788 const float tab1[] = {
02789 9.58737990959775e-5f,
02790 1.91747597310703e-4f,
02791 3.83495187571395e-4f,
02792 7.66990318742704e-4f,
02793 1.53398018628476e-3f,
02794 3.06795676296598e-3f,
02795 6.13588464915449e-3f,
02796 1.22715382857199e-2f,
02797 2.45412285229123e-2f,
02798 4.90676743274181e-2f,
02799 9.80171403295604e-2f,
02800 1.95090322016128e-1f,
02801 3.82683432365090e-1f,
02802 7.07106781186546e-1f,
02803 1.00000000000000f,
02804 };
02805
02806 n=(int)pow(2.0f,ln);
02807
02808 k=abs(ks);
02809 l=16-ln;
02810 b3=n*k;
02811 b6=b3;
02812 b7=k;
02813 if( ks > 0 ) {
02814 sgn=1.0f;
02815 } else {
02816 sgn=-1.0f;
02817 rni=1.0f/(float)n;
02818 j=1;
02819 for (i=1; i<=n; i++) {
02820 br(j)=br(j)*rni;
02821 bi(j)=bi(j)*rni;
02822 j=j+k;
02823 }
02824 }
02825 L12:
02826 b6=b6/2;
02827 b5=b6;
02828 b4=2*b6;
02829 b56=b5-b6;
02830 L14:
02831 tr1=br(b5+1);
02832 ti1=bi(b5+1);
02833
02834 tr2=br(b56+1);
02835 ti2=bi(b56+1);
02836
02837 br(b5+1)=tr2-tr1;
02838 bi(b5+1)=ti2-ti1;
02839 br(b56+1)=tr1+tr2;
02840 bi(b56+1)=ti1+ti2;
02841
02842 b5=b5+b4;
02843 b56=b5-b6;
02844 if ( b5 <= b3 ) goto L14;
02845 if ( b6 == b7 ) goto L20;
02846
02847 b4=b7;
02848 cc=2.0f*pow(tab1(l),2);
02849 c=1.0f-cc;
02850 l++;
02851 ss=sgn*tab1(l);
02852 s=ss;
02853 L16:
02854 b5=b6+b4;
02855 b4=2*b6;
02856 b56=b5-b6;
02857 L18:
02858 tr1=br(b5+1);
02859 ti1=bi(b5+1);
02860 tr2=br(b56+1);
02861 ti2=bi(b56+1);
02862 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02863 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02864 br(b56+1)=tr1+tr2;
02865 bi(b56+1)=ti1+ti2;
02866
02867 b5=b5+b4;
02868 b56=b5-b6;
02869 if(b5 <= b3) goto L18;
02870 b4=b5-b6;
02871 b5=b4-b3;
02872 c=-c;
02873 b4=b6-b5;
02874 if(b5 < b4) goto L16;
02875 b4=b4+b7;
02876 if(b4 >= b5) goto L12;
02877
02878 t=c-cc*c-ss*s;
02879 s=s+ss*c-cc*s;
02880 c=t;
02881 goto L16;
02882 L20:
02883 ix0=b3/2;
02884 b3=b3-b7;
02885 b4=0;
02886 b5=0;
02887 b6=ix0;
02888 ix1=0;
02889 if ( b6 == b7) goto EXIT;
02890 L22:
02891 b4=b3-b4;
02892 b5=b3-b5;
02893 x2=br(b4+1);
02894 x3=br(b5+1);
02895 x4=bi(b4+1);
02896 x5=bi(b5+1);
02897 br(b4+1)=x3;
02898 br(b5+1)=x2;
02899 bi(b4+1)=x5;
02900 bi(b5+1)=x4;
02901 if (b6 < b4) goto L22;
02902 L24:
02903 b4=b4+b7;
02904 b5=b6+b5;
02905 x2=br(b4+1);
02906 x3=br(b5+1);
02907 x4=bi(b4+1);
02908 x5=bi(b5+1);
02909 br(b4+1)=x3;
02910 br(b5+1)=x2;
02911 bi(b4+1)=x5;
02912 bi(b5+1)=x4;
02913 ix0=b6;
02914 L26:
02915 ix0=ix0/2;
02916 ix1=ix1-ix0;
02917 if(ix1 >= 0) goto L26;
02918
02919 ix0=2*ix0;
02920 b4=b4+b7;
02921 ix1=ix1+ix0;
02922 b5=ix1;
02923 if (b5 >= b4) goto L22;
02924 if (b4 < b6) goto L24;
02925 EXIT:
02926 status = 0;
02927 }
02928
02929 void Util::fftr_q(float *xcmplx, int nv)
02930 {
02931
02932
02933 int nu, inv, nu1, n, isub, n2, i1, i2, i;
02934 float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02935
02936 const float tab1[] = {
02937 9.58737990959775e-5f,
02938 1.91747597310703e-4f,
02939 3.83495187571395e-4f,
02940 7.66990318742704e-4f,
02941 1.53398018628476e-3f,
02942 3.06795676296598e-3f,
02943 6.13588464915449e-3f,
02944 1.22715382857199e-2f,
02945 2.45412285229123e-2f,
02946 4.90676743274181e-2f,
02947 9.80171403295604e-2f,
02948 1.95090322016128e-1f,
02949 3.82683432365090e-1f,
02950 7.07106781186546e-1f,
02951 1.00000000000000f,
02952 };
02953
02954 nu=abs(nv);
02955 inv=nv/nu;
02956 nu1=nu-1;
02957 n=(int)pow(2.f,nu1);
02958 isub=16-nu1;
02959
02960 ss=-tab1(isub);
02961 cc=-2.0f*pow(tab1(isub-1),2.f);
02962 c=1.0f;
02963 s=0.0f;
02964 n2=n/2;
02965 if ( inv > 0) {
02966 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02967 tr=xcmplx(1,1);
02968 ti=xcmplx(2,1);
02969 xcmplx(1,1)=tr+ti;
02970 xcmplx(2,1)=tr-ti;
02971 for (i=1;i<=n2;i++) {
02972 i1=i+1;
02973 i2=n-i+1;
02974 tr1=xcmplx(1,i1);
02975 tr2=xcmplx(1,i2);
02976 ti1=xcmplx(2,i1);
02977 ti2=xcmplx(2,i2);
02978 t=(cc*c-ss*s)+c;
02979 s=(cc*s+ss*c)+s;
02980 c=t;
02981 xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02982 xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02983 xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02984 xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02985 }
02986 } else {
02987 tr=xcmplx(1,1);
02988 ti=xcmplx(2,1);
02989 xcmplx(1,1)=0.5f*(tr+ti);
02990 xcmplx(2,1)=0.5f*(tr-ti);
02991 for (i=1; i<=n2; i++) {
02992 i1=i+1;
02993 i2=n-i+1;
02994 tr1=xcmplx(1,i1);
02995 tr2=xcmplx(1,i2);
02996 ti1=xcmplx(2,i1);
02997 ti2=xcmplx(2,i2);
02998 t=(cc*c-ss*s)+c;
02999 s=(cc*s+ss*c)+s;
03000 c=t;
03001 xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03002 xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03003 xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03004 xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03005 }
03006 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03007 }
03008 }
03009
03010
03011 void Util::fftr_d(double *xcmplx, int nv)
03012 {
03013
03014 int i1, i2, nu, inv, nu1, n, isub, n2, i;
03015 double tr1,tr2,ti1,ti2,tr,ti;
03016 double cc,c,ss,s,t;
03017 const double tab1[] = {
03018 9.58737990959775e-5,
03019 1.91747597310703e-4,
03020 3.83495187571395e-4,
03021 7.66990318742704e-4,
03022 1.53398018628476e-3,
03023 3.06795676296598e-3,
03024 6.13588464915449e-3,
03025 1.22715382857199e-2,
03026 2.45412285229123e-2,
03027 4.90676743274181e-2,
03028 9.80171403295604e-2,
03029 1.95090322016128e-1,
03030 3.82683432365090e-1,
03031 7.07106781186546e-1,
03032 1.00000000000000,
03033 };
03034
03035 nu=abs(nv);
03036 inv=nv/nu;
03037 nu1=nu-1;
03038 n=(int)pow(2.0f,nu1);
03039 isub=16-nu1;
03040 ss=-tab1(isub);
03041 cc=-2.0*pow(tab1(isub-1),2);
03042 c=1.0f;
03043 s=0.0f;
03044 n2=n/2;
03045
03046 if ( inv > 0 ) {
03047 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03048 tr=xcmplx(1,1);
03049 ti=xcmplx(2,1);
03050 xcmplx(1,1)=tr+ti;
03051 xcmplx(2,1)=tr-ti;
03052 for (i=1;i<=n2;i++) {
03053 i1=i+1;
03054 i2=n-i+1;
03055 tr1=xcmplx(1,i1);
03056 tr2=xcmplx(1,i2);
03057 ti1=xcmplx(2,i1);
03058 ti2=xcmplx(2,i2);
03059 t=(cc*c-ss*s)+c;
03060 s=(cc*s+ss*c)+s;
03061 c=t;
03062 xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03063 xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03064 xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03065 xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03066 }
03067 } else {
03068 tr=xcmplx(1,1);
03069 ti=xcmplx(2,1);
03070 xcmplx(1,1)=0.5*(tr+ti);
03071 xcmplx(2,1)=0.5*(tr-ti);
03072 for (i=1; i<=n2; i++) {
03073 i1=i+1;
03074 i2=n-i+1;
03075 tr1=xcmplx(1,i1);
03076 tr2=xcmplx(1,i2);
03077 ti1=xcmplx(2,i1);
03078 ti2=xcmplx(2,i2);
03079 t=(cc*c-ss*s)+c;
03080 s=(cc*s+ss*c)+s;
03081 c=t;
03082 xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03083 xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03084 xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03085 xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03086 }
03087 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03088 }
03089 }
03090 #undef tab1
03091 #undef xcmplx
03092 #undef br
03093 #undef bi
03094
03095
03096 void Util::Frngs(EMData* circp, vector<int> numr){
03097 int nring = numr.size()/3;
03098 float *circ = circp->get_data();
03099 int i, l;
03100 for (i=1; i<=nring;i++) {
03101
03102 #ifdef _WIN32
03103 l = (int)( log((float)numr(3,i))/log(2.0f) );
03104 #else
03105 l=(int)(log2(numr(3,i)));
03106 #endif //_WIN32
03107
03108 fftr_q(&circ(numr(2,i)),l);
03109 }
03110 }
03111
03112 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03113 int nring = numr.size()/3;
03114 float *circ = circp->get_data();
03115 int i, l;
03116 for (i=1; i<=nring;i++) {
03117
03118 #ifdef _WIN32
03119 l = (int)( log((float)numr(3,i))/log(2.0f) );
03120 #else
03121 l=(int)(log2(numr(3,i)));
03122 #endif //_WIN32
03123
03124 fftr_q(&circ(numr(2,i)),-l);
03125 }
03126 }
03127 #undef circ
03128
03129 #define b(i) b[i-1]
03130 void Util::prb1d(double *b, int npoint, float *pos) {
03131 double c2,c3;
03132 int nhalf;
03133
03134 nhalf = npoint/2 + 1;
03135 *pos = 0.0;
03136
03137 if (npoint == 7) {
03138 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03139 - 6.*b(6) + 31.*b(7);
03140 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03141 }
03142 else if (npoint == 5) {
03143 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03144 + 46.*b(5) ) / (-70.);
03145 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03146 }
03147 else if (npoint == 3) {
03148 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03149 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03150 }
03151
03152 else {
03153 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03154 - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03155 + 1092.*b(9) ) / (-4620.);
03156 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03157 - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03158 }
03159 if (c3 != 0.0) *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03160 }
03161 #undef b
03162
03163 #define circ1(i) circ1[i-1]
03164 #define circ2(i) circ2[i-1]
03165 #define t(i) t[i-1]
03166 #define q(i) q[i-1]
03167 #define b(i) b[i-1]
03168 #define t7(i) t7[i-1]
03169 Dict Util::Crosrng_e(EMData* circ1p, EMData* circ2p, vector<int> numr, int neg) {
03170
03171 int nring = numr.size()/3;
03172
03173 int maxrin = numr[numr.size()-1];
03174 double qn; float tot;
03175 float *circ1 = circ1p->get_data();
03176 float *circ2 = circ2p->get_data();
03177
03178
03179
03180
03181
03182
03183
03184
03185
03186
03187
03188 float *t;
03189 double t7[7], *q;
03190 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03191 float pos;
03192
03193 #ifdef _WIN32
03194 ip = -(int)(log((float)maxrin)/log(2.0f));
03195 #else
03196 ip = -(int) (log2(maxrin));
03197 #endif //_WIN32
03198
03199 q = (double*)calloc(maxrin, sizeof(double));
03200 t = (float*)calloc(maxrin, sizeof(float));
03201
03202
03203 for (i=1; i<=nring; i++) {
03204 numr3i = numr(3,i);
03205 numr2i = numr(2,i);
03206
03207 t(1) = (circ1(numr2i)) * circ2(numr2i);
03208
03209 if (numr3i != maxrin) {
03210
03211 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03212 t(2) = 0.0;
03213
03214 if (neg) {
03215
03216 for (j=3;j<=numr3i;j=j+2) {
03217 jc = j+numr2i-1;
03218 t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03219 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03220 }
03221 } else {
03222 for (j=3;j<=numr3i;j=j+2) {
03223 jc = j+numr2i-1;
03224 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03225 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03226 }
03227 }
03228 for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03229 } else {
03230 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03231 if (neg) {
03232
03233 for (j=3;j<=maxrin;j=j+2) {
03234 jc = j+numr2i-1;
03235 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03236 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03237 }
03238 } else {
03239 for (j=3;j<=maxrin;j=j+2) {
03240 jc = j+numr2i-1;
03241 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03242 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03243 }
03244 }
03245 for (j = 1; j <= maxrin; j++) q(j) += t(j);
03246 }
03247 }
03248
03249 fftr_d(q,ip);
03250
03251 qn = -1.0e20;
03252 for (j=1;j<=maxrin;j++) {
03253 if (q(j) >= qn) {
03254 qn = q(j); jtot = j;
03255 }
03256 }
03257
03258 for (k=-3; k<=3; k++) {
03259 j = (jtot+k+maxrin-1)%maxrin + 1;
03260 t7(k+4) = q(j);
03261 }
03262
03263 prb1d(t7,7,&pos);
03264
03265 tot = (float)jtot + pos;
03266
03267 if (q) free(q);
03268 if (t) free(t);
03269
03270 Dict retvals;
03271 retvals["qn"] = qn;
03272 retvals["tot"] = tot;
03273 return retvals;
03274 }
03275
03276 Dict Util::Crosrng_ew(EMData* circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03277
03278 int nring = numr.size()/3;
03279
03280 int maxrin = numr[numr.size()-1];
03281 double qn; float tot;
03282 float *circ1 = circ1p->get_data();
03283 float *circ2 = circ2p->get_data();
03284
03285
03286
03287
03288
03289
03290
03291
03292
03293
03294
03295 float *t;
03296 double t7[7], *q;
03297 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03298 float pos;
03299
03300 #ifdef _WIN32
03301 ip = -(int)(log((float)maxrin)/log(2.0f));
03302 #else
03303 ip = -(int) (log2(maxrin));
03304 #endif //_WIN32
03305
03306 q = (double*)calloc(maxrin, sizeof(double));
03307 t = (float*)calloc(maxrin, sizeof(float));
03308
03309
03310 for (i=1;i<=nring;i++) {
03311 numr3i = numr(3,i);
03312 numr2i = numr(2,i);
03313
03314 t(1) = circ1(numr2i) * circ2(numr2i);
03315
03316 if (numr3i != maxrin) {
03317
03318 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03319 t(2) = 0.0;
03320
03321 if (neg) {
03322
03323 for (j=3; j<=numr3i; j=j+2) {
03324 jc = j+numr2i-1;
03325 t(j) = (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03326 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03327 }
03328 } else {
03329 for (j=3; j<=numr3i; j=j+2) {
03330 jc = j+numr2i-1;
03331 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03332 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03333 }
03334 }
03335 for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03336 } else {
03337 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03338 if (neg) {
03339
03340 for (j=3; j<=maxrin; j=j+2) {
03341 jc = j+numr2i-1;
03342 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03343 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03344 }
03345 } else {
03346 for (j=3; j<=maxrin; j=j+2) {
03347 jc = j+numr2i-1;
03348 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03349 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03350 }
03351 }
03352 for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03353 }
03354 }
03355
03356 fftr_d(q,ip);
03357
03358 qn = -1.0e20;
03359 for (j=1;j<=maxrin;j++) {
03360
03361 if (q(j) >= qn) {
03362 qn = q(j);
03363 jtot = j;
03364 }
03365 }
03366
03367 for (k=-3; k<=3; k++) {
03368 j = (jtot+k+maxrin-1)%maxrin + 1;
03369 t7(k+4) = q(j);
03370 }
03371
03372 prb1d(t7,7,&pos);
03373
03374 tot = (float)jtot + pos;
03375
03376
03377 if (t) free(t);
03378
03379 Dict retvals;
03380
03381
03382 retvals["qn"] = qn;
03383 retvals["tot"] = tot;
03384
03385 if (q) free(q);
03386
03387 return retvals;
03388 }
03389
03390 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03391 int nring = numr.size()/3;
03392
03393 int maxrin = numr[numr.size()-1];
03394 double qn; float tot; double qm; float tmt;
03395 float *circ1 = circ1p->get_data();
03396 float *circ2 = circ2p->get_data();
03397
03398
03399
03400
03401
03402
03403
03404
03405
03406
03407
03408
03409 double *t, *q, t7[7];
03410
03411 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03412 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03413
03414 qn = 0.0f;
03415 qm = 0.0f;
03416 tot = 0.0f;
03417 tmt = 0.0f;
03418 #ifdef _WIN32
03419 ip = -(int)(log((float)maxrin)/log(2.0f));
03420 #else
03421 ip = -(int)(log2(maxrin));
03422 #endif //_WIN32
03423
03424
03425
03426
03427
03428 q = (double*)calloc(maxrin,sizeof(double));
03429
03430
03431
03432 t = (double*)calloc(maxrin,sizeof(double));
03433
03434
03435 for (i=1; i<=nring; i++) {
03436
03437 numr3i = numr(3,i);
03438 numr2i = numr(2,i);
03439
03440 t1 = circ1(numr2i) * circ2(numr2i);
03441 q(1) += t1;
03442 t(1) += t1;
03443
03444 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03445 if (numr3i == maxrin) {
03446 q(2) += t1;
03447 t(2) += t1;
03448 } else {
03449 q(numr3i+1) += t1;
03450 t(numr3i+1) += t1;
03451 }
03452
03453 for (j=3; j<=numr3i; j += 2) {
03454 jc = j+numr2i-1;
03455
03456
03457
03458
03459
03460
03461
03462
03463 c1 = circ1(jc);
03464 c2 = circ1(jc+1);
03465 d1 = circ2(jc);
03466 d2 = circ2(jc+1);
03467
03468 t1 = c1 * d1;
03469 t2 = c2 * d2;
03470 t3 = c1 * d2;
03471 t4 = c2 * d1;
03472
03473 q(j) += t1 + t2;
03474 q(j+1) += -t3 + t4;
03475 t(j) += t1 - t2;
03476 t(j+1) += -t3 - t4;
03477 }
03478 }
03479
03480 fftr_d(q,ip);
03481
03482 qn = -1.0e20;
03483 for (j=1; j<=maxrin; j++) {
03484 if (q(j) >= qn) {
03485 qn = q(j);
03486 jtot = j;
03487 }
03488 }
03489
03490 for (k=-3; k<=3; k++) {
03491 j = ((jtot+k+maxrin-1)%maxrin)+1;
03492 t7(k+4) = q(j);
03493 }
03494
03495
03496 prb1d(t7,7,&pos);
03497 tot = (float)(jtot)+pos;
03498
03499
03500
03501
03502 fftr_d(t,ip);
03503
03504
03505 qm = -1.0e20;
03506 for (j=1; j<=maxrin;j++) {
03507 if ( t(j) >= qm ) {
03508 qm = t(j);
03509 jtot = j;
03510 }
03511 }
03512
03513 for (k=-3; k<=3; k++) {
03514 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03515 t7(k+4) = t(j);
03516 }
03517
03518
03519
03520 prb1d(t7,7,&pos);
03521 tmt = float(jtot) + pos;
03522
03523
03524
03525 free(t);
03526 free(q);
03527
03528 Dict retvals;
03529 retvals["qn"] = qn;
03530 retvals["tot"] = tot;
03531 retvals["qm"] = qm;
03532 retvals["tmt"] = tmt;
03533 return retvals;
03534 }
03535
03536 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03537 int nring = numr.size()/3;
03538
03539 int maxrin = numr[numr.size()-1];
03540 double qn; float tot; double qm; float tmt;
03541 float *circ1 = circ1p->get_data();
03542 float *circ2 = circ2p->get_data();
03543
03544
03545
03546
03547
03548
03549
03550
03551
03552
03553
03554
03555 double *t, *q;
03556
03557 int ip, jc, numr3i, numr2i, i, j, jtot = 0;
03558 float t1, t2, t3, t4, c1, c2, d1, d2;
03559
03560 qn = 0.0f;
03561 qm = 0.0f;
03562 tot = 0.0f;
03563 tmt = 0.0f;
03564 #ifdef _WIN32
03565 ip = -(int)(log((float)maxrin)/log(2.0f));
03566 #else
03567 ip = -(int)(log2(maxrin));
03568 #endif //_WIN32
03569
03570
03571
03572
03573
03574 q = (double*)calloc(maxrin,sizeof(double));
03575
03576
03577
03578 t = (double*)calloc(maxrin,sizeof(double));
03579
03580
03581 for (i=1; i<=nring; i++) {
03582
03583 numr3i = numr(3,i);
03584 numr2i = numr(2,i);
03585
03586 t1 = circ1(numr2i) * circ2(numr2i);
03587 q(1) += t1;
03588 t(1) += t1;
03589
03590 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03591 if (numr3i == maxrin) {
03592 q(2) += t1;
03593 t(2) += t1;
03594 } else {
03595 q(numr3i+1) += t1;
03596 t(numr3i+1) += t1;
03597 }
03598
03599 for (j=3; j<=numr3i; j += 2) {
03600 jc = j+numr2i-1;
03601
03602
03603
03604
03605
03606
03607
03608
03609 c1 = circ1(jc);
03610 c2 = circ1(jc+1);
03611 d1 = circ2(jc);
03612 d2 = circ2(jc+1);
03613
03614 t1 = c1 * d1;
03615 t2 = c2 * d2;
03616 t3 = c1 * d2;
03617 t4 = c2 * d1;
03618
03619 q(j) += t1 + t2;
03620 q(j+1) += -t3 + t4;
03621 t(j) += t1 - t2;
03622 t(j+1) += -t3 - t4;
03623 }
03624 }
03625
03626 fftr_d(q,ip);
03627
03628 qn = -1.0e20;
03629
03630 int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03631 int jstep = static_cast<int>(delta/360.0*maxrin);
03632 if (jstep < 1) { jstep = 1; }
03633
03634 for (j=jstart; j<=maxrin; j+=jstep) {
03635 if (q(j) >= qn) {
03636 qn = q(j);
03637 jtot = j;
03638 }
03639 }
03640
03641
03642
03643
03644
03645
03646
03647
03648
03649
03650 tot = (float)(jtot);
03651
03652
03653 fftr_d(t,ip);
03654
03655
03656 qm = -1.0e20;
03657 for (j=jstart; j<=maxrin;j+=jstep) {
03658 if ( t(j) >= qm ) {
03659 qm = t(j);
03660 jtot = j;
03661 }
03662 }
03663
03664
03665
03666
03667
03668
03669
03670
03671
03672
03673
03674 tmt = float(jtot);
03675
03676 free(t);
03677 free(q);
03678
03679 Dict retvals;
03680 retvals["qn"] = qn;
03681 retvals["tot"] = tot;
03682 retvals["qm"] = qm;
03683 retvals["tmt"] = tmt;
03684 return retvals;
03685 }
03686
03687
03688 Dict Util::Crosrng_psi_0_180_no_mirror(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03689 int nring = numr.size()/3;
03690
03691 int maxrin = numr[numr.size()-1];
03692 double qn; float tot;
03693 float *circ1 = circ1p->get_data();
03694 float *circ2 = circ2p->get_data();
03695
03696
03697
03698
03699 double *q, t7[7];
03700
03701 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03702 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03703
03704 qn = 0.0f;
03705 tot = 0.0f;
03706 #ifdef _WIN32
03707 ip = -(int)(log((float)maxrin)/log(2.0f));
03708 #else
03709 ip = -(int)(log2(maxrin));
03710 #endif //_WIN32
03711
03712
03713
03714
03715
03716 q = (double*)calloc(maxrin,sizeof(double));
03717
03718
03719 for (i=1; i<=nring; i++) {
03720
03721 numr3i = numr(3,i);
03722 numr2i = numr(2,i);
03723
03724 t1 = circ1(numr2i) * circ2(numr2i);
03725 q(1) += t1;
03726
03727
03728 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03729 if (numr3i == maxrin) {
03730 q(2) += t1;
03731
03732 } else {
03733 q(numr3i+1) += t1;
03734 }
03735
03736 for (j=3; j<=numr3i; j += 2) {
03737 jc = j+numr2i-1;
03738
03739
03740
03741
03742
03743
03744
03745
03746 c1 = circ1(jc);
03747 c2 = circ1(jc+1);
03748 d1 = circ2(jc);
03749 d2 = circ2(jc+1);
03750
03751 t1 = c1 * d1;
03752 t2 = c2 * d2;
03753 t3 = c1 * d2;
03754 t4 = c2 * d1;
03755
03756 q(j) += t1 + t2;
03757 q(j+1) += -t3 + t4;
03758
03759 }
03760 }
03761
03762 fftr_d(q,ip);
03763
03764 int psi_range = int(psi_max/360.0*maxrin+0.5);
03765 const int psi_0 = 0;
03766 int psi_180 = int( 180.0/360.0*maxrin+0.5);
03767
03768 qn = -1.0e20;
03769 for (k=-psi_range; k<=psi_range; k++) {
03770 j = (k+psi_0+maxrin-1)%maxrin+1;
03771 if (q(j) >= qn) {
03772 qn = q(j);
03773 jtot = j;
03774 }
03775 }
03776
03777 for (k=-psi_range; k<=psi_range; k++) {
03778 j = (k+psi_180+maxrin-1)%maxrin+1;
03779 if (q(j) >= qn) {
03780 qn = q(j);
03781 jtot = j;
03782 }
03783 }
03784
03785 for (k=-3; k<=3; k++) {
03786 j = ((jtot+k+maxrin-1)%maxrin)+1;
03787 t7(k+4) = q(j);
03788 }
03789
03790
03791 prb1d(t7,7,&pos);
03792 tot = (float)(jtot)+pos;
03793
03794
03795
03796 free(q);
03797
03798 Dict retvals;
03799 retvals["qn"] = qn;
03800 retvals["tot"] = tot;
03801
03802 return retvals;
03803 }
03804
03805
03806
03807 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag, float psi_max) {
03808
03809
03810 int nring = numr.size()/3;
03811 int maxrin = numr[numr.size()-1];
03812 double qn; float tot;
03813 float *circ1 = circ1p->get_data();
03814 float *circ2 = circ2p->get_data();
03815
03816 double *q;
03817
03818 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03819 float t1, t2, t3, t4, c1, c2, d1, d2;
03820
03821 qn = 0.0f;
03822 tot = 0.0f;
03823 #ifdef _WIN32
03824 ip = -(int)(log((float)maxrin)/log(2.0f));
03825 #else
03826 ip = -(int)(log2(maxrin));
03827 #endif //_WIN32
03828
03829
03830
03831
03832 q = (double*)calloc(maxrin,sizeof(double));
03833 int neg = 1-2*flag;
03834
03835
03836 for (i=1; i<=nring; i++) {
03837
03838 numr3i = numr(3,i);
03839 numr2i = numr(2,i);
03840
03841 t1 = circ1(numr2i) * circ2(numr2i);
03842 q(1) += t1;
03843
03844 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03845 if (numr3i == maxrin) {
03846 q(2) += t1;
03847 } else {
03848 q(numr3i+1) += t1;
03849 }
03850
03851 for (j=3; j<=numr3i; j += 2) {
03852 jc = j+numr2i-1;
03853
03854
03855
03856
03857
03858 c1 = circ1(jc);
03859 c2 = circ1(jc+1);
03860 d1 = circ2(jc);
03861 d2 = circ2(jc+1);
03862
03863 t1 = c1 * d1;
03864 t3 = c1 * d2;
03865 t2 = c2 * d2;
03866 t4 = c2 * d1;
03867
03868 q(j) += t1 + t2*neg;
03869 q(j+1) += -t3 + t4*neg;
03870 }
03871 }
03872
03873
03874 fftr_d(q,ip);
03875
03876 qn = -1.0e20;
03877 int psi_pos = int(psi/360.0*maxrin+0.5);
03878 const int psi_range = int(psi_max/360.0*maxrin + 0.5);
03879
03880 for (k=-psi_range; k<=psi_range; k++) {
03881 j = (k+psi_pos+maxrin-1)%maxrin+1;
03882 if (q(j) >= qn) {
03883 qn = q(j);
03884 jtot = j;
03885 }
03886 }
03887
03888 tot = (float)(jtot);
03889 free(q);
03890
03891 Dict retvals;
03892 retvals["qn"] = qn;
03893 retvals["tot"] = tot;
03894 return retvals;
03895 }
03896
03897 Dict Util::Crosrng_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, float psi_max) {
03898
03899
03900 int nring = numr.size()/3;
03901 int maxrin = numr[numr.size()-1];
03902 double qn; float tot; double qm; float tmt;
03903 float *circ1 = circ1p->get_data();
03904 float *circ2 = circ2p->get_data();
03905
03906 double *t, *q;
03907
03908 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03909 float t1, t2, t3, t4, c1, c2, d1, d2;
03910
03911 qn = 0.0f;
03912 qm = 0.0f;
03913 tot = 0.0f;
03914 tmt = 0.0f;
03915 #ifdef _WIN32
03916 ip = -(int)(log((float)maxrin)/log(2.0f));
03917 #else
03918 ip = -(int)(log2(maxrin));
03919 #endif //_WIN32
03920
03921
03922
03923
03924 q = (double*)calloc(maxrin,sizeof(double));
03925
03926
03927
03928 t = (double*)calloc(maxrin,sizeof(double));
03929
03930
03931
03932 for (i=1; i<=nring; i++) {
03933
03934 numr3i = numr(3,i);
03935 numr2i = numr(2,i);
03936
03937 t1 = circ1(numr2i) * circ2(numr2i);
03938 q(1) += t1;
03939 t(1) += t1;
03940
03941 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03942 if (numr3i == maxrin) {
03943 q(2) += t1;
03944 t(2) += t1;
03945 } else {
03946 q(numr3i+1) += t1;
03947 t(numr3i+1) += t1;
03948 }
03949
03950 for (j=3; j<=numr3i; j += 2) {
03951 jc = j+numr2i-1;
03952
03953
03954
03955
03956
03957 c1 = circ1(jc);
03958 c2 = circ1(jc+1);
03959 d1 = circ2(jc);
03960 d2 = circ2(jc+1);
03961
03962 t1 = c1 * d1;
03963 t3 = c1 * d2;
03964 t2 = c2 * d2;
03965 t4 = c2 * d1;
03966
03967 q(j) += t1 + t2;
03968 q(j+1) += -t3 + t4;
03969 t(j) += t1 - t2;
03970 t(j+1) += -t3 - t4;
03971 }
03972 }
03973
03974
03975 fftr_d(q,ip);
03976
03977 qn = -1.0e20;
03978 int psi_pos = int(psi/360.0*maxrin+0.5);
03979 const int psi_range = int(psi_max/360.0*maxrin + 0.5);
03980
03981 for (k=-psi_range; k<=psi_range; k++) {
03982 j = (k+psi_pos+maxrin-1)%maxrin+1;
03983 if (q(j) >= qn) {
03984 qn = q(j);
03985 jtot = j;
03986 }
03987 }
03988
03989 tot = (float)(jtot);
03990 free(q);
03991
03992
03993 fftr_d(t,ip);
03994
03995 qm = -1.0e20;
03996
03997 for (k=-psi_range; k<=psi_range; k++) {
03998 j = (k+psi_pos+maxrin-1)%maxrin+1;
03999 if (t(j) >= qm) {
04000 qm = t(j);
04001 jtot = j;
04002 }
04003 }
04004
04005 tmt = (float)(jtot);
04006 free(t);
04007
04008 Dict retvals;
04009 retvals["qn"] = qn;
04010 retvals["tot"] = tot;
04011 retvals["qm"] = qm;
04012 retvals["tmt"] = tmt;
04013 return retvals;
04014 }
04015
04016 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
04017 int nring = numr.size()/3;
04018
04019 int maxrin = numr[numr.size()-1];
04020 double qn; float tot;
04021 float *circ1 = circ1p->get_data();
04022 float *circ2 = circ2p->get_data();
04023
04024
04025
04026
04027
04028
04029
04030
04031
04032
04033
04034
04035 double *q, t7[7];
04036
04037 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04038 float c1, c2, d1, d2, pos;
04039
04040 qn = 0.0;
04041 tot = 0.0;
04042 #ifdef _WIN32
04043 ip = -(int)(log((float)maxrin)/log(2.0f));
04044 #else
04045 ip = -(int)(log2(maxrin));
04046 #endif //_WIN32
04047
04048
04049
04050
04051
04052 q = (double*)calloc(maxrin,sizeof(double));
04053
04054
04055 for (i=1; i<=nring; i++) {
04056
04057 numr3i = numr(3,i);
04058 numr2i = numr(2,i);
04059
04060 q(1) += circ1(numr2i) * circ2(numr2i);
04061
04062 if (numr3i == maxrin) q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04063 else q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04064
04065 for (j=3; j<=numr3i; j += 2) {
04066 jc = j+numr2i-1;
04067
04068
04069
04070
04071
04072 c1 = circ1(jc);
04073 c2 = circ1(jc+1);
04074 d1 = circ2(jc);
04075 d2 = circ2(jc+1);
04076
04077 q(j) += c1 * d1 + c2 * d2;
04078 q(j+1) += -c1 * d2 + c2 * d1;
04079 }
04080 }
04081
04082 fftr_d(q,ip);
04083
04084 qn = -1.0e20;
04085 for (j=1; j<=maxrin; j++) {
04086 if (q(j) >= qn) {
04087 qn = q(j);
04088 jtot = j;
04089 }
04090 }
04091
04092 for (k=-3; k<=3; k++) {
04093 j = ((jtot+k+maxrin-1)%maxrin)+1;
04094 t7(k+4) = q(j);
04095 }
04096
04097
04098 prb1d(t7,7,&pos);
04099 tot = (float)(jtot)+pos;
04100
04101
04102
04103 free(q);
04104
04105 Dict retvals;
04106 retvals["qn"] = qn;
04107 retvals["tot"] = tot;
04108 return retvals;
04109 }
04110
04111 #define dout(i,j) dout[i+maxrin*j]
04112 #define circ1b(i) circ1b[i-1]
04113 #define circ2b(i) circ2b[i-1]
04114
04115 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04116
04117
04118
04119 int ip, jc, numr3i, numr2i, i, j;
04120 float t1, t2, t3, t4, c1, c2, d1, d2;
04121
04122 int nring = numr.size()/3;
04123
04124 int maxrin = numr[numr.size()-1];
04125
04126 float* circ1b = circ1->get_data();
04127 float* circ2b = circ2->get_data();
04128
04129
04130 double *t, *q;
04131
04132 q = (double*)calloc(maxrin,sizeof(double));
04133 t = (double*)calloc(maxrin,sizeof(double));
04134
04135 #ifdef _WIN32
04136 ip = -(int)(log((float)maxrin)/log(2.0f));
04137 #else
04138 ip = -(int)(log2(maxrin));
04139 #endif //_WIN32
04140
04141
04142
04143
04144
04145
04146
04147 for (i=1; i<=nring; i++) {
04148
04149 numr3i = numr(3,i);
04150 numr2i = numr(2,i);
04151
04152 t1 = circ1b(numr2i) * circ2b(numr2i);
04153 q(1) = q(1)+t1;
04154 t(1) = t(1)+t1;
04155
04156 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04157 if (numr3i == maxrin) {
04158 q(2) += t1;
04159 t(2) += t1;
04160 } else {
04161 q(numr3i+1) += t1;
04162 t(numr3i+1) += t1;
04163 }
04164
04165 for (j=3; j<=numr3i; j=j+2) {
04166 jc = j+numr2i-1;
04167
04168 c1 = circ1b(jc);
04169 c2 = circ1b(jc+1);
04170 d1 = circ2b(jc);
04171 d2 = circ2b(jc+1);
04172
04173 t1 = c1 * d1;
04174 t3 = c1 * d2;
04175 t2 = c2 * d2;
04176 t4 = c2 * d1;
04177
04178 q(j) += t1 + t2;
04179 q(j+1) += - t3 + t4;
04180 t(j) += t1 - t2;
04181 t(j+1) += - t3 - t4;
04182 }
04183 }
04184
04185
04186 fftr_d(q,ip);
04187
04188
04189 fftr_d(t,ip);
04190
04191 EMData* out = new EMData();
04192 out->set_size(maxrin,2,1);
04193 float *dout = out->get_data();
04194 for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04195
04196
04197
04198 free(t);
04199 free(q);
04200 return out;
04201 }
04202
04203
04204 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04205
04206 int maxrin = numr[numr.size()-1];
04207
04208 vector<float> r(2*maxrin);
04209
04210 Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04211
04212 return r;
04213 }
04214
04215 #define dout(i,j) dout[i+maxrin*j]
04216 #define circ1b(i) circ1b[i-1]
04217 #define circ2b(i) circ2b[i-1]
04218
04219 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04220
04221
04222
04223 int ip, jc, numr3i, numr2i, i, j;
04224 float t1, t2, t3, t4, c1, c2, d1, d2;
04225
04226 int nring = numr.size()/3;
04227
04228 int maxrin = numr[numr.size()-1];
04229
04230 float* circ1b = circ1->get_data();
04231 float* circ2b = circ2->get_data();
04232
04233 #ifdef _WIN32
04234 ip = -(int)(log((float)maxrin)/log(2.0f));
04235 #else
04236 ip = -(int)(log2(maxrin));
04237 #endif //_WIN32
04238 for (int i=1; i<=maxrin; i++) {q(i) = 0.0f; t(i) = 0.0f;}
04239
04240
04241
04242
04243
04244 for (i=1; i<=nring; i++) {
04245
04246 numr3i = numr(3,i);
04247 numr2i = numr(2,i);
04248
04249 t1 = circ1b(numr2i) * circ2b(numr2i);
04250 q(1) += t1;
04251 t(1) += t1;
04252
04253 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04254 if (numr3i == maxrin) {
04255 q(2) += t1;
04256 t(2) += t1;
04257 } else {
04258 q(numr3i+1) += t1;
04259 t(numr3i+1) += t1;
04260 }
04261
04262 for (j=3; j<=numr3i; j=j+2) {
04263 jc = j+numr2i-1;
04264
04265 c1 = circ1b(jc);
04266 c2 = circ1b(jc+1);
04267 d1 = circ2b(jc);
04268 d2 = circ2b(jc+1);
04269
04270 t1 = c1 * d1;
04271 t3 = c1 * d2;
04272 t2 = c2 * d2;
04273 t4 = c2 * d1;
04274
04275 q(j) += t1 + t2;
04276 q(j+1) += -t3 + t4;
04277 t(j) += t1 - t2;
04278 t(j+1) += -t3 - t4;
04279 }
04280 }
04281
04282 fftr_q(q,ip);
04283
04284
04285
04286 fftr_q(t,ip);
04287 }
04288
04289
04290
04291 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04292 {
04293
04294 int ip, jc, numr3i, numr2i, i, j;
04295 float t1, t2, t3, t4, c1, c2, d1, d2;
04296
04297 int nring = numr.size()/3;
04298 int maxrin = numr[numr.size()-1];
04299
04300 float* circ1b = circ1->get_data();
04301 float* circ2b = circ2->get_data();
04302
04303 double *q;
04304
04305 q = (double*)calloc(maxrin,sizeof(double));
04306
04307 #ifdef _WIN32
04308 ip = -(int)(log((float)maxrin)/log(2.0f));
04309 #else
04310 ip = -(int)(log2(maxrin));
04311 #endif //_WIN32
04312
04313
04314
04315 for (i=1;i<=nring;i++) {
04316
04317 numr3i = numr(3,i);
04318 numr2i = numr(2,i);
04319
04320 t1 = circ1b(numr2i) * circ2b(numr2i);
04321 q(1) = q(1)+t1;
04322
04323 if (numr3i == maxrin) {
04324 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04325 q(2) = q(2)+t1;
04326 } else {
04327 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04328 q(numr3i+1) = q(numr3i+1)+t1;
04329 }
04330
04331 for (j=3;j<=numr3i;j=j+2) {
04332 jc = j+numr2i-1;
04333
04334 c1 = circ1b(jc);
04335 c2 = circ1b(jc+1);
04336 d1 = circ2b(jc);
04337 d2 = circ2b(jc+1);
04338
04339 t1 = c1 * d1;
04340 t3 = c1 * d2;
04341 t2 = c2 * d2;
04342 t4 = c2 * d1;
04343
04344 q(j) = q(j) + t1 + t2;
04345 q(j+1) = q(j+1) - t3 + t4;
04346 }
04347 }
04348
04349
04350 fftr_d(q,ip);
04351
04352 EMData* out = new EMData();
04353 out->set_size(maxrin,1,1);
04354 float *dout = out->get_data();
04355 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04356 free(q);
04357 return out;
04358
04359 }
04360
04361
04362 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04363 {
04364
04365 int ip, jc, numr3i, numr2i, i, j;
04366 float t1, t2, t3, t4, c1, c2, d1, d2;
04367
04368 int nring = numr.size()/3;
04369 int maxrin = numr[numr.size()-1];
04370
04371 float* circ1b = circ1->get_data();
04372 float* circ2b = circ2->get_data();
04373
04374 double *t;
04375
04376 t = (double*)calloc(maxrin,sizeof(double));
04377
04378 #ifdef _WIN32
04379 ip = -(int)(log((float)maxrin)/log(2.0f));
04380 #else
04381 ip = -(int)(log2(maxrin));
04382 #endif //_WIN32
04383
04384
04385
04386 for (i=1;i<=nring;i++) {
04387
04388 numr3i = numr(3,i);
04389 numr2i = numr(2,i);
04390
04391 t1 = circ1b(numr2i) * circ2b(numr2i);
04392 t(1) = t(1)+t1;
04393
04394 if (numr3i == maxrin) {
04395 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04396 t(2) = t(2)+t1;
04397 }
04398
04399 for (j=3;j<=numr3i;j=j+2) {
04400 jc = j+numr2i-1;
04401
04402 c1 = circ1b(jc);
04403 c2 = circ1b(jc+1);
04404 d1 = circ2b(jc);
04405 d2 = circ2b(jc+1);
04406
04407 t1 = c1 * d1;
04408 t3 = c1 * d2;
04409 t2 = c2 * d2;
04410 t4 = c2 * d1;
04411
04412 t(j) = t(j) + t1 - t2;
04413 t(j+1) = t(j+1) - t3 - t4;
04414 }
04415 }
04416
04417
04418 fftr_d(t,ip);
04419
04420 EMData* out = new EMData();
04421 out->set_size(maxrin,1,1);
04422 float *dout = out->get_data();
04423 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04424 free(t);
04425 return out;
04426
04427 }
04428
04429 #undef circ1b
04430 #undef circ2b
04431 #undef dout
04432
04433 #undef circ1
04434 #undef circ2
04435 #undef t
04436 #undef q
04437 #undef b
04438 #undef t7
04439
04440
04441 #define QUADPI 3.141592653589793238462643383279502884197
04442 #define PI2 2*QUADPI
04443
04444 float Util::ener(EMData* ave, vector<int> numr) {
04445 ENTERFUNC;
04446 long double ener,en;
04447
04448 int nring = numr.size()/3;
04449 float *aveptr = ave->get_data();
04450
04451 ener = 0.0;
04452 for (int i=1; i<=nring; i++) {
04453 int numr3i = numr(3,i);
04454 int np = numr(2,i)-1;
04455 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04456 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04457 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04458 ener += en/numr3i;
04459 }
04460 EXITFUNC;
04461 return static_cast<float>(ener);
04462 }
04463
04464 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04465 ENTERFUNC;
04466 long double ener, en;
04467 float arg, cs, si;
04468
04469 int nima = data.size();
04470 int nring = numr.size()/3;
04471 int maxrin = numr(3,nring);
04472
04473 ener = 0.0;
04474 for (int i=1; i<=nring; i++) {
04475 int numr3i = numr(3,i);
04476 int np = numr(2,i)-1;
04477 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04478 float temp1 = 0.0, temp2 = 0.0;
04479 for (int kk=0; kk<nima; kk++) {
04480 float *ptr = data[kk]->get_data();
04481 temp1 += ptr[np];
04482 temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04483 }
04484 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04485 for (int j=2; j<numr3i; j+=2) {
04486 float tempr = 0.0, tempi = 0.0;
04487 for (int kk=0; kk<nima; kk++) {
04488 float *ptr = data[kk]->get_data();
04489 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04490 cs = cos(arg);
04491 si = sin(arg);
04492 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04493 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04494 }
04495 en += tq*(tempr*tempr+tempi*tempi);
04496 }
04497 ener += en/numr3i;
04498 }
04499 EXITFUNC;
04500 return static_cast<float>(ener);
04501 }
04502
04503 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04504 int nring = numr.size()/3;
04505 float *ave = avep->get_data();
04506 float *dat = datp->get_data();
04507 int i, j, numr3i, np;
04508 float arg, cs, si;
04509 int maxrin = numr(3,nring);
04510 if(mirror == 1) {
04511 for (i=1; i<=nring; i++) {
04512 numr3i = numr(3,i);
04513 np = numr(2,i)-1;
04514 ave[np] += dat[np];
04515 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04516 for (j=2; j<numr3i; j=j+2) {
04517 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04518 cs = cos(arg);
04519 si = sin(arg);
04520
04521 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04522 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04523 }
04524 }
04525 } else {
04526 for (i=1; i<=nring; i++) {
04527 numr3i = numr(3,i);
04528 np = numr(2,i)-1;
04529 ave[np] += dat[np];
04530 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04531 for (j=2; j<numr3i; j=j+2) {
04532 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04533 cs = cos(arg);
04534 si = sin(arg);
04535
04536 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04537 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04538 }
04539 }
04540 }
04541 avep->update();
04542 EXITFUNC;
04543 }
04544
04545 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04546 int nring = numr.size()/3;
04547 float *ave = avep->get_data();
04548 float *dat = datp->get_data();
04549 int i, j, numr3i, np;
04550 float arg, cs, si;
04551 int maxrin = numr(3,nring);
04552 if(mirror == 1) {
04553 for (i=1; i<=nring; i++) {
04554 numr3i = numr(3,i);
04555 np = numr(2,i)-1;
04556 ave[np] -= dat[np];
04557 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04558 for (j=2; j<numr3i; j=j+2) {
04559 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04560 cs = cos(arg);
04561 si = sin(arg);
04562
04563 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04564 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04565 }
04566 }
04567 } else {
04568 for (i=1; i<=nring; i++) {
04569 numr3i = numr(3,i);
04570 np = numr(2,i)-1;
04571 ave[np] -= dat[np];
04572 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04573 for (j=2; j<numr3i; j=j+2) {
04574 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04575 cs = cos(arg);
04576 si = sin(arg);
04577
04578 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04579 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04580 }
04581 }
04582 }
04583 avep->update();
04584 EXITFUNC;
04585 }
04586
04587
04588 #undef QUADPI
04589 #undef PI2
04590
04591 #undef numr
04592 #undef circ
04593
04594
04595 #define QUADPI 3.141592653589793238462643383279502884197
04596 #define PI2 QUADPI*2
04597 #define deg_rad QUADPI/180.0
04598 #define rad_deg 180.0/QUADPI
04599
04600 struct ori_t
04601 {
04602 int iphi;
04603 int itht;
04604 int id;
04605 };
04606
04607
04608 struct cmpang
04609 {
04610 bool operator()( const ori_t& a, const ori_t& b )
04611 {
04612 if( a.itht != b.itht )
04613 {
04614 return a.itht < b.itht;
04615 }
04616
04617 return a.iphi < b.iphi;
04618 }
04619 };
04620
04621
04622 vector<double> Util::cml_weights(const vector<float>& cml){
04623 static const int NBIN = 100;
04624 int nline=cml.size()/2;
04625 vector<double> weights(nline);
04626
04627 vector<ori_t> angs(nline);
04628 for( int i=0; i < nline; ++i ) {
04629 angs[i].iphi = int( NBIN*cml[2*i] );
04630 angs[i].itht = int( NBIN*cml[2*i+1] );
04631 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04632 angs[i].id = i;
04633 }
04634
04635
04636
04637 std::sort( angs.begin(), angs.end(), cmpang() );
04638
04639 vector<float> newphi;
04640 vector<float> newtht;
04641 vector< vector<int> > indices;
04642
04643 int curt_iphi = -1;
04644 int curt_itht = -1;
04645 for(unsigned int i=0 ;i < angs.size(); ++i ) {
04646 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04647 Assert( indices.size() > 0 );
04648 indices.back().push_back(angs[i].id);
04649 } else {
04650 curt_iphi = angs[i].iphi;
04651 curt_itht = angs[i].itht;
04652
04653 newphi.push_back( float(curt_iphi)/NBIN );
04654 newtht.push_back( float(curt_itht)/NBIN );
04655 indices.push_back( vector<int>(1,angs[i].id) );
04656 }
04657 }
04658
04659
04660
04661
04662 int num_agl = newphi.size();
04663
04664 if(num_agl>2) {
04665 vector<double> w=Util::vrdg(newphi, newtht);
04666
04667 Assert( w.size()==newphi.size() );
04668 Assert( indices.size()==newphi.size() );
04669
04670 for(unsigned int i=0; i < newphi.size(); ++i ) {
04671
04672
04673
04674
04675
04676
04677
04678
04679 for(unsigned int j=0; j < indices[i].size(); ++j ) {
04680 int id = indices[i][j];
04681 weights[id] = w[i]/indices[i].size();
04682
04683 }
04684
04685
04686
04687 }
04688 } else {
04689 cout<<"warning in Util.cml_weights"<<endl;
04690 double val = PI2/float(nline);
04691 for(int i=0; i<nline; i++) weights[i]=val;
04692 }
04693
04694 return weights;
04695
04696 }
04697
04698
04699
04700
04701
04702 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04703 {
04704 int i;
04705 int nx=img->get_xsize();
04706 float *img_ptr = img->get_data();
04707 float *line_ptr = line->get_data();
04708 for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04709 img->update();
04710 }
04711
04712 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04713 int j;
04714 int nx = sino->get_xsize();
04715 int i = nx * pos_line;
04716 float r1, r2;
04717 float *line_ptr = line->get_data();
04718 float *sino_ptr = sino->get_data();
04719 for (j=ilf;j<=ihf; j += 2) {
04720 r1 = line_ptr[j];
04721 r2 = line_ptr[j + 1];
04722 sino_ptr[i + j - ilf] = r1;
04723 sino_ptr[i + j - ilf + 1] = r2;
04724 sino_ptr[i + nx * nblines + j - ilf] = r1;
04725 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04726 }
04727 sino->update();
04728 }
04729
04730 vector<double> Util::cml_init_rot(vector<float> Ori){
04731 int nb_ori = Ori.size() / 4;
04732 int i, ind;
04733 float ph, th, ps;
04734 double cph, cth, cps, sph, sth, sps;
04735 vector<double> Rot(nb_ori*9);
04736 for (i=0; i<nb_ori; ++i){
04737 ind = i*4;
04738
04739 ph = Ori[ind+2]-90;
04740 th = Ori[ind+1];
04741 ps = Ori[ind]+90;
04742 ph *= deg_rad;
04743 th *= deg_rad;
04744 ps *= deg_rad;
04745
04746 cph = cos(ph);
04747 cth = cos(th);
04748 cps = cos(ps);
04749 sph = sin(ph);
04750 sth = sin(th);
04751 sps = sin(ps);
04752
04753 ind = i*9;
04754 Rot[ind] = cph*cps-cth*sps*sph;
04755 Rot[ind+1] = cph*sps+cth*cps*sph;
04756 Rot[ind+2] = sth*sph;
04757 Rot[ind+3] = -sph*cps-cth*sps*cph;
04758 Rot[ind+4] = -sph*sps+cth*cps*cph;
04759 Rot[ind+5] = sth*cph;
04760 Rot[ind+6] = sth*sps;
04761 Rot[ind+7] = -sth*cps;
04762 Rot[ind+8] = cth;
04763 }
04764
04765 return Rot;
04766 }
04767
04768 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04769 float ph, ps;
04770 double cph, cth, cps, sph, sth, sps;
04771 int ind = iprj*9;
04772
04773 ph = nps-90;
04774 ps = nph+90;
04775 ph *= deg_rad;
04776 th *= deg_rad;
04777 ps *= deg_rad;
04778
04779 cph = cos(ph);
04780 cth = cos(th);
04781 cps = cos(ps);
04782 sph = sin(ph);
04783 sth = sin(th);
04784 sps = sin(ps);
04785
04786 Rot[ind] = (float)(cph*cps-cth*sps*sph);
04787 Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04788 Rot[ind+2] = (float)(sth*sph);
04789 Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04790 Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04791 Rot[ind+5] = (float)(sth*cph);
04792 Rot[ind+6] = (float)(sth*sps);
04793 Rot[ind+7] = (float)(-sth*cps);
04794 Rot[ind+8] = (float)(cth);
04795
04796 return Rot;
04797 }
04798
04799 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04800 vector<int> com(2*(n_prj - 1));
04801 int a = i_prj*9;
04802 int i, b, c;
04803 int n1=0, n2=0;
04804 float vmax = 1 - 1.0e-6f;
04805 double r11, r12, r13, r23, r31, r32, r33;
04806
04807 c = 0;
04808 for (i=0; i<n_prj; ++i){
04809 if (i!=i_prj){
04810 b = i*9;
04811
04812 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04813 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04814 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04815 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04816 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04817 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04818 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04819 if (r33 > vmax) {
04820 n2 = 270;
04821 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04822 }
04823 else if (r33 < -vmax) {
04824 n2 = 270;
04825 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04826 } else {
04827 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04828 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04829 if (n1 < 0) {n1 += 360;}
04830 if (n2 <= 0) {n2 = abs(n2);}
04831 else {n2 = 360 - n2;}
04832 }
04833
04834 if (n1 >= 360){n1 = n1 % 360;}
04835 if (n2 >= 360){n2 = n2 % 360;}
04836
04837
04838 b = c*2;
04839 com[b] = n1;
04840 com[b+1] = n2;
04841 ++c;
04842 }
04843 }
04844
04845 return com;
04846
04847 }
04848
04849 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int, int n_lines) {
04850 vector<int> com(2*n_lines);
04851 int a=0, b, c, l;
04852 int n1=0, n2=0, mem=-1;
04853 float vmax = 1 - 1.0e-6f;
04854 double r11, r12, r13, r23, r31, r32, r33;
04855 c = 0;
04856 for (l=0; l<n_lines; ++l){
04857 c = 2*l;
04858 if (seq[c]!=mem){
04859 mem = seq[c];
04860 a = seq[c]*9;
04861 }
04862 b = seq[c+1]*9;
04863
04864
04865 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04866 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04867 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04868 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04869 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04870 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04871 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04872 if (r33 > vmax) {
04873 n2 = 270;
04874 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04875 }
04876 else if (r33 < -vmax) {
04877 n2 = 270;
04878 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04879 } else {
04880 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04881 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04882 if (n1 < 0) {n1 += 360;}
04883 if (n2 <= 0) {n2 = abs(n2);}
04884 else {n2 = 360 - n2;}
04885 }
04886 if (n1 >= 360){n1 = n1 % 360;}
04887 if (n2 >= 360){n2 = n2 % 360;}
04888
04889
04890 com[c] = n1;
04891 com[c+1] = n2;
04892 }
04893
04894 return com;
04895
04896 }
04897
04898 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int, int nlines){
04899
04900 vector<double> cml(2*nlines);
04901 float ph1, th1;
04902 float ph2, th2;
04903 double nx, ny, nz;
04904 double norm;
04905 double sth1=0, sph1=0, cth1=0, cph1=0;
04906 double sth2, sph2, cth2, cph2;
04907 int l, ind, c;
04908 int mem = -1;
04909 for (l=0; l<nlines; ++l){
04910 c = 2*l;
04911 if (seq[c]!=mem){
04912 mem = seq[c];
04913 ind = 4*seq[c];
04914 ph1 = Ori[ind]*deg_rad;
04915 th1 = Ori[ind+1]*deg_rad;
04916 sth1 = sin(th1);
04917 sph1 = sin(ph1);
04918 cth1 = cos(th1);
04919 cph1 = cos(ph1);
04920 }
04921 ind = 4*seq[c+1];
04922 ph2 = Ori[ind]*deg_rad;
04923 th2 = Ori[ind+1]*deg_rad;
04924 sth2 = sin(th2);
04925 cth2 = cos(th2);
04926 sph2 = sin(ph2);
04927 cph2 = cos(ph2);
04928
04929 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04930 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04931 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04932 norm = sqrt(nx*nx+ny*ny+nz*nz);
04933 nx /= norm;
04934 ny /= norm;
04935 nz /= norm;
04936
04937 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04938
04939 cml[c+1] = acos(nz);
04940 if (cml[c+1] == 0) {cml[c] = 0;}
04941 else {
04942 cml[c+1] *= rad_deg;
04943 if (cml[c+1] > 89.99) {cml[c+1] = 89.99;}
04944 cml[c] = rad_deg * atan2(nx, ny);
04945 cml[c] = fmod(360 + cml[c], 360);
04946
04947 }
04948 }
04949
04950 return cml;
04951 }
04952
04953 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04954 double res = 0;
04955 double buf = 0;
04956 float* line_1;
04957 float* line_2;
04958 int i, n, ind;
04959 int lnlen = data[0]->get_xsize();
04960 for (n=0; n<n_lines; ++n) {
04961 ind = n*2;
04962 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04963 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04964 buf = 0;
04965 for (i=0; i<lnlen; ++i) {
04966 buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04967 }
04968 res += buf * weights[n];
04969 }
04970
04971 return res;
04972
04973 }
04974
04975 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04976 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04977
04978
04979
04980 vector<double> res(2);
04981 int lnlen = data[0]->get_xsize();
04982 int end = 2*(n_prj-1);
04983 double disc, buf, bdisc, tmp;
04984 int n, i, ipsi, ind, bipsi, c;
04985 float* line_1;
04986 float* line_2;
04987 bdisc = 1.0e6;
04988 bipsi = -1;
04989
04990 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04991
04992 disc = 0;
04993 c = 0;
04994 for (n=0; n<n_prj; ++n) {
04995 if(n!=iprj) {
04996 ind = 2*c;
04997 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04998 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04999 buf = 0;
05000 for (i=0; i<lnlen; ++i) {
05001 tmp = line_1[i]-line_2[i];
05002 buf += tmp*tmp;
05003 }
05004 disc += buf * weights[iw[c]];
05005 ++c;
05006 }
05007 }
05008
05009 if (disc <= bdisc) {
05010 bdisc = disc;
05011 bipsi = ipsi;
05012 }
05013
05014 for (i=0; i<end; i+=2){
05015 com[i] += d_psi;
05016 if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05017 }
05018 }
05019 res[0] = bdisc;
05020 res[1] = float(bipsi);
05021
05022 return res;
05023 }
05024
05025 vector<double> Util::cml_spin_psi_now(const vector<EMData*>& data, vector<int> com, \
05026 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
05027
05028
05029
05030 vector<double> res(2);
05031 int lnlen = data[0]->get_xsize();
05032 int end = 2*(n_prj-1);
05033 double disc, buf, bdisc, tmp;
05034 int n, i, ipsi, ind, bipsi, c;
05035 float* line_1;
05036 float* line_2;
05037 bdisc = 1.0e6;
05038 bipsi = -1;
05039
05040 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
05041
05042 disc = 0;
05043 c = 0;
05044 for (n=0; n<n_prj; ++n) {
05045 if(n!=iprj) {
05046 ind = 2*c;
05047 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
05048 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
05049 buf = 0;
05050 for (i=0; i<lnlen; ++i) {
05051 tmp = line_1[i]-line_2[i];
05052 buf += tmp*tmp;
05053 }
05054 disc += buf;
05055 ++c;
05056 }
05057 }
05058
05059 if (disc <= bdisc) {
05060 bdisc = disc;
05061 bipsi = ipsi;
05062 }
05063
05064 for (i=0; i<end; i+=2){
05065 com[i] += d_psi;
05066 if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05067 }
05068 }
05069 res[0] = bdisc;
05070 res[1] = float(bipsi);
05071
05072 return res;
05073 }
05074
05075 #undef QUADPI
05076 #undef PI2
05077 #undef deg_rad
05078 #undef rad_deg
05079
05080
05081
05082
05083
05084
05085 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
05086 ENTERFUNC;
05087
05088 int nima = data.size();
05089 vector<float> res(nima);
05090 double result = 0.;
05091 double valmin = 1.0e20;
05092 int valpos = -1;
05093
05094 for (int kk=0; kk<nima; kk++){
05095 result = 0;
05096
05097 float *y_data = data[kk]->get_data();
05098 float *x_data = image->get_data();
05099 long totsize = image->get_xsize()*image->get_ysize();
05100 for (long i = 0; i < totsize; i++) {
05101 double temp = x_data[i]- y_data[i];
05102 result += temp*temp;
05103 }
05104 result /= totsize;
05105 res[kk] = (float)result;
05106
05107 if(result<valmin) {valmin = result; valpos = kk;}
05108
05109 }
05110
05111 Dict retvals;
05112 retvals["dist"] = res;
05113 retvals["pos"] = valpos;
05114
05115 EXITFUNC;
05116 return retvals;
05117
05118 }
05119
05120 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05121 ENTERFUNC;
05122
05123 int nima = data.size();
05124 vector<float> res(nima);
05125 double result = 0.;
05126 double valmin = 1.0e20;
05127 int valpos = -1;
05128
05129 for (int kk=0; kk<nima; kk++){
05130 result = 0;
05131
05132
05133 float *y_data = data[kk]->get_data();
05134 float *x_data = image->get_data();
05135
05136
05137 int nx = data[kk]->get_xsize();
05138 int ny = data[kk]->get_ysize();
05139 nx = (nx - 2 + data[kk]->is_fftodd());
05140 int lsd2 = (nx + 2 - nx%2) ;
05141
05142 int ixb = 2*((nx+1)%2);
05143 int iyb = ny%2;
05144 int iz = 0;
05145
05146 for ( int iy = 0; iy <= ny-1; iy++) {
05147 for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05148 int ii = ix + (iy + iz * ny)* lsd2;
05149 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05150 }
05151 }
05152 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05153 int ii = (iy + iz * ny)* lsd2;
05154 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05155 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05156 }
05157 if(nx%2 == 0) {
05158 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05159 int ii = lsd2 - 2 + (iy + iz * ny)* lsd2;
05160 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05161 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05162 }
05163
05164 }
05165 result *= 2;
05166 result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05167 if(ny%2 == 0) {
05168 int ii = (ny/2 + iz * ny)* lsd2;
05169 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05170 }
05171 if(nx%2 == 0) {
05172 int ii = lsd2 - 2 + (0 + iz * ny)* lsd2;
05173 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05174 if(ny%2 == 0) {
05175 int ii = lsd2 - 2 +(ny/2 + iz * ny)* lsd2;
05176 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05177 }
05178 }
05179
05180 result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05181 res[kk] = (float)result;
05182
05183 if(result<valmin) {valmin = result; valpos = kk;}
05184
05185 }
05186
05187 Dict retvals;
05188 retvals["dist"] = res;
05189 retvals["pos"] = valpos;
05190
05191 EXITFUNC;
05192 return retvals;
05193 }
05194
05195 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05196 long int d2 = group2[s2 - 1] - group2[0];
05197 long int p2 = 0;
05198 long int i1 = 0;
05199 long int i2 = 0;
05200 long int max = 0;
05201 long int cont = 0;
05202 long int i = 0;
05203 int stop1 = 0;
05204 int stop2 = 0;
05205
05206 for (i=0; i<s1; i++) {
05207 p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05208 if (p2 >= s2) {p2 = s2 - 1;}
05209 i1 = p2;
05210 i2 = p2;
05211 max = s2;
05212 if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05213
05214 stop1 = 0;
05215 stop2 = 0;
05216 while (max--) {
05217 if (group1[i] == group2[i1]) {
05218 if (flag) {stb[cont] = group1[i];}
05219 cont++;
05220 break;
05221 }
05222 if (group2[i1] < group1[i]) {stop1=1;}
05223 if (group1[i] == group2[i2]) {
05224 if (flag) {stb[cont] = group1[i];}
05225 cont++;
05226 break;
05227 }
05228 if (group2[i2] > group1[i]) {stop2=1;}
05229
05230
05231 if (stop1 & stop2) {break;}
05232 i1--;
05233 i2++;
05234 if (i1 < 0) {i1 = 0;}
05235 if (i2 >= s2) {i2 = s2 - 1;}
05236 }
05237
05238 }
05239
05240 return cont;
05241 }
05242
05243
05244
05245 #define old_ptr(i,j,k) old_ptr[i+(j+(k*ny))*(size_t)nx]
05246 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*(size_t)new_nx]
05247 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05248 {
05249
05250 if (!img) {
05251 throw NullPointerException("NULL input image");
05252 }
05253
05254
05255
05256 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05257
05258
05259
05260
05261 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)
05262 {
05263 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05264 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05265 }
05266
05267
05268
05269
05270 int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05271
05272
05273
05274
05275 int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05276 int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05277 int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05278 if(r1>1){r1=1;}
05279 if(r2>1){r2=1;}
05280 if(r3>1){r3=1;}
05281 int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05282
05283
05284
05285 EMData* img2 = new EMData();
05286 img2->set_size(new_nx,new_ny,new_nz);
05287 float *new_ptr = img2->get_data();
05288 float *old_ptr = img->get_data();
05289 int iptr, jptr, kptr = 0;
05290 for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05291 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05292 for (int i=new_st_x; i<nx; i+=x_step) {
05293 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05294 iptr++;}
05295 jptr++;}
05296 kptr++;}
05297 img2->update();
05298 return img2;
05299 }
05300 #undef old_ptr
05301 #undef new_ptr
05302
05303 #define inp(i,j,k) inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*(size_t)nx]
05304 #define outp(i,j,k) outp[i+(j+(k*new_ny))*(size_t)new_nx]
05305 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05306 {
05307
05308 if (!img) throw NullPointerException("NULL input image");
05309
05310
05311
05312 int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05313
05314
05315
05316 if(new_nx>nx || new_ny>ny || new_nz>nz)
05317 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size. 1");
05318 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)
05319 throw ImageDimensionException("The offset inconsistent with the input image size. 2");
05320 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))))
05321 throw ImageDimensionException("The offset inconsistent with the input image size. 3");
05322
05323
05324
05325 int new_st_x = nx/2-new_nx/2 + x_offset,
05326 new_st_y = ny/2-new_ny/2 + y_offset,
05327 new_st_z = nz/2-new_nz/2 + z_offset;
05328
05329
05330
05331 if (new_st_x<0 || new_st_y<0 || new_st_z<0)
05332 throw ImageDimensionException("The offset inconsistent with the input image size. 4");
05333
05334
05335 EMData* wind = img->copy_empty_head();
05336 wind->set_size(new_nx, new_ny, new_nz);
05337 float *outp=wind->get_data();
05338 float *inp=img->get_data();
05339
05340 for (int k=0; k<new_nz; k++)
05341 for(int j=0; j<new_ny; j++)
05342 for(int i=0; i<new_nx; i++)
05343 outp(i,j,k) = inp(i,j,k);
05344 wind->update();
05345 return wind;
05346 }
05347 #undef inp
05348 #undef outp
05349
05350 #define inp(i,j,k) inp[i+(j+(k*ny))*(size_t)nx]
05351 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*(size_t)new_nx]
05352 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)
05353 {
05354
05355 if (!img) throw NullPointerException("NULL input image");
05356
05357
05358
05359 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05360
05361
05362
05363 if(new_nx<nx || new_ny<ny || new_nz<nz)
05364 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05365 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)
05366 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05367 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))))
05368 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05369
05370
05371 EMData* pading = img->copy_head();
05372 pading->set_size(new_nx, new_ny, new_nz);
05373 float *inp = img->get_data();
05374 float *outp = pading->get_data();
05375
05376
05377
05378
05379 float background;
05380
05381 if (strcmp(params,"average")==0) background = img->get_attr("mean");
05382 else if (strcmp(params,"circumference")==0) {
05383 float sum1=0.0f;
05384 size_t cnt=0;
05385 for(int i=0;i<nx;i++) {
05386 sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05387 cnt+=2;
05388 }
05389 if(nz-1 == 0) {
05390 for (int j=1;j<ny-1;j++) {
05391 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05392 cnt+=2;
05393 }
05394 } else {
05395 for (int k=1;k<nz-1;k++) {
05396 for (int j=1;j<ny-1;j++) {
05397 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05398 cnt+=2;
05399 }
05400 }
05401 }
05402 background = sum1/cnt;
05403 } else {
05404 background = static_cast<float>( atof( params ) );
05405 }
05406
05407
05408
05409 int new_st_x=0,new_st_y=0,new_st_z=0;
05410 for (int k=0;k<new_nz;k++)
05411 for(int j=0;j<new_ny;j++)
05412 for (int i=0;i<new_nx;i++)
05413 outp(i,j,k)=background;
05414
05415
05416
05417 new_st_x=int((new_nx/2-nx/2) + x_offset);
05418 new_st_y=int((new_ny/2-ny/2) + y_offset);
05419 new_st_z=int((new_nz/2-nz/2) + z_offset);
05420
05421
05422 for (int k=0;k<nz;k++)
05423 for(int j=0;j<ny;j++)
05424 for(int i=0;i<nx;i++)
05425 outp(i,j,k)=inp(i,j,k);
05426 pading->update();
05427 return pading;
05428 }
05429 #undef inp
05430 #undef outp
05431
05432
05433 void Util::colreverse(float* beg, float* end, int nx) {
05434 float* tmp = new float[nx];
05435 int n = (end - beg)/nx;
05436 int nhalf = n/2;
05437 for (int i = 0; i < nhalf; i++) {
05438
05439 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05440 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05441 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05442 }
05443 delete[] tmp;
05444 }
05445
05446 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05447 {
05448 int nxy = nx*ny;
05449 colreverse(beg, end, nxy);
05450 }
05451
05452
05453 void Util::cyclicshift(EMData *image, Dict params) {
05454
05455 if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05456
05457 int dx = params["dx"];
05458 int dy = params["dy"];
05459 int dz = params["dz"];
05460
05461
05462 int nx = image->get_xsize();
05463 dx %= nx;
05464 if (dx < 0) dx += nx;
05465 int ny = image->get_ysize();
05466 dy %= ny;
05467 if (dy < 0) dy += ny;
05468 int nz = image->get_zsize();
05469 dz %= nz;
05470 if (dz < 0) dz += nz;
05471
05472 int mx = -(dx - nx);
05473 int my = -(dy - ny);
05474 int mz = -(dz - nz);
05475
05476 float* data = image->get_data();
05477
05478 if (mx != 0) {
05479 for (int iz = 0; iz < nz; iz++)
05480 for (int iy = 0; iy < ny; iy++) {
05481
05482 size_t offset = nx*iy + (size_t)nx*ny*iz;
05483 reverse(&data[offset],&data[offset+mx]);
05484 reverse(&data[offset+mx],&data[offset+nx]);
05485 reverse(&data[offset],&data[offset+nx]);
05486 }
05487 }
05488
05489 if (my != 0) {
05490 for (int iz = 0; iz < nz; iz++) {
05491 size_t offset = (size_t)nx*ny*iz;
05492 colreverse(&data[offset], &data[offset + my*nx], nx);
05493 colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05494 colreverse(&data[offset], &data[offset + ny*nx], nx);
05495 }
05496 }
05497 if (mz != 0) {
05498 slicereverse(&data[0], &data[(size_t)mz*ny*nx], nx, ny);
05499 slicereverse(&data[mz*ny*nx], &data[(size_t)nz*ny*nx], nx, ny);
05500 slicereverse(&data[0], &data[(size_t)nz*ny*nx], nx ,ny);
05501 }
05502 image->update();
05503 }
05504
05505
05506
05507
05508 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05509 {
05510 if (image->is_complex())
05511 throw ImageFormatException("Cannot do histogram on Fourier image");
05512
05513 float *imageptr=0, *maskptr=0;
05514 int nx=image->get_xsize();
05515 int ny=image->get_ysize();
05516 int nz=image->get_zsize();
05517
05518 if(mask != NULL){
05519 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05520 throw ImageDimensionException("The size of mask image should be of same size as the input image");
05521 maskptr =mask->get_data();
05522 }
05523 if( nbins == 0) nbins = nx;
05524 vector <float> freq(2*nbins, 0.0);
05525
05526 imageptr=image->get_data();
05527 if( hmin == hmax ) {
05528 if(mask == NULL) {
05529 hmax = image->get_attr("maximum");
05530 hmin = image->get_attr("minimum");
05531 } else {
05532 bool First = true;
05533 for (size_t i = 0;i < (size_t)nx*ny*nz; i++) {
05534 if (maskptr[i]>=0.5f) {
05535 if(First) {
05536 hmax = imageptr[i];
05537 hmin = imageptr[i];
05538 First = false;
05539 } else {
05540 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05541 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05542 }
05543 }
05544 }
05545 }
05546 }
05547 float hdiff = hmax - hmin;
05548 float ff = (nbins-1)/hdiff;
05549 for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05550 if(mask == NULL) {
05551 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05552 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05553 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05554 }
05555 } else {
05556 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05557 if(maskptr[i] >= 0.5) {
05558 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05559 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05560 }
05561 }
05562 }
05563 return freq;
05564 }
05565
05566 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05567 {
05568
05569 if (img->is_complex() || ref->is_complex())
05570 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05571
05572 if(mask != NULL){
05573 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05574 throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05575
05576
05577
05578 size_t size_ref = ((size_t)(ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05579 size_t size_img = ((size_t)(img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05580
05581
05582
05583 float *ref_ptr = ref->get_data();
05584 float ref_h_min = ref->get_attr("minimum");
05585 float ref_h_max = ref->get_attr("maximum");
05586 float ref_h_avg = ref->get_attr("mean");
05587 float ref_h_sig = ref->get_attr("sigma");
05588
05589
05590
05591 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05592
05593 vector<float> img_data = Util::infomask(img, mask);
05594 float img_avg = img_data[0];
05595 float img_sig = img_data[1];
05596
05597
05598 int cnt=0;
05599 for(size_t i=0;i<size_img;++i)
05600 if (mask_ptr[i]>0.5f)
05601 cnt++;
05602
05603
05604
05605 float ref_h_diff = ref_h_max - ref_h_min;
05606
05607 #ifdef _WIN32
05608 int hist_len = _cpp_min((unsigned long)size_ref/16,_cpp_min((unsigned long)size_img/16,256lu));
05609 #else
05610 int hist_len = std::min((unsigned long)size_ref/16,std::min((unsigned long)size_img/16,256lu));
05611 #endif //_WIN32
05612
05613 float *ref_freq_bin = new float[3*hist_len];
05614
05615
05616 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05617
05618 for (size_t i = 0;i < size_ref;++i) {
05619 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05620 ref_freq_bin[L]++;
05621 }
05622 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05623
05624
05625 float A = ref_h_sig/img_sig;
05626 float B = ref_h_avg - (A*img_avg);
05627
05628 vector<float> args;
05629 args.push_back(A);
05630 args.push_back(B);
05631
05632 vector<float> scale;
05633 scale.push_back(1.e-7f*A);
05634 scale.push_back(-1.e-7f*B);
05635
05636 vector<float> ref_freq_hist;
05637 for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05638
05639 vector<float> data;
05640 data.push_back(ref_h_diff);
05641 data.push_back(ref_h_min);
05642
05643 Dict parameter;
05644
05645
05646 parameter["args"] = args;
05647 parameter["scale"]= scale;
05648 parameter["data"] = data;
05649 parameter["ref_freq_bin"] = ref_freq_hist;
05650 parameter["size_img"]=(double)size_img;
05651 parameter["hist_len"]=hist_len;
05652
05653
05654 return parameter;
05655 }
05656
05657
05658 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)
05659 {
05660 float *img_ptr = img->get_data();
05661 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05662
05663 int *img_freq_bin = new int[3*hist_len];
05664 for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05665 for(size_t i = 0;i < size_img;++i) {
05666 if(mask_ptr[i] > 0.5f) {
05667 float img_xn = img_ptr[i]*PA + PB;
05668 int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05669 if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05670 }
05671 }
05672 int freq_hist = 0;
05673
05674 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);
05675 freq_hist = (-freq_hist);
05676 return static_cast<float>(freq_hist);
05677 }
05678
05679 #define QUADPI 3.141592653589793238462643383279502884197
05680 #define DGR_TO_RAD QUADPI/180
05681 #define DM(I) DM [I-1]
05682 #define SS(I) SS [I-1]
05683 Dict Util::CANG(float PHI,float THETA,float PSI)
05684 {
05685 double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05686 vector<float> DM,SS;
05687
05688 for(int i =0;i<9;i++) DM.push_back(0);
05689
05690 for(int i =0;i<6;i++) SS.push_back(0);
05691
05692 CPHI = cos(double(PHI)*DGR_TO_RAD);
05693 SPHI = sin(double(PHI)*DGR_TO_RAD);
05694 CTHE = cos(double(THETA)*DGR_TO_RAD);
05695 STHE = sin(double(THETA)*DGR_TO_RAD);
05696 CPSI = cos(double(PSI)*DGR_TO_RAD);
05697 SPSI = sin(double(PSI)*DGR_TO_RAD);
05698
05699 SS(1) = float(CPHI);
05700 SS(2) = float(SPHI);
05701 SS(3) = float(CTHE);
05702 SS(4) = float(STHE);
05703 SS(5) = float(CPSI);
05704 SS(6) = float(SPSI);
05705
05706 DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05707 DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05708 DM(3) = float(-STHE*CPSI);
05709 DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05710 DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05711 DM(6) = float(STHE*SPSI);
05712 DM(7) = float(STHE*CPHI);
05713 DM(8) = float(STHE*SPHI);
05714 DM(9) = float(CTHE);
05715
05716 Dict DMnSS;
05717 DMnSS["DM"] = DM;
05718 DMnSS["SS"] = SS;
05719
05720 return(DMnSS);
05721 }
05722 #undef SS
05723 #undef DM
05724 #undef QUADPI
05725 #undef DGR_TO_RAD
05726
05727 #define DM(I) DM[I-1]
05728 #define B(i,j) Bptr[i-1+((j-1)*NSAM)]
05729 #define CUBE(i,j,k) CUBEptr[(i-1)+((j-1)+((k-1)*NY3D))*(size_t)NX3D]
05730
05731 void Util::BPCQ(EMData *B,EMData *CUBE, vector<float> DM)
05732 {
05733
05734 float *Bptr = B->get_data();
05735 float *CUBEptr = CUBE->get_data();
05736
05737 int NSAM,NROW,NX3D,NY3D,NZC,KZ,IQX,IQY,LDPX,LDPY,LDPZ,LDPNMX,LDPNMY,NZ1;
05738 float DIPX,DIPY,XB,YB,XBB,YBB;
05739
05740 Transform * t = B->get_attr("xform.projection");
05741 Dict d = t->get_params("spider");
05742 if(t) {delete t; t=0;}
05743
05744 float x_shift = d[ "tx" ];
05745 float y_shift = d[ "ty" ];
05746 x_shift = -x_shift;
05747 y_shift = -y_shift;
05748
05749 NSAM = B->get_xsize();
05750 NROW = B->get_ysize();
05751 NX3D = CUBE->get_xsize();
05752 NY3D = CUBE->get_ysize();
05753 NZC = CUBE->get_zsize();
05754
05755
05756 LDPX = NX3D/2 +1;
05757 LDPY = NY3D/2 +1;
05758 LDPZ = NZC/2 +1;
05759 LDPNMX = NSAM/2 +1;
05760 LDPNMY = NROW/2 +1;
05761 NZ1 = 1;
05762
05763 for(int K=1;K<=NZC;K++) {
05764 KZ=K-1+NZ1;
05765 for(int J=1;J<=NY3D;J++) {
05766 XBB = (1-LDPX)*DM(1)+(J-LDPY)*DM(2)+(KZ-LDPZ)*DM(3);
05767 YBB = (1-LDPX)*DM(4)+(J-LDPY)*DM(5)+(KZ-LDPZ)*DM(6);
05768 for(int I=1;I<=NX3D;I++) {
05769 XB = (I-1)*DM(1)+XBB-x_shift;
05770 IQX = int(XB+float(LDPNMX));
05771 if (IQX <1 || IQX >= NSAM) continue;
05772 YB = (I-1)*DM(4)+YBB-y_shift;
05773 IQY = int(YB+float(LDPNMY));
05774 if (IQY<1 || IQY>=NROW) continue;
05775 DIPX = XB+LDPNMX-IQX;
05776 DIPY = YB+LDPNMY-IQY;
05777
05778 CUBE(I,J,K) = CUBE(I,J,K)+B(IQX,IQY)+DIPY*(B(IQX,IQY+1)-B(IQX,IQY))+DIPX*(B(IQX+1,IQY)-B(IQX,IQY)+DIPY*(B(IQX+1,IQY+1)-B(IQX+1,IQY)-B(IQX,IQY+1)+B(IQX,IQY)));
05779 }
05780 }
05781 }
05782 }
05783
05784 #undef DM
05785 #undef B
05786 #undef CUBE
05787
05788
05789 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
05790 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
05791 #define SS(I,J) SS [I-1 + (J-1)*6]
05792
05793 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05794 {
05795 int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05796 float WW,OX,OY;
05797
05798 NSAM = PROJ->get_xsize();
05799 NROW = PROJ->get_ysize();
05800 int ntotal = NSAM*NROW;
05801 float q = 2.0f;
05802 float qt = 8.0f/q;
05803
05804 int ipad = 1;
05805 NSAM *= ipad;
05806 NROW *= ipad;
05807 NNNN = NSAM+2-(NSAM%2);
05808 int NX2 = NSAM/2;
05809 NR2 = NROW/2;
05810
05811 NANG = int(SS.size())/6;
05812
05813 EMData* W = new EMData();
05814 int Wnx = NNNN/2;
05815 W->set_size(Wnx,NROW,1);
05816 W->to_zero();
05817 float *Wptr = W->get_data();
05818 float *PROJptr = PROJ->get_data();
05819 for (L=1; L<=NANG; L++) {
05820 float tmp1 = SS(3,K)*SS(4,L)*(SS(1,K)*SS(1,L) + SS(2,K)*SS(2,L)) - SS(3,L)*SS(4,K);
05821 float tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) );
05822 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05823 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05824 if(OX < 0.0f) {
05825 OX = -OX;
05826 OY = -OY;
05827 }
05828
05829 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f ) {
05830 for(int J=1;J<=NROW;J++) {
05831 JY = (J-1);
05832 if(JY > NR2) JY -= NROW;
05833 #ifdef _WIN32
05834 int xma = _cpp_min(int(0.5f+(q-JY*OY)/OX),NX2);
05835 int xmi = _cpp_max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05836 #else
05837 int xma = std::min(int(0.5f+(q-JY*OY)/OX),NX2);
05838 int xmi = std::max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05839 #endif //_WIN32
05840 if( xmi <= xma) {
05841 for(int I=xmi;I<=xma;I++) {
05842 float Y = fabs(OX*I + OY*JY);
05843 W(I+1,J) += exp(-qt*Y*Y);
05844
05845 }
05846 }
05847 }
05848 } else {
05849 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J) += 1.0f;
05850 }
05851 }
05852 EMData* proj_in = PROJ;
05853
05854 PROJ = PROJ->norm_pad( false, ipad);
05855 PROJ->do_fft_inplace();
05856 PROJ->update();
05857
05858 PROJptr = PROJ->get_data();
05859
05860 float WNRMinv,temp;
05861 float osnr = 1.0f/SNR;
05862 WNRMinv = 1.0f/W(1,1);
05863 for(int J=1;J<=NROW;J++) {
05864 JY = J-1;
05865 if( JY > NR2) JY -= NROW;
05866 float sy = JY;
05867 sy /= NROW;
05868 sy *= sy;
05869 for(int I=1;I<=NNNN;I+=2) {
05870 KX = (I+1)/2;
05871 temp = W(KX,J)*WNRMinv;
05872 WW = temp/(temp*temp + osnr);
05873
05874 float sx = KX-1;
05875 sx /= NSAM;
05876 WW *= exp(qt*(sy + sx*sx));
05877 PROJ(I,J) *= WW;
05878 PROJ(I+1,J) *= WW;
05879 }
05880 }
05881 delete W; W = 0;
05882 PROJ->do_ift_inplace();
05883 PROJ->depad();
05884
05885 float* data_src = PROJ->get_data();
05886 float* data_dst = proj_in->get_data();
05887
05888 for( int i=0; i < ntotal; ++i ) data_dst[i] = data_src[i];
05889
05890 proj_in->update();
05891
05892 delete PROJ;
05893 }
05894
05895
05896
05897
05898
05899
05900
05901
05902
05903
05904
05905
05906
05907
05908
05909
05910
05911
05912
05913
05914
05915
05916
05917
05918
05919
05920
05921
05922
05923
05924
05925
05926
05927
05928
05929
05930
05931
05932
05933
05934
05935
05936
05937
05938
05939
05940
05941
05942
05943
05944
05945
05946
05947
05948
05949
05950
05951
05952
05953
05954
05955
05956
05957
05958
05959
05960
05961
05962
05963
05964
05965
05966
05967
05968
05969
05970
05971
05972
05973 #undef PROJ
05974 #undef W
05975 #undef SS
05976
05977 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
05978 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
05979 #define SS(I,J) SS [I-1 + (J-1)*6]
05980 #define RI(i,j) RI [(i-1) + ((j-1)*3)]
05981 #define CC(i) CC [i-1]
05982 #define CP(i) CP [i-1]
05983 #define VP(i) VP [i-1]
05984 #define VV(i) VV [i-1]
05985 #define AMAX1(i,j) i>j?i:j
05986 #define AMIN1(i,j) i<j?i:j
05987 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
05988 {
05989 float rad2deg =(180.0f/3.1415926f);
05990 float deg2rad = (3.1415926f/180.0f);
05991
05992 int NSAM,NROW,NNNN,NR2,NANG,L,JY;
05993
05994 NSAM = PROJ->get_xsize();
05995 NROW = PROJ->get_ysize();
05996 NNNN = NSAM+2-(NSAM%2);
05997 NR2 = NROW/2;
05998 NANG = int(SS.size())/6;
05999
06000 float RI[9];
06001 RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
06002 RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
06003 RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
06004 RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
06005 RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
06006 RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
06007 RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
06008 RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
06009 RI(3,3)=SS(3,NUMP);
06010
06011 float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
06012
06013 EMData* W = new EMData();
06014 int Wnx = NNNN/2;
06015 W->set_size(NNNN/2,NROW,1);
06016 W->to_one();
06017 float *Wptr = W->get_data();
06018
06019 float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
06020
06021 for (L=1; L<=NANG; L++) {
06022 if (L != NUMP) {
06023 CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
06024 CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
06025 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);
06026
06027 TMP = sqrt(CC(1)*CC(1) + CC(2)*CC(2) + CC(3)*CC(3));
06028 CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
06029 ALPHA=rad2deg*float(asin(CCN));
06030 if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
06031 if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
06032 if(ALPHA<1.0E-6) {
06033 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
06034 } else {
06035 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
06036 CC(1) = CC(1)/CCN;CC(2) = CC(2)/CCN;CC(3) = CC(3)/CCN;
06037 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
06038 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
06039 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
06040 CP(1) = 0.0;CP(2) = 0.0;
06041 VP(1) = 0.0;VP(2) = 0.0;
06042
06043 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
06044 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
06045 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
06046 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
06047
06048 TMP = CP(1)*VP(2)-CP(2)*VP(1);
06049
06050
06051 TMP = AMAX1(1.0E-4f,fabs(TMP));
06052 float tmpinv = 1.0f/TMP;
06053 for(int J=1;J<=NROW;J++) {
06054 JY = (J-1);
06055 if (JY>NR2) JY=JY-NROW;
06056 for(int I=1;I<=NNNN/2;I++) {
06057 FV = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
06058 RT = 1.0f-FV/FM;
06059 W(I,J) += ((RT>0.0f)*RT);
06060 }
06061 }
06062 }
06063 }
06064 }
06065
06066 EMData* proj_in = PROJ;
06067
06068 PROJ = PROJ->norm_pad( false, 1);
06069 PROJ->do_fft_inplace();
06070 PROJ->update();
06071 float *PROJptr = PROJ->get_data();
06072
06073 int KX;
06074 float WW;
06075 for(int J=1; J<=NROW; J++)
06076 for(int I=1; I<=NNNN; I+=2) {
06077 KX = (I+1)/2;
06078 WW = 1.0f/W(KX,J);
06079 PROJ(I,J) = PROJ(I,J)*WW;
06080 PROJ(I+1,J) = PROJ(I+1,J)*WW;
06081 }
06082 delete W; W = 0;
06083 PROJ->do_ift_inplace();
06084 PROJ->depad();
06085
06086 float* data_src = PROJ->get_data();
06087 float* data_dst = proj_in->get_data();
06088
06089 int ntotal = NSAM*NROW;
06090 for( int i=0; i < ntotal; ++i ) data_dst[i] = data_src[i];
06091
06092 proj_in->update();
06093 delete PROJ;
06094 }
06095 #undef AMAX1
06096 #undef AMIN1
06097 #undef RI
06098 #undef CC
06099 #undef CP
06100 #undef VV
06101 #undef VP
06102
06103
06104 #undef W
06105 #undef SS
06106 #undef PROJ
06107
06108 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06109 {
06110 float cst = cs*1.0e7f;
06111
06112 wgh /= 100.0;
06113 float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06114 float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06115 float ak2 = ak*ak;
06116 float g1 = dzz*1.0e4f*lambda*ak2;
06117 float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06118
06119 float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06120 if(b_factor != 0.0f) ctfv *= exp(-b_factor*ak2/4.0f);
06121
06122 return ctfv;
06123 }
06124
06125 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06126 {
06127
06128
06129
06130 int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();
06131
06132
06133
06134 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06135 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06136
06137 size_t i, size = (size_t)nx*ny*nz;
06138
06139 float* img_ptr = image->get_data();
06140 float* mask_ptr = mask->get_data();
06141
06142 int ln=0;
06143 for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06144
06145 EMData* new_image = new EMData();
06146 new_image->set_size(ln,1,1);
06147 float *new_ptr = new_image->get_data();
06148
06149 ln=-1;
06150 for(i = 0;i < size;i++){
06151 if(mask_ptr[i] > 0.5f) {
06152 ln++;
06153 new_ptr[ln]=img_ptr[i];
06154 }
06155 }
06156
06157 return new_image;
06158 }
06159
06160 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06161 {
06162
06163
06164
06165 if(mask == NULL)
06166 throw ImageDimensionException("The mask cannot be an null image");
06167
06168
06169
06170
06171 int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06172
06173 size_t i,size = (size_t)nx*ny*nz;
06174
06175 EMData *new_image = new EMData();
06176 new_image->set_size(nx,ny,nz);
06177 float *new_ptr = new_image->get_data();
06178 float *mask_ptr = mask->get_data();
06179 float *img_ptr = image->get_data();
06180 int count = 0;
06181 float sum_under_mask = 0.0 ;
06182 for(i = 0;i < size;i++){
06183 if(mask_ptr[i] > 0.5f){
06184 new_ptr[i] = img_ptr[count];
06185 sum_under_mask += img_ptr[count];
06186 count++;
06187 if( count > image->get_xsize() ) {
06188 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06189 }
06190 }
06191 }
06192
06193 if( count > image->get_xsize() ) {
06194 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06195 }
06196
06197 float avg_under_mask = sum_under_mask / count;
06198 for(i = 0;i < size;i++) {
06199 if(mask_ptr[i] <= 0.5f) new_ptr[i] = avg_under_mask;
06200 }
06201 new_image->update();
06202 return new_image;
06203 }
06204
06205
06206
06207 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06208 {
06209 vector<float>new_peak;
06210 int n1=peak1.size()/3;
06211 float p_size2=p_size*p_size;
06212 for (int i=0;i<n1;++i) {
06213 vector<float>::iterator it2= peak1.begin()+3*i;
06214 bool push_back1=true;
06215 int n2=peak2.size()/3;
06216
06217
06218 if(n2 ==0) {
06219 new_peak.push_back(*it2);
06220 new_peak.push_back(*(it2+1));
06221 new_peak.push_back(*(it2+2));
06222 } else {
06223 int j=0;
06224 while (j< n2-1 ) {
06225 vector<float>::iterator it3= peak2.begin()+3*j;
06226 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06227 if(d2< p_size2 ) {
06228 if( (*it2)<(*it3) ) {
06229 new_peak.push_back(*it3);
06230 new_peak.push_back(*(it3+1));
06231 new_peak.push_back(*(it3+2));
06232 peak2.erase(it3);
06233 peak2.erase(it3);
06234 peak2.erase(it3);
06235 push_back1=false;
06236 } else {
06237 peak2.erase(it3);
06238 peak2.erase(it3);
06239 peak2.erase(it3);
06240 }
06241 } else j=j+1;
06242 n2=peak2.size()/3;
06243 }
06244 if(push_back1) {
06245 new_peak.push_back(*it2);
06246 new_peak.push_back(*(it2+1));
06247 new_peak.push_back(*(it2+2));
06248 }
06249 }
06250 }
06251 return new_peak;
06252 }
06253
06254 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06255 {
06256
06257
06258
06259
06260
06261 ENTERFUNC;
06262
06263 int i;
06264
06265
06266 for ( i = 0 ; i < n * n ; i++ ) eigvec[i] = covmat[i];
06267
06268 char NEEDV = 'V';
06269 char UPLO = 'U';
06270 int lwork = -1;
06271 int info = 0;
06272 float *work, wsize;
06273
06274
06275 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06276 lwork = (int)wsize;
06277
06278 work = (float *)calloc(lwork, sizeof(float));
06279
06280 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06281 free(work);
06282 EXITFUNC;
06283 return info;
06284 }
06285
06286 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06287 {
06288
06289 ENTERFUNC;
06290 int len = covmatpy.size();
06291 float *eigvec;
06292 float *eigval;
06293 float *covmat;
06294 int status = 0;
06295 eigval = (float*)calloc(ncov,sizeof(float));
06296 eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06297 covmat = (float*)calloc(ncov*ncov, sizeof(float));
06298
06299 const float *covmat_ptr;
06300 covmat_ptr = &covmatpy[0];
06301 for(int i=0;i<len;i++){
06302 covmat[i] = covmat_ptr[i];
06303 }
06304
06305 status = Util::coveig(ncov, covmat, eigval, eigvec);
06306
06307 vector<float> eigval_py(ncov);
06308 const float *eigval_ptr;
06309 eigval_ptr = &eigval[0];
06310 for(int i=0;i<ncov;i++){
06311 eigval_py[i] = eigval_ptr[i];
06312 }
06313
06314 vector<float> eigvec_py(ncov*ncov);
06315 const float *eigvec_ptr;
06316 eigvec_ptr = &eigvec[0];
06317 for(int i=0;i<ncov*ncov;i++){
06318 eigvec_py[i] = eigvec_ptr[i];
06319 }
06320
06321 Dict res;
06322 res["eigval"] = eigval_py;
06323 res["eigvec"] = eigvec_py;
06324
06325 EXITFUNC;
06326 return res;
06327 }
06328
06329 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06330 {
06331 int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06332
06333 k=(int)pw.size();
06334 l=0;
06335 m=k;
06336 n2=n+2;
06337 n1=n+1;
06338 klmd=k+l+m;
06339 klm2d= k+l+m+2;
06340 nklmd=k+l+m+n;
06341 n2d=n+2;
06342
06343 n_larg=klmd*2;
06344 klm2d=n_larg+klm2d;
06345 klmd=n_larg+klmd;
06346 nklmd=n_larg+nklmd;
06347 int size_q=klm2d*n2d;
06348 int size_cu=nklmd*2;
06349 static int i__;
06350
06351 double *q ;
06352 double *x ;
06353 double *res;
06354 double *cu;
06355 float *q2;
06356 float *pw_;
06357 long int *iu;
06358 double *s;
06359 q = (double*)calloc(size_q,sizeof(double));
06360 x = (double*)calloc(n2d,sizeof(double));
06361 res = (double*)calloc(klmd,sizeof(double));
06362 cu =(double*)calloc(size_cu,sizeof(double));
06363 s = (double*)calloc(klmd,sizeof(double));
06364 q2 = (float*)calloc(size_q,sizeof(float));
06365 iu = (long int*)calloc(size_cu,sizeof(long int));
06366 pw_ = (float*)calloc(k,sizeof(float));
06367
06368 for( i__ =0;i__<k;++i__)
06369 {
06370 pw_[i__]=log(pw[i__]); }
06371 long int l_k=k;
06372 long int l_n=n;
06373 long int l_iswi=iswi;
06374 vector<float> cl1_res;
06375 cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06376 free(q);
06377 free(x);
06378 free(res);
06379 free(s);
06380 free(cu);
06381 free(q2);
06382 free(iu);
06383 free(pw_);
06384 return cl1_res;
06385 }
06386 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)
06387 {
06388 long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06389 float r__1;
06390 int tmp__i;
06391 long int i__, j;
06392 --s;
06393 --res;
06394 iu -= 3;
06395 cu -= 3;
06396 --x;
06397 long int klm2d;
06398 klm2d= *k+*k+2;
06399 klm2d=klm2d+klm2d;
06400 q_dim1 = klm2d;
06401 q_offset = 1 + q_dim1;
06402 q -= q_offset;
06403 q2_dim1 = klm2d;
06404 q2_offset = 1 + q2_dim1;
06405 q2 -= q2_offset;
06406 i__2=0;
06407 i__1 = *n - 1;
06408 tmp__i=0;
06409 for (j = 1; j <= i__1; ++j) {
06410 i__2 = *k;
06411 tmp__i+=1;
06412 for (i__ = 1; i__ <= i__2; ++i__) {
06413 r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06414 q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06415 }
06416 }
06417 for (i__ = 1; i__ <= i__2; ++i__)
06418 { q2[i__ + *n * q2_dim1] = 1.f;
06419 q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06420 }
06421 vector<float> fit_res;
06422 fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06423 return fit_res;
06424 }
06425 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)
06426 {
06427
06428 long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06429
06430
06431 long int i__, j, m, n1, ii, jj;
06432 double tmp;
06433 vector<float> p;
06434 --x;
06435 q_dim1 = *klm2d;
06436 q_offset = 1 + q_dim1;
06437 q -= q_offset;
06438 q1_dim1 = *klm2d;
06439 q1_offset = 1 + q1_dim1;
06440 q1 -= q1_offset;
06441 --s;
06442 --res;
06443 iu -= 3;
06444 cu -= 3;
06445
06446
06447 long int l = 0;
06448
06449
06450 m = *ks;
06451 n1 = *n + 1;
06452 if (*iswi == 1) {
06453 i__1 = n1;
06454 for (jj = 1; jj <= i__1; ++jj) {
06455 i__2 = *ks;
06456 for (ii = 1; ii <= i__2; ++ii) {
06457
06458
06459 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06460 ;
06461 }
06462 }
06463 } else if (*iswi == 2) {
06464 i__1 = *ks;
06465 for (ii = 1; ii <= i__1; ++ii) {
06466 i__2 = n1;
06467 for (jj = 1; jj <= i__2; ++jj) {
06468 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06469 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06470 }
06471 }
06472 } else if (*iswi == 3) {
06473 l = 2;
06474 i__1 = n1;
06475 for (jj = 1; jj <= i__1; ++jj) {
06476 i__2 = *ks + 2;
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 + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06483 }
06484 }
06485 } else if (*iswi == 4) {
06486 l = 2;
06487 i__1 = n1;
06488 for (jj = 1; jj <= i__1; ++jj) {
06489 i__2 = *ks + 2;
06490 for (ii = 1; ii <= i__2; ++ii) {
06491 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06492 }
06493 i__2 = *ks;
06494 for (ii = 1; ii <= i__2; ++ii) {
06495 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06496 }
06497 }
06498 } else if (*iswi == 5) {
06499 l = 1;
06500 i__1 = n1;
06501 for (jj = 1; jj <= i__1; ++jj) {
06502 i__2 = *ks + 1;
06503 for (ii = 1; ii <= i__2; ++ii) {
06504 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06505 }
06506 i__2 = *ks;
06507 for (ii = 1; ii <= i__2; ++ii) {
06508 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06509 }
06510 }
06511 } else if (*iswi == 6) {
06512 l = 1;
06513 i__1 = n1;
06514 for (jj = 1; jj <= i__1; ++jj) {
06515 i__2 = *ks + 1;
06516 for (ii = 1; ii <= i__2; ++ii) {
06517 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06518 }
06519 i__2 = *ks;
06520 for (ii = 1; ii <= i__2; ++ii) {
06521 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06522 }
06523 }
06524 } else if (*iswi == 7) {
06525 l = 3;
06526 i__1 = n1;
06527 for (jj = 1; jj <= i__1; ++jj) {
06528 i__2 = *ks + 3;
06529 for (ii = 1; ii <= i__2; ++ii) {
06530 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06531 }
06532 i__2 = *ks;
06533 for (ii = 1; ii <= i__2; ++ii) {
06534 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06535 }
06536 }
06537 } else if (*iswi == 8) {
06538 l = 4;
06539 i__1 = n1;
06540 for (jj = 1; jj <= i__1; ++jj) {
06541 i__2 = *ks + 4;
06542 for (ii = 1; ii <= i__2; ++ii) {
06543 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06544 }
06545 i__2 = *ks;
06546 for (ii = 1; ii <= i__2; ++ii) {
06547 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06548 }
06549 }
06550 }
06551
06552 Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06553 i__1 = *ks;
06554 int tmp__j=0;
06555 for (i__ = 1; i__ <= i__1; ++i__) {
06556 tmp = 0.f;
06557 i__2 = *n - 1;
06558 for (j = 1; j <= i__2; ++j) {
06559 tmp__j=j;
06560 tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06561 }
06562 tmp += x[*n];
06563 p.push_back(static_cast<float>(exp(tmp)));
06564 p.push_back(q1[i__ + q1_dim1]);
06565 }
06566 i__2=*n;
06567 for (i__=1;i__<=i__2;++i__)
06568 { p.push_back(static_cast<float>(x[i__]));}
06569 return p;
06570 }
06571 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06572 double *q, double *x, double *res, double *cu, long int *iu, double *s)
06573 {
06574
06575 long int q_dim1, q_offset, i__1, i__2;
06576 double d__1;
06577
06578 static long int i__, j;
06579 static double z__;
06580 static long int n1, n2, ia, ii, kk, in, nk, js;
06581 static double sn, zu, zv;
06582 static long int nk1, klm, nkl, jmn, jpn;
06583 static double cuv;
06584 static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06585 static float xmin;
06586 static double xmax;
06587 static long int iout;
06588 static double xsum;
06589 static long int iineg, maxit;
06590 static double toler;
06591 static float error;
06592 static double pivot;
06593 static long int kforce, iphase;
06594 static double tpivot;
06595
06596 --s;
06597 --res;
06598 iu -= 3;
06599 cu -= 3;
06600 --x;
06601 q_dim1 = *klm2d;
06602 q_offset = 1 + q_dim1;
06603 q -= q_offset;
06604
06605
06606 maxit = 500;
06607 kode = 0;
06608 toler = 1e-4f;
06609 iter = 0;
06610 n1 = *n + 1;
06611 n2 = *n + 2;
06612 nk = *n + *k;
06613 nk1 = nk + 1;
06614 nkl = nk + *l;
06615 nkl1 = nkl + 1;
06616 klm = *k + *l + *m;
06617 klm1 = klm + 1;
06618 klm2 = klm + 2;
06619 nklm = *n + klm;
06620 kforce = 1;
06621 iter = 0;
06622 js = 1;
06623 ia = 0;
06624
06625 i__1 = *n;
06626 for (j = 1; j <= i__1; ++j) {
06627 q[klm2 + j * q_dim1] = (double) j;
06628
06629 }
06630 i__1 = klm;
06631 for (i__ = 1; i__ <= i__1; ++i__) {
06632 q[i__ + n2 * q_dim1] = (double) (*n + i__);
06633 if (q[i__ + n1 * q_dim1] >= 0.f) {
06634 goto L30;
06635 }
06636 i__2 = n2;
06637 for (j = 1; j <= i__2; ++j) {
06638 q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06639
06640 }
06641 L30:
06642 ;
06643 }
06644
06645 iphase = 2;
06646 i__1 = nklm;
06647 for (j = 1; j <= i__1; ++j) {
06648 cu[(j << 1) + 1] = 0.f;
06649 cu[(j << 1) + 2] = 0.f;
06650 iu[(j << 1) + 1] = 0;
06651 iu[(j << 1) + 2] = 0;
06652
06653 }
06654 if (*l == 0) {
06655 goto L60;
06656 }
06657 i__1 = nkl;
06658 for (j = nk1; j <= i__1; ++j) {
06659 cu[(j << 1) + 1] = 1.f;
06660 cu[(j << 1) + 2] = 1.f;
06661 iu[(j << 1) + 1] = 1;
06662 iu[(j << 1) + 2] = 1;
06663
06664 }
06665 iphase = 1;
06666 L60:
06667 if (*m == 0) {
06668 goto L80;
06669 }
06670 i__1 = nklm;
06671 for (j = nkl1; j <= i__1; ++j) {
06672 cu[(j << 1) + 2] = 1.f;
06673 iu[(j << 1) + 2] = 1;
06674 jmn = j - *n;
06675 if (q[jmn + n2 * q_dim1] < 0.f) {
06676 iphase = 1;
06677 }
06678
06679 }
06680 L80:
06681 if (kode == 0) {
06682 goto L150;
06683 }
06684 i__1 = *n;
06685 for (j = 1; j <= i__1; ++j) {
06686 if ((d__1 = x[j]) < 0.) {
06687 goto L90;
06688 } else if (d__1 == 0) {
06689 goto L110;
06690 } else {
06691 goto L100;
06692 }
06693 L90:
06694 cu[(j << 1) + 1] = 1.f;
06695 iu[(j << 1) + 1] = 1;
06696 goto L110;
06697 L100:
06698 cu[(j << 1) + 2] = 1.f;
06699 iu[(j << 1) + 2] = 1;
06700 L110:
06701 ;
06702 }
06703 i__1 = *k;
06704 for (j = 1; j <= i__1; ++j) {
06705 jpn = j + *n;
06706 if ((d__1 = res[j]) < 0.) {
06707 goto L120;
06708 } else if (d__1 == 0) {
06709 goto L140;
06710 } else {
06711 goto L130;
06712 }
06713 L120:
06714 cu[(jpn << 1) + 1] = 1.f;
06715 iu[(jpn << 1) + 1] = 1;
06716 if (q[j + n2 * q_dim1] > 0.f) {
06717 iphase = 1;
06718 }
06719 goto L140;
06720 L130:
06721 cu[(jpn << 1) + 2] = 1.f;
06722 iu[(jpn << 1) + 2] = 1;
06723 if (q[j + n2 * q_dim1] < 0.f) {
06724 iphase = 1;
06725 }
06726 L140:
06727 ;
06728 }
06729 L150:
06730 if (iphase == 2) {
06731 goto L500;
06732 }
06733
06734 L160:
06735 i__1 = n1;
06736 for (j = js; j <= i__1; ++j) {
06737 xsum = 0.;
06738 i__2 = klm;
06739 for (i__ = 1; i__ <= i__2; ++i__) {
06740 ii = (long int) q[i__ + n2 * q_dim1];
06741 if (ii < 0) {
06742 goto L170;
06743 }
06744 z__ = cu[(ii << 1) + 1];
06745 goto L180;
06746 L170:
06747 iineg = -ii;
06748 z__ = cu[(iineg << 1) + 2];
06749 L180:
06750 xsum += q[i__ + j * q_dim1] * z__;
06751
06752
06753 }
06754 q[klm1 + j * q_dim1] = xsum;
06755
06756 }
06757 i__1 = *n;
06758 for (j = js; j <= i__1; ++j) {
06759 ii = (long int) q[klm2 + j * q_dim1];
06760 if (ii < 0) {
06761 goto L210;
06762 }
06763 z__ = cu[(ii << 1) + 1];
06764 goto L220;
06765 L210:
06766 iineg = -ii;
06767 z__ = cu[(iineg << 1) + 2];
06768 L220:
06769 q[klm1 + j * q_dim1] -= z__;
06770
06771 }
06772
06773 L240:
06774 xmax = 0.f;
06775 if (js > *n) {
06776 goto L490;
06777 }
06778 i__1 = *n;
06779 for (j = js; j <= i__1; ++j) {
06780 zu = q[klm1 + j * q_dim1];
06781 ii = (long int) q[klm2 + j * q_dim1];
06782 if (ii > 0) {
06783 goto L250;
06784 }
06785 ii = -ii;
06786 zv = zu;
06787 zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06788 goto L260;
06789 L250:
06790 zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06791 L260:
06792 if (kforce == 1 && ii > *n) {
06793 goto L280;
06794 }
06795 if (iu[(ii << 1) + 1] == 1) {
06796 goto L270;
06797 }
06798 if (zu <= xmax) {
06799 goto L270;
06800 }
06801 xmax = zu;
06802 in = j;
06803 L270:
06804 if (iu[(ii << 1) + 2] == 1) {
06805 goto L280;
06806 }
06807 if (zv <= xmax) {
06808 goto L280;
06809 }
06810 xmax = zv;
06811 in = j;
06812 L280:
06813 ;
06814 }
06815 if (xmax <= toler) {
06816 goto L490;
06817 }
06818 if (q[klm1 + in * q_dim1] == xmax) {
06819 goto L300;
06820 }
06821 i__1 = klm2;
06822 for (i__ = 1; i__ <= i__1; ++i__) {
06823 q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06824
06825 }
06826 q[klm1 + in * q_dim1] = xmax;
06827
06828 L300:
06829 if (iphase == 1 || ia == 0) {
06830 goto L330;
06831 }
06832 xmax = 0.f;
06833 i__1 = ia;
06834 for (i__ = 1; i__ <= i__1; ++i__) {
06835 z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06836 if (z__ <= xmax) {
06837 goto L310;
06838 }
06839 xmax = z__;
06840 iout = i__;
06841 L310:
06842 ;
06843 }
06844 if (xmax <= toler) {
06845 goto L330;
06846 }
06847 i__1 = n2;
06848 for (j = 1; j <= i__1; ++j) {
06849 z__ = q[ia + j * q_dim1];
06850 q[ia + j * q_dim1] = q[iout + j * q_dim1];
06851 q[iout + j * q_dim1] = z__;
06852
06853 }
06854 iout = ia;
06855 --ia;
06856 pivot = q[iout + in * q_dim1];
06857 goto L420;
06858 L330:
06859 kk = 0;
06860 i__1 = klm;
06861 for (i__ = 1; i__ <= i__1; ++i__) {
06862 z__ = q[i__ + in * q_dim1];
06863 if (z__ <= toler) {
06864 goto L340;
06865 }
06866 ++kk;
06867 res[kk] = q[i__ + n1 * q_dim1] / z__;
06868 s[kk] = (double) i__;
06869 L340:
06870 ;
06871 }
06872 L350:
06873 if (kk > 0) {
06874 goto L360;
06875 }
06876 kode = 2;
06877 goto L590;
06878 L360:
06879 xmin = static_cast<float>( res[1] );
06880 iout = (long int) s[1];
06881 j = 1;
06882 if (kk == 1) {
06883 goto L380;
06884 }
06885 i__1 = kk;
06886 for (i__ = 2; i__ <= i__1; ++i__) {
06887 if (res[i__] >= xmin) {
06888 goto L370;
06889 }
06890 j = i__;
06891 xmin = static_cast<float>( res[i__] );
06892 iout = (long int) s[i__];
06893 L370:
06894 ;
06895 }
06896 res[j] = res[kk];
06897 s[j] = s[kk];
06898 L380:
06899 --kk;
06900 pivot = q[iout + in * q_dim1];
06901 ii = (long int) q[iout + n2 * q_dim1];
06902 if (iphase == 1) {
06903 goto L400;
06904 }
06905 if (ii < 0) {
06906 goto L390;
06907 }
06908 if (iu[(ii << 1) + 2] == 1) {
06909 goto L420;
06910 }
06911 goto L400;
06912 L390:
06913 iineg = -ii;
06914 if (iu[(iineg << 1) + 1] == 1) {
06915 goto L420;
06916 }
06917
06918 L400:
06919 ii = abs(ii);
06920 cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06921 if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06922 goto L420;
06923 }
06924
06925 i__1 = n1;
06926 for (j = js; j <= i__1; ++j) {
06927 z__ = q[iout + j * q_dim1];
06928 q[klm1 + j * q_dim1] -= z__ * cuv;
06929 q[iout + j * q_dim1] = -z__;
06930
06931 }
06932 q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06933 goto L350;
06934
06935 L420:
06936 if (iter < maxit) {
06937 goto L430;
06938 }
06939 kode = 3;
06940 goto L590;
06941 L430:
06942 ++iter;
06943 i__1 = n1;
06944 for (j = js; j <= i__1; ++j) {
06945 if (j != in) {
06946 q[iout + j * q_dim1] /= pivot;
06947 }
06948
06949 }
06950
06951
06952
06953
06954
06955
06956
06957
06958 i__1 = n1;
06959 for (j = js; j <= i__1; ++j) {
06960 if (j == in) {
06961 goto L460;
06962 }
06963 z__ = -q[iout + j * q_dim1];
06964 i__2 = klm1;
06965 for (i__ = 1; i__ <= i__2; ++i__) {
06966 if (i__ != iout) {
06967 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06968 }
06969
06970 }
06971 L460:
06972 ;
06973 }
06974 tpivot = -pivot;
06975 i__1 = klm1;
06976 for (i__ = 1; i__ <= i__1; ++i__) {
06977 if (i__ != iout) {
06978 q[i__ + in * q_dim1] /= tpivot;
06979 }
06980
06981 }
06982 q[iout + in * q_dim1] = 1.f / pivot;
06983 z__ = q[iout + n2 * q_dim1];
06984 q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
06985 q[klm2 + in * q_dim1] = z__;
06986 ii = (long int) abs(z__);
06987 if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
06988 goto L240;
06989 }
06990 i__1 = klm2;
06991 for (i__ = 1; i__ <= i__1; ++i__) {
06992 z__ = q[i__ + in * q_dim1];
06993 q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
06994 q[i__ + js * q_dim1] = z__;
06995
06996 }
06997 ++js;
06998 goto L240;
06999
07000 L490:
07001 if (kforce == 0) {
07002 goto L580;
07003 }
07004 if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
07005 goto L500;
07006 }
07007 kforce = 0;
07008 goto L240;
07009
07010 L500:
07011 iphase = 2;
07012 i__1 = nklm;
07013 for (j = 1; j <= i__1; ++j) {
07014 cu[(j << 1) + 1] = 0.f;
07015 cu[(j << 1) + 2] = 0.f;
07016
07017 }
07018 i__1 = nk;
07019 for (j = n1; j <= i__1; ++j) {
07020 cu[(j << 1) + 1] = 1.f;
07021 cu[(j << 1) + 2] = 1.f;
07022
07023 }
07024 i__1 = klm;
07025 for (i__ = 1; i__ <= i__1; ++i__) {
07026 ii = (long int) q[i__ + n2 * q_dim1];
07027 if (ii > 0) {
07028 goto L530;
07029 }
07030 ii = -ii;
07031 if (iu[(ii << 1) + 2] == 0) {
07032 goto L560;
07033 }
07034 cu[(ii << 1) + 2] = 0.f;
07035 goto L540;
07036 L530:
07037 if (iu[(ii << 1) + 1] == 0) {
07038 goto L560;
07039 }
07040 cu[(ii << 1) + 1] = 0.f;
07041 L540:
07042 ++ia;
07043 i__2 = n2;
07044 for (j = 1; j <= i__2; ++j) {
07045 z__ = q[ia + j * q_dim1];
07046 q[ia + j * q_dim1] = q[i__ + j * q_dim1];
07047 q[i__ + j * q_dim1] = z__;
07048
07049 }
07050 L560:
07051 ;
07052 }
07053 goto L160;
07054 L570:
07055 if (q[klm1 + n1 * q_dim1] <= toler) {
07056 goto L500;
07057 }
07058 kode = 1;
07059 goto L590;
07060 L580:
07061 if (iphase == 1) {
07062 goto L570;
07063 }
07064
07065 kode = 0;
07066 L590:
07067 xsum = 0.;
07068 i__1 = *n;
07069 for (j = 1; j <= i__1; ++j) {
07070 x[j] = 0.f;
07071
07072 }
07073 i__1 = klm;
07074 for (i__ = 1; i__ <= i__1; ++i__) {
07075 res[i__] = 0.f;
07076
07077 }
07078 i__1 = klm;
07079 for (i__ = 1; i__ <= i__1; ++i__) {
07080 ii = (long int) q[i__ + n2 * q_dim1];
07081 sn = 1.f;
07082 if (ii > 0) {
07083 goto L620;
07084 }
07085 ii = -ii;
07086 sn = -1.f;
07087 L620:
07088 if (ii > *n) {
07089 goto L630;
07090 }
07091 x[ii] = sn * q[i__ + n1 * q_dim1];
07092 goto L640;
07093 L630:
07094 iimn = ii - *n;
07095 res[iimn] = sn * q[i__ + n1 * q_dim1];
07096 if (ii >= n1 && ii <= nk) {
07097 xsum += q[i__ + n1 * q_dim1];
07098 }
07099 L640:
07100 ;
07101 }
07102 error = (float)xsum;
07103 return;
07104 }
07105
07106 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07107 {
07108 int j,d;
07109 EMData * e = new EMData();
07110 float *eptr, *imgptr;
07111 imgptr = img->get_data();
07112 float SSE = 0.f;
07113 for (j = 0 ; j < N ; j++) {
07114 e->read_image(images,S[j]);
07115 eptr = e->get_data();
07116 for (d = 0; d < size; d++) {
07117 SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07118 }
07119 delete e;
07120 return SSE;
07121 }
07122
07123
07124 #define mymax(x,y) (((x)>(y))?(x):(y))
07125 #define mymin(x,y) (((x)<(y))?(x):(y))
07126 #define sign(x,y) (((((y)>0)?(1):(-1))*(y!=0))*(x))
07127
07128
07129 #define quadpi 3.141592653589793238462643383279502884197
07130 #define dgr_to_rad quadpi/180
07131 #define deg_to_rad quadpi/180
07132 #define rad_to_deg 180/quadpi
07133 #define rad_to_dgr 180/quadpi
07134 #define TRUE 1
07135 #define FALSE 0
07136
07137
07138 #define theta(i) theta [i-1]
07139 #define phi(i) phi [i-1]
07140 #define weight(i) weight [i-1]
07141 #define lband(i) lband [i-1]
07142 #define ts(i) ts [i-1]
07143 #define thetast(i) thetast [i-1]
07144 #define key(i) key [i-1]
07145
07146
07147 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07148 {
07149
07150 ENTERFUNC;
07151
07152 if ( th.size() != ph.size() ) {
07153 LOGERR("images not same size");
07154 throw ImageFormatException( "images not same size");
07155 }
07156
07157
07158 srand(10);
07159
07160 int i,*key;
07161 int len = th.size();
07162 double *theta,*phi,*weight;
07163 theta = (double*) calloc(len,sizeof(double));
07164 phi = (double*) calloc(len,sizeof(double));
07165 weight = (double*) calloc(len,sizeof(double));
07166 key = (int*) calloc(len,sizeof(int));
07167 const float *thptr, *phptr;
07168
07169 thptr = &th[0];
07170 phptr = &ph[0];
07171 for(i=1;i<=len;i++){
07172 key(i) = i;
07173 weight(i) = 0.0;
07174 }
07175
07176 for(i = 0;i<len;i++){
07177 theta[i] = thptr[i];
07178 phi[i] = phptr[i];
07179 }
07180
07181
07182 Util::hsortd(theta, phi, key, len, 1);
07183
07184
07185 Util::voronoi(phi, theta, weight, len);
07186
07187
07188 Util::hsortd(weight, weight, key, len, 2);
07189
07190 free(theta);
07191 free(phi);
07192 free(key);
07193 vector<double> wt;
07194 double count = 0;
07195 for(i=1; i<= len; i++)
07196 {
07197 wt.push_back(weight(i));
07198 count += weight(i);
07199 }
07200
07201
07202
07203
07204
07205
07206 free(weight);
07207
07208 EXITFUNC;
07209 return wt;
07210
07211 }
07212
07213 struct tmpstruct{
07214 double theta1,phi1;
07215 int key1;
07216 };
07217
07218 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07219 {
07220 ENTERFUNC;
07221 vector<tmpstruct> tmp(len);
07222 int i;
07223 for(i = 1;i<=len;i++)
07224 {
07225 tmp[i-1].theta1 = theta(i);
07226 tmp[i-1].phi1 = phi(i);
07227 tmp[i-1].key1 = key(i);
07228 }
07229
07230 if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07231 if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07232
07233 for(i = 1;i<=len;i++)
07234 {
07235 theta(i) = tmp[i-1].theta1;
07236 phi(i) = tmp[i-1].phi1;
07237 key(i) = tmp[i-1].key1;
07238 }
07239 EXITFUNC;
07240 }
07241
07242 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07243 {
07244 return(tmp1.theta1 < tmp2.theta1);
07245 }
07246
07247 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07248 {
07249 return(tmp1.key1 < tmp2.key1);
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
07506
07507
07508
07509
07510
07511
07512
07513
07514
07515
07516
07517
07518
07519
07520
07521
07522
07523
07524
07525
07526
07527
07528
07529
07530
07531
07532
07533
07534
07535
07536
07537
07538
07539
07540
07541
07542
07543
07544
07545
07546
07547
07548
07549
07550
07551
07552
07553
07554
07555
07556
07557
07558
07559
07560
07561
07562
07563
07564
07565
07566
07567
07568
07569
07570 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07571 {
07572
07573 ENTERFUNC;
07574
07575 int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07576 int nt6, n, ier, nout, lnew, mdup, nd;
07577 int i,k,mt,status;
07578
07579
07580 double *ds, *x, *y, *z;
07581 double tol = 1.0e-8;
07582 double dtol = 15;
07583 double a;
07584
07585
07586
07587
07588
07589
07590
07591
07592
07593 n = nt + nt;
07594
07595 nt6 = n*6;
07596
07597 list = (int*)calloc(nt6,sizeof(int));
07598 lptr = (int*)calloc(nt6,sizeof(int));
07599 lend = (int*)calloc(n ,sizeof(int));
07600 iwk = (int*)calloc(n ,sizeof(int));
07601 good = (int*)calloc(n ,sizeof(int));
07602 key = (int*)calloc(n ,sizeof(int));
07603 indx = (int*)calloc(n ,sizeof(int));
07604 lcnt = (int*)calloc(n ,sizeof(int));
07605
07606 ds = (double*) calloc(n,sizeof(double));
07607 x = (double*) calloc(n,sizeof(double));
07608 y = (double*) calloc(n,sizeof(double));
07609 z = (double*) calloc(n,sizeof(double));
07610
07611 if (list == NULL ||
07612 lptr == NULL ||
07613 lend == NULL ||
07614 iwk == NULL ||
07615 good == NULL ||
07616 key == NULL ||
07617 indx == NULL ||
07618 lcnt == NULL ||
07619 x == NULL ||
07620 y == NULL ||
07621 z == NULL ||
07622 ds == NULL) {
07623 printf("memory allocation failure!\n");
07624 exit(1);
07625 }
07626
07627 bool colinear=true;
07628 while(colinear)
07629 {
07630
07631 L1:
07632 for(i = 0; i<nt; i++){
07633 x[i] = theta[i];
07634 y[i] = phi[i];
07635 x[nt+i] = 180.0 - x[i];
07636 y[nt+i] = 180.0 + y[i];
07637 }
07638
07639 Util::disorder2(x, y, key, n);
07640
07641
07642 double val;
07643 for(k=0; k<2; k++){
07644 for(i=k+1; i<3; i++){
07645 val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07646 if( val < dtol) {
07647 goto L1;
07648 }
07649 }
07650 }
07651
07652 Util::ang_to_xyz(x, y, z, n);
07653
07654
07655 bool dupnode=true;
07656 dupnode=true;
07657 while(dupnode)
07658 {
07659 for(k=0; k<2; k++){
07660 for(i=k+1; i<3; i++){
07661 if( x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07662 Util::flip23(x, y, z, key, k, n);
07663 continue;
07664 }
07665 }
07666 }
07667 dupnode = false;
07668 }
07669
07670
07671 ier = 0;
07672
07673 status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07674
07675 if (status != 0) {
07676 printf(" error in trmsh3 \n");
07677 exit(1);
07678 }
07679
07680 if (ier > 0) {
07681 printf("*** Error in TRMESH: duplicate nodes encountered ***\n");
07682 exit(1);
07683 }
07684
07685 mdup=n-nout;
07686 if (ier == -2) {
07687
07688 }
07689 else
07690 {
07691 colinear=false;
07692 }
07693 }
07694
07695
07696 Assert( ier != -2 );
07697
07698
07699 nd=0;
07700 for (k=1; k<=n; k++){
07701 if (indx[k-1]>0) {
07702 nd++;
07703 good[nd-1]=k;
07704 }
07705 }
07706
07707
07708
07709
07710 for(i = 1; i<=nout; i++) {
07711 k=good[i-1];
07712
07713 if (key[k-1] <= nt) {
07714
07715 a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07716 if (ier != 0){
07717
07718
07719 printf(" *** error in areav: ier = %d ***\n", ier);
07720 weight[key[k-1]-1] =-1.0;
07721 } else {
07722
07723 weight[key[k-1]-1]=a/lcnt[i-1];
07724 }
07725 }
07726 }
07727
07728
07729
07730 for(i = 1; i<=n; i++){
07731 mt =- indx[i-1];
07732 if (mt>0){
07733 k = good[mt-1];
07734
07735
07736
07737 if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07738 }
07739 }
07740
07741 free(list);
07742 free(lend);
07743 free(iwk);
07744 free(good);
07745 free(key);
07746 free(lptr);
07747 free(indx);
07748 free(lcnt);
07749 free(ds);
07750 free(x);
07751 free(y);
07752 free(z);
07753
07754
07755 EXITFUNC;
07756 }
07757
07758 void Util::disorder2(double *x,double *y, int *key, int len)
07759 {
07760 ENTERFUNC;
07761 int k, i;
07762 for(i=0; i<len; i++) key[i]=i+1;
07763
07764 for(i = 0; i<len;i++){
07765 k = rand()%len;
07766 std::swap(key[k], key[i]);
07767 std::swap(x[k], x[i]);
07768 std::swap(y[k], y[i]);
07769 }
07770 EXITFUNC;
07771 }
07772
07773 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07774 {
07775 ENTERFUNC;
07776 double costheta,sintheta,cosphi,sinphi;
07777 for(int i = 0; i<len; i++)
07778 {
07779 cosphi = cos(y[i]*dgr_to_rad);
07780 sinphi = sin(y[i]*dgr_to_rad);
07781 if(fabs(x[i]-90.0)< 1.0e-5){
07782 x[i] = cosphi;
07783 y[i] = sinphi;
07784 z[i] = 0.0;
07785 }
07786 else{
07787 costheta = cos(x[i]*dgr_to_rad);
07788 sintheta = sin(x[i]*dgr_to_rad);
07789 x[i] = cosphi*sintheta;
07790 y[i] = sinphi*sintheta;
07791 z[i] = costheta;
07792 }
07793 }
07794 EXITFUNC;
07795 }
07796
07797 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07798 {
07799 ENTERFUNC;
07800 int i = k;
07801 while( i == k ) i = rand()%len;
07802 std::swap(key[i], key[k]);
07803 std::swap(x[i], x[k]);
07804 std::swap(y[i], y[k]);
07805 std::swap(z[i], z[k]);
07806 EXITFUNC;
07807 }
07808
07809
07810 #undef mymax
07811 #undef mymin
07812 #undef sign
07813 #undef quadpi
07814 #undef dgr_to_rad
07815 #undef deg_to_rad
07816 #undef rad_to_deg
07817 #undef rad_to_dgr
07818 #undef TRUE
07819 #undef FALSE
07820 #undef theta
07821 #undef phi
07822 #undef weight
07823 #undef lband
07824 #undef ts
07825 #undef thetast
07826 #undef key
07827
07828
07829
07830
07831
07832
07833
07834
07835
07836
07837
07838 #define TRUE_ (1)
07839 #define FALSE_ (0)
07840 #define abs(x) ((x) >= 0 ? (x) : -(x))
07841
07842 struct stcom_{
07843 double y;
07844 };
07845 stcom_ stcom_1;
07846 #ifdef KR_headers
07847 double floor();
07848 int i_dnnt(x) double *x;
07849 #else
07850 int i_dnnt(double *x)
07851 #endif
07852 {
07853 return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07854 }
07855
07856
07857
07858
07859
07860 int Util::trmsh3_(int *n0, double *tol, double *x,
07861 double *y, double *z__, int *n, int *list, int *
07862 lptr, int *lend, int *lnew, int *indx, int *lcnt,
07863 int *near__, int *next, double *dist, int *ier)
07864 {
07865
07866 int i__1, i__2;
07867
07868
07869 static double d__;
07870 static int i__, j;
07871 static double d1, d2, d3;
07872 static int i0, lp, kt, ku, lpl, nku;
07873 static int nexti;
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
07959
07960
07961
07962
07963
07964
07965
07966
07967
07968
07969
07970
07971
07972
07973
07974
07975
07976
07977
07978
07979
07980
07981
07982
07983
07984
07985
07986
07987
07988
07989
07990
07991
07992
07993
07994
07995
07996
07997
07998
07999
08000
08001
08002
08003
08004
08005
08006
08007
08008
08009
08010
08011
08012
08013
08014
08015
08016
08017
08018
08019
08020
08021
08022
08023
08024
08025 --dist;
08026 --next;
08027 --near__;
08028 --indx;
08029 --lend;
08030 --z__;
08031 --y;
08032 --x;
08033 --list;
08034 --lptr;
08035 --lcnt;
08036
08037
08038 if (*n0 < 3) {
08039 *n = 0;
08040 *ier = -1;
08041 return 0;
08042 }
08043
08044
08045
08046 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
08047 z__[3])) {
08048
08049
08050
08051 list[1] = 3;
08052 lptr[1] = 2;
08053 list[2] = -2;
08054 lptr[2] = 1;
08055 lend[1] = 2;
08056
08057 list[3] = 1;
08058 lptr[3] = 4;
08059 list[4] = -3;
08060 lptr[4] = 3;
08061 lend[2] = 4;
08062
08063 list[5] = 2;
08064 lptr[5] = 6;
08065 list[6] = -1;
08066 lptr[6] = 5;
08067 lend[3] = 6;
08068
08069 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
08070 y[3], &z__[3])) {
08071
08072
08073
08074
08075
08076 list[1] = 2;
08077 lptr[1] = 2;
08078 list[2] = -3;
08079 lptr[2] = 1;
08080 lend[1] = 2;
08081
08082 list[3] = 3;
08083 lptr[3] = 4;
08084 list[4] = -1;
08085 lptr[4] = 3;
08086 lend[2] = 4;
08087
08088 list[5] = 1;
08089 lptr[5] = 6;
08090 list[6] = -2;
08091 lptr[6] = 5;
08092 lend[3] = 6;
08093
08094
08095 } else {
08096
08097
08098
08099 *n = 0;
08100 *ier = -2;
08101 return 0;
08102 }
08103
08104
08105
08106
08107
08108 *lnew = 7;
08109 indx[1] = 1;
08110 indx[2] = 2;
08111 indx[3] = 3;
08112 lcnt[1] = 1;
08113 lcnt[2] = 1;
08114 lcnt[3] = 1;
08115 if (*n0 == 3) {
08116 *n = 3;
08117 *ier = 0;
08118 return 0;
08119 }
08120
08121
08122
08123
08124
08125
08126
08127
08128
08129
08130
08131
08132
08133
08134
08135
08136
08137
08138
08139
08140
08141
08142
08143
08144
08145
08146
08147 near__[1] = 0;
08148 near__[2] = 0;
08149 near__[3] = 0;
08150 for (ku = *n0; ku >= 4; --ku) {
08151 d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08152 d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08153 d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08154 if (d1 <= d2 && d1 <= d3) {
08155 near__[ku] = 1;
08156 dist[ku] = d1;
08157 next[ku] = near__[1];
08158 near__[1] = ku;
08159 } else if (d2 <= d1 && d2 <= d3) {
08160 near__[ku] = 2;
08161 dist[ku] = d2;
08162 next[ku] = near__[2];
08163 near__[2] = ku;
08164 } else {
08165 near__[ku] = 3;
08166 dist[ku] = d3;
08167 next[ku] = near__[3];
08168 near__[3] = ku;
08169 }
08170
08171 }
08172
08173
08174
08175
08176 kt = 3;
08177 i__1 = *n0;
08178 for (ku = 4; ku <= i__1; ++ku) {
08179 nku = near__[ku];
08180
08181
08182
08183 i__ = nku;
08184 if (near__[i__] == ku) {
08185 near__[i__] = next[ku];
08186 } else {
08187 i__ = near__[i__];
08188 L2:
08189 i0 = i__;
08190 i__ = next[i0];
08191 if (i__ != ku) {
08192 goto L2;
08193 }
08194 next[i0] = next[ku];
08195 }
08196 near__[ku] = 0;
08197
08198
08199
08200 if (dist[ku] <= *tol - 1.) {
08201 indx[ku] = -nku;
08202 ++lcnt[nku];
08203 goto L6;
08204 }
08205
08206
08207
08208 ++kt;
08209 x[kt] = x[ku];
08210 y[kt] = y[ku];
08211 z__[kt] = z__[ku];
08212 indx[ku] = kt;
08213 lcnt[kt] = 1;
08214 addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08215 , lnew, ier);
08216 if (*ier != 0) {
08217 *n = 0;
08218 *ier = -3;
08219 return 0;
08220 }
08221
08222
08223
08224 lpl = lend[kt];
08225 lp = lpl;
08226 L3:
08227 lp = lptr[lp];
08228 j = (i__2 = list[lp], abs(i__2));
08229
08230
08231
08232
08233
08234
08235
08236 i__ = near__[j];
08237 L4:
08238 if (i__ == 0) {
08239 goto L5;
08240 }
08241 nexti = next[i__];
08242
08243
08244
08245
08246 d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08247 if (d__ < dist[i__]) {
08248
08249
08250
08251
08252
08253 near__[i__] = kt;
08254 dist[i__] = d__;
08255 if (i__ == near__[j]) {
08256 near__[j] = nexti;
08257 } else {
08258 next[i0] = nexti;
08259 }
08260 next[i__] = near__[kt];
08261 near__[kt] = i__;
08262 } else {
08263 i0 = i__;
08264 }
08265
08266
08267
08268 i__ = nexti;
08269 goto L4;
08270
08271
08272
08273 L5:
08274 if (lp != lpl) {
08275 goto L3;
08276 }
08277 L6:
08278 ;
08279 }
08280 *n = kt;
08281 *ier = 0;
08282 return 0;
08283 }
08284
08285
08286 int addnod_(int *nst, int *k, double *x,
08287 double *y, double *z__, int *list, int *lptr, int
08288 *lend, int *lnew, int *ier)
08289 {
08290
08291
08292 static double tol = 0.;
08293
08294
08295 int i__1;
08296
08297
08298 static int l;
08299 static double p[3], b1, b2, b3;
08300 static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08301 int swap_(int *, int *, int *,
08302 int *, int *, int *, int *, int *);
08303 static int lpo1s;
08304 int bdyadd_(int *, int *, int *,
08305 int *, int *, int *, int *), intadd_(int *,
08306 int *, int *, int *, int *, int *, int *,
08307 int *), trfind_(int *, double *, int *,
08308 double *, double *, double *, int *, int *,
08309 int *, double *, double *, double *, int *,
08310 int *, int *), covsph_(int *, int *, int *,
08311 int *, int *, int *);
08312 int lstptr_(int *, int *, int *, int *);
08313 long int swptst_(int *, int *, int *, int *,
08314 double *, double *, double *);
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
08351
08352
08353
08354
08355
08356
08357
08358
08359
08360
08361
08362
08363
08364
08365
08366
08367
08368
08369
08370
08371
08372
08373
08374
08375
08376
08377
08378
08379
08380
08381
08382
08383
08384
08385
08386
08387
08388
08389
08390
08391
08392
08393
08394
08395
08396
08397
08398
08399
08400
08401
08402
08403
08404
08405
08406
08407
08408
08409
08410
08411
08412
08413
08414
08415
08416
08417 --lend;
08418 --z__;
08419 --y;
08420 --x;
08421 --list;
08422 --lptr;
08423
08424
08425
08426 kk = *k;
08427 if (kk < 4) {
08428 goto L3;
08429 }
08430
08431
08432 km1 = kk - 1;
08433 ist = *nst;
08434 if (ist < 1) {
08435 ist = km1;
08436 }
08437 p[0] = x[kk];
08438 p[1] = y[kk];
08439 p[2] = z__[kk];
08440
08441
08442
08443
08444 trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08445 , &b1, &b2, &b3, &i1, &i2, &i3);
08446
08447
08448
08449 if (i1 == 0) {
08450 goto L4;
08451 }
08452 l = i1;
08453 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08454 goto L5;
08455 }
08456 l = i2;
08457 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08458 goto L5;
08459 }
08460 if (i3 != 0) {
08461 l = i3;
08462 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08463 goto L5;
08464 }
08465 intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08466 } else {
08467 if (i1 != i2) {
08468 bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08469 } else {
08470 covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08471 }
08472 }
08473 *ier = 0;
08474
08475
08476
08477 lp = lend[kk];
08478 lpf = lptr[lp];
08479 io2 = list[lpf];
08480 lpo1 = lptr[lpf];
08481 io1 = (i__1 = list[lpo1], abs(i__1));
08482
08483
08484
08485 L1:
08486 lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08487 if (list[lp] < 0) {
08488 goto L2;
08489 }
08490 lp = lptr[lp];
08491 in1 = (i__1 = list[lp], abs(i__1));
08492
08493
08494
08495
08496 lpo1s = lpo1;
08497 if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08498 goto L2;
08499 }
08500 swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08501 if (lpo1 == 0) {
08502
08503
08504
08505
08506
08507
08508 lpo1 = lpo1s;
08509 goto L2;
08510 }
08511 io1 = in1;
08512 goto L1;
08513
08514
08515
08516
08517 L2:
08518 if (lpo1 == lpf || list[lpo1] < 0) {
08519 return 0;
08520 }
08521 io2 = io1;
08522 lpo1 = lptr[lpo1];
08523 io1 = (i__1 = list[lpo1], abs(i__1));
08524 goto L1;
08525
08526
08527
08528 L3:
08529 *ier = -1;
08530 return 0;
08531
08532
08533
08534 L4:
08535 *ier = -2;
08536 return 0;
08537
08538
08539
08540 L5:
08541 *ier = l;
08542 return 0;
08543 }
08544
08545 double angle_(double *v1, double *v2, double *v3)
08546 {
08547
08548 double ret_val;
08549
08550
08551
08552
08553
08554 static double a;
08555 static int i__;
08556 static double ca, s21, s23, u21[3], u23[3];
08557
08558
08559
08560
08561
08562
08563
08564
08565
08566
08567
08568
08569
08570
08571
08572
08573
08574
08575
08576
08577
08578
08579
08580
08581
08582
08583
08584
08585
08586
08587
08588
08589
08590
08591
08592
08593
08594
08595
08596
08597
08598
08599
08600
08601
08602
08603
08604
08605
08606
08607
08608
08609
08610
08611
08612
08613
08614
08615
08616 --v3;
08617 --v2;
08618 --v1;
08619
08620
08621 u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08622 u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08623 u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08624
08625 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08626 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08627 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08628
08629
08630
08631 s21 = 0.;
08632 s23 = 0.;
08633 for (i__ = 1; i__ <= 3; ++i__) {
08634 s21 += u21[i__ - 1] * u21[i__ - 1];
08635 s23 += u23[i__ - 1] * u23[i__ - 1];
08636
08637 }
08638
08639
08640
08641
08642 if (s21 == 0. || s23 == 0.) {
08643 ret_val = 0.;
08644 return ret_val;
08645 }
08646 s21 = sqrt(s21);
08647 s23 = sqrt(s23);
08648 for (i__ = 1; i__ <= 3; ++i__) {
08649 u21[i__ - 1] /= s21;
08650 u23[i__ - 1] /= s23;
08651
08652 }
08653
08654
08655
08656
08657
08658 ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08659 if (ca < -1.) {
08660 ca = -1.;
08661 }
08662 if (ca > 1.) {
08663 ca = 1.;
08664 }
08665 a = acos(ca);
08666
08667
08668
08669
08670 if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08671 , &v3[3])) {
08672 a = acos(-1.) * 2. - a;
08673 }
08674 ret_val = a;
08675 return ret_val;
08676 }
08677
08678 double areas_(double *v1, double *v2, double *v3)
08679 {
08680
08681 double ret_val;
08682
08683
08684
08685
08686
08687 static int i__;
08688 static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08689 ca2, ca3;
08690
08691
08692
08693
08694
08695
08696
08697
08698
08699
08700
08701
08702
08703
08704
08705
08706
08707
08708
08709
08710
08711
08712
08713
08714
08715
08716
08717
08718
08719
08720
08721
08722
08723
08724
08725
08726
08727
08728
08729
08730
08731
08732
08733
08734
08735
08736
08737
08738
08739
08740
08741
08742
08743 --v3;
08744 --v2;
08745 --v1;
08746
08747
08748 u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08749 u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08750 u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08751
08752 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08753 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08754 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08755
08756 u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08757 u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08758 u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08759
08760
08761
08762 s12 = 0.;
08763 s23 = 0.;
08764 s31 = 0.;
08765 for (i__ = 1; i__ <= 3; ++i__) {
08766 s12 += u12[i__ - 1] * u12[i__ - 1];
08767 s23 += u23[i__ - 1] * u23[i__ - 1];
08768 s31 += u31[i__ - 1] * u31[i__ - 1];
08769
08770 }
08771
08772
08773
08774
08775 if (s12 == 0. || s23 == 0. || s31 == 0.) {
08776 ret_val = 0.;
08777 return ret_val;
08778 }
08779 s12 = sqrt(s12);
08780 s23 = sqrt(s23);
08781 s31 = sqrt(s31);
08782 for (i__ = 1; i__ <= 3; ++i__) {
08783 u12[i__ - 1] /= s12;
08784 u23[i__ - 1] /= s23;
08785 u31[i__ - 1] /= s31;
08786
08787 }
08788
08789
08790
08791
08792
08793
08794
08795 ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08796 ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08797 ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08798 if (ca1 < -1.) {
08799 ca1 = -1.;
08800 }
08801 if (ca1 > 1.) {
08802 ca1 = 1.;
08803 }
08804 if (ca2 < -1.) {
08805 ca2 = -1.;
08806 }
08807 if (ca2 > 1.) {
08808 ca2 = 1.;
08809 }
08810 if (ca3 < -1.) {
08811 ca3 = -1.;
08812 }
08813 if (ca3 > 1.) {
08814 ca3 = 1.;
08815 }
08816 a1 = acos(ca1);
08817 a2 = acos(ca2);
08818 a3 = acos(ca3);
08819
08820
08821
08822 ret_val = a1 + a2 + a3 - acos(-1.);
08823 if (ret_val < 0.) {
08824 ret_val = 0.;
08825 }
08826 return ret_val;
08827 }
08828
08829
08830
08831 double Util::areav_(int *k, int *n, double *x, double *y,
08832 double *z__, int *list, int *lptr, int *lend, int
08833 *ier)
08834 {
08835
08836
08837 static double amax = 6.28;
08838
08839
08840 double ret_val;
08841
08842
08843 static double a, c0[3], c2[3], c3[3];
08844 static int n1, n2, n3;
08845 static double v1[3], v2[3], v3[3];
08846 static int lp, lpl, ierr;
08847 static double asum;
08848 static long int first;
08849
08850
08851
08852
08853
08854
08855
08856
08857
08858
08859
08860
08861
08862
08863
08864
08865
08866
08867
08868
08869
08870
08871
08872
08873
08874
08875
08876
08877
08878
08879
08880
08881
08882
08883
08884
08885
08886
08887
08888
08889
08890
08891
08892
08893
08894
08895
08896
08897
08898
08899
08900
08901
08902
08903
08904
08905
08906
08907 --lend;
08908 --z__;
08909 --y;
08910 --x;
08911 --list;
08912 --lptr;
08913
08914
08915
08916
08917
08918 if (*k < 1 || *k > *n || *n <= 3) {
08919 goto L11;
08920 }
08921
08922
08923
08924
08925
08926 n1 = *k;
08927 v1[0] = x[n1];
08928 v1[1] = y[n1];
08929 v1[2] = z__[n1];
08930 lpl = lend[n1];
08931 n3 = list[lpl];
08932 if (n3 < 0) {
08933 goto L12;
08934 }
08935 lp = lpl;
08936 first = TRUE_;
08937 asum = 0.;
08938
08939
08940
08941 L1:
08942 n2 = n3;
08943 lp = lptr[lp];
08944 n3 = list[lp];
08945 v2[0] = x[n2];
08946 v2[1] = y[n2];
08947 v2[2] = z__[n2];
08948 v3[0] = x[n3];
08949 v3[1] = y[n3];
08950 v3[2] = z__[n3];
08951 if (first) {
08952
08953
08954
08955
08956 circum_(v1, v2, v3, c3, &ierr);
08957 if (ierr != 0) {
08958 goto L13;
08959 }
08960 c0[0] = c3[0];
08961 c0[1] = c3[1];
08962 c0[2] = c3[2];
08963 first = FALSE_;
08964 } else {
08965
08966
08967
08968
08969 c2[0] = c3[0];
08970 c2[1] = c3[1];
08971 c2[2] = c3[2];
08972 circum_(v1, v2, v3, c3, &ierr);
08973 if (ierr != 0) {
08974 goto L13;
08975 }
08976 a = areas_(v1, c2, c3);
08977 if (a > amax) {
08978 goto L14;
08979 }
08980 asum += a;
08981 }
08982
08983
08984
08985 if (lp != lpl) {
08986 goto L1;
08987 }
08988
08989
08990
08991 a = areas_(v1, c3, c0);
08992 if (a > amax) {
08993 goto L14;
08994 }
08995 asum += a;
08996
08997
08998
08999 *ier = 0;
09000 ret_val = asum;
09001 return ret_val;
09002
09003
09004
09005 L11:
09006 *ier = 1;
09007 ret_val = 0.;
09008 return ret_val;
09009
09010
09011
09012 L12:
09013 *ier = 2;
09014 ret_val = 0.;
09015 return ret_val;
09016
09017
09018
09019 L13:
09020 *ier = 3;
09021 ret_val = 0.;
09022 return ret_val;
09023
09024
09025
09026 L14:
09027 *ier = 4;
09028 ret_val = 0.;
09029 return ret_val;
09030 }
09031
09032 double areav_new__(int *k, int *n, double *x, double *y,
09033 double *z__, int *list, int *lptr, int *lend, int
09034 *ier)
09035 {
09036
09037 double ret_val = 0;
09038
09039
09040
09041
09042
09043 static int m;
09044 static double c1[3], c2[3], c3[3];
09045 static int n1, n2, n3;
09046 static double v1[3], v2[3], v3[3];
09047 static int lp;
09048 static double c1s[3], c2s[3];
09049 static int lpl, ierr;
09050 static double asum;
09051 double angle_(double *, double *, double *);
09052 static float areav;
09053
09054
09055
09056
09057
09058
09059
09060
09061
09062
09063
09064
09065
09066
09067
09068
09069
09070
09071
09072
09073
09074
09075
09076
09077
09078
09079
09080
09081
09082
09083
09084
09085
09086
09087
09088
09089
09090
09091
09092
09093
09094
09095
09096
09097
09098
09099
09100
09101
09102
09103
09104
09105
09106
09107
09108
09109
09110
09111
09112
09113
09114 --lend;
09115 --z__;
09116 --y;
09117 --x;
09118 --list;
09119 --lptr;
09120
09121
09122 if (*k < 1 || *k > *n || *n <= 3) {
09123 goto L11;
09124 }
09125
09126
09127
09128
09129
09130 n1 = *k;
09131 v1[0] = x[n1];
09132 v1[1] = y[n1];
09133 v1[2] = z__[n1];
09134 lpl = lend[n1];
09135 n3 = list[lpl];
09136 if (n3 < 0) {
09137 goto L12;
09138 }
09139 lp = lpl;
09140 m = 0;
09141 asum = 0.;
09142
09143
09144
09145 L1:
09146 ++m;
09147 n2 = n3;
09148 lp = lptr[lp];
09149 n3 = list[lp];
09150 v2[0] = x[n2];
09151 v2[1] = y[n2];
09152 v2[2] = z__[n2];
09153 v3[0] = x[n3];
09154 v3[1] = y[n3];
09155 v3[2] = z__[n3];
09156 if (m == 1) {
09157
09158
09159
09160
09161 circum_(v1, v2, v3, c2, &ierr);
09162 if (ierr != 0) {
09163 goto L13;
09164 }
09165 c1s[0] = c2[0];
09166 c1s[1] = c2[1];
09167 c1s[2] = c2[2];
09168 } else if (m == 2) {
09169
09170
09171
09172
09173 circum_(v1, v2, v3, c3, &ierr);
09174 if (ierr != 0) {
09175 goto L13;
09176 }
09177 c2s[0] = c3[0];
09178 c2s[1] = c3[1];
09179 c2s[2] = c3[2];
09180 } else {
09181
09182
09183
09184
09185
09186 c1[0] = c2[0];
09187 c1[1] = c2[1];
09188 c1[2] = c2[2];
09189 c2[0] = c3[0];
09190 c2[1] = c3[1];
09191 c2[2] = c3[2];
09192 circum_(v1, v2, v3, c3, &ierr);
09193 if (ierr != 0) {
09194 goto L13;
09195 }
09196 asum += angle_(c1, c2, c3);
09197 }
09198
09199
09200
09201 if (lp != lpl) {
09202 goto L1;
09203 }
09204
09205
09206
09207
09208 asum += angle_(c2, c3, c1s);
09209
09210
09211
09212
09213 asum += angle_(c3, c1s, c2s);
09214
09215
09216
09217 *ier = 0;
09218 ret_val = asum - (double) (m - 2) * acos(-1.);
09219 return ret_val;
09220
09221
09222
09223 L11:
09224 *ier = 1;
09225 areav = 0.f;
09226 return ret_val;
09227
09228
09229
09230 L12:
09231 *ier = 2;
09232 areav = 0.f;
09233 return ret_val;
09234
09235
09236
09237 L13:
09238 *ier = 3;
09239 areav = 0.f;
09240 return ret_val;
09241 }
09242
09243 int bdyadd_(int *kk, int *i1, int *i2, int *
09244 list, int *lptr, int *lend, int *lnew)
09245 {
09246 static int k, n1, n2, lp, lsav, nsav, next;
09247 int insert_(int *, int *, int *,
09248 int *, int *);
09249
09250
09251
09252
09253
09254
09255
09256
09257
09258
09259
09260
09261
09262
09263
09264
09265
09266
09267
09268
09269
09270
09271
09272
09273
09274
09275
09276
09277
09278
09279
09280
09281
09282
09283
09284
09285
09286
09287
09288
09289
09290
09291
09292
09293
09294
09295
09296
09297
09298
09299
09300
09301
09302
09303
09304
09305
09306
09307
09308
09309
09310
09311
09312
09313 --lend;
09314 --lptr;
09315 --list;
09316
09317
09318 k = *kk;
09319 n1 = *i1;
09320 n2 = *i2;
09321
09322
09323
09324 lp = lend[n1];
09325 lsav = lptr[lp];
09326 lptr[lp] = *lnew;
09327 list[*lnew] = -k;
09328 lptr[*lnew] = lsav;
09329 lend[n1] = *lnew;
09330 ++(*lnew);
09331 next = -list[lp];
09332 list[lp] = next;
09333 nsav = next;
09334
09335
09336
09337
09338 L1:
09339 lp = lend[next];
09340 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09341 if (next == n2) {
09342 goto L2;
09343 }
09344 next = -list[lp];
09345 list[lp] = next;
09346 goto L1;
09347
09348
09349
09350
09351 L2:
09352 lsav = *lnew;
09353 list[*lnew] = n1;
09354 lptr[*lnew] = *lnew + 1;
09355 ++(*lnew);
09356 next = nsav;
09357
09358 L3:
09359 if (next == n2) {
09360 goto L4;
09361 }
09362 list[*lnew] = next;
09363 lptr[*lnew] = *lnew + 1;
09364 ++(*lnew);
09365 lp = lend[next];
09366 next = list[lp];
09367 goto L3;
09368
09369 L4:
09370 list[*lnew] = -n2;
09371 lptr[*lnew] = lsav;
09372 lend[k] = *lnew;
09373 ++(*lnew);
09374 return 0;
09375 }
09376
09377 int bnodes_(int *n, int *list, int *lptr,
09378 int *lend, int *nodes, int *nb, int *na, int *nt)
09379 {
09380
09381 int i__1;
09382
09383
09384 static int k, n0, lp, nn, nst;
09385
09386
09387
09388
09389
09390
09391
09392
09393
09394
09395
09396
09397
09398
09399
09400
09401
09402
09403
09404
09405
09406
09407
09408
09409
09410
09411
09412
09413
09414
09415
09416
09417
09418
09419
09420
09421
09422
09423
09424
09425
09426
09427
09428
09429
09430
09431
09432
09433
09434
09435
09436
09437
09438
09439
09440
09441
09442
09443
09444
09445 --lend;
09446 --list;
09447 --lptr;
09448 --nodes;
09449
09450
09451 nn = *n;
09452
09453
09454
09455 i__1 = nn;
09456 for (nst = 1; nst <= i__1; ++nst) {
09457 lp = lend[nst];
09458 if (list[lp] < 0) {
09459 goto L2;
09460 }
09461
09462 }
09463
09464
09465
09466 *nb = 0;
09467 *na = (nn - 2) * 3;
09468 *nt = nn - (2<<1);
09469 return 0;
09470
09471
09472
09473
09474 L2:
09475 nodes[1] = nst;
09476 k = 1;
09477 n0 = nst;
09478
09479
09480
09481 L3:
09482 lp = lend[n0];
09483 lp = lptr[lp];
09484 n0 = list[lp];
09485 if (n0 == nst) {
09486 goto L4;
09487 }
09488 ++k;
09489 nodes[k] = n0;
09490 goto L3;
09491
09492
09493
09494 L4:
09495 *nb = k;
09496 *nt = (*n << 1) - *nb - 2;
09497 *na = *nt + *n - 1;
09498 return 0;
09499 }
09500
09501 int circle_(int *k, double *xc, double *yc,
09502 int *ier)
09503 {
09504
09505 int i__1;
09506
09507
09508
09509
09510
09511 static double a, c__;
09512 static int i__;
09513 static double s;
09514 static int k2, k3;
09515 static double x0, y0;
09516 static int kk, np1;
09517
09518
09519
09520
09521
09522
09523
09524
09525
09526
09527
09528
09529
09530
09531
09532
09533
09534
09535
09536
09537
09538
09539
09540
09541
09542
09543
09544
09545
09546
09547
09548
09549
09550
09551
09552
09553
09554
09555
09556
09557
09558
09559
09560
09561
09562
09563
09564
09565
09566
09567
09568
09569
09570
09571
09572
09573
09574
09575
09576
09577
09578
09579
09580
09581 --yc;
09582 --xc;
09583
09584
09585 kk = *k;
09586 k2 = kk << 1;
09587 k3 = kk * 3;
09588 np1 = (kk << 2) + 1;
09589
09590
09591
09592
09593 if (kk < 1) {
09594 goto L2;
09595 }
09596 a = atan(1.) * 2. / (double) kk;
09597 c__ = cos(a);
09598 s = sin(a);
09599 x0 = 1.;
09600 y0 = 0.;
09601
09602
09603
09604
09605
09606 i__1 = kk;
09607 for (i__ = 1; i__ <= i__1; ++i__) {
09608 xc[i__] = x0;
09609 yc[i__] = y0;
09610 xc[i__ + kk] = -y0;
09611 yc[i__ + kk] = x0;
09612 xc[i__ + k2] = -x0;
09613 yc[i__ + k2] = -y0;
09614 xc[i__ + k3] = y0;
09615 yc[i__ + k3] = -x0;
09616
09617
09618
09619 x0 = c__ * x0 - s * y0;
09620 y0 = s * x0 + c__ * y0;
09621
09622 }
09623
09624
09625
09626
09627 xc[np1] = xc[1];
09628 yc[np1] = yc[1];
09629 *ier = 0;
09630 return 0;
09631
09632
09633
09634 L2:
09635 *ier = 1;
09636 return 0;
09637 }
09638
09639 int circum_(double *v1, double *v2, double *v3,
09640 double *c__, int *ier)
09641 {
09642
09643
09644
09645
09646 static int i__;
09647 static double e1[3], e2[3], cu[3], cnorm;
09648
09649
09650
09651
09652
09653
09654
09655
09656
09657
09658
09659
09660
09661
09662
09663
09664
09665
09666
09667
09668
09669
09670
09671
09672
09673
09674
09675
09676
09677
09678
09679
09680
09681
09682
09683
09684
09685
09686
09687
09688
09689
09690
09691
09692
09693
09694
09695
09696
09697
09698
09699
09700
09701
09702
09703
09704 --c__;
09705 --v3;
09706 --v2;
09707 --v1;
09708
09709
09710 for (i__ = 1; i__ <= 3; ++i__) {
09711 e1[i__ - 1] = v2[i__] - v1[i__];
09712 e2[i__ - 1] = v3[i__] - v1[i__];
09713
09714 }
09715
09716
09717
09718 cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09719 cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09720 cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09721 cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09722
09723
09724
09725
09726 if (cnorm != 0.) {
09727
09728
09729
09730 cnorm = sqrt(cnorm);
09731 for (i__ = 1; i__ <= 3; ++i__) {
09732 c__[i__] = cu[i__ - 1] / cnorm;
09733
09734 }
09735
09736
09737
09738
09739
09740
09741 if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09742 c__[1] = -c__[1];
09743 c__[2] = -c__[2];
09744 c__[3] = -c__[3];
09745 }
09746 *ier = 0;
09747 } else {
09748
09749
09750
09751 *ier = 1;
09752 }
09753 return 0;
09754 }
09755
09756 int covsph_(int *kk, int *n0, int *list, int
09757 *lptr, int *lend, int *lnew)
09758 {
09759 static int k, lp, nst, lsav, next;
09760 int insert_(int *, int *, int *,
09761 int *, int *);
09762
09763
09764
09765
09766
09767
09768
09769
09770
09771
09772
09773
09774
09775
09776
09777
09778
09779
09780
09781
09782
09783
09784
09785
09786
09787
09788
09789
09790
09791
09792
09793
09794
09795
09796
09797
09798
09799
09800
09801
09802
09803
09804
09805
09806
09807
09808
09809
09810
09811
09812
09813
09814
09815
09816
09817
09818
09819 --lend;
09820 --lptr;
09821 --list;
09822
09823
09824 k = *kk;
09825 nst = *n0;
09826
09827
09828
09829
09830
09831 next = nst;
09832 L1:
09833 lp = lend[next];
09834 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09835 next = -list[lp];
09836 list[lp] = next;
09837 if (next != nst) {
09838 goto L1;
09839 }
09840
09841
09842
09843
09844 lsav = *lnew;
09845 L2:
09846 lp = lend[next];
09847 list[*lnew] = next;
09848 lptr[*lnew] = *lnew + 1;
09849 ++(*lnew);
09850 next = list[lp];
09851 if (next != nst) {
09852 goto L2;
09853 }
09854
09855 lptr[*lnew - 1] = lsav;
09856 lend[k] = *lnew - 1;
09857 return 0;
09858 }
09859
09860
09861 int crlist_(int *n, int *ncol, double *x,
09862 double *y, double *z__, int *list, int *lend, int
09863 *lptr, int *lnew, int *ltri, int *listc, int *nb,
09864 double *xc, double *yc, double *zc, double *rc,
09865 int *ier)
09866 {
09867
09868 int i__1, i__2;
09869
09870
09871
09872
09873
09874 static double c__[3], t;
09875 static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09876 static double v1[3], v2[3], v3[3];
09877 static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09878 lpn;
09879 static long int swp;
09880 static int ierr;
09881 int lstptr_(int *, int *, int *, int *);
09882 long int swptst_(int *, int *, int *, int *,
09883 double *, double *, double *);
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
09994
09995
09996
09997
09998
09999
10000
10001
10002
10003
10004
10005
10006
10007
10008
10009
10010
10011
10012
10013
10014
10015
10016
10017
10018
10019
10020
10021
10022
10023
10024
10025
10026
10027
10028
10029
10030
10031
10032
10033
10034
10035
10036
10037
10038
10039
10040
10041
10042
10043
10044
10045
10046
10047
10048
10049
10050
10051
10052
10053
10054
10055
10056
10057
10058
10059
10060 --lend;
10061 --z__;
10062 --y;
10063 --x;
10064 ltri -= 7;
10065 --list;
10066 --lptr;
10067 --listc;
10068 --xc;
10069 --yc;
10070 --zc;
10071 --rc;
10072
10073
10074 nn = *n;
10075 *nb = 0;
10076 nt = 0;
10077 if (nn < 3) {
10078 goto L21;
10079 }
10080
10081
10082
10083 i__1 = nn;
10084 for (n1 = 1; n1 <= i__1; ++n1) {
10085 lp = lend[n1];
10086 if (list[lp] < 0) {
10087 goto L2;
10088 }
10089
10090 }
10091
10092
10093
10094 goto L9;
10095
10096
10097
10098
10099
10100
10101
10102
10103 L2:
10104 n2 = -list[lp];
10105 lp = lptr[lp];
10106 n3 = list[lp];
10107
10108
10109
10110
10111
10112
10113 L3:
10114 ++nt;
10115 if (nt <= *ncol) {
10116 ltri[nt * 6 + 1] = n1;
10117 ltri[nt * 6 + 2] = n2;
10118 ltri[nt * 6 + 3] = n3;
10119 ltri[nt * 6 + 4] = nt + 1;
10120 ltri[nt * 6 + 5] = nt - 1;
10121 ltri[nt * 6 + 6] = 0;
10122 }
10123 n1 = n2;
10124 lp = lend[n1];
10125 n2 = -list[lp];
10126 if (n2 != n3) {
10127 goto L3;
10128 }
10129
10130 *nb = nt + 2;
10131 if (*ncol < nt) {
10132 goto L22;
10133 }
10134 ltri[nt * 6 + 4] = 0;
10135 if (nt == 1) {
10136 goto L7;
10137 }
10138
10139
10140
10141
10142
10143
10144
10145 L4:
10146 swp = FALSE_;
10147 i__1 = nt - 1;
10148 for (kt1 = 1; kt1 <= i__1; ++kt1) {
10149 for (i3 = 1; i3 <= 3; ++i3) {
10150 kt2 = ltri[i3 + 3 + kt1 * 6];
10151 if (kt2 <= kt1) {
10152 goto L5;
10153 }
10154
10155
10156
10157
10158 if (i3 == 1) {
10159 i1 = 2;
10160 i2 = 3;
10161 } else if (i3 == 2) {
10162 i1 = 3;
10163 i2 = 1;
10164 } else {
10165 i1 = 1;
10166 i2 = 2;
10167 }
10168 n1 = ltri[i1 + kt1 * 6];
10169 n2 = ltri[i2 + kt1 * 6];
10170 n3 = ltri[i3 + kt1 * 6];
10171
10172
10173
10174
10175 if (ltri[kt2 * 6 + 4] == kt1) {
10176 i4 = 1;
10177 } else if (ltri[kt2 * 6 + 5] == kt1) {
10178 i4 = 2;
10179 } else {
10180 i4 = 3;
10181 }
10182 n4 = ltri[i4 + kt2 * 6];
10183
10184
10185
10186
10187
10188 if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10189 goto L5;
10190 }
10191
10192
10193
10194
10195 swp = TRUE_;
10196 kt11 = ltri[i1 + 3 + kt1 * 6];
10197 kt12 = ltri[i2 + 3 + kt1 * 6];
10198 if (i4 == 1) {
10199 i2 = 2;
10200 i1 = 3;
10201 } else if (i4 == 2) {
10202 i2 = 3;
10203 i1 = 1;
10204 } else {
10205 i2 = 1;
10206 i1 = 2;
10207 }
10208 kt21 = ltri[i1 + 3 + kt2 * 6];
10209 kt22 = ltri[i2 + 3 + kt2 * 6];
10210 ltri[kt1 * 6 + 1] = n4;
10211 ltri[kt1 * 6 + 2] = n3;
10212 ltri[kt1 * 6 + 3] = n1;
10213 ltri[kt1 * 6 + 4] = kt12;
10214 ltri[kt1 * 6 + 5] = kt22;
10215 ltri[kt1 * 6 + 6] = kt2;
10216 ltri[kt2 * 6 + 1] = n3;
10217 ltri[kt2 * 6 + 2] = n4;
10218 ltri[kt2 * 6 + 3] = n2;
10219 ltri[kt2 * 6 + 4] = kt21;
10220 ltri[kt2 * 6 + 5] = kt11;
10221 ltri[kt2 * 6 + 6] = kt1;
10222
10223
10224
10225 if (kt11 != 0) {
10226 i4 = 4;
10227 if (ltri[kt11 * 6 + 4] != kt1) {
10228 i4 = 5;
10229 if (ltri[kt11 * 6 + 5] != kt1) {
10230 i4 = 6;
10231 }
10232 }
10233 ltri[i4 + kt11 * 6] = kt2;
10234 }
10235 if (kt22 != 0) {
10236 i4 = 4;
10237 if (ltri[kt22 * 6 + 4] != kt2) {
10238 i4 = 5;
10239 if (ltri[kt22 * 6 + 5] != kt2) {
10240 i4 = 6;
10241 }
10242 }
10243 ltri[i4 + kt22 * 6] = kt1;
10244 }
10245 L5:
10246 ;
10247 }
10248
10249 }
10250 if (swp) {
10251 goto L4;
10252 }
10253
10254
10255
10256
10257 L7:
10258 i__1 = nt;
10259 for (kt = 1; kt <= i__1; ++kt) {
10260 n1 = ltri[kt * 6 + 1];
10261 n2 = ltri[kt * 6 + 2];
10262 n3 = ltri[kt * 6 + 3];
10263 v1[0] = x[n1];
10264 v1[1] = y[n1];
10265 v1[2] = z__[n1];
10266 v2[0] = x[n2];
10267 v2[1] = y[n2];
10268 v2[2] = z__[n2];
10269 v3[0] = x[n3];
10270 v3[1] = y[n3];
10271 v3[2] = z__[n3];
10272 circum_(v2, v1, v3, c__, &ierr);
10273 if (ierr != 0) {
10274 goto L23;
10275 }
10276
10277
10278
10279
10280 xc[kt] = -c__[0];
10281 yc[kt] = -c__[1];
10282 zc[kt] = -c__[2];
10283 t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10284 if (t < -1.) {
10285 t = -1.;
10286 }
10287 if (t > 1.) {
10288 t = 1.;
10289 }
10290 rc[kt] = acos(t);
10291
10292 }
10293
10294
10295
10296
10297
10298
10299 L9:
10300 kt = nt;
10301
10302
10303
10304 nm2 = nn - 2;
10305 i__1 = nm2;
10306 for (n1 = 1; n1 <= i__1; ++n1) {
10307 lpl = lend[n1];
10308 lp = lpl;
10309 n3 = list[lp];
10310
10311
10312
10313
10314 L10:
10315 lp = lptr[lp];
10316 n2 = n3;
10317 n3 = (i__2 = list[lp], abs(i__2));
10318 if (n2 <= n1 || n3 <= n1) {
10319 goto L11;
10320 }
10321 ++kt;
10322
10323
10324
10325 v1[0] = x[n1];
10326 v1[1] = y[n1];
10327 v1[2] = z__[n1];
10328 v2[0] = x[n2];
10329 v2[1] = y[n2];
10330 v2[2] = z__[n2];
10331 v3[0] = x[n3];
10332 v3[1] = y[n3];
10333 v3[2] = z__[n3];
10334 circum_(v1, v2, v3, c__, &ierr);
10335 if (ierr != 0) {
10336 goto L23;
10337 }
10338
10339
10340
10341 xc[kt] = c__[0];
10342 yc[kt] = c__[1];
10343 zc[kt] = c__[2];
10344 t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10345 if (t < -1.) {
10346 t = -1.;
10347 }
10348 if (t > 1.) {
10349 t = 1.;
10350 }
10351 rc[kt] = acos(t);
10352
10353
10354
10355
10356
10357 lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10358 listc[lpn] = kt;
10359 lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10360 listc[lpn] = kt;
10361 lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10362 listc[lpn] = kt;
10363 L11:
10364 if (lp != lpl) {
10365 goto L10;
10366 }
10367
10368 }
10369 if (nt == 0) {
10370 goto L20;
10371 }
10372
10373
10374
10375
10376
10377
10378 kt1 = 0;
10379 L13:
10380 ++kt1;
10381 if (ltri[kt1 * 6 + 4] == 0) {
10382 i1 = 2;
10383 i2 = 3;
10384 i3 = 1;
10385 goto L14;
10386 } else if (ltri[kt1 * 6 + 5] == 0) {
10387 i1 = 3;
10388 i2 = 1;
10389 i3 = 2;
10390 goto L14;
10391 } else if (ltri[kt1 * 6 + 6] == 0) {
10392 i1 = 1;
10393 i2 = 2;
10394 i3 = 3;
10395 goto L14;
10396 }
10397 goto L13;
10398 L14:
10399 n1 = ltri[i1 + kt1 * 6];
10400 n0 = n1;
10401
10402
10403
10404
10405
10406
10407
10408
10409
10410 L15:
10411 lp = lend[n1];
10412 lpn = lptr[lp];
10413 listc[lp] = kt1;
10414
10415
10416
10417 L16:
10418 kt2 = ltri[i2 + 3 + kt1 * 6];
10419 if (kt2 != 0) {
10420
10421
10422
10423 lptr[lp] = *lnew;
10424 lp = *lnew;
10425 listc[lp] = kt2;
10426 ++(*lnew);
10427
10428
10429
10430
10431 kt1 = kt2;
10432 if (ltri[kt1 * 6 + 1] == n1) {
10433 i1 = 1;
10434 i2 = 2;
10435 i3 = 3;
10436 } else if (ltri[kt1 * 6 + 2] == n1) {
10437 i1 = 2;
10438 i2 = 3;
10439 i3 = 1;
10440 } else {
10441 i1 = 3;
10442 i2 = 1;
10443 i3 = 2;
10444 }
10445 goto L16;
10446 }
10447
10448
10449
10450
10451
10452
10453
10454 lptr[lp] = lpn;
10455 n1 = ltri[i3 + kt1 * 6];
10456 if (n1 != n0) {
10457 i4 = i3;
10458 i3 = i2;
10459 i2 = i1;
10460 i1 = i4;
10461 goto L15;
10462 }
10463
10464
10465
10466 L20:
10467 *ier = 0;
10468 return 0;
10469
10470
10471
10472 L21:
10473 *ier = 1;
10474 return 0;
10475
10476
10477
10478 L22:
10479 *ier = 2;
10480 return 0;
10481
10482
10483
10484 L23:
10485 *ier = 3;
10486 return 0;
10487 }
10488
10489 int delarc_(int *n, int *io1, int *io2, int *
10490 list, int *lptr, int *lend, int *lnew, int *ier)
10491 {
10492
10493 int i__1;
10494
10495
10496 static int n1, n2, n3, lp, lph, lpl;
10497 int delnb_(int *, int *, int *,
10498 int *, int *, int *, int *, int *);
10499 int lstptr_(int *, int *, int *, int *);
10500
10501
10502
10503
10504
10505
10506
10507
10508
10509
10510
10511
10512
10513
10514
10515
10516
10517
10518
10519
10520
10521
10522
10523
10524
10525
10526
10527
10528
10529
10530
10531
10532
10533
10534
10535
10536
10537
10538
10539
10540
10541
10542
10543
10544
10545
10546
10547
10548
10549
10550
10551
10552
10553
10554
10555
10556
10557
10558
10559
10560
10561
10562
10563
10564
10565
10566
10567
10568
10569
10570
10571
10572
10573 --lend;
10574 --list;
10575 --lptr;
10576
10577
10578 n1 = *io1;
10579 n2 = *io2;
10580
10581
10582
10583
10584
10585 if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10586 *ier = 1;
10587 return 0;
10588 }
10589
10590 lpl = lend[n2];
10591 if (-list[lpl] != n1) {
10592 n1 = n2;
10593 n2 = *io1;
10594 lpl = lend[n2];
10595 if (-list[lpl] != n1) {
10596 *ier = 2;
10597 return 0;
10598 }
10599 }
10600
10601
10602
10603
10604
10605 lpl = lend[n1];
10606 lp = lptr[lpl];
10607 lp = lptr[lp];
10608 n3 = (i__1 = list[lp], abs(i__1));
10609 lpl = lend[n3];
10610 if (list[lpl] <= 0) {
10611 *ier = 3;
10612 return 0;
10613 }
10614
10615
10616
10617
10618
10619
10620 delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10621 if (lph < 0) {
10622 *ier = 4;
10623 return 0;
10624 }
10625
10626
10627
10628
10629 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10630
10631
10632
10633
10634 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10635 lend[n3] = lp;
10636 list[lp] = -n1;
10637
10638
10639
10640 *ier = 0;
10641 return 0;
10642 }
10643
10644 int delnb_(int *n0, int *nb, int *n, int *
10645 list, int *lptr, int *lend, int *lnew, int *lph)
10646 {
10647
10648 int i__1;
10649
10650
10651 static int i__, lp, nn, lpb, lpl, lpp, lnw;
10652
10653
10654
10655
10656
10657
10658
10659
10660
10661
10662
10663
10664
10665
10666
10667
10668
10669
10670
10671
10672
10673
10674
10675
10676
10677
10678
10679
10680
10681
10682
10683
10684
10685
10686
10687
10688
10689
10690
10691
10692
10693
10694
10695
10696
10697
10698
10699
10700
10701
10702
10703
10704
10705
10706
10707
10708
10709
10710
10711
10712
10713
10714
10715
10716
10717
10718
10719
10720
10721
10722
10723 --lend;
10724 --list;
10725 --lptr;
10726
10727
10728 nn = *n;
10729
10730
10731
10732 if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10733 *lph = -1;
10734 return 0;
10735 }
10736
10737
10738
10739
10740
10741
10742
10743 lpl = lend[*n0];
10744 lpp = lpl;
10745 lpb = lptr[lpp];
10746 L1:
10747 if (list[lpb] == *nb) {
10748 goto L2;
10749 }
10750 lpp = lpb;
10751 lpb = lptr[lpp];
10752 if (lpb != lpl) {
10753 goto L1;
10754 }
10755
10756
10757
10758 if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10759 *lph = -2;
10760 return 0;
10761 }
10762
10763
10764
10765
10766
10767 lend[*n0] = lpp;
10768 lp = lend[*nb];
10769 if (list[lp] < 0) {
10770 list[lpp] = -list[lpp];
10771 }
10772 goto L3;
10773
10774
10775
10776
10777
10778 L2:
10779 lp = lend[*nb];
10780 if (list[lp] < 0 && list[lpl] > 0) {
10781 lend[*n0] = lpp;
10782 list[lpp] = -list[lpp];
10783 }
10784
10785
10786
10787
10788 L3:
10789 lptr[lpp] = lptr[lpb];
10790 lnw = *lnew - 1;
10791 list[lpb] = list[lnw];
10792 lptr[lpb] = lptr[lnw];
10793 for (i__ = nn; i__ >= 1; --i__) {
10794 if (lend[i__] == lnw) {
10795 lend[i__] = lpb;
10796 goto L5;
10797 }
10798
10799 }
10800
10801 L5:
10802 i__1 = lnw - 1;
10803 for (i__ = 1; i__ <= i__1; ++i__) {
10804 if (lptr[i__] == lnw) {
10805 lptr[i__] = lpb;
10806 }
10807
10808 }
10809
10810
10811
10812 *lnew = lnw;
10813 *lph = lpb;
10814 return 0;
10815 }
10816
10817 int delnod_(int *k, int *n, double *x,
10818 double *y, double *z__, int *list, int *lptr, int
10819 *lend, int *lnew, int *lwk, int *iwk, int *ier)
10820 {
10821
10822 int i__1;
10823
10824
10825 static int i__, j, n1, n2;
10826 static double x1, x2, y1, y2, z1, z2;
10827 static int nl, lp, nn, nr;
10828 static double xl, yl, zl, xr, yr, zr;
10829 static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10830 static long int bdry;
10831 static int ierr, lwkl;
10832 int swap_(int *, int *, int *,
10833 int *, int *, int *, int *, int *), delnb_(
10834 int *, int *, int *, int *, int *, int *,
10835 int *, int *);
10836 int nbcnt_(int *, int *);
10837 int optim_(double *, double *, double
10838 *, int *, int *, int *, int *, int *, int
10839 *, int *);
10840 static int nfrst;
10841 int lstptr_(int *, int *, int *, int *);
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
10919
10920
10921
10922
10923
10924
10925
10926
10927
10928
10929
10930
10931
10932
10933
10934
10935
10936
10937
10938
10939
10940
10941
10942
10943
10944
10945
10946
10947
10948
10949
10950
10951
10952
10953
10954
10955
10956
10957
10958
10959
10960
10961
10962
10963
10964
10965
10966
10967
10968
10969
10970
10971
10972
10973
10974
10975
10976
10977
10978
10979
10980
10981
10982
10983
10984
10985 iwk -= 3;
10986 --lend;
10987 --lptr;
10988 --list;
10989 --z__;
10990 --y;
10991 --x;
10992
10993
10994 n1 = *k;
10995 nn = *n;
10996 if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
10997 goto L21;
10998 }
10999 lpl = lend[n1];
11000 lpf = lptr[lpl];
11001 nnb = nbcnt_(&lpl, &lptr[1]);
11002 bdry = list[lpl] < 0;
11003 if (bdry) {
11004 ++nnb;
11005 }
11006 if (nnb < 3) {
11007 goto L23;
11008 }
11009 lwkl = *lwk;
11010 *lwk = nnb - 3;
11011 if (lwkl < *lwk) {
11012 goto L22;
11013 }
11014 iwl = 0;
11015 if (nnb == 3) {
11016 goto L3;
11017 }
11018
11019
11020
11021
11022
11023
11024
11025 x1 = x[n1];
11026 y1 = y[n1];
11027 z1 = z__[n1];
11028 nfrst = list[lpf];
11029 nr = nfrst;
11030 xr = x[nr];
11031 yr = y[nr];
11032 zr = z__[nr];
11033 lp = lptr[lpf];
11034 n2 = list[lp];
11035 x2 = x[n2];
11036 y2 = y[n2];
11037 z2 = z__[n2];
11038 lp = lptr[lp];
11039
11040
11041
11042 L1:
11043 nl = (i__1 = list[lp], abs(i__1));
11044 if (nl == nfrst && bdry) {
11045 goto L3;
11046 }
11047 xl = x[nl];
11048 yl = y[nl];
11049 zl = z__[nl];
11050
11051
11052
11053
11054
11055
11056 lpl2 = lend[n2];
11057 if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
11058 list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
11059 z2)))) {
11060
11061
11062
11063 nr = n2;
11064 xr = x2;
11065 yr = y2;
11066 zr = z2;
11067 goto L2;
11068 }
11069
11070
11071
11072
11073
11074
11075
11076
11077 swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11078 if (lp21 == 0) {
11079 nr = n2;
11080 xr = x2;
11081 yr = y2;
11082 zr = z2;
11083 goto L2;
11084 }
11085 ++iwl;
11086 if (nl <= n1) {
11087 iwk[(iwl << 1) + 1] = nl;
11088 } else {
11089 iwk[(iwl << 1) + 1] = nl - 1;
11090 }
11091 if (nr <= n1) {
11092 iwk[(iwl << 1) + 2] = nr;
11093 } else {
11094 iwk[(iwl << 1) + 2] = nr - 1;
11095 }
11096
11097
11098
11099 lpl = lend[n1];
11100 --nnb;
11101 if (nnb == 3) {
11102 goto L3;
11103 }
11104 lpf = lptr[lpl];
11105 nfrst = list[lpf];
11106 lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11107 if (nr == nfrst) {
11108 goto L2;
11109 }
11110
11111
11112
11113
11114
11115
11116
11117 n2 = nr;
11118 x2 = xr;
11119 y2 = yr;
11120 z2 = zr;
11121 lp21 = lptr[lp21];
11122 lp21 = lptr[lp21];
11123 nr = (i__1 = list[lp21], abs(i__1));
11124 xr = x[nr];
11125 yr = y[nr];
11126 zr = z__[nr];
11127 goto L1;
11128
11129
11130
11131 L2:
11132 if (n2 == nfrst) {
11133 goto L3;
11134 }
11135 n2 = nl;
11136 x2 = xl;
11137 y2 = yl;
11138 z2 = zl;
11139 lp = lptr[lp];
11140 goto L1;
11141
11142
11143
11144
11145
11146
11147
11148
11149
11150
11151 L3:
11152 if (! bdry) {
11153 if (nnb > 3) {
11154 bdry = TRUE_;
11155 } else {
11156 lpf = lptr[lpl];
11157 nr = list[lpf];
11158 lp = lptr[lpf];
11159 n2 = list[lp];
11160 nl = list[lpl];
11161 bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11162 x[n2], &y[n2], &z__[n2]);
11163 }
11164 if (bdry) {
11165
11166
11167
11168
11169
11170
11171 i__1 = nn;
11172 for (i__ = 1; i__ <= i__1; ++i__) {
11173 if (list[lend[i__]] < 0) {
11174 bdry = FALSE_;
11175 goto L5;
11176 }
11177
11178 }
11179 list[lpl] = -list[lpl];
11180 ++nnb;
11181 }
11182 }
11183 L5:
11184 if (! bdry && nnb > 3) {
11185 goto L24;
11186 }
11187
11188
11189
11190
11191 lp = lpl;
11192 lnw = *lnew;
11193
11194
11195
11196 L6:
11197 lp = lptr[lp];
11198 n2 = (i__1 = list[lp], abs(i__1));
11199 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11200 if (lph < 0) {
11201 goto L23;
11202 }
11203
11204
11205
11206 if (lpl == lnw) {
11207 lpl = lph;
11208 }
11209 if (lp == lnw) {
11210 lp = lph;
11211 }
11212 if (lp != lpl) {
11213 goto L6;
11214 }
11215
11216
11217
11218
11219
11220 --nn;
11221 if (n1 > nn) {
11222 goto L9;
11223 }
11224 i__1 = nn;
11225 for (i__ = n1; i__ <= i__1; ++i__) {
11226 x[i__] = x[i__ + 1];
11227 y[i__] = y[i__ + 1];
11228 z__[i__] = z__[i__ + 1];
11229 lend[i__] = lend[i__ + 1];
11230
11231 }
11232
11233 i__1 = lnw - 1;
11234 for (i__ = 1; i__ <= i__1; ++i__) {
11235 if (list[i__] > n1) {
11236 --list[i__];
11237 }
11238 if (list[i__] < -n1) {
11239 ++list[i__];
11240 }
11241
11242 }
11243
11244
11245
11246
11247
11248
11249
11250
11251
11252
11253
11254 L9:
11255 if (bdry) {
11256 --nnb;
11257 }
11258 lpn = lpl;
11259 i__1 = nnb;
11260 for (j = 1; j <= i__1; ++j) {
11261 --lnw;
11262 lp = lpn;
11263 lpn = lptr[lp];
11264 list[lp] = list[lnw];
11265 lptr[lp] = lptr[lnw];
11266 if (lptr[lpn] == lnw) {
11267 lptr[lpn] = lp;
11268 }
11269 if (lpn == lnw) {
11270 lpn = lp;
11271 }
11272 for (i__ = nn; i__ >= 1; --i__) {
11273 if (lend[i__] == lnw) {
11274 lend[i__] = lp;
11275 goto L11;
11276 }
11277
11278 }
11279
11280 L11:
11281 for (i__ = lnw - 1; i__ >= 1; --i__) {
11282 if (lptr[i__] == lnw) {
11283 lptr[i__] = lp;
11284 }
11285
11286 }
11287
11288 }
11289
11290
11291
11292
11293
11294 *n = nn;
11295 *lnew = lnw;
11296 if (iwl > 0) {
11297 nit = iwl << 2;
11298 optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11299 nit, &iwk[3], &ierr);
11300 if (ierr != 0 && ierr != 1) {
11301 goto L25;
11302 }
11303 if (ierr == 1) {
11304 goto L26;
11305 }
11306 }
11307
11308
11309
11310 *ier = 0;
11311 return 0;
11312
11313
11314
11315 L21:
11316 *ier = 1;
11317 return 0;
11318
11319
11320
11321 L22:
11322 *ier = 2;
11323 return 0;
11324
11325
11326
11327
11328 L23:
11329 *ier = 3;
11330 return 0;
11331
11332
11333
11334 L24:
11335 *ier = 4;
11336 return 0;
11337
11338
11339
11340 L25:
11341 *ier = 5;
11342
11343
11344
11345 return 0;
11346
11347
11348
11349 L26:
11350 *ier = 6;
11351 return 0;
11352 }
11353
11354 int drwarc_(int *, double *p, double *q,
11355 double *tol, int *nseg)
11356 {
11357
11358 int i__1;
11359 double d__1;
11360
11361
11362
11363
11364
11365 static int i__, k;
11366 static double s, p1[3], p2[3], u1, u2, v1, v2;
11367 static int na;
11368 static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11369
11370
11371
11372
11373
11374
11375
11376
11377
11378
11379
11380
11381
11382
11383
11384
11385
11386
11387
11388
11389
11390
11391
11392
11393
11394
11395
11396
11397
11398
11399
11400
11401
11402
11403
11404
11405
11406
11407
11408
11409
11410
11411
11412
11413
11414
11415
11416
11417
11418
11419
11420
11421
11422
11423
11424
11425
11426
11427
11428
11429
11430
11431
11432
11433
11434
11435
11436
11437
11438
11439
11440
11441
11442
11443
11444
11445
11446 --q;
11447 --p;
11448
11449
11450 enrm = 0.;
11451 for (i__ = 1; i__ <= 3; ++i__) {
11452 pm[i__ - 1] = p[i__] + q[i__];
11453 enrm += pm[i__ - 1] * pm[i__ - 1];
11454
11455 }
11456 if (enrm == 0.) {
11457 goto L5;
11458 }
11459 enrm = sqrt(enrm);
11460 pm[0] /= enrm;
11461 pm[1] /= enrm;
11462 pm[2] /= enrm;
11463
11464
11465
11466
11467 u1 = p[1];
11468 v1 = p[2];
11469 u2 = q[1];
11470 v2 = q[2];
11471 um = pm[0];
11472 vm = pm[1];
11473
11474
11475
11476
11477
11478
11479 du = u2 - u1;
11480 dv = v2 - v1;
11481 enrm = du * du + dv * dv;
11482 if (enrm == 0.) {
11483 goto L5;
11484 }
11485 err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11486
11487
11488
11489
11490
11491 na = (int) (err / *tol + 1.);
11492
11493
11494
11495
11496
11497 s = 1. / (double) na;
11498 for (i__ = 1; i__ <= 3; ++i__) {
11499 dp[i__ - 1] = s * (q[i__] - p[i__]);
11500 pm[i__ - 1] = p[i__];
11501 p1[i__ - 1] = p[i__];
11502
11503 }
11504
11505
11506
11507
11508 i__1 = na - 1;
11509 for (k = 1; k <= i__1; ++k) {
11510 enrm = 0.;
11511 for (i__ = 1; i__ <= 3; ++i__) {
11512 pm[i__ - 1] += dp[i__ - 1];
11513 enrm += pm[i__ - 1] * pm[i__ - 1];
11514
11515 }
11516 if (enrm == 0.) {
11517 goto L5;
11518 }
11519 enrm = sqrt(enrm);
11520 p2[0] = pm[0] / enrm;
11521 p2[1] = pm[1] / enrm;
11522 p2[2] = pm[2] / enrm;
11523
11524
11525 p1[0] = p2[0];
11526 p1[1] = p2[1];
11527 p1[2] = p2[2];
11528
11529 }
11530
11531
11532
11533
11534 *nseg = na;
11535 return 0;
11536
11537
11538
11539 L5:
11540 *nseg = 0;
11541 return 0;
11542 }
11543
11544 int edge_(int *in1, int *in2, double *x,
11545 double *y, double *z__, int *lwk, int *iwk, int *
11546 list, int *lptr, int *lend, int *ier)
11547 {
11548
11549 int i__1;
11550
11551
11552 static int i__, n0, n1, n2;
11553 static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11554 static int nl, lp, nr;
11555 static double dp12;
11556 static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11557 static double dp1l, dp2l, dp1r, dp2r;
11558 static int ierr;
11559 int swap_(int *, int *, int *,
11560 int *, int *, int *, int *, int *);
11561 static int next, iwcp1, n1lst, iwend;
11562 int optim_(double *, double *, double
11563 *, int *, int *, int *, int *, int *, int
11564 *, int *);
11565 static int n1frst;
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
11637
11638
11639
11640
11641
11642
11643
11644
11645
11646
11647
11648
11649
11650
11651
11652
11653
11654
11655
11656
11657
11658
11659
11660
11661
11662
11663
11664
11665
11666
11667
11668
11669
11670
11671
11672
11673
11674
11675
11676
11677
11678
11679
11680
11681
11682
11683
11684
11685
11686
11687
11688
11689
11690
11691
11692
11693
11694
11695
11696
11697
11698
11699
11700
11701
11702
11703 --lend;
11704 --lptr;
11705 --list;
11706 iwk -= 3;
11707 --z__;
11708 --y;
11709 --x;
11710
11711
11712 n1 = *in1;
11713 n2 = *in2;
11714 iwend = *lwk;
11715 if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11716 goto L31;
11717 }
11718
11719
11720
11721
11722 lpl = lend[n1];
11723 n0 = (i__1 = list[lpl], abs(i__1));
11724 lp = lpl;
11725 L1:
11726 if (n0 == n2) {
11727 goto L30;
11728 }
11729 lp = lptr[lp];
11730 n0 = list[lp];
11731 if (lp != lpl) {
11732 goto L1;
11733 }
11734
11735
11736
11737 iwl = 0;
11738 nit = 0;
11739
11740
11741
11742 L2:
11743 x1 = x[n1];
11744 y1 = y[n1];
11745 z1 = z__[n1];
11746 x2 = x[n2];
11747 y2 = y[n2];
11748 z2 = z__[n2];
11749
11750
11751
11752
11753
11754
11755
11756
11757
11758
11759 lpl = lend[n1];
11760 n1lst = list[lpl];
11761 lp = lptr[lpl];
11762 n1frst = list[lp];
11763 nl = n1frst;
11764 if (n1lst < 0) {
11765 goto L4;
11766 }
11767
11768
11769
11770
11771 L3:
11772 if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11773 goto L4;
11774 }
11775 lp = lptr[lp];
11776 nl = list[lp];
11777 if (nl != n1frst) {
11778 goto L3;
11779 }
11780
11781
11782
11783 goto L5;
11784
11785
11786
11787
11788 L4:
11789 nr = nl;
11790 lp = lptr[lp];
11791 nl = (i__1 = list[lp], abs(i__1));
11792 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11793
11794
11795
11796
11797
11798 dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11799 dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11800 dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11801 dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11802 dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11803 if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11804 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11805 goto L6;
11806 }
11807
11808
11809
11810
11811
11812 if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11813 goto L5;
11814 }
11815 }
11816
11817
11818
11819 if (nl != n1frst) {
11820 goto L4;
11821 }
11822
11823
11824
11825
11826
11827
11828
11829 L5:
11830 if (nit > 0) {
11831 goto L33;
11832 }
11833 nit = 1;
11834 n1 = n2;
11835 n2 = *in1;
11836 goto L2;
11837
11838
11839
11840
11841 L6:
11842 ++iwl;
11843 if (iwl > iwend) {
11844 goto L32;
11845 }
11846 iwk[(iwl << 1) + 1] = nl;
11847 iwk[(iwl << 1) + 2] = nr;
11848
11849
11850
11851 lpl = lend[nl];
11852 lp = lptr[lpl];
11853
11854
11855
11856
11857 L7:
11858 if (list[lp] == nr) {
11859 goto L8;
11860 }
11861 lp = lptr[lp];
11862 if (lp != lpl) {
11863 goto L7;
11864 }
11865
11866
11867
11868
11869 if (list[lp] != nr) {
11870 goto L33;
11871 }
11872
11873
11874
11875
11876 L8:
11877 lp = lptr[lp];
11878 next = (i__1 = list[lp], abs(i__1));
11879 if (next == n2) {
11880 goto L9;
11881 }
11882
11883
11884
11885 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11886 nl = next;
11887 } else {
11888 nr = next;
11889 }
11890 goto L6;
11891
11892
11893
11894
11895 L9:
11896 *lwk = iwl;
11897 iwend = iwl;
11898
11899
11900
11901
11902
11903
11904
11905
11906 iwf = 1;
11907
11908
11909
11910
11911
11912 L10:
11913 lft = 0;
11914 n0 = n1;
11915 x0 = x1;
11916 y0 = y1;
11917 z0 = z1;
11918 nl = iwk[(iwf << 1) + 1];
11919 nr = iwk[(iwf << 1) + 2];
11920 iwc = iwf;
11921
11922
11923
11924
11925 L11:
11926 if (iwc == iwl) {
11927 goto L21;
11928 }
11929 iwcp1 = iwc + 1;
11930 next = iwk[(iwcp1 << 1) + 1];
11931 if (next != nl) {
11932 goto L16;
11933 }
11934 next = iwk[(iwcp1 << 1) + 2];
11935
11936
11937
11938
11939 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11940 z__[next])) {
11941 goto L14;
11942 }
11943 if (lft >= 0) {
11944 goto L12;
11945 }
11946 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11947 z__[next])) {
11948 goto L14;
11949 }
11950
11951
11952
11953 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11954 iwk[(iwc << 1) + 1] = n0;
11955 iwk[(iwc << 1) + 2] = next;
11956 goto L15;
11957
11958
11959
11960
11961
11962 L12:
11963 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11964 i__1 = iwl;
11965 for (i__ = iwcp1; i__ <= i__1; ++i__) {
11966 iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11967 iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11968
11969 }
11970 iwk[(iwl << 1) + 1] = n0;
11971 iwk[(iwl << 1) + 2] = next;
11972 --iwl;
11973 nr = next;
11974 goto L11;
11975
11976
11977
11978 L14:
11979 n0 = nr;
11980 x0 = x[n0];
11981 y0 = y[n0];
11982 z0 = z__[n0];
11983 lft = 1;
11984
11985
11986
11987 L15:
11988 nr = next;
11989 ++iwc;
11990 goto L11;
11991
11992
11993
11994
11995 L16:
11996 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11997 z__[next])) {
11998 goto L19;
11999 }
12000 if (lft <= 0) {
12001 goto L17;
12002 }
12003 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
12004 z__[next])) {
12005 goto L19;
12006 }
12007
12008
12009
12010 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12011 iwk[(iwc << 1) + 1] = next;
12012 iwk[(iwc << 1) + 2] = n0;
12013 goto L20;
12014
12015
12016
12017
12018
12019 L17:
12020 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12021 i__1 = iwf;
12022 for (i__ = iwc - 1; i__ >= i__1; --i__) {
12023 iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
12024 iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
12025
12026 }
12027 iwk[(iwf << 1) + 1] = n0;
12028 iwk[(iwf << 1) + 2] = next;
12029 ++iwf;
12030 goto L20;
12031
12032
12033
12034 L19:
12035 n0 = nl;
12036 x0 = x[n0];
12037 y0 = y[n0];
12038 z0 = z__[n0];
12039 lft = -1;
12040
12041
12042
12043 L20:
12044 nl = next;
12045 ++iwc;
12046 goto L11;
12047
12048
12049
12050 L21:
12051 if (n0 == n1) {
12052 goto L24;
12053 }
12054 if (lft < 0) {
12055 goto L22;
12056 }
12057
12058
12059
12060 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
12061 goto L10;
12062 }
12063
12064
12065
12066
12067 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12068 iwk[(iwl << 1) + 1] = n0;
12069 iwk[(iwl << 1) + 2] = n2;
12070 --iwl;
12071 goto L10;
12072
12073
12074
12075 L22:
12076 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12077 goto L10;
12078 }
12079
12080
12081
12082
12083 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12084 i__ = iwl;
12085 L23:
12086 iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12087 iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12088 --i__;
12089 if (i__ > iwf) {
12090 goto L23;
12091 }
12092 iwk[(iwf << 1) + 1] = n0;
12093 iwk[(iwf << 1) + 2] = n2;
12094 ++iwf;
12095 goto L10;
12096
12097
12098
12099
12100 L24:
12101 swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12102 iwk[(iwc << 1) + 1] = 0;
12103 iwk[(iwc << 1) + 2] = 0;
12104
12105
12106
12107 *ier = 0;
12108 if (iwc > 1) {
12109
12110
12111
12112 nit = iwc - (1<<2);
12113 i__1 = iwc - 1;
12114 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12115 nit, &iwk[3], &ierr);
12116 if (ierr != 0 && ierr != 1) {
12117 goto L34;
12118 }
12119 if (ierr == 1) {
12120 *ier = 5;
12121 }
12122 }
12123 if (iwc < iwend) {
12124
12125
12126
12127 nit = iwend - (iwc<<2);
12128 i__1 = iwend - iwc;
12129 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12130 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12131 if (ierr != 0 && ierr != 1) {
12132 goto L34;
12133 }
12134 if (ierr == 1) {
12135 goto L35;
12136 }
12137 }
12138 if (*ier == 5) {
12139 goto L35;
12140 }
12141
12142
12143
12144 return 0;
12145
12146
12147
12148 L30:
12149 *ier = 0;
12150 return 0;
12151
12152
12153
12154 L31:
12155 *ier = 1;
12156 return 0;
12157
12158
12159
12160 L32:
12161 *ier = 2;
12162 return 0;
12163
12164
12165
12166
12167 L33:
12168 *ier = 3;
12169
12170
12171
12172
12173 return 0;
12174
12175
12176
12177 L34:
12178 *ier = 4;
12179
12180
12181
12182 return 0;
12183
12184
12185
12186 L35:
12187 *ier = 5;
12188 return 0;
12189 }
12190
12191 int getnp_(double *x, double *y, double *z__,
12192 int *list, int *lptr, int *lend, int *l, int *
12193 npts, double *df, int *ier)
12194 {
12195
12196 int i__1, i__2;
12197
12198
12199 static int i__, n1;
12200 static double x1, y1, z1;
12201 static int nb, ni, lp, np, lm1;
12202 static double dnb, dnp;
12203 static int lpl;
12204
12205
12206
12207
12208
12209
12210
12211
12212
12213
12214
12215
12216
12217
12218
12219
12220
12221
12222
12223
12224
12225
12226
12227
12228
12229
12230
12231
12232
12233
12234
12235
12236
12237
12238
12239
12240
12241
12242
12243
12244
12245
12246
12247
12248
12249
12250
12251
12252
12253
12254
12255
12256
12257
12258
12259
12260
12261
12262
12263
12264
12265
12266
12267
12268
12269
12270
12271
12272
12273
12274
12275
12276
12277
12278
12279
12280
12281
12282
12283 --x;
12284 --y;
12285 --z__;
12286 --list;
12287 --lptr;
12288 --lend;
12289 --npts;
12290
12291
12292 lm1 = *l - 1;
12293 if (lm1 < 1) {
12294 goto L6;
12295 }
12296 *ier = 0;
12297
12298
12299
12300 n1 = npts[1];
12301 x1 = x[n1];
12302 y1 = y[n1];
12303 z1 = z__[n1];
12304 i__1 = lm1;
12305 for (i__ = 1; i__ <= i__1; ++i__) {
12306 ni = npts[i__];
12307 lend[ni] = -lend[ni];
12308
12309 }
12310
12311
12312
12313
12314
12315 dnp = 2.;
12316
12317
12318
12319 i__1 = lm1;
12320 for (i__ = 1; i__ <= i__1; ++i__) {
12321 ni = npts[i__];
12322 lpl = -lend[ni];
12323 lp = lpl;
12324
12325
12326
12327 L2:
12328 nb = (i__2 = list[lp], abs(i__2));
12329 if (lend[nb] < 0) {
12330 goto L3;
12331 }
12332
12333
12334
12335
12336 dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12337 if (dnb >= dnp) {
12338 goto L3;
12339 }
12340 np = nb;
12341 dnp = dnb;
12342 L3:
12343 lp = lptr[lp];
12344 if (lp != lpl) {
12345 goto L2;
12346 }
12347
12348 }
12349 npts[*l] = np;
12350 *df = dnp;
12351
12352
12353
12354 i__1 = lm1;
12355 for (i__ = 1; i__ <= i__1; ++i__) {
12356 ni = npts[i__];
12357 lend[ni] = -lend[ni];
12358
12359 }
12360 return 0;
12361
12362
12363
12364 L6:
12365 *ier = 1;
12366 return 0;
12367 }
12368
12369 int insert_(int *k, int *lp, int *list, int *
12370 lptr, int *lnew)
12371 {
12372 static int lsav;
12373
12374
12375
12376
12377
12378
12379
12380
12381
12382
12383
12384
12385
12386
12387
12388
12389
12390
12391
12392
12393
12394
12395
12396
12397
12398
12399
12400
12401
12402
12403
12404
12405
12406
12407
12408
12409
12410
12411
12412
12413
12414
12415
12416 --lptr;
12417 --list;
12418
12419
12420 lsav = lptr[*lp];
12421 lptr[*lp] = *lnew;
12422 list[*lnew] = *k;
12423 lptr[*lnew] = lsav;
12424 ++(*lnew);
12425 return 0;
12426 }
12427
12428 long int inside_(double *p, int *lv, double *xv, double *yv,
12429 double *zv, int *nv, int *listv, int *ier)
12430 {
12431
12432
12433 static double eps = .001;
12434
12435
12436 int i__1;
12437 long int ret_val = 0;
12438
12439
12440
12441
12442
12443 static double b[3], d__;
12444 static int k, n;
12445 static double q[3];
12446 static int i1, i2, k0;
12447 static double v1[3], v2[3], cn[3], bp, bq;
12448 static int ni;
12449 static double pn[3], qn[3], vn[3];
12450 static int imx;
12451 static long int lft1, lft2, even;
12452 static int ierr;
12453 static long int pinr, qinr;
12454 static double qnrm, vnrm;
12455 int intrsc_(double *, double *,
12456 double *, double *, int *);
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
12543
12544
12545
12546
12547
12548
12549
12550
12551
12552
12553
12554
12555
12556
12557
12558
12559
12560
12561
12562
12563
12564
12565
12566
12567
12568
12569
12570
12571
12572
12573
12574
12575
12576
12577
12578
12579
12580
12581
12582
12583
12584
12585
12586
12587
12588
12589
12590
12591
12592
12593
12594
12595
12596
12597
12598
12599
12600
12601
12602
12603
12604
12605
12606
12607
12608
12609 --p;
12610 --zv;
12611 --yv;
12612 --xv;
12613 --listv;
12614
12615
12616
12617
12618
12619
12620 imx = *lv;
12621 n = *nv;
12622 if (n < 3 || n > imx) {
12623 goto L11;
12624 }
12625 k0 = 0;
12626 i1 = listv[1];
12627 if (i1 < 1 || i1 > imx) {
12628 goto L12;
12629 }
12630
12631
12632
12633
12634
12635 L1:
12636 ++k0;
12637 if (k0 > n) {
12638 goto L14;
12639 }
12640 i1 = listv[k0];
12641 if (k0 < n) {
12642 i2 = listv[k0 + 1];
12643 } else {
12644 i2 = listv[1];
12645 }
12646 if (i2 < 1 || i2 > imx) {
12647 goto L12;
12648 }
12649 vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12650 vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12651 vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12652 vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12653 if (vnrm == 0.) {
12654 goto L1;
12655 }
12656 q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12657 q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12658 q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12659 qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12660 q[0] /= qnrm;
12661 q[1] /= qnrm;
12662 q[2] /= qnrm;
12663
12664
12665
12666 cn[0] = q[1] * p[3] - q[2] * p[2];
12667 cn[1] = q[2] * p[1] - q[0] * p[3];
12668 cn[2] = q[0] * p[2] - q[1] * p[1];
12669 if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12670 goto L1;
12671 }
12672 pn[0] = p[2] * cn[2] - p[3] * cn[1];
12673 pn[1] = p[3] * cn[0] - p[1] * cn[2];
12674 pn[2] = p[1] * cn[1] - p[2] * cn[0];
12675 qn[0] = cn[1] * q[2] - cn[2] * q[1];
12676 qn[1] = cn[2] * q[0] - cn[0] * q[2];
12677 qn[2] = cn[0] * q[1] - cn[1] * q[0];
12678
12679
12680
12681 ni = 0;
12682 even = TRUE_;
12683 bp = -2.;
12684 bq = -2.;
12685 pinr = TRUE_;
12686 qinr = TRUE_;
12687 i2 = listv[n];
12688 if (i2 < 1 || i2 > imx) {
12689 goto L12;
12690 }
12691 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12692
12693
12694
12695 i__1 = n;
12696 for (k = 1; k <= i__1; ++k) {
12697 i1 = i2;
12698 lft1 = lft2;
12699 i2 = listv[k];
12700 if (i2 < 1 || i2 > imx) {
12701 goto L12;
12702 }
12703 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12704 if (lft1 == lft2) {
12705 goto L2;
12706 }
12707
12708
12709
12710
12711 ++ni;
12712 v1[0] = xv[i1];
12713 v1[1] = yv[i1];
12714 v1[2] = zv[i1];
12715 v2[0] = xv[i2];
12716 v2[1] = yv[i2];
12717 v2[2] = zv[i2];
12718 intrsc_(v1, v2, cn, b, &ierr);
12719
12720
12721
12722
12723
12724 if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12725 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12726
12727
12728
12729 even = ! even;
12730 d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12731 if (d__ > bq) {
12732 bq = d__;
12733 qinr = lft2;
12734 }
12735 d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12736 if (d__ > bp) {
12737 bp = d__;
12738 pinr = lft1;
12739 }
12740 }
12741 L2:
12742 ;
12743 }
12744
12745
12746
12747
12748 if (ni != ni / 2 << 1 || ! qinr) {
12749 goto L1;
12750 }
12751
12752
12753
12754 if (pinr != even) {
12755 goto L13;
12756 }
12757
12758
12759
12760 *ier = 0;
12761 ret_val = even;
12762 return ret_val;
12763
12764
12765
12766 L11:
12767 *ier = 1;
12768 return ret_val;
12769
12770
12771
12772 L12:
12773 *ier = 2;
12774 return ret_val;
12775
12776
12777
12778 L13:
12779 *ier = 3;
12780 return ret_val;
12781
12782
12783
12784 L14:
12785 *ier = 4;
12786 return ret_val;
12787 }
12788
12789 int intadd_(int *kk, int *i1, int *i2, int *
12790 i3, int *list, int *lptr, int *lend, int *lnew)
12791 {
12792 static int k, n1, n2, n3, lp;
12793 int insert_(int *, int *, int *,
12794 int *, int *);
12795 int lstptr_(int *, int *, int *, int *);
12796
12797
12798
12799
12800
12801
12802
12803
12804
12805
12806
12807
12808
12809
12810
12811
12812
12813
12814
12815
12816
12817
12818
12819
12820
12821
12822
12823
12824
12825
12826
12827
12828
12829
12830
12831
12832
12833
12834
12835
12836
12837
12838
12839
12840
12841
12842
12843
12844
12845
12846
12847
12848
12849
12850
12851
12852
12853 --lend;
12854 --lptr;
12855 --list;
12856
12857
12858 k = *kk;
12859
12860
12861
12862 n1 = *i1;
12863 n2 = *i2;
12864 n3 = *i3;
12865
12866
12867
12868 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12869 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12870 lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12871 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12872 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12873 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12874
12875
12876
12877 list[*lnew] = n1;
12878 list[*lnew + 1] = n2;
12879 list[*lnew + 2] = n3;
12880 lptr[*lnew] = *lnew + 1;
12881 lptr[*lnew + 1] = *lnew + 2;
12882 lptr[*lnew + 2] = *lnew;
12883 lend[k] = *lnew + 2;
12884 *lnew += 3;
12885 return 0;
12886 }
12887
12888 int intrsc_(double *p1, double *p2, double *cn,
12889 double *p, int *ier)
12890 {
12891
12892
12893
12894
12895 static int i__;
12896 static double t, d1, d2, pp[3], ppn;
12897
12898
12899
12900
12901
12902
12903
12904
12905
12906
12907
12908
12909
12910
12911
12912
12913
12914
12915
12916
12917
12918
12919
12920
12921
12922
12923
12924
12925
12926
12927
12928
12929
12930
12931
12932
12933
12934
12935
12936
12937
12938
12939
12940
12941
12942
12943
12944
12945
12946
12947
12948
12949
12950
12951
12952
12953
12954
12955
12956
12957
12958
12959
12960
12961
12962
12963
12964
12965
12966 --p;
12967 --cn;
12968 --p2;
12969 --p1;
12970
12971
12972 d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
12973 d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
12974
12975 if (d1 == d2) {
12976 *ier = 1;
12977 return 0;
12978 }
12979
12980
12981
12982 t = d1 / (d1 - d2);
12983 ppn = 0.;
12984 for (i__ = 1; i__ <= 3; ++i__) {
12985 pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
12986 ppn += pp[i__ - 1] * pp[i__ - 1];
12987
12988 }
12989
12990
12991
12992 if (ppn == 0.) {
12993 *ier = 2;
12994 return 0;
12995 }
12996 ppn = sqrt(ppn);
12997
12998
12999
13000 for (i__ = 1; i__ <= 3; ++i__) {
13001 p[i__] = pp[i__ - 1] / ppn;
13002
13003 }
13004 *ier = 0;
13005 return 0;
13006 }
13007
13008 int jrand_(int *n, int *ix, int *iy, int *iz)
13009 {
13010
13011 int ret_val;
13012
13013
13014 static float u, x;
13015
13016
13017
13018
13019
13020
13021
13022
13023
13024
13025
13026
13027
13028
13029
13030
13031
13032
13033
13034
13035
13036
13037
13038
13039
13040
13041
13042
13043
13044
13045
13046
13047
13048
13049
13050
13051
13052
13053
13054
13055
13056
13057
13058
13059
13060
13061
13062
13063
13064
13065
13066
13067
13068 *ix = *ix * 171 % 30269;
13069 *iy = *iy * 172 % 30307;
13070 *iz = *iz * 170 % 30323;
13071 x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13072 30323.f;
13073 u = x - (int) x;
13074 ret_val = (int) ((float) (*n) * u + 1.f);
13075 return ret_val;
13076 }
13077
13078 long int left_(double *x1, double *y1, double *z1, double *x2,
13079 double *y2, double *z2, double *x0, double *y0,
13080 double *z0)
13081 {
13082
13083 long int ret_val;
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
13117
13118
13119
13120
13121
13122 ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13123 *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13124
13125
13126 return ret_val;
13127 }
13128
13129 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13130 {
13131
13132 int ret_val;
13133
13134
13135 static int nd, lp;
13136
13137
13138
13139
13140
13141
13142
13143
13144
13145
13146
13147
13148
13149
13150
13151
13152
13153
13154
13155
13156
13157
13158
13159
13160
13161
13162
13163
13164
13165
13166
13167
13168
13169
13170
13171
13172
13173
13174
13175
13176
13177
13178
13179
13180
13181
13182
13183 --lptr;
13184 --list;
13185
13186
13187 lp = lptr[*lpl];
13188 L1:
13189 nd = list[lp];
13190 if (nd == *nb) {
13191 goto L2;
13192 }
13193 lp = lptr[lp];
13194 if (lp != *lpl) {
13195 goto L1;
13196 }
13197
13198 L2:
13199 ret_val = lp;
13200 return ret_val;
13201 }
13202
13203 int nbcnt_(int *lpl, int *lptr)
13204 {
13205
13206 int ret_val;
13207
13208
13209 static int k, lp;
13210
13211
13212
13213
13214
13215
13216
13217
13218
13219
13220
13221
13222
13223
13224
13225
13226
13227
13228
13229
13230
13231
13232
13233
13234
13235
13236
13237
13238
13239
13240
13241
13242
13243
13244
13245
13246
13247
13248
13249
13250
13251
13252 --lptr;
13253
13254
13255 lp = *lpl;
13256 k = 1;
13257
13258 L1:
13259 lp = lptr[lp];
13260 if (lp == *lpl) {
13261 goto L2;
13262 }
13263 ++k;
13264 goto L1;
13265
13266 L2:
13267 ret_val = k;
13268 return ret_val;
13269 }
13270
13271 int nearnd_(double *p, int *ist, int *n, double *x,
13272 double *y, double *z__, int *list, int *lptr, int
13273 *lend, double *al)
13274 {
13275
13276 int ret_val, i__1;
13277
13278
13279
13280
13281
13282 static int l;
13283 static double b1, b2, b3;
13284 static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13285 static double ds1;
13286 static int lp1, lp2;
13287 static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13288 static int lpl;
13289 static double dsr;
13290 static int nst, listp[25], lptrp[25];
13291 int trfind_(int *, double *, int *,
13292 double *, double *, double *, int *, int *,
13293 int *, double *, double *, double *, int *,
13294 int *, int *);
13295 int lstptr_(int *, int *, int *, int *);
13296
13297
13298
13299
13300
13301
13302
13303
13304
13305
13306
13307
13308
13309
13310
13311
13312
13313
13314
13315
13316
13317
13318
13319
13320
13321
13322
13323
13324
13325
13326
13327
13328
13329
13330
13331
13332
13333
13334
13335
13336
13337
13338
13339
13340
13341
13342
13343
13344
13345
13346
13347
13348
13349
13350
13351
13352
13353
13354
13355
13356
13357
13358
13359
13360
13361
13362
13363
13364
13365
13366
13367
13368
13369
13370
13371
13372
13373
13374
13375
13376
13377
13378
13379
13380
13381
13382
13383
13384
13385
13386
13387
13388
13389
13390
13391
13392 --p;
13393 --lend;
13394 --z__;
13395 --y;
13396 --x;
13397 --list;
13398 --lptr;
13399
13400
13401 nn = *n;
13402 if (nn < 3) {
13403 goto L6;
13404 }
13405 nst = *ist;
13406 if (nst < 1 || nst > nn) {
13407 nst = 1;
13408 }
13409
13410
13411
13412
13413
13414 trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13415 1], &b1, &b2, &b3, &i1, &i2, &i3);
13416
13417
13418
13419 if (i1 == 0) {
13420 goto L6;
13421 }
13422
13423
13424
13425
13426
13427
13428
13429 if (i3 != 0) {
13430 listp[0] = i1;
13431 lptrp[0] = 2;
13432 listp[1] = i2;
13433 lptrp[1] = 3;
13434 listp[2] = i3;
13435 lptrp[2] = 1;
13436 l = 3;
13437 } else {
13438 n1 = i1;
13439 l = 1;
13440 lp1 = 2;
13441 listp[l - 1] = n1;
13442 lptrp[l - 1] = lp1;
13443
13444
13445
13446
13447 L1:
13448 lpl = lend[n1];
13449 n1 = -list[lpl];
13450 l = lp1;
13451 lp1 = l + 1;
13452 listp[l - 1] = n1;
13453 lptrp[l - 1] = lp1;
13454 if (n1 != i2 && lp1 < 25) {
13455 goto L1;
13456 }
13457 l = lp1;
13458 listp[l - 1] = 0;
13459 lptrp[l - 1] = 1;
13460 }
13461
13462
13463
13464
13465
13466
13467 lp2 = 1;
13468 n2 = i1;
13469 lp1 = lptrp[0];
13470 n1 = listp[lp1 - 1];
13471
13472
13473
13474 L2:
13475 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13476 if (list[lp] < 0) {
13477 goto L3;
13478 }
13479 lp = lptr[lp];
13480 n3 = (i__1 = list[lp], abs(i__1));
13481
13482
13483
13484 if (l == 25) {
13485 goto L4;
13486 }
13487 dx1 = x[n1] - p[1];
13488 dy1 = y[n1] - p[2];
13489 dz1 = z__[n1] - p[3];
13490
13491 dx2 = x[n2] - p[1];
13492 dy2 = y[n2] - p[2];
13493 dz2 = z__[n2] - p[3];
13494
13495 dx3 = x[n3] - p[1];
13496 dy3 = y[n3] - p[2];
13497 dz3 = z__[n3] - p[3];
13498 if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13499 (dx2 * dy1 - dx1 * dy2) <= 0.) {
13500 goto L3;
13501 }
13502
13503
13504
13505
13506 ++l;
13507 lptrp[lp2 - 1] = l;
13508 listp[l - 1] = n3;
13509 lptrp[l - 1] = lp1;
13510 lp1 = l;
13511 n1 = n3;
13512 goto L2;
13513
13514
13515
13516
13517 L3:
13518 if (lp1 == 1) {
13519 goto L4;
13520 }
13521 lp2 = lp1;
13522 n2 = n1;
13523 lp1 = lptrp[lp1 - 1];
13524 n1 = listp[lp1 - 1];
13525 if (n1 == 0) {
13526 goto L4;
13527 }
13528 goto L2;
13529
13530
13531
13532
13533
13534 L4:
13535 nr = i1;
13536 dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13537 i__1 = l;
13538 for (lp = 2; lp <= i__1; ++lp) {
13539 n1 = listp[lp - 1];
13540 if (n1 == 0) {
13541 goto L5;
13542 }
13543 ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13544 if (ds1 < dsr) {
13545 nr = n1;
13546 dsr = ds1;
13547 }
13548 L5:
13549 ;
13550 }
13551 dsr = -dsr;
13552 if (dsr > 1.) {
13553 dsr = 1.;
13554 }
13555 *al = acos(dsr);
13556 ret_val = nr;
13557 return ret_val;
13558
13559
13560
13561 L6:
13562 ret_val = 0;
13563 return ret_val;
13564 }
13565
13566 int optim_(double *x, double *y, double *z__,
13567 int *na, int *list, int *lptr, int *lend, int *
13568 nit, int *iwk, int *ier)
13569 {
13570
13571 int i__1, i__2;
13572
13573
13574 static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13575 static long int swp;
13576 static int iter;
13577 int swap_(int *, int *, int *,
13578 int *, int *, int *, int *, int *);
13579 static int maxit;
13580 long int swptst_(int *, int *, int *, int *,
13581 double *, double *, double *);
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
13612
13613
13614
13615
13616
13617
13618
13619
13620
13621
13622
13623
13624
13625
13626
13627
13628
13629
13630
13631
13632
13633
13634
13635
13636
13637
13638
13639
13640
13641
13642
13643
13644
13645
13646
13647
13648
13649
13650
13651
13652
13653
13654
13655
13656
13657
13658
13659
13660
13661
13662
13663
13664
13665
13666
13667
13668
13669
13670
13671
13672
13673
13674
13675
13676
13677
13678 --x;
13679 --y;
13680 --z__;
13681 iwk -= 3;
13682 --list;
13683 --lptr;
13684 --lend;
13685
13686
13687 nna = *na;
13688 maxit = *nit;
13689 if (nna < 0 || maxit < 1) {
13690 goto L7;
13691 }
13692
13693
13694
13695 iter = 0;
13696 if (nna == 0) {
13697 goto L5;
13698 }
13699
13700
13701
13702
13703 L1:
13704 if (iter == maxit) {
13705 goto L6;
13706 }
13707 ++iter;
13708 swp = FALSE_;
13709
13710
13711
13712 i__1 = nna;
13713 for (i__ = 1; i__ <= i__1; ++i__) {
13714 io1 = iwk[(i__ << 1) + 1];
13715 io2 = iwk[(i__ << 1) + 2];
13716
13717
13718
13719
13720
13721
13722
13723
13724 lpl = lend[io1];
13725 lpp = lpl;
13726 lp = lptr[lpp];
13727 L2:
13728 if (list[lp] == io2) {
13729 goto L3;
13730 }
13731 lpp = lp;
13732 lp = lptr[lpp];
13733 if (lp != lpl) {
13734 goto L2;
13735 }
13736
13737
13738
13739
13740
13741 if ((i__2 = list[lp], abs(i__2)) != io2) {
13742 goto L8;
13743 }
13744 if (list[lp] < 0) {
13745 goto L4;
13746 }
13747
13748
13749
13750
13751 L3:
13752 n2 = list[lpp];
13753 if (n2 < 0) {
13754 goto L4;
13755 }
13756 lp = lptr[lp];
13757 n1 = (i__2 = list[lp], abs(i__2));
13758
13759
13760
13761 if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13762 goto L4;
13763 }
13764 swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13765 if (lp21 == 0) {
13766 goto L9;
13767 }
13768 swp = TRUE_;
13769 iwk[(i__ << 1) + 1] = n1;
13770 iwk[(i__ << 1) + 2] = n2;
13771 L4:
13772 ;
13773 }
13774 if (swp) {
13775 goto L1;
13776 }
13777
13778
13779
13780 L5:
13781 *nit = iter;
13782 *ier = 0;
13783 return 0;
13784
13785
13786
13787 L6:
13788 *nit = maxit;
13789 *ier = 1;
13790 return 0;
13791
13792
13793
13794 L7:
13795 *nit = 0;
13796 *ier = 2;
13797 return 0;
13798
13799
13800
13801 L8:
13802 *nit = iter;
13803 *ier = 3;
13804 return 0;
13805
13806
13807
13808 L9:
13809 *nit = iter;
13810 *ier = 4;
13811 return 0;
13812 }
13813
13814 int projct_(double *px, double *py, double *pz,
13815 double *ox, double *oy, double *oz, double *ex,
13816 double *ey, double *ez, double *vx, double *vy,
13817 double *vz, long int *init, double *x, double *y,
13818 double *z__, int *ier)
13819 {
13820
13821
13822
13823
13824 static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13825 oes, xoe, yoe, zoe, xep, yep, zep;
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
13874
13875
13876
13877
13878
13879
13880
13881
13882
13883
13884
13885
13886
13887
13888
13889
13890
13891
13892
13893
13894
13895
13896
13897
13898
13899
13900
13901
13902
13903
13904
13905
13906
13907
13908
13909
13910
13911
13912
13913
13914
13915
13916
13917
13918
13919
13920
13921
13922
13923
13924
13925
13926
13927
13928
13929
13930
13931
13932
13933
13934
13935
13936
13937
13938
13939
13940 if (*init) {
13941
13942
13943
13944
13945
13946
13947
13948
13949 xe = *ex;
13950 ye = *ey;
13951 ze = *ez;
13952 xoe = xe - *ox;
13953 yoe = ye - *oy;
13954 zoe = ze - *oz;
13955 oes = xoe * xoe + yoe * yoe + zoe * zoe;
13956 if (oes == 0.) {
13957 goto L1;
13958 }
13959
13960
13961
13962 s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
13963 xv = *vx - s * xoe;
13964 yv = *vy - s * yoe;
13965 zv = *vz - s * zoe;
13966
13967
13968
13969 sc = xv * xv + yv * yv + zv * zv;
13970 if (sc == 0.) {
13971 goto L2;
13972 }
13973 sc = 1. / sqrt(sc);
13974 xv = sc * xv;
13975 yv = sc * yv;
13976 zv = sc * zv;
13977
13978
13979
13980 xh = yv * zoe - yoe * zv;
13981 yh = xoe * zv - xv * zoe;
13982 zh = xv * yoe - xoe * yv;
13983 sc = sqrt(xh * xh + yh * yh + zh * zh);
13984 if (sc == 0.) {
13985 goto L2;
13986 }
13987 sc = 1. / sc;
13988 xh = sc * xh;
13989 yh = sc * yh;
13990 zh = sc * zh;
13991 }
13992
13993
13994
13995
13996
13997
13998 xep = *px - xe;
13999 yep = *py - ye;
14000 zep = *pz - ze;
14001 s = xoe * xep + yoe * yep + zoe * zep;
14002 if (s >= 0.) {
14003 goto L1;
14004 }
14005 s = oes / s;
14006 xw = xoe - s * xep;
14007 yw = yoe - s * yep;
14008 zw = zoe - s * zep;
14009
14010
14011
14012
14013 *x = xw * xh + yw * yh + zw * zh;
14014 *y = xw * xv + yw * yv + zw * zv;
14015 *z__ = s + 1.;
14016 *init = FALSE_;
14017 *ier = 0;
14018 return 0;
14019
14020
14021
14022 L1:
14023 *ier = 1;
14024 return 0;
14025
14026
14027
14028 L2:
14029 *ier = 2;
14030 return 0;
14031 }
14032
14033 int scoord_(double *px, double *py, double *pz,
14034 double *plat, double *plon, double *pnrm)
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
14064
14065
14066
14067
14068
14069
14070
14071
14072
14073
14074
14075
14076
14077
14078 *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14079 if (*px != 0. || *py != 0.) {
14080 *plon = atan2(*py, *px);
14081 } else {
14082 *plon = 0.;
14083 }
14084 if (*pnrm != 0.) {
14085 *plat = asin(*pz / *pnrm);
14086 } else {
14087 *plat = 0.;
14088 }
14089 return 0;
14090 }
14091
14092 double store_(double *x)
14093 {
14094
14095 double ret_val;
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 stcom_1.y = *x;
14131 ret_val = stcom_1.y;
14132 return ret_val;
14133 }
14134
14135 int swap_(int *in1, int *in2, int *io1, int *
14136 io2, int *list, int *lptr, int *lend, int *lp21)
14137 {
14138
14139 int i__1;
14140
14141
14142 static int lp, lph, lpsav;
14143 int lstptr_(int *, int *, int *, int *);
14144
14145
14146
14147
14148
14149
14150
14151
14152
14153
14154
14155
14156
14157
14158
14159
14160
14161
14162
14163
14164
14165
14166
14167
14168
14169
14170
14171
14172
14173
14174
14175
14176
14177
14178
14179
14180
14181
14182
14183
14184
14185
14186
14187
14188
14189
14190
14191
14192
14193
14194
14195
14196
14197
14198
14199
14200
14201
14202
14203
14204 --lend;
14205 --lptr;
14206 --list;
14207
14208
14209 lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14210 if ((i__1 = list[lp], abs(i__1)) == *in2) {
14211 *lp21 = 0;
14212 return 0;
14213 }
14214
14215
14216
14217 lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14218 lph = lptr[lp];
14219 lptr[lp] = lptr[lph];
14220
14221
14222
14223
14224 if (lend[*io1] == lph) {
14225 lend[*io1] = lp;
14226 }
14227
14228
14229
14230
14231 lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14232 lpsav = lptr[lp];
14233 lptr[lp] = lph;
14234 list[lph] = *in2;
14235 lptr[lph] = lpsav;
14236
14237
14238
14239 lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14240 lph = lptr[lp];
14241 lptr[lp] = lptr[lph];
14242
14243
14244
14245
14246 if (lend[*io2] == lph) {
14247 lend[*io2] = lp;
14248 }
14249
14250
14251
14252 lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14253 lpsav = lptr[lp];
14254 lptr[lp] = lph;
14255 list[lph] = *in1;
14256 lptr[lph] = lpsav;
14257 *lp21 = lph;
14258 return 0;
14259 }
14260
14261 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14262 double *x, double *y, double *z__)
14263 {
14264
14265 long int ret_val;
14266
14267
14268 static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14269
14270
14271
14272
14273
14274
14275
14276
14277
14278
14279
14280
14281
14282
14283
14284
14285
14286
14287
14288
14289
14290
14291
14292
14293
14294
14295
14296
14297
14298
14299
14300
14301
14302
14303
14304
14305
14306
14307
14308
14309
14310
14311
14312
14313
14314
14315
14316
14317
14318
14319
14320
14321
14322
14323
14324
14325 --z__;
14326 --y;
14327 --x;
14328
14329
14330 x4 = x[*n4];
14331 y4 = y[*n4];
14332 z4 = z__[*n4];
14333 dx1 = x[*n1] - x4;
14334 dx2 = x[*n2] - x4;
14335 dx3 = x[*n3] - x4;
14336 dy1 = y[*n1] - y4;
14337 dy2 = y[*n2] - y4;
14338 dy3 = y[*n3] - y4;
14339 dz1 = z__[*n1] - z4;
14340 dz2 = z__[*n2] - z4;
14341 dz3 = z__[*n3] - z4;
14342
14343
14344
14345
14346
14347 ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14348 dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14349 return ret_val;
14350 }
14351
14352 int trans_(int *n, double *rlat, double *rlon,
14353 double *x, double *y, double *z__)
14354 {
14355
14356 int i__1;
14357
14358
14359
14360
14361
14362 static int i__, nn;
14363 static double phi, theta, cosphi;
14364
14365
14366
14367
14368
14369
14370
14371
14372
14373
14374
14375
14376
14377
14378
14379
14380
14381
14382
14383
14384
14385
14386
14387
14388
14389
14390
14391
14392
14393
14394
14395
14396
14397
14398
14399
14400
14401
14402
14403
14404
14405
14406
14407
14408
14409
14410
14411
14412
14413
14414
14415
14416
14417
14418 --z__;
14419 --y;
14420 --x;
14421 --rlon;
14422 --rlat;
14423
14424
14425 nn = *n;
14426 i__1 = nn;
14427 for (i__ = 1; i__ <= i__1; ++i__) {
14428 phi = rlat[i__];
14429 theta = rlon[i__];
14430 cosphi = cos(phi);
14431 x[i__] = cosphi * cos(theta);
14432 y[i__] = cosphi * sin(theta);
14433 z__[i__] = sin(phi);
14434
14435 }
14436 return 0;
14437 }
14438
14439 int trfind_(int *nst, double *p, int *n,
14440 double *x, double *y, double *z__, int *list, int
14441 *lptr, int *lend, double *b1, double *b2, double *b3,
14442 int *i1, int *i2, int *i3)
14443 {
14444
14445
14446 static int ix = 1;
14447 static int iy = 2;
14448 static int iz = 3;
14449
14450
14451 int i__1;
14452 double d__1, d__2;
14453
14454
14455 static double q[3];
14456 static int n0, n1, n2, n3, n4, nf;
14457 static double s12;
14458 static int nl, lp;
14459 static double xp, yp, zp;
14460 static int n1s, n2s;
14461 static double eps, tol, ptn1, ptn2;
14462 static int next;
14463 int jrand_(int *, int *, int *, int *);
14464 double store_(double *);
14465 int lstptr_(int *, int *, int *, int *);
14466
14467
14468
14469
14470
14471
14472
14473
14474
14475
14476
14477
14478
14479
14480
14481
14482
14483
14484
14485
14486
14487
14488
14489
14490
14491
14492
14493
14494
14495
14496
14497
14498
14499
14500
14501
14502
14503
14504
14505
14506
14507
14508
14509
14510
14511
14512
14513
14514
14515
14516
14517
14518
14519
14520
14521
14522
14523
14524
14525
14526
14527
14528
14529
14530
14531
14532
14533
14534
14535 --p;
14536 --lend;
14537 --z__;
14538 --y;
14539 --x;
14540 --list;
14541 --lptr;
14542
14543
14544
14545
14546
14547
14548
14549
14550
14551
14552
14553
14554
14555
14556
14557
14558
14559
14560
14561
14562
14563
14564
14565
14566
14567
14568
14569
14570
14571
14572
14573
14574
14575
14576
14577
14578
14579
14580
14581
14582
14583
14584
14585
14586
14587
14588
14589
14590 xp = p[1];
14591 yp = p[2];
14592 zp = p[3];
14593 n0 = *nst;
14594 if (n0 < 1 || n0 > *n) {
14595 n0 = jrand_(n, &ix, &iy, &iz);
14596 }
14597
14598
14599
14600 eps = 1.;
14601 L1:
14602 eps /= 2.;
14603 d__1 = eps + 1.;
14604 if (store_(&d__1) > 1.) {
14605 goto L1;
14606 }
14607 eps *= 2.;
14608 tol = eps * 4.;
14609
14610
14611
14612
14613 L2:
14614 lp = lend[n0];
14615 nl = list[lp];
14616 lp = lptr[lp];
14617 nf = list[lp];
14618 n1 = nf;
14619
14620
14621
14622
14623 if (nl > 0) {
14624
14625
14626
14627 L3:
14628 if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14629 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14630 -1e-10) {
14631 lp = lptr[lp];
14632 n1 = list[lp];
14633 if (n1 == nl) {
14634 goto L6;
14635 }
14636 goto L3;
14637 }
14638 } else {
14639
14640
14641
14642 nl = -nl;
14643 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14644 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14645 -1e-10) {
14646
14647
14648
14649 n1 = n0;
14650 n2 = nf;
14651 goto L9;
14652 }
14653 if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14654 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14655 -1e-10) {
14656
14657
14658
14659 n1 = nl;
14660 n2 = n0;
14661 goto L9;
14662 }
14663 }
14664
14665
14666
14667
14668 L4:
14669 lp = lptr[lp];
14670 n2 = (i__1 = list[lp], abs(i__1));
14671 if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14672 n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14673 goto L7;
14674 }
14675 n1 = n2;
14676 if (n1 != nl) {
14677 goto L4;
14678 }
14679 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14680 nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14681 goto L6;
14682 }
14683
14684
14685
14686
14687 d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14688 if (store_(&d__2) < 1. - eps * 4.) {
14689
14690
14691
14692
14693
14694 L5:
14695 if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14696 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14697 -1e-10) {
14698 lp = lptr[lp];
14699 n1 = (i__1 = list[lp], abs(i__1));
14700 if (n1 == nl) {
14701 goto L14;
14702 }
14703 goto L5;
14704 }
14705 }
14706
14707
14708
14709
14710 n0 = n1;
14711 goto L2;
14712
14713
14714
14715 L6:
14716 n2 = nf;
14717
14718
14719
14720
14721
14722 L7:
14723 n3 = n0;
14724 n1s = n1;
14725 n2s = n2;
14726
14727
14728
14729 L8:
14730
14731 *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14732 x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14733 if (*b3 < -1e-10) {
14734
14735
14736
14737
14738 lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14739 if (list[lp] < 0) {
14740 goto L9;
14741 }
14742 lp = lptr[lp];
14743 n4 = (i__1 = list[lp], abs(i__1));
14744
14745
14746
14747 if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14748 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14749 -1e-10) {
14750 n3 = n2;
14751 n2 = n4;
14752 n1s = n1;
14753 if (n2 != n2s && n2 != n0) {
14754 goto L8;
14755 }
14756 } else {
14757 n3 = n1;
14758 n1 = n4;
14759 n2s = n2;
14760 if (n1 != n1s && n1 != n0) {
14761 goto L8;
14762 }
14763 }
14764
14765
14766
14767
14768
14769 n0 = jrand_(n, &ix, &iy, &iz);
14770 goto L2;
14771 }
14772
14773
14774
14775
14776 if (*b3 >= eps) {
14777
14778
14779
14780 *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14781 - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14782 *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14783 - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14784 if (*b1 < -tol || *b2 < -tol) {
14785
14786
14787
14788 n0 = jrand_(n, &ix, &iy, &iz);
14789 goto L2;
14790 }
14791 } else {
14792
14793
14794
14795
14796 *b3 = 0.;
14797 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14798 ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14799 ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14800 *b1 = ptn1 - s12 * ptn2;
14801 *b2 = ptn2 - s12 * ptn1;
14802 if (*b1 < -tol || *b2 < -tol) {
14803
14804
14805
14806 n0 = jrand_(n, &ix, &iy, &iz);
14807 goto L2;
14808 }
14809 }
14810
14811
14812
14813 *i1 = n1;
14814 *i2 = n2;
14815 *i3 = n3;
14816 if (*b1 < 0.f) {
14817 *b1 = 0.f;
14818 }
14819 if (*b2 < 0.f) {
14820 *b2 = 0.f;
14821 }
14822 return 0;
14823
14824
14825
14826
14827
14828 L9:
14829 n1s = n1;
14830 n2s = n2;
14831 nl = 0;
14832
14833
14834
14835 L10:
14836
14837 lp = lend[n2];
14838 lp = lptr[lp];
14839 next = list[lp];
14840 if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14841 - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14842 >= -1e-10) {
14843
14844
14845
14846
14847 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14848 q[0] = x[n1] - s12 * x[n2];
14849 q[1] = y[n1] - s12 * y[n2];
14850 q[2] = z__[n1] - s12 * z__[n2];
14851 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14852 goto L11;
14853 }
14854 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14855 goto L11;
14856 }
14857
14858
14859
14860
14861 nl = n2;
14862 }
14863
14864
14865
14866 n1 = n2;
14867 n2 = next;
14868 if (n2 != n1s) {
14869 goto L10;
14870 }
14871
14872
14873
14874 *i1 = n1s;
14875 *i2 = n1s;
14876 *i3 = 0;
14877 return 0;
14878
14879
14880
14881 L11:
14882 nf = n2;
14883 if (nl == 0) {
14884
14885
14886
14887
14888 n2 = n2s;
14889 n1 = n1s;
14890
14891
14892
14893 L12:
14894 lp = lend[n1];
14895 next = -list[lp];
14896 if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14897 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14898 y[next]) >= -1e-10) {
14899
14900
14901
14902
14903 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14904 q[0] = x[n2] - s12 * x[n1];
14905 q[1] = y[n2] - s12 * y[n1];
14906 q[2] = z__[n2] - s12 * z__[n1];
14907 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14908 goto L13;
14909 }
14910 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14911 goto L13;
14912 }
14913
14914
14915
14916
14917 nf = n1;
14918 }
14919
14920
14921
14922 n2 = n1;
14923 n1 = next;
14924 if (n1 != n1s) {
14925 goto L12;
14926 }
14927
14928
14929
14930 *i1 = n1;
14931 *i2 = n1;
14932 *i3 = 0;
14933 return 0;
14934
14935
14936
14937 L13:
14938 nl = n1;
14939 }
14940
14941
14942
14943 *i1 = nf;
14944 *i2 = nl;
14945 *i3 = 0;
14946 return 0;
14947
14948
14949
14950 L14:
14951 *i1 = 0;
14952 *i2 = 0;
14953 *i3 = 0;
14954 return 0;
14955 }
14956
14957 int trlist_(int *n, int *list, int *lptr,
14958 int *lend, int *nrow, int *nt, int *ltri, int *
14959 ier)
14960 {
14961
14962 int ltri_dim1, ltri_offset, i__1, i__2;
14963
14964
14965 static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
14966 lpl, isv;
14967 static long int arcs;
14968 static int lpln1;
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
15007
15008
15009
15010
15011
15012
15013
15014
15015
15016
15017
15018
15019
15020
15021
15022
15023
15024
15025
15026
15027
15028
15029
15030
15031
15032
15033
15034
15035
15036
15037
15038
15039
15040
15041
15042
15043
15044
15045
15046
15047
15048
15049
15050
15051
15052
15053
15054
15055
15056
15057
15058
15059
15060
15061
15062
15063
15064
15065
15066
15067
15068
15069
15070
15071
15072
15073 --lend;
15074 --list;
15075 --lptr;
15076 ltri_dim1 = *nrow;
15077 ltri_offset = 1 + ltri_dim1;
15078 ltri -= ltri_offset;
15079
15080
15081 if (*n < 3 || (*nrow != 6 && *nrow != 9)) {
15082 goto L11;
15083 }
15084
15085
15086
15087
15088
15089
15090
15091
15092 arcs = *nrow == 9;
15093 ka = 0;
15094 kt = 0;
15095 nm2 = *n - 2;
15096
15097
15098
15099 i__1 = nm2;
15100 for (n1 = 1; n1 <= i__1; ++n1) {
15101
15102
15103
15104
15105 lpln1 = lend[n1];
15106 lp2 = lpln1;
15107 L1:
15108 lp2 = lptr[lp2];
15109 n2 = list[lp2];
15110 lp = lptr[lp2];
15111 n3 = (i__2 = list[lp], abs(i__2));
15112 if (n2 < n1 || n3 < n1) {
15113 goto L8;
15114 }
15115
15116
15117
15118 ++kt;
15119 ltri[kt * ltri_dim1 + 1] = n1;
15120 ltri[kt * ltri_dim1 + 2] = n2;
15121 ltri[kt * ltri_dim1 + 3] = n3;
15122
15123
15124
15125
15126 for (i__ = 1; i__ <= 3; ++i__) {
15127 if (i__ == 1) {
15128 i1 = n3;
15129 i2 = n2;
15130 } else if (i__ == 2) {
15131 i1 = n1;
15132 i2 = n3;
15133 } else {
15134 i1 = n2;
15135 i2 = n1;
15136 }
15137
15138
15139
15140
15141 lpl = lend[i1];
15142 lp = lptr[lpl];
15143 L2:
15144 if (list[lp] == i2) {
15145 goto L3;
15146 }
15147 lp = lptr[lp];
15148 if (lp != lpl) {
15149 goto L2;
15150 }
15151
15152
15153
15154
15155
15156 if ((i__2 = list[lp], abs(i__2)) != i2) {
15157 goto L12;
15158 }
15159 kn = 0;
15160 if (list[lp] < 0) {
15161 goto L6;
15162 }
15163
15164
15165
15166
15167 L3:
15168 lp = lptr[lp];
15169 i3 = (i__2 = list[lp], abs(i__2));
15170
15171
15172
15173
15174
15175 if (i1 < i2 && i1 < i3) {
15176 j = 3;
15177 } else if (i2 < i3) {
15178 j = 2;
15179 isv = i1;
15180 i1 = i2;
15181 i2 = i3;
15182 i3 = isv;
15183 } else {
15184 j = 1;
15185 isv = i1;
15186 i1 = i3;
15187 i3 = i2;
15188 i2 = isv;
15189 }
15190
15191
15192
15193 if (i1 > n1) {
15194 goto L7;
15195 }
15196
15197
15198
15199
15200 for (kn = kt - 1; kn >= 1; --kn) {
15201 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15202 == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15203 goto L5;
15204 }
15205
15206 }
15207 goto L7;
15208
15209
15210
15211 L5:
15212 ltri[j + 3 + kn * ltri_dim1] = kt;
15213
15214
15215
15216 L6:
15217 ltri[i__ + 3 + kt * ltri_dim1] = kn;
15218 if (arcs) {
15219 ++ka;
15220 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15221 if (kn != 0) {
15222 ltri[j + 6 + kn * ltri_dim1] = ka;
15223 }
15224 }
15225 L7:
15226 ;
15227 }
15228
15229
15230
15231 L8:
15232 if (lp2 != lpln1) {
15233 goto L1;
15234 }
15235
15236 }
15237
15238
15239
15240 *nt = kt;
15241 *ier = 0;
15242 return 0;
15243
15244
15245
15246 L11:
15247 *nt = 0;
15248 *ier = 1;
15249 return 0;
15250
15251
15252
15253
15254 L12:
15255 *nt = 0;
15256 *ier = 2;
15257 return 0;
15258 }
15259
15260 int trlprt_(int *n, double *x, double *y,
15261 double *z__, int *iflag, int *nrow, int *nt, int *
15262 ltri, int *lout)
15263 {
15264
15265
15266 static int nmax = 9999;
15267 static int nlmax = 58;
15268
15269
15270 int ltri_dim1, ltri_offset, i__1;
15271
15272
15273 static int i__, k, na, nb, nl, lun;
15274
15275
15276
15277
15278
15279
15280
15281
15282
15283
15284
15285
15286
15287
15288
15289
15290
15291
15292
15293
15294
15295
15296
15297
15298
15299
15300
15301
15302
15303
15304
15305
15306
15307
15308
15309
15310
15311
15312
15313
15314
15315
15316
15317
15318
15319
15320
15321
15322
15323
15324
15325
15326
15327
15328
15329
15330
15331
15332
15333
15334
15335
15336
15337
15338
15339
15340
15341
15342
15343
15344
15345
15346
15347 --z__;
15348 --y;
15349 --x;
15350 ltri_dim1 = *nrow;
15351 ltri_offset = 1 + ltri_dim1;
15352 ltri -= ltri_offset;
15353
15354
15355
15356
15357
15358
15359
15360
15361
15362
15363
15364
15365
15366
15367
15368
15369 lun = *lout;
15370 if (lun < 0 || lun > 99) {
15371 lun = 6;
15372 }
15373
15374
15375
15376
15377 nl = 3;
15378 if (*n < 3 || *n > nmax || (*nrow != 6 && *nrow != 9) || *nt < 1 || *nt >
15379 nmax) {
15380
15381
15382
15383
15384 return 0;
15385 }
15386 if (*iflag == 0) {
15387
15388
15389
15390
15391 nl = 6;
15392 i__1 = *n;
15393 for (i__ = 1; i__ <= i__1; ++i__) {
15394 if (nl >= nlmax) {
15395
15396 nl = 0;
15397 }
15398
15399 ++nl;
15400
15401 }
15402 } else if (*iflag > 0) {
15403
15404
15405
15406
15407 nl = 6;
15408 i__1 = *n;
15409 for (i__ = 1; i__ <= i__1; ++i__) {
15410 if (nl >= nlmax) {
15411
15412 nl = 0;
15413 }
15414
15415 ++nl;
15416
15417 }
15418 }
15419
15420
15421
15422 if (nl > nlmax / 2) {
15423
15424 nl = 0;
15425 }
15426 if (*nrow == 6) {
15427
15428 } else {
15429
15430 }
15431 nl += 5;
15432 i__1 = *nt;
15433 for (k = 1; k <= i__1; ++k) {
15434 if (nl >= nlmax) {
15435
15436 nl = 0;
15437 }
15438
15439 ++nl;
15440
15441 }
15442
15443
15444
15445
15446 nb = (*n << 1) - *nt - 2;
15447 if (nb < 3) {
15448 nb = 0;
15449 na = *n * 3 - 6;
15450 } else {
15451 na = *nt + *n - 1;
15452 }
15453
15454 return 0;
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 int trmesh_(int *n, double *x, double *y,
15481 double *z__, int *list, int *lptr, int *lend, int
15482 *lnew, int *near__, int *next, double *dist, int *ier)
15483 {
15484
15485 int i__1, i__2;
15486
15487
15488 static double d__;
15489 static int i__, j, k;
15490 static double d1, d2, d3;
15491 static int i0, lp, nn, lpl;
15492 long int left_(double *, double *, double *, double
15493 *, double *, double *, double *, double *,
15494 double *);
15495 static int nexti;
15496
15497
15498
15499
15500
15501
15502
15503
15504
15505
15506
15507
15508
15509
15510
15511
15512
15513
15514
15515
15516
15517
15518
15519
15520
15521
15522
15523
15524
15525
15526
15527
15528
15529
15530
15531
15532
15533
15534
15535
15536
15537
15538
15539
15540
15541
15542
15543
15544
15545
15546
15547
15548
15549
15550
15551
15552
15553
15554
15555
15556
15557
15558
15559
15560
15561
15562
15563
15564
15565
15566
15567
15568
15569
15570
15571
15572
15573
15574
15575
15576
15577
15578
15579
15580
15581
15582
15583
15584
15585
15586
15587
15588
15589
15590
15591
15592
15593
15594
15595
15596
15597
15598
15599
15600
15601
15602
15603
15604
15605
15606
15607
15608
15609
15610
15611
15612
15613
15614
15615
15616
15617
15618
15619
15620
15621
15622
15623
15624
15625
15626
15627
15628
15629
15630
15631
15632
15633
15634
15635
15636
15637
15638
15639
15640
15641
15642
15643
15644
15645
15646
15647
15648
15649
15650
15651
15652
15653
15654
15655
15656
15657
15658
15659
15660
15661
15662
15663
15664
15665
15666
15667
15668
15669
15670
15671
15672
15673
15674
15675
15676
15677
15678
15679
15680
15681
15682
15683
15684
15685
15686
15687
15688
15689
15690
15691
15692
15693
15694
15695
15696
15697
15698
15699
15700
15701
15702
15703
15704
15705
15706
15707
15708
15709
15710
15711
15712
15713
15714
15715
15716
15717
15718
15719
15720
15721
15722 --dist;
15723 --next;
15724 --near__;
15725 --lend;
15726 --z__;
15727 --y;
15728 --x;
15729 --list;
15730 --lptr;
15731
15732
15733 nn = *n;
15734 if (nn < 3) {
15735 *ier = -1;
15736 return 0;
15737 }
15738
15739
15740
15741 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15742 z__[3])) {
15743
15744
15745
15746 list[1] = 3;
15747 lptr[1] = 2;
15748 list[2] = -2;
15749 lptr[2] = 1;
15750 lend[1] = 2;
15751
15752 list[3] = 1;
15753 lptr[3] = 4;
15754 list[4] = -3;
15755 lptr[4] = 3;
15756 lend[2] = 4;
15757
15758 list[5] = 2;
15759 lptr[5] = 6;
15760 list[6] = -1;
15761 lptr[6] = 5;
15762 lend[3] = 6;
15763
15764 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15765 y[3], &z__[3])) {
15766
15767
15768
15769
15770
15771 list[1] = 2;
15772 lptr[1] = 2;
15773 list[2] = -3;
15774 lptr[2] = 1;
15775 lend[1] = 2;
15776
15777 list[3] = 3;
15778 lptr[3] = 4;
15779 list[4] = -1;
15780 lptr[4] = 3;
15781 lend[2] = 4;
15782
15783 list[5] = 1;
15784 lptr[5] = 6;
15785 list[6] = -2;
15786 lptr[6] = 5;
15787 lend[3] = 6;
15788
15789 } else {
15790
15791
15792
15793 *ier = -2;
15794 return 0;
15795 }
15796
15797
15798
15799 *lnew = 7;
15800 if (nn == 3) {
15801 *ier = 0;
15802 return 0;
15803 }
15804
15805
15806
15807
15808
15809
15810
15811
15812
15813
15814
15815
15816
15817
15818
15819
15820
15821
15822
15823
15824
15825
15826
15827
15828
15829
15830
15831 near__[1] = 0;
15832 near__[2] = 0;
15833 near__[3] = 0;
15834 for (k = nn; k >= 4; --k) {
15835 d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15836 d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15837 d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15838 if (d1 <= d2 && d1 <= d3) {
15839 near__[k] = 1;
15840 dist[k] = d1;
15841 next[k] = near__[1];
15842 near__[1] = k;
15843 } else if (d2 <= d1 && d2 <= d3) {
15844 near__[k] = 2;
15845 dist[k] = d2;
15846 next[k] = near__[2];
15847 near__[2] = k;
15848 } else {
15849 near__[k] = 3;
15850 dist[k] = d3;
15851 next[k] = near__[3];
15852 near__[3] = k;
15853 }
15854
15855 }
15856
15857
15858
15859 i__1 = nn;
15860 for (k = 4; k <= i__1; ++k) {
15861 addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15862 lend[1], lnew, ier);
15863 if (*ier != 0) {
15864 return 0;
15865 }
15866
15867
15868
15869
15870 i__ = near__[k];
15871 if (near__[i__] == k) {
15872 near__[i__] = next[k];
15873 } else {
15874 i__ = near__[i__];
15875 L2:
15876 i0 = i__;
15877 i__ = next[i0];
15878 if (i__ != k) {
15879 goto L2;
15880 }
15881 next[i0] = next[k];
15882 }
15883 near__[k] = 0;
15884
15885
15886
15887 lpl = lend[k];
15888 lp = lpl;
15889 L3:
15890 lp = lptr[lp];
15891 j = (i__2 = list[lp], abs(i__2));
15892
15893
15894
15895
15896
15897
15898
15899 i__ = near__[j];
15900 L4:
15901 if (i__ == 0) {
15902 goto L5;
15903 }
15904 nexti = next[i__];
15905
15906
15907
15908
15909 d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15910 if (d__ < dist[i__]) {
15911
15912
15913
15914
15915
15916 near__[i__] = k;
15917 dist[i__] = d__;
15918 if (i__ == near__[j]) {
15919 near__[j] = nexti;
15920 } else {
15921 next[i0] = nexti;
15922 }
15923 next[i__] = near__[k];
15924 near__[k] = i__;
15925 } else {
15926 i0 = i__;
15927 }
15928
15929
15930
15931 i__ = nexti;
15932 goto L4;
15933
15934
15935
15936 L5:
15937 if (lp != lpl) {
15938 goto L3;
15939 }
15940
15941 }
15942 return 0;
15943 }
15944
15945 int trplot_(int *lun, double *pltsiz, double *
15946 elat, double *elon, double *a, int *n, double *x,
15947 double *y, double *z__, int *list, int *lptr, int
15948 *lend, char *, long int *numbr, int *ier, short )
15949 {
15950
15951
15952 static long int annot = TRUE_;
15953 static double fsizn = 10.;
15954 static double fsizt = 16.;
15955 static double tol = .5;
15956
15957
15958 int i__1, i__2;
15959 double d__1;
15960
15961
15962
15963
15964
15965
15966
15967 static double t;
15968 static int n0, n1;
15969 static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
15970 static int ir, lp;
15971 static double ex, ey, ez, wr, tx, ty;
15972 static int lpl;
15973 static double wrs;
15974 static int ipx1, ipx2, ipy1, ipy2, nseg;
15975 int drwarc_(int *, double *, double *,
15976 double *, int *);
15977
15978
15979
15980
15981
15982
15983
15984
15985
15986
15987
15988
15989
15990
15991
15992
15993
15994
15995
15996
15997
15998
15999
16000
16001
16002
16003
16004
16005
16006
16007
16008
16009
16010
16011
16012
16013
16014
16015
16016
16017
16018
16019
16020
16021
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
16078
16079
16080
16081
16082
16083 --lend;
16084 --z__;
16085 --y;
16086 --x;
16087 --list;
16088 --lptr;
16089
16090
16091
16092
16093
16094
16095
16096
16097
16098
16099
16100
16101
16102
16103
16104
16105
16106
16107
16108
16109
16110
16111
16112
16113
16114
16115
16116
16117
16118
16119
16120
16121
16122
16123
16124
16125
16126
16127
16128
16129
16130
16131
16132
16133
16134
16135
16136
16137
16138
16139
16140
16141
16142
16143
16144 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16145 goto L11;
16146 }
16147 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16148 goto L12;
16149 }
16150
16151
16152
16153
16154 cf = atan(1.) / 45.;
16155 wr = sin(cf * *a);
16156 wrs = wr * wr;
16157
16158
16159
16160
16161
16162
16163
16164
16165
16166
16167 d__1 = *pltsiz * 36.;
16168 ir = i_dnnt(&d__1);
16169 ipx1 = 306 - ir;
16170 ipx2 = ir + 306;
16171 ipy1 = 396 - ir;
16172 ipy2 = ir + 396;
16173
16174
16175
16176
16177
16178
16179
16180
16181
16182
16183
16184
16185
16186
16187 d__1 = (double) ir * .88;
16188 ir = i_dnnt(&d__1);
16189 ipx1 = 306 - ir;
16190 ipx2 = ir + 306;
16191 ipy1 = 396 - ir;
16192 ipy2 = ir + 396;
16193
16194
16195
16196
16197 t = 2.;
16198
16199
16200
16201
16202
16203
16204
16205
16206
16207
16208 sf = (double) ir / wr;
16209 tx = ipx1 + sf * wr;
16210 ty = ipy1 + sf * wr;
16211
16212
16213
16214
16215
16216
16217
16218
16219 t = 1. / sf;
16220
16221
16222
16223
16224
16225
16226
16227
16228
16229
16230
16231
16232
16233
16234
16235
16236
16237
16238
16239
16240
16241
16242
16243 t = cf * *elon;
16244 ct = cos(cf * *elat);
16245 ex = ct * cos(t);
16246 ey = ct * sin(t);
16247 ez = sin(cf * *elat);
16248 if (ct != 0.) {
16249 r11 = -ey / ct;
16250 r12 = ex / ct;
16251 } else {
16252 r11 = 0.;
16253 r12 = 1.;
16254 }
16255 r21 = -ez * r12;
16256 r22 = ez * r11;
16257 r23 = ct;
16258
16259
16260
16261
16262 i__1 = *n;
16263 for (n0 = 1; n0 <= i__1; ++n0) {
16264 p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16265 if (p0[2] < 0.) {
16266 goto L3;
16267 }
16268 p0[0] = r11 * x[n0] + r12 * y[n0];
16269 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16270 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16271 goto L3;
16272 }
16273 lpl = lend[n0];
16274 lp = lpl;
16275
16276
16277
16278
16279 L1:
16280 lp = lptr[lp];
16281 n1 = (i__2 = list[lp], abs(i__2));
16282 p1[0] = r11 * x[n1] + r12 * y[n1];
16283 p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16284 p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16285 if (p1[2] < 0.) {
16286
16287
16288
16289
16290
16291 p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16292 p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16293 t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16294 p1[0] /= t;
16295 p1[1] /= t;
16296 }
16297
16298
16299
16300
16301 if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16302 goto L2;
16303 }
16304
16305
16306
16307
16308 if (p1[2] < 0.) {
16309 p1[2] = 0.;
16310 }
16311 d__1 = tol / sf;
16312 drwarc_(lun, p0, p1, &d__1, &nseg);
16313
16314
16315
16316 L2:
16317 if (lp != lpl) {
16318 goto L1;
16319 }
16320 L3:
16321 ;
16322 }
16323
16324
16325
16326
16327
16328
16329
16330 if (*numbr) {
16331
16332
16333
16334
16335
16336 t = fsizn / sf;
16337
16338
16339
16340
16341
16342
16343
16344 i__1 = *n;
16345 for (n0 = 1; n0 <= i__1; ++n0) {
16346 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16347 goto L4;
16348 }
16349 p0[0] = r11 * x[n0] + r12 * y[n0];
16350 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16351 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16352 goto L4;
16353 }
16354
16355
16356
16357
16358
16359
16360
16361
16362
16363 L4:
16364 ;
16365 }
16366 }
16367
16368
16369
16370
16371 t = fsizt / sf;
16372
16373
16374
16375
16376 p0[1] = wr + t * 3.;
16377
16378
16379
16380
16381
16382 if (annot) {
16383
16384
16385
16386 p0[0] = -wr;
16387 p0[1] = -wr - 50. / sf;
16388
16389
16390 p0[1] -= t * 2.;
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 *ier = 0;
16416 return 0;
16417
16418
16419
16420 L11:
16421 *ier = 1;
16422 return 0;
16423
16424
16425
16426 L12:
16427 *ier = 2;
16428 return 0;
16429
16430
16431
16432
16433 *ier = 3;
16434 return 0;
16435 }
16436
16437 int trprnt_(int *n, double *x, double *y,
16438 double *z__, int *iflag, int *list, int *lptr,
16439 int *lend, int *lout)
16440 {
16441
16442
16443 static int nmax = 9999;
16444 static int nlmax = 58;
16445
16446
16447 int i__1;
16448
16449
16450 static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16451 400];
16452
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
16481
16482
16483
16484
16485
16486
16487
16488
16489
16490
16491
16492
16493
16494
16495
16496
16497
16498
16499
16500
16501
16502
16503
16504
16505
16506
16507
16508
16509
16510
16511
16512
16513
16514 --lend;
16515 --z__;
16516 --y;
16517 --x;
16518 --list;
16519 --lptr;
16520
16521
16522
16523
16524
16525
16526
16527
16528
16529
16530
16531
16532
16533
16534
16535
16536
16537
16538
16539
16540
16541
16542
16543
16544
16545
16546
16547 nn = *n;
16548 lun = *lout;
16549 if (lun < 0 || lun > 99) {
16550 lun = 6;
16551 }
16552
16553
16554
16555
16556 if (nn < 3 || nn > nmax) {
16557
16558
16559
16560
16561 return 0;
16562 }
16563
16564
16565
16566
16567 nl = 6;
16568 nb = 0;
16569 if (*iflag < 0) {
16570
16571
16572
16573
16574
16575 i__1 = nn;
16576 for (node = 1; node <= i__1; ++node) {
16577 lpl = lend[node];
16578 lp = lpl;
16579 k = 0;
16580
16581 L1:
16582 ++k;
16583 lp = lptr[lp];
16584 nd = list[lp];
16585 nabor[k - 1] = nd;
16586 if (lp != lpl) {
16587 goto L1;
16588 }
16589 if (nd <= 0) {
16590
16591
16592
16593
16594
16595 nabor[k - 1] = -nd;
16596 ++k;
16597 nabor[k - 1] = 0;
16598 ++nb;
16599 }
16600
16601
16602
16603 inc = (k - 1) / 14 + 2;
16604 nl += inc;
16605 if (nl > nlmax) {
16606
16607 nl = inc;
16608 }
16609
16610
16611
16612
16613 }
16614 } else if (*iflag > 0) {
16615
16616
16617
16618
16619 i__1 = nn;
16620 for (node = 1; node <= i__1; ++node) {
16621 lpl = lend[node];
16622 lp = lpl;
16623 k = 0;
16624
16625 L3:
16626 ++k;
16627 lp = lptr[lp];
16628 nd = list[lp];
16629 nabor[k - 1] = nd;
16630 if (lp != lpl) {
16631 goto L3;
16632 }
16633 if (nd <= 0) {
16634
16635
16636
16637 nabor[k - 1] = -nd;
16638 ++k;
16639 nabor[k - 1] = 0;
16640 ++nb;
16641 }
16642
16643
16644
16645 inc = (k - 1) / 8 + 2;
16646 nl += inc;
16647 if (nl > nlmax) {
16648
16649 nl = inc;
16650 }
16651
16652
16653
16654
16655
16656 }
16657 } else {
16658
16659
16660
16661
16662 i__1 = nn;
16663 for (node = 1; node <= i__1; ++node) {
16664 lpl = lend[node];
16665 lp = lpl;
16666 k = 0;
16667
16668 L5:
16669 ++k;
16670 lp = lptr[lp];
16671 nd = list[lp];
16672 nabor[k - 1] = nd;
16673 if (lp != lpl) {
16674 goto L5;
16675 }
16676 if (nd <= 0) {
16677
16678
16679
16680 nabor[k - 1] = -nd;
16681 ++k;
16682 nabor[k - 1] = 0;
16683 ++nb;
16684 }
16685
16686
16687
16688 inc = (k - 1) / 5 + 2;
16689 nl += inc;
16690 if (nl > nlmax) {
16691
16692 nl = inc;
16693 }
16694
16695
16696
16697
16698
16699 }
16700 }
16701
16702
16703
16704
16705 if (nb != 0) {
16706 na = nn * 3 - nb - 3;
16707 nt = (nn << 1) - nb - 2;
16708 } else {
16709 na = nn * 3 - 6;
16710 nt = (nn << 1) - 4;
16711 }
16712
16713 return 0;
16714
16715
16716
16717
16718
16719
16720
16721
16722
16723
16724
16725
16726
16727
16728
16729
16730
16731
16732
16733
16734 }
16735
16736 int vrplot_(int *lun, double *pltsiz, double *
16737 elat, double *elon, double *a, int *n, double *x,
16738 double *y, double *z__, int *nt, int *listc, int *
16739 lptr, int *lend, double *xc, double *yc, double *zc,
16740 char *, long int *numbr, int *ier, short)
16741 {
16742
16743
16744 static long int annot = TRUE_;
16745 static double fsizn = 10.;
16746 static double fsizt = 16.;
16747 static double tol = .5;
16748
16749
16750 int i__1;
16751 double d__1;
16752
16753
16754
16755
16756
16757
16758
16759 static double t;
16760 static int n0;
16761 static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16762 sf;
16763 static int ir, lp;
16764 static double ex, ey, ez, wr, tx, ty;
16765 static long int in1, in2;
16766 static int kv1, kv2, lpl;
16767 static double wrs;
16768 static int ipx1, ipx2, ipy1, ipy2, nseg;
16769 int drwarc_(int *, double *, double *,
16770 double *, int *);
16771
16772
16773
16774
16775
16776
16777
16778
16779
16780
16781
16782
16783
16784
16785
16786
16787
16788
16789
16790
16791
16792
16793
16794
16795
16796
16797
16798
16799
16800
16801
16802
16803
16804
16805
16806
16807
16808
16809
16810
16811
16812
16813
16814
16815
16816
16817
16818
16819
16820
16821
16822
16823
16824
16825
16826
16827
16828
16829
16830
16831
16832
16833
16834
16835
16836
16837
16838
16839
16840
16841
16842
16843
16844
16845
16846
16847
16848
16849
16850
16851
16852
16853
16854
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 --lend;
16914 --z__;
16915 --y;
16916 --x;
16917 --zc;
16918 --yc;
16919 --xc;
16920 --listc;
16921 --lptr;
16922
16923
16924
16925
16926
16927
16928
16929
16930
16931
16932
16933
16934
16935
16936
16937
16938
16939
16940
16941
16942
16943
16944
16945
16946
16947
16948
16949
16950
16951
16952
16953
16954
16955
16956
16957
16958
16959
16960
16961
16962
16963
16964
16965
16966
16967
16968
16969
16970
16971
16972
16973
16974
16975
16976
16977
16978
16979
16980 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
16981 nt != 2 * *n - 4) {
16982 goto L11;
16983 }
16984 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16985 goto L12;
16986 }
16987
16988
16989
16990
16991 cf = atan(1.) / 45.;
16992 wr = sin(cf * *a);
16993 wrs = wr * wr;
16994
16995
16996
16997
16998
16999
17000
17001
17002
17003
17004 d__1 = *pltsiz * 36.;
17005 ir = i_dnnt(&d__1);
17006 ipx1 = 306 - ir;
17007 ipx2 = ir + 306;
17008 ipy1 = 396 - ir;
17009 ipy2 = ir + 396;
17010
17011
17012
17013
17014
17015
17016
17017
17018
17019
17020
17021
17022
17023 d__1 = (double) ir * .88;
17024 ir = i_dnnt(&d__1);
17025 ipx1 = 306 - ir;
17026 ipx2 = ir + 306;
17027 ipy1 = 396 - ir;
17028 ipy2 = ir + 396;
17029
17030
17031
17032
17033 t = 2.;
17034
17035
17036
17037
17038
17039
17040
17041
17042
17043
17044 sf = (double) ir / wr;
17045 tx = ipx1 + sf * wr;
17046 ty = ipy1 + sf * wr;
17047
17048
17049
17050
17051
17052
17053
17054
17055 t = 1. / sf;
17056
17057
17058
17059
17060
17061
17062
17063
17064
17065
17066
17067
17068
17069
17070
17071
17072
17073
17074
17075
17076
17077
17078
17079 t = cf * *elon;
17080 ct = cos(cf * *elat);
17081 ex = ct * cos(t);
17082 ey = ct * sin(t);
17083 ez = sin(cf * *elat);
17084 if (ct != 0.) {
17085 r11 = -ey / ct;
17086 r12 = ex / ct;
17087 } else {
17088 r11 = 0.;
17089 r12 = 1.;
17090 }
17091 r21 = -ez * r12;
17092 r22 = ez * r11;
17093 r23 = ct;
17094
17095
17096
17097
17098 i__1 = *n;
17099 for (n0 = 1; n0 <= i__1; ++n0) {
17100 lpl = lend[n0];
17101
17102
17103
17104
17105 kv2 = listc[lpl];
17106 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17107 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17108 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17109
17110
17111
17112 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17113
17114
17115
17116
17117 lp = lpl;
17118 L1:
17119 lp = lptr[lp];
17120 kv1 = kv2;
17121 p1[0] = p2[0];
17122 p1[1] = p2[1];
17123 p1[2] = p2[2];
17124 in1 = in2;
17125 kv2 = listc[lp];
17126
17127
17128
17129 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17130 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17131 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17132 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17133
17134
17135
17136
17137
17138 if (! in1 || (in2 && kv2 <= kv1)) {
17139 goto L2;
17140 }
17141 if (p2[2] < 0.) {
17142
17143
17144
17145
17146
17147 p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17148 p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17149 t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17150 p2[0] /= t;
17151 p2[1] /= t;
17152 }
17153
17154
17155
17156
17157 if (p2[2] < 0.) {
17158 p2[2] = 0.f;
17159 }
17160 d__1 = tol / sf;
17161 drwarc_(lun, p1, p2, &d__1, &nseg);
17162
17163
17164
17165 L2:
17166 if (lp != lpl) {
17167 goto L1;
17168 }
17169
17170 }
17171
17172
17173
17174
17175
17176
17177
17178 if (*numbr) {
17179
17180
17181
17182
17183
17184 t = fsizn / sf;
17185
17186
17187
17188
17189
17190
17191
17192 i__1 = *n;
17193 for (n0 = 1; n0 <= i__1; ++n0) {
17194 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17195 goto L4;
17196 }
17197 x0 = r11 * x[n0] + r12 * y[n0];
17198 y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17199 if (x0 * x0 + y0 * y0 > wrs) {
17200 goto L4;
17201 }
17202
17203
17204
17205
17206
17207
17208
17209
17210 L4:
17211 ;
17212 }
17213 }
17214
17215
17216
17217
17218 t = fsizt / sf;
17219
17220
17221
17222
17223 y0 = wr + t * 3.;
17224
17225
17226
17227
17228
17229 if (annot) {
17230
17231
17232
17233 x0 = -wr;
17234 y0 = -wr - 50. / sf;
17235
17236
17237 y0 -= t * 2.;
17238
17239
17240
17241
17242
17243 }
17244
17245
17246
17247
17248
17249
17250
17251
17252
17253
17254
17255
17256
17257
17258
17259
17260
17261
17262 *ier = 0;
17263 return 0;
17264
17265
17266
17267 L11:
17268 *ier = 1;
17269 return 0;
17270
17271
17272
17273 L12:
17274 *ier = 2;
17275 return 0;
17276
17277
17278
17279
17280 *ier = 3;
17281 return 0;
17282 }
17283
17284 int random_(int *ix, int *iy, int *iz,
17285 double *rannum)
17286 {
17287 static double x;
17288
17289
17290
17291
17292
17293
17294
17295
17296
17297
17298
17299
17300
17301
17302
17303 *ix = *ix * 171 % 30269;
17304 *iy = *iy * 172 % 30307;
17305 *iz = *iz * 170 % 30323;
17306 x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17307 double) (*iz) / 30323.;
17308 *rannum = x - (int) x;
17309 return 0;
17310 }
17311
17312 #undef TRUE_
17313 #undef FALSE_
17314 #undef abs
17315
17316
17317
17318
17319
17320
17321
17322
17323
17324 EMData* Util::mult_scalar(EMData* img, float scalar)
17325 {
17326 ENTERFUNC;
17327
17328 if (!img) {
17329 throw NullPointerException("NULL input image");
17330 }
17331
17332
17333 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17334 size_t size = (size_t)nx*ny*nz;
17335 EMData * img2 = img->copy_head();
17336 float *img_ptr =img->get_data();
17337 float *img2_ptr = img2->get_data();
17338 for (size_t i=0;i<size;++i)img2_ptr[i] = img_ptr[i]*scalar;
17339 img2->update();
17340
17341 if(img->is_complex()) {
17342 img2->set_complex(true);
17343 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17344 }
17345 EXITFUNC;
17346 return img2;
17347 }
17348
17349 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17350 {
17351 ENTERFUNC;
17352
17353 if (!img) {
17354 throw NullPointerException("NULL input image");
17355 }
17356
17357
17358 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17359 size_t size = (size_t)nx*ny*nz;
17360 EMData * img2 = img->copy_head();
17361 float *img_ptr =img->get_data();
17362 float *img2_ptr = img2->get_data();
17363 float *img1_ptr = img1->get_data();
17364 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17365 img2->update();
17366 if(img->is_complex()) {
17367 img2->set_complex(true);
17368 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17369 }
17370
17371 EXITFUNC;
17372 return img2;
17373 }
17374
17375 EMData* Util::addn_img(EMData* img, EMData* img1)
17376 {
17377 ENTERFUNC;
17378
17379 if (!img) {
17380 throw NullPointerException("NULL input image");
17381 }
17382
17383
17384 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17385 size_t size = (size_t)nx*ny*nz;
17386 EMData * img2 = img->copy_head();
17387 float *img_ptr =img->get_data();
17388 float *img2_ptr = img2->get_data();
17389 float *img1_ptr = img1->get_data();
17390 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17391 img2->update();
17392 if(img->is_complex()) {
17393 img2->set_complex(true);
17394 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17395 }
17396
17397 EXITFUNC;
17398 return img2;
17399 }
17400
17401 EMData* Util::subn_img(EMData* img, EMData* img1)
17402 {
17403 ENTERFUNC;
17404
17405 if (!img) {
17406 throw NullPointerException("NULL input image");
17407 }
17408
17409
17410 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17411 size_t size = (size_t)nx*ny*nz;
17412 EMData * img2 = img->copy_head();
17413 float *img_ptr =img->get_data();
17414 float *img2_ptr = img2->get_data();
17415 float *img1_ptr = img1->get_data();
17416 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17417 img2->update();
17418 if(img->is_complex()) {
17419 img2->set_complex(true);
17420 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17421 }
17422
17423 EXITFUNC;
17424 return img2;
17425 }
17426
17427 EMData* Util::muln_img(EMData* img, EMData* img1)
17428 {
17429 ENTERFUNC;
17430
17431 if (!img) {
17432 throw NullPointerException("NULL input image");
17433 }
17434
17435
17436 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17437 size_t size = (size_t)nx*ny*nz;
17438 EMData * img2 = img->copy_head();
17439 float *img_ptr =img->get_data();
17440 float *img2_ptr = img2->get_data();
17441 float *img1_ptr = img1->get_data();
17442 if(img->is_complex()) {
17443 for (size_t i=0; i<size; i+=2) {
17444 img2_ptr[i] = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17445 img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17446 }
17447 img2->set_complex(true);
17448 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17449 } else {
17450 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17451 img2->update();
17452 }
17453
17454 EXITFUNC;
17455 return img2;
17456 }
17457
17458 EMData* Util::divn_img(EMData* img, EMData* img1)
17459 {
17460 ENTERFUNC;
17461
17462 if (!img) {
17463 throw NullPointerException("NULL input image");
17464 }
17465
17466
17467 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17468 size_t size = (size_t)nx*ny*nz;
17469 EMData * img2 = img->copy_head();
17470 float *img_ptr =img->get_data();
17471 float *img2_ptr = img2->get_data();
17472 float *img1_ptr = img1->get_data();
17473 if(img->is_complex()) {
17474 float sq2;
17475 for (size_t i=0; i<size; i+=2) {
17476 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17477 img2_ptr[i] = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17478 img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17479 }
17480 img2->set_complex(true);
17481 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17482 } else {
17483 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17484 img2->update();
17485 }
17486
17487 EXITFUNC;
17488 return img2;
17489 }
17490
17491 EMData* Util::divn_filter(EMData* img, EMData* img1)
17492 {
17493 ENTERFUNC;
17494
17495 if (!img) {
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 EMData * img2 = img->copy_head();
17503 float *img_ptr =img->get_data();
17504 float *img1_ptr = img1->get_data();
17505 float *img2_ptr = img2->get_data();
17506 if(img->is_complex()) {
17507 for (size_t i=0; i<size; i+=2) {
17508 if(img1_ptr[i] > 1.e-10f) {
17509 img2_ptr[i] = img_ptr[i] /img1_ptr[i];
17510 img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17511 } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17512 }
17513 } else throw ImageFormatException("Only Fourier image allowed");
17514
17515 img->update();
17516
17517 EXITFUNC;
17518 return img2;
17519 }
17520
17521 void Util::mul_scalar(EMData* img, float scalar)
17522 {
17523 ENTERFUNC;
17524
17525 if (!img) {
17526 throw NullPointerException("NULL input image");
17527 }
17528
17529
17530 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17531 size_t size = (size_t)nx*ny*nz;
17532 float *img_ptr =img->get_data();
17533 for (size_t i=0;i<size;++i) img_ptr[i] *= scalar;
17534 img->update();
17535
17536 EXITFUNC;
17537 }
17538
17539 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17540 {
17541 ENTERFUNC;
17542
17543 if (!img) {
17544 throw NullPointerException("NULL input image");
17545 }
17546
17547
17548 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17549 size_t size = (size_t)nx*ny*nz;
17550 float *img_ptr =img->get_data();
17551 float *img1_ptr = img1->get_data();
17552 for (size_t i=0;i<size;++i)img_ptr[i] += img1_ptr[i]*scalar;
17553 img1->update();
17554
17555 EXITFUNC;
17556 }
17557
17558 void Util::add_img(EMData* img, EMData* img1)
17559 {
17560 ENTERFUNC;
17561
17562 if (!img || !img1) {
17563 throw NullPointerException("NULL input image");
17564 }
17565
17566
17567 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17568 size_t size = (size_t)nx*ny*nz;
17569 float *img_ptr = img->get_data();
17570 float *img1_ptr = img1->get_data();
17571 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i];
17572 img->update();
17573
17574 EXITFUNC;
17575 }
17576
17577 void Util::add_img_abs(EMData* img, EMData* img1)
17578 {
17579 ENTERFUNC;
17580
17581 if (!img) {
17582 throw NullPointerException("NULL input image");
17583 }
17584
17585
17586 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17587 size_t size = (size_t)nx*ny*nz;
17588 float *img_ptr = img->get_data();
17589 float *img1_ptr = img1->get_data();
17590 for (size_t i=0;i<size;++i) img_ptr[i] += abs(img1_ptr[i]);
17591 img->update();
17592
17593 EXITFUNC;
17594 }
17595
17596 void Util::add_img2(EMData* img, EMData* img1)
17597 {
17598 ENTERFUNC;
17599
17600 if (!img) {
17601 throw NullPointerException("NULL input image");
17602 }
17603
17604
17605 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17606 size_t size = (size_t)nx*ny*nz;
17607 float *img_ptr = img->get_data();
17608 float *img1_ptr = img1->get_data();
17609 if(img->is_complex()) {
17610 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] ;
17611 } else {
17612 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17613 }
17614 img->update();
17615
17616 EXITFUNC;
17617 }
17618
17619 void Util::sub_img(EMData* img, EMData* img1)
17620 {
17621 ENTERFUNC;
17622
17623 if (!img) {
17624 throw NullPointerException("NULL input image");
17625 }
17626
17627
17628 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17629 size_t size = (size_t)nx*ny*nz;
17630 float *img_ptr = img->get_data();
17631 float *img1_ptr = img1->get_data();
17632 for (size_t i=0;i<size;++i) img_ptr[i] -= img1_ptr[i];
17633 img->update();
17634
17635 EXITFUNC;
17636 }
17637
17638 void Util::mul_img(EMData* img, EMData* img1)
17639 {
17640 ENTERFUNC;
17641
17642 if (!img) {
17643 throw NullPointerException("NULL input image");
17644 }
17645
17646
17647 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17648 size_t size = (size_t)nx*ny*nz;
17649 float *img_ptr = img->get_data();
17650 float *img1_ptr = img1->get_data();
17651 if(img->is_complex()) {
17652 for (size_t i=0; i<size; i+=2) {
17653 float tmp = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17654 img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17655 img_ptr[i] = tmp;
17656
17657 }
17658 } else {
17659 for (size_t i=0;i<size;++i) img_ptr[i] *= img1_ptr[i];
17660 }
17661 img->update();
17662
17663 EXITFUNC;
17664 }
17665
17666 void Util::div_img(EMData* img, EMData* img1)
17667 {
17668 ENTERFUNC;
17669
17670 if (!img) {
17671 throw NullPointerException("NULL input image");
17672 }
17673
17674
17675 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17676 size_t size = (size_t)nx*ny*nz;
17677 float *img_ptr = img->get_data();
17678 float *img1_ptr = img1->get_data();
17679 if(img->is_complex()) {
17680 float sq2;
17681 for (size_t i=0; i<size; i+=2) {
17682 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17683 float tmp = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17684 img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17685 img_ptr[i] = tmp;
17686 }
17687 } else {
17688 for (size_t i=0; i<size; ++i) img_ptr[i] /= img1_ptr[i];
17689 }
17690 img->update();
17691
17692 EXITFUNC;
17693 }
17694
17695 void Util::div_filter(EMData* img, EMData* img1)
17696 {
17697 ENTERFUNC;
17698
17699 if (!img) {
17700 throw NullPointerException("NULL input image");
17701 }
17702
17703
17704 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17705 size_t size = (size_t)nx*ny*nz;
17706 float *img_ptr = img->get_data();
17707 float *img1_ptr = img1->get_data();
17708 if(img->is_complex()) {
17709 for (size_t i=0; i<size; i+=2) {
17710 if(img1_ptr[i] > 1.e-10f) {
17711 img_ptr[i] /= img1_ptr[i];
17712 img_ptr[i+1] /= img1_ptr[i];
17713 } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17714 }
17715 } else throw ImageFormatException("Only Fourier image allowed");
17716
17717 img->update();
17718
17719 EXITFUNC;
17720 }
17721
17722 #define img_ptr(i,j,k) img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*(size_t)nxo]
17723
17724 EMData* Util::pack_complex_to_real(EMData* img)
17725 {
17726 ENTERFUNC;
17727
17728 if (!img) {
17729 throw NullPointerException("NULL input image");
17730 }
17731
17732
17733
17734 int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17735 int nx = nxo - 2 + img->is_fftodd();
17736 int lsd2 = (nx + 2 - nx%2) / 2;
17737 int nyt, nzt;
17738 int nx2 = nx/2;
17739 int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17740 int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17741 int nx2p = nx2+nx%2;
17742 int ny2p = ny2+ny%2;
17743 int nz2p = nz2+nz%2;
17744 EMData& power = *(new EMData());
17745 power.set_size(nx, ny, nz);
17746 power.set_array_offsets(-nx2,-ny2,-nz2);
17747
17748 float *img_ptr = img->get_data();
17749 for (int iz = 1; iz <= nz; iz++) {
17750 int jz=iz-1;
17751 if(jz>=nz2p) jz=jz-nzt;
17752 for (int iy = 1; iy <= ny; iy++) {
17753 int jy=iy-1;
17754 if(jy>=ny2p) jy=jy-nyt;
17755 for (int ix = 1; ix <= lsd2; ix++) {
17756 int jx=ix-1;
17757 if(jx>=nx2p) jx=jx-nx;
17758 power(jx,jy,jz) = img_ptr(ix,iy,iz);
17759 }
17760 }
17761 }
17762
17763 int nzb, nze, nyb, nye, nxb, nxe;
17764 nxb =-nx2+(nx+1)%2;
17765 nxe = nx2-(nx+1)%2;
17766 if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17767 if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17768 for (int iz = nzb; iz <= nze; iz++) {
17769 for (int iy = nyb; iy <= nye; iy++) {
17770 for (int ix = 1; ix <= nxe; ix++) {
17771 power(-ix,-iy,-iz) = power(ix,iy,iz);
17772 }
17773 }
17774 }
17775 if(ny2 != 0) {
17776 if(nz2 != 0) {
17777 if(nz%2 == 0) {
17778 for (int iy = nyb; iy <= nye; iy++) {
17779 for (int ix = nxb; ix <= -1; ix++) {
17780 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17781 }
17782 }
17783 if(ny%2 == 0) {
17784 for (int ix = nxb; ix <= -1; ix++) {
17785 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17786 }
17787 }
17788 }
17789 }
17790 if(ny%2 == 0) {
17791 for (int iz = nzb; iz <= nze; iz++) {
17792 for (int ix = nxb; ix <= -1; ix++) {
17793 power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17794 }
17795 }
17796 }
17797
17798 }
17799 power.update();
17800 power.set_array_offsets(0,0,0);
17801 return &power;
17802 }
17803 #undef img_ptr
17804
17805 float Util::ang_n(float peakp, string mode, int maxrin)
17806 {
17807 if (mode == "f" || mode == "F")
17808 return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17809 else
17810 return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17811 }
17812
17813
17814 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17815 {
17816 float* data = ring->get_data();
17817 float av=0.0;
17818 float sq=0.0;
17819 float nn=0.0;
17820 int nring = numr.size()/3;
17821 for( int i=0; i < nring; ++i )
17822 {
17823 int numr3i = numr[3*i+2];
17824 int numr2i = numr[3*i+1]-1;
17825 float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17826 for( int j=0; j < numr3i; ++j )
17827 {
17828 int jc = numr2i+j;
17829 av += data[jc] * w;
17830 sq += data[jc] * data[jc] * w;
17831 nn += w;
17832 }
17833 }
17834
17835 float avg = av/nn;
17836 float sgm = sqrt( (sq-av*av/nn)/nn );
17837 size_t n = (size_t)ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17838 for( size_t i=0; i < n; ++i )
17839 {
17840 data[i] -= avg;
17841 data[i] /= sgm;
17842 }
17843
17844 ring->update();
17845 }
17846
17847 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17848 float xrng, float yrng, float step, string mode,
17849 vector<int>numr, float cnx, float cny) {
17850
17851
17852
17853
17854
17855
17856
17857
17858
17859
17860
17861
17862 size_t crefim_len = crefim.size();
17863
17864 int ky = int(2*yrng/step+0.5)/2;
17865 int kx = int(2*xrng/step+0.5)/2;
17866 int iref, nref=0, mirror=0;
17867 float iy, ix, sx=0, sy=0;
17868 float peak = -1.0E23f;
17869 float ang=0.0f;
17870 for (int i = -ky; i <= ky; i++) {
17871 iy = i * step ;
17872 for (int j = -kx; j <= kx; j++) {
17873 ix = j*step ;
17874 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17875
17876 Normalize_ring( cimage, numr );
17877
17878 Frngs(cimage, numr);
17879
17880
17881 for ( iref = 0; iref < (int)crefim_len; iref++) {
17882 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17883 double qn = retvals["qn"];
17884 double qm = retvals["qm"];
17885 if(qn >= peak || qm >= peak) {
17886 sx = -ix;
17887 sy = -iy;
17888 nref = iref;
17889 if (qn >= qm) {
17890 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17891 peak = static_cast<float>(qn);
17892 mirror = 0;
17893 } else {
17894 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17895 peak = static_cast<float>(qm);
17896 mirror = 1;
17897 }
17898 }
17899 } delete cimage; cimage = 0;
17900 }
17901 }
17902 float co, so, sxs, sys;
17903 co = static_cast<float>( cos(ang*pi/180.0) );
17904 so = static_cast<float>( -sin(ang*pi/180.0) );
17905 sxs = sx*co - sy*so;
17906 sys = sx*so + sy*co;
17907 vector<float> res;
17908 res.push_back(ang);
17909 res.push_back(sxs);
17910 res.push_back(sys);
17911 res.push_back(static_cast<float>(mirror));
17912 res.push_back(static_cast<float>(nref));
17913 res.push_back(peak);
17914 return res;
17915 }
17916
17917 vector<float> Util::multiref_polar_ali_2d_peaklist(EMData* image, const vector< EMData* >& crefim,
17918 float xrng, float yrng, float step, string mode,
17919 vector<int>numr, float cnx, float cny) {
17920
17921 size_t crefim_len = crefim.size();
17922
17923 int ky = int(2*yrng/step+0.5)/2;
17924 int kx = int(2*xrng/step+0.5)/2;
17925 float iy, ix;
17926 vector<float> peak(crefim_len*5, -1.0e23f);
17927 for (int i = -ky; i <= ky; i++) {
17928 iy = i * step ;
17929 for (int j = -kx; j <= kx; j++) {
17930 ix = j*step ;
17931 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17932 Normalize_ring( cimage, numr );
17933 Frngs(cimage, numr);
17934 for (int iref = 0; iref < (int)crefim_len; iref++) {
17935 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17936 double qn = retvals["qn"];
17937 double qm = retvals["qm"];
17938 if(qn >= peak[iref*5] || qm >= peak[iref*5]) {
17939 if (qn >= qm) {
17940 peak[iref*5] = static_cast<float>(qn);
17941 peak[iref*5+1] = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17942 peak[iref*5+2] = -ix;
17943 peak[iref*5+3] = -iy;
17944 peak[iref*5+4] = 0;
17945 } else {
17946 peak[iref*5] = static_cast<float>(qm);
17947 peak[iref*5+1] = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17948 peak[iref*5+2] = -ix;
17949 peak[iref*5+3] = -iy;
17950 peak[iref*5+4] = 1;
17951 }
17952 }
17953 } delete cimage; cimage = 0;
17954 }
17955 }
17956 for (int iref = 0; iref < (int)crefim_len; iref++) {
17957 float ang = peak[iref*5+1];
17958 float sx = peak[iref*5+2];
17959 float sy = peak[iref*5+3];
17960 float co = cos(ang*pi/180.0);
17961 float so = -sin(ang*pi/180.0);
17962 float sxs = sx*co - sy*so;
17963 float sys = sx*so + sy*co;
17964 peak[iref*5+2] = sxs;
17965 peak[iref*5+3] = sys;
17966 }
17967 return peak;
17968 }
17969
17970 struct peak_table {
17971 float value;
17972 int index;
17973 bool operator<(const peak_table& b) const { return value > b.value; }
17974 };
17975
17976 vector<int> Util::assign_groups(const vector<float>& d, int nref, int nima) {
17977
17978 int kt = nref;
17979 unsigned int maxasi = nima/nref;
17980 vector< vector<int> > id_list;
17981 id_list.resize(nref);
17982 int group, ima;
17983
17984 peak_table* dd = new peak_table[nref*nima];
17985 for (int i=0; i<nref*nima; i++) {
17986 dd[i].value = d[i];
17987 dd[i].index = i;
17988 }
17989 sort(dd, dd+nref*nima);
17990 int begin = 0;
17991
17992 bool* del_row = new bool[nref];
17993 for (int i=0; i<nref; i++) del_row[i] = false;
17994 bool* del_column = new bool[nima];
17995 for (int i=0; i<nima; i++) del_column[i] = false;
17996 while (kt > 0) {
17997 bool flag = true;
17998 while (flag) {
17999 int l = dd[begin].index;
18000 group = l/nima;
18001 ima = l%nima;
18002 if (del_column[ima] || del_row[group]) begin++;
18003 else flag = false;
18004 }
18005
18006 id_list[group].push_back(ima);
18007 if (kt > 1) {
18008 if (id_list[group].size() < maxasi) group = -1;
18009 else kt -= 1;
18010 } else {
18011 if (id_list[group].size() < maxasi+nima%nref) group = -1;
18012 else kt -= 1;
18013 }
18014 del_column[ima] = true;
18015 if (group != -1) {
18016 del_row[group] = true;
18017 }
18018 }
18019
18020 vector<int> id_list_1;
18021 for (int iref=0; iref<nref; iref++)
18022 for (unsigned int im=0; im<maxasi; im++)
18023 id_list_1.push_back(id_list[iref][im]);
18024 for (unsigned int im=maxasi; im<maxasi+nima%nref; im++)
18025 id_list_1.push_back(id_list[group][im]);
18026 id_list_1.push_back(group);
18027
18028 delete[] del_row;
18029 delete[] del_column;
18030 delete[] dd;
18031 return id_list_1;
18032 }
18033
18034 int Util::nearest_ang(const vector<float>& vecref, float x, float y, float z) {
18035 float best_v = -1.0f;
18036 int best_i = -1.0;
18037
18038 for (int i=0; i<vecref.size()/3; i++) {
18039 float v = abs(vecref[i*3]*x+vecref[i*3+1]*y+vecref[i*3+2]*z);
18040 if (v > best_v) {
18041 best_v = v;
18042 best_i = i;
18043 }
18044 }
18045 return best_i;
18046 }
18047
18048 vector<int> Util::assign_projangles(const vector<float>& projangles, const vector<float>& refangles) {
18049 int nref = refangles.size()/2;
18050 int nproj = projangles.size()/2;
18051
18052 vector<float> vecref(nref*3, 0.0f);
18053 vector<int> asg(nproj, 0);
18054 for (int i=0; i<nref; i++)
18055 getvec(refangles[i*2], refangles[i*2+1], vecref[i*3], vecref[i*3+1], vecref[i*3+2]);
18056 for (int i=0; i<nproj; i++) {
18057 float x, y, z;
18058 getvec(projangles[i*2], projangles[i*2+1], x, y, z);
18059 asg[i] = nearest_ang(vecref, x, y, z);
18060 }
18061
18062 return asg;
18063 }
18064
18065 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
18066 float xrng, float yrng, float step, string mode,
18067 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
18068
18069
18070
18071
18072
18073
18074
18075
18076
18077
18078
18079
18080 size_t crefim_len = crefim.size();
18081
18082 int ky = int(2*yrng/step+0.5)/2;
18083 int kx = int(2*xrng/step+0.5)/2;
18084 int iref, nref=0, mirror=0;
18085 float iy, ix, sx=0, sy=0;
18086 float peak = -1.0E23f;
18087 float ang=0.0f;
18088 for (int i = -ky; i <= ky; i++) {
18089 iy = i * step ;
18090 for (int j = -kx; j <= kx; j++) {
18091 ix = j*step ;
18092 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18093
18094 Normalize_ring( cimage, numr );
18095
18096 Frngs(cimage, numr);
18097
18098
18099 for ( iref = 0; iref < (int)crefim_len; iref++) {
18100 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
18101 double qn = retvals["qn"];
18102 double qm = retvals["qm"];
18103 if(qn >= peak || qm >= peak) {
18104 sx = -ix;
18105 sy = -iy;
18106 nref = iref;
18107 if (qn >= qm) {
18108 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18109 peak = static_cast<float>(qn);
18110 mirror = 0;
18111 } else {
18112 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18113 peak = static_cast<float>(qm);
18114 mirror = 1;
18115 }
18116 }
18117 } delete cimage; cimage = 0;
18118 }
18119 }
18120 float co, so, sxs, sys;
18121 co = static_cast<float>( cos(ang*pi/180.0) );
18122 so = static_cast<float>( -sin(ang*pi/180.0) );
18123 sxs = sx*co - sy*so;
18124 sys = sx*so + sy*co;
18125 vector<float> res;
18126 res.push_back(ang);
18127 res.push_back(sxs);
18128 res.push_back(sys);
18129 res.push_back(static_cast<float>(mirror));
18130 res.push_back(static_cast<float>(nref));
18131 res.push_back(peak);
18132 return res;
18133 }
18134
18135 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
18136 float xrng, float yrng, float step, string mode,
18137 vector< int >numr, float cnx, float cny) {
18138
18139
18140
18141
18142
18143
18144
18145
18146
18147
18148
18149 size_t crefim_len = crefim.size();
18150
18151 int ky = int(2*yrng/step+0.5)/2;
18152 int kx = int(2*xrng/step+0.5)/2;
18153 int iref, nref=0;
18154 float iy, ix, sx=0, sy=0;
18155 float peak = -1.0E23f;
18156 float ang=0.0f;
18157 for (int i = -ky; i <= ky; i++) {
18158 iy = i * step ;
18159 for (int j = -kx; j <= kx; j++) {
18160 ix = j*step ;
18161 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18162 Frngs(cimage, numr);
18163
18164
18165 for ( iref = 0; iref < (int)crefim_len; iref++) {
18166 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
18167 double qn = retvals["qn"];
18168 if(qn >= peak) {
18169 sx = -ix;
18170 sy = -iy;
18171 nref = iref;
18172 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18173 peak = static_cast<float>(qn);
18174 }
18175 } delete cimage; cimage = 0;
18176 }
18177 }
18178 float co, so, sxs, sys;
18179 co = static_cast<float>( cos(ang*pi/180.0) );
18180 so = static_cast<float>( -sin(ang*pi/180.0) );
18181 sxs = sx*co - sy*so;
18182 sys = sx*so + sy*co;
18183 vector<float> res;
18184 res.push_back(ang);
18185 res.push_back(sxs);
18186 res.push_back(sys);
18187 res.push_back(static_cast<float>(nref));
18188 res.push_back(peak);
18189 return res;
18190 }
18191
18192 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
18193 float xrng, float yrng, float step, float ant, string mode,
18194 vector<int>numr, float cnx, float cny) {
18195
18196
18197
18198
18199
18200
18201
18202
18203
18204
18205
18206 size_t crefim_len = crefim.size();
18207 const float qv = static_cast<float>( pi/180.0 );
18208
18209 Transform * t = image->get_attr("xform.projection");
18210 Dict d = t->get_params("spider");
18211 if(t) {delete t; t=0;}
18212 float phi = d["phi"];
18213 float theta = d["theta"];
18214 int ky = int(2*yrng/step+0.5)/2;
18215 int kx = int(2*xrng/step+0.5)/2;
18216 int iref, nref=0, mirror=0;
18217 float iy, ix, sx=0, sy=0;
18218 float peak = -1.0E23f;
18219 float ang=0.0f;
18220 float imn1 = sin(theta*qv)*cos(phi*qv);
18221 float imn2 = sin(theta*qv)*sin(phi*qv);
18222 float imn3 = cos(theta*qv);
18223 vector<float> n1(crefim_len);
18224 vector<float> n2(crefim_len);
18225 vector<float> n3(crefim_len);
18226 for ( iref = 0; iref < (int)crefim_len; iref++) {
18227 n1[iref] = crefim[iref]->get_attr("n1");
18228 n2[iref] = crefim[iref]->get_attr("n2");
18229 n3[iref] = crefim[iref]->get_attr("n3");
18230 }
18231 for (int i = -ky; i <= ky; i++) {
18232 iy = i * step ;
18233 for (int j = -kx; j <= kx; j++) {
18234 ix = j*step;
18235 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18236
18237 Normalize_ring( cimage, numr );
18238
18239 Frngs(cimage, numr);
18240
18241
18242 for ( iref = 0; iref < (int)crefim_len; iref++) {
18243 if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18244 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18245 double qn = retvals["qn"];
18246 double qm = retvals["qm"];
18247 if(qn >= peak || qm >= peak) {
18248 sx = -ix;
18249 sy = -iy;
18250 nref = iref;
18251 if (qn >= qm) {
18252 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18253 peak = static_cast<float>( qn );
18254 mirror = 0;
18255 } else {
18256 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18257 peak = static_cast<float>( qm );
18258 mirror = 1;
18259 }
18260 }
18261 }
18262 } delete cimage; cimage = 0;
18263 }
18264 }
18265 float co, so, sxs, sys;
18266 if(peak == -1.0E23) {
18267 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18268 nref = -1;
18269 } else {
18270 co = cos(ang*qv);
18271 so = -sin(ang*qv);
18272 sxs = sx*co - sy*so;
18273 sys = sx*so + sy*co;
18274 }
18275 vector<float> res;
18276 res.push_back(ang);
18277 res.push_back(sxs);
18278 res.push_back(sys);
18279 res.push_back(static_cast<float>(mirror));
18280 res.push_back(static_cast<float>(nref));
18281 res.push_back(peak);
18282 return res;
18283 }
18284
18285 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18286 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18287 vector<int>numr, float cnx, float cny) {
18288
18289
18290
18291
18292
18293
18294
18295
18296
18297
18298
18299 size_t crefim_len = crefim.size();
18300 const float qv = static_cast<float>(pi/180.0);
18301
18302 Transform* t = image->get_attr("xform.projection");
18303 Dict d = t->get_params("spider");
18304 if(t) {delete t; t=0;}
18305 float phi = d["phi"];
18306 float theta = d["theta"];
18307 float psi = d["psi"];
18308 int ky = int(2*yrng/step+0.5)/2;
18309 int kx = int(2*xrng/step+0.5)/2;
18310 int iref, nref = 0, mirror = 0;
18311 float iy, ix, sx = 0, sy = 0;
18312 float peak = -1.0E23f;
18313 float ang = 0.0f;
18314 float imn1 = sin(theta*qv)*cos(phi*qv);
18315 float imn2 = sin(theta*qv)*sin(phi*qv);
18316 float imn3 = cos(theta*qv);
18317 vector<float> n1(crefim_len);
18318 vector<float> n2(crefim_len);
18319 vector<float> n3(crefim_len);
18320 for (iref = 0; iref < (int)crefim_len; iref++) {
18321 n1[iref] = crefim[iref]->get_attr("n1");
18322 n2[iref] = crefim[iref]->get_attr("n2");
18323 n3[iref] = crefim[iref]->get_attr("n3");
18324 }
18325 bool nomirror = (theta<90.0) || ((theta==90.0) && (psi<psi_max));
18326 if (!nomirror) {
18327 phi = fmod(phi+540.0f, 360.0f);
18328 theta = 180-theta;
18329 psi = fmod(540.0f-psi, 360.0f);
18330 }
18331 for (int i = -ky; i <= ky; i++) {
18332 iy = i * step ;
18333 for (int j = -kx; j <= kx; j++) {
18334 ix = j*step;
18335 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18336
18337 Normalize_ring(cimage, numr);
18338
18339 Frngs(cimage, numr);
18340
18341
18342 for (iref = 0; iref < (int)crefim_len; iref++) {
18343 if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18344 if (nomirror) {
18345 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 0, psi_max);
18346 double qn = retvals["qn"];
18347 if (qn >= peak) {
18348 sx = -ix;
18349 sy = -iy;
18350 nref = iref;
18351 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18352 peak = static_cast<float>(qn);
18353 mirror = 0;
18354 }
18355 } else {
18356 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 1, psi_max);
18357 double qn = retvals["qn"];
18358 if (qn >= peak) {
18359 sx = -ix;
18360 sy = -iy;
18361 nref = iref;
18362 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18363 peak = static_cast<float>(qn);
18364 mirror = 1;
18365 }
18366 }
18367 }
18368 } delete cimage; cimage = 0;
18369 }
18370 }
18371 float co, so, sxs, sys;
18372 if(peak == -1.0E23) {
18373 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18374 nref = -1;
18375 } else {
18376 co = cos(ang*qv);
18377 so = -sin(ang*qv);
18378 sxs = sx*co - sy*so;
18379 sys = sx*so + sy*co;
18380 }
18381 vector<float> res;
18382 res.push_back(ang);
18383 res.push_back(sxs);
18384 res.push_back(sys);
18385 res.push_back(static_cast<float>(mirror));
18386 res.push_back(static_cast<float>(nref));
18387 res.push_back(peak);
18388 return res;
18389 }
18390
18391
18392 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18393 float xrng, float yrng, float step, float psi_max, string mode,
18394 vector<int>numr, float cnx, float cny, int ynumber) {
18395
18396 size_t crefim_len = crefim.size();
18397
18398 int iref, nref=0, mirror=0;
18399 float iy, ix, sx=0, sy=0;
18400 float peak = -1.0E23f;
18401 float ang=0.0f;
18402 int kx = int(2*xrng/step+0.5)/2;
18403
18404
18405
18406 int ky;
18407 float stepy;
18408 int kystart;
18409
18410 if (ynumber == -1){
18411 ky = int(2*yrng/step+0.5)/2;
18412 stepy = step;
18413 kystart = -ky;
18414 }
18415 else if(ynumber == 0){
18416 ky = 0;
18417 stepy = 0.0f;
18418 kystart = ky;
18419 }
18420 else {
18421 ky = int(ynumber/2);
18422 stepy=2*yrng/ynumber;
18423 kystart = -ky + 1;
18424 }
18425
18426 for (int i = kystart; i <= ky; i++) {
18427 iy = i * stepy ;
18428 for (int j = -kx; j <= kx; j++) {
18429 ix = j*step ;
18430 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18431
18432 Normalize_ring( cimage, numr );
18433
18434 Frngs(cimage, numr);
18435
18436
18437 for ( iref = 0; iref < (int)crefim_len; iref++) {
18438 Dict retvals_0 = Crosrng_psi(crefim[iref], cimage, numr, 0, psi_max);
18439 Dict retvals_180 = Crosrng_psi(crefim[iref], cimage, numr, 180, psi_max);
18440 double qn_0 = retvals_0["qn"];
18441 double qn_180 = retvals_180["qn"];
18442 double qm_0 = retvals_0["qm"];
18443 double qm_180 = retvals_180["qm"];
18444 double qn;
18445 double qm;
18446 bool qn_is_zero = false;
18447 bool qm_is_zero = false;
18448
18449 if (qn_0 >= qn_180){
18450 qn = qn_0;
18451 qn_is_zero = true;
18452 }
18453 else{
18454 qn = qn_180;
18455 qn_is_zero = false;
18456 }
18457
18458 if (qm_0 >= qm_180){
18459 qm = qm_0;
18460 qm_is_zero = true;
18461 }
18462 else{
18463 qm = qm_180;
18464 qm_is_zero = false;
18465 }
18466
18467 if(qn >= peak || qm >= peak) {
18468 sx = -ix;
18469 sy = -iy;
18470 nref = iref;
18471 if (qn >= qm) {
18472 if (qn_is_zero){
18473 ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
18474 }
18475 else{
18476 ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
18477 }
18478 peak = static_cast<float>(qn);
18479 mirror = 0;
18480 } else {
18481 if (qm_is_zero){
18482 ang = ang_n(retvals_0["tmt"], mode, numr[numr.size()-1]);
18483 }
18484 else{
18485 ang = ang_n(retvals_180["tmt"], mode, numr[numr.size()-1]);
18486 }
18487 peak = static_cast<float>(qm);
18488 mirror = 1;
18489 }
18490 }
18491 }
18492 delete cimage; cimage = 0;
18493 }
18494 }
18495 float co, so, sxs, sys;
18496 co = static_cast<float>( cos(ang*pi/180.0) );
18497 so = static_cast<float>( -sin(ang*pi/180.0) );
18498 sxs = sx*co - sy*so;
18499 sys = sx*so + sy*co;
18500 vector<float> res;
18501 res.push_back(ang);
18502 res.push_back(sxs);
18503 res.push_back(sys);
18504 res.push_back(static_cast<float>(mirror));
18505 res.push_back(static_cast<float>(nref));
18506 res.push_back(peak);
18507 return res;
18508 }
18509
18510 vector<float> Util::multiref_polar_ali_helical_local(EMData* image, const vector< EMData* >& crefim,
18511 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18512 vector<int>numr, float cnx, float cny, int ynumber, bool mirror_only, float yrnglocal) {
18513
18514 size_t crefim_len = crefim.size();
18515
18516 int iref, nref=-1, mirror=0;
18517 float iy, ix, sx=0, sy=0;
18518 float peak = -1.0E23f;
18519 float ang=0.0f;
18520 const float qv = static_cast<float>( pi/180.0 );
18521 Transform * t = image->get_attr("xform.projection");
18522 Dict d = t->get_params("spider");
18523 if(t) {delete t; t=0;}
18524 float phi = d["phi"];
18525 float theta = d["theta"];
18526 float psi = d["psi"];
18527 float imn1 = sin(theta*qv)*cos(phi*qv);
18528 float imn2 = sin(theta*qv)*sin(phi*qv);
18529 float imn3 = cos(theta*qv);
18530 vector<float> n1(crefim_len);
18531 vector<float> n2(crefim_len);
18532 vector<float> n3(crefim_len);
18533 for ( iref = 0; iref < (int)crefim_len; iref++) {
18534 n1[iref] = crefim[iref]->get_attr("n1");
18535 n2[iref] = crefim[iref]->get_attr("n2");
18536 n3[iref] = crefim[iref]->get_attr("n3");
18537 }
18538 float nbrinp;
18539 bool use_ref;
18540 int kx = int(2*xrng/step+0.5)/2;
18541
18542
18543 if(ynumber==-1) {
18544 int ky = int(2*yrng/step+0.5)/2;
18545 for (int i = -ky; i <= ky; i++) {
18546 iy = i * step ;
18547 for (int j = -kx; j <= kx; j++) {
18548 ix = j*step ;
18549 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18550
18551 Normalize_ring( cimage, numr );
18552
18553 Frngs(cimage, numr);
18554
18555
18556 for ( iref = 0; iref < (int)crefim_len; iref++) {
18557
18558 use_ref = false;
18559 if (!mirror_only){
18560
18561 nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18562 if (nbrinp >= ant){
18563 use_ref = true;
18564 }
18565 }
18566 else if (mirror_only) {
18567
18568 nbrinp = (-1.0*n1[iref]*imn1) + (-1.0*n2[iref]*imn2) + n3[iref]*imn3;
18569 if (nbrinp >= ant){
18570 use_ref = true;
18571 }
18572 }
18573
18574
18575 if(use_ref) {
18576 Dict retvals;
18577 if (mirror_only == true){
18578 if ((psi-90) < 90)
18579 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
18580 else
18581 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max);
18582 }
18583 else{
18584 if ((psi-90) < 90)
18585 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18586 else
18587 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18588 }
18589 double qn = retvals["qn"];
18590
18591 if(qn >= peak) {
18592 sx = -ix;
18593 sy = -iy;
18594 nref = iref;
18595 if (!mirror_only) {
18596 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18597 peak = static_cast<float>(qn);
18598 mirror = 0;
18599 } else {
18600 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18601 peak = static_cast<float>(qn);
18602 mirror = 1;
18603 }
18604 }
18605 }
18606 }
18607 delete cimage; cimage = 0;
18608 }
18609 }
18610 }
18611
18612 else if(ynumber==0) {
18613 sy = 0.0f;
18614 for (int j = -kx; j <= kx; j++) {
18615 ix = j*step ;
18616 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18617
18618 Normalize_ring( cimage, numr );
18619
18620 Frngs(cimage, numr);
18621
18622
18623 for ( iref = 0; iref < (int)crefim_len; iref++) {
18624
18625 use_ref = false;
18626 if (!mirror_only){
18627
18628 nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18629 if (nbrinp >= ant){
18630 use_ref = true;
18631 }
18632 }
18633 else if (mirror_only) {
18634
18635 nbrinp = (-1.0f*n1[iref]*imn1) + (-1.0f*n2[iref]*imn2) + n3[iref]*imn3;
18636 if (nbrinp >= ant){
18637 use_ref = true;
18638 }
18639 }
18640
18641 if(use_ref) {
18642 Dict retvals;
18643 if (mirror_only == true){
18644 if ((psi-90) < 90)
18645 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
18646 else
18647 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max);
18648 }
18649 else{
18650 if ((psi-90) < 90)
18651 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18652 else
18653 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18654 }
18655 double qn = retvals["qn"];
18656
18657 if(qn >= peak) {
18658 sx = -ix;
18659 sy = -iy;
18660 nref = iref;
18661 if (!mirror_only) {
18662 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18663 peak = static_cast<float>(qn);
18664 mirror = 0;
18665 } else {
18666 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18667 peak = static_cast<float>(qn);
18668 mirror = 1;
18669 }
18670 }
18671 }
18672 }
18673 delete cimage; cimage = 0;
18674 }
18675 } else {
18676 int ky = int(ynumber/2);
18677 float stepy=2*yrng/ynumber;
18678
18679
18680 if (yrnglocal >= 0.0){
18681 ky = int(yrnglocal/stepy);
18682 }
18683
18684
18685
18686 for (int i = -ky+1; i <= ky; i++) {
18687 iy = i * stepy ;
18688 for (int j = -kx; j <= kx; j++) {
18689 ix = j*step ;
18690 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18691
18692 Normalize_ring( cimage, numr );
18693
18694 Frngs(cimage, numr);
18695
18696
18697 for ( iref = 0; iref < (int)crefim_len; iref++) {
18698
18699 use_ref = false;
18700 if (!mirror_only){
18701
18702 nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18703 if (nbrinp >= ant){
18704 use_ref = true;
18705 }
18706 }
18707 else if (mirror_only) {
18708
18709 nbrinp = (-1.0*n1[iref]*imn1) + (-1.0*n2[iref]*imn2) + n3[iref]*imn3;
18710 if (nbrinp >= ant){
18711 use_ref = true;
18712 }
18713 }
18714 if(use_ref) {
18715 Dict retvals;
18716 if (mirror_only == true){
18717 if ((psi-90) < 90)
18718 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
18719 else
18720 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max);
18721 }
18722 else{
18723 if ((psi-90) < 90)
18724 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18725 else
18726 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18727 }
18728 double qn = retvals["qn"];
18729
18730 if(qn >= peak) {
18731 sx = -ix;
18732 sy = -iy;
18733 nref = iref;
18734 if (!mirror_only) {
18735 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18736 peak = static_cast<float>(qn);
18737 mirror = 0;
18738 } else {
18739 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18740 peak = static_cast<float>(qn);
18741 mirror = 1;
18742 }
18743 }
18744 }
18745 }
18746 delete cimage; cimage = 0;
18747 }
18748 }
18749 }
18750 float co, so, sxs, sys;
18751 co = static_cast<float>( cos(ang*pi/180.0) );
18752 so = static_cast<float>( -sin(ang*pi/180.0) );
18753 sxs = sx*co - sy*so;
18754 sys = sx*so + sy*co;
18755 vector<float> res;
18756 res.push_back(ang);
18757 res.push_back(sxs);
18758 res.push_back(sys);
18759 res.push_back(static_cast<float>(mirror));
18760 res.push_back(static_cast<float>(nref));
18761 res.push_back(peak);
18762 return res;
18763 }
18764
18765
18766 vector<float> Util::multiref_polar_ali_helical_90(EMData* image, const vector< EMData* >& crefim,
18767 float xrng, float yrng, float step, float psi_max, string mode,
18768 vector<int>numr, float cnx, float cny, int ynumber) {
18769
18770 size_t crefim_len = crefim.size();
18771
18772 int iref, nref=0, mirror=0;
18773 float iy, ix, sx=0, sy=0;
18774 float peak = -1.0E23f;
18775 float ang=0.0f;
18776 int kx = int(2*xrng/step+0.5)/2;
18777
18778
18779 int ky;
18780 float stepy;
18781 int kystart;
18782
18783 if (ynumber == -1){
18784 ky = int(2*yrng/step+0.5)/2;
18785 stepy = step;
18786 kystart = -ky;
18787 }
18788 else if(ynumber == 0){
18789 ky = 0;
18790 stepy = 0.0f;
18791 kystart = ky;
18792 }
18793 else {
18794 ky = int(ynumber/2);
18795 stepy=2*yrng/ynumber;
18796 kystart = -ky + 1;
18797 }
18798
18799
18800 for (int i = kystart; i <= ky; i++) {
18801 iy = i * stepy ;
18802 for (int j = -kx; j <= kx; j++) {
18803 ix = j*step ;
18804 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18805
18806 Normalize_ring( cimage, numr );
18807
18808 Frngs(cimage, numr);
18809
18810
18811 for ( iref = 0; iref < (int)crefim_len; iref++) {
18812 Dict retvals_0 = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18813 Dict retvals_180 = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18814 double qn_0 = retvals_0["qn"];
18815 double qn_180 = retvals_180["qn"];
18816 double qn;
18817 bool qn_is_zero = false;
18818
18819 if (qn_0 >= qn_180){
18820 qn = qn_0;
18821 qn_is_zero = true;
18822 }
18823 else{
18824 qn = qn_180;
18825 qn_is_zero = false;
18826 }
18827
18828 if(qn >= peak) {
18829 sx = -ix;
18830 sy = -iy;
18831 nref = iref;
18832
18833 if (qn_is_zero){
18834 ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
18835 }
18836 else{
18837 ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
18838 }
18839 peak = static_cast<float>(qn);
18840 mirror = 0;
18841
18842 }
18843 }
18844 delete cimage; cimage = 0;
18845 }
18846 }
18847 float co, so, sxs, sys;
18848 co = static_cast<float>( cos(ang*pi/180.0) );
18849 so = static_cast<float>( -sin(ang*pi/180.0) );
18850 sxs = sx*co - sy*so;
18851 sys = sx*so + sy*co;
18852 vector<float> res;
18853 res.push_back(ang);
18854 res.push_back(sxs);
18855 res.push_back(sys);
18856 res.push_back(static_cast<float>(mirror));
18857 res.push_back(static_cast<float>(nref));
18858 res.push_back(peak);
18859 return res;
18860 }
18861
18862
18863 vector<float> Util::multiref_polar_ali_helical_90_local(EMData* image, const vector< EMData* >& crefim,
18864 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18865 vector<int>numr, float cnx, float cny, int ynumber) {
18866
18867 size_t crefim_len = crefim.size();
18868 const float qv = static_cast<float>( pi/180.0 );
18869 Transform * t = image->get_attr("xform.projection");
18870 Dict d = t->get_params("spider");
18871 if(t) {delete t; t=0;}
18872 float phi = d["phi"];
18873 float theta = d["theta"];
18874 float imn1 = sin(theta*qv)*cos(phi*qv);
18875 float imn2 = sin(theta*qv)*sin(phi*qv);
18876 float imn3 = cos(theta*qv);
18877 vector<float> n1(crefim_len);
18878 vector<float> n2(crefim_len);
18879 vector<float> n3(crefim_len);
18880 int iref, nref=-1, mirror=0;
18881 float iy, ix, sx=0, sy=0;
18882 float peak = -1.0E23f;
18883 float ang=0.0f;
18884 int kx = int(2*xrng/step+0.5)/2;
18885
18886 for ( iref = 0; iref < (int)crefim_len; iref++) {
18887 n1[iref] = crefim[iref]->get_attr("n1");
18888 n2[iref] = crefim[iref]->get_attr("n2");
18889 n3[iref] = crefim[iref]->get_attr("n3");
18890 }
18891
18892
18893 if(ynumber==-1) {
18894 int ky = int(2*yrng/step+0.5)/2;
18895 for (int i = -ky; i <= ky; i++) {
18896 iy = i * step ;
18897 for (int j = -kx; j <= kx; j++) {
18898 ix = j*step ;
18899 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18900
18901 Normalize_ring( cimage, numr );
18902
18903 Frngs(cimage, numr);
18904
18905
18906 for ( iref = 0; iref < (int)crefim_len; iref++) {
18907 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18908 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18909 double qn = retvals["qn"];
18910 if( qn >= peak) {
18911 sx = -ix;
18912 sy = -iy;
18913 nref = iref;
18914 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18915 peak = static_cast<float>(qn);
18916 mirror = 0;
18917 }
18918 }
18919 }
18920 delete cimage; cimage = 0;
18921 }
18922 }
18923 }
18924
18925 else if(ynumber==0) {
18926 sy = 0.0f;
18927 for (int j = -kx; j <= kx; j++) {
18928 ix = j*step ;
18929 iy = 0.0f ;
18930 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18931
18932 Normalize_ring( cimage, numr );
18933
18934 Frngs(cimage, numr);
18935
18936
18937 for ( iref = 0; iref < (int)crefim_len; iref++) {
18938 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18939 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18940 double qn = retvals["qn"];
18941 if( qn >= peak ) {
18942 sx = -ix;
18943 nref = iref;
18944 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18945 peak = static_cast<float>(qn);
18946 mirror = 0;
18947 }
18948 }
18949 }
18950 delete cimage; cimage = 0;
18951 }
18952 } else {
18953 int ky = int(ynumber/2);
18954 float stepy=2*yrng/ynumber;
18955
18956 for (int i = -ky+1; i <= ky; i++) {
18957 iy = i * stepy ;
18958 for (int j = -kx; j <= kx; j++) {
18959 ix = j*step ;
18960 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18961
18962 Normalize_ring( cimage, numr );
18963
18964 Frngs(cimage, numr);
18965
18966
18967 for ( iref = 0; iref < (int)crefim_len; iref++) {
18968 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18969 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18970 double qn = retvals["qn"];
18971 if( qn >= peak) {
18972 sx = -ix;
18973 sy = -iy;
18974 nref = iref;
18975 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18976 peak = static_cast<float>(qn);
18977 mirror = 0;
18978 }
18979 }
18980 }
18981 delete cimage; cimage = 0;
18982 }
18983 }
18984 }
18985 float co, so, sxs, sys;
18986 co = static_cast<float>( cos(ang*pi/180.0) );
18987 so = static_cast<float>( -sin(ang*pi/180.0) );
18988 sxs = sx*co - sy*so;
18989 sys = sx*so + sy*co;
18990 vector<float> res;
18991 res.push_back(ang);
18992 res.push_back(sxs);
18993 res.push_back(sys);
18994 res.push_back(static_cast<float>(mirror));
18995 res.push_back(static_cast<float>(nref));
18996 res.push_back(peak);
18997 return res;
18998 }
18999
19000
19001 void Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
19002 float xrng, float yrng, float step, string mode,
19003 vector< int >numr, float cnx, float cny,
19004 EMData *peaks, EMData *peakm) {
19005
19006 int maxrin = numr[numr.size()-1];
19007
19008 int ky = int(2*yrng/step+0.5)/2;
19009 int kx = int(2*xrng/step+0.5)/2;
19010
19011 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19012 float *p_ccf1ds = peaks->get_data();
19013
19014 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19015 float *p_ccf1dm = peakm->get_data();
19016
19017 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19018 p_ccf1ds[i] = -1.e20f;
19019 p_ccf1dm[i] = -1.e20f;
19020 }
19021
19022 for (int i = -ky; i <= ky; i++) {
19023 float iy = i * step;
19024 for (int j = -kx; j <= kx; j++) {
19025 float ix = j*step;
19026 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19027 Frngs(cimage, numr);
19028 Crosrng_msg_vec(crefim, cimage, numr,
19029 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19030 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19031 delete cimage; cimage = 0;
19032 }
19033 }
19034 return;
19035 }
19036
19037 void Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
19038 float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
19039 EMData *peaks_compress, EMData *peakm_compress) {
19040
19041 int maxrin = numr[numr.size()-1];
19042
19043 int ky = int(2*yrng/step+0.5)/2;
19044 int kx = int(2*xrng/step+0.5)/2;
19045
19046 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19047 float *p_ccf1ds = peaks->get_data();
19048
19049 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19050 float *p_ccf1dm = peakm->get_data();
19051
19052 peaks_compress->set_size(maxrin, 1, 1);
19053 float *p_ccf1ds_compress = peaks_compress->get_data();
19054
19055 peakm_compress->set_size(maxrin, 1, 1);
19056 float *p_ccf1dm_compress = peakm_compress->get_data();
19057
19058 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19059 p_ccf1ds[i] = -1.e20f;
19060 p_ccf1dm[i] = -1.e20f;
19061 }
19062
19063 for (int i = -ky; i <= ky; i++) {
19064 float iy = i * step;
19065 for (int j = -kx; j <= kx; j++) {
19066 float ix = j*step;
19067 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19068 Frngs(cimage, numr);
19069 Crosrng_msg_vec(crefim, cimage, numr,
19070 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19071 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19072 delete cimage; cimage = 0;
19073 }
19074 }
19075 for (int x=0; x<maxrin; x++) {
19076 float maxs = -1.0e22f;
19077 float maxm = -1.0e22f;
19078 for (int i=1; i<=2*ky+1; i++) {
19079 for (int j=1; j<=2*kx+1; j++) {
19080 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
19081 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
19082 }
19083 }
19084 p_ccf1ds_compress[x] = maxs;
19085 p_ccf1dm_compress[x] = maxm;
19086 }
19087 return;
19088 }
19089
19090 struct ccf_point
19091 {
19092 float value;
19093 int i;
19094 int j;
19095 int k;
19096 int mirror;
19097 };
19098
19099
19100 struct ccf_value
19101 {
19102 bool operator()( const ccf_point& a, const ccf_point& b )
19103 {
19104 return a.value > b.value;
19105 }
19106 };
19107
19108
19109 vector<float> Util::ali2d_ccf_list(EMData* image, EMData* crefim,
19110 float xrng, float yrng, float step, string mode,
19111 vector< int >numr, float cnx, float cny, double T) {
19112
19113 int maxrin = numr[numr.size()-1];
19114
19115 int ky = int(2*yrng/step+0.5)/2;
19116 int kx = int(2*xrng/step+0.5)/2;
19117
19118 float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
19119 float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
19120 int vol = maxrin*(2*kx+1)*(2*ky+1);
19121 vector<ccf_point> ccf(2*vol);
19122 ccf_point temp;
19123
19124 int index = 0;
19125 for (int i = -ky; i <= ky; i++) {
19126 float iy = i * step;
19127 for (int j = -kx; j <= kx; j++) {
19128 float ix = j*step;
19129 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19130 Frngs(cimage, numr);
19131 Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
19132 for (int k=0; k<maxrin; k++) {
19133 temp.value = p_ccf1ds[k];
19134 temp.i = k;
19135 temp.j = j;
19136 temp.k = i;
19137 temp.mirror = 0;
19138 ccf[index] = temp;
19139 index++;
19140 temp.value = p_ccf1dm[k];
19141 temp.mirror = 1;
19142 ccf[index] = temp;
19143 index++;
19144 }
19145 delete cimage; cimage = 0;
19146 }
19147 }
19148
19149 delete p_ccf1ds;
19150 delete p_ccf1dm;
19151 std::sort(ccf.begin(), ccf.end(), ccf_value());
19152
19153 double qt = (double)ccf[0].value;
19154 vector <double> p(2*vol), cp(2*vol);
19155
19156 double sump = 0.0;
19157 for (int i=0; i<2*vol; i++) {
19158 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
19159 sump += p[i];
19160 }
19161 for (int i=0; i<2*vol; i++) {
19162 p[i] /= sump;
19163 }
19164 for (int i=1; i<2*vol; i++) {
19165 p[i] += p[i-1];
19166 }
19167 p[2*vol-1] = 2.0;
19168
19169 float t = get_frand(0.0f, 1.0f);
19170 int select = 0;
19171 while (p[select] < t) select += 1;
19172
19173 vector<float> a(6);
19174 a[0] = ccf[select].value;
19175 a[1] = (float)ccf[select].i;
19176 a[2] = (float)ccf[select].j;
19177 a[3] = (float)ccf[select].k;
19178 a[4] = (float)ccf[select].mirror;
19179 a[5] = (float)select;
19180 return a;
19181 }
19182
19183
19184
19185
19186
19187
19188
19189
19190
19191
19192
19193
19194
19195
19196
19197
19198
19199
19200
19201
19202
19203
19204
19205
19206
19207
19208
19209
19210
19211
19212
19213
19214
19215
19216
19217
19218
19219
19220
19221
19222
19223
19224
19225
19226
19227
19228
19229
19230
19231
19232
19233
19234
19235
19236
19237
19238
19239
19240
19241
19242
19243
19244
19245
19246
19247
19248
19249
19250 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19251
19252 EMData *rot;
19253
19254 const int nmax=3, mmax=3;
19255 char task[60], csave[60];
19256 long int lsave[4];
19257 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19258 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];
19259 long int SIXTY=60;
19260
19261
19262 iprint = -1;
19263
19264
19265 factr=1.0e1;
19266 pgtol=1.0e-5;
19267
19268
19269
19270
19271 n=3;
19272 m=3;
19273
19274
19275
19276
19277
19278 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19279 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19280 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19281
19282
19283
19284
19285 strcpy(task,"START");
19286 for (int i=5;i<60;i++) task[i]=' ';
19287
19288
19289
19290 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19291
19292
19293
19294 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19295
19296 if (strncmp(task,"FG",2)==0) {
19297
19298
19299
19300
19301 rot = new EMData();
19302 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
19303 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19304
19305 delete rot;
19306
19307
19308 float dt = 1.0e-3f;
19309 rot = new EMData();
19310 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
19311 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19312
19313 g[0] = (f1-f)/dt;
19314 delete rot;
19315
19316 dt = 1.0e-2f;
19317 rot = new EMData();
19318 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
19319 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19320
19321 g[1] = (f2-f)/dt;
19322 delete rot;
19323
19324 rot = new EMData();
19325 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
19326 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19327
19328 g[2] = (f3-f)/dt;
19329 delete rot;
19330 }
19331
19332
19333 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19334
19335 }
19336
19337
19338 vector<float> res;
19339 res.push_back(static_cast<float>(x[0]));
19340 res.push_back(static_cast<float>(x[1]));
19341 res.push_back(static_cast<float>(x[2]));
19342
19343 return res;
19344 }
19345
19346 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19347
19348 EMData *rot;
19349
19350 const int nmax=3, mmax=3;
19351 char task[60], csave[60];
19352 long int lsave[4];
19353 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19354 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];
19355 long int SIXTY=60;
19356
19357
19358 iprint = -1;
19359
19360
19361 factr=1.0e1;
19362 pgtol=1.0e-5;
19363
19364
19365
19366
19367 n=3;
19368 m=3;
19369
19370
19371
19372
19373
19374 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19375 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19376 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19377
19378
19379
19380
19381 strcpy(task,"START");
19382 for (int i=5;i<60;i++) task[i]=' ';
19383
19384
19385
19386 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19387
19388
19389
19390 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19391
19392 if (strncmp(task,"FG",2)==0) {
19393
19394
19395
19396
19397 rot = new EMData();
19398 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19399 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19400
19401 delete rot;
19402
19403
19404 float dt = 1.0e-3f;
19405 rot = new EMData();
19406 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19407 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19408
19409 g[0] = (f1-f)/dt;
19410 delete rot;
19411
19412 rot = new EMData();
19413 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
19414 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19415
19416 g[1] = (f2-f)/dt;
19417 delete rot;
19418
19419 rot = new EMData();
19420 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
19421 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19422
19423 g[2] = (f3-f)/dt;
19424 delete rot;
19425 }
19426
19427
19428 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19429
19430 }
19431
19432
19433 vector<float> res;
19434 res.push_back(static_cast<float>(x[0]));
19435 res.push_back(static_cast<float>(x[1]));
19436 res.push_back(static_cast<float>(x[2]));
19437
19438 return res;
19439 }
19440
19441 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) {
19442
19443 EMData *proj, *proj2;
19444
19445 const int nmax=5, mmax=5;
19446 char task[60], csave[60];
19447 long int lsave[4];
19448 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19449 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];
19450 long int SIXTY=60;
19451
19452
19453 iprint = -1;
19454
19455
19456 factr=1.0e1;
19457 pgtol=1.0e-5;
19458
19459
19460
19461
19462 n=5;
19463 m=5;
19464
19465
19466
19467
19468
19469 x[0] = phi; nbd[0] = 2; l[0] = phi-2.0; u[0] = phi+2.0;
19470 x[1] = theta; nbd[1] = 2; l[1] = theta-2.0; u[1] = theta+2.0;
19471 x[2] = psi; nbd[2] = 2; l[2] = psi-2.0; u[2] = psi+2.0;
19472 x[3] = sxs; nbd[3] = 2; l[3] = sxs-2.0; u[3] = sxs+2.0;
19473 x[4] = sys; nbd[4] = 2; l[4] = sys-2.0; u[4] = sys+2.0;
19474
19475
19476
19477
19478 strcpy(task,"START");
19479 for (int i=5;i<60;i++) task[i]=' ';
19480
19481
19482
19483 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19484 int step = 1;
19485
19486
19487 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19488
19489 if (strncmp(task,"FG",2)==0) {
19490
19491
19492
19493
19494 proj = new EMData();
19495 proj2 = new EMData();
19496 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19497 proj->fft_shuffle();
19498 proj->center_origin_fft();
19499 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19500 proj->do_ift_inplace();
19501 int M = proj->get_ysize()/2;
19502 proj2 = proj->window_center(M);
19503 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19504
19505 delete proj;
19506 delete proj2;
19507
19508
19509 float dt = 1.0e-3f;
19510 proj = new EMData();
19511 proj2 = new EMData();
19512 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
19513 proj->fft_shuffle();
19514 proj->center_origin_fft();
19515 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19516 proj->do_ift_inplace();
19517 proj2 = proj->window_center(M);
19518 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19519
19520 delete proj;
19521 delete proj2;
19522 g[0] = (ft-f)/dt;
19523
19524 proj = new EMData();
19525 proj2 = new EMData();
19526 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
19527 proj->fft_shuffle();
19528 proj->center_origin_fft();
19529 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19530 proj->do_ift_inplace();
19531 proj2 = proj->window_center(M);
19532 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19533
19534 delete proj;
19535 delete proj2;
19536 g[1] = (ft-f)/dt;
19537
19538 proj = new EMData();
19539 proj2 = new EMData();
19540 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
19541 proj->fft_shuffle();
19542 proj->center_origin_fft();
19543 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19544 proj->do_ift_inplace();
19545 proj2 = proj->window_center(M);
19546 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19547
19548 delete proj;
19549 delete proj2;
19550 g[2] = (ft-f)/dt;
19551
19552 proj = new EMData();
19553 proj2 = new EMData();
19554 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19555 proj->fft_shuffle();
19556 proj->center_origin_fft();
19557 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
19558 proj->do_ift_inplace();
19559 proj2 = proj->window_center(M);
19560 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19561
19562 delete proj;
19563 delete proj2;
19564 g[3] = (ft-f)/dt;
19565
19566 proj = new EMData();
19567 proj2 = new EMData();
19568 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19569 proj->fft_shuffle();
19570 proj->center_origin_fft();
19571 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
19572 proj->do_ift_inplace();
19573 proj2 = proj->window_center(M);
19574 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19575
19576 delete proj;
19577 delete proj2;
19578 g[4] = (ft-f)/dt;
19579 }
19580
19581
19582 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19583 step++;
19584 }
19585
19586
19587 vector<float> res;
19588 res.push_back(static_cast<float>(x[0]));
19589 res.push_back(static_cast<float>(x[1]));
19590 res.push_back(static_cast<float>(x[2]));
19591 res.push_back(static_cast<float>(x[3]));
19592 res.push_back(static_cast<float>(x[4]));
19593
19594 return res;
19595 }
19596
19597
19598 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19599
19600 double x[4];
19601 int n;
19602 int l = 3;
19603 int m = 200;
19604 double e = 1e-9;
19605 double step = 0.01;
19606 float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
19607
19608 x[1] = ang;
19609 x[2] = sxs;
19610 x[3] = sys;
19611
19612 Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);
19613
19614
19615 vector<float> res;
19616 res.push_back(static_cast<float>(x[1]));
19617 res.push_back(static_cast<float>(x[2]));
19618 res.push_back(static_cast<float>(x[3]));
19619 res.push_back(static_cast<float>(n));
19620 return res;
19621 }
19622
19623 vector<float> Util::multi_align_error(vector<float> args, vector<float> all_ali_params, int d) {
19624
19625 const int nmax=args.size(), mmax=nmax;
19626 char task[60], csave[60];
19627 long int lsave[4];
19628 long int n, m, iprint, isave[44];
19629 long int* nbd = new long int[nmax];
19630 long int* iwa = new long int[3*nmax];
19631 double f, factr, pgtol;
19632 double* x = new double[nmax];
19633 double* l = new double[nmax];
19634 double* u = new double[nmax];
19635 double* g = new double[nmax];
19636 double dsave[29];
19637 double* wa = new double[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
19638 long int SIXTY=60;
19639
19640 int num_ali = nmax/3+1;
19641 int nima = all_ali_params.size()/(num_ali*4);
19642
19643
19644 iprint = -1;
19645
19646
19647 factr=1.0e1;
19648 pgtol=1.0e-9;
19649
19650
19651
19652
19653 n=nmax;
19654 m=mmax;
19655
19656
19657
19658
19659
19660 for (int i=0; i<nmax; i++) {
19661 x[i] = args[i];
19662 nbd[i] = 0;
19663 }
19664
19665
19666
19667 strcpy(task,"START");
19668 for (int i=5;i<60;i++) task[i]=' ';
19669
19670
19671
19672 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19673 int step = 1;
19674
19675
19676 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19677
19678 if (strncmp(task,"FG",2)==0) {
19679
19680
19681
19682
19683 f = multi_align_error_func(x, all_ali_params, nima, num_ali, d);
19684
19685
19686 multi_align_error_dfunc(x, all_ali_params, nima, num_ali, g, d);
19687
19688 }
19689
19690 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19691 step++;
19692 }
19693
19694
19695 vector<float> res;
19696 for (int i=0; i<nmax; i++) res.push_back(static_cast<float>(x[i]));
19697 res.push_back(static_cast<float>(f));
19698
19699 delete[] nbd;
19700 delete[] iwa;
19701 delete[] x;
19702 delete[] l;
19703 delete[] u;
19704 delete[] g;
19705 delete[] wa;
19706
19707 return res;
19708
19709 }
19710
19711 double Util::multi_align_error_func(double* x, vector<float> all_ali_params, int nima, int num_ali, int d) {
19712
19713 vector<double> sqr_pixel_error = multi_align_error_func2(x, all_ali_params, nima, num_ali, d);
19714 double sum_sqr_pixel_error = 0.0;
19715 for (int i=0; i<nima; i++) sum_sqr_pixel_error += sqr_pixel_error[i];
19716 return sum_sqr_pixel_error/static_cast<float>(nima);
19717 }
19718
19719
19720 vector<double> Util::multi_align_error_func2(double* x, vector<float> ali_params, int nima, int num_ali, int d) {
19721
19722 double* args = new double[num_ali*3];
19723 for (int i=0; i<3*num_ali-3; i++) args[i] = x[i];
19724 args[3*num_ali-3] = 0.0;
19725 args[3*num_ali-2] = 0.0;
19726 args[3*num_ali-1] = 0.0;
19727 double* cosa = new double[num_ali];
19728 double* sina = new double[num_ali];
19729 for (int i=0; i<num_ali; i++) {
19730 cosa[i] = cos(args[i*3]*M_PI/180.0);
19731 sina[i] = sin(args[i*3]*M_PI/180.0);
19732 }
19733 double* sx = new double[num_ali];
19734 double* sy = new double[num_ali];
19735
19736 vector<double> sqr_pixel_error(nima);
19737
19738 for (int i=0; i<nima; i++) {
19739 double sum_cosa = 0.0;
19740 double sum_sina = 0.0;
19741 for (int j=0; j<num_ali; j++) {
19742 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19743 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19744 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19745 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];
19746 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];
19747 } else {
19748 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19749 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19750 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];
19751 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];
19752 }
19753 }
19754 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
19755 sum_cosa /= P;
19756 sum_sina /= P;
19757 sqr_pixel_error[i] = d*d/4.0*(1.0-P/num_ali)+var(sx, num_ali)+var(sy, num_ali);
19758 }
19759
19760 delete[] args;
19761 delete[] cosa;
19762 delete[] sina;
19763 delete[] sx;
19764 delete[] sy;
19765
19766 return sqr_pixel_error;
19767 }
19768
19769 void Util::multi_align_error_dfunc(double* x, vector<float> ali_params, int nima, int num_ali, double* g, int d) {
19770
19771 for (int i=0; i<num_ali*3-3; i++) g[i] = 0.0;
19772
19773 double* args = new double[num_ali*3];
19774 for (int i=0; i<3*num_ali-3; i++) args[i] = x[i];
19775 args[3*num_ali-3] = 0.0;
19776 args[3*num_ali-2] = 0.0;
19777 args[3*num_ali-1] = 0.0;
19778 double* cosa = new double[num_ali];
19779 double* sina = new double[num_ali];
19780 for (int i=0; i<num_ali; i++) {
19781 cosa[i] = cos(args[i*3]*M_PI/180.0);
19782 sina[i] = sin(args[i*3]*M_PI/180.0);
19783 }
19784 double* sx = new double[num_ali];
19785 double* sy = new double[num_ali];
19786
19787 vector<float> sqr_pixel_error(nima);
19788
19789 for (int i=0; i<nima; i++) {
19790 double sum_cosa = 0.0;
19791 double sum_sina = 0.0;
19792 for (int j=0; j<num_ali; j++) {
19793 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19794 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19795 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19796 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];
19797 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];
19798 } else {
19799 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19800 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
19801 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];
19802 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];
19803 }
19804 }
19805 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
19806 sum_cosa /= P;
19807 sum_sina /= P;
19808 for (int j=0; j<num_ali-1; j++) {
19809 double dx = 2.0*(sx[j]-mean(sx, num_ali));
19810 double dy = 2.0*(sy[j]-mean(sy, num_ali));
19811 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
19812 g[j*3] += (d*d/4.0*(sum_cosa*sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) -
19813 sum_sina*cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
19814 dx*(-ali_params[j*nima*4+i*4+1]*sina[j]-ali_params[j*nima*4+i*4+2]*cosa[j])+
19815 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;
19816 g[j*3+1] += dx;
19817 g[j*3+2] += dy;
19818 } else {
19819 g[j*3] += (d*d/4.0*(-sum_cosa*sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) +
19820 sum_sina*cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
19821 dx*(-ali_params[j*nima*4+i*4+1]*sina[j]+ali_params[j*nima*4+i*4+2]*cosa[j])+
19822 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;
19823 g[j*3+1] += -dx;
19824 g[j*3+2] += dy;
19825 }
19826 }
19827 }
19828
19829 for (int i=0; i<3*num_ali-3; i++) g[i] /= (num_ali*nima);
19830
19831 delete[] args;
19832 delete[] cosa;
19833 delete[] sina;
19834 delete[] sx;
19835 delete[] sy;
19836 }
19837
19838 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
19839
19840 EMData *rot= new EMData();
19841 float ccc;
19842
19843 rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
19844 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19845 delete rot;
19846 return ccc;
19847 }
19848
19849 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19850
19851 double x[4];
19852 int n;
19853 int l = 3;
19854 int m = 200;
19855 double e = 1e-9;
19856 double step = 0.001;
19857 float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
19858
19859 x[1] = ang;
19860 x[2] = sxs;
19861 x[3] = sys;
19862
19863 Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);
19864
19865
19866 vector<float> res;
19867 res.push_back(static_cast<float>(x[1]));
19868 res.push_back(static_cast<float>(x[2]));
19869 res.push_back(static_cast<float>(x[3]));
19870 res.push_back(static_cast<float>(n));
19871 return res;
19872 }
19873
19874
19875 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
19876
19877 EMData *rot= new EMData();
19878 float ccc;
19879
19880 rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
19881 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19882 delete rot;
19883 return ccc;
19884 }
19885
19886 #define img_ptr(i,j,k) img_ptr[i+(j+(k*ny))*(size_t)nx]
19887 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*(size_t)nx]
19888 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
19889 {
19890 ENTERFUNC;
19891
19892 if (!img) {
19893 throw NullPointerException("NULL input image");
19894 }
19895
19896 int newx, newy, newz;
19897 bool keep_going;
19898 cout << " entered " <<endl;
19899 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
19900
19901 EMData * img2 = new EMData();
19902 img2->set_size(nx,ny,nz);
19903 img2->to_zero();
19904 float *img_ptr =img->get_data();
19905 float *img2_ptr = img2->get_data();
19906 int r2 = ro*ro;
19907 int r3 = r2*ro;
19908 int ri2 = ri*ri;
19909 int ri3 = ri2*ri;
19910
19911 int n2 = nx/2;
19912
19913 for (int k=-n2; k<=n2; k++) {
19914 float z2 = static_cast<float>(k*k);
19915 for (int j=-n2; j<=n2; j++) {
19916 float y2 = z2 + j*j;
19917 if(y2 <= r2) {
19918
19919
19920 for (int i=-n2; i<=n2; i++) {
19921 float x2 = y2 + i*i;
19922 if(x2 <= r3) {
19923
19924 int ib = i+n2; int jb = j+n2; int kb = k+n2;
19925 if(x2 >= ri3) {
19926
19927 if(img_ptr(ib,jb,kb) == 1.0f) {
19928
19929 if(Util::get_frand(0.0f, 1.0f) > qprob){
19930 img2_ptr(ib,jb,kb) = 0.0f;
19931 keep_going = true;
19932
19933 while(keep_going) {
19934 newx = Util::get_irand(-ro,ro);
19935 newy = Util::get_irand(-ro,ro);
19936 newz = Util::get_irand(-ro,ro);
19937 if(newx*newx+newy*newy+newz*newz <= r3) {
19938 newx += n2; newy += n2; newz += n2;
19939 if( img_ptr(newx,newy,newz) == 0.0f) {
19940 img2_ptr(newx,newy,newz) = 1.0f;
19941 keep_going = false; }
19942 }
19943 }
19944 } else img2_ptr(ib,jb,kb) = 1.0f;
19945 }
19946 } else {
19947
19948 if(img_ptr(ib,jb,kb) == 1.0) {
19949 if(Util::get_frand(0.0f,1.0f) > qprob) {
19950
19951 float numn = -1.0f;
19952 for (newz = -1; newz <= 1; newz++)
19953 for (newy = -1; newy <= 1; newy++)
19954 for (newx = -1; newx <= 1; newx++)
19955 numn += img_ptr(ib+newx,jb+newy,kb+newz);
19956 img2_ptr(ib,jb,kb) = 0.0;
19957 if(numn == 26.0f) {
19958
19959 keep_going = true;
19960 while(keep_going) {
19961 newx = Util::get_irand(-ro,ro);
19962 newy = Util::get_irand(-ro,ro);
19963 newz = Util::get_irand(-ro,ro);
19964 if(newx*newx+newy*newy+newz*newz < r3) {
19965 newx += n2; newy += n2; newz += n2;
19966 if( img_ptr(newx,newy,newz) == 0.0f) {
19967 if(newx*newx+newy*newy+newz*newz < r3) {
19968 if(newx*newx+newy*newy+newz*newz < r3) {
19969 newx += n2; newy += n2; newz += n2;
19970 if( img_ptr(newx,newy,newz) == 0.0f) {
19971 img2_ptr(newx,newy,newz) = 1.0f;
19972 keep_going = false; }
19973 }
19974 }
19975 }
19976 }
19977 }
19978 } else if(numn == 25.0f) {
19979
19980 for (newz = -1; newz <= 1; newz++) {
19981 for (newy = -1; newy <= 1; newy++) {
19982 for (newx = -1; newx <= 1; newx++) {
19983 if( newx != 0 && newy != 0 && newz != 0) {
19984 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
19985 img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
19986 }
19987 }
19988 }
19989 }
19990 }
19991 } else {
19992
19993 keep_going = true;
19994 while(keep_going) {
19995 newx = Util::get_irand(-1,1);
19996 newy = Util::get_irand(-1,1);
19997 newz = Util::get_irand(-1,1);
19998 if(newx != 0 && newy != 0 && newz != 0) {
19999 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
20000 img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;
20001 keep_going = false;
20002 }
20003 }
20004 }
20005 }
20006 } else img2_ptr(ib,jb,kb) = 1.0f;
20007 }
20008 }
20009 }
20010 }
20011 }
20012 }
20013 }
20014
20015 img2->update();
20016
20017 EXITFUNC;
20018 return img2;
20019 }
20020 #undef img_ptr
20021 #undef img2_ptr
20022
20023 struct point3d_t
20024 {
20025 point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
20026
20027 int x;
20028 int y;
20029 int z;
20030 };
20031
20032
20033 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
20034 {
20035 int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
20036 int noff = 6;
20037
20038 int nx = visited->get_xsize();
20039 int ny = visited->get_ysize();
20040 int nz = visited->get_zsize();
20041
20042 vector< point3d_t > pts;
20043 pts.push_back( point3d_t(ix, iy, iz) );
20044 visited->set_value_at( ix, iy, iz, (float)grpid );
20045
20046 int start = 0;
20047 int end = pts.size();
20048
20049 while( end > start ) {
20050 for(int i=start; i < end; ++i ) {
20051 int ix = pts[i].x;
20052 int iy = pts[i].y;
20053 int iz = pts[i].z;
20054
20055 for( int j=0; j < noff; ++j ) {
20056 int jx = ix + offs[j][0];
20057 int jy = iy + offs[j][1];
20058 int jz = iz + offs[j][2];
20059
20060 if( jx < 0 || jx >= nx ) continue;
20061 if( jy < 0 || jy >= ny ) continue;
20062 if( jz < 0 || jz >= nz ) continue;
20063
20064
20065 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
20066 pts.push_back( point3d_t(jx, jy, jz) );
20067 visited->set_value_at( jx, jy, jz, (float)grpid );
20068 }
20069
20070 }
20071 }
20072
20073 start = end;
20074 end = pts.size();
20075 }
20076 return pts.size();
20077 }
20078
20079
20080 EMData* Util::get_biggest_cluster( EMData* mg )
20081 {
20082 int nx = mg->get_xsize();
20083 int ny = mg->get_ysize();
20084 int nz = mg->get_zsize();
20085
20086 EMData* visited = new EMData();
20087 visited->set_size( nx, ny, nz );
20088 visited->to_zero();
20089 int grpid = 0;
20090 int maxgrp = 0;
20091 int maxsize = 0;
20092 for( int iz=0; iz < nz; ++iz ) {
20093 for( int iy=0; iy < ny; ++iy ) {
20094 for( int ix=0; ix < nx; ++ix ) {
20095 if( (*mg)(ix, iy, iz)==0.0 ) continue;
20096
20097 if( (*visited)(ix, iy, iz) > 0.0 ) {
20098
20099 continue;
20100 }
20101
20102 grpid++;
20103 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
20104 if( grpsize > maxsize ) {
20105 maxsize = grpsize;
20106 maxgrp = grpid;
20107 }
20108 }
20109 }
20110 }
20111
20112 Assert( maxgrp > 0 );
20113
20114 int npoint = 0;
20115 EMData* result = new EMData();
20116 result->set_size( nx, ny, nz );
20117 result->to_zero();
20118
20119 for( int iz=0; iz < nz; ++iz ) {
20120 for( int iy=0; iy < ny; ++iy ) {
20121 for( int ix=0; ix < nx; ++ix ) {
20122 if( (*visited)(ix, iy, iz)==maxgrp ) {
20123 (*result)(ix,iy,iz) = 1.0;
20124 npoint++;
20125 }
20126 }
20127 }
20128 }
20129
20130 Assert( npoint==maxsize );
20131 delete visited;
20132 return result;
20133
20134 }
20135
20136 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)
20137 {
20138 int ix, iy, iz;
20139 int i, j, k;
20140 int nr2, nl2;
20141 float az, ak;
20142 float scx, scy, scz;
20143 int offset = 2 - nx%2;
20144 int lsm = nx + offset;
20145 EMData* ctf_img1 = new EMData();
20146 ctf_img1->set_size(lsm, ny, nz);
20147 float freq = 1.0f/(2.0f*ps);
20148 scx = 2.0f/float(nx);
20149 if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20150 if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20151 nr2 = ny/2 ;
20152 nl2 = nz/2 ;
20153 float pihalf = M_PI/2.0f;
20154 for ( k=0; k<nz;k++) {
20155 iz = k; if(k>nl2) iz=k-nz;
20156 float oz2 = iz*scz*iz*scz;
20157 for ( j=0; j<ny;j++) {
20158 iy = j; if(j>nr2) iy=j - ny;
20159 float oy = iy*scy;
20160 float oy2 = oy*oy;
20161 for ( i=0; i<lsm/2; i++) {
20162 ix=i;
20163 if( dza == 0.0f) {
20164 ak=pow(ix*ix*scx*scx + oy2 + oz2, 0.5f)*freq;
20165 (*ctf_img1) (i*2,j,k) = Util::tf(dz, ak, voltage, cs, wgh, b_factor, sign);
20166 } else {
20167 float ox = ix*scx;
20168 ak=pow(ox*ox + oy2 + oz2, 0.5f)*freq;
20169 az = atan2(oy, ox);
20170 float dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f-pihalf));
20171 (*ctf_img1) (i*2,j,k) = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
20172 }
20173
20174 }
20175 }
20176 }
20177 ctf_img1->update();
20178 ctf_img1->set_complex(true);
20179 ctf_img1->set_ri(true);
20180
20181
20182 if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
20183 return ctf_img1;
20184 }
20185
20186
20187
20188
20189
20190
20191
20192
20193
20194
20195
20196
20197
20198
20199
20200
20201
20202
20203
20204
20205
20206
20207
20208
20209
20210
20211
20212
20213
20214
20215
20216
20217
20218
20219
20220
20221
20222
20223
20224
20225
20226
20227
20228
20229
20230
20231
20232
20233
20234
20235
20236
20237
20238
20239
20240
20241
20242
20243
20244
20245
20246
20247
20248
20249
20250
20251
20252
20253
20254
20255
20256
20257
20258
20259
20260
20261
20262
20263
20264
20265
20266
20267
20268
20269
20270
20271
20272 #define cent(i) out[i+N]
20273 #define assign(i) out[i]
20274 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
20275 int nx = d->get_xsize();
20276 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20277 vector<float> out(N+K+2);
20278 if(N*(N-1)/2 != nx) {
20279
20280 return out;}
20281
20282 for(int i=0; i<N; i++) assign(i) = float(i);
20283
20284 for(int i=0; i<N; i++) {
20285 int j = Util::get_irand(0,N-1);
20286 float temp = assign(i);
20287 assign(i) = assign(j);
20288 assign(j) = temp;
20289 }
20290 for(int k=0; k<K; k++) cent(k) = float(assign(k));
20291
20292
20293 for(int i=0; i<N; i++) assign(i) = 0.0f;
20294 float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
20295 bool change = true;
20296 int it = -1;
20297 int ct = -1;
20298 while ((change && disp < dispold) || ct > 0) {
20299
20300 change = false;
20301 dispold = disp;
20302 it++;
20303
20304
20305 disp = 0.0f;
20306 ct = 0;
20307 for(int i=0; i<N; i++) {
20308 qm = 1.0e23f;
20309 for(int k=0; k<K; k++) {
20310 if(float(i) == cent(k)) {
20311 qm = 0.0f;
20312 na = (float)k;
20313 } else {
20314 float dt = (*d)(mono(i,int(cent(k))));
20315 if(dt < qm) {
20316 qm = dt;
20317 na = (float)k;
20318 }
20319 }
20320 }
20321
20322
20323
20324 if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
20325 na = (float)(Util::get_irand(0, K));
20326 qm = (*d)(mono(i,int(na)));
20327 ct++;
20328 }
20329
20330 disp += qm;
20331
20332 if(na != assign(i)) {
20333 assign(i) = na;
20334 change = true;
20335 }
20336 }
20337
20338
20339 T = T*F;
20340
20341
20342
20343
20344
20345 for(int k=0; k<K; k++) {
20346 qm = 1.0e23f;
20347 for(int i=0; i<N; i++) {
20348 if(assign(i) == float(k)) {
20349 float q = 0.0;
20350 for(int j=0; j<N; j++) {
20351 if(assign(j) == float(k)) {
20352
20353 if(i != j) q += (*d)(mono(i,j));
20354
20355 }
20356 }
20357 if(q < qm) {
20358
20359 qm = q;
20360 cent(k) = float(i);
20361 }
20362 }
20363 }
20364 }
20365
20366 }
20367 out[N+K] = disp;
20368 out[N+K+1] = float(it);
20369 return out;
20370 }
20371 #undef cent
20372 #undef assign
20373
20374
20375
20376
20377
20378
20379
20380
20381
20382
20383
20384
20385
20386
20387
20388
20389
20390
20391
20392
20393
20394
20395
20396
20397
20398
20399
20400
20401
20402
20403
20404
20405
20406
20407
20408
20409
20410
20411
20412
20413
20414
20415
20416
20417
20418
20419
20420
20421
20422
20423
20424
20425
20426
20427
20428
20429
20430
20431
20432
20433
20434
20435
20436
20437
20438
20439
20440
20441
20442
20443
20444
20445
20446
20447
20448
20449
20450
20451
20452
20453
20454
20455
20456
20457
20458
20459
20460
20461
20462
20463
20464
20465
20466
20467
20468
20469
20470
20471
20472
20473
20474
20475
20476
20477
20478
20479
20480
20481
20482 vector<float> Util::cluster_equalsize(EMData* d) {
20483
20484 int nx = d->get_xsize();
20485 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20486 int K = N/2;
20487 vector<float> group(N);
20488 if(N*(N-1)/2 != nx) {
20489
20490 return group;}
20491
20492 bool * active = new bool[N];
20493 for(int i=0; i<N; i++) active[i] = true;
20494
20495 float dm, qd;
20496 int ppi = 0, ppj = 0;
20497 for(int k=0; k<K; k++) {
20498
20499
20500 dm = 1.0e23f;
20501 for(int i=1; i<N; i++) {
20502 if(active[i]) {
20503 for(int j=0; j<i; j++) {
20504 if(active[j]) {
20505 qd = (*d)(i*(i - 1)/2 + j);
20506 if(qd < dm) {
20507 dm = qd;
20508 ppi = i;
20509 ppj = j;
20510 }
20511 }
20512 }
20513 }
20514 }
20515 group[2*k] = float(ppi);
20516 group[1+2*k] = float(ppj);
20517 active[ppi] = false;
20518 active[ppj] = false;
20519 }
20520
20521 delete [] active;
20522 active = NULL;
20523 return group;
20524 }
20525
20526
20527
20528
20529
20530
20531
20532
20533
20534
20535
20536
20537
20538
20539
20540
20541
20542
20543
20544
20545
20546
20547
20548
20549
20550
20551
20552
20553
20554
20555
20556
20557
20558
20559
20560
20561
20562
20563
20564
20565
20566
20567
20568
20569
20570
20571
20572
20573
20574
20575
20576
20577
20578
20579 #define data(i,j) group[i*ny+j]
20580 vector<float> Util::vareas(EMData* d) {
20581 const float step=0.001f;
20582 int ny = d->get_ysize();
20583
20584
20585
20586 vector<float> group(2*ny);
20587 for(int i=0; i<2*ny; i++) group[i] = 0.0f;
20588 int K = int(1.0f/step) +1;
20589 int hit = 0;
20590 for(int kx=0; kx<=K; kx++) {
20591 float tx = kx*step;
20592 for(int ky=0; ky<=K; ky++) {
20593 float ty = ky*step;
20594 float dm = 1.0e23f;
20595 for(int i=0; i<ny; i++) {
20596 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
20597 if( qd < dm) {
20598 dm = qd;
20599 hit = i;
20600 }
20601 }
20602 data(0,hit) += 1.0f;
20603 if(kx == 0 || ky == 0 || kx == K || ky == K) data(1,hit) = 1.0f;
20604 }
20605 }
20606 return group;
20607 }
20608 #undef data
20609
20610 EMData* Util::get_slice(EMData *vol, int dim, int index) {
20611
20612 int nx = vol->get_xsize();
20613 int ny = vol->get_ysize();
20614 int nz = vol->get_zsize();
20615 float *vol_data = vol->get_data();
20616 int new_nx, new_ny;
20617
20618 if (nz == 1)
20619 throw ImageDimensionException("Error: Input must be a 3-D object");
20620 if ((dim < 1) || (dim > 3))
20621 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
20622 if (((dim == 1) && (index < 0 || index > nx-1)) ||
20623 ((dim == 1) && (index < 0 || index > nx-1)) ||
20624 ((dim == 1) && (index < 0 || index > nx-1)))
20625 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
20626
20627 if (dim == 1) {
20628 new_nx = ny;
20629 new_ny = nz;
20630 } else if (dim == 2) {
20631 new_nx = nx;
20632 new_ny = nz;
20633 } else {
20634 new_nx = nx;
20635 new_ny = ny;
20636 }
20637
20638 EMData *slice = new EMData();
20639 slice->set_size(new_nx, new_ny, 1);
20640 float *slice_data = slice->get_data();
20641
20642 if (dim == 1) {
20643 for (int x=0; x<new_nx; x++)
20644 for (int y=0; y<new_ny; y++)
20645 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
20646 } else if (dim == 2) {
20647 for (int x=0; x<new_nx; x++)
20648 for (int y=0; y<new_ny; y++)
20649 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
20650 } else {
20651 for (int x=0; x<new_nx; x++)
20652 for (int y=0; y<new_ny; y++)
20653 slice_data[y*new_nx+x] = vol_data[((size_t)index*ny+y)*nx+x];
20654 }
20655
20656 return slice;
20657 }
20658
20659 void Util::image_mutation(EMData *img, float mutation_rate) {
20660 int nx = img->get_xsize();
20661 float min = img->get_attr("minimum");
20662 float max = img->get_attr("maximum");
20663 float* img_data = img->get_data();
20664 array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
20665 return;
20666 }
20667
20668
20669 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20670
20671 if (is_mirror != 0) {
20672 for (int i=0; i<len_list; i++) {
20673 int r = rand()%10000;
20674 float f = r/10000.0f;
20675 if (f < mutation_rate) list[i] = 1-list[i];
20676 }
20677 } else {
20678 map<int, vector<int> > graycode;
20679 map<vector<int>, int> rev_graycode;
20680 vector <int> gray;
20681
20682 int K=1;
20683 for (int i=0; i<L; i++) K*=2;
20684
20685 for (int k=0; k<K; k++) {
20686 int shift = 0;
20687 vector <int> gray;
20688 for (int i=L-1; i>-1; i--) {
20689 int t = ((k>>i)%2-shift)%2;
20690 gray.push_back(t);
20691 shift += t-2;
20692 }
20693 graycode[k] = gray;
20694 rev_graycode[gray] = k;
20695 }
20696
20697 float gap = (K-1)/(max_val-min_val);
20698 for (int i=0; i<len_list; i++) {
20699 float val = list[i];
20700 if (val < min_val) { val = min_val; }
20701 else if (val > max_val) { val = max_val; }
20702 int k = int((val-min_val)*gap+0.5);
20703 vector<int> gray = graycode[k];
20704 bool changed = false;
20705 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20706 int r = rand()%10000;
20707 float f = r/10000.0f;
20708 if (f < mutation_rate) {
20709 *p = 1-*p;
20710 changed = true;
20711 }
20712 }
20713 if (changed) {
20714 k = rev_graycode[gray];
20715 list[i] = k/gap+min_val;
20716 }
20717 }
20718 }
20719
20720 }
20721
20722 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20723
20724 if (is_mirror != 0) {
20725 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20726 int r = rand()%10000;
20727 float f = r/10000.0f;
20728 if (f < mutation_rate) *q = 1-*q;
20729 }
20730 } else {
20731 map<int, vector<int> > graycode;
20732 map<vector<int>, int> rev_graycode;
20733 vector <int> gray;
20734
20735 int K=1;
20736 for (int i=0; i<L; i++) K*=2;
20737
20738 for (int k=0; k<K; k++) {
20739 int shift = 0;
20740 vector <int> gray;
20741 for (int i=L-1; i>-1; i--) {
20742 int t = ((k>>i)%2-shift)%2;
20743 gray.push_back(t);
20744 shift += t-2;
20745 }
20746 graycode[k] = gray;
20747 rev_graycode[gray] = k;
20748 }
20749
20750 float gap = (K-1)/(max_val-min_val);
20751 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20752 float val = *q;
20753 if (val < min_val) { val = min_val; }
20754 else if (val > max_val) { val = max_val; }
20755 int k = int((val-min_val)*gap+0.5);
20756 vector<int> gray = graycode[k];
20757 bool changed = false;
20758 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20759 int r = rand()%10000;
20760 float f = r/10000.0f;
20761 if (f < mutation_rate) {
20762 *p = 1-*p;
20763 changed = true;
20764 }
20765 }
20766 if (changed) {
20767 k = rev_graycode[gray];
20768 *q = k/gap+min_val;
20769 }
20770 }
20771 }
20772 return list;
20773 }
20774
20775
20776 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
20777
20778 int total_cost = *output;
20779 int num_matches = *(output+1);
20780
20781 int cost=0;
20782 int* intx;
20783 int intx_size;
20784 int* intx_next(0);
20785 int intx_next_size = 0;
20786 int curclass;
20787 int curclass_size;
20788
20789 for(int i = 0; i < num_matches; i++){
20790 curclass = *(output+2+ i*nParts);
20791
20792 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
20793 *(argParts + Indices[curclass]+1) = -5;
20794
20795 curclass_size = *(dimClasses+curclass)-2;
20796 intx = new int[curclass_size];
20797 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
20798 intx_size = curclass_size;
20799
20800 for (int j=1; j < nParts; j++){
20801 curclass = *(output+2+ i*nParts+j);
20802 if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
20803 *(argParts + Indices[j*K+curclass]+1)=-5;
20804
20805 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);
20806 intx_next = new int[intx_next_size];
20807 Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
20808 delete[] intx;
20809 intx=intx_next;
20810 intx_size= intx_next_size;
20811 }
20812 delete[] intx_next;
20813
20814 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
20815
20816 cost = cost + intx_next_size;
20817 }
20818
20819 if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
20820
20821 return 1;
20822
20823 }
20824
20825
20826
20827
20828
20829
20830 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int* costlist, int J){
20831
20832
20833 bool flag = 0;
20834 int nintx;
20835 int* dummy(0);
20836
20837 int* curbranch = new int[nParts];
20838
20839
20840 for(int jit= 0; jit< J; jit++) *(costlist+jit) = 0;
20841
20842
20843 for(int a=0; a<K; a++)
20844 {
20845
20846
20847 if (*(argParts + Indices[a] + 1) < 1) continue;
20848 if (*(dimClasses + a)-2 <= T) continue;
20849
20850
20851
20852 for( int i=1; i < nParts; i++){
20853 flag = 0;
20854 for(int j=0; j < K; j++){
20855 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
20856 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);
20857 if (nintx > T) flag=1;
20858 else *(argParts + Indices[i*K+j] + 1) =-4;
20859 }
20860 if (flag==0) {break;}
20861 }
20862
20863
20864 *curbranch = a;
20865
20866 if (flag > 0)
20867 Util::explore2(argParts, Indices, dimClasses, nParts, K, T, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2,
20868 *(dimClasses+a)-2,0, J, matchlist, costlist, curbranch);
20869
20870
20871 for( int i=1; i < nParts; i++){
20872 for(int j=0; j < K; j++){
20873 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
20874
20875 }
20876 }
20877 }
20878
20879 delete[] curbranch;
20880 }
20881
20882
20883 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){
20884
20885
20886 int* curintx2(0);
20887 int nintx = size_curintx;
20888
20889
20890
20891 if (depth >0){
20892 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
20893 if (nintx <= T) return;
20894 }
20895
20896
20897 if (depth == (nParts-1)) {
20898
20899 int replace = 0;
20900 int ind_smallest = -1;
20901 int smallest_cost = -1;
20902
20903 for (int jit = 0; jit < J; jit++){
20904 if (*(costlist+jit) < nintx){
20905 replace = 1;
20906 if (ind_smallest == -1) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20907 if (*(costlist+jit) < smallest_cost) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20908 }
20909 }
20910
20911 if (replace > 0){
20912
20913 *(costlist + ind_smallest) = nintx;
20914 for (int xit = 0; xit < nParts; xit++)
20915 *(matchlist + ind_smallest*nParts + xit) = *(curbranch+xit);
20916
20917 }
20918
20919 return;
20920 }
20921
20922
20923
20924
20925 if (depth > 0){
20926 curintx2 = new int[nintx];
20927 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
20928 }
20929
20930 if (depth == 0){
20931
20932 curintx2 = new int[size_curintx];
20933 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
20934 }
20935
20936
20937
20938 depth=depth+1;
20939
20940 for (int i=0; i < K; i++){
20941
20942 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue;
20943 size_next = (*(dimClasses + depth*K+i ))-2;
20944 if (size_next <= T) continue;
20945 *(curbranch+depth) = i;
20946 Util::explore2(argParts,Indices, dimClasses, nParts, K, T, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth,J, matchlist,
20947 costlist, curbranch);
20948
20949 }
20950
20951 delete[] curintx2;
20952 }
20953
20954 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
20955
20956
20957
20958
20959
20960
20961
20962
20963
20964
20965 int* dummy(0);
20966 int* cref;
20967 int cref_size;
20968 int* ccomp;
20969 int ccomp_size;
20970 int nintx;
20971 for (int i=0; i < nParts; i++){
20972 for (int j =0; j < K; j++){
20973
20974
20975 cref = Parts[i][j];
20976 cref_size = dimClasses[i*K+cref[0]]-2;
20977
20978
20979 if (cref_size <= T){
20980 cref[0] = -1;
20981 continue;
20982 }
20983 bool done = 0;
20984 for (int a = 0; a < nParts; a++){
20985 if (a == i) continue;
20986 bool hasActive=0;
20987 for (unsigned int b=0; b < Parts[a].size(); b++){
20988
20989
20990 ccomp = Parts[a][b];
20991 ccomp_size= dimClasses[a*K+ccomp[0]]-2;
20992 nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
20993
20994
20995 if (nintx <= T)
20996 ccomp[1] = 0;
20997 else{
20998 ccomp[1] = 1;
20999 hasActive=1;
21000 }
21001 }
21002
21003 if (hasActive < 1){
21004 done=1;
21005 break;
21006 }
21007
21008 }
21009
21010 if (done > 0){
21011
21012
21013 cref[0] = -1;
21014 continue;
21015 }
21016
21017
21018
21019
21020
21021
21022
21023
21024
21025
21026 bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
21027
21028 if (found<1){
21029
21030 cref[0] = -1;
21031 }
21032 }
21033
21034
21035
21036 for (int d = K-1; d > -1; d--){
21037 if (Parts[i][d][0] < 0) Parts[i].erase(Parts[i].begin()+d);
21038 }
21039
21040 }
21041
21042
21043
21044
21045
21046 }
21047
21048
21049 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) {
21050
21051
21052 if (size_next <= T) return 0;
21053
21054
21055 int* curintx2(0);
21056 int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
21057 if (nintx <= T) return 0;
21058
21059 int old_depth=depth;
21060 if (depth == partref) depth = depth + 1;
21061 if (depth == nParts && old_depth>0) return 1;
21062
21063
21064
21065 curintx2 = new int[nintx];
21066 Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
21067
21068
21069
21070
21071 bool gt_thresh;
21072 int num_classes = Parts[depth].size();
21073
21074 for (int i=0; i < num_classes; i++){
21075 if (Parts[depth][i][1] < 1) continue;
21076 size_next = dimClasses[depth*K + Parts[depth][i][0] ]-2;
21077 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
21078 if (gt_thresh) { delete[] curintx2; return 1; }
21079 }
21080 delete[] curintx2;
21081 return 0;
21082 }
21083
21084
21085
21086
21087 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int n_guesses, int LARGEST_CLASS, int J,
21088 int max_branching, float stmult, int branchfunc, int LIM) {
21089
21090
21091
21092
21093
21094 int* Indices = new int[nParts*K];
21095 int ind_c = 0;
21096 for (int i=0; i < nParts; i++){
21097 for(int j = 0; j < K; j++){
21098 Indices[i*K + j] = ind_c;
21099 ind_c = ind_c + dimClasses[i*K + j];
21100 }
21101 }
21102
21103
21104
21105
21106 vector <vector <int*> > Parts(nParts,vector<int*>(K));
21107 ind_c = 0;
21108 int argParts_size=0;
21109 for (int i=0; i < nParts; i++){
21110 for(int j = 0; j < K; j++){
21111 Parts[i][j] = argParts + ind_c;
21112 ind_c = ind_c + dimClasses[i*K + j];
21113 argParts_size = argParts_size + dimClasses[i*K + j];
21114 }
21115 }
21116
21117
21118
21119
21120
21121
21122 Util::initial_prune(Parts, dimClasses, nParts, K, T);
21123 for(int i = 0; i < nParts; i++){
21124 for(int j=0; j < K; j++){
21125 argParts[Indices[i*K + j]+1] = -1;
21126 }
21127 }
21128
21129 int num_classes;
21130 int old_index;
21131 for(int i=0; i<nParts; i++){
21132 num_classes = Parts[i].size();
21133 for (int j=0; j < num_classes; j++){
21134 old_index = Parts[i][j][0];
21135
21136 argParts[Indices[i*K + old_index]+1] = 1;
21137 }
21138 }
21139
21140
21141
21142
21143
21144 int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T, 0, n_guesses, LARGEST_CLASS, J, max_branching, stmult, branchfunc, LIM);
21145
21146
21147
21148
21149 bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
21150
21151 delete[] Indices;
21152
21153
21154 if (correct < 1){
21155 cout << "something is wrong with output of branchMPI!\n";
21156 vector<int> ret(1);
21157 ret[0] = -1;
21158 if (output != 0) { delete[] output; output = 0; }
21159 return ret;
21160 }
21161
21162
21163
21164
21165
21166 int output_size = 2 + output[1] * nParts;
21167 vector<int> ret(output_size);
21168 for (int i = 0; i < output_size; i++) {
21169 ret[i]= output[i];
21170 }
21171 if (output != 0) { delete[] output; output = 0; }
21172 return ret;
21173
21174 }
21175
21176
21177 int branch_all=0;
21178 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int curlevel,int n_guesses, int
21179 LARGEST_CLASS, int J, int max_branching, float stmult, int branchfunc, int LIM) {
21180
21181
21182
21183 if (1 == 0){
21184 cout <<"begin test search2\n";
21185 int* matchlist = new int[J*nParts];
21186 int* costlist = new int[J];
21187 for (int jit = 0; jit < nParts; jit++) *(costlist+jit) = 0;
21188 Util::search2(argParts,Indices, dimClasses, nParts, K, T, matchlist,costlist,J);
21189
21190 for (int jit = 0; jit < J; jit++) {
21191 cout << *(costlist +jit)<<": ";
21192 for (int yit = 0; yit < nParts; yit++)
21193 cout << *(matchlist + jit*nParts + yit)<<",";
21194 cout <<"\n";
21195
21196 }
21197 cout <<"end test search2\n";
21198 int* output = new int[1];
21199 output[0] = 1;
21200 delete [] matchlist;
21201 delete [] costlist;
21202 return output;
21203 }
21204
21205
21206
21207 if (curlevel > K -1){
21208 int* output = new int[2];
21209 output[0] = 0;
21210 output[1] = 0;
21211 return output;
21212 }
21213
21214
21215
21216 int* matchlist = new int[J*nParts];
21217 int* costlist = new int[J];
21218 Util::search2(argParts, Indices, dimClasses, nParts, K, T, matchlist, costlist, J);
21219
21220
21221
21222
21223
21224
21225 for (int jit = 0; jit < J ; jit++){
21226
21227 if (costlist[jit] > T) break;
21228 if (jit == J-1){
21229 int* output = new int[2];
21230 output[0] = 0;
21231 output[1] = 0;
21232 delete[] matchlist;
21233 delete[] costlist;
21234 return output;
21235 }
21236 }
21237
21238
21239
21240
21241 if (curlevel==0) branch_all = 0;
21242
21243 int nBranches = -1;
21244
21245 if (branchfunc == 0)
21246 nBranches = branch_factor_0(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM);
21247
21248 if (branchfunc == 2)
21249 nBranches = branch_factor_2(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM);
21250
21251 if (branchfunc == 3)
21252 nBranches = branch_factor_3(costlist,matchlist,J, T, nParts, curlevel, max_branching, K, LIM);
21253
21254 if (branchfunc == 4)
21255 nBranches = branch_factor_4(costlist,matchlist,J, T, nParts, curlevel, max_branching, stmult);
21256
21257 int* newcostlist= new int[nBranches];
21258 int* newmatchlist = new int[nBranches*nParts];
21259 for (int i=0; i<nBranches; i++){
21260 newcostlist[i] = costlist[i];
21261 for (int j=0; j< nParts; j++)
21262 newmatchlist[i*nParts + j] = matchlist[i*nParts + j];
21263 }
21264
21265 delete[] costlist;
21266 delete[] matchlist;
21267
21268
21269 int* output = new int[2+K*nParts];
21270 output[0] = 0;
21271 output[1] = 0;
21272
21273 int old_index;
21274 int totalcost;
21275 int nmatches;
21276
21277
21278 for(int i=0; i < nBranches ; i++){
21279
21280
21281
21282
21283
21284
21285
21286 for(int j=0; j < nParts; j++){
21287
21288 old_index = newmatchlist[i*nParts + j];
21289 argParts[Indices[j*K+old_index] + 1] = -2;
21290 }
21291
21292
21293 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, curlevel+1, n_guesses, LARGEST_CLASS,
21294 J, max_branching, stmult,branchfunc, LIM);
21295
21296
21297 totalcost = newcostlist[i] + ret[0];
21298
21299
21300
21301
21302
21303 if (totalcost > output[0])
21304 {
21305 nmatches = 1 + ret[1];
21306
21307
21308 output[0] = totalcost;
21309 output[1] = nmatches;
21310 int nret = 2+(nmatches-1)*nParts;
21311 for(int iret=2; iret < nret; iret++) output[iret] = ret[iret];
21312 for(int imax=0; imax < nParts; imax++) output[nret+imax] = newmatchlist[i*nParts + imax];
21313 }
21314
21315
21316 delete[] ret;
21317
21318
21319
21320 for(int j=0; j < nParts; j++){
21321 old_index = newmatchlist[i*nParts + j];
21322 argParts[Indices[j*K+old_index] + 1] = 1;
21323 }
21324
21325 }
21326
21327 delete[] newmatchlist;
21328 delete[] newcostlist;
21329
21330 return output;
21331 }
21332
21333 int* costlist_global;
21334
21335 bool jiafunc(int i, int j){
21336 return (costlist_global[j] < costlist_global[i]) ;
21337
21338 }
21339
21340
21341
21342
21343
21344 int Util::branch_factor_2(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21345
21346 int ntot=0;
21347 for (int jit=0; jit < J; jit++){
21348 if (*(costlist+jit) > T) ntot++;
21349 }
21350
21351 int cur;
21352
21353 int* indx = new int[J];
21354 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21355 vector<int> myindx (indx, indx+J);
21356 vector<int>::iterator it;
21357 costlist_global=costlist;
21358 sort(myindx.begin(), myindx.end(), jiafunc);
21359
21360
21361 int* templist = new int[J];
21362 int* temp2list = new int[J*nParts];
21363 int next = 0;
21364
21365 for (it=myindx.begin(); it!=myindx.end();++it){
21366 cur = *(costlist + *it);
21367 if (cur > T){
21368
21369 templist[next] = cur;
21370 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21371 next = next + 1;
21372 }
21373 }
21374
21375 for (int jit=0; jit < ntot; jit++){
21376 *(costlist+jit)=*(templist + jit);
21377
21378 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21379 }
21380
21381
21382 delete [] indx;
21383
21384
21385
21386
21387
21388
21389 int B=1;
21390 int B_init=B;
21391 int infeasible=0;
21392
21393 for (int i=B_init; i<ntot; i++){
21394 if (i==ntot) continue;
21395
21396
21397 infeasible = 0;
21398 if (LIM < 0) LIM = B;
21399 for (int j=0; j<B; j++){
21400
21401 for (int vit=0; vit<nParts; vit++){
21402 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21403 }
21404 if (infeasible >= LIM) break;
21405 }
21406
21407 if (infeasible >= LIM){
21408 *(costlist+B)=*(templist+i);
21409 for (int vit=0; vit < nParts; vit++)
21410 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21411 B=B+1;
21412 }
21413 }
21414
21415 delete [] templist;
21416 delete [] temp2list;
21417
21418
21419 if (branch_all < max_branching){
21420 if (B>1)
21421 {branch_all = branch_all + B -1 ; }
21422 }
21423 else B=1;
21424
21425 return B;
21426
21427
21428 }
21429
21430
21431
21432 int Util::branch_factor_3(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int, int LIM){
21433
21434 int ntot=0;
21435 for (int jit=0; jit < J; jit++){
21436 if (*(costlist+jit) > T) ntot++;
21437 }
21438
21439 int cur;
21440
21441 int* indx = new int[J];
21442 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21443 vector<int> myindx (indx, indx+J);
21444 vector<int>::iterator it;
21445 costlist_global=costlist;
21446 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21447
21448
21449 int* templist = new int[J];
21450 int* temp2list = new int[J*nParts];
21451 int next = 0;
21452
21453 for (it=myindx.begin(); it!=myindx.end();++it){
21454 cur = *(costlist + *it);
21455 if (cur > T){
21456
21457 templist[next] = cur;
21458 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21459 next = next + 1;
21460 }
21461 }
21462
21463 for (int jit=0; jit < ntot; jit++){
21464 *(costlist+jit)=*(templist + jit);
21465
21466 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21467 }
21468
21469
21470 delete [] indx;
21471
21472
21473
21474
21475
21476
21477 int B=1;
21478 int B_init=B;
21479 int infeasible=0;
21480
21481
21482 if (LIM < 0) LIM = ntot-1;
21483 for (int i=B_init; i<ntot; i++){
21484 if (i==ntot) continue;
21485
21486
21487 infeasible = 0;
21488
21489 for (int j=0; j<ntot; j++){
21490 if (j == i) continue;
21491 for (int vit=0; vit<nParts; vit++){
21492 if (temp2list[i*nParts+vit] == temp2list[j*nParts+vit]) {infeasible++; break;}
21493 }
21494 if (infeasible >= LIM) break;
21495 }
21496
21497 if (infeasible >= LIM){
21498 *(costlist+B)=*(templist+i);
21499 for (int vit=0; vit < nParts; vit++)
21500 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21501 B=B+1;
21502 }
21503 }
21504
21505 delete [] templist;
21506 delete [] temp2list;
21507
21508
21509
21510 if (branch_all < max_branching){
21511 if (B>1)
21512 {branch_all = branch_all + B-1;}
21513 }
21514 else B=1;
21515
21516 return B;
21517
21518
21519 }
21520
21521
21522
21523
21524
21525 int Util::branch_factor_4(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, float stmult){
21526 int sum=0;
21527 float average =0;
21528 int ntot=0;
21529 for (int jit=0; jit < J; jit++){
21530 if (*(costlist+jit) > T) {ntot++; sum = sum +*(costlist+jit);}
21531 }
21532 average = ((float)sum)/((float)ntot);
21533 int cur;
21534
21535 int* indx = new int[J];
21536 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21537 vector<int> myindx (indx, indx+J);
21538 vector<int>::iterator it;
21539 costlist_global=costlist;
21540 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21541
21542
21543 int* templist = new int[J];
21544 int* temp2list = new int[J*nParts];
21545 int next = 0;
21546
21547 for (it=myindx.begin(); it!=myindx.end();++it){
21548 cur = *(costlist + *it);
21549 if (cur > T){
21550
21551 templist[next] = cur;
21552 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21553 next = next + 1;
21554 }
21555 }
21556
21557 for (int jit=0; jit < ntot; jit++){
21558 *(costlist+jit)=*(templist + jit);
21559
21560 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21561 }
21562
21563
21564 delete [] indx;
21565 delete [] templist;
21566 delete [] temp2list;
21567
21568 if (ntot == 1) return 1;
21569
21570
21571
21572 float sq_sum=0.0;
21573
21574 for (int i=0; i< ntot; i++){
21575 sq_sum = sq_sum + (float) pow((float) *(costlist+i) - average, (float)2.0);
21576
21577 }
21578
21579
21580 float variance = sq_sum/ntot;
21581 float stdev = (float)pow((float)variance,(float)0.5);
21582
21583
21584
21585 int B=1;
21586 int largest = *costlist;
21587
21588 for (int i=1; i<ntot; i++){
21589 int cur = *(costlist+i);
21590 if (largest-cur < (float)(stdev*stmult)) B++;
21591 else break;
21592
21593 }
21594
21595 if (branch_all < max_branching){
21596 if (B>1)
21597 {branch_all = branch_all + B-1;}
21598 }
21599 else B=1;
21600
21601 return B;
21602
21603
21604 }
21605
21606 int Util::branch_factor_0(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21607
21608 int ntot=0;
21609 for (int jit=0; jit < J; jit++){
21610 if (*(costlist+jit) > T) ntot++;
21611 }
21612
21613 int cur;
21614
21615 int* indx = new int[J];
21616 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21617 vector<int> myindx (indx, indx+J);
21618 vector<int>::iterator it;
21619 costlist_global=costlist;
21620 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21621
21622
21623 int* templist = new int[J];
21624 int* temp2list = new int[J*nParts];
21625 int next = 0;
21626
21627 for (it=myindx.begin(); it!=myindx.end();++it){
21628 cur = *(costlist + *it);
21629 if (cur > T){
21630
21631 templist[next] = cur;
21632 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21633 next = next + 1;
21634 }
21635 }
21636
21637 for (int jit=0; jit < ntot; jit++){
21638 *(costlist+jit)=*(templist + jit);
21639
21640 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21641 }
21642
21643
21644 for (int jit=1; jit < ntot; jit++){
21645
21646 if ((costlist[jit] == costlist[0]) && costlist[jit] > T){
21647
21648 for (int vit=0; vit < nParts; vit++){
21649 if ( matchlist[jit*nParts + vit] > matchlist[vit])
21650 break;
21651 if ( matchlist[jit*nParts + vit] == matchlist[vit])
21652 continue;
21653 if ( matchlist[jit*nParts + vit] < matchlist[vit])
21654 {
21655
21656 for (int swp=0; swp < nParts; swp++){
21657 int tmp = matchlist[swp];
21658 matchlist[swp]= matchlist[jit*nParts + swp];
21659 matchlist[jit*nParts + swp] = tmp;
21660 }
21661 break;
21662
21663 }
21664 }
21665 }
21666
21667 }
21668
21669
21670 delete [] indx;
21671
21672
21673
21674
21675
21676
21677 int B=1;
21678 int B_init=B;
21679 int infeasible=0;
21680
21681 for (int i=B_init; i<ntot; i++){
21682 if (i==ntot) continue;
21683
21684
21685 infeasible = 0;
21686 if (LIM < 0) LIM = B;
21687 for (int j=0; j<B; j++){
21688
21689 for (int vit=0; vit<nParts; vit++){
21690 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21691 }
21692 if (infeasible >= LIM) break;
21693 }
21694
21695 if (infeasible >= LIM){
21696 *(costlist+B)=*(templist+i);
21697 for (int vit=0; vit < nParts; vit++)
21698 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21699 B=B+1;
21700 }
21701 }
21702
21703 delete [] templist;
21704 delete [] temp2list;
21705
21706
21707 if (branch_all < max_branching){
21708 if (B>1)
21709 {branch_all = branch_all + B -1 ; }
21710 }
21711 else B=1;
21712
21713 return B;
21714
21715
21716 }