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 #include <malloc.h>
00039 #endif //_WIN32
00040
00041 #include <cstring>
00042 #include <ctime>
00043 #include <iostream>
00044 #include <cstdio>
00045 #include <cstdlib>
00046 #include <cassert>
00047 #include <boost/format.hpp>
00048 #include "emdata.h"
00049 #include "util.h"
00050 #include "fundamentals.h"
00051 #include "lapackblas.h"
00052 #include "lbfgsb.h"
00053 using namespace EMAN;
00054 #include "steepest.h"
00055 #include "emassert.h"
00056 #include "randnum.h"
00057
00058 #include <gsl/gsl_sf_bessel.h>
00059 #include <gsl/gsl_sf_bessel.h>
00060 #include <cmath>
00061 using namespace std;
00062 using std::complex;
00063
00064
00065 int circum_(double *, double *, double *, double *, int *);
00066 long int left_(double *, double *, double *, double *, double *, double *, double *, double *, double *);
00067 int addnod_(int *, int *, double *, double *, double *, int *, int *, int *, int *, int *);
00068
00069 vector<float> Util::infomask(EMData* Vol, EMData* mask, bool flip = false)
00070
00071
00072 {
00073 ENTERFUNC;
00074 vector<float> stats;
00075 float *Volptr, *maskptr,MAX,MIN;
00076 long double Sum1,Sum2;
00077 long count;
00078
00079 MAX = -FLT_MAX;
00080 MIN = FLT_MAX;
00081 count = 0L;
00082 Sum1 = 0.0L;
00083 Sum2 = 0.0L;
00084
00085 if (mask == NULL) {
00086
00087 stats.push_back(Vol->get_attr("mean"));
00088 stats.push_back(Vol->get_attr("sigma"));
00089 stats.push_back(Vol->get_attr("minimum"));
00090 stats.push_back(Vol->get_attr("maximum"));
00091 return stats;
00092 }
00093
00094
00095
00096 size_t nx = Vol->get_xsize();
00097 size_t ny = Vol->get_ysize();
00098 size_t nz = Vol->get_zsize();
00099
00100 size_t mask_nx = mask->get_xsize();
00101 size_t mask_ny = mask->get_ysize();
00102 size_t mask_nz = mask->get_zsize();
00103
00104 if (nx != mask_nx || ny != mask_ny || nz != mask_nz )
00105 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118 Volptr = Vol->get_data();
00119 maskptr = mask->get_data();
00120
00121 for (size_t i = 0; i < (size_t)nx*ny*nz; ++i) {
00122 if ((maskptr[i]>0.5f) == flip) {
00123 Sum1 += Volptr[i];
00124 Sum2 += Volptr[i]*double(Volptr[i]);
00125 MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00126 MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00127 count++;
00128 }
00129 }
00130
00131 if (count == 0) {
00132 LOGERR("Invalid mask");
00133 throw ImageFormatException( "Invalid mask");
00134 }
00135
00136 float avg = static_cast<float>(Sum1/count);
00137 float sig = static_cast<float>(sqrt((Sum2 - Sum1*Sum1/count)/(count-1)));
00138
00139 stats.push_back(avg);
00140 stats.push_back(sig);
00141 stats.push_back(MIN);
00142 stats.push_back(MAX);
00143
00144 return stats;
00145 }
00146
00147
00148
00149
00150 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00151 {
00152 ENTERFUNC;
00153
00154 if (!EMUtil::is_same_size(V1, V2)) {
00155 LOGERR("images not same size");
00156 throw ImageFormatException( "images not same size");
00157 }
00158
00159 size_t nx = V1->get_xsize();
00160 size_t ny = V1->get_ysize();
00161 size_t nz = V1->get_zsize();
00162 size_t size = (size_t)nx*ny*nz;
00163
00164 EMData *BD = new EMData();
00165 BD->set_size(nx, ny, nz);
00166
00167 float *params = new float[2];
00168
00169 float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00170 long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00171 int nvox = 0L;
00172
00173 V1ptr = V1->get_data();
00174 V2ptr = V2->get_data();
00175 BDptr = BD->get_data();
00176
00177
00178 if(!mask){
00179 EMData * Mask = new EMData();
00180 Mask->set_size(nx,ny,nz);
00181 Mask->to_one();
00182 MASKptr = Mask->get_data();
00183 } else {
00184 if (!EMUtil::is_same_size(V1, mask)) {
00185 LOGERR("mask not same size");
00186 throw ImageFormatException( "mask not same size");
00187 }
00188
00189 MASKptr = mask->get_data();
00190 }
00191
00192
00193
00194
00195
00196 for (size_t i = 0L;i < size; i++) {
00197 if (MASKptr[i]>0.5f) {
00198 S1 += V1ptr[i]*V2ptr[i];
00199 S2 += V1ptr[i]*V1ptr[i];
00200 S3 += V2ptr[i];
00201 S4 += V1ptr[i];
00202 nvox ++;
00203 }
00204 }
00205
00206 if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00207 A =1.0f ;
00208 } else {
00209 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00210 }
00211 B = static_cast<float> (A*S4 - S3)/nvox;
00212
00213
00214
00215 for (size_t i = 0L;i < size; i++) {
00216 if (MASKptr[i]>0.5f) {
00217 BDptr[i] = A*V1ptr[i] - B - V2ptr[i];
00218 } else {
00219 BDptr[i] = 0.f;
00220 }
00221 }
00222
00223 BD->update();
00224
00225 params[0] = A;
00226 params[1] = B;
00227
00228 Dict BDnParams;
00229 BDnParams["imdiff"] = BD;
00230 BDnParams["A"] = params[0];
00231 BDnParams["B"] = params[1];
00232
00233 EXITFUNC;
00234 return BDnParams;
00235 }
00236
00237
00238
00239
00240
00241 EMData *Util::TwoDTestFunc(int Size, float p, float q, float a, float b, int flag, float alphaDeg)
00242 {
00243 ENTERFUNC;
00244 int Mid= (Size+1)/2;
00245
00246 if (flag==0) {
00247 EMData* ImBW = new EMData();
00248 ImBW->set_size(Size,Size,1);
00249 ImBW->to_zero();
00250
00251 float tempIm;
00252 float x,y;
00253
00254 for (int ix=(1-Mid); ix<Mid; ix++){
00255 for (int iy=(1-Mid); iy<Mid; iy++){
00256 x = (float)ix;
00257 y = (float)iy;
00258 tempIm= static_cast<float>( (1/(2*M_PI)) * cos(p*x)* cos(q*y) * exp(-.5*x*x/(a*a))* exp(-.5*y*y/(b*b)) );
00259 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00260 }
00261 }
00262 ImBW->update();
00263 ImBW->set_complex(false);
00264 ImBW->set_ri(true);
00265
00266 return ImBW;
00267 }
00268 else if (flag==1) {
00269 EMData* ImBWFFT = new EMData();
00270 ImBWFFT ->set_size(2*Size,Size,1);
00271 ImBWFFT ->to_zero();
00272
00273 float r,s;
00274
00275 for (int ir=(1-Mid); ir<Mid; ir++){
00276 for (int is=(1-Mid); is<Mid; is++){
00277 r = (float)ir;
00278 s = (float)is;
00279 (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00280 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00281 }
00282 }
00283 ImBWFFT->update();
00284 ImBWFFT->set_complex(true);
00285 ImBWFFT->set_ri(true);
00286 ImBWFFT->set_shuffled(true);
00287 ImBWFFT->set_fftodd(true);
00288
00289 return ImBWFFT;
00290 }
00291 else if (flag==2 || flag==3) {
00292 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00293 float C=cos(alpha);
00294 float S=sin(alpha);
00295 float D= sqrt(S*S*b*b + C*C*a*a);
00296
00297
00298 float P = p * C *a*a/D ;
00299 float Q = q * S *b*b/D ;
00300
00301 if (flag==2) {
00302 EMData* pofalpha = new EMData();
00303 pofalpha ->set_size(Size,1,1);
00304 pofalpha ->to_zero();
00305
00306 float Norm0 = D*(float)sqrt(2*pi);
00307 float Norm1 = exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00308 float Norm2 = exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00309 float sD;
00310
00311 for (int is=(1-Mid); is<Mid; is++){
00312 sD = is/D ;
00313 (*pofalpha)(is+Mid-1) = Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00314 + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00315 }
00316 pofalpha-> update();
00317 pofalpha-> set_complex(false);
00318 pofalpha-> set_ri(true);
00319
00320 return pofalpha;
00321 }
00322 if (flag==3) {
00323 float vD;
00324
00325 EMData* pofalphak = new EMData();
00326 pofalphak ->set_size(2*Size,1,1);
00327 pofalphak ->to_zero();
00328
00329 for (int iv=(1-Mid); iv<Mid; iv++){
00330 vD = iv*D ;
00331 (*pofalphak)(2*(iv+Mid-1)) = exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00332 }
00333 pofalphak-> update();
00334 pofalphak-> set_complex(false);
00335 pofalphak-> set_ri(true);
00336
00337 return pofalphak;
00338 }
00339 }
00340 else if (flag==4) {
00341 cout <<" FH under construction";
00342 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00343 EMData* TryFH= OutFT -> real2FH(4.0);
00344 return TryFH;
00345 } else {
00346 cout <<" flag must be 0,1,2,3, or 4";
00347 }
00348
00349 EXITFUNC;
00350 return 0;
00351 }
00352
00353
00354 void Util::spline_mat(float *x, float *y, int n, float *xq, float *yq, int m)
00355 {
00356
00357 float x0= x[0];
00358 float x1= x[1];
00359 float x2= x[2];
00360 float y0= y[0];
00361 float y1= y[1];
00362 float y2= y[2];
00363 float yp1 = (y1-y0)/(x1-x0) + (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1) ;
00364 float xn = x[n];
00365 float xnm1= x[n-1];
00366 float xnm2= x[n-2];
00367 float yn = y[n];
00368 float ynm1= y[n-1];
00369 float ynm2= y[n-2];
00370 float ypn= (yn-ynm1)/(xn-xnm1) + (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00371 float *y2d = new float[n];
00372 Util::spline(x,y,n,yp1,ypn,y2d);
00373 Util::splint(x,y,y2d,n,xq,yq,m);
00374 delete [] y2d;
00375 return;
00376 }
00377
00378
00379 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2)
00380 {
00381 int i,k;
00382 float p, qn, sig, un, *u;
00383 u = new float[n-1];
00384
00385 if (yp1 > .99e30){
00386 y2[0]=u[0]=0.0;
00387 } else {
00388 y2[0]=-.5f;
00389 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00390 }
00391
00392 for (i=1; i < n-1; i++) {
00393 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00394 p = sig*y2[i-1] + 2.0f;
00395 y2[i] = (sig-1.0f)/p;
00396 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) - (y[i] - y[i-1] )/(x[i] -x[i-1]);
00397 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00398 }
00399
00400 if (ypn>.99e30){
00401 qn=0; un=0;
00402 } else {
00403 qn= .5f;
00404 un= (3.0f/(x[n-1] -x[n-2])) * (ypn - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00405 }
00406 y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00407 for (k=n-2; k>=0; k--){
00408 y2[k]=y2[k]*y2[k+1]+u[k];
00409 }
00410 delete [] u;
00411 }
00412
00413
00414 void Util::splint( float *xa, float *ya, float *y2a, int n, float *xq, float *yq, int m)
00415 {
00416 int klo, khi, k;
00417 float h, b, a;
00418
00419
00420 for (int j=0; j<m;j++){
00421 klo=0;
00422 khi=n-1;
00423 while (khi-klo >1) {
00424 k=(khi+klo) >>1;
00425 if (xa[k]>xq[j]){ khi=k;}
00426 else { klo=k;}
00427 }
00428 h=xa[khi]- xa[klo];
00429 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00430 a =(xa[khi]-xq[j])/h;
00431 b=(xq[j]-xa[klo])/h;
00432 yq[j]=a*ya[klo] + b*ya[khi]
00433 + ((a*a*a-a)*y2a[klo]
00434 +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00435 }
00436
00437 }
00438
00439
00440 void Util::Radialize(int *PermMatTr, float *kValsSorted,
00441 float *weightofkValsSorted, int Size, int *SizeReturned)
00442 {
00443 int iMax = (int) floor( (Size-1.0)/2 +.01);
00444 int CountMax = (iMax+2)*(iMax+1)/2;
00445 int Count=-1;
00446 float *kVals = new float[CountMax];
00447 float *weightMat = new float[CountMax];
00448 int *PermMat = new int[CountMax];
00449 SizeReturned[0] = CountMax;
00450
00451
00452 for (int jkx=0; jkx< iMax+1; jkx++) {
00453 for (int jky=0; jky< jkx+1; jky++) {
00454 Count++;
00455 kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00456 weightMat[Count]= 1.0;
00457 if (jkx!=0) { weightMat[Count] *=2;}
00458 if (jky!=0) { weightMat[Count] *=2;}
00459 if (jkx!=jky){ weightMat[Count] *=2;}
00460 PermMat[Count]=Count+1;
00461 }
00462 }
00463
00464 int lkVals = Count+1;
00465
00466
00467 sort_mat(&kVals[0],&kVals[Count],
00468 &PermMat[0], &PermMat[Count]);
00469
00470 fflush(stdout);
00471
00472 int newInd;
00473
00474 for (int iP=0; iP < lkVals ; iP++ ) {
00475 newInd = PermMat[iP];
00476 PermMatTr[newInd-1] = iP+1;
00477 }
00478
00479
00480
00481 int CountA=-1;
00482 int CountB=-1;
00483
00484 while (CountB< (CountMax-1)) {
00485 CountA++;
00486 CountB++;
00487
00488 kValsSorted[CountA] = kVals[CountB] ;
00489 if (CountB<(CountMax-1) ) {
00490 while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001 ) {
00491 SizeReturned[0]--;
00492 for (int iP=0; iP < lkVals; iP++){
00493
00494 if (PermMatTr[iP]>CountA+1) {
00495 PermMatTr[iP]--;
00496 }
00497 }
00498 CountB++;
00499 }
00500 }
00501 }
00502
00503
00504 for (int CountD=0; CountD < CountMax; CountD++) {
00505 newInd = PermMatTr[CountD];
00506 weightofkValsSorted[newInd-1] += weightMat[CountD];
00507 }
00508
00509 }
00510
00511
00512 vector<float>
00513 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00514 {
00515 vector<float> angles;
00516 float psi = 0.0;
00517 if ((0.0 == t1 && 0.0 == t2)||(t1 >= t2)) {
00518 t1 = 0.0f;
00519 t2 = 90.0f;
00520 }
00521 if ((0.0 == p1 && 0.0 == p2)||(p1 >= p2)) {
00522 p1 = 0.0f;
00523 p2 = 359.9f;
00524 }
00525 bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00526 for (float theta = t1; theta <= t2; theta += delta) {
00527 float detphi;
00528 int lt;
00529 if ((0.0 == theta)||(180.0 == theta)) {
00530 detphi = 360.0f;
00531 lt = 1;
00532 } else {
00533 detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00534 lt = int((p2 - p1)/detphi)-1;
00535 if (lt < 1) lt = 1;
00536 detphi = (p2 - p1)/lt;
00537 }
00538 for (int i = 0; i < lt; i++) {
00539 float phi = p1 + i*detphi;
00540 if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00541 angles.push_back(phi);
00542 angles.push_back(theta);
00543 angles.push_back(psi);
00544 }
00545 }
00546 return angles;
00547 }
00548
00549
00550 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00653 {
00654
00655
00656 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00657 float quadri;
00658 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00659
00660 x = xx;
00661 y = yy;
00662
00663
00664 while ( x < 1.0 ) x += nxdata;
00665 while ( x >= (float)(nxdata+1) ) x -= nxdata;
00666 while ( y < 1.0 ) y += nydata;
00667 while ( y >= (float)(nydata+1) ) y -= nydata;
00668
00669 i = (int) x;
00670 j = (int) y;
00671
00672 dx0 = x - i;
00673 dy0 = y - j;
00674
00675 ip1 = i + 1;
00676 im1 = i - 1;
00677 jp1 = j + 1;
00678 jm1 = j - 1;
00679
00680 if (ip1 > nxdata) ip1 -= nxdata;
00681 if (im1 < 1) im1 += nxdata;
00682 if (jp1 > nydata) jp1 -= nydata;
00683 if (jm1 < 1) jm1 += nydata;
00684
00685 f0 = fdata(i,j);
00686 c1 = fdata(ip1,j) - f0;
00687 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00688 c3 = fdata(i,jp1) - f0;
00689 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00690
00691 dxb = dx0 - 1;
00692 dyb = dy0 - 1;
00693
00694
00695 if (dx0 >= 0) hxc = 1; else hxc = -1;
00696 if (dy0 >= 0) hyc = 1; else hyc = -1;
00697
00698 ic = i + hxc;
00699 jc = j + hyc;
00700
00701 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00702 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00703
00704 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00705 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00706
00707
00708 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00709
00710 return quadri;
00711 }
00712
00713 #undef fdata
00714
00715 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00716 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00717 {
00718
00719
00720 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00721 float quadri;
00722 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00723
00724 x = xx;
00725 y = yy;
00726
00727
00728 if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00729 x = (float)xnew;
00730 y = (float)ynew;
00731 }
00732
00733
00734 i = (int) x;
00735 j = (int) y;
00736
00737 dx0 = x - i;
00738 dy0 = y - j;
00739
00740 ip1 = i + 1;
00741 im1 = i - 1;
00742 jp1 = j + 1;
00743 jm1 = j - 1;
00744
00745 if (ip1 > nxdata) ip1 -= nxdata;
00746 if (im1 < 1) im1 += nxdata;
00747 if (jp1 > nydata) jp1 -= nydata;
00748 if (jm1 < 1) jm1 += nydata;
00749
00750 f0 = fdata(i,j);
00751 c1 = fdata(ip1,j) - f0;
00752 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00753 c3 = fdata(i,jp1) - f0;
00754 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00755
00756 dxb = dx0 - 1;
00757 dyb = dy0 - 1;
00758
00759
00760 if (dx0 >= 0) hxc = 1; else hxc = -1;
00761 if (dy0 >= 0) hyc = 1; else hyc = -1;
00762
00763 ic = i + hxc;
00764 jc = j + hyc;
00765
00766 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00767 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00768
00769 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00770 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00771
00772
00773 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00774
00775 return quadri;
00776 }
00777
00778 #undef fdata
00779
00780
00781 float Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00782 int K = kb.get_window_size();
00783 int kbmin = -K/2;
00784 int kbmax = -kbmin;
00785 int kbc = kbmax+1;
00786
00787 float pixel =0.0f;
00788 float w=0.0f;
00789
00790 delx = restrict1(delx, nx);
00791 int inxold = int(round(delx));
00792 if ( ny < 2 ) {
00793 float tablex1 = kb.i0win_tab(delx-inxold+3);
00794 float tablex2 = kb.i0win_tab(delx-inxold+2);
00795 float tablex3 = kb.i0win_tab(delx-inxold+1);
00796 float tablex4 = kb.i0win_tab(delx-inxold);
00797 float tablex5 = kb.i0win_tab(delx-inxold-1);
00798 float tablex6 = kb.i0win_tab(delx-inxold-2);
00799 float tablex7 = kb.i0win_tab(delx-inxold-3);
00800
00801 int x1, x2, x3, x4, x5, x6, x7;
00802
00803 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
00804 x1 = (inxold-3+nx)%nx;
00805 x2 = (inxold-2+nx)%nx;
00806 x3 = (inxold-1+nx)%nx;
00807 x4 = (inxold +nx)%nx;
00808 x5 = (inxold+1+nx)%nx;
00809 x6 = (inxold+2+nx)%nx;
00810 x7 = (inxold+3+nx)%nx;
00811 } else {
00812 x1 = inxold-3;
00813 x2 = inxold-2;
00814 x3 = inxold-1;
00815 x4 = inxold;
00816 x5 = inxold+1;
00817 x6 = inxold+2;
00818 x7 = inxold+3;
00819 }
00820
00821 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00822 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00823 data[x7]*tablex7 ;
00824
00825 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00826 } else if ( nz < 2 ) {
00827 dely = restrict1(dely, ny);
00828 int inyold = int(round(dely));
00829 float tablex1 = kb.i0win_tab(delx-inxold+3);
00830 float tablex2 = kb.i0win_tab(delx-inxold+2);
00831 float tablex3 = kb.i0win_tab(delx-inxold+1);
00832 float tablex4 = kb.i0win_tab(delx-inxold);
00833 float tablex5 = kb.i0win_tab(delx-inxold-1);
00834 float tablex6 = kb.i0win_tab(delx-inxold-2);
00835 float tablex7 = kb.i0win_tab(delx-inxold-3);
00836
00837 float tabley1 = kb.i0win_tab(dely-inyold+3);
00838 float tabley2 = kb.i0win_tab(dely-inyold+2);
00839 float tabley3 = kb.i0win_tab(dely-inyold+1);
00840 float tabley4 = kb.i0win_tab(dely-inyold);
00841 float tabley5 = kb.i0win_tab(dely-inyold-1);
00842 float tabley6 = kb.i0win_tab(dely-inyold-2);
00843 float tabley7 = kb.i0win_tab(dely-inyold-3);
00844
00845 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00846
00847 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
00848 x1 = (inxold-3+nx)%nx;
00849 x2 = (inxold-2+nx)%nx;
00850 x3 = (inxold-1+nx)%nx;
00851 x4 = (inxold +nx)%nx;
00852 x5 = (inxold+1+nx)%nx;
00853 x6 = (inxold+2+nx)%nx;
00854 x7 = (inxold+3+nx)%nx;
00855
00856 y1 = ((inyold-3+ny)%ny)*nx;
00857 y2 = ((inyold-2+ny)%ny)*nx;
00858 y3 = ((inyold-1+ny)%ny)*nx;
00859 y4 = ((inyold +ny)%ny)*nx;
00860 y5 = ((inyold+1+ny)%ny)*nx;
00861 y6 = ((inyold+2+ny)%ny)*nx;
00862 y7 = ((inyold+3+ny)%ny)*nx;
00863 } else {
00864 x1 = inxold-3;
00865 x2 = inxold-2;
00866 x3 = inxold-1;
00867 x4 = inxold;
00868 x5 = inxold+1;
00869 x6 = inxold+2;
00870 x7 = inxold+3;
00871
00872 y1 = (inyold-3)*nx;
00873 y2 = (inyold-2)*nx;
00874 y3 = (inyold-1)*nx;
00875 y4 = inyold*nx;
00876 y5 = (inyold+1)*nx;
00877 y6 = (inyold+2)*nx;
00878 y7 = (inyold+3)*nx;
00879 }
00880
00881 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00882 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00883 data[x7+y1]*tablex7 ) * tabley1 +
00884 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00885 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00886 data[x7+y2]*tablex7 ) * tabley2 +
00887 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00888 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00889 data[x7+y3]*tablex7 ) * tabley3 +
00890 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00891 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00892 data[x7+y4]*tablex7 ) * tabley4 +
00893 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00894 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00895 data[x7+y5]*tablex7 ) * tabley5 +
00896 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00897 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00898 data[x7+y6]*tablex7 ) * tabley6 +
00899 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00900 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00901 data[x7+y7]*tablex7 ) * tabley7;
00902
00903 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00904 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00905 } else {
00906 dely = restrict1(dely, ny);
00907 int inyold = int(Util::round(dely));
00908 delz = restrict1(delz, nz);
00909 int inzold = int(Util::round(delz));
00910
00911 float tablex1 = kb.i0win_tab(delx-inxold+3);
00912 float tablex2 = kb.i0win_tab(delx-inxold+2);
00913 float tablex3 = kb.i0win_tab(delx-inxold+1);
00914 float tablex4 = kb.i0win_tab(delx-inxold);
00915 float tablex5 = kb.i0win_tab(delx-inxold-1);
00916 float tablex6 = kb.i0win_tab(delx-inxold-2);
00917 float tablex7 = kb.i0win_tab(delx-inxold-3);
00918
00919 float tabley1 = kb.i0win_tab(dely-inyold+3);
00920 float tabley2 = kb.i0win_tab(dely-inyold+2);
00921 float tabley3 = kb.i0win_tab(dely-inyold+1);
00922 float tabley4 = kb.i0win_tab(dely-inyold);
00923 float tabley5 = kb.i0win_tab(dely-inyold-1);
00924 float tabley6 = kb.i0win_tab(dely-inyold-2);
00925 float tabley7 = kb.i0win_tab(dely-inyold-3);
00926
00927 float tablez1 = kb.i0win_tab(delz-inzold+3);
00928 float tablez2 = kb.i0win_tab(delz-inzold+2);
00929 float tablez3 = kb.i0win_tab(delz-inzold+1);
00930 float tablez4 = kb.i0win_tab(delz-inzold);
00931 float tablez5 = kb.i0win_tab(delz-inzold-1);
00932 float tablez6 = kb.i0win_tab(delz-inzold-2);
00933 float tablez7 = kb.i0win_tab(delz-inzold-3);
00934
00935 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00936
00937 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
00938 x1 = (inxold-3+nx)%nx;
00939 x2 = (inxold-2+nx)%nx;
00940 x3 = (inxold-1+nx)%nx;
00941 x4 = (inxold +nx)%nx;
00942 x5 = (inxold+1+nx)%nx;
00943 x6 = (inxold+2+nx)%nx;
00944 x7 = (inxold+3+nx)%nx;
00945
00946 y1 = ((inyold-3+ny)%ny)*nx;
00947 y2 = ((inyold-2+ny)%ny)*nx;
00948 y3 = ((inyold-1+ny)%ny)*nx;
00949 y4 = ((inyold +ny)%ny)*nx;
00950 y5 = ((inyold+1+ny)%ny)*nx;
00951 y6 = ((inyold+2+ny)%ny)*nx;
00952 y7 = ((inyold+3+ny)%ny)*nx;
00953
00954 z1 = ((inzold-3+nz)%nz)*nx*ny;
00955 z2 = ((inzold-2+nz)%nz)*nx*ny;
00956 z3 = ((inzold-1+nz)%nz)*nx*ny;
00957 z4 = ((inzold +nz)%nz)*nx*ny;
00958 z5 = ((inzold+1+nz)%nz)*nx*ny;
00959 z6 = ((inzold+2+nz)%nz)*nx*ny;
00960 z7 = ((inzold+3+nz)%nz)*nx*ny;
00961 } else {
00962 x1 = inxold-3;
00963 x2 = inxold-2;
00964 x3 = inxold-1;
00965 x4 = inxold;
00966 x5 = inxold+1;
00967 x6 = inxold+2;
00968 x7 = inxold+3;
00969
00970 y1 = (inyold-3)*nx;
00971 y2 = (inyold-2)*nx;
00972 y3 = (inyold-1)*nx;
00973 y4 = inyold*nx;
00974 y5 = (inyold+1)*nx;
00975 y6 = (inyold+2)*nx;
00976 y7 = (inyold+3)*nx;
00977
00978 z1 = (inzold-3)*nx*ny;
00979 z2 = (inzold-2)*nx*ny;
00980 z3 = (inzold-1)*nx*ny;
00981 z4 = inzold*nx*ny;
00982 z5 = (inzold+1)*nx*ny;
00983 z6 = (inzold+2)*nx*ny;
00984 z7 = (inzold+3)*nx*ny;
00985 }
00986
00987 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00988 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00989 data[x7+y1+z1]*tablex7 ) * tabley1 +
00990 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00991 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00992 data[x7+y2+z1]*tablex7 ) * tabley2 +
00993 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00994 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00995 data[x7+y3+z1]*tablex7 ) * tabley3 +
00996 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00997 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00998 data[x7+y4+z1]*tablex7 ) * tabley4 +
00999 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01000 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01001 data[x7+y5+z1]*tablex7 ) * tabley5 +
01002 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01003 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01004 data[x7+y6+z1]*tablex7 ) * tabley6 +
01005 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01006 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01007 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01008 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01009 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01010 data[x7+y1+z2]*tablex7 ) * tabley1 +
01011 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01012 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01013 data[x7+y2+z2]*tablex7 ) * tabley2 +
01014 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01015 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01016 data[x7+y3+z2]*tablex7 ) * tabley3 +
01017 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01018 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01019 data[x7+y4+z2]*tablex7 ) * tabley4 +
01020 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01021 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01022 data[x7+y5+z2]*tablex7 ) * tabley5 +
01023 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01024 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01025 data[x7+y6+z2]*tablex7 ) * tabley6 +
01026 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01027 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01028 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01029 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01030 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01031 data[x7+y1+z3]*tablex7 ) * tabley1 +
01032 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01033 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01034 data[x7+y2+z3]*tablex7 ) * tabley2 +
01035 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01036 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01037 data[x7+y3+z3]*tablex7 ) * tabley3 +
01038 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01039 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01040 data[x7+y4+z3]*tablex7 ) * tabley4 +
01041 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01042 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01043 data[x7+y5+z3]*tablex7 ) * tabley5 +
01044 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01045 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01046 data[x7+y6+z3]*tablex7 ) * tabley6 +
01047 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01048 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01049 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01050 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01051 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01052 data[x7+y1+z4]*tablex7 ) * tabley1 +
01053 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01054 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01055 data[x7+y2+z4]*tablex7 ) * tabley2 +
01056 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01057 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01058 data[x7+y3+z4]*tablex7 ) * tabley3 +
01059 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01060 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01061 data[x7+y4+z4]*tablex7 ) * tabley4 +
01062 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01063 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01064 data[x7+y5+z4]*tablex7 ) * tabley5 +
01065 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01066 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01067 data[x7+y6+z4]*tablex7 ) * tabley6 +
01068 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01069 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01070 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01071 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01072 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01073 data[x7+y1+z5]*tablex7 ) * tabley1 +
01074 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01075 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01076 data[x7+y2+z5]*tablex7 ) * tabley2 +
01077 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01078 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01079 data[x7+y3+z5]*tablex7 ) * tabley3 +
01080 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01081 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01082 data[x7+y4+z5]*tablex7 ) * tabley4 +
01083 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01084 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01085 data[x7+y5+z5]*tablex7 ) * tabley5 +
01086 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01087 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01088 data[x7+y6+z5]*tablex7 ) * tabley6 +
01089 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01090 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01091 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01092 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01093 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01094 data[x7+y1+z6]*tablex7 ) * tabley1 +
01095 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01096 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01097 data[x7+y2+z6]*tablex7 ) * tabley2 +
01098 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01099 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01100 data[x7+y3+z6]*tablex7 ) * tabley3 +
01101 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01102 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01103 data[x7+y4+z6]*tablex7 ) * tabley4 +
01104 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01105 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01106 data[x7+y5+z6]*tablex7 ) * tabley5 +
01107 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01108 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01109 data[x7+y6+z6]*tablex7 ) * tabley6 +
01110 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01111 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01112 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01113 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01114 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01115 data[x7+y1+z7]*tablex7 ) * tabley1 +
01116 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01117 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01118 data[x7+y2+z7]*tablex7 ) * tabley2 +
01119 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01120 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01121 data[x7+y3+z7]*tablex7 ) * tabley3 +
01122 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01123 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01124 data[x7+y4+z7]*tablex7 ) * tabley4 +
01125 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01126 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01127 data[x7+y5+z7]*tablex7 ) * tabley5 +
01128 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01129 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01130 data[x7+y6+z7]*tablex7 ) * tabley6 +
01131 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01132 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01133 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01134
01135 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01136 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01137 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01138 }
01139 return pixel/w;
01140 }
01141
01142 float Util::get_pixel_conv_new_background(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb, int xnew, int ynew) {
01143 int K = kb.get_window_size();
01144 int kbmin = -K/2;
01145 int kbmax = -kbmin;
01146 int kbc = kbmax+1;
01147
01148 float pixel =0.0f;
01149 float w=0.0f;
01150
01151 float argdelx = delx;
01152 delx = restrict1(delx, nx);
01153 int inxold = int(round(delx));
01154 if ( ny < 2 ) {
01155 float tablex1 = kb.i0win_tab(delx-inxold+3);
01156 float tablex2 = kb.i0win_tab(delx-inxold+2);
01157 float tablex3 = kb.i0win_tab(delx-inxold+1);
01158 float tablex4 = kb.i0win_tab(delx-inxold);
01159 float tablex5 = kb.i0win_tab(delx-inxold-1);
01160 float tablex6 = kb.i0win_tab(delx-inxold-2);
01161 float tablex7 = kb.i0win_tab(delx-inxold-3);
01162
01163 int x1, x2, x3, x4, x5, x6, x7;
01164
01165 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
01166 x1 = (inxold-3+nx)%nx;
01167 x2 = (inxold-2+nx)%nx;
01168 x3 = (inxold-1+nx)%nx;
01169 x4 = (inxold +nx)%nx;
01170 x5 = (inxold+1+nx)%nx;
01171 x6 = (inxold+2+nx)%nx;
01172 x7 = (inxold+3+nx)%nx;
01173 } else {
01174 x1 = inxold-3;
01175 x2 = inxold-2;
01176 x3 = inxold-1;
01177 x4 = inxold;
01178 x5 = inxold+1;
01179 x6 = inxold+2;
01180 x7 = inxold+3;
01181 }
01182
01183 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01184 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01185 data[x7]*tablex7 ;
01186
01187 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01188 } else if ( nz < 2 ) {
01189
01190 delx = argdelx;
01191
01192 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01193 delx = (float)xnew*2.0f;
01194 dely = (float)ynew*2.0f;
01195 }
01196
01197 int inxold = int(round(delx));
01198 int inyold = int(round(dely));
01199
01200 float tablex1 = kb.i0win_tab(delx-inxold+3);
01201 float tablex2 = kb.i0win_tab(delx-inxold+2);
01202 float tablex3 = kb.i0win_tab(delx-inxold+1);
01203 float tablex4 = kb.i0win_tab(delx-inxold);
01204 float tablex5 = kb.i0win_tab(delx-inxold-1);
01205 float tablex6 = kb.i0win_tab(delx-inxold-2);
01206 float tablex7 = kb.i0win_tab(delx-inxold-3);
01207
01208 float tabley1 = kb.i0win_tab(dely-inyold+3);
01209 float tabley2 = kb.i0win_tab(dely-inyold+2);
01210 float tabley3 = kb.i0win_tab(dely-inyold+1);
01211 float tabley4 = kb.i0win_tab(dely-inyold);
01212 float tabley5 = kb.i0win_tab(dely-inyold-1);
01213 float tabley6 = kb.i0win_tab(dely-inyold-2);
01214 float tabley7 = kb.i0win_tab(dely-inyold-3);
01215
01216 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01217
01218 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
01219 x1 = (inxold-3+nx)%nx;
01220 x2 = (inxold-2+nx)%nx;
01221 x3 = (inxold-1+nx)%nx;
01222 x4 = (inxold +nx)%nx;
01223 x5 = (inxold+1+nx)%nx;
01224 x6 = (inxold+2+nx)%nx;
01225 x7 = (inxold+3+nx)%nx;
01226
01227 y1 = ((inyold-3+ny)%ny)*nx;
01228 y2 = ((inyold-2+ny)%ny)*nx;
01229 y3 = ((inyold-1+ny)%ny)*nx;
01230 y4 = ((inyold +ny)%ny)*nx;
01231 y5 = ((inyold+1+ny)%ny)*nx;
01232 y6 = ((inyold+2+ny)%ny)*nx;
01233 y7 = ((inyold+3+ny)%ny)*nx;
01234 } else {
01235 x1 = inxold-3;
01236 x2 = inxold-2;
01237 x3 = inxold-1;
01238 x4 = inxold;
01239 x5 = inxold+1;
01240 x6 = inxold+2;
01241 x7 = inxold+3;
01242
01243 y1 = (inyold-3)*nx;
01244 y2 = (inyold-2)*nx;
01245 y3 = (inyold-1)*nx;
01246 y4 = inyold*nx;
01247 y5 = (inyold+1)*nx;
01248 y6 = (inyold+2)*nx;
01249 y7 = (inyold+3)*nx;
01250 }
01251
01252 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01253 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01254 data[x7+y1]*tablex7 ) * tabley1 +
01255 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01256 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01257 data[x7+y2]*tablex7 ) * tabley2 +
01258 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01259 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01260 data[x7+y3]*tablex7 ) * tabley3 +
01261 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01262 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01263 data[x7+y4]*tablex7 ) * tabley4 +
01264 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01265 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01266 data[x7+y5]*tablex7 ) * tabley5 +
01267 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01268 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01269 data[x7+y6]*tablex7 ) * tabley6 +
01270 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01271 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01272 data[x7+y7]*tablex7 ) * tabley7;
01273
01274 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01275 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01276 } else {
01277 dely = restrict1(dely, ny);
01278 int inyold = int(Util::round(dely));
01279 delz = restrict1(delz, nz);
01280 int inzold = int(Util::round(delz));
01281
01282 float tablex1 = kb.i0win_tab(delx-inxold+3);
01283 float tablex2 = kb.i0win_tab(delx-inxold+2);
01284 float tablex3 = kb.i0win_tab(delx-inxold+1);
01285 float tablex4 = kb.i0win_tab(delx-inxold);
01286 float tablex5 = kb.i0win_tab(delx-inxold-1);
01287 float tablex6 = kb.i0win_tab(delx-inxold-2);
01288 float tablex7 = kb.i0win_tab(delx-inxold-3);
01289
01290 float tabley1 = kb.i0win_tab(dely-inyold+3);
01291 float tabley2 = kb.i0win_tab(dely-inyold+2);
01292 float tabley3 = kb.i0win_tab(dely-inyold+1);
01293 float tabley4 = kb.i0win_tab(dely-inyold);
01294 float tabley5 = kb.i0win_tab(dely-inyold-1);
01295 float tabley6 = kb.i0win_tab(dely-inyold-2);
01296 float tabley7 = kb.i0win_tab(dely-inyold-3);
01297
01298 float tablez1 = kb.i0win_tab(delz-inzold+3);
01299 float tablez2 = kb.i0win_tab(delz-inzold+2);
01300 float tablez3 = kb.i0win_tab(delz-inzold+1);
01301 float tablez4 = kb.i0win_tab(delz-inzold);
01302 float tablez5 = kb.i0win_tab(delz-inzold-1);
01303 float tablez6 = kb.i0win_tab(delz-inzold-2);
01304 float tablez7 = kb.i0win_tab(delz-inzold-3);
01305
01306 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01307
01308 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
01309 x1 = (inxold-3+nx)%nx;
01310 x2 = (inxold-2+nx)%nx;
01311 x3 = (inxold-1+nx)%nx;
01312 x4 = (inxold +nx)%nx;
01313 x5 = (inxold+1+nx)%nx;
01314 x6 = (inxold+2+nx)%nx;
01315 x7 = (inxold+3+nx)%nx;
01316
01317 y1 = ((inyold-3+ny)%ny)*nx;
01318 y2 = ((inyold-2+ny)%ny)*nx;
01319 y3 = ((inyold-1+ny)%ny)*nx;
01320 y4 = ((inyold +ny)%ny)*nx;
01321 y5 = ((inyold+1+ny)%ny)*nx;
01322 y6 = ((inyold+2+ny)%ny)*nx;
01323 y7 = ((inyold+3+ny)%ny)*nx;
01324
01325 z1 = ((inzold-3+nz)%nz)*nx*ny;
01326 z2 = ((inzold-2+nz)%nz)*nx*ny;
01327 z3 = ((inzold-1+nz)%nz)*nx*ny;
01328 z4 = ((inzold +nz)%nz)*nx*ny;
01329 z5 = ((inzold+1+nz)%nz)*nx*ny;
01330 z6 = ((inzold+2+nz)%nz)*nx*ny;
01331 z7 = ((inzold+3+nz)%nz)*nx*ny;
01332 } else {
01333 x1 = inxold-3;
01334 x2 = inxold-2;
01335 x3 = inxold-1;
01336 x4 = inxold;
01337 x5 = inxold+1;
01338 x6 = inxold+2;
01339 x7 = inxold+3;
01340
01341 y1 = (inyold-3)*nx;
01342 y2 = (inyold-2)*nx;
01343 y3 = (inyold-1)*nx;
01344 y4 = inyold*nx;
01345 y5 = (inyold+1)*nx;
01346 y6 = (inyold+2)*nx;
01347 y7 = (inyold+3)*nx;
01348
01349 z1 = (inzold-3)*nx*ny;
01350 z2 = (inzold-2)*nx*ny;
01351 z3 = (inzold-1)*nx*ny;
01352 z4 = inzold*nx*ny;
01353 z5 = (inzold+1)*nx*ny;
01354 z6 = (inzold+2)*nx*ny;
01355 z7 = (inzold+3)*nx*ny;
01356 }
01357
01358 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01359 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01360 data[x7+y1+z1]*tablex7 ) * tabley1 +
01361 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01362 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01363 data[x7+y2+z1]*tablex7 ) * tabley2 +
01364 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01365 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01366 data[x7+y3+z1]*tablex7 ) * tabley3 +
01367 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01368 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01369 data[x7+y4+z1]*tablex7 ) * tabley4 +
01370 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01371 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01372 data[x7+y5+z1]*tablex7 ) * tabley5 +
01373 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01374 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01375 data[x7+y6+z1]*tablex7 ) * tabley6 +
01376 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01377 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01378 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01379 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01380 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01381 data[x7+y1+z2]*tablex7 ) * tabley1 +
01382 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01383 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01384 data[x7+y2+z2]*tablex7 ) * tabley2 +
01385 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01386 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01387 data[x7+y3+z2]*tablex7 ) * tabley3 +
01388 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01389 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01390 data[x7+y4+z2]*tablex7 ) * tabley4 +
01391 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01392 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01393 data[x7+y5+z2]*tablex7 ) * tabley5 +
01394 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01395 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01396 data[x7+y6+z2]*tablex7 ) * tabley6 +
01397 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01398 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01399 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01400 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01401 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01402 data[x7+y1+z3]*tablex7 ) * tabley1 +
01403 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01404 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01405 data[x7+y2+z3]*tablex7 ) * tabley2 +
01406 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01407 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01408 data[x7+y3+z3]*tablex7 ) * tabley3 +
01409 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01410 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01411 data[x7+y4+z3]*tablex7 ) * tabley4 +
01412 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01413 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01414 data[x7+y5+z3]*tablex7 ) * tabley5 +
01415 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01416 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01417 data[x7+y6+z3]*tablex7 ) * tabley6 +
01418 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01419 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01420 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01421 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01422 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01423 data[x7+y1+z4]*tablex7 ) * tabley1 +
01424 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01425 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01426 data[x7+y2+z4]*tablex7 ) * tabley2 +
01427 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01428 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01429 data[x7+y3+z4]*tablex7 ) * tabley3 +
01430 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01431 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01432 data[x7+y4+z4]*tablex7 ) * tabley4 +
01433 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01434 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01435 data[x7+y5+z4]*tablex7 ) * tabley5 +
01436 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01437 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01438 data[x7+y6+z4]*tablex7 ) * tabley6 +
01439 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01440 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01441 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01442 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01443 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01444 data[x7+y1+z5]*tablex7 ) * tabley1 +
01445 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01446 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01447 data[x7+y2+z5]*tablex7 ) * tabley2 +
01448 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01449 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01450 data[x7+y3+z5]*tablex7 ) * tabley3 +
01451 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01452 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01453 data[x7+y4+z5]*tablex7 ) * tabley4 +
01454 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01455 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01456 data[x7+y5+z5]*tablex7 ) * tabley5 +
01457 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01458 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01459 data[x7+y6+z5]*tablex7 ) * tabley6 +
01460 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01461 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01462 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01463 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01464 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01465 data[x7+y1+z6]*tablex7 ) * tabley1 +
01466 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01467 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01468 data[x7+y2+z6]*tablex7 ) * tabley2 +
01469 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01470 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01471 data[x7+y3+z6]*tablex7 ) * tabley3 +
01472 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01473 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01474 data[x7+y4+z6]*tablex7 ) * tabley4 +
01475 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01476 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01477 data[x7+y5+z6]*tablex7 ) * tabley5 +
01478 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01479 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01480 data[x7+y6+z6]*tablex7 ) * tabley6 +
01481 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01482 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01483 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01484 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01485 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01486 data[x7+y1+z7]*tablex7 ) * tabley1 +
01487 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01488 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01489 data[x7+y2+z7]*tablex7 ) * tabley2 +
01490 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01491 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01492 data[x7+y3+z7]*tablex7 ) * tabley3 +
01493 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01494 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01495 data[x7+y4+z7]*tablex7 ) * tabley4 +
01496 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01497 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01498 data[x7+y5+z7]*tablex7 ) * tabley5 +
01499 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01500 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01501 data[x7+y6+z7]*tablex7 ) * tabley6 +
01502 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01503 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01504 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01505
01506 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01507 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01508 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01509 }
01510 return pixel/w;
01511 }
01512
01513
01514
01515
01516
01517
01518
01519
01520
01521
01522
01523
01524
01525
01526
01527
01528
01529
01530
01531
01532
01533
01534
01535
01536
01537
01538
01539
01540
01541
01542
01543
01544
01545
01546
01547
01548
01549
01550
01551
01552
01553
01554
01555
01556
01557
01558
01559
01560
01561
01562
01563
01564
01565
01566
01567
01568
01569
01570
01571
01572
01573
01574
01575
01576
01577
01578
01579
01580
01581
01582
01583
01584
01585
01586
01587
01588
01589
01590
01591
01592
01593
01594
01595
01596
01597
01598
01599
01600
01601
01602
01603
01604
01605
01606
01607
01608
01609
01610
01611
01612
01613
01614
01615
01616
01617
01618
01619
01620
01621
01622
01623
01624
01625
01626
01627
01628
01629
01630
01631
01632
01633
01634
01635
01636
01637
01638
01639
01640
01641
01642
01643
01644
01645
01646
01647
01648
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672
01673
01674
01675
01676
01677
01678
01679
01680
01681
01682
01683
01684
01685
01686
01687
01688
01689
01690
01691 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01692
01693 int nxreal = nx - 2;
01694 if (nxreal != ny)
01695 throw ImageDimensionException("extractpoint requires ny == nx");
01696 int nhalf = nxreal/2;
01697 bool flip = (nuxnew < 0.f);
01698 if (flip) {
01699 nuxnew *= -1;
01700 nuynew *= -1;
01701 }
01702 if (nuynew >= nhalf-0.5) {
01703 nuynew -= nxreal;
01704 } else if (nuynew < -nhalf-0.5) {
01705 nuynew += nxreal;
01706 }
01707
01708
01709
01710
01711 int ixn = int(Util::round(nuxnew));
01712 int iyn = int(Util::round(nuynew));
01713
01714
01715 static float wy[7];
01716 static float wx[7];
01717
01718 float iynn = nuynew - iyn;
01719 wy[0] = kb.i0win_tab(iynn+3);
01720 wy[1] = kb.i0win_tab(iynn+2);
01721 wy[2] = kb.i0win_tab(iynn+1);
01722 wy[3] = kb.i0win_tab(iynn);
01723 wy[4] = kb.i0win_tab(iynn-1);
01724 wy[5] = kb.i0win_tab(iynn-2);
01725 wy[6] = kb.i0win_tab(iynn-3);
01726
01727 float ixnn = nuxnew - ixn;
01728 wx[0] = kb.i0win_tab(ixnn+3);
01729 wx[1] = kb.i0win_tab(ixnn+2);
01730 wx[2] = kb.i0win_tab(ixnn+1);
01731 wx[3] = kb.i0win_tab(ixnn);
01732 wx[4] = kb.i0win_tab(ixnn-1);
01733 wx[5] = kb.i0win_tab(ixnn-2);
01734 wx[6] = kb.i0win_tab(ixnn-3);
01735
01736 float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01737
01738 complex<float> result(0.f,0.f);
01739 if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01740
01741 for (int iy = 0; iy < 7; iy++) {
01742 int iyp = iyn + iy - 3 ;
01743 for (int ix = 0; ix < 7; ix++) {
01744 int ixp = ixn + ix - 3;
01745 float w = wx[ix]*wy[iy];
01746 complex<float> val = fimage->cmplx(ixp,iyp);
01747 result += val*w;
01748 }
01749 }
01750 } else {
01751
01752 for (int iy = 0; iy < 7; iy++) {
01753 int iyp = iyn + iy - 3;
01754 for (int ix = 0; ix < 7; ix++) {
01755 int ixp = ixn + ix - 3;
01756 bool mirror = false;
01757 int ixt = ixp, iyt = iyp;
01758 if (ixt < 0) {
01759 ixt = -ixt;
01760 iyt = -iyt;
01761 mirror = !mirror;
01762 }
01763 if (ixt > nhalf) {
01764 ixt = nxreal - ixt;
01765 iyt = -iyt;
01766 mirror = !mirror;
01767 }
01768 if (iyt > nhalf-1) iyt -= nxreal;
01769 if (iyt < -nhalf) iyt += nxreal;
01770 float w = wx[ix]*wy[iy];
01771 complex<float> val = fimage->cmplx(ixt,iyt);
01772 if (mirror) result += conj(val)*w;
01773 else result += val*w;
01774 }
01775 }
01776 }
01777 if (flip) result = conj(result)/wsum;
01778 else result /= wsum;
01779 return result;
01780 }
01781
01782
01783
01784
01785
01786
01787
01788
01789
01790
01791
01792
01793
01794
01795
01796
01797
01798
01799
01800
01801
01802
01803
01804
01805
01806
01807
01808
01809
01810
01811
01812
01813
01814
01815
01816
01817
01818
01819
01820
01821
01822
01823
01824
01825
01826
01827
01828
01829
01830
01831
01832
01833
01834
01835
01836
01837
01838
01839
01840
01841
01842
01843
01844
01845
01846
01847
01848
01849
01850
01851
01852
01853
01854
01855
01856
01857
01858
01859
01860
01861
01862
01863
01864
01865
01866
01867
01868
01869
01870
01871
01872
01873
01874
01875
01876
01877
01878
01879
01880
01881
01882
01883
01884
01885
01886
01887
01888
01889
01890
01891
01892
01893
01894
01895
01896
01897
01898
01899
01900
01901
01902
01903
01904
01905
01906
01907
01908
01909
01910 float Util::triquad(float R, float S, float T, float* fdata)
01911 {
01912
01913 const float C2 = 0.5f;
01914 const float C4 = 0.25f;
01915 const float C8 = 0.125f;
01916
01917 float RS = R * S;
01918 float ST = S * T;
01919 float RT = R * T;
01920 float RST = R * ST;
01921
01922 float RSQ = 1-R*R;
01923 float SSQ = 1-S*S;
01924 float TSQ = 1-T*T;
01925
01926 float RM1 = (1-R);
01927 float SM1 = (1-S);
01928 float TM1 = (1-T);
01929
01930 float RP1 = (1+R);
01931 float SP1 = (1+S);
01932 float TP1 = (1+T);
01933
01934 float triquad =
01935 (-C8) * RST * RM1 * SM1 * TM1 * fdata[0] +
01936 ( C4) * ST * RSQ * SM1 * TM1 * fdata[1] +
01937 ( C8) * RST * RP1 * SM1 * TM1 * fdata[2] +
01938 ( C4) * RT * RM1 * SSQ * TM1 * fdata[3] +
01939 (-C2) * T * RSQ * SSQ * TM1 * fdata[4] +
01940 (-C4) * RT * RP1 * SSQ * TM1 * fdata[5] +
01941 ( C8) * RST * RM1 * SP1 * TM1 * fdata[6] +
01942 (-C4) * ST * RSQ * SP1 * TM1 * fdata[7] +
01943 (-C8) * RST * RP1 * SP1 * TM1 * fdata[8] +
01944
01945 ( C4) * RS * RM1 * SM1 * TSQ * fdata[9] +
01946 (-C2) * S * RSQ * SM1 * TSQ * fdata[10] +
01947 (-C4) * RS * RP1 * SM1 * TSQ * fdata[11] +
01948 (-C2) * R * RM1 * SSQ * TSQ * fdata[12] +
01949 RSQ * SSQ * TSQ * fdata[13] +
01950 ( C2) * R * RP1 * SSQ * TSQ * fdata[14] +
01951 (-C4) * RS * RM1 * SP1 * TSQ * fdata[15] +
01952 ( C2) * S * RSQ * SP1 * TSQ * fdata[16] +
01953 ( C4) * RS * RP1 * SP1 * TSQ * fdata[17] +
01954
01955 ( C8) * RST * RM1 * SM1 * TP1 * fdata[18] +
01956 (-C4) * ST * RSQ * SM1 * TP1 * fdata[19] +
01957 (-C8) * RST * RP1 * SM1 * TP1 * fdata[20] +
01958 (-C4) * RT * RM1 * SSQ * TP1 * fdata[21] +
01959 ( C2) * T * RSQ * SSQ * TP1 * fdata[22] +
01960 ( C4) * RT * RP1 * SSQ * TP1 * fdata[23] +
01961 (-C8) * RST * RM1 * SP1 * TP1 * fdata[24] +
01962 ( C4) * ST * RSQ * SP1 * TP1 * fdata[25] +
01963 ( C8) * RST * RP1 * SP1 * TP1 * fdata[26] ;
01964 return triquad;
01965 }
01966
01967 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01968 : M(M_), fc(fc_), ntable(ntable_) {
01969
01970 build_sBtable();
01971 }
01972
01973 void Util::sincBlackman::build_sBtable() {
01974 sBtable.resize(ntable+1);
01975 int ltab = int(round(float(ntable)/1.25f));
01976 int M2 = M/2;
01977 fltb = float(ltab)/M2;
01978 for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01979 float x = 1.0e-7f;
01980 sBtable[0] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01981 for (int i=1; i <= ltab; i++) {
01982 x = float(i)/fltb;
01983 sBtable[i] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01984
01985 }
01986 }
01987
01988 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01989 int N_, float vtable_, int ntable_)
01990 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01991 ntable(ntable_) {
01992
01993 if (0.f == v) v = float(K)/2;
01994 if (0.f == vtable) vtable = v;
01995 alphar = alpha*r;
01996 fac = static_cast<float>(twopi)*alphar*v;
01997 vadjust = 1.0f*v;
01998 facadj = static_cast<float>(twopi)*alphar*vadjust;
01999 build_I0table();
02000 }
02001
02002 float Util::KaiserBessel::i0win(float x) const {
02003 float val0 = float(gsl_sf_bessel_I0(facadj));
02004 float absx = fabs(x);
02005 if (absx > vadjust) return 0.f;
02006 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02007 float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02008 return res;
02009 }
02010
02011 void Util::KaiserBessel::build_I0table() {
02012 i0table.resize(ntable+1);
02013 int ltab = int(round(float(ntable)/1.25f));
02014 fltb = float(ltab)/(K/2);
02015 float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02016 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02017 for (int i=0; i <= ltab; i++) {
02018 float s = float(i)/fltb/N;
02019 if (s < vadjust) {
02020 float rt = sqrt(1.f - pow(s/vadjust, 2));
02021 i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02022 } else {
02023 i0table[i] = 0.f;
02024 }
02025
02026 }
02027 }
02028
02029 float Util::KaiserBessel::I0table_maxerror() {
02030 float maxdiff = 0.f;
02031 for (int i = 1; i <= ntable; i++) {
02032 float diff = fabs(i0table[i] - i0table[i-1]);
02033 if (diff > maxdiff) maxdiff = diff;
02034 }
02035 return maxdiff;
02036 }
02037
02038 float Util::KaiserBessel::sinhwin(float x) const {
02039 float val0 = sinh(fac)/fac;
02040 float absx = fabs(x);
02041 if (0.0 == x) {
02042 float res = 1.0f;
02043 return res;
02044 } else if (absx == alphar) {
02045 return 1.0f/val0;
02046 } else if (absx < alphar) {
02047 float rt = sqrt(1.0f - pow((x/alphar), 2));
02048 float facrt = fac*rt;
02049 float res = (sinh(facrt)/facrt)/val0;
02050 return res;
02051 } else {
02052 float rt = sqrt(pow((x/alphar),2) - 1.f);
02053 float facrt = fac*rt;
02054 float res = (sin(facrt)/facrt)/val0;
02055 return res;
02056 }
02057 }
02058
02059 float Util::FakeKaiserBessel::i0win(float x) const {
02060 float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02061 float absx = fabs(x);
02062 if (absx > vadjust) return 0.f;
02063 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02064 float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02065 return res;
02066 }
02067
02068 void Util::FakeKaiserBessel::build_I0table() {
02069 i0table.resize(ntable+1);
02070 int ltab = int(round(float(ntable)/1.1f));
02071 fltb = float(ltab)/(K/2);
02072 float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02073 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02074 for (int i=0; i <= ltab; i++) {
02075 float s = float(i)/fltb/N;
02076 if (s < vadjust) {
02077 float rt = sqrt(1.f - pow(s/vadjust, 2));
02078 i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02079 } else {
02080 i0table[i] = 0.f;
02081 }
02082 }
02083 }
02084
02085 float Util::FakeKaiserBessel::sinhwin(float x) const {
02086 float val0 = sinh(fac)/fac;
02087 float absx = fabs(x);
02088 if (0.0 == x) {
02089 float res = 1.0f;
02090 return res;
02091 } else if (absx == alphar) {
02092 return 1.0f/val0;
02093 } else if (absx < alphar) {
02094 float rt = sqrt(1.0f - pow((x/alphar), 2));
02095 float facrt = fac*rt;
02096 float res = (sinh(facrt)/facrt)/val0;
02097 return res;
02098 } else {
02099 float rt = sqrt(pow((x/alphar),2) - 1.f);
02100 float facrt = fac*rt;
02101 float res = (sin(facrt)/facrt)/val0;
02102 return res;
02103 }
02104 }
02105
02106 #if 0 // 1-st order KB window
02107 float Util::FakeKaiserBessel::sinhwin(float x) const {
02108
02109 float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02110 float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02111 float absx = fabs(x);
02112 if (0.0 == x) {
02113
02114 float res = val0;
02115 return res;
02116 } else if (absx == alphar) {
02117
02118 return prefix;
02119 } else if (absx < alphar) {
02120 float rt = sqrt(1.0f - pow((x/alphar), 2));
02121
02122 float facrt = facadj*rt;
02123
02124 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02125 return res;
02126 } else {
02127 float rt = sqrt(pow((x/alphar),2) - 1.f);
02128
02129 float facrt = facadj*rt;
02130
02131 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02132 return res;
02133 }
02134 }
02135 #endif // 0
02136
02137
02138
02139 #define circ(i) circ[i-1]
02140 #define numr(i,j) numr[(j-1)*3 + i-1]
02141 #define xim(i,j) xim[(j-1)*nsam + i-1]
02142
02143 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02144 int nsam = image->get_xsize();
02145 int nrow = image->get_ysize();
02146 int nring = numr.size()/3;
02147 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02148 EMData* out = new EMData();
02149 out->set_size(lcirc,1,1);
02150 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02151 float *xim = image->get_data();
02152 float *circ = out->get_data();
02153
02154
02155
02156
02157
02158
02159
02160
02161
02162
02163
02164
02165
02166
02167
02168
02169 double dfi, dpi;
02170 int ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02171 float yq, xold, yold, fi, x, y;
02172
02173 ns2 = nsam/2+1;
02174 nr2 = nrow/2+1;
02175 dpi = 2.0*atan(1.0);
02176
02177 for (i=1;i<=nring;i++) {
02178
02179 inr = numr(1,i);
02180 yq = static_cast<float>(inr);
02181 l = numr(3,i);
02182 if (mode == 'h' || mode == 'H') lt = l/2;
02183 else lt = l/4;
02184
02185 nsim = lt-1;
02186 dfi = dpi/(nsim+1);
02187 kcirc = numr(2,i);
02188 xold = 0.0f;
02189 yold = static_cast<float>(inr);
02190 circ(kcirc) = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02191 xold = static_cast<float>(inr);
02192 yold = 0.0f;
02193 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02194
02195 if (mode == 'f' || mode == 'F') {
02196 xold = 0.0f;
02197 yold = static_cast<float>(-inr);
02198 circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02199 xold = static_cast<float>(-inr);
02200 yold = 0.0f;
02201 circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02202 }
02203
02204 for (j=1;j<=nsim;j++) {
02205 fi = static_cast<float>(dfi*j);
02206 x = sin(fi)*yq;
02207 y = cos(fi)*yq;
02208 xold = x;
02209 yold = y;
02210 circ(j+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02211 xold = y;
02212 yold = -x;
02213 circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02214
02215 if (mode == 'f' || mode == 'F') {
02216 xold = -x;
02217 yold = -y;
02218 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02219 xold = -y;
02220 yold = x;
02221 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02222 }
02223 }
02224 }
02225 return out;
02226 }
02227
02228 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02229 int nsam = image->get_xsize();
02230 int nrow = image->get_ysize();
02231 int nring = numr.size()/3;
02232 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02233 EMData* out = new EMData();
02234 out->set_size(lcirc,1,1);
02235 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02236 float *xim = image->get_data();
02237 float *circ = out->get_data();
02238 double dpi, dfi;
02239 int it, jt, inr, l, nsim, kcirc, lt;
02240 float xold, yold, fi, x, y;
02241
02242
02243
02244 dpi = 2*atan(1.0);
02245 for (it=1; it<=nring; it++) {
02246
02247 inr = numr(1,it);
02248
02249
02250
02251
02252 l = numr(3,it);
02253 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02254 else lt = l / 4;
02255
02256 nsim = lt - 1;
02257 dfi = dpi / (nsim+1);
02258 kcirc = numr(2,it);
02259 xold = 0.0f+cns2;
02260 yold = inr+cnr2;
02261
02262 Assert( kcirc <= lcirc );
02263 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02264
02265 xold = inr+cns2;
02266 yold = 0.0f+cnr2;
02267 Assert( lt+kcirc <= lcirc );
02268 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02269
02270 if ( mode == 'f' || mode == 'F' ) {
02271 xold = 0.0f+cns2;
02272 yold = -inr+cnr2;
02273 Assert( lt+lt+kcirc <= lcirc );
02274 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02275
02276 xold = -inr+cns2;
02277 yold = 0.0f+cnr2;
02278 Assert(lt+lt+lt+kcirc <= lcirc );
02279 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02280 }
02281
02282 for (jt=1; jt<=nsim; jt++) {
02283 fi = static_cast<float>(dfi * jt);
02284 x = sin(fi) * inr;
02285 y = cos(fi) * inr;
02286
02287 xold = x+cns2;
02288 yold = y+cnr2;
02289
02290 Assert( jt+kcirc <= lcirc );
02291 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02292
02293 xold = y+cns2;
02294 yold = -x+cnr2;
02295
02296 Assert( jt+lt+kcirc <= lcirc );
02297 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02298
02299 if ( mode == 'f' || mode == 'F' ) {
02300 xold = -x+cns2;
02301 yold = -y+cnr2;
02302
02303 Assert( jt+lt+lt+kcirc <= lcirc );
02304 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02305
02306 xold = -y+cns2;
02307 yold = x+cnr2;
02308
02309 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02310 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02311 }
02312 }
02313 }
02314 return out;
02315 }
02316
02317 float Util::bilinear(float xold, float yold, int nsam, int, float* xim)
02318 {
02319
02320
02321
02322
02323 float bilinear;
02324 int ixold, iyold;
02325
02326
02327
02328
02329
02330
02331
02332
02333
02334
02335
02336
02337
02338
02339
02340
02341
02342
02343
02344
02345
02346
02347
02348 float xdif, ydif;
02349
02350 ixold = (int) xold;
02351 iyold = (int) yold;
02352 ydif = yold - iyold;
02353
02354
02355
02356
02357
02358 xdif = xold - ixold;
02359 bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02360 xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02361 ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02362
02363 return bilinear;
02364 }
02365
02366 void Util::alrl_ms(float *xim, int nsam, int nrow, float cns2, float cnr2,
02367 int *numr, float *circ, int , int nring, char mode) {
02368 double dpi, dfi;
02369 int it, jt, inr, l, nsim, kcirc, lt;
02370 float xold, yold, fi, x, y;
02371
02372
02373
02374
02375 dpi = 2*atan(1.0);
02376 for (it=1; it<=nring; it++) {
02377
02378 inr = numr(1,it);
02379
02380 l = numr(3,it);
02381 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02382 else lt = l / 4;
02383
02384 nsim = lt - 1;
02385 dfi = dpi / (nsim+1);
02386 kcirc = numr(2,it);
02387
02388
02389 xold = 0.0f+cns2;
02390 yold = inr+cnr2;
02391
02392 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02393
02394 xold = inr+cns2;
02395 yold = 0.0f+cnr2;
02396 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02397
02398 if ( mode == 'f' || mode == 'F' ) {
02399 xold = 0.0f+cns2;
02400 yold = -inr+cnr2;
02401 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02402
02403 xold = -inr+cns2;
02404 yold = 0.0f+cnr2;
02405 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02406 }
02407
02408 for (jt=1; jt<=nsim; jt++) {
02409 fi = static_cast<float>(dfi * jt);
02410 x = sin(fi) * inr;
02411 y = cos(fi) * inr;
02412
02413 xold = x+cns2;
02414 yold = y+cnr2;
02415 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02416
02417 xold = y+cns2;
02418 yold = -x+cnr2;
02419 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02420
02421 if ( mode == 'f' || mode == 'F' ) {
02422 xold = -x+cns2;
02423 yold = -y+cnr2;
02424 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02425
02426 xold = -y+cns2;
02427 yold = x+cnr2;
02428 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02429 }
02430 }
02431 }
02432 }
02433
02434
02435
02436
02437
02438
02439
02440
02441
02442
02443
02444
02445
02446
02447
02448
02449
02450
02451
02452
02453
02454
02455
02456
02457
02458
02459
02460
02461
02462
02463
02464
02465
02466
02467
02468
02469
02470
02471
02472
02473
02474
02475
02476
02477
02478
02479
02480
02481
02482
02483
02484
02485
02486
02487
02488
02489
02490
02491
02492
02493
02494
02495
02496
02497
02498
02499
02500
02501
02502
02503
02504
02505
02506
02507
02508
02509 #undef xim
02510
02511 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02512
02513 int nring = numr.size()/3;
02514 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02515 EMData* out = new EMData();
02516 out->set_size(lcirc,1,1);
02517 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02518 float *circ = out->get_data();
02519 float *fimage = image->get_data();
02520 int nx = image->get_xsize();
02521 int ny = image->get_ysize();
02522 int nz = image->get_zsize();
02523 double dpi, dfi;
02524 int it, jt, inr, l, nsim, kcirc, lt;
02525 float yq, xold, yold, fi, x, y;
02526
02527
02528
02529
02530 dpi = 2*atan(1.0);
02531 for (it=1;it<=nring;it++) {
02532
02533 inr = numr(1,it);
02534 yq = static_cast<float>(inr);
02535
02536 l = numr(3,it);
02537 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02538 else lt = l / 4;
02539
02540 nsim = lt - 1;
02541 dfi = dpi / (nsim+1);
02542 kcirc = numr(2,it);
02543 xold = 0.0f;
02544 yold = static_cast<float>(inr);
02545 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02546
02547
02548 xold = static_cast<float>(inr);
02549 yold = 0.0f;
02550 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02551
02552
02553 if ( mode == 'f' || mode == 'F' ) {
02554 xold = 0.0f;
02555 yold = static_cast<float>(-inr);
02556 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02557
02558
02559 xold = static_cast<float>(-inr);
02560 yold = 0.0f;
02561 circ(lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02562
02563 }
02564
02565 for (jt=1;jt<=nsim;jt++) {
02566 fi = static_cast<float>(dfi * jt);
02567 x = sin(fi) * yq;
02568 y = cos(fi) * yq;
02569
02570 xold = x;
02571 yold = y;
02572 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02573
02574
02575 xold = y;
02576 yold = -x;
02577 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02578
02579
02580 if ( mode == 'f' || mode == 'F' ) {
02581 xold = -x;
02582 yold = -y;
02583 circ(jt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02584
02585
02586 xold = -y;
02587 yold = x;
02588 circ(jt+lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02589
02590 }
02591 }
02592 }
02593 return out;
02594 }
02595
02596
02597
02598
02599
02600
02601
02602
02603
02604
02605
02606
02607
02608
02609
02610
02611
02612
02613
02614
02615
02616
02617
02618
02619 #define tab1(i) tab1[i-1]
02620 #define xcmplx(i,j) xcmplx [(j-1)*2 + i-1]
02621 #define br(i) br[i-1]
02622 #define bi(i) bi[i-1]
02623
02624 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02625 {
02626 double rni,sgn,tr1,tr2,ti1,ti2;
02627 double cc,c,ss,s,t,x2,x3,x4,x5;
02628 int b3,b4,b5,b6,b7,b56;
02629 int n, k, l, j, i, ix0, ix1, status=0;
02630
02631 const double tab1[] = {
02632 9.58737990959775e-5,
02633 1.91747597310703e-4,
02634 3.83495187571395e-4,
02635 7.66990318742704e-4,
02636 1.53398018628476e-3,
02637 3.06795676296598e-3,
02638 6.13588464915449e-3,
02639 1.22715382857199e-2,
02640 2.45412285229123e-2,
02641 4.90676743274181e-2,
02642 9.80171403295604e-2,
02643 1.95090322016128e-1,
02644 3.82683432365090e-1,
02645 7.07106781186546e-1,
02646 1.00000000000000,
02647 };
02648
02649 n=(int)pow(2.0f,ln);
02650
02651 k=abs(ks);
02652 l=16-ln;
02653 b3=n*k;
02654 b6=b3;
02655 b7=k;
02656 if (ks > 0) {
02657 sgn=1.0f;
02658 } else {
02659 sgn=-1.0f;
02660 rni=1.0f/(float)(n);
02661 j=1;
02662 for (i=1; i<=n; i++) {
02663 br(j)=br(j)*rni;
02664 bi(j)=bi(j)*rni;
02665 j=j+k;
02666 }
02667 }
02668
02669 L12:
02670 b6=b6/2;
02671 b5=b6;
02672 b4=2*b6;
02673 b56=b5-b6;
02674
02675 L14:
02676 tr1=br(b5+1);
02677 ti1=bi(b5+1);
02678 tr2=br(b56+1);
02679 ti2=bi(b56+1);
02680
02681 br(b5+1)=tr2-tr1;
02682 bi(b5+1)=ti2-ti1;
02683 br(b56+1)=tr1+tr2;
02684 bi(b56+1)=ti1+ti2;
02685
02686 b5=b5+b4;
02687 b56=b5-b6;
02688 if ( b5 <= b3 ) goto L14;
02689 if ( b6 == b7 ) goto L20;
02690
02691 b4=b7;
02692 cc=2.0f*pow(tab1(l),2);
02693 c=1.0f-cc;
02694 l++;
02695 ss=sgn*tab1(l);
02696 s=ss;
02697
02698 L16:
02699 b5=b6+b4;
02700 b4=2*b6;
02701 b56=b5-b6;
02702
02703 L18:
02704 tr1=br(b5+1);
02705 ti1=bi(b5+1);
02706 tr2=br(b56+1);
02707 ti2=bi(b56+1);
02708 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02709 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02710 br(b56+1)=tr1+tr2;
02711 bi(b56+1)=ti1+ti2;
02712
02713 b5=b5+b4;
02714 b56=b5-b6;
02715 if ( b5 <= b3 ) goto L18;
02716 b4=b5-b6;
02717 b5=b4-b3;
02718 c=-c;
02719 b4=b6-b5;
02720 if ( b5 < b4 ) goto L16;
02721 b4=b4+b7;
02722 if ( b4 >= b5 ) goto L12;
02723
02724 t=c-cc*c-ss*s;
02725 s=s+ss*c-cc*s;
02726 c=t;
02727 goto L16;
02728
02729 L20:
02730 ix0=b3/2;
02731 b3=b3-b7;
02732 b4=0;
02733 b5=0;
02734 b6=ix0;
02735 ix1=0;
02736 if (b6 == b7) goto EXIT;
02737
02738 L22:
02739 b4=b3-b4;
02740 b5=b3-b5;
02741 x2=br(b4+1);
02742 x3=br(b5+1);
02743 x4=bi(b4+1);
02744 x5=bi(b5+1);
02745 br(b4+1)=x3;
02746 br(b5+1)=x2;
02747 bi(b4+1)=x5;
02748 bi(b5+1)=x4;
02749 if(b6 < b4) goto L22;
02750
02751 L24:
02752 b4=b4+b7;
02753 b5=b6+b5;
02754 x2=br(b4+1);
02755 x3=br(b5+1);
02756 x4=bi(b4+1);
02757 x5=bi(b5+1);
02758 br(b4+1)=x3;
02759 br(b5+1)=x2;
02760 bi(b4+1)=x5;
02761 bi(b5+1)=x4;
02762 ix0=b6;
02763
02764 L26:
02765 ix0=ix0/2;
02766 ix1=ix1-ix0;
02767 if( ix1 >= 0) goto L26;
02768
02769 ix0=2*ix0;
02770 b4=b4+b7;
02771 ix1=ix1+ix0;
02772 b5=ix1;
02773 if ( b5 >= b4) goto L22;
02774 if ( b4 < b6) goto L24;
02775
02776 EXIT:
02777 status = 0;
02778 }
02779
02780
02781 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02782 {
02783
02784
02785 int b3,b4,b5,b6,b7,b56;
02786 int n, k, l, j, i, ix0, ix1;
02787 float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02788 int status=0;
02789
02790 const float tab1[] = {
02791 9.58737990959775e-5f,
02792 1.91747597310703e-4f,
02793 3.83495187571395e-4f,
02794 7.66990318742704e-4f,
02795 1.53398018628476e-3f,
02796 3.06795676296598e-3f,
02797 6.13588464915449e-3f,
02798 1.22715382857199e-2f,
02799 2.45412285229123e-2f,
02800 4.90676743274181e-2f,
02801 9.80171403295604e-2f,
02802 1.95090322016128e-1f,
02803 3.82683432365090e-1f,
02804 7.07106781186546e-1f,
02805 1.00000000000000f,
02806 };
02807
02808 n=(int)pow(2.0f,ln);
02809
02810 k=abs(ks);
02811 l=16-ln;
02812 b3=n*k;
02813 b6=b3;
02814 b7=k;
02815 if( ks > 0 ) {
02816 sgn=1.0f;
02817 } else {
02818 sgn=-1.0f;
02819 rni=1.0f/(float)n;
02820 j=1;
02821 for (i=1; i<=n; i++) {
02822 br(j)=br(j)*rni;
02823 bi(j)=bi(j)*rni;
02824 j=j+k;
02825 }
02826 }
02827 L12:
02828 b6=b6/2;
02829 b5=b6;
02830 b4=2*b6;
02831 b56=b5-b6;
02832 L14:
02833 tr1=br(b5+1);
02834 ti1=bi(b5+1);
02835
02836 tr2=br(b56+1);
02837 ti2=bi(b56+1);
02838
02839 br(b5+1)=tr2-tr1;
02840 bi(b5+1)=ti2-ti1;
02841 br(b56+1)=tr1+tr2;
02842 bi(b56+1)=ti1+ti2;
02843
02844 b5=b5+b4;
02845 b56=b5-b6;
02846 if ( b5 <= b3 ) goto L14;
02847 if ( b6 == b7 ) goto L20;
02848
02849 b4=b7;
02850 cc=2.0f*pow(tab1(l),2);
02851 c=1.0f-cc;
02852 l++;
02853 ss=sgn*tab1(l);
02854 s=ss;
02855 L16:
02856 b5=b6+b4;
02857 b4=2*b6;
02858 b56=b5-b6;
02859 L18:
02860 tr1=br(b5+1);
02861 ti1=bi(b5+1);
02862 tr2=br(b56+1);
02863 ti2=bi(b56+1);
02864 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02865 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02866 br(b56+1)=tr1+tr2;
02867 bi(b56+1)=ti1+ti2;
02868
02869 b5=b5+b4;
02870 b56=b5-b6;
02871 if(b5 <= b3) goto L18;
02872 b4=b5-b6;
02873 b5=b4-b3;
02874 c=-c;
02875 b4=b6-b5;
02876 if(b5 < b4) goto L16;
02877 b4=b4+b7;
02878 if(b4 >= b5) goto L12;
02879
02880 t=c-cc*c-ss*s;
02881 s=s+ss*c-cc*s;
02882 c=t;
02883 goto L16;
02884 L20:
02885 ix0=b3/2;
02886 b3=b3-b7;
02887 b4=0;
02888 b5=0;
02889 b6=ix0;
02890 ix1=0;
02891 if ( b6 == b7) goto EXIT;
02892 L22:
02893 b4=b3-b4;
02894 b5=b3-b5;
02895 x2=br(b4+1);
02896 x3=br(b5+1);
02897 x4=bi(b4+1);
02898 x5=bi(b5+1);
02899 br(b4+1)=x3;
02900 br(b5+1)=x2;
02901 bi(b4+1)=x5;
02902 bi(b5+1)=x4;
02903 if (b6 < b4) goto L22;
02904 L24:
02905 b4=b4+b7;
02906 b5=b6+b5;
02907 x2=br(b4+1);
02908 x3=br(b5+1);
02909 x4=bi(b4+1);
02910 x5=bi(b5+1);
02911 br(b4+1)=x3;
02912 br(b5+1)=x2;
02913 bi(b4+1)=x5;
02914 bi(b5+1)=x4;
02915 ix0=b6;
02916 L26:
02917 ix0=ix0/2;
02918 ix1=ix1-ix0;
02919 if(ix1 >= 0) goto L26;
02920
02921 ix0=2*ix0;
02922 b4=b4+b7;
02923 ix1=ix1+ix0;
02924 b5=ix1;
02925 if (b5 >= b4) goto L22;
02926 if (b4 < b6) goto L24;
02927 EXIT:
02928 status = 0;
02929 }
02930
02931 void Util::fftr_q(float *xcmplx, int nv)
02932 {
02933
02934
02935 int nu, inv, nu1, n, isub, n2, i1, i2, i;
02936 float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02937
02938 const float tab1[] = {
02939 9.58737990959775e-5f,
02940 1.91747597310703e-4f,
02941 3.83495187571395e-4f,
02942 7.66990318742704e-4f,
02943 1.53398018628476e-3f,
02944 3.06795676296598e-3f,
02945 6.13588464915449e-3f,
02946 1.22715382857199e-2f,
02947 2.45412285229123e-2f,
02948 4.90676743274181e-2f,
02949 9.80171403295604e-2f,
02950 1.95090322016128e-1f,
02951 3.82683432365090e-1f,
02952 7.07106781186546e-1f,
02953 1.00000000000000f,
02954 };
02955
02956 nu=abs(nv);
02957 inv=nv/nu;
02958 nu1=nu-1;
02959 n=(int)pow(2.f,nu1);
02960 isub=16-nu1;
02961
02962 ss=-tab1(isub);
02963 cc=-2.0f*pow(tab1(isub-1),2.f);
02964 c=1.0f;
02965 s=0.0f;
02966 n2=n/2;
02967 if ( inv > 0) {
02968 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02969 tr=xcmplx(1,1);
02970 ti=xcmplx(2,1);
02971 xcmplx(1,1)=tr+ti;
02972 xcmplx(2,1)=tr-ti;
02973 for (i=1;i<=n2;i++) {
02974 i1=i+1;
02975 i2=n-i+1;
02976 tr1=xcmplx(1,i1);
02977 tr2=xcmplx(1,i2);
02978 ti1=xcmplx(2,i1);
02979 ti2=xcmplx(2,i2);
02980 t=(cc*c-ss*s)+c;
02981 s=(cc*s+ss*c)+s;
02982 c=t;
02983 xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02984 xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02985 xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02986 xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02987 }
02988 } else {
02989 tr=xcmplx(1,1);
02990 ti=xcmplx(2,1);
02991 xcmplx(1,1)=0.5f*(tr+ti);
02992 xcmplx(2,1)=0.5f*(tr-ti);
02993 for (i=1; i<=n2; i++) {
02994 i1=i+1;
02995 i2=n-i+1;
02996 tr1=xcmplx(1,i1);
02997 tr2=xcmplx(1,i2);
02998 ti1=xcmplx(2,i1);
02999 ti2=xcmplx(2,i2);
03000 t=(cc*c-ss*s)+c;
03001 s=(cc*s+ss*c)+s;
03002 c=t;
03003 xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03004 xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03005 xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03006 xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03007 }
03008 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03009 }
03010 }
03011
03012
03013 void Util::fftr_d(double *xcmplx, int nv)
03014 {
03015
03016 int i1, i2, nu, inv, nu1, n, isub, n2, i;
03017 double tr1,tr2,ti1,ti2,tr,ti;
03018 double cc,c,ss,s,t;
03019 const double tab1[] = {
03020 9.58737990959775e-5,
03021 1.91747597310703e-4,
03022 3.83495187571395e-4,
03023 7.66990318742704e-4,
03024 1.53398018628476e-3,
03025 3.06795676296598e-3,
03026 6.13588464915449e-3,
03027 1.22715382857199e-2,
03028 2.45412285229123e-2,
03029 4.90676743274181e-2,
03030 9.80171403295604e-2,
03031 1.95090322016128e-1,
03032 3.82683432365090e-1,
03033 7.07106781186546e-1,
03034 1.00000000000000,
03035 };
03036
03037 nu=abs(nv);
03038 inv=nv/nu;
03039 nu1=nu-1;
03040 n=(int)pow(2.0f,nu1);
03041 isub=16-nu1;
03042 ss=-tab1(isub);
03043 cc=-2.0*pow(tab1(isub-1),2);
03044 c=1.0f;
03045 s=0.0f;
03046 n2=n/2;
03047
03048 if ( inv > 0 ) {
03049 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03050 tr=xcmplx(1,1);
03051 ti=xcmplx(2,1);
03052 xcmplx(1,1)=tr+ti;
03053 xcmplx(2,1)=tr-ti;
03054 for (i=1;i<=n2;i++) {
03055 i1=i+1;
03056 i2=n-i+1;
03057 tr1=xcmplx(1,i1);
03058 tr2=xcmplx(1,i2);
03059 ti1=xcmplx(2,i1);
03060 ti2=xcmplx(2,i2);
03061 t=(cc*c-ss*s)+c;
03062 s=(cc*s+ss*c)+s;
03063 c=t;
03064 xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03065 xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03066 xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03067 xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03068 }
03069 } else {
03070 tr=xcmplx(1,1);
03071 ti=xcmplx(2,1);
03072 xcmplx(1,1)=0.5*(tr+ti);
03073 xcmplx(2,1)=0.5*(tr-ti);
03074 for (i=1; i<=n2; i++) {
03075 i1=i+1;
03076 i2=n-i+1;
03077 tr1=xcmplx(1,i1);
03078 tr2=xcmplx(1,i2);
03079 ti1=xcmplx(2,i1);
03080 ti2=xcmplx(2,i2);
03081 t=(cc*c-ss*s)+c;
03082 s=(cc*s+ss*c)+s;
03083 c=t;
03084 xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03085 xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03086 xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03087 xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03088 }
03089 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03090 }
03091 }
03092 #undef tab1
03093 #undef xcmplx
03094 #undef br
03095 #undef bi
03096
03097
03098 void Util::Frngs(EMData* circp, vector<int> numr){
03099 int nring = numr.size()/3;
03100 float *circ = circp->get_data();
03101 int i, l;
03102 for (i=1; i<=nring;i++) {
03103
03104 #ifdef _WIN32
03105 l = (int)( log((float)numr(3,i))/log(2.0f) );
03106 #else
03107 l=(int)(log2(numr(3,i)));
03108 #endif //_WIN32
03109
03110 fftr_q(&circ(numr(2,i)),l);
03111 }
03112 }
03113
03114 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03115 int nring = numr.size()/3;
03116 float *circ = circp->get_data();
03117 int i, l;
03118 for (i=1; i<=nring;i++) {
03119
03120 #ifdef _WIN32
03121 l = (int)( log((float)numr(3,i))/log(2.0f) );
03122 #else
03123 l=(int)(log2(numr(3,i)));
03124 #endif //_WIN32
03125
03126 fftr_q(&circ(numr(2,i)),-l);
03127 }
03128 }
03129 #undef circ
03130
03131 void Util::Applyws(EMData* circp, vector<int> numr, vector<float> wr)
03132 {
03133
03134
03135 const int nring = numr.size() / 3;
03136 const int maxrin = numr.back();
03137 float *circ = circp->get_data();
03138 for (int i = 0; i < nring; ++i) {
03139 const int numr3i = numr[2+i*3];
03140 const int numr2i = numr[1+i*3]-1;
03141 const float w = wr[i];
03142 circ[numr2i] *= w;
03143 if (numr3i == maxrin) {
03144 circ[numr2i+1] *= w;
03145 } else {
03146 circ[numr2i+1] *= 0.5*w;
03147 }
03148 for (int j = 2+numr2i; j < numr3i+numr2i; ++j) {
03149 circ[j] *= w;
03150 }
03151 }
03152 }
03153
03154 #define b(i) b[i-1]
03155 void Util::prb1d(double *b, int npoint, float *pos) {
03156 double c2,c3;
03157 int nhalf;
03158
03159 nhalf = npoint/2 + 1;
03160 *pos = 0.0;
03161
03162 if (npoint == 7) {
03163 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03164 - 6.*b(6) + 31.*b(7);
03165 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03166 }
03167 else if (npoint == 5) {
03168 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03169 + 46.*b(5) ) / (-70.);
03170 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03171 }
03172 else if (npoint == 3) {
03173 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03174 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03175 }
03176
03177 else {
03178 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03179 - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03180 + 1092.*b(9) ) / (-4620.);
03181 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03182 - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03183 }
03184 if (c3 != 0.0) *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03185 }
03186 #undef b
03187
03188 #define circ1(i) circ1[i-1]
03189 #define circ2(i) circ2[i-1]
03190 #define t(i) t[i-1]
03191 #define q(i) q[i-1]
03192 #define b(i) b[i-1]
03193 #define t7(i) t7[i-1]
03194 Dict Util::Crosrng_e(EMData* circ1p, EMData* circ2p, vector<int> numr, int neg) {
03195
03196 int nring = numr.size()/3;
03197
03198 int maxrin = numr[numr.size()-1];
03199 double qn; float tot;
03200 float *circ1 = circ1p->get_data();
03201 float *circ2 = circ2p->get_data();
03202
03203
03204
03205
03206
03207
03208
03209
03210
03211
03212
03213 float *t;
03214 double t7[7], *q;
03215 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03216 float pos;
03217
03218 #ifdef _WIN32
03219 ip = -(int)(log((float)maxrin)/log(2.0f));
03220 #else
03221 ip = -(int) (log2(maxrin));
03222 #endif //_WIN32
03223
03224 q = (double*)calloc(maxrin, sizeof(double));
03225 t = (float*)calloc(maxrin, sizeof(float));
03226
03227
03228 for (i=1; i<=nring; i++) {
03229 numr3i = numr(3,i);
03230 numr2i = numr(2,i);
03231
03232 t(1) = (circ1(numr2i)) * circ2(numr2i);
03233
03234 if (numr3i != maxrin) {
03235
03236 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03237 t(2) = 0.0;
03238
03239 if (neg) {
03240
03241 for (j=3;j<=numr3i;j=j+2) {
03242 jc = j+numr2i-1;
03243 t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03244 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03245 }
03246 } else {
03247 for (j=3;j<=numr3i;j=j+2) {
03248 jc = j+numr2i-1;
03249 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03250 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03251 }
03252 }
03253 for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03254 } else {
03255 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03256 if (neg) {
03257
03258 for (j=3;j<=maxrin;j=j+2) {
03259 jc = j+numr2i-1;
03260 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03261 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03262 }
03263 } else {
03264 for (j=3;j<=maxrin;j=j+2) {
03265 jc = j+numr2i-1;
03266 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03267 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03268 }
03269 }
03270 for (j = 1; j <= maxrin; j++) q(j) += t(j);
03271 }
03272 }
03273
03274 fftr_d(q,ip);
03275
03276 qn = -1.0e20;
03277 for (j=1;j<=maxrin;j++) {
03278 if (q(j) >= qn) {
03279 qn = q(j); jtot = j;
03280 }
03281 }
03282
03283 for (k=-3; k<=3; k++) {
03284 j = (jtot+k+maxrin-1)%maxrin + 1;
03285 t7(k+4) = q(j);
03286 }
03287
03288 prb1d(t7,7,&pos);
03289
03290 tot = (float)jtot + pos;
03291
03292 if (q) free(q);
03293 if (t) free(t);
03294
03295 Dict retvals;
03296 retvals["qn"] = qn;
03297 retvals["tot"] = tot;
03298 return retvals;
03299 }
03300
03301 Dict Util::Crosrng_ew(EMData* circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03302
03303 int nring = numr.size()/3;
03304
03305 int maxrin = numr[numr.size()-1];
03306 double qn; float tot;
03307 float *circ1 = circ1p->get_data();
03308 float *circ2 = circ2p->get_data();
03309
03310
03311
03312
03313
03314
03315
03316
03317
03318
03319
03320 float *t;
03321 double t7[7], *q;
03322 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03323 float pos;
03324
03325 #ifdef _WIN32
03326 ip = -(int)(log((float)maxrin)/log(2.0f));
03327 #else
03328 ip = -(int) (log2(maxrin));
03329 #endif //_WIN32
03330
03331 q = (double*)calloc(maxrin, sizeof(double));
03332 t = (float*)calloc(maxrin, sizeof(float));
03333
03334
03335 for (i=1;i<=nring;i++) {
03336 numr3i = numr(3,i);
03337 numr2i = numr(2,i);
03338
03339 t(1) = circ1(numr2i) * circ2(numr2i);
03340
03341 if (numr3i != maxrin) {
03342
03343 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03344 t(2) = 0.0;
03345
03346 if (neg) {
03347
03348 for (j=3; j<=numr3i; j=j+2) {
03349 jc = j+numr2i-1;
03350 t(j) = (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03351 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03352 }
03353 } else {
03354 for (j=3; j<=numr3i; j=j+2) {
03355 jc = j+numr2i-1;
03356 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03357 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03358 }
03359 }
03360 for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03361 } else {
03362 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03363 if (neg) {
03364
03365 for (j=3; j<=maxrin; j=j+2) {
03366 jc = j+numr2i-1;
03367 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03368 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03369 }
03370 } else {
03371 for (j=3; j<=maxrin; j=j+2) {
03372 jc = j+numr2i-1;
03373 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03374 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03375 }
03376 }
03377 for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03378 }
03379 }
03380
03381 fftr_d(q,ip);
03382
03383 qn = -1.0e20;
03384 for (j=1;j<=maxrin;j++) {
03385
03386 if (q(j) >= qn) {
03387 qn = q(j);
03388 jtot = j;
03389 }
03390 }
03391
03392 for (k=-3; k<=3; k++) {
03393 j = (jtot+k+maxrin-1)%maxrin + 1;
03394 t7(k+4) = q(j);
03395 }
03396
03397 prb1d(t7,7,&pos);
03398
03399 tot = (float)jtot + pos;
03400
03401
03402 if (t) free(t);
03403
03404 Dict retvals;
03405
03406
03407 retvals["qn"] = qn;
03408 retvals["tot"] = tot;
03409
03410 if (q) free(q);
03411
03412 return retvals;
03413 }
03414
03415 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03416 int nring = numr.size()/3;
03417
03418 int maxrin = numr[numr.size()-1];
03419 double qn; float tot; double qm; float tmt;
03420 float *circ1 = circ1p->get_data();
03421 float *circ2 = circ2p->get_data();
03422
03423
03424
03425
03426
03427
03428
03429
03430
03431
03432
03433
03434 double *t, *q, t7[7];
03435
03436 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03437 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03438
03439 qn = 0.0f;
03440 qm = 0.0f;
03441 tot = 0.0f;
03442 tmt = 0.0f;
03443 #ifdef _WIN32
03444 ip = -(int)(log((float)maxrin)/log(2.0f));
03445 #else
03446 ip = -(int)(log2(maxrin));
03447 #endif //_WIN32
03448
03449
03450
03451
03452
03453 q = (double*)calloc(maxrin,sizeof(double));
03454
03455
03456
03457 t = (double*)calloc(maxrin,sizeof(double));
03458
03459
03460 for (i=1; i<=nring; i++) {
03461
03462 numr3i = numr(3,i);
03463 numr2i = numr(2,i);
03464
03465 t1 = circ1(numr2i) * circ2(numr2i);
03466 q(1) += t1;
03467 t(1) += t1;
03468
03469 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03470 if (numr3i == maxrin) {
03471 q(2) += t1;
03472 t(2) += t1;
03473 } else {
03474 q(numr3i+1) += t1;
03475 t(numr3i+1) += t1;
03476 }
03477
03478 for (j=3; j<=numr3i; j += 2) {
03479 jc = j+numr2i-1;
03480
03481
03482
03483
03484
03485
03486
03487
03488 c1 = circ1(jc);
03489 c2 = circ1(jc+1);
03490 d1 = circ2(jc);
03491 d2 = circ2(jc+1);
03492
03493 t1 = c1 * d1;
03494 t2 = c2 * d2;
03495 t3 = c1 * d2;
03496 t4 = c2 * d1;
03497
03498 q(j) += t1 + t2;
03499 q(j+1) += -t3 + t4;
03500 t(j) += t1 - t2;
03501 t(j+1) += -t3 - t4;
03502 }
03503 }
03504
03505 fftr_d(q,ip);
03506
03507 qn = -1.0e20;
03508 for (j=1; j<=maxrin; j++) {
03509 if (q(j) >= qn) {
03510 qn = q(j);
03511 jtot = j;
03512 }
03513 }
03514
03515 for (k=-3; k<=3; k++) {
03516 j = ((jtot+k+maxrin-1)%maxrin)+1;
03517 t7(k+4) = q(j);
03518 }
03519
03520
03521 prb1d(t7,7,&pos);
03522 tot = (float)(jtot)+pos;
03523
03524
03525
03526
03527 fftr_d(t,ip);
03528
03529
03530 qm = -1.0e20;
03531 for (j=1; j<=maxrin;j++) {
03532 if ( t(j) >= qm ) {
03533 qm = t(j);
03534 jtot = j;
03535 }
03536 }
03537
03538 for (k=-3; k<=3; k++) {
03539 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03540 t7(k+4) = t(j);
03541 }
03542
03543
03544
03545 prb1d(t7,7,&pos);
03546 tmt = float(jtot) + pos;
03547
03548
03549
03550 free(t);
03551 free(q);
03552
03553 Dict retvals;
03554 retvals["qn"] = qn;
03555 retvals["tot"] = tot;
03556 retvals["qm"] = qm;
03557 retvals["tmt"] = tmt;
03558 return retvals;
03559 }
03560
03561 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03562 int nring = numr.size()/3;
03563
03564 int maxrin = numr[numr.size()-1];
03565 double qn; float tot; double qm; float tmt;
03566 float *circ1 = circ1p->get_data();
03567 float *circ2 = circ2p->get_data();
03568
03569
03570
03571
03572
03573
03574
03575
03576
03577
03578
03579
03580 double *t, *q;
03581
03582 int ip, jc, numr3i, numr2i, i, j, jtot = 0;
03583 float t1, t2, t3, t4, c1, c2, d1, d2;
03584
03585 qn = 0.0f;
03586 qm = 0.0f;
03587 tot = 0.0f;
03588 tmt = 0.0f;
03589 #ifdef _WIN32
03590 ip = -(int)(log((float)maxrin)/log(2.0f));
03591 #else
03592 ip = -(int)(log2(maxrin));
03593 #endif //_WIN32
03594
03595
03596
03597
03598
03599 q = (double*)calloc(maxrin,sizeof(double));
03600
03601
03602
03603 t = (double*)calloc(maxrin,sizeof(double));
03604
03605
03606 for (i=1; i<=nring; i++) {
03607
03608 numr3i = numr(3,i);
03609 numr2i = numr(2,i);
03610
03611 t1 = circ1(numr2i) * circ2(numr2i);
03612 q(1) += t1;
03613 t(1) += t1;
03614
03615 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03616 if (numr3i == maxrin) {
03617 q(2) += t1;
03618 t(2) += t1;
03619 } else {
03620 q(numr3i+1) += t1;
03621 t(numr3i+1) += t1;
03622 }
03623
03624 for (j=3; j<=numr3i; j += 2) {
03625 jc = j+numr2i-1;
03626
03627
03628
03629
03630
03631
03632
03633
03634 c1 = circ1(jc);
03635 c2 = circ1(jc+1);
03636 d1 = circ2(jc);
03637 d2 = circ2(jc+1);
03638
03639 t1 = c1 * d1;
03640 t2 = c2 * d2;
03641 t3 = c1 * d2;
03642 t4 = c2 * d1;
03643
03644 q(j) += t1 + t2;
03645 q(j+1) += -t3 + t4;
03646 t(j) += t1 - t2;
03647 t(j+1) += -t3 - t4;
03648 }
03649 }
03650
03651 fftr_d(q,ip);
03652
03653 qn = -1.0e20;
03654
03655 int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03656 int jstep = static_cast<int>(delta/360.0*maxrin);
03657 if (jstep < 1) { jstep = 1; }
03658
03659 for (j=jstart; j<=maxrin; j+=jstep) {
03660 if (q(j) >= qn) {
03661 qn = q(j);
03662 jtot = j;
03663 }
03664 }
03665
03666
03667
03668
03669
03670
03671
03672
03673
03674
03675 tot = (float)(jtot);
03676
03677
03678 fftr_d(t,ip);
03679
03680
03681 qm = -1.0e20;
03682 for (j=jstart; j<=maxrin;j+=jstep) {
03683 if ( t(j) >= qm ) {
03684 qm = t(j);
03685 jtot = j;
03686 }
03687 }
03688
03689
03690
03691
03692
03693
03694
03695
03696
03697
03698
03699 tmt = float(jtot);
03700
03701 free(t);
03702 free(q);
03703
03704 Dict retvals;
03705 retvals["qn"] = qn;
03706 retvals["tot"] = tot;
03707 retvals["qm"] = qm;
03708 retvals["tmt"] = tmt;
03709 return retvals;
03710 }
03711
03712
03713 Dict Util::Crosrng_psi_0_180_no_mirror(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03714 int nring = numr.size()/3;
03715
03716 int maxrin = numr[numr.size()-1];
03717 double qn; float tot;
03718 float *circ1 = circ1p->get_data();
03719 float *circ2 = circ2p->get_data();
03720
03721
03722
03723
03724 double *q, t7[7];
03725
03726 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03727 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03728
03729 qn = 0.0f;
03730 tot = 0.0f;
03731 #ifdef _WIN32
03732 ip = -(int)(log((float)maxrin)/log(2.0f));
03733 #else
03734 ip = -(int)(log2(maxrin));
03735 #endif //_WIN32
03736
03737
03738
03739
03740
03741 q = (double*)calloc(maxrin,sizeof(double));
03742
03743
03744 for (i=1; i<=nring; i++) {
03745
03746 numr3i = numr(3,i);
03747 numr2i = numr(2,i);
03748
03749 t1 = circ1(numr2i) * circ2(numr2i);
03750 q(1) += t1;
03751
03752 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03753 if (numr3i == maxrin) {
03754 q(2) += t1;
03755
03756 } else {
03757 q(numr3i+1) += t1;
03758 }
03759
03760 for (j=3; j<=numr3i; j += 2) {
03761 jc = j+numr2i-1;
03762
03763
03764
03765
03766
03767
03768
03769
03770 c1 = circ1(jc);
03771 c2 = circ1(jc+1);
03772 d1 = circ2(jc);
03773 d2 = circ2(jc+1);
03774
03775 t1 = c1 * d1;
03776 t2 = c2 * d2;
03777 t3 = c1 * d2;
03778 t4 = c2 * d1;
03779
03780 q(j) += t1 + t2;
03781 q(j+1) += -t3 + t4;
03782
03783 }
03784 }
03785
03786
03787 fftr_d(q,ip);
03788
03789 int psi_range = int(psi_max/360.0*maxrin+0.5);
03790 const int psi_0 = 0;
03791 int psi_180 = int( 180.0/360.0*maxrin+0.5);
03792
03793 qn = -1.0e20;
03794 for (k=-psi_range; k<=psi_range; k++) {
03795 j = (k+psi_0+maxrin-1)%maxrin+1;
03796 if (q(j) >= qn) {
03797 qn = q(j);
03798 jtot = j;
03799 }
03800 }
03801
03802 for (k=-psi_range; k<=psi_range; k++) {
03803 j = (k+psi_180+maxrin-1)%maxrin+1;
03804 if (q(j) >= qn) {
03805 qn = q(j);
03806 jtot = j;
03807 }
03808 }
03809
03810 for (k=-3; k<=3; k++) {
03811 j = ((jtot+k+maxrin-1)%maxrin)+1;
03812 t7(k+4) = q(j);
03813 }
03814
03815
03816 prb1d(t7,7,&pos);
03817 tot = (float)(jtot)+pos;
03818
03819
03820
03821 free(q);
03822
03823 Dict retvals;
03824 retvals["qn"] = qn;
03825 retvals["tot"] = tot;
03826
03827 return retvals;
03828 }
03829
03830
03831
03832 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag, float psi_max) {
03833
03834
03835 int nring = numr.size()/3;
03836 int maxrin = numr[numr.size()-1];
03837 double qn; float tot;
03838 float *circ1 = circ1p->get_data();
03839 float *circ2 = circ2p->get_data();
03840
03841 double *q;
03842
03843 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03844 float t1, t2, t3, t4, c1, c2, d1, d2;
03845
03846 qn = 0.0f;
03847 tot = 0.0f;
03848 #ifdef _WIN32
03849 ip = -(int)(log((float)maxrin)/log(2.0f));
03850 #else
03851 ip = -(int)(log2(maxrin));
03852 #endif //_WIN32
03853
03854
03855
03856
03857 q = (double*)calloc(maxrin,sizeof(double));
03858 int neg = 1-2*flag;
03859
03860
03861 for (i=1; i<=nring; i++) {
03862
03863 numr3i = numr(3,i);
03864 numr2i = numr(2,i);
03865
03866 t1 = circ1(numr2i) * circ2(numr2i);
03867 q(1) += t1;
03868
03869 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03870 if (numr3i == maxrin) {
03871 q(2) += t1;
03872 } else {
03873 q(numr3i+1) += t1;
03874 }
03875
03876 for (j=3; j<=numr3i; j += 2) {
03877 jc = j+numr2i-1;
03878
03879
03880
03881
03882
03883 c1 = circ1(jc);
03884 c2 = circ1(jc+1);
03885 d1 = circ2(jc);
03886 d2 = circ2(jc+1);
03887
03888 t1 = c1 * d1;
03889 t3 = c1 * d2;
03890 t2 = c2 * d2;
03891 t4 = c2 * d1;
03892
03893 q(j) += t1 + t2*neg;
03894 q(j+1) += -t3 + t4*neg;
03895 }
03896 }
03897
03898 fftr_d(q,ip);
03899
03900 qn = -1.0e20;
03901 int psi_pos = int(psi/360.0*maxrin+0.5);
03902 const int psi_range = int(psi_max/360.0*maxrin + 0.5);
03903
03904 for (k=-psi_range; k<=psi_range; k++) {
03905 j = ( k + psi_pos + maxrin -1 )%maxrin+1;
03906 if (q(j) >= qn) {
03907 qn = q(j);
03908 jtot = j;
03909 }
03910 }
03911
03912 tot = (float)(jtot);
03913 free(q);
03914
03915 Dict retvals;
03916 retvals["qn"] = qn;
03917 retvals["tot"] = tot;
03918 return retvals;
03919 }
03920
03921 Dict Util::Crosrng_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, float psi_max) {
03922
03923
03924 int nring = numr.size()/3;
03925 int maxrin = numr[numr.size()-1];
03926 double qn; float tot; double qm; float tmt;
03927 float *circ1 = circ1p->get_data();
03928 float *circ2 = circ2p->get_data();
03929
03930 double *t, *q;
03931
03932 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03933 float t1, t2, t3, t4, c1, c2, d1, d2;
03934
03935 qn = 0.0f;
03936 qm = 0.0f;
03937 tot = 0.0f;
03938 tmt = 0.0f;
03939 #ifdef _WIN32
03940 ip = -(int)(log((float)maxrin)/log(2.0f));
03941 #else
03942 ip = -(int)(log2(maxrin));
03943 #endif //_WIN32
03944
03945
03946
03947
03948 q = (double*)calloc(maxrin,sizeof(double));
03949
03950
03951
03952 t = (double*)calloc(maxrin,sizeof(double));
03953
03954
03955
03956 for (i=1; i<=nring; i++) {
03957
03958 numr3i = numr(3,i);
03959 numr2i = numr(2,i);
03960
03961 t1 = circ1(numr2i) * circ2(numr2i);
03962 q(1) += t1;
03963 t(1) += t1;
03964
03965 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03966 if (numr3i == maxrin) {
03967 q(2) += t1;
03968 t(2) += t1;
03969 } else {
03970 q(numr3i+1) += t1;
03971 t(numr3i+1) += t1;
03972 }
03973
03974 for (j=3; j<=numr3i; j += 2) {
03975 jc = j+numr2i-1;
03976
03977
03978
03979
03980
03981 c1 = circ1(jc);
03982 c2 = circ1(jc+1);
03983 d1 = circ2(jc);
03984 d2 = circ2(jc+1);
03985
03986 t1 = c1 * d1;
03987 t3 = c1 * d2;
03988 t2 = c2 * d2;
03989 t4 = c2 * d1;
03990
03991 q(j) += t1 + t2;
03992 q(j+1) += -t3 + t4;
03993 t(j) += t1 - t2;
03994 t(j+1) += -t3 - t4;
03995 }
03996 }
03997
03998 fftr_d(q,ip);
03999
04000 qn = -1.0e20;
04001 int psi_pos = int(psi/360.0*maxrin+0.5);
04002 const int psi_range = int(psi_max/360.0*maxrin + 0.5);
04003
04004 for (k=-psi_range; k<=psi_range; k++) {
04005 j = (k+psi_pos+maxrin-1)%maxrin+1;
04006 if (q(j) >= qn) {
04007 qn = q(j);
04008 jtot = j;
04009 }
04010 }
04011
04012 tot = (float)(jtot);
04013 free(q);
04014
04015
04016 fftr_d(t,ip);
04017
04018 qm = -1.0e20;
04019
04020 for (k=-psi_range; k<=psi_range; k++) {
04021 j = (k+psi_pos+maxrin-1)%maxrin+1;
04022 if (t(j) >= qm) {
04023 qm = t(j);
04024 jtot = j;
04025 }
04026 }
04027
04028 tmt = (float)(jtot);
04029 free(t);
04030
04031 Dict retvals;
04032 retvals["qn"] = qn;
04033 retvals["tot"] = tot;
04034 retvals["qm"] = qm;
04035 retvals["tmt"] = tmt;
04036 return retvals;
04037 }
04038
04039 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
04040 int nring = numr.size()/3;
04041
04042 int maxrin = numr[numr.size()-1];
04043 double qn; float tot;
04044 float *circ1 = circ1p->get_data();
04045 float *circ2 = circ2p->get_data();
04046
04047
04048
04049
04050
04051
04052
04053
04054
04055
04056
04057
04058 double *q, t7[7];
04059
04060 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04061 float c1, c2, d1, d2, pos;
04062
04063 qn = 0.0;
04064 tot = 0.0;
04065 #ifdef _WIN32
04066 ip = -(int)(log((float)maxrin)/log(2.0f));
04067 #else
04068 ip = -(int)(log2(maxrin));
04069 #endif //_WIN32
04070
04071
04072
04073
04074
04075 q = (double*)calloc(maxrin,sizeof(double));
04076
04077
04078 for (i=1; i<=nring; i++) {
04079
04080 numr3i = numr(3,i);
04081 numr2i = numr(2,i);
04082
04083 q(1) += circ1(numr2i) * circ2(numr2i);
04084
04085 if (numr3i == maxrin) q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04086 else q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04087
04088 for (j=3; j<=numr3i; j += 2) {
04089 jc = j+numr2i-1;
04090
04091
04092
04093
04094
04095 c1 = circ1(jc);
04096 c2 = circ1(jc+1);
04097 d1 = circ2(jc);
04098 d2 = circ2(jc+1);
04099
04100 q(j) += c1 * d1 + c2 * d2;
04101 q(j+1) += -c1 * d2 + c2 * d1;
04102 }
04103 }
04104
04105 fftr_d(q,ip);
04106
04107 qn = -1.0e20;
04108 for (j=1; j<=maxrin; j++) {
04109 if (q(j) >= qn) {
04110 qn = q(j);
04111 jtot = j;
04112 }
04113 }
04114
04115 for (k=-3; k<=3; k++) {
04116 j = ((jtot+k+maxrin-1)%maxrin)+1;
04117 t7(k+4) = q(j);
04118 }
04119
04120
04121 prb1d(t7,7,&pos);
04122 tot = (float)(jtot)+pos;
04123
04124
04125
04126 free(q);
04127
04128 Dict retvals;
04129 retvals["qn"] = qn;
04130 retvals["tot"] = tot;
04131 return retvals;
04132 }
04133
04134 #define dout(i,j) dout[i+maxrin*j]
04135 #define circ1b(i) circ1b[i-1]
04136 #define circ2b(i) circ2b[i-1]
04137
04138 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04139
04140
04141
04142 int ip, jc, numr3i, numr2i, i, j;
04143 float t1, t2, t3, t4, c1, c2, d1, d2;
04144
04145 int nring = numr.size()/3;
04146
04147 int maxrin = numr[numr.size()-1];
04148
04149 float* circ1b = circ1->get_data();
04150 float* circ2b = circ2->get_data();
04151
04152
04153 double *t, *q;
04154
04155 q = (double*)calloc(maxrin,sizeof(double));
04156 t = (double*)calloc(maxrin,sizeof(double));
04157
04158 #ifdef _WIN32
04159 ip = -(int)(log((float)maxrin)/log(2.0f));
04160 #else
04161 ip = -(int)(log2(maxrin));
04162 #endif //_WIN32
04163
04164
04165
04166
04167
04168
04169
04170 for (i=1; i<=nring; i++) {
04171
04172 numr3i = numr(3,i);
04173 numr2i = numr(2,i);
04174
04175 t1 = circ1b(numr2i) * circ2b(numr2i);
04176 q(1) = q(1)+t1;
04177 t(1) = t(1)+t1;
04178
04179 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04180 if (numr3i == maxrin) {
04181 q(2) += t1;
04182 t(2) += t1;
04183 } else {
04184 q(numr3i+1) += t1;
04185 t(numr3i+1) += t1;
04186 }
04187
04188 for (j=3; j<=numr3i; j=j+2) {
04189 jc = j+numr2i-1;
04190
04191 c1 = circ1b(jc);
04192 c2 = circ1b(jc+1);
04193 d1 = circ2b(jc);
04194 d2 = circ2b(jc+1);
04195
04196 t1 = c1 * d1;
04197 t3 = c1 * d2;
04198 t2 = c2 * d2;
04199 t4 = c2 * d1;
04200
04201 q(j) += t1 + t2;
04202 q(j+1) += - t3 + t4;
04203 t(j) += t1 - t2;
04204 t(j+1) += - t3 - t4;
04205 }
04206 }
04207
04208
04209 fftr_d(q,ip);
04210
04211
04212 fftr_d(t,ip);
04213
04214 EMData* out = new EMData();
04215 out->set_size(maxrin,2,1);
04216 float *dout = out->get_data();
04217 for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04218
04219
04220
04221 free(t);
04222 free(q);
04223 return out;
04224 }
04225
04226
04227 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04228
04229 int maxrin = numr[numr.size()-1];
04230
04231 vector<float> r(2*maxrin);
04232
04233 Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04234
04235 return r;
04236 }
04237
04238 #define dout(i,j) dout[i+maxrin*j]
04239 #define circ1b(i) circ1b[i-1]
04240 #define circ2b(i) circ2b[i-1]
04241
04242 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04243
04244
04245
04246 int ip, jc, numr3i, numr2i, i, j;
04247 float t1, t2, t3, t4, c1, c2, d1, d2;
04248
04249 int nring = numr.size()/3;
04250
04251 int maxrin = numr[numr.size()-1];
04252
04253 float* circ1b = circ1->get_data();
04254 float* circ2b = circ2->get_data();
04255
04256 #ifdef _WIN32
04257 ip = -(int)(log((float)maxrin)/log(2.0f));
04258 #else
04259 ip = -(int)(log2(maxrin));
04260 #endif //_WIN32
04261 for (int i=1; i<=maxrin; i++) {q(i) = 0.0f; t(i) = 0.0f;}
04262
04263
04264
04265
04266
04267 for (i=1; i<=nring; i++) {
04268
04269 numr3i = numr(3,i);
04270 numr2i = numr(2,i);
04271
04272 t1 = circ1b(numr2i) * circ2b(numr2i);
04273 q(1) += t1;
04274 t(1) += t1;
04275
04276 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04277 if (numr3i == maxrin) {
04278 q(2) += t1;
04279 t(2) += t1;
04280 } else {
04281 q(numr3i+1) += t1;
04282 t(numr3i+1) += t1;
04283 }
04284
04285 for (j=3; j<=numr3i; j=j+2) {
04286 jc = j+numr2i-1;
04287
04288 c1 = circ1b(jc);
04289 c2 = circ1b(jc+1);
04290 d1 = circ2b(jc);
04291 d2 = circ2b(jc+1);
04292
04293 t1 = c1 * d1;
04294 t3 = c1 * d2;
04295 t2 = c2 * d2;
04296 t4 = c2 * d1;
04297
04298 q(j) += t1 + t2;
04299 q(j+1) += -t3 + t4;
04300 t(j) += t1 - t2;
04301 t(j+1) += -t3 - t4;
04302 }
04303 }
04304
04305 fftr_q(q,ip);
04306
04307
04308
04309 fftr_q(t,ip);
04310 }
04311
04312
04313
04314 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04315 {
04316
04317 int ip, jc, numr3i, numr2i, i, j;
04318 float t1, t2, t3, t4, c1, c2, d1, d2;
04319
04320 int nring = numr.size()/3;
04321 int maxrin = numr[numr.size()-1];
04322
04323 float* circ1b = circ1->get_data();
04324 float* circ2b = circ2->get_data();
04325
04326 double *q;
04327
04328 q = (double*)calloc(maxrin,sizeof(double));
04329
04330 #ifdef _WIN32
04331 ip = -(int)(log((float)maxrin)/log(2.0f));
04332 #else
04333 ip = -(int)(log2(maxrin));
04334 #endif //_WIN32
04335
04336
04337
04338 for (i=1;i<=nring;i++) {
04339
04340 numr3i = numr(3,i);
04341 numr2i = numr(2,i);
04342
04343 t1 = circ1b(numr2i) * circ2b(numr2i);
04344 q(1) = q(1)+t1;
04345
04346 if (numr3i == maxrin) {
04347 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04348 q(2) = q(2)+t1;
04349 } else {
04350 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04351 q(numr3i+1) = q(numr3i+1)+t1;
04352 }
04353
04354 for (j=3;j<=numr3i;j=j+2) {
04355 jc = j+numr2i-1;
04356
04357 c1 = circ1b(jc);
04358 c2 = circ1b(jc+1);
04359 d1 = circ2b(jc);
04360 d2 = circ2b(jc+1);
04361
04362 t1 = c1 * d1;
04363 t3 = c1 * d2;
04364 t2 = c2 * d2;
04365 t4 = c2 * d1;
04366
04367 q(j) = q(j) + t1 + t2;
04368 q(j+1) = q(j+1) - t3 + t4;
04369 }
04370 }
04371
04372
04373 fftr_d(q,ip);
04374
04375 EMData* out = new EMData();
04376 out->set_size(maxrin,1,1);
04377 float *dout = out->get_data();
04378 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04379 free(q);
04380 return out;
04381
04382 }
04383
04384
04385 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04386 {
04387
04388 int ip, jc, numr3i, numr2i, i, j;
04389 float t1, t2, t3, t4, c1, c2, d1, d2;
04390
04391 int nring = numr.size()/3;
04392 int maxrin = numr[numr.size()-1];
04393
04394 float* circ1b = circ1->get_data();
04395 float* circ2b = circ2->get_data();
04396
04397 double *t;
04398
04399 t = (double*)calloc(maxrin,sizeof(double));
04400
04401 #ifdef _WIN32
04402 ip = -(int)(log((float)maxrin)/log(2.0f));
04403 #else
04404 ip = -(int)(log2(maxrin));
04405 #endif //_WIN32
04406
04407
04408
04409 for (i=1;i<=nring;i++) {
04410
04411 numr3i = numr(3,i);
04412 numr2i = numr(2,i);
04413
04414 t1 = circ1b(numr2i) * circ2b(numr2i);
04415 t(1) = t(1)+t1;
04416
04417 if (numr3i == maxrin) {
04418 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04419 t(2) = t(2)+t1;
04420 }
04421
04422 for (j=3;j<=numr3i;j=j+2) {
04423 jc = j+numr2i-1;
04424
04425 c1 = circ1b(jc);
04426 c2 = circ1b(jc+1);
04427 d1 = circ2b(jc);
04428 d2 = circ2b(jc+1);
04429
04430 t1 = c1 * d1;
04431 t3 = c1 * d2;
04432 t2 = c2 * d2;
04433 t4 = c2 * d1;
04434
04435 t(j) = t(j) + t1 - t2;
04436 t(j+1) = t(j+1) - t3 - t4;
04437 }
04438 }
04439
04440
04441 fftr_d(t,ip);
04442
04443 EMData* out = new EMData();
04444 out->set_size(maxrin,1,1);
04445 float *dout = out->get_data();
04446 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04447 free(t);
04448 return out;
04449
04450 }
04451
04452 #undef circ1b
04453 #undef circ2b
04454 #undef dout
04455
04456 #undef circ1
04457 #undef circ2
04458 #undef t
04459 #undef q
04460 #undef b
04461 #undef t7
04462
04463
04464 #define QUADPI 3.141592653589793238462643383279502884197
04465 #define PI2 2*QUADPI
04466
04467 float Util::ener(EMData* ave, vector<int> numr) {
04468 ENTERFUNC;
04469 long double ener,en;
04470
04471 int nring = numr.size()/3;
04472 float *aveptr = ave->get_data();
04473
04474 ener = 0.0;
04475 for (int i=1; i<=nring; i++) {
04476 int numr3i = numr(3,i);
04477 int np = numr(2,i)-1;
04478 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04479 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04480 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04481 ener += en/numr3i;
04482 }
04483 EXITFUNC;
04484 return static_cast<float>(ener);
04485 }
04486
04487 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04488 ENTERFUNC;
04489 long double ener, en;
04490 float arg, cs, si;
04491
04492 int nima = data.size();
04493 int nring = numr.size()/3;
04494 int maxrin = numr(3,nring);
04495
04496 ener = 0.0;
04497 for (int i=1; i<=nring; i++) {
04498 int numr3i = numr(3,i);
04499 int np = numr(2,i)-1;
04500 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04501 float temp1 = 0.0, temp2 = 0.0;
04502 for (int kk=0; kk<nima; kk++) {
04503 float *ptr = data[kk]->get_data();
04504 temp1 += ptr[np];
04505 temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04506 }
04507 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04508 for (int j=2; j<numr3i; j+=2) {
04509 float tempr = 0.0, tempi = 0.0;
04510 for (int kk=0; kk<nima; kk++) {
04511 float *ptr = data[kk]->get_data();
04512 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04513 cs = cos(arg);
04514 si = sin(arg);
04515 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04516 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04517 }
04518 en += tq*(tempr*tempr+tempi*tempi);
04519 }
04520 ener += en/numr3i;
04521 }
04522 EXITFUNC;
04523 return static_cast<float>(ener);
04524 }
04525
04526 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04527 int nring = numr.size()/3;
04528 float *ave = avep->get_data();
04529 float *dat = datp->get_data();
04530 int i, j, numr3i, np;
04531 float arg, cs, si;
04532 int maxrin = numr(3,nring);
04533 if(mirror == 1) {
04534 for (i=1; i<=nring; i++) {
04535 numr3i = numr(3,i);
04536 np = numr(2,i)-1;
04537 ave[np] += dat[np];
04538 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04539 for (j=2; j<numr3i; j=j+2) {
04540 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04541 cs = cos(arg);
04542 si = sin(arg);
04543
04544 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04545 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04546 }
04547 }
04548 } else {
04549 for (i=1; i<=nring; i++) {
04550 numr3i = numr(3,i);
04551 np = numr(2,i)-1;
04552 ave[np] += dat[np];
04553 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04554 for (j=2; j<numr3i; j=j+2) {
04555 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04556 cs = cos(arg);
04557 si = sin(arg);
04558
04559 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04560 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04561 }
04562 }
04563 }
04564 avep->update();
04565 EXITFUNC;
04566 }
04567
04568 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04569 int nring = numr.size()/3;
04570 float *ave = avep->get_data();
04571 float *dat = datp->get_data();
04572 int i, j, numr3i, np;
04573 float arg, cs, si;
04574 int maxrin = numr(3,nring);
04575 if(mirror == 1) {
04576 for (i=1; i<=nring; i++) {
04577 numr3i = numr(3,i);
04578 np = numr(2,i)-1;
04579 ave[np] -= dat[np];
04580 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04581 for (j=2; j<numr3i; j=j+2) {
04582 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04583 cs = cos(arg);
04584 si = sin(arg);
04585
04586 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04587 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04588 }
04589 }
04590 } else {
04591 for (i=1; i<=nring; i++) {
04592 numr3i = numr(3,i);
04593 np = numr(2,i)-1;
04594 ave[np] -= dat[np];
04595 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04596 for (j=2; j<numr3i; j=j+2) {
04597 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04598 cs = cos(arg);
04599 si = sin(arg);
04600
04601 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04602 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04603 }
04604 }
04605 }
04606 avep->update();
04607 EXITFUNC;
04608 }
04609
04610
04611 #undef QUADPI
04612 #undef PI2
04613
04614 #undef numr
04615 #undef circ
04616
04617
04618 #define QUADPI 3.141592653589793238462643383279502884197
04619 #define PI2 QUADPI*2
04620 #define deg_rad QUADPI/180.0
04621 #define rad_deg 180.0/QUADPI
04622
04623 struct ori_t
04624 {
04625 int iphi;
04626 int itht;
04627 int id;
04628 };
04629
04630
04631 struct cmpang
04632 {
04633 bool operator()( const ori_t& a, const ori_t& b )
04634 {
04635 if( a.itht != b.itht )
04636 {
04637 return a.itht < b.itht;
04638 }
04639
04640 return a.iphi < b.iphi;
04641 }
04642 };
04643
04644
04645 vector<double> Util::cml_weights(const vector<float>& cml){
04646 static const int NBIN = 100;
04647 int nline=cml.size()/2;
04648 vector<double> weights(nline);
04649
04650 vector<ori_t> angs(nline);
04651 for( int i=0; i < nline; ++i ) {
04652 angs[i].iphi = int( NBIN*cml[2*i] );
04653 angs[i].itht = int( NBIN*cml[2*i+1] );
04654 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04655 angs[i].id = i;
04656 }
04657
04658
04659
04660 std::sort( angs.begin(), angs.end(), cmpang() );
04661
04662 vector<float> newphi;
04663 vector<float> newtht;
04664 vector< vector<int> > indices;
04665
04666 int curt_iphi = -1;
04667 int curt_itht = -1;
04668 for(unsigned int i=0 ;i < angs.size(); ++i ) {
04669 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04670 Assert( indices.size() > 0 );
04671 indices.back().push_back(angs[i].id);
04672 } else {
04673 curt_iphi = angs[i].iphi;
04674 curt_itht = angs[i].itht;
04675
04676 newphi.push_back( float(curt_iphi)/NBIN );
04677 newtht.push_back( float(curt_itht)/NBIN );
04678 indices.push_back( vector<int>(1,angs[i].id) );
04679 }
04680 }
04681
04682
04683
04684
04685 int num_agl = newphi.size();
04686
04687 if(num_agl>2) {
04688 vector<double> w=Util::vrdg(newphi, newtht);
04689
04690 Assert( w.size()==newphi.size() );
04691 Assert( indices.size()==newphi.size() );
04692
04693 for(unsigned int i=0; i < newphi.size(); ++i ) {
04694
04695
04696
04697
04698
04699
04700
04701
04702 for(unsigned int j=0; j < indices[i].size(); ++j ) {
04703 int id = indices[i][j];
04704 weights[id] = w[i]/indices[i].size();
04705
04706 }
04707
04708
04709
04710 }
04711 } else {
04712 cout<<"warning in Util.cml_weights"<<endl;
04713 double val = PI2/float(nline);
04714 for(int i=0; i<nline; i++) weights[i]=val;
04715 }
04716
04717 return weights;
04718
04719 }
04720
04721
04722
04723
04724
04725 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04726 {
04727 int i;
04728 int nx=img->get_xsize();
04729 float *img_ptr = img->get_data();
04730 float *line_ptr = line->get_data();
04731 for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04732 img->update();
04733 }
04734
04735 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04736 int j;
04737 int nx = sino->get_xsize();
04738 int i = nx * pos_line;
04739 float r1, r2;
04740 float *line_ptr = line->get_data();
04741 float *sino_ptr = sino->get_data();
04742 for (j=ilf;j<=ihf; j += 2) {
04743 r1 = line_ptr[j];
04744 r2 = line_ptr[j + 1];
04745 sino_ptr[i + j - ilf] = r1;
04746 sino_ptr[i + j - ilf + 1] = r2;
04747 sino_ptr[i + nx * nblines + j - ilf] = r1;
04748 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04749 }
04750 sino->update();
04751 }
04752
04753 vector<double> Util::cml_init_rot(vector<float> Ori){
04754 int nb_ori = Ori.size() / 4;
04755 int i, ind;
04756 float ph, th, ps;
04757 double cph, cth, cps, sph, sth, sps;
04758 vector<double> Rot(nb_ori*9);
04759 for (i=0; i<nb_ori; ++i){
04760 ind = i*4;
04761
04762 ph = Ori[ind+2]-90;
04763 th = Ori[ind+1];
04764 ps = Ori[ind]+90;
04765 ph *= deg_rad;
04766 th *= deg_rad;
04767 ps *= deg_rad;
04768
04769 cph = cos(ph);
04770 cth = cos(th);
04771 cps = cos(ps);
04772 sph = sin(ph);
04773 sth = sin(th);
04774 sps = sin(ps);
04775
04776 ind = i*9;
04777 Rot[ind] = cph*cps-cth*sps*sph;
04778 Rot[ind+1] = cph*sps+cth*cps*sph;
04779 Rot[ind+2] = sth*sph;
04780 Rot[ind+3] = -sph*cps-cth*sps*cph;
04781 Rot[ind+4] = -sph*sps+cth*cps*cph;
04782 Rot[ind+5] = sth*cph;
04783 Rot[ind+6] = sth*sps;
04784 Rot[ind+7] = -sth*cps;
04785 Rot[ind+8] = cth;
04786 }
04787
04788 return Rot;
04789 }
04790
04791 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04792 float ph, ps;
04793 double cph, cth, cps, sph, sth, sps;
04794 int ind = iprj*9;
04795
04796 ph = nps-90;
04797 ps = nph+90;
04798 ph *= deg_rad;
04799 th *= deg_rad;
04800 ps *= deg_rad;
04801
04802 cph = cos(ph);
04803 cth = cos(th);
04804 cps = cos(ps);
04805 sph = sin(ph);
04806 sth = sin(th);
04807 sps = sin(ps);
04808
04809 Rot[ind] = (float)(cph*cps-cth*sps*sph);
04810 Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04811 Rot[ind+2] = (float)(sth*sph);
04812 Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04813 Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04814 Rot[ind+5] = (float)(sth*cph);
04815 Rot[ind+6] = (float)(sth*sps);
04816 Rot[ind+7] = (float)(-sth*cps);
04817 Rot[ind+8] = (float)(cth);
04818
04819 return Rot;
04820 }
04821
04822 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04823 vector<int> com(2*(n_prj - 1));
04824 int a = i_prj*9;
04825 int i, b, c;
04826 int n1=0, n2=0;
04827 float vmax = 1 - 1.0e-6f;
04828 double r11, r12, r13, r23, r31, r32, r33;
04829
04830 c = 0;
04831 for (i=0; i<n_prj; ++i){
04832 if (i!=i_prj){
04833 b = i*9;
04834
04835 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04836 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04837 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04838 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04839 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04840 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04841 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04842 if (r33 > vmax) {
04843 n2 = 270;
04844 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04845 }
04846 else if (r33 < -vmax) {
04847 n2 = 270;
04848 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04849 } else {
04850 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04851 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04852 if (n1 < 0) {n1 += 360;}
04853 if (n2 <= 0) {n2 = abs(n2);}
04854 else {n2 = 360 - n2;}
04855 }
04856
04857 if (n1 >= 360){n1 = n1 % 360;}
04858 if (n2 >= 360){n2 = n2 % 360;}
04859
04860
04861 b = c*2;
04862 com[b] = n1;
04863 com[b+1] = n2;
04864 ++c;
04865 }
04866 }
04867
04868 return com;
04869
04870 }
04871
04872 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int, int n_lines) {
04873 vector<int> com(2*n_lines);
04874 int a=0, b, c, l;
04875 int n1=0, n2=0, mem=-1;
04876 float vmax = 1 - 1.0e-6f;
04877 double r11, r12, r13, r23, r31, r32, r33;
04878 c = 0;
04879 for (l=0; l<n_lines; ++l){
04880 c = 2*l;
04881 if (seq[c]!=mem){
04882 mem = seq[c];
04883 a = seq[c]*9;
04884 }
04885 b = seq[c+1]*9;
04886
04887
04888 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04889 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04890 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04891 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04892 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04893 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04894 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04895 if (r33 > vmax) {
04896 n2 = 270;
04897 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04898 }
04899 else if (r33 < -vmax) {
04900 n2 = 270;
04901 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04902 } else {
04903 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04904 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04905 if (n1 < 0) {n1 += 360;}
04906 if (n2 <= 0) {n2 = abs(n2);}
04907 else {n2 = 360 - n2;}
04908 }
04909 if (n1 >= 360){n1 = n1 % 360;}
04910 if (n2 >= 360){n2 = n2 % 360;}
04911
04912
04913 com[c] = n1;
04914 com[c+1] = n2;
04915 }
04916
04917 return com;
04918
04919 }
04920
04921 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int, int nlines){
04922
04923 vector<double> cml(2*nlines);
04924 float ph1, th1;
04925 float ph2, th2;
04926 double nx, ny, nz;
04927 double norm;
04928 double sth1=0, sph1=0, cth1=0, cph1=0;
04929 double sth2, sph2, cth2, cph2;
04930 int l, ind, c;
04931 int mem = -1;
04932 for (l=0; l<nlines; ++l){
04933 c = 2*l;
04934 if (seq[c]!=mem){
04935 mem = seq[c];
04936 ind = 4*seq[c];
04937 ph1 = Ori[ind]*deg_rad;
04938 th1 = Ori[ind+1]*deg_rad;
04939 sth1 = sin(th1);
04940 sph1 = sin(ph1);
04941 cth1 = cos(th1);
04942 cph1 = cos(ph1);
04943 }
04944 ind = 4*seq[c+1];
04945 ph2 = Ori[ind]*deg_rad;
04946 th2 = Ori[ind+1]*deg_rad;
04947 sth2 = sin(th2);
04948 cth2 = cos(th2);
04949 sph2 = sin(ph2);
04950 cph2 = cos(ph2);
04951
04952 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04953 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04954 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04955 norm = sqrt(nx*nx+ny*ny+nz*nz);
04956 nx /= norm;
04957 ny /= norm;
04958 nz /= norm;
04959
04960 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04961
04962 cml[c+1] = acos(nz);
04963 if (cml[c+1] == 0) {cml[c] = 0;}
04964 else {
04965 cml[c+1] *= rad_deg;
04966 if (cml[c+1] > 89.99) {cml[c+1] = 89.99;}
04967 cml[c] = rad_deg * atan2(nx, ny);
04968 cml[c] = fmod(360 + cml[c], 360);
04969
04970 }
04971 }
04972
04973 return cml;
04974 }
04975
04976 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04977 double res = 0;
04978 double buf = 0;
04979 float* line_1;
04980 float* line_2;
04981 int i, n, ind;
04982 int lnlen = data[0]->get_xsize();
04983 for (n=0; n<n_lines; ++n) {
04984 ind = n*2;
04985 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04986 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04987 buf = 0;
04988 for (i=0; i<lnlen; ++i) {
04989 buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04990 }
04991 res += buf * weights[n];
04992 }
04993
04994 return res;
04995
04996 }
04997
04998 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04999 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
05000
05001
05002
05003 vector<double> res(2);
05004 int lnlen = data[0]->get_xsize();
05005 int end = 2*(n_prj-1);
05006 double disc, buf, bdisc, tmp;
05007 int n, i, ipsi, ind, bipsi, c;
05008 float* line_1;
05009 float* line_2;
05010 bdisc = 1.0e6;
05011 bipsi = -1;
05012
05013 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
05014
05015 disc = 0;
05016 c = 0;
05017 for (n=0; n<n_prj; ++n) {
05018 if(n!=iprj) {
05019 ind = 2*c;
05020 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
05021 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
05022 buf = 0;
05023 for (i=0; i<lnlen; ++i) {
05024 tmp = line_1[i]-line_2[i];
05025 buf += tmp*tmp;
05026 }
05027 disc += buf * weights[iw[c]];
05028 ++c;
05029 }
05030 }
05031
05032 if (disc <= bdisc) {
05033 bdisc = disc;
05034 bipsi = ipsi;
05035 }
05036
05037 for (i=0; i<end; i+=2){
05038 com[i] += d_psi;
05039 if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05040 }
05041 }
05042 res[0] = bdisc;
05043 res[1] = float(bipsi);
05044
05045 return res;
05046 }
05047
05048 vector<double> Util::cml_spin_psi_now(const vector<EMData*>& data, vector<int> com, \
05049 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
05050
05051
05052
05053 vector<double> res(2);
05054 int lnlen = data[0]->get_xsize();
05055 int end = 2*(n_prj-1);
05056 double disc, buf, bdisc, tmp;
05057 int n, i, ipsi, ind, bipsi, c;
05058 float* line_1;
05059 float* line_2;
05060 bdisc = 1.0e6;
05061 bipsi = -1;
05062
05063 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
05064
05065 disc = 0;
05066 c = 0;
05067 for (n=0; n<n_prj; ++n) {
05068 if(n!=iprj) {
05069 ind = 2*c;
05070 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
05071 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
05072 buf = 0;
05073 for (i=0; i<lnlen; ++i) {
05074 tmp = line_1[i]-line_2[i];
05075 buf += tmp*tmp;
05076 }
05077 disc += buf;
05078 ++c;
05079 }
05080 }
05081
05082 if (disc <= bdisc) {
05083 bdisc = disc;
05084 bipsi = ipsi;
05085 }
05086
05087 for (i=0; i<end; i+=2){
05088 com[i] += d_psi;
05089 if (com[i] >= n_psi) com[i] = com[i] - n_psi;
05090 }
05091 }
05092 res[0] = bdisc;
05093 res[1] = float(bipsi);
05094
05095 return res;
05096 }
05097
05098 #undef QUADPI
05099 #undef PI2
05100 #undef deg_rad
05101 #undef rad_deg
05102
05103
05104
05105
05106
05107
05108 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
05109 ENTERFUNC;
05110
05111 int nima = data.size();
05112 vector<float> res(nima);
05113 double result = 0.;
05114 double valmin = 1.0e20;
05115 int valpos = -1;
05116
05117 for (int kk=0; kk<nima; kk++){
05118 result = 0;
05119
05120 float *y_data = data[kk]->get_data();
05121 float *x_data = image->get_data();
05122 long totsize = image->get_xsize()*image->get_ysize();
05123 for (long i = 0; i < totsize; i++) {
05124 double temp = x_data[i]- y_data[i];
05125 result += temp*temp;
05126 }
05127 result /= totsize;
05128 res[kk] = (float)result;
05129
05130 if(result<valmin) {valmin = result; valpos = kk;}
05131
05132 }
05133
05134 Dict retvals;
05135 retvals["dist"] = res;
05136 retvals["pos"] = valpos;
05137
05138 EXITFUNC;
05139 return retvals;
05140
05141 }
05142
05143 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05144 ENTERFUNC;
05145
05146 int nima = data.size();
05147 vector<float> res(nima);
05148 double result = 0.;
05149 double valmin = 1.0e20;
05150 int valpos = -1;
05151
05152 for (int kk=0; kk<nima; kk++){
05153 result = 0;
05154
05155
05156 float *y_data = data[kk]->get_data();
05157 float *x_data = image->get_data();
05158
05159
05160 int nx = data[kk]->get_xsize();
05161 int ny = data[kk]->get_ysize();
05162 nx = (nx - 2 + data[kk]->is_fftodd());
05163 int lsd2 = (nx + 2 - nx%2) ;
05164
05165 int ixb = 2*((nx+1)%2);
05166 int iyb = ny%2;
05167 int iz = 0;
05168
05169 for ( int iy = 0; iy <= ny-1; iy++) {
05170 for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05171 int ii = ix + (iy + iz * ny)* lsd2;
05172 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05173 }
05174 }
05175 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05176 int ii = (iy + iz * ny)* lsd2;
05177 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05178 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05179 }
05180 if(nx%2 == 0) {
05181 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05182 int ii = lsd2 - 2 + (iy + iz * ny)* lsd2;
05183 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05184 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05185 }
05186
05187 }
05188 result *= 2;
05189 result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05190 if(ny%2 == 0) {
05191 int ii = (ny/2 + iz * ny)* lsd2;
05192 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05193 }
05194 if(nx%2 == 0) {
05195 int ii = lsd2 - 2 + (0 + iz * ny)* lsd2;
05196 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05197 if(ny%2 == 0) {
05198 int ii = lsd2 - 2 +(ny/2 + iz * ny)* lsd2;
05199 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05200 }
05201 }
05202
05203 result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05204 res[kk] = (float)result;
05205
05206 if(result<valmin) {valmin = result; valpos = kk;}
05207
05208 }
05209
05210 Dict retvals;
05211 retvals["dist"] = res;
05212 retvals["pos"] = valpos;
05213
05214 EXITFUNC;
05215 return retvals;
05216 }
05217
05218 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05219 long int d2 = group2[s2 - 1] - group2[0];
05220 long int p2 = 0;
05221 long int i1 = 0;
05222 long int i2 = 0;
05223 long int max = 0;
05224 long int cont = 0;
05225 long int i = 0;
05226 int stop1 = 0;
05227 int stop2 = 0;
05228
05229 for (i=0; i<s1; i++) {
05230 p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05231 if (p2 >= s2) {p2 = s2 - 1;}
05232 i1 = p2;
05233 i2 = p2;
05234 max = s2;
05235 if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05236
05237 stop1 = 0;
05238 stop2 = 0;
05239 while (max--) {
05240 if (group1[i] == group2[i1]) {
05241 if (flag) {stb[cont] = group1[i];}
05242 cont++;
05243 break;
05244 }
05245 if (group2[i1] < group1[i]) {stop1=1;}
05246 if (group1[i] == group2[i2]) {
05247 if (flag) {stb[cont] = group1[i];}
05248 cont++;
05249 break;
05250 }
05251 if (group2[i2] > group1[i]) {stop2=1;}
05252
05253
05254 if (stop1 & stop2) {break;}
05255 i1--;
05256 i2++;
05257 if (i1 < 0) {i1 = 0;}
05258 if (i2 >= s2) {i2 = s2 - 1;}
05259 }
05260
05261 }
05262
05263 return cont;
05264 }
05265
05266
05267
05268 #define old_ptr(i,j,k) old_ptr[i+(j+(k*ny))*(size_t)nx]
05269 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*(size_t)new_nx]
05270 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05271 {
05272
05273 if (!img) {
05274 throw NullPointerException("NULL input image");
05275 }
05276
05277
05278
05279 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05280
05281
05282
05283
05284 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)
05285 {
05286 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05287 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05288 }
05289
05290
05291
05292
05293 int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05294
05295
05296
05297
05298 int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05299 int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05300 int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05301 if(r1>1){r1=1;}
05302 if(r2>1){r2=1;}
05303 if(r3>1){r3=1;}
05304 int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05305
05306
05307
05308 EMData* img2 = new EMData();
05309 img2->set_size(new_nx,new_ny,new_nz);
05310 float *new_ptr = img2->get_data();
05311 float *old_ptr = img->get_data();
05312 int iptr, jptr, kptr = 0;
05313 for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05314 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05315 for (int i=new_st_x; i<nx; i+=x_step) {
05316 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05317 iptr++;}
05318 jptr++;}
05319 kptr++;}
05320 img2->update();
05321 return img2;
05322 }
05323 #undef old_ptr
05324 #undef new_ptr
05325
05326 #define inp(i,j,k) inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*(size_t)nx]
05327 #define outp(i,j,k) outp[i+(j+(k*new_ny))*(size_t)new_nx]
05328 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05329 {
05330
05331 if (!img) throw NullPointerException("NULL input image");
05332
05333
05334
05335 int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05336
05337
05338
05339 if(new_nx>nx || new_ny>ny || new_nz>nz)
05340 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size. 1");
05341 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)
05342 throw ImageDimensionException("The offset inconsistent with the input image size. 2");
05343 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))))
05344 throw ImageDimensionException("The offset inconsistent with the input image size. 3");
05345
05346
05347
05348 int new_st_x = nx/2-new_nx/2 + x_offset,
05349 new_st_y = ny/2-new_ny/2 + y_offset,
05350 new_st_z = nz/2-new_nz/2 + z_offset;
05351
05352
05353
05354 if (new_st_x<0 || new_st_y<0 || new_st_z<0)
05355 throw ImageDimensionException("The offset inconsistent with the input image size. 4");
05356
05357
05358 EMData* wind = img->copy_empty_head();
05359 wind->set_size(new_nx, new_ny, new_nz);
05360 float *outp=wind->get_data();
05361 float *inp=img->get_data();
05362
05363 for (int k=0; k<new_nz; k++)
05364 for(int j=0; j<new_ny; j++)
05365 for(int i=0; i<new_nx; i++)
05366 outp(i,j,k) = inp(i,j,k);
05367 wind->update();
05368 return wind;
05369 }
05370 #undef inp
05371 #undef outp
05372
05373 #define inp(i,j,k) inp[i+(j+(k*ny))*(size_t)nx]
05374 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*(size_t)new_nx]
05375 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)
05376 {
05377
05378 if (!img) throw NullPointerException("NULL input image");
05379
05380
05381
05382 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05383
05384
05385
05386 if(new_nx<nx || new_ny<ny || new_nz<nz)
05387 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05388 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)
05389 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05390 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))))
05391 throw ImageDimensionException("The offset inconsistent with the input image size. Solution: Change the offset parameters");
05392
05393
05394 EMData* pading = img->copy_head();
05395 pading->set_size(new_nx, new_ny, new_nz);
05396 float *inp = img->get_data();
05397 float *outp = pading->get_data();
05398
05399
05400
05401
05402 float background;
05403
05404 if (strcmp(params,"average")==0) background = img->get_attr("mean");
05405 else if (strcmp(params,"circumference")==0) {
05406 float sum1=0.0f;
05407 size_t cnt=0;
05408 for(int i=0;i<nx;i++) {
05409 sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05410 cnt+=2;
05411 }
05412 if(nz-1 == 0) {
05413 for (int j=1;j<ny-1;j++) {
05414 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05415 cnt+=2;
05416 }
05417 } else {
05418 for (int k=1;k<nz-1;k++) {
05419 for (int j=1;j<ny-1;j++) {
05420 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05421 cnt+=2;
05422 }
05423 }
05424 }
05425 background = sum1/cnt;
05426 } else {
05427 background = static_cast<float>( atof( params ) );
05428 }
05429
05430
05431
05432 int new_st_x=0,new_st_y=0,new_st_z=0;
05433 for (int k=0;k<new_nz;k++)
05434 for(int j=0;j<new_ny;j++)
05435 for (int i=0;i<new_nx;i++)
05436 outp(i,j,k)=background;
05437
05438
05439
05440 new_st_x=int((new_nx/2-nx/2) + x_offset);
05441 new_st_y=int((new_ny/2-ny/2) + y_offset);
05442 new_st_z=int((new_nz/2-nz/2) + z_offset);
05443
05444
05445 for (int k=0;k<nz;k++)
05446 for(int j=0;j<ny;j++)
05447 for(int i=0;i<nx;i++)
05448 outp(i,j,k)=inp(i,j,k);
05449 pading->update();
05450 return pading;
05451 }
05452 #undef inp
05453 #undef outp
05454
05455
05456 void Util::colreverse(float* beg, float* end, int nx) {
05457 float* tmp = new float[nx];
05458 int n = (end - beg)/nx;
05459 int nhalf = n/2;
05460 for (int i = 0; i < nhalf; i++) {
05461
05462 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05463 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05464 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05465 }
05466 delete[] tmp;
05467 }
05468
05469 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05470 {
05471 int nxy = nx*ny;
05472 colreverse(beg, end, nxy);
05473 }
05474
05475
05476 void Util::cyclicshift(EMData *image, Dict params) {
05477
05478 if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05479
05480 int dx = params["dx"];
05481 int dy = params["dy"];
05482 int dz = params["dz"];
05483
05484
05485 int nx = image->get_xsize();
05486 dx %= nx;
05487 if (dx < 0) dx += nx;
05488 int ny = image->get_ysize();
05489 dy %= ny;
05490 if (dy < 0) dy += ny;
05491 int nz = image->get_zsize();
05492 dz %= nz;
05493 if (dz < 0) dz += nz;
05494
05495 int mx = -(dx - nx);
05496 int my = -(dy - ny);
05497 int mz = -(dz - nz);
05498
05499 float* data = image->get_data();
05500
05501 if (mx != 0) {
05502 for (int iz = 0; iz < nz; iz++)
05503 for (int iy = 0; iy < ny; iy++) {
05504
05505 size_t offset = nx*iy + (size_t)nx*ny*iz;
05506 reverse(&data[offset],&data[offset+mx]);
05507 reverse(&data[offset+mx],&data[offset+nx]);
05508 reverse(&data[offset],&data[offset+nx]);
05509 }
05510 }
05511
05512 if (my != 0) {
05513 for (int iz = 0; iz < nz; iz++) {
05514 size_t offset = (size_t)nx*ny*iz;
05515 colreverse(&data[offset], &data[offset + my*nx], nx);
05516 colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05517 colreverse(&data[offset], &data[offset + ny*nx], nx);
05518 }
05519 }
05520 if (mz != 0) {
05521 slicereverse(&data[0], &data[(size_t)mz*ny*nx], nx, ny);
05522 slicereverse(&data[mz*ny*nx], &data[(size_t)nz*ny*nx], nx, ny);
05523 slicereverse(&data[0], &data[(size_t)nz*ny*nx], nx ,ny);
05524 }
05525 image->update();
05526 }
05527
05528
05529
05530
05531 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05532 {
05533 if (image->is_complex())
05534 throw ImageFormatException("Cannot do histogram on Fourier image");
05535
05536 float *imageptr=0, *maskptr=0;
05537 int nx=image->get_xsize();
05538 int ny=image->get_ysize();
05539 int nz=image->get_zsize();
05540
05541 if(mask != NULL){
05542 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05543 throw ImageDimensionException("The size of mask image should be of same size as the input image");
05544 maskptr =mask->get_data();
05545 }
05546 if( nbins == 0) nbins = nx;
05547 vector <float> freq(2*nbins, 0.0);
05548
05549 imageptr=image->get_data();
05550 if( hmin == hmax ) {
05551 if(mask == NULL) {
05552 hmax = image->get_attr("maximum");
05553 hmin = image->get_attr("minimum");
05554 } else {
05555 bool First = true;
05556 for (size_t i = 0;i < (size_t)nx*ny*nz; i++) {
05557 if (maskptr[i]>=0.5f) {
05558 if(First) {
05559 hmax = imageptr[i];
05560 hmin = imageptr[i];
05561 First = false;
05562 } else {
05563 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05564 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05565 }
05566 }
05567 }
05568 }
05569 }
05570 float hdiff = hmax - hmin;
05571 float ff = (nbins-1)/hdiff;
05572 for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05573 if(mask == NULL) {
05574 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05575 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05576 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05577 }
05578 } else {
05579 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05580 if(maskptr[i] >= 0.5) {
05581 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05582 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05583 }
05584 }
05585 }
05586 return freq;
05587 }
05588
05589 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05590 {
05591
05592 if (img->is_complex() || ref->is_complex())
05593 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05594
05595 if(mask != NULL){
05596 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05597 throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05598
05599
05600
05601 size_t size_ref = ((size_t)(ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05602 size_t size_img = ((size_t)(img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05603
05604
05605
05606 float *ref_ptr = ref->get_data();
05607 float ref_h_min = ref->get_attr("minimum");
05608 float ref_h_max = ref->get_attr("maximum");
05609 float ref_h_avg = ref->get_attr("mean");
05610 float ref_h_sig = ref->get_attr("sigma");
05611
05612
05613
05614 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05615
05616 vector<float> img_data = Util::infomask(img, mask);
05617 float img_avg = img_data[0];
05618 float img_sig = img_data[1];
05619
05620
05621 int cnt=0;
05622 for(size_t i=0;i<size_img;++i)
05623 if (mask_ptr[i]>0.5f)
05624 cnt++;
05625
05626
05627
05628 float ref_h_diff = ref_h_max - ref_h_min;
05629
05630 #ifdef _WIN32
05631 int hist_len = _cpp_min((unsigned long)size_ref/16,_cpp_min((unsigned long)size_img/16,256lu));
05632 #else
05633 int hist_len = std::min((unsigned long)size_ref/16,std::min((unsigned long)size_img/16,256lu));
05634 #endif //_WIN32
05635
05636 float *ref_freq_bin = new float[3*hist_len];
05637
05638
05639 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05640
05641 for (size_t i = 0;i < size_ref;++i) {
05642 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05643 ref_freq_bin[L]++;
05644 }
05645 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05646
05647
05648 float A = ref_h_sig/img_sig;
05649 float B = ref_h_avg - (A*img_avg);
05650
05651 vector<float> args;
05652 args.push_back(A);
05653 args.push_back(B);
05654
05655 vector<float> scale;
05656 scale.push_back(1.e-7f*A);
05657 scale.push_back(-1.e-7f*B);
05658
05659 vector<float> ref_freq_hist;
05660 for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05661
05662 vector<float> data;
05663 data.push_back(ref_h_diff);
05664 data.push_back(ref_h_min);
05665
05666 Dict parameter;
05667
05668
05669 parameter["args"] = args;
05670 parameter["scale"]= scale;
05671 parameter["data"] = data;
05672 parameter["ref_freq_bin"] = ref_freq_hist;
05673 parameter["size_img"]=(double)size_img;
05674 parameter["hist_len"]=hist_len;
05675
05676
05677 return parameter;
05678 }
05679
05680
05681 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)
05682 {
05683 float *img_ptr = img->get_data();
05684 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05685
05686 int *img_freq_bin = new int[3*hist_len];
05687 for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05688 for(size_t i = 0;i < size_img;++i) {
05689 if(mask_ptr[i] > 0.5f) {
05690 float img_xn = img_ptr[i]*PA + PB;
05691 int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05692 if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05693 }
05694 }
05695 int freq_hist = 0;
05696
05697 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);
05698 freq_hist = (-freq_hist);
05699 return static_cast<float>(freq_hist);
05700 }
05701
05702 #define QUADPI 3.141592653589793238462643383279502884197
05703 #define DGR_TO_RAD QUADPI/180
05704 #define DM(I) DM [I-1]
05705 #define SS(I) SS [I-1]
05706 Dict Util::CANG(float PHI,float THETA,float PSI)
05707 {
05708 double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05709 vector<float> DM,SS;
05710
05711 for(int i =0;i<9;i++) DM.push_back(0);
05712
05713 for(int i =0;i<6;i++) SS.push_back(0);
05714
05715 CPHI = cos(double(PHI)*DGR_TO_RAD);
05716 SPHI = sin(double(PHI)*DGR_TO_RAD);
05717 CTHE = cos(double(THETA)*DGR_TO_RAD);
05718 STHE = sin(double(THETA)*DGR_TO_RAD);
05719 CPSI = cos(double(PSI)*DGR_TO_RAD);
05720 SPSI = sin(double(PSI)*DGR_TO_RAD);
05721
05722 SS(1) = float(CPHI);
05723 SS(2) = float(SPHI);
05724 SS(3) = float(CTHE);
05725 SS(4) = float(STHE);
05726 SS(5) = float(CPSI);
05727 SS(6) = float(SPSI);
05728
05729 DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05730 DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05731 DM(3) = float(-STHE*CPSI);
05732 DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05733 DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05734 DM(6) = float(STHE*SPSI);
05735 DM(7) = float(STHE*CPHI);
05736 DM(8) = float(STHE*SPHI);
05737 DM(9) = float(CTHE);
05738
05739 Dict DMnSS;
05740 DMnSS["DM"] = DM;
05741 DMnSS["SS"] = SS;
05742
05743 return(DMnSS);
05744 }
05745 #undef SS
05746 #undef DM
05747 #undef QUADPI
05748 #undef DGR_TO_RAD
05749
05750 struct t_BPCQ_line{
05751 int rX;
05752 int offset;
05753 float xbb;
05754 float ybb;
05755 };
05756
05757 void Util::BPCQ( EMData *B, EMData *CUBE, const int radius )
05758 {
05759 if (B->is_complex()) {
05760 B->do_ift_inplace();
05761 B->depad();
05762 }
05763
05764 const Transform * transform = B->get_attr("xform.projection");
05765 Dict transform_params = transform->get_params("spider");
05766
05767
05768 std::vector<float> DM = transform->get_matrix();
05769 DM[3+0] = DM[4+0];
05770 DM[3+1] = DM[4+1];
05771 DM[3+2] = DM[4+2];
05772
05773 delete transform;
05774
05775 const int NSAM = B->get_xsize();
05776 const int NROW = B->get_ysize();
05777
05778
05779 t_BPCQ_line * lines_to_process;
05780 #ifdef _WIN32
05781 if ( (lines_to_process = (t_BPCQ_line *)_aligned_malloc( 4*radius*radius*sizeof(t_BPCQ_line), 256 )) == NULL )
05782 #else
05783 if ( posix_memalign( reinterpret_cast<void**>(&lines_to_process), 256, 4*radius*radius*sizeof(t_BPCQ_line) ) != 0 )
05784 #endif //_WIN32
05785 {
05786 throw std::bad_alloc();
05787 }
05788 t_BPCQ_line * first_free_line = lines_to_process;
05789
05790
05791 {
05792
05793 const float x_shift_plus_center = float(NSAM/2 +1) + float(transform_params[ "tx" ]);
05794 const float y_shift_plus_center = float(NROW/2 +1) + float(transform_params[ "ty" ]);
05795
05796 const int sizeX = CUBE->get_xsize();
05797 const int sizeY = CUBE->get_ysize();
05798
05799 const int centerX = sizeX / 2;
05800 const int centerY = sizeY / 2;
05801 const int centerZ = CUBE->get_zsize() /2;
05802
05803 for ( int rZ=-radius; rZ<=radius; ++rZ ) {
05804 for ( int rY=-radius; rY<=radius; ++rY ) {
05805 const int sqRX = radius*radius - rZ*rZ - rY*rY;
05806 if (sqRX >= 0) {
05807 #ifdef _WIN32
05808 first_free_line->rX = static_cast<int>( floor(sqrtf(sqRX)+0.5) );
05809 #else
05810 first_free_line->rX = static_cast<int>( roundf(sqrtf(sqRX)) );
05811 #endif //_WIN32
05812 first_free_line->offset = sizeX*( centerY+rY + sizeY*(centerZ+rZ) ) + centerX - first_free_line->rX;
05813 first_free_line->xbb = rZ*DM[2] + rY*DM[1] + x_shift_plus_center;
05814 first_free_line->ybb = rZ*DM[5] + rY*DM[4] + y_shift_plus_center;
05815 ++first_free_line;
05816 }
05817 }
05818 }
05819 }
05820
05821 const float * const Bptr = B->get_data();
05822 float * const CUBE_begin = CUBE->get_data();
05823
05824
05825
05826 for ( t_BPCQ_line * iLine = lines_to_process; iLine < first_free_line; ++iLine ) {
05827 const int rX_first = -(iLine->rX);
05828 const int rX_last = iLine->rX;
05829 float *CUBE_ptr = CUBE_begin + iLine->offset;
05830 for (int rX=rX_first; rX<=rX_last; ++rX, ++CUBE_ptr) {
05831 const float XB = rX * DM[0] + iLine->xbb;
05832 const float YB = rX * DM[3] + iLine->ybb;
05833 const int IQX = int(XB);
05834 const int IQY = int(YB);
05835 if ( IQX < 1 || IQX >= NSAM || IQY < 1 || IQY >= NROW ) continue;
05836 const float DIPX = XB-IQX;
05837 const float DIPY = YB-IQY;
05838 const float b00 = Bptr[IQX-1+((IQY-1)*NSAM)];
05839 const float b01 = Bptr[IQX-1+((IQY-0)*NSAM)];
05840 const float b10 = Bptr[IQX-0+((IQY-1)*NSAM)];
05841 const float b11 = Bptr[IQX-0+((IQY-0)*NSAM)];
05842 *(CUBE_ptr) = *(CUBE_ptr) + b00 + DIPY*(b01-b00) + DIPX*(b10-b00+DIPY*(b11-b10-b01+b00));
05843 }
05844 }
05845
05846 free(lines_to_process);
05847 }
05848
05849 #define W(i,j) Wptr [i+(j)*Wnx]
05850 #define PROJ(i,j) PROJptr [i+(j)*NNNN]
05851 #define SS(i,j) SS [i+(j)*6]
05852
05853 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05854 {
05855
05856 --K;
05857
05858 int NSAM = PROJ->get_xsize();
05859 int NROW = PROJ->get_ysize();
05860
05861 if (PROJ->is_fftpadded()) {
05862 NSAM -= (PROJ->is_fftodd()) ? (1) : (2);
05863 }
05864
05865 const int ntotal = NSAM*NROW;
05866 const float q = 2.0f;
05867 const float qt = 8.0f/q;
05868
05869 const int ipad = 1;
05870 NSAM *= ipad;
05871 NROW *= ipad;
05872 const int NNNN = NSAM+2-(NSAM%2);
05873 const int NX2 = NSAM/2;
05874 const int NR2 = NROW/2;
05875
05876 const int NANG = int(SS.size())/6;
05877
05878 EMData* W = new EMData();
05879 const int Wnx = NNNN/2;
05880 W->set_size(Wnx,NROW,1);
05881 W->to_zero();
05882 float *Wptr = W->get_data();
05883
05884 for (int L=0; L<NANG; L++) {
05885 const float tmp1 = SS(2,K)*SS(3,L)*(SS(0,K)*SS(0,L) + SS(1,K)*SS(1,L)) - SS(2,L)*SS(3,K);
05886 const float tmp2 = SS(3,L)*( SS(0,K)*SS(1,L) - SS(0,L)*SS(1,K) );
05887 float OX = SS(5,K)*tmp2 + SS(4,K)*tmp1;
05888 float OY = SS(4,K)*tmp2 - SS(5,K)*tmp1;
05889 if(OX < 0.0f) {
05890 OX = -OX;
05891 OY = -OY;
05892 }
05893
05894 if( OX > 1.0e-6f || fabs(OY) > 1.0e-6f ) {
05895 for (int J=0; J<NROW; ++J) {
05896 const float JY_OY = (J > NR2) ? ((J-NROW)*OY) : (J*OY);
05897 int xma = NX2;
05898 int xmi = 0;
05899 const float fxma = ( q-JY_OY) / OX;
05900 const float fxmi = (-q-JY_OY) / OX;
05901 if (fxma < xmi || fxmi > xma ) {
05902 continue;
05903 }
05904 if (fxma < xma) {
05905 xma = static_cast<int>(fxma+0.5f);
05906 }
05907 if (fxmi > xmi) {
05908 xmi = static_cast<int>(fxmi+0.5f);
05909 }
05910 for( int I=xmi; I<=xma; ++I ) {
05911 const float Y = I*OX + JY_OY;
05912 W(I,J) += exp(-qt*Y*Y);
05913 }
05914 }
05915 } else {
05916 for (int J=0; J<NROW; ++J) {
05917 for (int I=0; I<NNNN/2; ++I) {
05918 W(I,J) += 1.0f;
05919 }
05920 }
05921 }
05922 }
05923
05924 EMData* proj_in = PROJ;
05925
05926 const bool realOnInput = PROJ->is_real();
05927 if (realOnInput) {
05928
05929 PROJ = PROJ->norm_pad( false, ipad);
05930 PROJ->do_fft_inplace();
05931 PROJ->update();
05932 }
05933 float * PROJptr = PROJ->get_data();
05934
05935 const float osnr = 1.0f/SNR;
05936 const float WNRMinv = 1.0f/W(0,0);
05937 for (int J=0; J<NROW; ++J) {
05938 float sy = (J > NR2) ? (J - NROW) : (J);
05939 sy /= NROW;
05940 sy *= sy;
05941 for (int I=0; I<NNNN; I+=2) {
05942 const int KX = I/2;
05943 const float temp = W(KX,J)*WNRMinv;
05944 float WW = temp/(temp*temp + osnr);
05945
05946 const float sx = float(KX) / NSAM;
05947 WW *= exp(qt*(sy + sx*sx));
05948 PROJ(I,J) *= WW;
05949 PROJ(I+1,J) *= WW;
05950 }
05951 }
05952 delete W; W = 0;
05953
05954 PROJ->do_ift_inplace();
05955 PROJ->depad();
05956
05957 if (realOnInput) {
05958
05959 float* data_src = PROJ->get_data();
05960 float* data_dst = proj_in->get_data();
05961 memcpy( data_dst, data_src, ntotal * sizeof(float) );
05962 delete PROJ;
05963 }
05964
05965 proj_in->update();
05966 }
05967
05968
05969
05970
05971
05972
05973
05974
05975
05976
05977
05978
05979
05980
05981
05982
05983
05984
05985
05986
05987
05988
05989
05990
05991
05992
05993
05994
05995
05996
05997
05998
05999
06000
06001
06002
06003
06004
06005
06006
06007
06008
06009
06010
06011
06012
06013
06014
06015
06016
06017
06018
06019
06020
06021
06022
06023
06024
06025
06026
06027
06028
06029
06030
06031
06032
06033
06034
06035
06036
06037
06038
06039
06040
06041
06042
06043
06044
06045
06046 #undef PROJ
06047 #undef W
06048 #undef SS
06049
06050 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
06051 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
06052 #define SS(I,J) SS [I-1 + (J-1)*6]
06053 #define RI(i,j) RI [(i-1) + ((j-1)*3)]
06054 #define CC(i) CC [i-1]
06055 #define CP(i) CP [i-1]
06056 #define VP(i) VP [i-1]
06057 #define VV(i) VV [i-1]
06058 #define AMAX1(i,j) i>j?i:j
06059 #define AMIN1(i,j) i<j?i:j
06060 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
06061 {
06062 float rad2deg =(180.0f/3.1415926f);
06063 float deg2rad = (3.1415926f/180.0f);
06064
06065 int NSAM,NROW,NNNN,NR2,NANG,L,JY;
06066
06067 NSAM = PROJ->get_xsize();
06068 NROW = PROJ->get_ysize();
06069
06070 if (PROJ->is_fftpadded()) {
06071 NSAM -= (PROJ->is_fftodd()) ? (1) : (2);
06072 }
06073
06074 NNNN = NSAM+2-(NSAM%2);
06075 NR2 = NROW/2;
06076 NANG = int(SS.size())/6;
06077
06078 float RI[9];
06079 RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
06080 RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
06081 RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
06082 RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
06083 RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
06084 RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
06085 RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
06086 RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
06087 RI(3,3)=SS(3,NUMP);
06088
06089 float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
06090
06091 EMData* W = new EMData();
06092 int Wnx = NNNN/2;
06093 W->set_size(NNNN/2,NROW,1);
06094 W->to_one();
06095 float *Wptr = W->get_data();
06096
06097 float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
06098
06099 for (L=1; L<=NANG; L++) {
06100 if (L != NUMP) {
06101 CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
06102 CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
06103 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);
06104
06105 TMP = sqrt(CC(1)*CC(1) + CC(2)*CC(2) + CC(3)*CC(3));
06106 CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
06107 ALPHA=rad2deg*float(asin(CCN));
06108 if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
06109 if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
06110 if(ALPHA<1.0E-6) {
06111 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
06112 } else {
06113 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
06114 CC(1) = CC(1)/CCN;CC(2) = CC(2)/CCN;CC(3) = CC(3)/CCN;
06115 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
06116 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
06117 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
06118 CP(1) = 0.0;CP(2) = 0.0;
06119 VP(1) = 0.0;VP(2) = 0.0;
06120
06121 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
06122 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
06123 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
06124 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
06125
06126 TMP = CP(1)*VP(2)-CP(2)*VP(1);
06127
06128
06129 TMP = AMAX1(1.0E-4f,fabs(TMP));
06130 float tmpinv = 1.0f/TMP;
06131 for(int J=1;J<=NROW;J++) {
06132 JY = (J-1);
06133 if (JY>NR2) JY=JY-NROW;
06134 for(int I=1;I<=NNNN/2;I++) {
06135 FV = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
06136 RT = 1.0f-FV/FM;
06137 W(I,J) += ((RT>0.0f)*RT);
06138 }
06139 }
06140 }
06141 }
06142 }
06143
06144 EMData* proj_in = PROJ;
06145 const bool realOnInput = PROJ->is_real();
06146
06147 if (realOnInput) {
06148
06149 PROJ = PROJ->norm_pad( false, 1 );
06150 PROJ->do_fft_inplace();
06151 PROJ->update();
06152 }
06153 float *PROJptr = PROJ->get_data();
06154
06155 int KX;
06156 float WW;
06157 for(int J=1; J<=NROW; J++)
06158 for(int I=1; I<=NNNN; I+=2) {
06159 KX = (I+1)/2;
06160 WW = 1.0f/W(KX,J);
06161 PROJ(I,J) = PROJ(I,J)*WW;
06162 PROJ(I+1,J) = PROJ(I+1,J)*WW;
06163 }
06164 delete W; W = 0;
06165 PROJ->do_ift_inplace();
06166 PROJ->depad();
06167
06168 if (realOnInput) {
06169
06170 float* data_src = PROJ->get_data();
06171 float* data_dst = proj_in->get_data();
06172 int ntotal = NSAM*NROW;
06173 memcpy( data_dst, data_src, ntotal * sizeof(float) );
06174 delete PROJ;
06175 }
06176
06177 proj_in->update();
06178 }
06179 #undef AMAX1
06180 #undef AMIN1
06181 #undef RI
06182 #undef CC
06183 #undef CP
06184 #undef VV
06185 #undef VP
06186 #undef W
06187 #undef SS
06188 #undef PROJ
06189
06190 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06191 {
06192 float cst = cs*1.0e7f;
06193
06194 wgh /= 100.0;
06195 float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06196 float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06197 float ak2 = ak*ak;
06198 float g1 = dzz*1.0e4f*lambda*ak2;
06199 float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06200
06201 float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06202 if(b_factor != 0.0f) ctfv *= exp(-b_factor*ak2/4.0f);
06203
06204 return ctfv;
06205 }
06206
06207 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06208 {
06209
06210
06211
06212 int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();
06213
06214
06215
06216 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06217 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06218
06219 size_t i, size = (size_t)nx*ny*nz;
06220
06221 float* img_ptr = image->get_data();
06222 float* mask_ptr = mask->get_data();
06223
06224 int ln=0;
06225 for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06226
06227 EMData* new_image = new EMData();
06228 new_image->set_size(ln,1,1);
06229 float *new_ptr = new_image->get_data();
06230
06231 ln=-1;
06232 for(i = 0;i < size;i++){
06233 if(mask_ptr[i] > 0.5f) {
06234 ln++;
06235 new_ptr[ln]=img_ptr[i];
06236 }
06237 }
06238
06239 return new_image;
06240 }
06241
06242 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06243 {
06244
06245
06246
06247 if(mask == NULL)
06248 throw ImageDimensionException("The mask cannot be an null image");
06249
06250
06251
06252
06253 int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06254
06255 size_t i,size = (size_t)nx*ny*nz;
06256
06257 EMData *new_image = new EMData();
06258 new_image->set_size(nx,ny,nz);
06259 float *new_ptr = new_image->get_data();
06260 float *mask_ptr = mask->get_data();
06261 float *img_ptr = image->get_data();
06262 int count = 0;
06263 float sum_under_mask = 0.0 ;
06264 for(i = 0;i < size;i++){
06265 if(mask_ptr[i] > 0.5f){
06266 new_ptr[i] = img_ptr[count];
06267 sum_under_mask += img_ptr[count];
06268 count++;
06269 if( count > image->get_xsize() ) {
06270 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06271 }
06272 }
06273 }
06274
06275 if( count > image->get_xsize() ) {
06276 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06277 }
06278
06279 float avg_under_mask = sum_under_mask / count;
06280 for(i = 0;i < size;i++) {
06281 if(mask_ptr[i] <= 0.5f) new_ptr[i] = avg_under_mask;
06282 }
06283 new_image->update();
06284 return new_image;
06285 }
06286
06287
06288
06289 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06290 {
06291 vector<float>new_peak;
06292 int n1=peak1.size()/3;
06293 float p_size2=p_size*p_size;
06294 for (int i=0;i<n1;++i) {
06295 vector<float>::iterator it2= peak1.begin()+3*i;
06296 bool push_back1=true;
06297 int n2=peak2.size()/3;
06298
06299
06300 if(n2 ==0) {
06301 new_peak.push_back(*it2);
06302 new_peak.push_back(*(it2+1));
06303 new_peak.push_back(*(it2+2));
06304 } else {
06305 int j=0;
06306 while (j< n2-1 ) {
06307 vector<float>::iterator it3= peak2.begin()+3*j;
06308 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06309 if(d2< p_size2 ) {
06310 if( (*it2)<(*it3) ) {
06311 new_peak.push_back(*it3);
06312 new_peak.push_back(*(it3+1));
06313 new_peak.push_back(*(it3+2));
06314 peak2.erase(it3);
06315 peak2.erase(it3);
06316 peak2.erase(it3);
06317 push_back1=false;
06318 } else {
06319 peak2.erase(it3);
06320 peak2.erase(it3);
06321 peak2.erase(it3);
06322 }
06323 } else j=j+1;
06324 n2=peak2.size()/3;
06325 }
06326 if(push_back1) {
06327 new_peak.push_back(*it2);
06328 new_peak.push_back(*(it2+1));
06329 new_peak.push_back(*(it2+2));
06330 }
06331 }
06332 }
06333 return new_peak;
06334 }
06335
06336 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06337 {
06338
06339
06340
06341
06342
06343 ENTERFUNC;
06344
06345 int i;
06346
06347
06348 for ( i = 0 ; i < n * n ; i++ ) eigvec[i] = covmat[i];
06349
06350 char NEEDV = 'V';
06351 char UPLO = 'U';
06352 int lwork = -1;
06353 int info = 0;
06354 float *work, wsize;
06355
06356
06357 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06358 lwork = (int)wsize;
06359
06360 work = (float *)calloc(lwork, sizeof(float));
06361
06362 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06363 free(work);
06364 EXITFUNC;
06365 return info;
06366 }
06367
06368 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06369 {
06370
06371 ENTERFUNC;
06372 int len = covmatpy.size();
06373 float *eigvec;
06374 float *eigval;
06375 float *covmat;
06376 int status = 0;
06377 eigval = (float*)calloc(ncov,sizeof(float));
06378 eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06379 covmat = (float*)calloc(ncov*ncov, sizeof(float));
06380
06381 const float *covmat_ptr;
06382 covmat_ptr = &covmatpy[0];
06383 for(int i=0;i<len;i++){
06384 covmat[i] = covmat_ptr[i];
06385 }
06386
06387 status = Util::coveig(ncov, covmat, eigval, eigvec);
06388
06389 vector<float> eigval_py(ncov);
06390 const float *eigval_ptr;
06391 eigval_ptr = &eigval[0];
06392 for(int i=0;i<ncov;i++){
06393 eigval_py[i] = eigval_ptr[i];
06394 }
06395
06396 vector<float> eigvec_py(ncov*ncov);
06397 const float *eigvec_ptr;
06398 eigvec_ptr = &eigvec[0];
06399 for(int i=0;i<ncov*ncov;i++){
06400 eigvec_py[i] = eigvec_ptr[i];
06401 }
06402
06403 Dict res;
06404 res["eigval"] = eigval_py;
06405 res["eigvec"] = eigvec_py;
06406
06407 EXITFUNC;
06408 return res;
06409 }
06410
06411 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06412 {
06413 int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06414
06415 k=(int)pw.size();
06416 l=0;
06417 m=k;
06418 n2=n+2;
06419 n1=n+1;
06420 klmd=k+l+m;
06421 klm2d= k+l+m+2;
06422 nklmd=k+l+m+n;
06423 n2d=n+2;
06424
06425 n_larg=klmd*2;
06426 klm2d=n_larg+klm2d;
06427 klmd=n_larg+klmd;
06428 nklmd=n_larg+nklmd;
06429 int size_q=klm2d*n2d;
06430 int size_cu=nklmd*2;
06431 static int i__;
06432
06433 double *q ;
06434 double *x ;
06435 double *res;
06436 double *cu;
06437 float *q2;
06438 float *pw_;
06439 long int *iu;
06440 double *s;
06441 q = (double*)calloc(size_q,sizeof(double));
06442 x = (double*)calloc(n2d,sizeof(double));
06443 res = (double*)calloc(klmd,sizeof(double));
06444 cu =(double*)calloc(size_cu,sizeof(double));
06445 s = (double*)calloc(klmd,sizeof(double));
06446 q2 = (float*)calloc(size_q,sizeof(float));
06447 iu = (long int*)calloc(size_cu,sizeof(long int));
06448 pw_ = (float*)calloc(k,sizeof(float));
06449
06450 for( i__ =0;i__<k;++i__)
06451 {
06452 pw_[i__]=log(pw[i__]); }
06453 long int l_k=k;
06454 long int l_n=n;
06455 long int l_iswi=iswi;
06456 vector<float> cl1_res;
06457 cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06458 free(q);
06459 free(x);
06460 free(res);
06461 free(s);
06462 free(cu);
06463 free(q2);
06464 free(iu);
06465 free(pw_);
06466 return cl1_res;
06467 }
06468 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)
06469 {
06470 long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06471 float r__1;
06472 int tmp__i;
06473 long int i__, j;
06474 --s;
06475 --res;
06476 iu -= 3;
06477 cu -= 3;
06478 --x;
06479 long int klm2d;
06480 klm2d= *k+*k+2;
06481 klm2d=klm2d+klm2d;
06482 q_dim1 = klm2d;
06483 q_offset = 1 + q_dim1;
06484 q -= q_offset;
06485 q2_dim1 = klm2d;
06486 q2_offset = 1 + q2_dim1;
06487 q2 -= q2_offset;
06488 i__2=0;
06489 i__1 = *n - 1;
06490 tmp__i=0;
06491 for (j = 1; j <= i__1; ++j) {
06492 i__2 = *k;
06493 tmp__i+=1;
06494 for (i__ = 1; i__ <= i__2; ++i__) {
06495 r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06496 q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06497 }
06498 }
06499 for (i__ = 1; i__ <= i__2; ++i__)
06500 { q2[i__ + *n * q2_dim1] = 1.f;
06501 q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06502 }
06503 vector<float> fit_res;
06504 fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06505 return fit_res;
06506 }
06507 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)
06508 {
06509
06510 long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06511
06512
06513 long int i__, j, m, n1, ii, jj;
06514 double tmp;
06515 vector<float> p;
06516 --x;
06517 q_dim1 = *klm2d;
06518 q_offset = 1 + q_dim1;
06519 q -= q_offset;
06520 q1_dim1 = *klm2d;
06521 q1_offset = 1 + q1_dim1;
06522 q1 -= q1_offset;
06523 --s;
06524 --res;
06525 iu -= 3;
06526 cu -= 3;
06527
06528
06529 long int l = 0;
06530
06531
06532 m = *ks;
06533 n1 = *n + 1;
06534 if (*iswi == 1) {
06535 i__1 = n1;
06536 for (jj = 1; jj <= i__1; ++jj) {
06537 i__2 = *ks;
06538 for (ii = 1; ii <= i__2; ++ii) {
06539
06540
06541 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06542 ;
06543 }
06544 }
06545 } else if (*iswi == 2) {
06546 i__1 = *ks;
06547 for (ii = 1; ii <= i__1; ++ii) {
06548 i__2 = n1;
06549 for (jj = 1; jj <= i__2; ++jj) {
06550 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06551 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06552 }
06553 }
06554 } else if (*iswi == 3) {
06555 l = 2;
06556 i__1 = n1;
06557 for (jj = 1; jj <= i__1; ++jj) {
06558 i__2 = *ks + 2;
06559 for (ii = 1; ii <= i__2; ++ii) {
06560 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06561 }
06562 i__2 = *ks;
06563 for (ii = 1; ii <= i__2; ++ii) {
06564 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06565 }
06566 }
06567 } else if (*iswi == 4) {
06568 l = 2;
06569 i__1 = n1;
06570 for (jj = 1; jj <= i__1; ++jj) {
06571 i__2 = *ks + 2;
06572 for (ii = 1; ii <= i__2; ++ii) {
06573 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06574 }
06575 i__2 = *ks;
06576 for (ii = 1; ii <= i__2; ++ii) {
06577 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06578 }
06579 }
06580 } else if (*iswi == 5) {
06581 l = 1;
06582 i__1 = n1;
06583 for (jj = 1; jj <= i__1; ++jj) {
06584 i__2 = *ks + 1;
06585 for (ii = 1; ii <= i__2; ++ii) {
06586 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06587 }
06588 i__2 = *ks;
06589 for (ii = 1; ii <= i__2; ++ii) {
06590 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06591 }
06592 }
06593 } else if (*iswi == 6) {
06594 l = 1;
06595 i__1 = n1;
06596 for (jj = 1; jj <= i__1; ++jj) {
06597 i__2 = *ks + 1;
06598 for (ii = 1; ii <= i__2; ++ii) {
06599 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06600 }
06601 i__2 = *ks;
06602 for (ii = 1; ii <= i__2; ++ii) {
06603 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06604 }
06605 }
06606 } else if (*iswi == 7) {
06607 l = 3;
06608 i__1 = n1;
06609 for (jj = 1; jj <= i__1; ++jj) {
06610 i__2 = *ks + 3;
06611 for (ii = 1; ii <= i__2; ++ii) {
06612 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06613 }
06614 i__2 = *ks;
06615 for (ii = 1; ii <= i__2; ++ii) {
06616 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06617 }
06618 }
06619 } else if (*iswi == 8) {
06620 l = 4;
06621 i__1 = n1;
06622 for (jj = 1; jj <= i__1; ++jj) {
06623 i__2 = *ks + 4;
06624 for (ii = 1; ii <= i__2; ++ii) {
06625 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06626 }
06627 i__2 = *ks;
06628 for (ii = 1; ii <= i__2; ++ii) {
06629 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06630 }
06631 }
06632 }
06633
06634 Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06635 i__1 = *ks;
06636 int tmp__j=0;
06637 for (i__ = 1; i__ <= i__1; ++i__) {
06638 tmp = 0.f;
06639 i__2 = *n - 1;
06640 for (j = 1; j <= i__2; ++j) {
06641 tmp__j=j;
06642 tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06643 }
06644 tmp += x[*n];
06645 p.push_back(static_cast<float>(exp(tmp)));
06646 p.push_back(q1[i__ + q1_dim1]);
06647 }
06648 i__2=*n;
06649 for (i__=1;i__<=i__2;++i__)
06650 { p.push_back(static_cast<float>(x[i__]));}
06651 return p;
06652 }
06653 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06654 double *q, double *x, double *res, double *cu, long int *iu, double *s)
06655 {
06656
06657 long int q_dim1, q_offset, i__1, i__2;
06658 double d__1;
06659
06660 static long int i__, j;
06661 static double z__;
06662 static long int n1, n2, ia, ii, kk, in, nk, js;
06663 static double sn, zu, zv;
06664 static long int nk1, klm, nkl, jmn, jpn;
06665 static double cuv;
06666 static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06667 static float xmin;
06668 static double xmax;
06669 static long int iout;
06670 static double xsum;
06671 static long int iineg, maxit;
06672 static double toler;
06673 static float error;
06674 static double pivot;
06675 static long int kforce, iphase;
06676 static double tpivot;
06677
06678 --s;
06679 --res;
06680 iu -= 3;
06681 cu -= 3;
06682 --x;
06683 q_dim1 = *klm2d;
06684 q_offset = 1 + q_dim1;
06685 q -= q_offset;
06686
06687
06688 maxit = 500;
06689 kode = 0;
06690 toler = 1e-4f;
06691 iter = 0;
06692 n1 = *n + 1;
06693 n2 = *n + 2;
06694 nk = *n + *k;
06695 nk1 = nk + 1;
06696 nkl = nk + *l;
06697 nkl1 = nkl + 1;
06698 klm = *k + *l + *m;
06699 klm1 = klm + 1;
06700 klm2 = klm + 2;
06701 nklm = *n + klm;
06702 kforce = 1;
06703 iter = 0;
06704 js = 1;
06705 ia = 0;
06706
06707 i__1 = *n;
06708 for (j = 1; j <= i__1; ++j) {
06709 q[klm2 + j * q_dim1] = (double) j;
06710
06711 }
06712 i__1 = klm;
06713 for (i__ = 1; i__ <= i__1; ++i__) {
06714 q[i__ + n2 * q_dim1] = (double) (*n + i__);
06715 if (q[i__ + n1 * q_dim1] >= 0.f) {
06716 goto L30;
06717 }
06718 i__2 = n2;
06719 for (j = 1; j <= i__2; ++j) {
06720 q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06721
06722 }
06723 L30:
06724 ;
06725 }
06726
06727 iphase = 2;
06728 i__1 = nklm;
06729 for (j = 1; j <= i__1; ++j) {
06730 cu[(j << 1) + 1] = 0.f;
06731 cu[(j << 1) + 2] = 0.f;
06732 iu[(j << 1) + 1] = 0;
06733 iu[(j << 1) + 2] = 0;
06734
06735 }
06736 if (*l == 0) {
06737 goto L60;
06738 }
06739 i__1 = nkl;
06740 for (j = nk1; j <= i__1; ++j) {
06741 cu[(j << 1) + 1] = 1.f;
06742 cu[(j << 1) + 2] = 1.f;
06743 iu[(j << 1) + 1] = 1;
06744 iu[(j << 1) + 2] = 1;
06745
06746 }
06747 iphase = 1;
06748 L60:
06749 if (*m == 0) {
06750 goto L80;
06751 }
06752 i__1 = nklm;
06753 for (j = nkl1; j <= i__1; ++j) {
06754 cu[(j << 1) + 2] = 1.f;
06755 iu[(j << 1) + 2] = 1;
06756 jmn = j - *n;
06757 if (q[jmn + n2 * q_dim1] < 0.f) {
06758 iphase = 1;
06759 }
06760
06761 }
06762 L80:
06763 if (kode == 0) {
06764 goto L150;
06765 }
06766 i__1 = *n;
06767 for (j = 1; j <= i__1; ++j) {
06768 if ((d__1 = x[j]) < 0.) {
06769 goto L90;
06770 } else if (d__1 == 0) {
06771 goto L110;
06772 } else {
06773 goto L100;
06774 }
06775 L90:
06776 cu[(j << 1) + 1] = 1.f;
06777 iu[(j << 1) + 1] = 1;
06778 goto L110;
06779 L100:
06780 cu[(j << 1) + 2] = 1.f;
06781 iu[(j << 1) + 2] = 1;
06782 L110:
06783 ;
06784 }
06785 i__1 = *k;
06786 for (j = 1; j <= i__1; ++j) {
06787 jpn = j + *n;
06788 if ((d__1 = res[j]) < 0.) {
06789 goto L120;
06790 } else if (d__1 == 0) {
06791 goto L140;
06792 } else {
06793 goto L130;
06794 }
06795 L120:
06796 cu[(jpn << 1) + 1] = 1.f;
06797 iu[(jpn << 1) + 1] = 1;
06798 if (q[j + n2 * q_dim1] > 0.f) {
06799 iphase = 1;
06800 }
06801 goto L140;
06802 L130:
06803 cu[(jpn << 1) + 2] = 1.f;
06804 iu[(jpn << 1) + 2] = 1;
06805 if (q[j + n2 * q_dim1] < 0.f) {
06806 iphase = 1;
06807 }
06808 L140:
06809 ;
06810 }
06811 L150:
06812 if (iphase == 2) {
06813 goto L500;
06814 }
06815
06816 L160:
06817 i__1 = n1;
06818 for (j = js; j <= i__1; ++j) {
06819 xsum = 0.;
06820 i__2 = klm;
06821 for (i__ = 1; i__ <= i__2; ++i__) {
06822 ii = (long int) q[i__ + n2 * q_dim1];
06823 if (ii < 0) {
06824 goto L170;
06825 }
06826 z__ = cu[(ii << 1) + 1];
06827 goto L180;
06828 L170:
06829 iineg = -ii;
06830 z__ = cu[(iineg << 1) + 2];
06831 L180:
06832 xsum += q[i__ + j * q_dim1] * z__;
06833
06834
06835 }
06836 q[klm1 + j * q_dim1] = xsum;
06837
06838 }
06839 i__1 = *n;
06840 for (j = js; j <= i__1; ++j) {
06841 ii = (long int) q[klm2 + j * q_dim1];
06842 if (ii < 0) {
06843 goto L210;
06844 }
06845 z__ = cu[(ii << 1) + 1];
06846 goto L220;
06847 L210:
06848 iineg = -ii;
06849 z__ = cu[(iineg << 1) + 2];
06850 L220:
06851 q[klm1 + j * q_dim1] -= z__;
06852
06853 }
06854
06855 L240:
06856 xmax = 0.f;
06857 if (js > *n) {
06858 goto L490;
06859 }
06860 i__1 = *n;
06861 for (j = js; j <= i__1; ++j) {
06862 zu = q[klm1 + j * q_dim1];
06863 ii = (long int) q[klm2 + j * q_dim1];
06864 if (ii > 0) {
06865 goto L250;
06866 }
06867 ii = -ii;
06868 zv = zu;
06869 zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06870 goto L260;
06871 L250:
06872 zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06873 L260:
06874 if (kforce == 1 && ii > *n) {
06875 goto L280;
06876 }
06877 if (iu[(ii << 1) + 1] == 1) {
06878 goto L270;
06879 }
06880 if (zu <= xmax) {
06881 goto L270;
06882 }
06883 xmax = zu;
06884 in = j;
06885 L270:
06886 if (iu[(ii << 1) + 2] == 1) {
06887 goto L280;
06888 }
06889 if (zv <= xmax) {
06890 goto L280;
06891 }
06892 xmax = zv;
06893 in = j;
06894 L280:
06895 ;
06896 }
06897 if (xmax <= toler) {
06898 goto L490;
06899 }
06900 if (q[klm1 + in * q_dim1] == xmax) {
06901 goto L300;
06902 }
06903 i__1 = klm2;
06904 for (i__ = 1; i__ <= i__1; ++i__) {
06905 q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06906
06907 }
06908 q[klm1 + in * q_dim1] = xmax;
06909
06910 L300:
06911 if (iphase == 1 || ia == 0) {
06912 goto L330;
06913 }
06914 xmax = 0.f;
06915 i__1 = ia;
06916 for (i__ = 1; i__ <= i__1; ++i__) {
06917 z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06918 if (z__ <= xmax) {
06919 goto L310;
06920 }
06921 xmax = z__;
06922 iout = i__;
06923 L310:
06924 ;
06925 }
06926 if (xmax <= toler) {
06927 goto L330;
06928 }
06929 i__1 = n2;
06930 for (j = 1; j <= i__1; ++j) {
06931 z__ = q[ia + j * q_dim1];
06932 q[ia + j * q_dim1] = q[iout + j * q_dim1];
06933 q[iout + j * q_dim1] = z__;
06934
06935 }
06936 iout = ia;
06937 --ia;
06938 pivot = q[iout + in * q_dim1];
06939 goto L420;
06940 L330:
06941 kk = 0;
06942 i__1 = klm;
06943 for (i__ = 1; i__ <= i__1; ++i__) {
06944 z__ = q[i__ + in * q_dim1];
06945 if (z__ <= toler) {
06946 goto L340;
06947 }
06948 ++kk;
06949 res[kk] = q[i__ + n1 * q_dim1] / z__;
06950 s[kk] = (double) i__;
06951 L340:
06952 ;
06953 }
06954 L350:
06955 if (kk > 0) {
06956 goto L360;
06957 }
06958 kode = 2;
06959 goto L590;
06960 L360:
06961 xmin = static_cast<float>( res[1] );
06962 iout = (long int) s[1];
06963 j = 1;
06964 if (kk == 1) {
06965 goto L380;
06966 }
06967 i__1 = kk;
06968 for (i__ = 2; i__ <= i__1; ++i__) {
06969 if (res[i__] >= xmin) {
06970 goto L370;
06971 }
06972 j = i__;
06973 xmin = static_cast<float>( res[i__] );
06974 iout = (long int) s[i__];
06975 L370:
06976 ;
06977 }
06978 res[j] = res[kk];
06979 s[j] = s[kk];
06980 L380:
06981 --kk;
06982 pivot = q[iout + in * q_dim1];
06983 ii = (long int) q[iout + n2 * q_dim1];
06984 if (iphase == 1) {
06985 goto L400;
06986 }
06987 if (ii < 0) {
06988 goto L390;
06989 }
06990 if (iu[(ii << 1) + 2] == 1) {
06991 goto L420;
06992 }
06993 goto L400;
06994 L390:
06995 iineg = -ii;
06996 if (iu[(iineg << 1) + 1] == 1) {
06997 goto L420;
06998 }
06999
07000 L400:
07001 ii = abs(ii);
07002 cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
07003 if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
07004 goto L420;
07005 }
07006
07007 i__1 = n1;
07008 for (j = js; j <= i__1; ++j) {
07009 z__ = q[iout + j * q_dim1];
07010 q[klm1 + j * q_dim1] -= z__ * cuv;
07011 q[iout + j * q_dim1] = -z__;
07012
07013 }
07014 q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
07015 goto L350;
07016
07017 L420:
07018 if (iter < maxit) {
07019 goto L430;
07020 }
07021 kode = 3;
07022 goto L590;
07023 L430:
07024 ++iter;
07025 i__1 = n1;
07026 for (j = js; j <= i__1; ++j) {
07027 if (j != in) {
07028 q[iout + j * q_dim1] /= pivot;
07029 }
07030
07031 }
07032
07033
07034
07035
07036
07037
07038
07039
07040 i__1 = n1;
07041 for (j = js; j <= i__1; ++j) {
07042 if (j == in) {
07043 goto L460;
07044 }
07045 z__ = -q[iout + j * q_dim1];
07046 i__2 = klm1;
07047 for (i__ = 1; i__ <= i__2; ++i__) {
07048 if (i__ != iout) {
07049 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
07050 }
07051
07052 }
07053 L460:
07054 ;
07055 }
07056 tpivot = -pivot;
07057 i__1 = klm1;
07058 for (i__ = 1; i__ <= i__1; ++i__) {
07059 if (i__ != iout) {
07060 q[i__ + in * q_dim1] /= tpivot;
07061 }
07062
07063 }
07064 q[iout + in * q_dim1] = 1.f / pivot;
07065 z__ = q[iout + n2 * q_dim1];
07066 q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
07067 q[klm2 + in * q_dim1] = z__;
07068 ii = (long int) abs(z__);
07069 if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
07070 goto L240;
07071 }
07072 i__1 = klm2;
07073 for (i__ = 1; i__ <= i__1; ++i__) {
07074 z__ = q[i__ + in * q_dim1];
07075 q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
07076 q[i__ + js * q_dim1] = z__;
07077
07078 }
07079 ++js;
07080 goto L240;
07081
07082 L490:
07083 if (kforce == 0) {
07084 goto L580;
07085 }
07086 if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
07087 goto L500;
07088 }
07089 kforce = 0;
07090 goto L240;
07091
07092 L500:
07093 iphase = 2;
07094 i__1 = nklm;
07095 for (j = 1; j <= i__1; ++j) {
07096 cu[(j << 1) + 1] = 0.f;
07097 cu[(j << 1) + 2] = 0.f;
07098
07099 }
07100 i__1 = nk;
07101 for (j = n1; j <= i__1; ++j) {
07102 cu[(j << 1) + 1] = 1.f;
07103 cu[(j << 1) + 2] = 1.f;
07104
07105 }
07106 i__1 = klm;
07107 for (i__ = 1; i__ <= i__1; ++i__) {
07108 ii = (long int) q[i__ + n2 * q_dim1];
07109 if (ii > 0) {
07110 goto L530;
07111 }
07112 ii = -ii;
07113 if (iu[(ii << 1) + 2] == 0) {
07114 goto L560;
07115 }
07116 cu[(ii << 1) + 2] = 0.f;
07117 goto L540;
07118 L530:
07119 if (iu[(ii << 1) + 1] == 0) {
07120 goto L560;
07121 }
07122 cu[(ii << 1) + 1] = 0.f;
07123 L540:
07124 ++ia;
07125 i__2 = n2;
07126 for (j = 1; j <= i__2; ++j) {
07127 z__ = q[ia + j * q_dim1];
07128 q[ia + j * q_dim1] = q[i__ + j * q_dim1];
07129 q[i__ + j * q_dim1] = z__;
07130
07131 }
07132 L560:
07133 ;
07134 }
07135 goto L160;
07136 L570:
07137 if (q[klm1 + n1 * q_dim1] <= toler) {
07138 goto L500;
07139 }
07140 kode = 1;
07141 goto L590;
07142 L580:
07143 if (iphase == 1) {
07144 goto L570;
07145 }
07146
07147 kode = 0;
07148 L590:
07149 xsum = 0.;
07150 i__1 = *n;
07151 for (j = 1; j <= i__1; ++j) {
07152 x[j] = 0.f;
07153
07154 }
07155 i__1 = klm;
07156 for (i__ = 1; i__ <= i__1; ++i__) {
07157 res[i__] = 0.f;
07158
07159 }
07160 i__1 = klm;
07161 for (i__ = 1; i__ <= i__1; ++i__) {
07162 ii = (long int) q[i__ + n2 * q_dim1];
07163 sn = 1.f;
07164 if (ii > 0) {
07165 goto L620;
07166 }
07167 ii = -ii;
07168 sn = -1.f;
07169 L620:
07170 if (ii > *n) {
07171 goto L630;
07172 }
07173 x[ii] = sn * q[i__ + n1 * q_dim1];
07174 goto L640;
07175 L630:
07176 iimn = ii - *n;
07177 res[iimn] = sn * q[i__ + n1 * q_dim1];
07178 if (ii >= n1 && ii <= nk) {
07179 xsum += q[i__ + n1 * q_dim1];
07180 }
07181 L640:
07182 ;
07183 }
07184 error = (float)xsum;
07185 return;
07186 }
07187
07188 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07189 {
07190 int j,d;
07191 EMData * e = new EMData();
07192 float *eptr, *imgptr;
07193 imgptr = img->get_data();
07194 float SSE = 0.f;
07195 for (j = 0 ; j < N ; j++) {
07196 e->read_image(images,S[j]);
07197 eptr = e->get_data();
07198 for (d = 0; d < size; d++) {
07199 SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07200 }
07201 delete e;
07202 return SSE;
07203 }
07204
07205
07206 #define mymax(x,y) (((x)>(y))?(x):(y))
07207 #define mymin(x,y) (((x)<(y))?(x):(y))
07208 #define sign(x,y) (((((y)>0)?(1):(-1))*(y!=0))*(x))
07209
07210
07211 #define quadpi 3.141592653589793238462643383279502884197
07212 #define dgr_to_rad quadpi/180
07213 #define deg_to_rad quadpi/180
07214 #define rad_to_deg 180/quadpi
07215 #define rad_to_dgr 180/quadpi
07216 #define TRUE 1
07217 #define FALSE 0
07218
07219
07220 #define theta(i) theta [i-1]
07221 #define phi(i) phi [i-1]
07222 #define weight(i) weight [i-1]
07223 #define lband(i) lband [i-1]
07224 #define ts(i) ts [i-1]
07225 #define thetast(i) thetast [i-1]
07226 #define key(i) key [i-1]
07227
07228
07229 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07230 {
07231
07232 ENTERFUNC;
07233
07234 if ( th.size() != ph.size() ) {
07235 LOGERR("images not same size");
07236 throw ImageFormatException( "images not same size");
07237 }
07238
07239
07240 srand(10);
07241
07242 int i,*key;
07243 int len = th.size();
07244 double *theta,*phi,*weight;
07245 theta = (double*) calloc(len,sizeof(double));
07246 phi = (double*) calloc(len,sizeof(double));
07247 weight = (double*) calloc(len,sizeof(double));
07248 key = (int*) calloc(len,sizeof(int));
07249 const float *thptr, *phptr;
07250
07251 thptr = &th[0];
07252 phptr = &ph[0];
07253 for(i=1;i<=len;i++){
07254 key(i) = i;
07255 weight(i) = 0.0;
07256 }
07257
07258 for(i = 0;i<len;i++){
07259 theta[i] = thptr[i];
07260 phi[i] = phptr[i];
07261 }
07262
07263
07264 Util::hsortd(theta, phi, key, len, 1);
07265
07266
07267 Util::voronoi(phi, theta, weight, len);
07268
07269
07270 Util::hsortd(weight, weight, key, len, 2);
07271
07272 free(theta);
07273 free(phi);
07274 free(key);
07275 vector<double> wt;
07276 double count = 0;
07277 for(i=1; i<= len; i++)
07278 {
07279 wt.push_back(weight(i));
07280 count += weight(i);
07281 }
07282
07283
07284
07285
07286
07287
07288 free(weight);
07289
07290 EXITFUNC;
07291 return wt;
07292
07293 }
07294
07295 struct tmpstruct{
07296 double theta1,phi1;
07297 int key1;
07298 };
07299
07300 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07301 {
07302 ENTERFUNC;
07303 vector<tmpstruct> tmp(len);
07304 int i;
07305 for(i = 1;i<=len;i++)
07306 {
07307 tmp[i-1].theta1 = theta(i);
07308 tmp[i-1].phi1 = phi(i);
07309 tmp[i-1].key1 = key(i);
07310 }
07311
07312 if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07313 if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07314
07315 for(i = 1;i<=len;i++)
07316 {
07317 theta(i) = tmp[i-1].theta1;
07318 phi(i) = tmp[i-1].phi1;
07319 key(i) = tmp[i-1].key1;
07320 }
07321 EXITFUNC;
07322 }
07323
07324 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07325 {
07326 return(tmp1.theta1 < tmp2.theta1);
07327 }
07328
07329 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07330 {
07331 return(tmp1.key1 < tmp2.key1);
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
07571
07572
07573
07574
07575
07576
07577
07578
07579
07580
07581
07582
07583
07584
07585
07586
07587
07588
07589
07590
07591
07592
07593
07594
07595
07596
07597
07598
07599
07600
07601
07602
07603
07604
07605
07606
07607
07608
07609
07610
07611
07612
07613
07614
07615
07616
07617
07618
07619
07620
07621
07622
07623
07624
07625
07626
07627
07628
07629
07630
07631
07632
07633
07634
07635
07636
07637
07638
07639
07640
07641
07642
07643
07644
07645
07646
07647
07648
07649
07650
07651
07652 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07653 {
07654
07655 ENTERFUNC;
07656
07657 int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07658 int nt6, n, ier, nout, lnew, mdup, nd;
07659 int i,k,mt,status;
07660
07661
07662 double *ds, *x, *y, *z;
07663 double tol = 1.0e-8;
07664 double dtol = 15;
07665 double a;
07666
07667
07668
07669
07670
07671
07672
07673
07674
07675 n = nt + nt;
07676
07677 nt6 = n*6;
07678
07679 list = (int*)calloc(nt6,sizeof(int));
07680 lptr = (int*)calloc(nt6,sizeof(int));
07681 lend = (int*)calloc(n ,sizeof(int));
07682 iwk = (int*)calloc(n ,sizeof(int));
07683 good = (int*)calloc(n ,sizeof(int));
07684 key = (int*)calloc(n ,sizeof(int));
07685 indx = (int*)calloc(n ,sizeof(int));
07686 lcnt = (int*)calloc(n ,sizeof(int));
07687
07688 ds = (double*) calloc(n,sizeof(double));
07689 x = (double*) calloc(n,sizeof(double));
07690 y = (double*) calloc(n,sizeof(double));
07691 z = (double*) calloc(n,sizeof(double));
07692
07693 if (list == NULL ||
07694 lptr == NULL ||
07695 lend == NULL ||
07696 iwk == NULL ||
07697 good == NULL ||
07698 key == NULL ||
07699 indx == NULL ||
07700 lcnt == NULL ||
07701 x == NULL ||
07702 y == NULL ||
07703 z == NULL ||
07704 ds == NULL) {
07705 printf("memory allocation failure!\n");
07706 exit(1);
07707 }
07708
07709 bool colinear=true;
07710 while(colinear)
07711 {
07712
07713 L1:
07714 for(i = 0; i<nt; i++){
07715 x[i] = theta[i];
07716 y[i] = phi[i];
07717 x[nt+i] = 180.0 - x[i];
07718 y[nt+i] = 180.0 + y[i];
07719 }
07720
07721 Util::disorder2(x, y, key, n);
07722
07723
07724 double val;
07725 for(k=0; k<2; k++){
07726 for(i=k+1; i<3; i++){
07727 val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07728 if( val < dtol) {
07729 goto L1;
07730 }
07731 }
07732 }
07733
07734 Util::ang_to_xyz(x, y, z, n);
07735
07736
07737 bool dupnode=true;
07738 dupnode=true;
07739 while(dupnode)
07740 {
07741 for(k=0; k<2; k++){
07742 for(i=k+1; i<3; i++){
07743 if( x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07744 Util::flip23(x, y, z, key, k, n);
07745 continue;
07746 }
07747 }
07748 }
07749 dupnode = false;
07750 }
07751
07752
07753 ier = 0;
07754
07755 status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07756
07757 if (status != 0) {
07758 printf(" error in trmsh3 \n");
07759 exit(1);
07760 }
07761
07762 if (ier > 0) {
07763 printf("*** Error in TRMESH: duplicate nodes encountered ***\n");
07764 exit(1);
07765 }
07766
07767 mdup=n-nout;
07768 if (ier == -2) {
07769
07770 }
07771 else
07772 {
07773 colinear=false;
07774 }
07775 }
07776
07777
07778 Assert( ier != -2 );
07779
07780
07781 nd=0;
07782 for (k=1; k<=n; k++){
07783 if (indx[k-1]>0) {
07784 nd++;
07785 good[nd-1]=k;
07786 }
07787 }
07788
07789
07790
07791
07792 for(i = 1; i<=nout; i++) {
07793 k=good[i-1];
07794
07795 if (key[k-1] <= nt) {
07796
07797 a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07798 if (ier != 0){
07799
07800
07801 printf(" *** error in areav: ier = %d ***\n", ier);
07802 weight[key[k-1]-1] =-1.0;
07803 } else {
07804
07805 weight[key[k-1]-1]=a/lcnt[i-1];
07806 }
07807 }
07808 }
07809
07810
07811
07812 for(i = 1; i<=n; i++){
07813 mt =- indx[i-1];
07814 if (mt>0){
07815 k = good[mt-1];
07816
07817
07818
07819 if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07820 }
07821 }
07822
07823 free(list);
07824 free(lend);
07825 free(iwk);
07826 free(good);
07827 free(key);
07828 free(lptr);
07829 free(indx);
07830 free(lcnt);
07831 free(ds);
07832 free(x);
07833 free(y);
07834 free(z);
07835
07836
07837 EXITFUNC;
07838 }
07839
07840 void Util::disorder2(double *x,double *y, int *key, int len)
07841 {
07842 ENTERFUNC;
07843 int k, i;
07844 for(i=0; i<len; i++) key[i]=i+1;
07845
07846 for(i = 0; i<len;i++){
07847 k = rand()%len;
07848 std::swap(key[k], key[i]);
07849 std::swap(x[k], x[i]);
07850 std::swap(y[k], y[i]);
07851 }
07852 EXITFUNC;
07853 }
07854
07855 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07856 {
07857 ENTERFUNC;
07858 double costheta,sintheta,cosphi,sinphi;
07859 for(int i = 0; i<len; i++)
07860 {
07861 cosphi = cos(y[i]*dgr_to_rad);
07862 sinphi = sin(y[i]*dgr_to_rad);
07863 if(fabs(x[i]-90.0)< 1.0e-5){
07864 x[i] = cosphi;
07865 y[i] = sinphi;
07866 z[i] = 0.0;
07867 }
07868 else{
07869 costheta = cos(x[i]*dgr_to_rad);
07870 sintheta = sin(x[i]*dgr_to_rad);
07871 x[i] = cosphi*sintheta;
07872 y[i] = sinphi*sintheta;
07873 z[i] = costheta;
07874 }
07875 }
07876 EXITFUNC;
07877 }
07878
07879 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07880 {
07881 ENTERFUNC;
07882 int i = k;
07883 while( i == k ) i = rand()%len;
07884 std::swap(key[i], key[k]);
07885 std::swap(x[i], x[k]);
07886 std::swap(y[i], y[k]);
07887 std::swap(z[i], z[k]);
07888 EXITFUNC;
07889 }
07890
07891
07892 #undef mymax
07893 #undef mymin
07894 #undef sign
07895 #undef quadpi
07896 #undef dgr_to_rad
07897 #undef deg_to_rad
07898 #undef rad_to_deg
07899 #undef rad_to_dgr
07900 #undef TRUE
07901 #undef FALSE
07902 #undef theta
07903 #undef phi
07904 #undef weight
07905 #undef lband
07906 #undef ts
07907 #undef thetast
07908 #undef key
07909
07910
07911
07912
07913
07914
07915
07916
07917
07918
07919
07920 #define TRUE_ (1)
07921 #define FALSE_ (0)
07922 #define abs(x) ((x) >= 0 ? (x) : -(x))
07923
07924 struct stcom_{
07925 double y;
07926 };
07927 stcom_ stcom_1;
07928 #ifdef KR_headers
07929 double floor();
07930 int i_dnnt(x) double *x;
07931 #else
07932 int i_dnnt(double *x)
07933 #endif
07934 {
07935 return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07936 }
07937
07938
07939
07940
07941
07942 int Util::trmsh3_(int *n0, double *tol, double *x,
07943 double *y, double *z__, int *n, int *list, int *
07944 lptr, int *lend, int *lnew, int *indx, int *lcnt,
07945 int *near__, int *next, double *dist, int *ier)
07946 {
07947
07948 int i__1, i__2;
07949
07950
07951 static double d__;
07952 static int i__, j;
07953 static double d1, d2, d3;
07954 static int i0, lp, kt, ku, lpl, nku;
07955 static int nexti;
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
08026
08027
08028
08029
08030
08031
08032
08033
08034
08035
08036
08037
08038
08039
08040
08041
08042
08043
08044
08045
08046
08047
08048
08049
08050
08051
08052
08053
08054
08055
08056
08057
08058
08059
08060
08061
08062
08063
08064
08065
08066
08067
08068
08069
08070
08071
08072
08073
08074
08075
08076
08077
08078
08079
08080
08081
08082
08083
08084
08085
08086
08087
08088
08089
08090
08091
08092
08093
08094
08095
08096
08097
08098
08099
08100
08101
08102
08103
08104
08105
08106
08107 --dist;
08108 --next;
08109 --near__;
08110 --indx;
08111 --lend;
08112 --z__;
08113 --y;
08114 --x;
08115 --list;
08116 --lptr;
08117 --lcnt;
08118
08119
08120 if (*n0 < 3) {
08121 *n = 0;
08122 *ier = -1;
08123 return 0;
08124 }
08125
08126
08127
08128 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
08129 z__[3])) {
08130
08131
08132
08133 list[1] = 3;
08134 lptr[1] = 2;
08135 list[2] = -2;
08136 lptr[2] = 1;
08137 lend[1] = 2;
08138
08139 list[3] = 1;
08140 lptr[3] = 4;
08141 list[4] = -3;
08142 lptr[4] = 3;
08143 lend[2] = 4;
08144
08145 list[5] = 2;
08146 lptr[5] = 6;
08147 list[6] = -1;
08148 lptr[6] = 5;
08149 lend[3] = 6;
08150
08151 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
08152 y[3], &z__[3])) {
08153
08154
08155
08156
08157
08158 list[1] = 2;
08159 lptr[1] = 2;
08160 list[2] = -3;
08161 lptr[2] = 1;
08162 lend[1] = 2;
08163
08164 list[3] = 3;
08165 lptr[3] = 4;
08166 list[4] = -1;
08167 lptr[4] = 3;
08168 lend[2] = 4;
08169
08170 list[5] = 1;
08171 lptr[5] = 6;
08172 list[6] = -2;
08173 lptr[6] = 5;
08174 lend[3] = 6;
08175
08176
08177 } else {
08178
08179
08180
08181 *n = 0;
08182 *ier = -2;
08183 return 0;
08184 }
08185
08186
08187
08188
08189
08190 *lnew = 7;
08191 indx[1] = 1;
08192 indx[2] = 2;
08193 indx[3] = 3;
08194 lcnt[1] = 1;
08195 lcnt[2] = 1;
08196 lcnt[3] = 1;
08197 if (*n0 == 3) {
08198 *n = 3;
08199 *ier = 0;
08200 return 0;
08201 }
08202
08203
08204
08205
08206
08207
08208
08209
08210
08211
08212
08213
08214
08215
08216
08217
08218
08219
08220
08221
08222
08223
08224
08225
08226
08227
08228
08229 near__[1] = 0;
08230 near__[2] = 0;
08231 near__[3] = 0;
08232 for (ku = *n0; ku >= 4; --ku) {
08233 d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08234 d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08235 d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08236 if (d1 <= d2 && d1 <= d3) {
08237 near__[ku] = 1;
08238 dist[ku] = d1;
08239 next[ku] = near__[1];
08240 near__[1] = ku;
08241 } else if (d2 <= d1 && d2 <= d3) {
08242 near__[ku] = 2;
08243 dist[ku] = d2;
08244 next[ku] = near__[2];
08245 near__[2] = ku;
08246 } else {
08247 near__[ku] = 3;
08248 dist[ku] = d3;
08249 next[ku] = near__[3];
08250 near__[3] = ku;
08251 }
08252
08253 }
08254
08255
08256
08257
08258 kt = 3;
08259 i__1 = *n0;
08260 for (ku = 4; ku <= i__1; ++ku) {
08261 nku = near__[ku];
08262
08263
08264
08265 i__ = nku;
08266 if (near__[i__] == ku) {
08267 near__[i__] = next[ku];
08268 } else {
08269 i__ = near__[i__];
08270 L2:
08271 i0 = i__;
08272 i__ = next[i0];
08273 if (i__ != ku) {
08274 goto L2;
08275 }
08276 next[i0] = next[ku];
08277 }
08278 near__[ku] = 0;
08279
08280
08281
08282 if (dist[ku] <= *tol - 1.) {
08283 indx[ku] = -nku;
08284 ++lcnt[nku];
08285 goto L6;
08286 }
08287
08288
08289
08290 ++kt;
08291 x[kt] = x[ku];
08292 y[kt] = y[ku];
08293 z__[kt] = z__[ku];
08294 indx[ku] = kt;
08295 lcnt[kt] = 1;
08296 addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08297 , lnew, ier);
08298 if (*ier != 0) {
08299 *n = 0;
08300 *ier = -3;
08301 return 0;
08302 }
08303
08304
08305
08306 lpl = lend[kt];
08307 lp = lpl;
08308 L3:
08309 lp = lptr[lp];
08310 j = (i__2 = list[lp], abs(i__2));
08311
08312
08313
08314
08315
08316
08317
08318 i__ = near__[j];
08319 L4:
08320 if (i__ == 0) {
08321 goto L5;
08322 }
08323 nexti = next[i__];
08324
08325
08326
08327
08328 d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08329 if (d__ < dist[i__]) {
08330
08331
08332
08333
08334
08335 near__[i__] = kt;
08336 dist[i__] = d__;
08337 if (i__ == near__[j]) {
08338 near__[j] = nexti;
08339 } else {
08340 next[i0] = nexti;
08341 }
08342 next[i__] = near__[kt];
08343 near__[kt] = i__;
08344 } else {
08345 i0 = i__;
08346 }
08347
08348
08349
08350 i__ = nexti;
08351 goto L4;
08352
08353
08354
08355 L5:
08356 if (lp != lpl) {
08357 goto L3;
08358 }
08359 L6:
08360 ;
08361 }
08362 *n = kt;
08363 *ier = 0;
08364 return 0;
08365 }
08366
08367
08368 int addnod_(int *nst, int *k, double *x,
08369 double *y, double *z__, int *list, int *lptr, int
08370 *lend, int *lnew, int *ier)
08371 {
08372
08373
08374 static double tol = 0.;
08375
08376
08377 int i__1;
08378
08379
08380 static int l;
08381 static double p[3], b1, b2, b3;
08382 static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08383 int swap_(int *, int *, int *,
08384 int *, int *, int *, int *, int *);
08385 static int lpo1s;
08386 int bdyadd_(int *, int *, int *,
08387 int *, int *, int *, int *), intadd_(int *,
08388 int *, int *, int *, int *, int *, int *,
08389 int *), trfind_(int *, double *, int *,
08390 double *, double *, double *, int *, int *,
08391 int *, double *, double *, double *, int *,
08392 int *, int *), covsph_(int *, int *, int *,
08393 int *, int *, int *);
08394 int lstptr_(int *, int *, int *, int *);
08395 long int swptst_(int *, int *, int *, int *,
08396 double *, double *, double *);
08397
08398
08399
08400
08401
08402
08403
08404
08405
08406
08407
08408
08409
08410
08411
08412
08413
08414
08415
08416
08417
08418
08419
08420
08421
08422
08423
08424
08425
08426
08427
08428
08429
08430
08431
08432
08433
08434
08435
08436
08437
08438
08439
08440
08441
08442
08443
08444
08445
08446
08447
08448
08449
08450
08451
08452
08453
08454
08455
08456
08457
08458
08459
08460
08461
08462
08463
08464
08465
08466
08467
08468
08469
08470
08471
08472
08473
08474
08475
08476
08477
08478
08479
08480
08481
08482
08483
08484
08485
08486
08487
08488
08489
08490
08491
08492
08493
08494
08495
08496
08497
08498
08499 --lend;
08500 --z__;
08501 --y;
08502 --x;
08503 --list;
08504 --lptr;
08505
08506
08507
08508 kk = *k;
08509 if (kk < 4) {
08510 goto L3;
08511 }
08512
08513
08514 km1 = kk - 1;
08515 ist = *nst;
08516 if (ist < 1) {
08517 ist = km1;
08518 }
08519 p[0] = x[kk];
08520 p[1] = y[kk];
08521 p[2] = z__[kk];
08522
08523
08524
08525
08526 trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08527 , &b1, &b2, &b3, &i1, &i2, &i3);
08528
08529
08530
08531 if (i1 == 0) {
08532 goto L4;
08533 }
08534 l = i1;
08535 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08536 goto L5;
08537 }
08538 l = i2;
08539 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08540 goto L5;
08541 }
08542 if (i3 != 0) {
08543 l = i3;
08544 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08545 goto L5;
08546 }
08547 intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08548 } else {
08549 if (i1 != i2) {
08550 bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08551 } else {
08552 covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08553 }
08554 }
08555 *ier = 0;
08556
08557
08558
08559 lp = lend[kk];
08560 lpf = lptr[lp];
08561 io2 = list[lpf];
08562 lpo1 = lptr[lpf];
08563 io1 = (i__1 = list[lpo1], abs(i__1));
08564
08565
08566
08567 L1:
08568 lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08569 if (list[lp] < 0) {
08570 goto L2;
08571 }
08572 lp = lptr[lp];
08573 in1 = (i__1 = list[lp], abs(i__1));
08574
08575
08576
08577
08578 lpo1s = lpo1;
08579 if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08580 goto L2;
08581 }
08582 swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08583 if (lpo1 == 0) {
08584
08585
08586
08587
08588
08589
08590 lpo1 = lpo1s;
08591 goto L2;
08592 }
08593 io1 = in1;
08594 goto L1;
08595
08596
08597
08598
08599 L2:
08600 if (lpo1 == lpf || list[lpo1] < 0) {
08601 return 0;
08602 }
08603 io2 = io1;
08604 lpo1 = lptr[lpo1];
08605 io1 = (i__1 = list[lpo1], abs(i__1));
08606 goto L1;
08607
08608
08609
08610 L3:
08611 *ier = -1;
08612 return 0;
08613
08614
08615
08616 L4:
08617 *ier = -2;
08618 return 0;
08619
08620
08621
08622 L5:
08623 *ier = l;
08624 return 0;
08625 }
08626
08627 double angle_(double *v1, double *v2, double *v3)
08628 {
08629
08630 double ret_val;
08631
08632
08633
08634
08635
08636 static double a;
08637 static int i__;
08638 static double ca, s21, s23, u21[3], u23[3];
08639
08640
08641
08642
08643
08644
08645
08646
08647
08648
08649
08650
08651
08652
08653
08654
08655
08656
08657
08658
08659
08660
08661
08662
08663
08664
08665
08666
08667
08668
08669
08670
08671
08672
08673
08674
08675
08676
08677
08678
08679
08680
08681
08682
08683
08684
08685
08686
08687
08688
08689
08690
08691
08692
08693
08694
08695
08696
08697
08698 --v3;
08699 --v2;
08700 --v1;
08701
08702
08703 u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08704 u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08705 u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08706
08707 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08708 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08709 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08710
08711
08712
08713 s21 = 0.;
08714 s23 = 0.;
08715 for (i__ = 1; i__ <= 3; ++i__) {
08716 s21 += u21[i__ - 1] * u21[i__ - 1];
08717 s23 += u23[i__ - 1] * u23[i__ - 1];
08718
08719 }
08720
08721
08722
08723
08724 if (s21 == 0. || s23 == 0.) {
08725 ret_val = 0.;
08726 return ret_val;
08727 }
08728 s21 = sqrt(s21);
08729 s23 = sqrt(s23);
08730 for (i__ = 1; i__ <= 3; ++i__) {
08731 u21[i__ - 1] /= s21;
08732 u23[i__ - 1] /= s23;
08733
08734 }
08735
08736
08737
08738
08739
08740 ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08741 if (ca < -1.) {
08742 ca = -1.;
08743 }
08744 if (ca > 1.) {
08745 ca = 1.;
08746 }
08747 a = acos(ca);
08748
08749
08750
08751
08752 if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08753 , &v3[3])) {
08754 a = acos(-1.) * 2. - a;
08755 }
08756 ret_val = a;
08757 return ret_val;
08758 }
08759
08760 double areas_(double *v1, double *v2, double *v3)
08761 {
08762
08763 double ret_val;
08764
08765
08766
08767
08768
08769 static int i__;
08770 static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08771 ca2, ca3;
08772
08773
08774
08775
08776
08777
08778
08779
08780
08781
08782
08783
08784
08785
08786
08787
08788
08789
08790
08791
08792
08793
08794
08795
08796
08797
08798
08799
08800
08801
08802
08803
08804
08805
08806
08807
08808
08809
08810
08811
08812
08813
08814
08815
08816
08817
08818
08819
08820
08821
08822
08823
08824
08825 --v3;
08826 --v2;
08827 --v1;
08828
08829
08830 u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08831 u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08832 u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08833
08834 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08835 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08836 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08837
08838 u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08839 u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08840 u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08841
08842
08843
08844 s12 = 0.;
08845 s23 = 0.;
08846 s31 = 0.;
08847 for (i__ = 1; i__ <= 3; ++i__) {
08848 s12 += u12[i__ - 1] * u12[i__ - 1];
08849 s23 += u23[i__ - 1] * u23[i__ - 1];
08850 s31 += u31[i__ - 1] * u31[i__ - 1];
08851
08852 }
08853
08854
08855
08856
08857 if (s12 == 0. || s23 == 0. || s31 == 0.) {
08858 ret_val = 0.;
08859 return ret_val;
08860 }
08861 s12 = sqrt(s12);
08862 s23 = sqrt(s23);
08863 s31 = sqrt(s31);
08864 for (i__ = 1; i__ <= 3; ++i__) {
08865 u12[i__ - 1] /= s12;
08866 u23[i__ - 1] /= s23;
08867 u31[i__ - 1] /= s31;
08868
08869 }
08870
08871
08872
08873
08874
08875
08876
08877 ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08878 ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08879 ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08880 if (ca1 < -1.) {
08881 ca1 = -1.;
08882 }
08883 if (ca1 > 1.) {
08884 ca1 = 1.;
08885 }
08886 if (ca2 < -1.) {
08887 ca2 = -1.;
08888 }
08889 if (ca2 > 1.) {
08890 ca2 = 1.;
08891 }
08892 if (ca3 < -1.) {
08893 ca3 = -1.;
08894 }
08895 if (ca3 > 1.) {
08896 ca3 = 1.;
08897 }
08898 a1 = acos(ca1);
08899 a2 = acos(ca2);
08900 a3 = acos(ca3);
08901
08902
08903
08904 ret_val = a1 + a2 + a3 - acos(-1.);
08905 if (ret_val < 0.) {
08906 ret_val = 0.;
08907 }
08908 return ret_val;
08909 }
08910
08911
08912
08913 double Util::areav_(int *k, int *n, double *x, double *y,
08914 double *z__, int *list, int *lptr, int *lend, int
08915 *ier)
08916 {
08917
08918
08919 static double amax = 6.28;
08920
08921
08922 double ret_val;
08923
08924
08925 static double a, c0[3], c2[3], c3[3];
08926 static int n1, n2, n3;
08927 static double v1[3], v2[3], v3[3];
08928 static int lp, lpl, ierr;
08929 static double asum;
08930 static long int first;
08931
08932
08933
08934
08935
08936
08937
08938
08939
08940
08941
08942
08943
08944
08945
08946
08947
08948
08949
08950
08951
08952
08953
08954
08955
08956
08957
08958
08959
08960
08961
08962
08963
08964
08965
08966
08967
08968
08969
08970
08971
08972
08973
08974
08975
08976
08977
08978
08979
08980
08981
08982
08983
08984
08985
08986
08987
08988
08989 --lend;
08990 --z__;
08991 --y;
08992 --x;
08993 --list;
08994 --lptr;
08995
08996
08997
08998
08999
09000 if (*k < 1 || *k > *n || *n <= 3) {
09001 goto L11;
09002 }
09003
09004
09005
09006
09007
09008 n1 = *k;
09009 v1[0] = x[n1];
09010 v1[1] = y[n1];
09011 v1[2] = z__[n1];
09012 lpl = lend[n1];
09013 n3 = list[lpl];
09014 if (n3 < 0) {
09015 goto L12;
09016 }
09017 lp = lpl;
09018 first = TRUE_;
09019 asum = 0.;
09020
09021
09022
09023 L1:
09024 n2 = n3;
09025 lp = lptr[lp];
09026 n3 = list[lp];
09027 v2[0] = x[n2];
09028 v2[1] = y[n2];
09029 v2[2] = z__[n2];
09030 v3[0] = x[n3];
09031 v3[1] = y[n3];
09032 v3[2] = z__[n3];
09033 if (first) {
09034
09035
09036
09037
09038 circum_(v1, v2, v3, c3, &ierr);
09039 if (ierr != 0) {
09040 goto L13;
09041 }
09042 c0[0] = c3[0];
09043 c0[1] = c3[1];
09044 c0[2] = c3[2];
09045 first = FALSE_;
09046 } else {
09047
09048
09049
09050
09051 c2[0] = c3[0];
09052 c2[1] = c3[1];
09053 c2[2] = c3[2];
09054 circum_(v1, v2, v3, c3, &ierr);
09055 if (ierr != 0) {
09056 goto L13;
09057 }
09058 a = areas_(v1, c2, c3);
09059 if (a > amax) {
09060 goto L14;
09061 }
09062 asum += a;
09063 }
09064
09065
09066
09067 if (lp != lpl) {
09068 goto L1;
09069 }
09070
09071
09072
09073 a = areas_(v1, c3, c0);
09074 if (a > amax) {
09075 goto L14;
09076 }
09077 asum += a;
09078
09079
09080
09081 *ier = 0;
09082 ret_val = asum;
09083 return ret_val;
09084
09085
09086
09087 L11:
09088 *ier = 1;
09089 ret_val = 0.;
09090 return ret_val;
09091
09092
09093
09094 L12:
09095 *ier = 2;
09096 ret_val = 0.;
09097 return ret_val;
09098
09099
09100
09101 L13:
09102 *ier = 3;
09103 ret_val = 0.;
09104 return ret_val;
09105
09106
09107
09108 L14:
09109 *ier = 4;
09110 ret_val = 0.;
09111 return ret_val;
09112 }
09113
09114 double areav_new__(int *k, int *n, double *x, double *y,
09115 double *z__, int *list, int *lptr, int *lend, int
09116 *ier)
09117 {
09118
09119 double ret_val = 0;
09120
09121
09122
09123
09124
09125 static int m;
09126 static double c1[3], c2[3], c3[3];
09127 static int n1, n2, n3;
09128 static double v1[3], v2[3], v3[3];
09129 static int lp;
09130 static double c1s[3], c2s[3];
09131 static int lpl, ierr;
09132 static double asum;
09133 double angle_(double *, double *, double *);
09134 static float areav;
09135
09136
09137
09138
09139
09140
09141
09142
09143
09144
09145
09146
09147
09148
09149
09150
09151
09152
09153
09154
09155
09156
09157
09158
09159
09160
09161
09162
09163
09164
09165
09166
09167
09168
09169
09170
09171
09172
09173
09174
09175
09176
09177
09178
09179
09180
09181
09182
09183
09184
09185
09186
09187
09188
09189
09190
09191
09192
09193
09194
09195
09196 --lend;
09197 --z__;
09198 --y;
09199 --x;
09200 --list;
09201 --lptr;
09202
09203
09204 if (*k < 1 || *k > *n || *n <= 3) {
09205 goto L11;
09206 }
09207
09208
09209
09210
09211
09212 n1 = *k;
09213 v1[0] = x[n1];
09214 v1[1] = y[n1];
09215 v1[2] = z__[n1];
09216 lpl = lend[n1];
09217 n3 = list[lpl];
09218 if (n3 < 0) {
09219 goto L12;
09220 }
09221 lp = lpl;
09222 m = 0;
09223 asum = 0.;
09224
09225
09226
09227 L1:
09228 ++m;
09229 n2 = n3;
09230 lp = lptr[lp];
09231 n3 = list[lp];
09232 v2[0] = x[n2];
09233 v2[1] = y[n2];
09234 v2[2] = z__[n2];
09235 v3[0] = x[n3];
09236 v3[1] = y[n3];
09237 v3[2] = z__[n3];
09238 if (m == 1) {
09239
09240
09241
09242
09243 circum_(v1, v2, v3, c2, &ierr);
09244 if (ierr != 0) {
09245 goto L13;
09246 }
09247 c1s[0] = c2[0];
09248 c1s[1] = c2[1];
09249 c1s[2] = c2[2];
09250 } else if (m == 2) {
09251
09252
09253
09254
09255 circum_(v1, v2, v3, c3, &ierr);
09256 if (ierr != 0) {
09257 goto L13;
09258 }
09259 c2s[0] = c3[0];
09260 c2s[1] = c3[1];
09261 c2s[2] = c3[2];
09262 } else {
09263
09264
09265
09266
09267
09268 c1[0] = c2[0];
09269 c1[1] = c2[1];
09270 c1[2] = c2[2];
09271 c2[0] = c3[0];
09272 c2[1] = c3[1];
09273 c2[2] = c3[2];
09274 circum_(v1, v2, v3, c3, &ierr);
09275 if (ierr != 0) {
09276 goto L13;
09277 }
09278 asum += angle_(c1, c2, c3);
09279 }
09280
09281
09282
09283 if (lp != lpl) {
09284 goto L1;
09285 }
09286
09287
09288
09289
09290 asum += angle_(c2, c3, c1s);
09291
09292
09293
09294
09295 asum += angle_(c3, c1s, c2s);
09296
09297
09298
09299 *ier = 0;
09300 ret_val = asum - (double) (m - 2) * acos(-1.);
09301 return ret_val;
09302
09303
09304
09305 L11:
09306 *ier = 1;
09307 areav = 0.f;
09308 return ret_val;
09309
09310
09311
09312 L12:
09313 *ier = 2;
09314 areav = 0.f;
09315 return ret_val;
09316
09317
09318
09319 L13:
09320 *ier = 3;
09321 areav = 0.f;
09322 return ret_val;
09323 }
09324
09325 int bdyadd_(int *kk, int *i1, int *i2, int *
09326 list, int *lptr, int *lend, int *lnew)
09327 {
09328 static int k, n1, n2, lp, lsav, nsav, next;
09329 int insert_(int *, int *, int *,
09330 int *, int *);
09331
09332
09333
09334
09335
09336
09337
09338
09339
09340
09341
09342
09343
09344
09345
09346
09347
09348
09349
09350
09351
09352
09353
09354
09355
09356
09357
09358
09359
09360
09361
09362
09363
09364
09365
09366
09367
09368
09369
09370
09371
09372
09373
09374
09375
09376
09377
09378
09379
09380
09381
09382
09383
09384
09385
09386
09387
09388
09389
09390
09391
09392
09393
09394
09395 --lend;
09396 --lptr;
09397 --list;
09398
09399
09400 k = *kk;
09401 n1 = *i1;
09402 n2 = *i2;
09403
09404
09405
09406 lp = lend[n1];
09407 lsav = lptr[lp];
09408 lptr[lp] = *lnew;
09409 list[*lnew] = -k;
09410 lptr[*lnew] = lsav;
09411 lend[n1] = *lnew;
09412 ++(*lnew);
09413 next = -list[lp];
09414 list[lp] = next;
09415 nsav = next;
09416
09417
09418
09419
09420 L1:
09421 lp = lend[next];
09422 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09423 if (next == n2) {
09424 goto L2;
09425 }
09426 next = -list[lp];
09427 list[lp] = next;
09428 goto L1;
09429
09430
09431
09432
09433 L2:
09434 lsav = *lnew;
09435 list[*lnew] = n1;
09436 lptr[*lnew] = *lnew + 1;
09437 ++(*lnew);
09438 next = nsav;
09439
09440 L3:
09441 if (next == n2) {
09442 goto L4;
09443 }
09444 list[*lnew] = next;
09445 lptr[*lnew] = *lnew + 1;
09446 ++(*lnew);
09447 lp = lend[next];
09448 next = list[lp];
09449 goto L3;
09450
09451 L4:
09452 list[*lnew] = -n2;
09453 lptr[*lnew] = lsav;
09454 lend[k] = *lnew;
09455 ++(*lnew);
09456 return 0;
09457 }
09458
09459 int bnodes_(int *n, int *list, int *lptr,
09460 int *lend, int *nodes, int *nb, int *na, int *nt)
09461 {
09462
09463 int i__1;
09464
09465
09466 static int k, n0, lp, nn, nst;
09467
09468
09469
09470
09471
09472
09473
09474
09475
09476
09477
09478
09479
09480
09481
09482
09483
09484
09485
09486
09487
09488
09489
09490
09491
09492
09493
09494
09495
09496
09497
09498
09499
09500
09501
09502
09503
09504
09505
09506
09507
09508
09509
09510
09511
09512
09513
09514
09515
09516
09517
09518
09519
09520
09521
09522
09523
09524
09525
09526
09527 --lend;
09528 --list;
09529 --lptr;
09530 --nodes;
09531
09532
09533 nn = *n;
09534
09535
09536
09537 i__1 = nn;
09538 for (nst = 1; nst <= i__1; ++nst) {
09539 lp = lend[nst];
09540 if (list[lp] < 0) {
09541 goto L2;
09542 }
09543
09544 }
09545
09546
09547
09548 *nb = 0;
09549 *na = (nn - 2) * 3;
09550 *nt = nn - (2<<1);
09551 return 0;
09552
09553
09554
09555
09556 L2:
09557 nodes[1] = nst;
09558 k = 1;
09559 n0 = nst;
09560
09561
09562
09563 L3:
09564 lp = lend[n0];
09565 lp = lptr[lp];
09566 n0 = list[lp];
09567 if (n0 == nst) {
09568 goto L4;
09569 }
09570 ++k;
09571 nodes[k] = n0;
09572 goto L3;
09573
09574
09575
09576 L4:
09577 *nb = k;
09578 *nt = (*n << 1) - *nb - 2;
09579 *na = *nt + *n - 1;
09580 return 0;
09581 }
09582
09583 int circle_(int *k, double *xc, double *yc,
09584 int *ier)
09585 {
09586
09587 int i__1;
09588
09589
09590
09591
09592
09593 static double a, c__;
09594 static int i__;
09595 static double s;
09596 static int k2, k3;
09597 static double x0, y0;
09598 static int kk, np1;
09599
09600
09601
09602
09603
09604
09605
09606
09607
09608
09609
09610
09611
09612
09613
09614
09615
09616
09617
09618
09619
09620
09621
09622
09623
09624
09625
09626
09627
09628
09629
09630
09631
09632
09633
09634
09635
09636
09637
09638
09639
09640
09641
09642
09643
09644
09645
09646
09647
09648
09649
09650
09651
09652
09653
09654
09655
09656
09657
09658
09659
09660
09661
09662
09663 --yc;
09664 --xc;
09665
09666
09667 kk = *k;
09668 k2 = kk << 1;
09669 k3 = kk * 3;
09670 np1 = (kk << 2) + 1;
09671
09672
09673
09674
09675 if (kk < 1) {
09676 goto L2;
09677 }
09678 a = atan(1.) * 2. / (double) kk;
09679 c__ = cos(a);
09680 s = sin(a);
09681 x0 = 1.;
09682 y0 = 0.;
09683
09684
09685
09686
09687
09688 i__1 = kk;
09689 for (i__ = 1; i__ <= i__1; ++i__) {
09690 xc[i__] = x0;
09691 yc[i__] = y0;
09692 xc[i__ + kk] = -y0;
09693 yc[i__ + kk] = x0;
09694 xc[i__ + k2] = -x0;
09695 yc[i__ + k2] = -y0;
09696 xc[i__ + k3] = y0;
09697 yc[i__ + k3] = -x0;
09698
09699
09700
09701 x0 = c__ * x0 - s * y0;
09702 y0 = s * x0 + c__ * y0;
09703
09704 }
09705
09706
09707
09708
09709 xc[np1] = xc[1];
09710 yc[np1] = yc[1];
09711 *ier = 0;
09712 return 0;
09713
09714
09715
09716 L2:
09717 *ier = 1;
09718 return 0;
09719 }
09720
09721 int circum_(double *v1, double *v2, double *v3,
09722 double *c__, int *ier)
09723 {
09724
09725
09726
09727
09728 static int i__;
09729 static double e1[3], e2[3], cu[3], cnorm;
09730
09731
09732
09733
09734
09735
09736
09737
09738
09739
09740
09741
09742
09743
09744
09745
09746
09747
09748
09749
09750
09751
09752
09753
09754
09755
09756
09757
09758
09759
09760
09761
09762
09763
09764
09765
09766
09767
09768
09769
09770
09771
09772
09773
09774
09775
09776
09777
09778
09779
09780
09781
09782
09783
09784
09785
09786 --c__;
09787 --v3;
09788 --v2;
09789 --v1;
09790
09791
09792 for (i__ = 1; i__ <= 3; ++i__) {
09793 e1[i__ - 1] = v2[i__] - v1[i__];
09794 e2[i__ - 1] = v3[i__] - v1[i__];
09795
09796 }
09797
09798
09799
09800 cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09801 cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09802 cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09803 cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09804
09805
09806
09807
09808 if (cnorm != 0.) {
09809
09810
09811
09812 cnorm = sqrt(cnorm);
09813 for (i__ = 1; i__ <= 3; ++i__) {
09814 c__[i__] = cu[i__ - 1] / cnorm;
09815
09816 }
09817
09818
09819
09820
09821
09822
09823 if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09824 c__[1] = -c__[1];
09825 c__[2] = -c__[2];
09826 c__[3] = -c__[3];
09827 }
09828 *ier = 0;
09829 } else {
09830
09831
09832
09833 *ier = 1;
09834 }
09835 return 0;
09836 }
09837
09838 int covsph_(int *kk, int *n0, int *list, int
09839 *lptr, int *lend, int *lnew)
09840 {
09841 static int k, lp, nst, lsav, next;
09842 int insert_(int *, int *, int *,
09843 int *, int *);
09844
09845
09846
09847
09848
09849
09850
09851
09852
09853
09854
09855
09856
09857
09858
09859
09860
09861
09862
09863
09864
09865
09866
09867
09868
09869
09870
09871
09872
09873
09874
09875
09876
09877
09878
09879
09880
09881
09882
09883
09884
09885
09886
09887
09888
09889
09890
09891
09892
09893
09894
09895
09896
09897
09898
09899
09900
09901 --lend;
09902 --lptr;
09903 --list;
09904
09905
09906 k = *kk;
09907 nst = *n0;
09908
09909
09910
09911
09912
09913 next = nst;
09914 L1:
09915 lp = lend[next];
09916 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09917 next = -list[lp];
09918 list[lp] = next;
09919 if (next != nst) {
09920 goto L1;
09921 }
09922
09923
09924
09925
09926 lsav = *lnew;
09927 L2:
09928 lp = lend[next];
09929 list[*lnew] = next;
09930 lptr[*lnew] = *lnew + 1;
09931 ++(*lnew);
09932 next = list[lp];
09933 if (next != nst) {
09934 goto L2;
09935 }
09936
09937 lptr[*lnew - 1] = lsav;
09938 lend[k] = *lnew - 1;
09939 return 0;
09940 }
09941
09942
09943 int crlist_(int *n, int *ncol, double *x,
09944 double *y, double *z__, int *list, int *lend, int
09945 *lptr, int *lnew, int *ltri, int *listc, int *nb,
09946 double *xc, double *yc, double *zc, double *rc,
09947 int *ier)
09948 {
09949
09950 int i__1, i__2;
09951
09952
09953
09954
09955
09956 static double c__[3], t;
09957 static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09958 static double v1[3], v2[3], v3[3];
09959 static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09960 lpn;
09961 static long int swp;
09962 static int ierr;
09963 int lstptr_(int *, int *, int *, int *);
09964 long int swptst_(int *, int *, int *, int *,
09965 double *, double *, double *);
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
10061
10062
10063
10064
10065
10066
10067
10068
10069
10070
10071
10072
10073
10074
10075
10076
10077
10078
10079
10080
10081
10082
10083
10084
10085
10086
10087
10088
10089
10090
10091
10092
10093
10094
10095
10096
10097
10098
10099
10100
10101
10102
10103
10104
10105
10106
10107
10108
10109
10110
10111
10112
10113
10114
10115
10116
10117
10118
10119
10120
10121
10122
10123
10124
10125
10126
10127
10128
10129
10130
10131
10132
10133
10134
10135
10136
10137
10138
10139
10140
10141
10142 --lend;
10143 --z__;
10144 --y;
10145 --x;
10146 ltri -= 7;
10147 --list;
10148 --lptr;
10149 --listc;
10150 --xc;
10151 --yc;
10152 --zc;
10153 --rc;
10154
10155
10156 nn = *n;
10157 *nb = 0;
10158 nt = 0;
10159 if (nn < 3) {
10160 goto L21;
10161 }
10162
10163
10164
10165 i__1 = nn;
10166 for (n1 = 1; n1 <= i__1; ++n1) {
10167 lp = lend[n1];
10168 if (list[lp] < 0) {
10169 goto L2;
10170 }
10171
10172 }
10173
10174
10175
10176 goto L9;
10177
10178
10179
10180
10181
10182
10183
10184
10185 L2:
10186 n2 = -list[lp];
10187 lp = lptr[lp];
10188 n3 = list[lp];
10189
10190
10191
10192
10193
10194
10195 L3:
10196 ++nt;
10197 if (nt <= *ncol) {
10198 ltri[nt * 6 + 1] = n1;
10199 ltri[nt * 6 + 2] = n2;
10200 ltri[nt * 6 + 3] = n3;
10201 ltri[nt * 6 + 4] = nt + 1;
10202 ltri[nt * 6 + 5] = nt - 1;
10203 ltri[nt * 6 + 6] = 0;
10204 }
10205 n1 = n2;
10206 lp = lend[n1];
10207 n2 = -list[lp];
10208 if (n2 != n3) {
10209 goto L3;
10210 }
10211
10212 *nb = nt + 2;
10213 if (*ncol < nt) {
10214 goto L22;
10215 }
10216 ltri[nt * 6 + 4] = 0;
10217 if (nt == 1) {
10218 goto L7;
10219 }
10220
10221
10222
10223
10224
10225
10226
10227 L4:
10228 swp = FALSE_;
10229 i__1 = nt - 1;
10230 for (kt1 = 1; kt1 <= i__1; ++kt1) {
10231 for (i3 = 1; i3 <= 3; ++i3) {
10232 kt2 = ltri[i3 + 3 + kt1 * 6];
10233 if (kt2 <= kt1) {
10234 goto L5;
10235 }
10236
10237
10238
10239
10240 if (i3 == 1) {
10241 i1 = 2;
10242 i2 = 3;
10243 } else if (i3 == 2) {
10244 i1 = 3;
10245 i2 = 1;
10246 } else {
10247 i1 = 1;
10248 i2 = 2;
10249 }
10250 n1 = ltri[i1 + kt1 * 6];
10251 n2 = ltri[i2 + kt1 * 6];
10252 n3 = ltri[i3 + kt1 * 6];
10253
10254
10255
10256
10257 if (ltri[kt2 * 6 + 4] == kt1) {
10258 i4 = 1;
10259 } else if (ltri[kt2 * 6 + 5] == kt1) {
10260 i4 = 2;
10261 } else {
10262 i4 = 3;
10263 }
10264 n4 = ltri[i4 + kt2 * 6];
10265
10266
10267
10268
10269
10270 if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10271 goto L5;
10272 }
10273
10274
10275
10276
10277 swp = TRUE_;
10278 kt11 = ltri[i1 + 3 + kt1 * 6];
10279 kt12 = ltri[i2 + 3 + kt1 * 6];
10280 if (i4 == 1) {
10281 i2 = 2;
10282 i1 = 3;
10283 } else if (i4 == 2) {
10284 i2 = 3;
10285 i1 = 1;
10286 } else {
10287 i2 = 1;
10288 i1 = 2;
10289 }
10290 kt21 = ltri[i1 + 3 + kt2 * 6];
10291 kt22 = ltri[i2 + 3 + kt2 * 6];
10292 ltri[kt1 * 6 + 1] = n4;
10293 ltri[kt1 * 6 + 2] = n3;
10294 ltri[kt1 * 6 + 3] = n1;
10295 ltri[kt1 * 6 + 4] = kt12;
10296 ltri[kt1 * 6 + 5] = kt22;
10297 ltri[kt1 * 6 + 6] = kt2;
10298 ltri[kt2 * 6 + 1] = n3;
10299 ltri[kt2 * 6 + 2] = n4;
10300 ltri[kt2 * 6 + 3] = n2;
10301 ltri[kt2 * 6 + 4] = kt21;
10302 ltri[kt2 * 6 + 5] = kt11;
10303 ltri[kt2 * 6 + 6] = kt1;
10304
10305
10306
10307 if (kt11 != 0) {
10308 i4 = 4;
10309 if (ltri[kt11 * 6 + 4] != kt1) {
10310 i4 = 5;
10311 if (ltri[kt11 * 6 + 5] != kt1) {
10312 i4 = 6;
10313 }
10314 }
10315 ltri[i4 + kt11 * 6] = kt2;
10316 }
10317 if (kt22 != 0) {
10318 i4 = 4;
10319 if (ltri[kt22 * 6 + 4] != kt2) {
10320 i4 = 5;
10321 if (ltri[kt22 * 6 + 5] != kt2) {
10322 i4 = 6;
10323 }
10324 }
10325 ltri[i4 + kt22 * 6] = kt1;
10326 }
10327 L5:
10328 ;
10329 }
10330
10331 }
10332 if (swp) {
10333 goto L4;
10334 }
10335
10336
10337
10338
10339 L7:
10340 i__1 = nt;
10341 for (kt = 1; kt <= i__1; ++kt) {
10342 n1 = ltri[kt * 6 + 1];
10343 n2 = ltri[kt * 6 + 2];
10344 n3 = ltri[kt * 6 + 3];
10345 v1[0] = x[n1];
10346 v1[1] = y[n1];
10347 v1[2] = z__[n1];
10348 v2[0] = x[n2];
10349 v2[1] = y[n2];
10350 v2[2] = z__[n2];
10351 v3[0] = x[n3];
10352 v3[1] = y[n3];
10353 v3[2] = z__[n3];
10354 circum_(v2, v1, v3, c__, &ierr);
10355 if (ierr != 0) {
10356 goto L23;
10357 }
10358
10359
10360
10361
10362 xc[kt] = -c__[0];
10363 yc[kt] = -c__[1];
10364 zc[kt] = -c__[2];
10365 t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10366 if (t < -1.) {
10367 t = -1.;
10368 }
10369 if (t > 1.) {
10370 t = 1.;
10371 }
10372 rc[kt] = acos(t);
10373
10374 }
10375
10376
10377
10378
10379
10380
10381 L9:
10382 kt = nt;
10383
10384
10385
10386 nm2 = nn - 2;
10387 i__1 = nm2;
10388 for (n1 = 1; n1 <= i__1; ++n1) {
10389 lpl = lend[n1];
10390 lp = lpl;
10391 n3 = list[lp];
10392
10393
10394
10395
10396 L10:
10397 lp = lptr[lp];
10398 n2 = n3;
10399 n3 = (i__2 = list[lp], abs(i__2));
10400 if (n2 <= n1 || n3 <= n1) {
10401 goto L11;
10402 }
10403 ++kt;
10404
10405
10406
10407 v1[0] = x[n1];
10408 v1[1] = y[n1];
10409 v1[2] = z__[n1];
10410 v2[0] = x[n2];
10411 v2[1] = y[n2];
10412 v2[2] = z__[n2];
10413 v3[0] = x[n3];
10414 v3[1] = y[n3];
10415 v3[2] = z__[n3];
10416 circum_(v1, v2, v3, c__, &ierr);
10417 if (ierr != 0) {
10418 goto L23;
10419 }
10420
10421
10422
10423 xc[kt] = c__[0];
10424 yc[kt] = c__[1];
10425 zc[kt] = c__[2];
10426 t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10427 if (t < -1.) {
10428 t = -1.;
10429 }
10430 if (t > 1.) {
10431 t = 1.;
10432 }
10433 rc[kt] = acos(t);
10434
10435
10436
10437
10438
10439 lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10440 listc[lpn] = kt;
10441 lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10442 listc[lpn] = kt;
10443 lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10444 listc[lpn] = kt;
10445 L11:
10446 if (lp != lpl) {
10447 goto L10;
10448 }
10449
10450 }
10451 if (nt == 0) {
10452 goto L20;
10453 }
10454
10455
10456
10457
10458
10459
10460 kt1 = 0;
10461 L13:
10462 ++kt1;
10463 if (ltri[kt1 * 6 + 4] == 0) {
10464 i1 = 2;
10465 i2 = 3;
10466 i3 = 1;
10467 goto L14;
10468 } else if (ltri[kt1 * 6 + 5] == 0) {
10469 i1 = 3;
10470 i2 = 1;
10471 i3 = 2;
10472 goto L14;
10473 } else if (ltri[kt1 * 6 + 6] == 0) {
10474 i1 = 1;
10475 i2 = 2;
10476 i3 = 3;
10477 goto L14;
10478 }
10479 goto L13;
10480 L14:
10481 n1 = ltri[i1 + kt1 * 6];
10482 n0 = n1;
10483
10484
10485
10486
10487
10488
10489
10490
10491
10492 L15:
10493 lp = lend[n1];
10494 lpn = lptr[lp];
10495 listc[lp] = kt1;
10496
10497
10498
10499 L16:
10500 kt2 = ltri[i2 + 3 + kt1 * 6];
10501 if (kt2 != 0) {
10502
10503
10504
10505 lptr[lp] = *lnew;
10506 lp = *lnew;
10507 listc[lp] = kt2;
10508 ++(*lnew);
10509
10510
10511
10512
10513 kt1 = kt2;
10514 if (ltri[kt1 * 6 + 1] == n1) {
10515 i1 = 1;
10516 i2 = 2;
10517 i3 = 3;
10518 } else if (ltri[kt1 * 6 + 2] == n1) {
10519 i1 = 2;
10520 i2 = 3;
10521 i3 = 1;
10522 } else {
10523 i1 = 3;
10524 i2 = 1;
10525 i3 = 2;
10526 }
10527 goto L16;
10528 }
10529
10530
10531
10532
10533
10534
10535
10536 lptr[lp] = lpn;
10537 n1 = ltri[i3 + kt1 * 6];
10538 if (n1 != n0) {
10539 i4 = i3;
10540 i3 = i2;
10541 i2 = i1;
10542 i1 = i4;
10543 goto L15;
10544 }
10545
10546
10547
10548 L20:
10549 *ier = 0;
10550 return 0;
10551
10552
10553
10554 L21:
10555 *ier = 1;
10556 return 0;
10557
10558
10559
10560 L22:
10561 *ier = 2;
10562 return 0;
10563
10564
10565
10566 L23:
10567 *ier = 3;
10568 return 0;
10569 }
10570
10571 int delarc_(int *n, int *io1, int *io2, int *
10572 list, int *lptr, int *lend, int *lnew, int *ier)
10573 {
10574
10575 int i__1;
10576
10577
10578 static int n1, n2, n3, lp, lph, lpl;
10579 int delnb_(int *, int *, int *,
10580 int *, int *, int *, int *, int *);
10581 int lstptr_(int *, int *, int *, int *);
10582
10583
10584
10585
10586
10587
10588
10589
10590
10591
10592
10593
10594
10595
10596
10597
10598
10599
10600
10601
10602
10603
10604
10605
10606
10607
10608
10609
10610
10611
10612
10613
10614
10615
10616
10617
10618
10619
10620
10621
10622
10623
10624
10625
10626
10627
10628
10629
10630
10631
10632
10633
10634
10635
10636
10637
10638
10639
10640
10641
10642
10643
10644
10645
10646
10647
10648
10649
10650
10651
10652
10653
10654
10655 --lend;
10656 --list;
10657 --lptr;
10658
10659
10660 n1 = *io1;
10661 n2 = *io2;
10662
10663
10664
10665
10666
10667 if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10668 *ier = 1;
10669 return 0;
10670 }
10671
10672 lpl = lend[n2];
10673 if (-list[lpl] != n1) {
10674 n1 = n2;
10675 n2 = *io1;
10676 lpl = lend[n2];
10677 if (-list[lpl] != n1) {
10678 *ier = 2;
10679 return 0;
10680 }
10681 }
10682
10683
10684
10685
10686
10687 lpl = lend[n1];
10688 lp = lptr[lpl];
10689 lp = lptr[lp];
10690 n3 = (i__1 = list[lp], abs(i__1));
10691 lpl = lend[n3];
10692 if (list[lpl] <= 0) {
10693 *ier = 3;
10694 return 0;
10695 }
10696
10697
10698
10699
10700
10701
10702 delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10703 if (lph < 0) {
10704 *ier = 4;
10705 return 0;
10706 }
10707
10708
10709
10710
10711 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10712
10713
10714
10715
10716 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10717 lend[n3] = lp;
10718 list[lp] = -n1;
10719
10720
10721
10722 *ier = 0;
10723 return 0;
10724 }
10725
10726 int delnb_(int *n0, int *nb, int *n, int *
10727 list, int *lptr, int *lend, int *lnew, int *lph)
10728 {
10729
10730 int i__1;
10731
10732
10733 static int i__, lp, nn, lpb, lpl, lpp, lnw;
10734
10735
10736
10737
10738
10739
10740
10741
10742
10743
10744
10745
10746
10747
10748
10749
10750
10751
10752
10753
10754
10755
10756
10757
10758
10759
10760
10761
10762
10763
10764
10765
10766
10767
10768
10769
10770
10771
10772
10773
10774
10775
10776
10777
10778
10779
10780
10781
10782
10783
10784
10785
10786
10787
10788
10789
10790
10791
10792
10793
10794
10795
10796
10797
10798
10799
10800
10801
10802
10803
10804
10805 --lend;
10806 --list;
10807 --lptr;
10808
10809
10810 nn = *n;
10811
10812
10813
10814 if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10815 *lph = -1;
10816 return 0;
10817 }
10818
10819
10820
10821
10822
10823
10824
10825 lpl = lend[*n0];
10826 lpp = lpl;
10827 lpb = lptr[lpp];
10828 L1:
10829 if (list[lpb] == *nb) {
10830 goto L2;
10831 }
10832 lpp = lpb;
10833 lpb = lptr[lpp];
10834 if (lpb != lpl) {
10835 goto L1;
10836 }
10837
10838
10839
10840 if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10841 *lph = -2;
10842 return 0;
10843 }
10844
10845
10846
10847
10848
10849 lend[*n0] = lpp;
10850 lp = lend[*nb];
10851 if (list[lp] < 0) {
10852 list[lpp] = -list[lpp];
10853 }
10854 goto L3;
10855
10856
10857
10858
10859
10860 L2:
10861 lp = lend[*nb];
10862 if (list[lp] < 0 && list[lpl] > 0) {
10863 lend[*n0] = lpp;
10864 list[lpp] = -list[lpp];
10865 }
10866
10867
10868
10869
10870 L3:
10871 lptr[lpp] = lptr[lpb];
10872 lnw = *lnew - 1;
10873 list[lpb] = list[lnw];
10874 lptr[lpb] = lptr[lnw];
10875 for (i__ = nn; i__ >= 1; --i__) {
10876 if (lend[i__] == lnw) {
10877 lend[i__] = lpb;
10878 goto L5;
10879 }
10880
10881 }
10882
10883 L5:
10884 i__1 = lnw - 1;
10885 for (i__ = 1; i__ <= i__1; ++i__) {
10886 if (lptr[i__] == lnw) {
10887 lptr[i__] = lpb;
10888 }
10889
10890 }
10891
10892
10893
10894 *lnew = lnw;
10895 *lph = lpb;
10896 return 0;
10897 }
10898
10899 int delnod_(int *k, int *n, double *x,
10900 double *y, double *z__, int *list, int *lptr, int
10901 *lend, int *lnew, int *lwk, int *iwk, int *ier)
10902 {
10903
10904 int i__1;
10905
10906
10907 static int i__, j, n1, n2;
10908 static double x1, x2, y1, y2, z1, z2;
10909 static int nl, lp, nn, nr;
10910 static double xl, yl, zl, xr, yr, zr;
10911 static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10912 static long int bdry;
10913 static int ierr, lwkl;
10914 int swap_(int *, int *, int *,
10915 int *, int *, int *, int *, int *), delnb_(
10916 int *, int *, int *, int *, int *, int *,
10917 int *, int *);
10918 int nbcnt_(int *, int *);
10919 int optim_(double *, double *, double
10920 *, int *, int *, int *, int *, int *, int
10921 *, int *);
10922 static int nfrst;
10923 int lstptr_(int *, int *, int *, int *);
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
10986
10987
10988
10989
10990
10991
10992
10993
10994
10995
10996
10997
10998
10999
11000
11001
11002
11003
11004
11005
11006
11007
11008
11009
11010
11011
11012
11013
11014
11015
11016
11017
11018
11019
11020
11021
11022
11023
11024
11025
11026
11027
11028
11029
11030
11031
11032
11033
11034
11035
11036
11037
11038
11039
11040
11041
11042
11043
11044
11045
11046
11047
11048
11049
11050
11051
11052
11053
11054
11055
11056
11057
11058
11059
11060
11061
11062
11063
11064
11065
11066
11067 iwk -= 3;
11068 --lend;
11069 --lptr;
11070 --list;
11071 --z__;
11072 --y;
11073 --x;
11074
11075
11076 n1 = *k;
11077 nn = *n;
11078 if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
11079 goto L21;
11080 }
11081 lpl = lend[n1];
11082 lpf = lptr[lpl];
11083 nnb = nbcnt_(&lpl, &lptr[1]);
11084 bdry = list[lpl] < 0;
11085 if (bdry) {
11086 ++nnb;
11087 }
11088 if (nnb < 3) {
11089 goto L23;
11090 }
11091 lwkl = *lwk;
11092 *lwk = nnb - 3;
11093 if (lwkl < *lwk) {
11094 goto L22;
11095 }
11096 iwl = 0;
11097 if (nnb == 3) {
11098 goto L3;
11099 }
11100
11101
11102
11103
11104
11105
11106
11107 x1 = x[n1];
11108 y1 = y[n1];
11109 z1 = z__[n1];
11110 nfrst = list[lpf];
11111 nr = nfrst;
11112 xr = x[nr];
11113 yr = y[nr];
11114 zr = z__[nr];
11115 lp = lptr[lpf];
11116 n2 = list[lp];
11117 x2 = x[n2];
11118 y2 = y[n2];
11119 z2 = z__[n2];
11120 lp = lptr[lp];
11121
11122
11123
11124 L1:
11125 nl = (i__1 = list[lp], abs(i__1));
11126 if (nl == nfrst && bdry) {
11127 goto L3;
11128 }
11129 xl = x[nl];
11130 yl = y[nl];
11131 zl = z__[nl];
11132
11133
11134
11135
11136
11137
11138 lpl2 = lend[n2];
11139 if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
11140 list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
11141 z2)))) {
11142
11143
11144
11145 nr = n2;
11146 xr = x2;
11147 yr = y2;
11148 zr = z2;
11149 goto L2;
11150 }
11151
11152
11153
11154
11155
11156
11157
11158
11159 swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11160 if (lp21 == 0) {
11161 nr = n2;
11162 xr = x2;
11163 yr = y2;
11164 zr = z2;
11165 goto L2;
11166 }
11167 ++iwl;
11168 if (nl <= n1) {
11169 iwk[(iwl << 1) + 1] = nl;
11170 } else {
11171 iwk[(iwl << 1) + 1] = nl - 1;
11172 }
11173 if (nr <= n1) {
11174 iwk[(iwl << 1) + 2] = nr;
11175 } else {
11176 iwk[(iwl << 1) + 2] = nr - 1;
11177 }
11178
11179
11180
11181 lpl = lend[n1];
11182 --nnb;
11183 if (nnb == 3) {
11184 goto L3;
11185 }
11186 lpf = lptr[lpl];
11187 nfrst = list[lpf];
11188 lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11189 if (nr == nfrst) {
11190 goto L2;
11191 }
11192
11193
11194
11195
11196
11197
11198
11199 n2 = nr;
11200 x2 = xr;
11201 y2 = yr;
11202 z2 = zr;
11203 lp21 = lptr[lp21];
11204 lp21 = lptr[lp21];
11205 nr = (i__1 = list[lp21], abs(i__1));
11206 xr = x[nr];
11207 yr = y[nr];
11208 zr = z__[nr];
11209 goto L1;
11210
11211
11212
11213 L2:
11214 if (n2 == nfrst) {
11215 goto L3;
11216 }
11217 n2 = nl;
11218 x2 = xl;
11219 y2 = yl;
11220 z2 = zl;
11221 lp = lptr[lp];
11222 goto L1;
11223
11224
11225
11226
11227
11228
11229
11230
11231
11232
11233 L3:
11234 if (! bdry) {
11235 if (nnb > 3) {
11236 bdry = TRUE_;
11237 } else {
11238 lpf = lptr[lpl];
11239 nr = list[lpf];
11240 lp = lptr[lpf];
11241 n2 = list[lp];
11242 nl = list[lpl];
11243 bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11244 x[n2], &y[n2], &z__[n2]);
11245 }
11246 if (bdry) {
11247
11248
11249
11250
11251
11252
11253 i__1 = nn;
11254 for (i__ = 1; i__ <= i__1; ++i__) {
11255 if (list[lend[i__]] < 0) {
11256 bdry = FALSE_;
11257 goto L5;
11258 }
11259
11260 }
11261 list[lpl] = -list[lpl];
11262 ++nnb;
11263 }
11264 }
11265 L5:
11266 if (! bdry && nnb > 3) {
11267 goto L24;
11268 }
11269
11270
11271
11272
11273 lp = lpl;
11274 lnw = *lnew;
11275
11276
11277
11278 L6:
11279 lp = lptr[lp];
11280 n2 = (i__1 = list[lp], abs(i__1));
11281 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11282 if (lph < 0) {
11283 goto L23;
11284 }
11285
11286
11287
11288 if (lpl == lnw) {
11289 lpl = lph;
11290 }
11291 if (lp == lnw) {
11292 lp = lph;
11293 }
11294 if (lp != lpl) {
11295 goto L6;
11296 }
11297
11298
11299
11300
11301
11302 --nn;
11303 if (n1 > nn) {
11304 goto L9;
11305 }
11306 i__1 = nn;
11307 for (i__ = n1; i__ <= i__1; ++i__) {
11308 x[i__] = x[i__ + 1];
11309 y[i__] = y[i__ + 1];
11310 z__[i__] = z__[i__ + 1];
11311 lend[i__] = lend[i__ + 1];
11312
11313 }
11314
11315 i__1 = lnw - 1;
11316 for (i__ = 1; i__ <= i__1; ++i__) {
11317 if (list[i__] > n1) {
11318 --list[i__];
11319 }
11320 if (list[i__] < -n1) {
11321 ++list[i__];
11322 }
11323
11324 }
11325
11326
11327
11328
11329
11330
11331
11332
11333
11334
11335
11336 L9:
11337 if (bdry) {
11338 --nnb;
11339 }
11340 lpn = lpl;
11341 i__1 = nnb;
11342 for (j = 1; j <= i__1; ++j) {
11343 --lnw;
11344 lp = lpn;
11345 lpn = lptr[lp];
11346 list[lp] = list[lnw];
11347 lptr[lp] = lptr[lnw];
11348 if (lptr[lpn] == lnw) {
11349 lptr[lpn] = lp;
11350 }
11351 if (lpn == lnw) {
11352 lpn = lp;
11353 }
11354 for (i__ = nn; i__ >= 1; --i__) {
11355 if (lend[i__] == lnw) {
11356 lend[i__] = lp;
11357 goto L11;
11358 }
11359
11360 }
11361
11362 L11:
11363 for (i__ = lnw - 1; i__ >= 1; --i__) {
11364 if (lptr[i__] == lnw) {
11365 lptr[i__] = lp;
11366 }
11367
11368 }
11369
11370 }
11371
11372
11373
11374
11375
11376 *n = nn;
11377 *lnew = lnw;
11378 if (iwl > 0) {
11379 nit = iwl << 2;
11380 optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11381 nit, &iwk[3], &ierr);
11382 if (ierr != 0 && ierr != 1) {
11383 goto L25;
11384 }
11385 if (ierr == 1) {
11386 goto L26;
11387 }
11388 }
11389
11390
11391
11392 *ier = 0;
11393 return 0;
11394
11395
11396
11397 L21:
11398 *ier = 1;
11399 return 0;
11400
11401
11402
11403 L22:
11404 *ier = 2;
11405 return 0;
11406
11407
11408
11409
11410 L23:
11411 *ier = 3;
11412 return 0;
11413
11414
11415
11416 L24:
11417 *ier = 4;
11418 return 0;
11419
11420
11421
11422 L25:
11423 *ier = 5;
11424
11425
11426
11427 return 0;
11428
11429
11430
11431 L26:
11432 *ier = 6;
11433 return 0;
11434 }
11435
11436 int drwarc_(int *, double *p, double *q,
11437 double *tol, int *nseg)
11438 {
11439
11440 int i__1;
11441 double d__1;
11442
11443
11444
11445
11446
11447 static int i__, k;
11448 static double s, p1[3], p2[3], u1, u2, v1, v2;
11449 static int na;
11450 static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11451
11452
11453
11454
11455
11456
11457
11458
11459
11460
11461
11462
11463
11464
11465
11466
11467
11468
11469
11470
11471
11472
11473
11474
11475
11476
11477
11478
11479
11480
11481
11482
11483
11484
11485
11486
11487
11488
11489
11490
11491
11492
11493
11494
11495
11496
11497
11498
11499
11500
11501
11502
11503
11504
11505
11506
11507
11508
11509
11510
11511
11512
11513
11514
11515
11516
11517
11518
11519
11520
11521
11522
11523
11524
11525
11526
11527
11528 --q;
11529 --p;
11530
11531
11532 enrm = 0.;
11533 for (i__ = 1; i__ <= 3; ++i__) {
11534 pm[i__ - 1] = p[i__] + q[i__];
11535 enrm += pm[i__ - 1] * pm[i__ - 1];
11536
11537 }
11538 if (enrm == 0.) {
11539 goto L5;
11540 }
11541 enrm = sqrt(enrm);
11542 pm[0] /= enrm;
11543 pm[1] /= enrm;
11544 pm[2] /= enrm;
11545
11546
11547
11548
11549 u1 = p[1];
11550 v1 = p[2];
11551 u2 = q[1];
11552 v2 = q[2];
11553 um = pm[0];
11554 vm = pm[1];
11555
11556
11557
11558
11559
11560
11561 du = u2 - u1;
11562 dv = v2 - v1;
11563 enrm = du * du + dv * dv;
11564 if (enrm == 0.) {
11565 goto L5;
11566 }
11567 err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11568
11569
11570
11571
11572
11573 na = (int) (err / *tol + 1.);
11574
11575
11576
11577
11578
11579 s = 1. / (double) na;
11580 for (i__ = 1; i__ <= 3; ++i__) {
11581 dp[i__ - 1] = s * (q[i__] - p[i__]);
11582 pm[i__ - 1] = p[i__];
11583 p1[i__ - 1] = p[i__];
11584
11585 }
11586
11587
11588
11589
11590 i__1 = na - 1;
11591 for (k = 1; k <= i__1; ++k) {
11592 enrm = 0.;
11593 for (i__ = 1; i__ <= 3; ++i__) {
11594 pm[i__ - 1] += dp[i__ - 1];
11595 enrm += pm[i__ - 1] * pm[i__ - 1];
11596
11597 }
11598 if (enrm == 0.) {
11599 goto L5;
11600 }
11601 enrm = sqrt(enrm);
11602 p2[0] = pm[0] / enrm;
11603 p2[1] = pm[1] / enrm;
11604 p2[2] = pm[2] / enrm;
11605
11606
11607 p1[0] = p2[0];
11608 p1[1] = p2[1];
11609 p1[2] = p2[2];
11610
11611 }
11612
11613
11614
11615
11616 *nseg = na;
11617 return 0;
11618
11619
11620
11621 L5:
11622 *nseg = 0;
11623 return 0;
11624 }
11625
11626 int edge_(int *in1, int *in2, double *x,
11627 double *y, double *z__, int *lwk, int *iwk, int *
11628 list, int *lptr, int *lend, int *ier)
11629 {
11630
11631 int i__1;
11632
11633
11634 static int i__, n0, n1, n2;
11635 static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11636 static int nl, lp, nr;
11637 static double dp12;
11638 static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11639 static double dp1l, dp2l, dp1r, dp2r;
11640 static int ierr;
11641 int swap_(int *, int *, int *,
11642 int *, int *, int *, int *, int *);
11643 static int next, iwcp1, n1lst, iwend;
11644 int optim_(double *, double *, double
11645 *, int *, int *, int *, int *, int *, int
11646 *, int *);
11647 static int n1frst;
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
11704
11705
11706
11707
11708
11709
11710
11711
11712
11713
11714
11715
11716
11717
11718
11719
11720
11721
11722
11723
11724
11725
11726
11727
11728
11729
11730
11731
11732
11733
11734
11735
11736
11737
11738
11739
11740
11741
11742
11743
11744
11745
11746
11747
11748
11749
11750
11751
11752
11753
11754
11755
11756
11757
11758
11759
11760
11761
11762
11763
11764
11765
11766
11767
11768
11769
11770
11771
11772
11773
11774
11775
11776
11777
11778
11779
11780
11781
11782
11783
11784
11785 --lend;
11786 --lptr;
11787 --list;
11788 iwk -= 3;
11789 --z__;
11790 --y;
11791 --x;
11792
11793
11794 n1 = *in1;
11795 n2 = *in2;
11796 iwend = *lwk;
11797 if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11798 goto L31;
11799 }
11800
11801
11802
11803
11804 lpl = lend[n1];
11805 n0 = (i__1 = list[lpl], abs(i__1));
11806 lp = lpl;
11807 L1:
11808 if (n0 == n2) {
11809 goto L30;
11810 }
11811 lp = lptr[lp];
11812 n0 = list[lp];
11813 if (lp != lpl) {
11814 goto L1;
11815 }
11816
11817
11818
11819 iwl = 0;
11820 nit = 0;
11821
11822
11823
11824 L2:
11825 x1 = x[n1];
11826 y1 = y[n1];
11827 z1 = z__[n1];
11828 x2 = x[n2];
11829 y2 = y[n2];
11830 z2 = z__[n2];
11831
11832
11833
11834
11835
11836
11837
11838
11839
11840
11841 lpl = lend[n1];
11842 n1lst = list[lpl];
11843 lp = lptr[lpl];
11844 n1frst = list[lp];
11845 nl = n1frst;
11846 if (n1lst < 0) {
11847 goto L4;
11848 }
11849
11850
11851
11852
11853 L3:
11854 if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11855 goto L4;
11856 }
11857 lp = lptr[lp];
11858 nl = list[lp];
11859 if (nl != n1frst) {
11860 goto L3;
11861 }
11862
11863
11864
11865 goto L5;
11866
11867
11868
11869
11870 L4:
11871 nr = nl;
11872 lp = lptr[lp];
11873 nl = (i__1 = list[lp], abs(i__1));
11874 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11875
11876
11877
11878
11879
11880 dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11881 dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11882 dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11883 dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11884 dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11885 if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11886 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11887 goto L6;
11888 }
11889
11890
11891
11892
11893
11894 if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11895 goto L5;
11896 }
11897 }
11898
11899
11900
11901 if (nl != n1frst) {
11902 goto L4;
11903 }
11904
11905
11906
11907
11908
11909
11910
11911 L5:
11912 if (nit > 0) {
11913 goto L33;
11914 }
11915 nit = 1;
11916 n1 = n2;
11917 n2 = *in1;
11918 goto L2;
11919
11920
11921
11922
11923 L6:
11924 ++iwl;
11925 if (iwl > iwend) {
11926 goto L32;
11927 }
11928 iwk[(iwl << 1) + 1] = nl;
11929 iwk[(iwl << 1) + 2] = nr;
11930
11931
11932
11933 lpl = lend[nl];
11934 lp = lptr[lpl];
11935
11936
11937
11938
11939 L7:
11940 if (list[lp] == nr) {
11941 goto L8;
11942 }
11943 lp = lptr[lp];
11944 if (lp != lpl) {
11945 goto L7;
11946 }
11947
11948
11949
11950
11951 if (list[lp] != nr) {
11952 goto L33;
11953 }
11954
11955
11956
11957
11958 L8:
11959 lp = lptr[lp];
11960 next = (i__1 = list[lp], abs(i__1));
11961 if (next == n2) {
11962 goto L9;
11963 }
11964
11965
11966
11967 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11968 nl = next;
11969 } else {
11970 nr = next;
11971 }
11972 goto L6;
11973
11974
11975
11976
11977 L9:
11978 *lwk = iwl;
11979 iwend = iwl;
11980
11981
11982
11983
11984
11985
11986
11987
11988 iwf = 1;
11989
11990
11991
11992
11993
11994 L10:
11995 lft = 0;
11996 n0 = n1;
11997 x0 = x1;
11998 y0 = y1;
11999 z0 = z1;
12000 nl = iwk[(iwf << 1) + 1];
12001 nr = iwk[(iwf << 1) + 2];
12002 iwc = iwf;
12003
12004
12005
12006
12007 L11:
12008 if (iwc == iwl) {
12009 goto L21;
12010 }
12011 iwcp1 = iwc + 1;
12012 next = iwk[(iwcp1 << 1) + 1];
12013 if (next != nl) {
12014 goto L16;
12015 }
12016 next = iwk[(iwcp1 << 1) + 2];
12017
12018
12019
12020
12021 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
12022 z__[next])) {
12023 goto L14;
12024 }
12025 if (lft >= 0) {
12026 goto L12;
12027 }
12028 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
12029 z__[next])) {
12030 goto L14;
12031 }
12032
12033
12034
12035 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12036 iwk[(iwc << 1) + 1] = n0;
12037 iwk[(iwc << 1) + 2] = next;
12038 goto L15;
12039
12040
12041
12042
12043
12044 L12:
12045 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12046 i__1 = iwl;
12047 for (i__ = iwcp1; i__ <= i__1; ++i__) {
12048 iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
12049 iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
12050
12051 }
12052 iwk[(iwl << 1) + 1] = n0;
12053 iwk[(iwl << 1) + 2] = next;
12054 --iwl;
12055 nr = next;
12056 goto L11;
12057
12058
12059
12060 L14:
12061 n0 = nr;
12062 x0 = x[n0];
12063 y0 = y[n0];
12064 z0 = z__[n0];
12065 lft = 1;
12066
12067
12068
12069 L15:
12070 nr = next;
12071 ++iwc;
12072 goto L11;
12073
12074
12075
12076
12077 L16:
12078 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
12079 z__[next])) {
12080 goto L19;
12081 }
12082 if (lft <= 0) {
12083 goto L17;
12084 }
12085 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
12086 z__[next])) {
12087 goto L19;
12088 }
12089
12090
12091
12092 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12093 iwk[(iwc << 1) + 1] = next;
12094 iwk[(iwc << 1) + 2] = n0;
12095 goto L20;
12096
12097
12098
12099
12100
12101 L17:
12102 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12103 i__1 = iwf;
12104 for (i__ = iwc - 1; i__ >= i__1; --i__) {
12105 iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
12106 iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
12107
12108 }
12109 iwk[(iwf << 1) + 1] = n0;
12110 iwk[(iwf << 1) + 2] = next;
12111 ++iwf;
12112 goto L20;
12113
12114
12115
12116 L19:
12117 n0 = nl;
12118 x0 = x[n0];
12119 y0 = y[n0];
12120 z0 = z__[n0];
12121 lft = -1;
12122
12123
12124
12125 L20:
12126 nl = next;
12127 ++iwc;
12128 goto L11;
12129
12130
12131
12132 L21:
12133 if (n0 == n1) {
12134 goto L24;
12135 }
12136 if (lft < 0) {
12137 goto L22;
12138 }
12139
12140
12141
12142 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
12143 goto L10;
12144 }
12145
12146
12147
12148
12149 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12150 iwk[(iwl << 1) + 1] = n0;
12151 iwk[(iwl << 1) + 2] = n2;
12152 --iwl;
12153 goto L10;
12154
12155
12156
12157 L22:
12158 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12159 goto L10;
12160 }
12161
12162
12163
12164
12165 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12166 i__ = iwl;
12167 L23:
12168 iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12169 iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12170 --i__;
12171 if (i__ > iwf) {
12172 goto L23;
12173 }
12174 iwk[(iwf << 1) + 1] = n0;
12175 iwk[(iwf << 1) + 2] = n2;
12176 ++iwf;
12177 goto L10;
12178
12179
12180
12181
12182 L24:
12183 swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12184 iwk[(iwc << 1) + 1] = 0;
12185 iwk[(iwc << 1) + 2] = 0;
12186
12187
12188
12189 *ier = 0;
12190 if (iwc > 1) {
12191
12192
12193
12194 nit = iwc - (1<<2);
12195 i__1 = iwc - 1;
12196 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12197 nit, &iwk[3], &ierr);
12198 if (ierr != 0 && ierr != 1) {
12199 goto L34;
12200 }
12201 if (ierr == 1) {
12202 *ier = 5;
12203 }
12204 }
12205 if (iwc < iwend) {
12206
12207
12208
12209 nit = iwend - (iwc<<2);
12210 i__1 = iwend - iwc;
12211 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12212 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12213 if (ierr != 0 && ierr != 1) {
12214 goto L34;
12215 }
12216 if (ierr == 1) {
12217 goto L35;
12218 }
12219 }
12220 if (*ier == 5) {
12221 goto L35;
12222 }
12223
12224
12225
12226 return 0;
12227
12228
12229
12230 L30:
12231 *ier = 0;
12232 return 0;
12233
12234
12235
12236 L31:
12237 *ier = 1;
12238 return 0;
12239
12240
12241
12242 L32:
12243 *ier = 2;
12244 return 0;
12245
12246
12247
12248
12249 L33:
12250 *ier = 3;
12251
12252
12253
12254
12255 return 0;
12256
12257
12258
12259 L34:
12260 *ier = 4;
12261
12262
12263
12264 return 0;
12265
12266
12267
12268 L35:
12269 *ier = 5;
12270 return 0;
12271 }
12272
12273 int getnp_(double *x, double *y, double *z__,
12274 int *list, int *lptr, int *lend, int *l, int *
12275 npts, double *df, int *ier)
12276 {
12277
12278 int i__1, i__2;
12279
12280
12281 static int i__, n1;
12282 static double x1, y1, z1;
12283 static int nb, ni, lp, np, lm1;
12284 static double dnb, dnp;
12285 static int lpl;
12286
12287
12288
12289
12290
12291
12292
12293
12294
12295
12296
12297
12298
12299
12300
12301
12302
12303
12304
12305
12306
12307
12308
12309
12310
12311
12312
12313
12314
12315
12316
12317
12318
12319
12320
12321
12322
12323
12324
12325
12326
12327
12328
12329
12330
12331
12332
12333
12334
12335
12336
12337
12338
12339
12340
12341
12342
12343
12344
12345
12346
12347
12348
12349
12350
12351
12352
12353
12354
12355
12356
12357
12358
12359
12360
12361
12362
12363
12364
12365 --x;
12366 --y;
12367 --z__;
12368 --list;
12369 --lptr;
12370 --lend;
12371 --npts;
12372
12373
12374 lm1 = *l - 1;
12375 if (lm1 < 1) {
12376 goto L6;
12377 }
12378 *ier = 0;
12379
12380
12381
12382 n1 = npts[1];
12383 x1 = x[n1];
12384 y1 = y[n1];
12385 z1 = z__[n1];
12386 i__1 = lm1;
12387 for (i__ = 1; i__ <= i__1; ++i__) {
12388 ni = npts[i__];
12389 lend[ni] = -lend[ni];
12390
12391 }
12392
12393
12394
12395
12396
12397 dnp = 2.;
12398
12399
12400
12401 i__1 = lm1;
12402 for (i__ = 1; i__ <= i__1; ++i__) {
12403 ni = npts[i__];
12404 lpl = -lend[ni];
12405 lp = lpl;
12406
12407
12408
12409 L2:
12410 nb = (i__2 = list[lp], abs(i__2));
12411 if (lend[nb] < 0) {
12412 goto L3;
12413 }
12414
12415
12416
12417
12418 dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12419 if (dnb >= dnp) {
12420 goto L3;
12421 }
12422 np = nb;
12423 dnp = dnb;
12424 L3:
12425 lp = lptr[lp];
12426 if (lp != lpl) {
12427 goto L2;
12428 }
12429
12430 }
12431 npts[*l] = np;
12432 *df = dnp;
12433
12434
12435
12436 i__1 = lm1;
12437 for (i__ = 1; i__ <= i__1; ++i__) {
12438 ni = npts[i__];
12439 lend[ni] = -lend[ni];
12440
12441 }
12442 return 0;
12443
12444
12445
12446 L6:
12447 *ier = 1;
12448 return 0;
12449 }
12450
12451 int insert_(int *k, int *lp, int *list, int *
12452 lptr, int *lnew)
12453 {
12454 static int lsav;
12455
12456
12457
12458
12459
12460
12461
12462
12463
12464
12465
12466
12467
12468
12469
12470
12471
12472
12473
12474
12475
12476
12477
12478
12479
12480
12481
12482
12483
12484
12485
12486
12487
12488
12489
12490
12491
12492
12493
12494
12495
12496
12497
12498 --lptr;
12499 --list;
12500
12501
12502 lsav = lptr[*lp];
12503 lptr[*lp] = *lnew;
12504 list[*lnew] = *k;
12505 lptr[*lnew] = lsav;
12506 ++(*lnew);
12507 return 0;
12508 }
12509
12510 long int inside_(double *p, int *lv, double *xv, double *yv,
12511 double *zv, int *nv, int *listv, int *ier)
12512 {
12513
12514
12515 static double eps = .001;
12516
12517
12518 int i__1;
12519 long int ret_val = 0;
12520
12521
12522
12523
12524
12525 static double b[3], d__;
12526 static int k, n;
12527 static double q[3];
12528 static int i1, i2, k0;
12529 static double v1[3], v2[3], cn[3], bp, bq;
12530 static int ni;
12531 static double pn[3], qn[3], vn[3];
12532 static int imx;
12533 static long int lft1, lft2, even;
12534 static int ierr;
12535 static long int pinr, qinr;
12536 static double qnrm, vnrm;
12537 int intrsc_(double *, double *,
12538 double *, double *, int *);
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
12610
12611
12612
12613
12614
12615
12616
12617
12618
12619
12620
12621
12622
12623
12624
12625
12626
12627
12628
12629
12630
12631
12632
12633
12634
12635
12636
12637
12638
12639
12640
12641
12642
12643
12644
12645
12646
12647
12648
12649
12650
12651
12652
12653
12654
12655
12656
12657
12658
12659
12660
12661
12662
12663
12664
12665
12666
12667
12668
12669
12670
12671
12672
12673
12674
12675
12676
12677
12678
12679
12680
12681
12682
12683
12684
12685
12686
12687
12688
12689
12690
12691 --p;
12692 --zv;
12693 --yv;
12694 --xv;
12695 --listv;
12696
12697
12698
12699
12700
12701
12702 imx = *lv;
12703 n = *nv;
12704 if (n < 3 || n > imx) {
12705 goto L11;
12706 }
12707 k0 = 0;
12708 i1 = listv[1];
12709 if (i1 < 1 || i1 > imx) {
12710 goto L12;
12711 }
12712
12713
12714
12715
12716
12717 L1:
12718 ++k0;
12719 if (k0 > n) {
12720 goto L14;
12721 }
12722 i1 = listv[k0];
12723 if (k0 < n) {
12724 i2 = listv[k0 + 1];
12725 } else {
12726 i2 = listv[1];
12727 }
12728 if (i2 < 1 || i2 > imx) {
12729 goto L12;
12730 }
12731 vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12732 vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12733 vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12734 vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12735 if (vnrm == 0.) {
12736 goto L1;
12737 }
12738 q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12739 q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12740 q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12741 qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12742 q[0] /= qnrm;
12743 q[1] /= qnrm;
12744 q[2] /= qnrm;
12745
12746
12747
12748 cn[0] = q[1] * p[3] - q[2] * p[2];
12749 cn[1] = q[2] * p[1] - q[0] * p[3];
12750 cn[2] = q[0] * p[2] - q[1] * p[1];
12751 if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12752 goto L1;
12753 }
12754 pn[0] = p[2] * cn[2] - p[3] * cn[1];
12755 pn[1] = p[3] * cn[0] - p[1] * cn[2];
12756 pn[2] = p[1] * cn[1] - p[2] * cn[0];
12757 qn[0] = cn[1] * q[2] - cn[2] * q[1];
12758 qn[1] = cn[2] * q[0] - cn[0] * q[2];
12759 qn[2] = cn[0] * q[1] - cn[1] * q[0];
12760
12761
12762
12763 ni = 0;
12764 even = TRUE_;
12765 bp = -2.;
12766 bq = -2.;
12767 pinr = TRUE_;
12768 qinr = TRUE_;
12769 i2 = listv[n];
12770 if (i2 < 1 || i2 > imx) {
12771 goto L12;
12772 }
12773 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12774
12775
12776
12777 i__1 = n;
12778 for (k = 1; k <= i__1; ++k) {
12779 i1 = i2;
12780 lft1 = lft2;
12781 i2 = listv[k];
12782 if (i2 < 1 || i2 > imx) {
12783 goto L12;
12784 }
12785 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12786 if (lft1 == lft2) {
12787 goto L2;
12788 }
12789
12790
12791
12792
12793 ++ni;
12794 v1[0] = xv[i1];
12795 v1[1] = yv[i1];
12796 v1[2] = zv[i1];
12797 v2[0] = xv[i2];
12798 v2[1] = yv[i2];
12799 v2[2] = zv[i2];
12800 intrsc_(v1, v2, cn, b, &ierr);
12801
12802
12803
12804
12805
12806 if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12807 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12808
12809
12810
12811 even = ! even;
12812 d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12813 if (d__ > bq) {
12814 bq = d__;
12815 qinr = lft2;
12816 }
12817 d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12818 if (d__ > bp) {
12819 bp = d__;
12820 pinr = lft1;
12821 }
12822 }
12823 L2:
12824 ;
12825 }
12826
12827
12828
12829
12830 if (ni != ni / 2 << 1 || ! qinr) {
12831 goto L1;
12832 }
12833
12834
12835
12836 if (pinr != even) {
12837 goto L13;
12838 }
12839
12840
12841
12842 *ier = 0;
12843 ret_val = even;
12844 return ret_val;
12845
12846
12847
12848 L11:
12849 *ier = 1;
12850 return ret_val;
12851
12852
12853
12854 L12:
12855 *ier = 2;
12856 return ret_val;
12857
12858
12859
12860 L13:
12861 *ier = 3;
12862 return ret_val;
12863
12864
12865
12866 L14:
12867 *ier = 4;
12868 return ret_val;
12869 }
12870
12871 int intadd_(int *kk, int *i1, int *i2, int *
12872 i3, int *list, int *lptr, int *lend, int *lnew)
12873 {
12874 static int k, n1, n2, n3, lp;
12875 int insert_(int *, int *, int *,
12876 int *, int *);
12877 int lstptr_(int *, int *, int *, int *);
12878
12879
12880
12881
12882
12883
12884
12885
12886
12887
12888
12889
12890
12891
12892
12893
12894
12895
12896
12897
12898
12899
12900
12901
12902
12903
12904
12905
12906
12907
12908
12909
12910
12911
12912
12913
12914
12915
12916
12917
12918
12919
12920
12921
12922
12923
12924
12925
12926
12927
12928
12929
12930
12931
12932
12933
12934
12935 --lend;
12936 --lptr;
12937 --list;
12938
12939
12940 k = *kk;
12941
12942
12943
12944 n1 = *i1;
12945 n2 = *i2;
12946 n3 = *i3;
12947
12948
12949
12950 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12951 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12952 lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12953 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12954 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12955 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12956
12957
12958
12959 list[*lnew] = n1;
12960 list[*lnew + 1] = n2;
12961 list[*lnew + 2] = n3;
12962 lptr[*lnew] = *lnew + 1;
12963 lptr[*lnew + 1] = *lnew + 2;
12964 lptr[*lnew + 2] = *lnew;
12965 lend[k] = *lnew + 2;
12966 *lnew += 3;
12967 return 0;
12968 }
12969
12970 int intrsc_(double *p1, double *p2, double *cn,
12971 double *p, int *ier)
12972 {
12973
12974
12975
12976
12977 static int i__;
12978 static double t, d1, d2, pp[3], ppn;
12979
12980
12981
12982
12983
12984
12985
12986
12987
12988
12989
12990
12991
12992
12993
12994
12995
12996
12997
12998
12999
13000
13001
13002
13003
13004
13005
13006
13007
13008
13009
13010
13011
13012
13013
13014
13015
13016
13017
13018
13019
13020
13021
13022
13023
13024
13025
13026
13027
13028
13029
13030
13031
13032
13033
13034
13035
13036
13037
13038
13039
13040
13041
13042
13043
13044
13045
13046
13047
13048 --p;
13049 --cn;
13050 --p2;
13051 --p1;
13052
13053
13054 d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
13055 d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
13056
13057 if (d1 == d2) {
13058 *ier = 1;
13059 return 0;
13060 }
13061
13062
13063
13064 t = d1 / (d1 - d2);
13065 ppn = 0.;
13066 for (i__ = 1; i__ <= 3; ++i__) {
13067 pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
13068 ppn += pp[i__ - 1] * pp[i__ - 1];
13069
13070 }
13071
13072
13073
13074 if (ppn == 0.) {
13075 *ier = 2;
13076 return 0;
13077 }
13078 ppn = sqrt(ppn);
13079
13080
13081
13082 for (i__ = 1; i__ <= 3; ++i__) {
13083 p[i__] = pp[i__ - 1] / ppn;
13084
13085 }
13086 *ier = 0;
13087 return 0;
13088 }
13089
13090 int jrand_(int *n, int *ix, int *iy, int *iz)
13091 {
13092
13093 int ret_val;
13094
13095
13096 static float u, x;
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
13123
13124
13125
13126
13127
13128
13129
13130
13131
13132
13133
13134
13135
13136
13137
13138
13139
13140
13141
13142
13143
13144
13145
13146
13147
13148
13149
13150 *ix = *ix * 171 % 30269;
13151 *iy = *iy * 172 % 30307;
13152 *iz = *iz * 170 % 30323;
13153 x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13154 30323.f;
13155 u = x - (int) x;
13156 ret_val = (int) ((float) (*n) * u + 1.f);
13157 return ret_val;
13158 }
13159
13160 long int left_(double *x1, double *y1, double *z1, double *x2,
13161 double *y2, double *z2, double *x0, double *y0,
13162 double *z0)
13163 {
13164
13165 long int ret_val;
13166
13167
13168
13169
13170
13171
13172
13173
13174
13175
13176
13177
13178
13179
13180
13181
13182
13183
13184
13185
13186
13187
13188
13189
13190
13191
13192
13193
13194
13195
13196
13197
13198
13199
13200
13201
13202
13203
13204 ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13205 *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13206
13207
13208 return ret_val;
13209 }
13210
13211 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13212 {
13213
13214 int ret_val;
13215
13216
13217 static int nd, lp;
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
13253
13254
13255
13256
13257
13258
13259
13260
13261
13262
13263
13264
13265 --lptr;
13266 --list;
13267
13268
13269 lp = lptr[*lpl];
13270 L1:
13271 nd = list[lp];
13272 if (nd == *nb) {
13273 goto L2;
13274 }
13275 lp = lptr[lp];
13276 if (lp != *lpl) {
13277 goto L1;
13278 }
13279
13280 L2:
13281 ret_val = lp;
13282 return ret_val;
13283 }
13284
13285 int nbcnt_(int *lpl, int *lptr)
13286 {
13287
13288 int ret_val;
13289
13290
13291 static int k, lp;
13292
13293
13294
13295
13296
13297
13298
13299
13300
13301
13302
13303
13304
13305
13306
13307
13308
13309
13310
13311
13312
13313
13314
13315
13316
13317
13318
13319
13320
13321
13322
13323
13324
13325
13326
13327
13328
13329
13330
13331
13332
13333
13334 --lptr;
13335
13336
13337 lp = *lpl;
13338 k = 1;
13339
13340 L1:
13341 lp = lptr[lp];
13342 if (lp == *lpl) {
13343 goto L2;
13344 }
13345 ++k;
13346 goto L1;
13347
13348 L2:
13349 ret_val = k;
13350 return ret_val;
13351 }
13352
13353 int nearnd_(double *p, int *ist, int *n, double *x,
13354 double *y, double *z__, int *list, int *lptr, int
13355 *lend, double *al)
13356 {
13357
13358 int ret_val, i__1;
13359
13360
13361
13362
13363
13364 static int l;
13365 static double b1, b2, b3;
13366 static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13367 static double ds1;
13368 static int lp1, lp2;
13369 static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13370 static int lpl;
13371 static double dsr;
13372 static int nst, listp[25], lptrp[25];
13373 int trfind_(int *, double *, int *,
13374 double *, double *, double *, int *, int *,
13375 int *, double *, double *, double *, int *,
13376 int *, int *);
13377 int lstptr_(int *, int *, int *, int *);
13378
13379
13380
13381
13382
13383
13384
13385
13386
13387
13388
13389
13390
13391
13392
13393
13394
13395
13396
13397
13398
13399
13400
13401
13402
13403
13404
13405
13406
13407
13408
13409
13410
13411
13412
13413
13414
13415
13416
13417
13418
13419
13420
13421
13422
13423
13424
13425
13426
13427
13428
13429
13430
13431
13432
13433
13434
13435
13436
13437
13438
13439
13440
13441
13442
13443
13444
13445
13446
13447
13448
13449
13450
13451
13452
13453
13454
13455
13456
13457
13458
13459
13460
13461
13462
13463
13464
13465
13466
13467
13468
13469
13470
13471
13472
13473
13474 --p;
13475 --lend;
13476 --z__;
13477 --y;
13478 --x;
13479 --list;
13480 --lptr;
13481
13482
13483 nn = *n;
13484 if (nn < 3) {
13485 goto L6;
13486 }
13487 nst = *ist;
13488 if (nst < 1 || nst > nn) {
13489 nst = 1;
13490 }
13491
13492
13493
13494
13495
13496 trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13497 1], &b1, &b2, &b3, &i1, &i2, &i3);
13498
13499
13500
13501 if (i1 == 0) {
13502 goto L6;
13503 }
13504
13505
13506
13507
13508
13509
13510
13511 if (i3 != 0) {
13512 listp[0] = i1;
13513 lptrp[0] = 2;
13514 listp[1] = i2;
13515 lptrp[1] = 3;
13516 listp[2] = i3;
13517 lptrp[2] = 1;
13518 l = 3;
13519 } else {
13520 n1 = i1;
13521 l = 1;
13522 lp1 = 2;
13523 listp[l - 1] = n1;
13524 lptrp[l - 1] = lp1;
13525
13526
13527
13528
13529 L1:
13530 lpl = lend[n1];
13531 n1 = -list[lpl];
13532 l = lp1;
13533 lp1 = l + 1;
13534 listp[l - 1] = n1;
13535 lptrp[l - 1] = lp1;
13536 if (n1 != i2 && lp1 < 25) {
13537 goto L1;
13538 }
13539 l = lp1;
13540 listp[l - 1] = 0;
13541 lptrp[l - 1] = 1;
13542 }
13543
13544
13545
13546
13547
13548
13549 lp2 = 1;
13550 n2 = i1;
13551 lp1 = lptrp[0];
13552 n1 = listp[lp1 - 1];
13553
13554
13555
13556 L2:
13557 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13558 if (list[lp] < 0) {
13559 goto L3;
13560 }
13561 lp = lptr[lp];
13562 n3 = (i__1 = list[lp], abs(i__1));
13563
13564
13565
13566 if (l == 25) {
13567 goto L4;
13568 }
13569 dx1 = x[n1] - p[1];
13570 dy1 = y[n1] - p[2];
13571 dz1 = z__[n1] - p[3];
13572
13573 dx2 = x[n2] - p[1];
13574 dy2 = y[n2] - p[2];
13575 dz2 = z__[n2] - p[3];
13576
13577 dx3 = x[n3] - p[1];
13578 dy3 = y[n3] - p[2];
13579 dz3 = z__[n3] - p[3];
13580 if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13581 (dx2 * dy1 - dx1 * dy2) <= 0.) {
13582 goto L3;
13583 }
13584
13585
13586
13587
13588 ++l;
13589 lptrp[lp2 - 1] = l;
13590 listp[l - 1] = n3;
13591 lptrp[l - 1] = lp1;
13592 lp1 = l;
13593 n1 = n3;
13594 goto L2;
13595
13596
13597
13598
13599 L3:
13600 if (lp1 == 1) {
13601 goto L4;
13602 }
13603 lp2 = lp1;
13604 n2 = n1;
13605 lp1 = lptrp[lp1 - 1];
13606 n1 = listp[lp1 - 1];
13607 if (n1 == 0) {
13608 goto L4;
13609 }
13610 goto L2;
13611
13612
13613
13614
13615
13616 L4:
13617 nr = i1;
13618 dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13619 i__1 = l;
13620 for (lp = 2; lp <= i__1; ++lp) {
13621 n1 = listp[lp - 1];
13622 if (n1 == 0) {
13623 goto L5;
13624 }
13625 ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13626 if (ds1 < dsr) {
13627 nr = n1;
13628 dsr = ds1;
13629 }
13630 L5:
13631 ;
13632 }
13633 dsr = -dsr;
13634 if (dsr > 1.) {
13635 dsr = 1.;
13636 }
13637 *al = acos(dsr);
13638 ret_val = nr;
13639 return ret_val;
13640
13641
13642
13643 L6:
13644 ret_val = 0;
13645 return ret_val;
13646 }
13647
13648 int optim_(double *x, double *y, double *z__,
13649 int *na, int *list, int *lptr, int *lend, int *
13650 nit, int *iwk, int *ier)
13651 {
13652
13653 int i__1, i__2;
13654
13655
13656 static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13657 static long int swp;
13658 static int iter;
13659 int swap_(int *, int *, int *,
13660 int *, int *, int *, int *, int *);
13661 static int maxit;
13662 long int swptst_(int *, int *, int *, int *,
13663 double *, double *, double *);
13664
13665
13666
13667
13668
13669
13670
13671
13672
13673
13674
13675
13676
13677
13678
13679
13680
13681
13682
13683
13684
13685
13686
13687
13688
13689
13690
13691
13692
13693
13694
13695
13696
13697
13698
13699
13700
13701
13702
13703
13704
13705
13706
13707
13708
13709
13710
13711
13712
13713
13714
13715
13716
13717
13718
13719
13720
13721
13722
13723
13724
13725
13726
13727
13728
13729
13730
13731
13732
13733
13734
13735
13736
13737
13738
13739
13740
13741
13742
13743
13744
13745
13746
13747
13748
13749
13750
13751
13752
13753
13754
13755
13756
13757
13758
13759
13760 --x;
13761 --y;
13762 --z__;
13763 iwk -= 3;
13764 --list;
13765 --lptr;
13766 --lend;
13767
13768
13769 nna = *na;
13770 maxit = *nit;
13771 if (nna < 0 || maxit < 1) {
13772 goto L7;
13773 }
13774
13775
13776
13777 iter = 0;
13778 if (nna == 0) {
13779 goto L5;
13780 }
13781
13782
13783
13784
13785 L1:
13786 if (iter == maxit) {
13787 goto L6;
13788 }
13789 ++iter;
13790 swp = FALSE_;
13791
13792
13793
13794 i__1 = nna;
13795 for (i__ = 1; i__ <= i__1; ++i__) {
13796 io1 = iwk[(i__ << 1) + 1];
13797 io2 = iwk[(i__ << 1) + 2];
13798
13799
13800
13801
13802
13803
13804
13805
13806 lpl = lend[io1];
13807 lpp = lpl;
13808 lp = lptr[lpp];
13809 L2:
13810 if (list[lp] == io2) {
13811 goto L3;
13812 }
13813 lpp = lp;
13814 lp = lptr[lpp];
13815 if (lp != lpl) {
13816 goto L2;
13817 }
13818
13819
13820
13821
13822
13823 if ((i__2 = list[lp], abs(i__2)) != io2) {
13824 goto L8;
13825 }
13826 if (list[lp] < 0) {
13827 goto L4;
13828 }
13829
13830
13831
13832
13833 L3:
13834 n2 = list[lpp];
13835 if (n2 < 0) {
13836 goto L4;
13837 }
13838 lp = lptr[lp];
13839 n1 = (i__2 = list[lp], abs(i__2));
13840
13841
13842
13843 if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13844 goto L4;
13845 }
13846 swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13847 if (lp21 == 0) {
13848 goto L9;
13849 }
13850 swp = TRUE_;
13851 iwk[(i__ << 1) + 1] = n1;
13852 iwk[(i__ << 1) + 2] = n2;
13853 L4:
13854 ;
13855 }
13856 if (swp) {
13857 goto L1;
13858 }
13859
13860
13861
13862 L5:
13863 *nit = iter;
13864 *ier = 0;
13865 return 0;
13866
13867
13868
13869 L6:
13870 *nit = maxit;
13871 *ier = 1;
13872 return 0;
13873
13874
13875
13876 L7:
13877 *nit = 0;
13878 *ier = 2;
13879 return 0;
13880
13881
13882
13883 L8:
13884 *nit = iter;
13885 *ier = 3;
13886 return 0;
13887
13888
13889
13890 L9:
13891 *nit = iter;
13892 *ier = 4;
13893 return 0;
13894 }
13895
13896 int projct_(double *px, double *py, double *pz,
13897 double *ox, double *oy, double *oz, double *ex,
13898 double *ey, double *ez, double *vx, double *vy,
13899 double *vz, long int *init, double *x, double *y,
13900 double *z__, int *ier)
13901 {
13902
13903
13904
13905
13906 static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13907 oes, xoe, yoe, zoe, xep, yep, zep;
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
13941
13942
13943
13944
13945
13946
13947
13948
13949
13950
13951
13952
13953
13954
13955
13956
13957
13958
13959
13960
13961
13962
13963
13964
13965
13966
13967
13968
13969
13970
13971
13972
13973
13974
13975
13976
13977
13978
13979
13980
13981
13982
13983
13984
13985
13986
13987
13988
13989
13990
13991
13992
13993
13994
13995
13996
13997
13998
13999
14000
14001
14002
14003
14004
14005
14006
14007
14008
14009
14010
14011
14012
14013
14014
14015
14016
14017
14018
14019
14020
14021
14022 if (*init) {
14023
14024
14025
14026
14027
14028
14029
14030
14031 xe = *ex;
14032 ye = *ey;
14033 ze = *ez;
14034 xoe = xe - *ox;
14035 yoe = ye - *oy;
14036 zoe = ze - *oz;
14037 oes = xoe * xoe + yoe * yoe + zoe * zoe;
14038 if (oes == 0.) {
14039 goto L1;
14040 }
14041
14042
14043
14044 s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
14045 xv = *vx - s * xoe;
14046 yv = *vy - s * yoe;
14047 zv = *vz - s * zoe;
14048
14049
14050
14051 sc = xv * xv + yv * yv + zv * zv;
14052 if (sc == 0.) {
14053 goto L2;
14054 }
14055 sc = 1. / sqrt(sc);
14056 xv = sc * xv;
14057 yv = sc * yv;
14058 zv = sc * zv;
14059
14060
14061
14062 xh = yv * zoe - yoe * zv;
14063 yh = xoe * zv - xv * zoe;
14064 zh = xv * yoe - xoe * yv;
14065 sc = sqrt(xh * xh + yh * yh + zh * zh);
14066 if (sc == 0.) {
14067 goto L2;
14068 }
14069 sc = 1. / sc;
14070 xh = sc * xh;
14071 yh = sc * yh;
14072 zh = sc * zh;
14073 }
14074
14075
14076
14077
14078
14079
14080 xep = *px - xe;
14081 yep = *py - ye;
14082 zep = *pz - ze;
14083 s = xoe * xep + yoe * yep + zoe * zep;
14084 if (s >= 0.) {
14085 goto L1;
14086 }
14087 s = oes / s;
14088 xw = xoe - s * xep;
14089 yw = yoe - s * yep;
14090 zw = zoe - s * zep;
14091
14092
14093
14094
14095 *x = xw * xh + yw * yh + zw * zh;
14096 *y = xw * xv + yw * yv + zw * zv;
14097 *z__ = s + 1.;
14098 *init = FALSE_;
14099 *ier = 0;
14100 return 0;
14101
14102
14103
14104 L1:
14105 *ier = 1;
14106 return 0;
14107
14108
14109
14110 L2:
14111 *ier = 2;
14112 return 0;
14113 }
14114
14115 int scoord_(double *px, double *py, double *pz,
14116 double *plat, double *plon, double *pnrm)
14117 {
14118
14119
14120
14121
14122
14123
14124
14125
14126
14127
14128
14129
14130
14131
14132
14133
14134
14135
14136
14137
14138
14139
14140
14141
14142
14143
14144
14145
14146
14147
14148
14149
14150
14151
14152
14153
14154
14155
14156
14157
14158
14159
14160 *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14161 if (*px != 0. || *py != 0.) {
14162 *plon = atan2(*py, *px);
14163 } else {
14164 *plon = 0.;
14165 }
14166 if (*pnrm != 0.) {
14167 *plat = asin(*pz / *pnrm);
14168 } else {
14169 *plat = 0.;
14170 }
14171 return 0;
14172 }
14173
14174 double store_(double *x)
14175 {
14176
14177 double ret_val;
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
14205
14206
14207
14208
14209
14210
14211
14212 stcom_1.y = *x;
14213 ret_val = stcom_1.y;
14214 return ret_val;
14215 }
14216
14217 int swap_(int *in1, int *in2, int *io1, int *
14218 io2, int *list, int *lptr, int *lend, int *lp21)
14219 {
14220
14221 int i__1;
14222
14223
14224 static int lp, lph, lpsav;
14225 int lstptr_(int *, int *, int *, int *);
14226
14227
14228
14229
14230
14231
14232
14233
14234
14235
14236
14237
14238
14239
14240
14241
14242
14243
14244
14245
14246
14247
14248
14249
14250
14251
14252
14253
14254
14255
14256
14257
14258
14259
14260
14261
14262
14263
14264
14265
14266
14267
14268
14269
14270
14271
14272
14273
14274
14275
14276
14277
14278
14279
14280
14281
14282
14283
14284
14285
14286 --lend;
14287 --lptr;
14288 --list;
14289
14290
14291 lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14292 if ((i__1 = list[lp], abs(i__1)) == *in2) {
14293 *lp21 = 0;
14294 return 0;
14295 }
14296
14297
14298
14299 lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14300 lph = lptr[lp];
14301 lptr[lp] = lptr[lph];
14302
14303
14304
14305
14306 if (lend[*io1] == lph) {
14307 lend[*io1] = lp;
14308 }
14309
14310
14311
14312
14313 lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14314 lpsav = lptr[lp];
14315 lptr[lp] = lph;
14316 list[lph] = *in2;
14317 lptr[lph] = lpsav;
14318
14319
14320
14321 lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14322 lph = lptr[lp];
14323 lptr[lp] = lptr[lph];
14324
14325
14326
14327
14328 if (lend[*io2] == lph) {
14329 lend[*io2] = lp;
14330 }
14331
14332
14333
14334 lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14335 lpsav = lptr[lp];
14336 lptr[lp] = lph;
14337 list[lph] = *in1;
14338 lptr[lph] = lpsav;
14339 *lp21 = lph;
14340 return 0;
14341 }
14342
14343 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14344 double *x, double *y, double *z__)
14345 {
14346
14347 long int ret_val;
14348
14349
14350 static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14351
14352
14353
14354
14355
14356
14357
14358
14359
14360
14361
14362
14363
14364
14365
14366
14367
14368
14369
14370
14371
14372
14373
14374
14375
14376
14377
14378
14379
14380
14381
14382
14383
14384
14385
14386
14387
14388
14389
14390
14391
14392
14393
14394
14395
14396
14397
14398
14399
14400
14401
14402
14403
14404
14405
14406
14407 --z__;
14408 --y;
14409 --x;
14410
14411
14412 x4 = x[*n4];
14413 y4 = y[*n4];
14414 z4 = z__[*n4];
14415 dx1 = x[*n1] - x4;
14416 dx2 = x[*n2] - x4;
14417 dx3 = x[*n3] - x4;
14418 dy1 = y[*n1] - y4;
14419 dy2 = y[*n2] - y4;
14420 dy3 = y[*n3] - y4;
14421 dz1 = z__[*n1] - z4;
14422 dz2 = z__[*n2] - z4;
14423 dz3 = z__[*n3] - z4;
14424
14425
14426
14427
14428
14429 ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14430 dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14431 return ret_val;
14432 }
14433
14434 int trans_(int *n, double *rlat, double *rlon,
14435 double *x, double *y, double *z__)
14436 {
14437
14438 int i__1;
14439
14440
14441
14442
14443
14444 static int i__, nn;
14445 static double phi, theta, cosphi;
14446
14447
14448
14449
14450
14451
14452
14453
14454
14455
14456
14457
14458
14459
14460
14461
14462
14463
14464
14465
14466
14467
14468
14469
14470
14471
14472
14473
14474
14475
14476
14477
14478
14479
14480
14481
14482
14483
14484
14485
14486
14487
14488
14489
14490
14491
14492
14493
14494
14495
14496
14497
14498
14499
14500 --z__;
14501 --y;
14502 --x;
14503 --rlon;
14504 --rlat;
14505
14506
14507 nn = *n;
14508 i__1 = nn;
14509 for (i__ = 1; i__ <= i__1; ++i__) {
14510 phi = rlat[i__];
14511 theta = rlon[i__];
14512 cosphi = cos(phi);
14513 x[i__] = cosphi * cos(theta);
14514 y[i__] = cosphi * sin(theta);
14515 z__[i__] = sin(phi);
14516
14517 }
14518 return 0;
14519 }
14520
14521 int trfind_(int *nst, double *p, int *n,
14522 double *x, double *y, double *z__, int *list, int
14523 *lptr, int *lend, double *b1, double *b2, double *b3,
14524 int *i1, int *i2, int *i3)
14525 {
14526
14527
14528 static int ix = 1;
14529 static int iy = 2;
14530 static int iz = 3;
14531
14532
14533 int i__1;
14534 double d__1, d__2;
14535
14536
14537 static double q[3];
14538 static int n0, n1, n2, n3, n4, nf;
14539 static double s12;
14540 static int nl, lp;
14541 static double xp, yp, zp;
14542 static int n1s, n2s;
14543 static double eps, tol, ptn1, ptn2;
14544 static int next;
14545 int jrand_(int *, int *, int *, int *);
14546 double store_(double *);
14547 int lstptr_(int *, int *, int *, int *);
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
14591
14592
14593
14594
14595
14596
14597
14598
14599
14600
14601
14602
14603
14604
14605
14606
14607
14608
14609
14610
14611
14612
14613
14614
14615
14616
14617 --p;
14618 --lend;
14619 --z__;
14620 --y;
14621 --x;
14622 --list;
14623 --lptr;
14624
14625
14626
14627
14628
14629
14630
14631
14632
14633
14634
14635
14636
14637
14638
14639
14640
14641
14642
14643
14644
14645
14646
14647
14648
14649
14650
14651
14652
14653
14654
14655
14656
14657
14658
14659
14660
14661
14662
14663
14664
14665
14666
14667
14668
14669
14670
14671
14672 xp = p[1];
14673 yp = p[2];
14674 zp = p[3];
14675 n0 = *nst;
14676 if (n0 < 1 || n0 > *n) {
14677 n0 = jrand_(n, &ix, &iy, &iz);
14678 }
14679
14680
14681
14682 eps = 1.;
14683 L1:
14684 eps /= 2.;
14685 d__1 = eps + 1.;
14686 if (store_(&d__1) > 1.) {
14687 goto L1;
14688 }
14689 eps *= 2.;
14690 tol = eps * 4.;
14691
14692
14693
14694
14695 L2:
14696 lp = lend[n0];
14697 nl = list[lp];
14698 lp = lptr[lp];
14699 nf = list[lp];
14700 n1 = nf;
14701
14702
14703
14704
14705 if (nl > 0) {
14706
14707
14708
14709 L3:
14710 if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14711 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14712 -1e-10) {
14713 lp = lptr[lp];
14714 n1 = list[lp];
14715 if (n1 == nl) {
14716 goto L6;
14717 }
14718 goto L3;
14719 }
14720 } else {
14721
14722
14723
14724 nl = -nl;
14725 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14726 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14727 -1e-10) {
14728
14729
14730
14731 n1 = n0;
14732 n2 = nf;
14733 goto L9;
14734 }
14735 if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14736 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14737 -1e-10) {
14738
14739
14740
14741 n1 = nl;
14742 n2 = n0;
14743 goto L9;
14744 }
14745 }
14746
14747
14748
14749
14750 L4:
14751 lp = lptr[lp];
14752 n2 = (i__1 = list[lp], abs(i__1));
14753 if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14754 n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14755 goto L7;
14756 }
14757 n1 = n2;
14758 if (n1 != nl) {
14759 goto L4;
14760 }
14761 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14762 nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14763 goto L6;
14764 }
14765
14766
14767
14768
14769 d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14770 if (store_(&d__2) < 1. - eps * 4.) {
14771
14772
14773
14774
14775
14776 L5:
14777 if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14778 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14779 -1e-10) {
14780 lp = lptr[lp];
14781 n1 = (i__1 = list[lp], abs(i__1));
14782 if (n1 == nl) {
14783 goto L14;
14784 }
14785 goto L5;
14786 }
14787 }
14788
14789
14790
14791
14792 n0 = n1;
14793 goto L2;
14794
14795
14796
14797 L6:
14798 n2 = nf;
14799
14800
14801
14802
14803
14804 L7:
14805 n3 = n0;
14806 n1s = n1;
14807 n2s = n2;
14808
14809
14810
14811 L8:
14812
14813 *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14814 x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14815 if (*b3 < -1e-10) {
14816
14817
14818
14819
14820 lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14821 if (list[lp] < 0) {
14822 goto L9;
14823 }
14824 lp = lptr[lp];
14825 n4 = (i__1 = list[lp], abs(i__1));
14826
14827
14828
14829 if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14830 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14831 -1e-10) {
14832 n3 = n2;
14833 n2 = n4;
14834 n1s = n1;
14835 if (n2 != n2s && n2 != n0) {
14836 goto L8;
14837 }
14838 } else {
14839 n3 = n1;
14840 n1 = n4;
14841 n2s = n2;
14842 if (n1 != n1s && n1 != n0) {
14843 goto L8;
14844 }
14845 }
14846
14847
14848
14849
14850
14851 n0 = jrand_(n, &ix, &iy, &iz);
14852 goto L2;
14853 }
14854
14855
14856
14857
14858 if (*b3 >= eps) {
14859
14860
14861
14862 *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14863 - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14864 *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14865 - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14866 if (*b1 < -tol || *b2 < -tol) {
14867
14868
14869
14870 n0 = jrand_(n, &ix, &iy, &iz);
14871 goto L2;
14872 }
14873 } else {
14874
14875
14876
14877
14878 *b3 = 0.;
14879 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14880 ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14881 ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14882 *b1 = ptn1 - s12 * ptn2;
14883 *b2 = ptn2 - s12 * ptn1;
14884 if (*b1 < -tol || *b2 < -tol) {
14885
14886
14887
14888 n0 = jrand_(n, &ix, &iy, &iz);
14889 goto L2;
14890 }
14891 }
14892
14893
14894
14895 *i1 = n1;
14896 *i2 = n2;
14897 *i3 = n3;
14898 if (*b1 < 0.f) {
14899 *b1 = 0.f;
14900 }
14901 if (*b2 < 0.f) {
14902 *b2 = 0.f;
14903 }
14904 return 0;
14905
14906
14907
14908
14909
14910 L9:
14911 n1s = n1;
14912 n2s = n2;
14913 nl = 0;
14914
14915
14916
14917 L10:
14918
14919 lp = lend[n2];
14920 lp = lptr[lp];
14921 next = list[lp];
14922 if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14923 - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14924 >= -1e-10) {
14925
14926
14927
14928
14929 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14930 q[0] = x[n1] - s12 * x[n2];
14931 q[1] = y[n1] - s12 * y[n2];
14932 q[2] = z__[n1] - s12 * z__[n2];
14933 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14934 goto L11;
14935 }
14936 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14937 goto L11;
14938 }
14939
14940
14941
14942
14943 nl = n2;
14944 }
14945
14946
14947
14948 n1 = n2;
14949 n2 = next;
14950 if (n2 != n1s) {
14951 goto L10;
14952 }
14953
14954
14955
14956 *i1 = n1s;
14957 *i2 = n1s;
14958 *i3 = 0;
14959 return 0;
14960
14961
14962
14963 L11:
14964 nf = n2;
14965 if (nl == 0) {
14966
14967
14968
14969
14970 n2 = n2s;
14971 n1 = n1s;
14972
14973
14974
14975 L12:
14976 lp = lend[n1];
14977 next = -list[lp];
14978 if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14979 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14980 y[next]) >= -1e-10) {
14981
14982
14983
14984
14985 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14986 q[0] = x[n2] - s12 * x[n1];
14987 q[1] = y[n2] - s12 * y[n1];
14988 q[2] = z__[n2] - s12 * z__[n1];
14989 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14990 goto L13;
14991 }
14992 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14993 goto L13;
14994 }
14995
14996
14997
14998
14999 nf = n1;
15000 }
15001
15002
15003
15004 n2 = n1;
15005 n1 = next;
15006 if (n1 != n1s) {
15007 goto L12;
15008 }
15009
15010
15011
15012 *i1 = n1;
15013 *i2 = n1;
15014 *i3 = 0;
15015 return 0;
15016
15017
15018
15019 L13:
15020 nl = n1;
15021 }
15022
15023
15024
15025 *i1 = nf;
15026 *i2 = nl;
15027 *i3 = 0;
15028 return 0;
15029
15030
15031
15032 L14:
15033 *i1 = 0;
15034 *i2 = 0;
15035 *i3 = 0;
15036 return 0;
15037 }
15038
15039 int trlist_(int *n, int *list, int *lptr,
15040 int *lend, int *nrow, int *nt, int *ltri, int *
15041 ier)
15042 {
15043
15044 int ltri_dim1, ltri_offset, i__1, i__2;
15045
15046
15047 static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
15048 lpl, isv;
15049 static long int arcs;
15050 static int lpln1;
15051
15052
15053
15054
15055
15056
15057
15058
15059
15060
15061
15062
15063
15064
15065
15066
15067
15068
15069
15070
15071
15072
15073
15074
15075
15076
15077
15078
15079
15080
15081
15082
15083
15084
15085
15086
15087
15088
15089
15090
15091
15092
15093
15094
15095
15096
15097
15098
15099
15100
15101
15102
15103
15104
15105
15106
15107
15108
15109
15110
15111
15112
15113
15114
15115
15116
15117
15118
15119
15120
15121
15122
15123
15124
15125
15126
15127
15128
15129
15130
15131
15132
15133
15134
15135
15136
15137
15138
15139
15140
15141
15142
15143
15144
15145
15146
15147
15148
15149
15150
15151
15152
15153
15154
15155 --lend;
15156 --list;
15157 --lptr;
15158 ltri_dim1 = *nrow;
15159 ltri_offset = 1 + ltri_dim1;
15160 ltri -= ltri_offset;
15161
15162
15163 if (*n < 3 || (*nrow != 6 && *nrow != 9)) {
15164 goto L11;
15165 }
15166
15167
15168
15169
15170
15171
15172
15173
15174 arcs = *nrow == 9;
15175 ka = 0;
15176 kt = 0;
15177 nm2 = *n - 2;
15178
15179
15180
15181 i__1 = nm2;
15182 for (n1 = 1; n1 <= i__1; ++n1) {
15183
15184
15185
15186
15187 lpln1 = lend[n1];
15188 lp2 = lpln1;
15189 L1:
15190 lp2 = lptr[lp2];
15191 n2 = list[lp2];
15192 lp = lptr[lp2];
15193 n3 = (i__2 = list[lp], abs(i__2));
15194 if (n2 < n1 || n3 < n1) {
15195 goto L8;
15196 }
15197
15198
15199
15200 ++kt;
15201 ltri[kt * ltri_dim1 + 1] = n1;
15202 ltri[kt * ltri_dim1 + 2] = n2;
15203 ltri[kt * ltri_dim1 + 3] = n3;
15204
15205
15206
15207
15208 for (i__ = 1; i__ <= 3; ++i__) {
15209 if (i__ == 1) {
15210 i1 = n3;
15211 i2 = n2;
15212 } else if (i__ == 2) {
15213 i1 = n1;
15214 i2 = n3;
15215 } else {
15216 i1 = n2;
15217 i2 = n1;
15218 }
15219
15220
15221
15222
15223 lpl = lend[i1];
15224 lp = lptr[lpl];
15225 L2:
15226 if (list[lp] == i2) {
15227 goto L3;
15228 }
15229 lp = lptr[lp];
15230 if (lp != lpl) {
15231 goto L2;
15232 }
15233
15234
15235
15236
15237
15238 if ((i__2 = list[lp], abs(i__2)) != i2) {
15239 goto L12;
15240 }
15241 kn = 0;
15242 if (list[lp] < 0) {
15243 goto L6;
15244 }
15245
15246
15247
15248
15249 L3:
15250 lp = lptr[lp];
15251 i3 = (i__2 = list[lp], abs(i__2));
15252
15253
15254
15255
15256
15257 if (i1 < i2 && i1 < i3) {
15258 j = 3;
15259 } else if (i2 < i3) {
15260 j = 2;
15261 isv = i1;
15262 i1 = i2;
15263 i2 = i3;
15264 i3 = isv;
15265 } else {
15266 j = 1;
15267 isv = i1;
15268 i1 = i3;
15269 i3 = i2;
15270 i2 = isv;
15271 }
15272
15273
15274
15275 if (i1 > n1) {
15276 goto L7;
15277 }
15278
15279
15280
15281
15282 for (kn = kt - 1; kn >= 1; --kn) {
15283 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15284 == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15285 goto L5;
15286 }
15287
15288 }
15289 goto L7;
15290
15291
15292
15293 L5:
15294 ltri[j + 3 + kn * ltri_dim1] = kt;
15295
15296
15297
15298 L6:
15299 ltri[i__ + 3 + kt * ltri_dim1] = kn;
15300 if (arcs) {
15301 ++ka;
15302 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15303 if (kn != 0) {
15304 ltri[j + 6 + kn * ltri_dim1] = ka;
15305 }
15306 }
15307 L7:
15308 ;
15309 }
15310
15311
15312
15313 L8:
15314 if (lp2 != lpln1) {
15315 goto L1;
15316 }
15317
15318 }
15319
15320
15321
15322 *nt = kt;
15323 *ier = 0;
15324 return 0;
15325
15326
15327
15328 L11:
15329 *nt = 0;
15330 *ier = 1;
15331 return 0;
15332
15333
15334
15335
15336 L12:
15337 *nt = 0;
15338 *ier = 2;
15339 return 0;
15340 }
15341
15342 int trlprt_(int *n, double *x, double *y,
15343 double *z__, int *iflag, int *nrow, int *nt, int *
15344 ltri, int *lout)
15345 {
15346
15347
15348 static int nmax = 9999;
15349 static int nlmax = 58;
15350
15351
15352 int ltri_dim1, ltri_offset, i__1;
15353
15354
15355 static int i__, k, na, nb, nl, lun;
15356
15357
15358
15359
15360
15361
15362
15363
15364
15365
15366
15367
15368
15369
15370
15371
15372
15373
15374
15375
15376
15377
15378
15379
15380
15381
15382
15383
15384
15385
15386
15387
15388
15389
15390
15391
15392
15393
15394
15395
15396
15397
15398
15399
15400
15401
15402
15403
15404
15405
15406
15407
15408
15409
15410
15411
15412
15413
15414
15415
15416
15417
15418
15419
15420
15421
15422
15423
15424
15425
15426
15427
15428
15429 --z__;
15430 --y;
15431 --x;
15432 ltri_dim1 = *nrow;
15433 ltri_offset = 1 + ltri_dim1;
15434 ltri -= ltri_offset;
15435
15436
15437
15438
15439
15440
15441
15442
15443
15444
15445
15446
15447
15448
15449
15450
15451 lun = *lout;
15452 if (lun < 0 || lun > 99) {
15453 lun = 6;
15454 }
15455
15456
15457
15458
15459 nl = 3;
15460 if (*n < 3 || *n > nmax || (*nrow != 6 && *nrow != 9) || *nt < 1 || *nt >
15461 nmax) {
15462
15463
15464
15465
15466 return 0;
15467 }
15468 if (*iflag == 0) {
15469
15470
15471
15472
15473 nl = 6;
15474 i__1 = *n;
15475 for (i__ = 1; i__ <= i__1; ++i__) {
15476 if (nl >= nlmax) {
15477
15478 nl = 0;
15479 }
15480
15481 ++nl;
15482
15483 }
15484 } else if (*iflag > 0) {
15485
15486
15487
15488
15489 nl = 6;
15490 i__1 = *n;
15491 for (i__ = 1; i__ <= i__1; ++i__) {
15492 if (nl >= nlmax) {
15493
15494 nl = 0;
15495 }
15496
15497 ++nl;
15498
15499 }
15500 }
15501
15502
15503
15504 if (nl > nlmax / 2) {
15505
15506 nl = 0;
15507 }
15508 if (*nrow == 6) {
15509
15510 } else {
15511
15512 }
15513 nl += 5;
15514 i__1 = *nt;
15515 for (k = 1; k <= i__1; ++k) {
15516 if (nl >= nlmax) {
15517
15518 nl = 0;
15519 }
15520
15521 ++nl;
15522
15523 }
15524
15525
15526
15527
15528 nb = (*n << 1) - *nt - 2;
15529 if (nb < 3) {
15530 nb = 0;
15531 na = *n * 3 - 6;
15532 } else {
15533 na = *nt + *n - 1;
15534 }
15535
15536 return 0;
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 int trmesh_(int *n, double *x, double *y,
15563 double *z__, int *list, int *lptr, int *lend, int
15564 *lnew, int *near__, int *next, double *dist, int *ier)
15565 {
15566
15567 int i__1, i__2;
15568
15569
15570 static double d__;
15571 static int i__, j, k;
15572 static double d1, d2, d3;
15573 static int i0, lp, nn, lpl;
15574 long int left_(double *, double *, double *, double
15575 *, double *, double *, double *, double *,
15576 double *);
15577 static int nexti;
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
15723
15724
15725
15726
15727
15728
15729
15730
15731
15732
15733
15734
15735
15736
15737
15738
15739
15740
15741
15742
15743
15744
15745
15746
15747
15748
15749
15750
15751
15752
15753
15754
15755
15756
15757
15758
15759
15760
15761
15762
15763
15764
15765
15766
15767
15768
15769
15770
15771
15772
15773
15774
15775
15776
15777
15778
15779
15780
15781
15782
15783
15784
15785
15786
15787
15788
15789
15790
15791
15792
15793
15794
15795
15796
15797
15798
15799
15800
15801
15802
15803
15804 --dist;
15805 --next;
15806 --near__;
15807 --lend;
15808 --z__;
15809 --y;
15810 --x;
15811 --list;
15812 --lptr;
15813
15814
15815 nn = *n;
15816 if (nn < 3) {
15817 *ier = -1;
15818 return 0;
15819 }
15820
15821
15822
15823 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15824 z__[3])) {
15825
15826
15827
15828 list[1] = 3;
15829 lptr[1] = 2;
15830 list[2] = -2;
15831 lptr[2] = 1;
15832 lend[1] = 2;
15833
15834 list[3] = 1;
15835 lptr[3] = 4;
15836 list[4] = -3;
15837 lptr[4] = 3;
15838 lend[2] = 4;
15839
15840 list[5] = 2;
15841 lptr[5] = 6;
15842 list[6] = -1;
15843 lptr[6] = 5;
15844 lend[3] = 6;
15845
15846 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15847 y[3], &z__[3])) {
15848
15849
15850
15851
15852
15853 list[1] = 2;
15854 lptr[1] = 2;
15855 list[2] = -3;
15856 lptr[2] = 1;
15857 lend[1] = 2;
15858
15859 list[3] = 3;
15860 lptr[3] = 4;
15861 list[4] = -1;
15862 lptr[4] = 3;
15863 lend[2] = 4;
15864
15865 list[5] = 1;
15866 lptr[5] = 6;
15867 list[6] = -2;
15868 lptr[6] = 5;
15869 lend[3] = 6;
15870
15871 } else {
15872
15873
15874
15875 *ier = -2;
15876 return 0;
15877 }
15878
15879
15880
15881 *lnew = 7;
15882 if (nn == 3) {
15883 *ier = 0;
15884 return 0;
15885 }
15886
15887
15888
15889
15890
15891
15892
15893
15894
15895
15896
15897
15898
15899
15900
15901
15902
15903
15904
15905
15906
15907
15908
15909
15910
15911
15912
15913 near__[1] = 0;
15914 near__[2] = 0;
15915 near__[3] = 0;
15916 for (k = nn; k >= 4; --k) {
15917 d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15918 d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15919 d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15920 if (d1 <= d2 && d1 <= d3) {
15921 near__[k] = 1;
15922 dist[k] = d1;
15923 next[k] = near__[1];
15924 near__[1] = k;
15925 } else if (d2 <= d1 && d2 <= d3) {
15926 near__[k] = 2;
15927 dist[k] = d2;
15928 next[k] = near__[2];
15929 near__[2] = k;
15930 } else {
15931 near__[k] = 3;
15932 dist[k] = d3;
15933 next[k] = near__[3];
15934 near__[3] = k;
15935 }
15936
15937 }
15938
15939
15940
15941 i__1 = nn;
15942 for (k = 4; k <= i__1; ++k) {
15943 addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15944 lend[1], lnew, ier);
15945 if (*ier != 0) {
15946 return 0;
15947 }
15948
15949
15950
15951
15952 i__ = near__[k];
15953 if (near__[i__] == k) {
15954 near__[i__] = next[k];
15955 } else {
15956 i__ = near__[i__];
15957 L2:
15958 i0 = i__;
15959 i__ = next[i0];
15960 if (i__ != k) {
15961 goto L2;
15962 }
15963 next[i0] = next[k];
15964 }
15965 near__[k] = 0;
15966
15967
15968
15969 lpl = lend[k];
15970 lp = lpl;
15971 L3:
15972 lp = lptr[lp];
15973 j = (i__2 = list[lp], abs(i__2));
15974
15975
15976
15977
15978
15979
15980
15981 i__ = near__[j];
15982 L4:
15983 if (i__ == 0) {
15984 goto L5;
15985 }
15986 nexti = next[i__];
15987
15988
15989
15990
15991 d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15992 if (d__ < dist[i__]) {
15993
15994
15995
15996
15997
15998 near__[i__] = k;
15999 dist[i__] = d__;
16000 if (i__ == near__[j]) {
16001 near__[j] = nexti;
16002 } else {
16003 next[i0] = nexti;
16004 }
16005 next[i__] = near__[k];
16006 near__[k] = i__;
16007 } else {
16008 i0 = i__;
16009 }
16010
16011
16012
16013 i__ = nexti;
16014 goto L4;
16015
16016
16017
16018 L5:
16019 if (lp != lpl) {
16020 goto L3;
16021 }
16022
16023 }
16024 return 0;
16025 }
16026
16027 int trplot_(int *lun, double *pltsiz, double *
16028 elat, double *elon, double *a, int *n, double *x,
16029 double *y, double *z__, int *list, int *lptr, int
16030 *lend, char *, long int *numbr, int *ier, short )
16031 {
16032
16033
16034 static long int annot = TRUE_;
16035 static double fsizn = 10.;
16036 static double fsizt = 16.;
16037 static double tol = .5;
16038
16039
16040 int i__1, i__2;
16041 double d__1;
16042
16043
16044
16045
16046
16047
16048
16049 static double t;
16050 static int n0, n1;
16051 static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
16052 static int ir, lp;
16053 static double ex, ey, ez, wr, tx, ty;
16054 static int lpl;
16055 static double wrs;
16056 static int ipx1, ipx2, ipy1, ipy2, nseg;
16057 int drwarc_(int *, double *, double *,
16058 double *, int *);
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
16084
16085
16086
16087
16088
16089
16090
16091
16092
16093
16094
16095
16096
16097
16098
16099
16100
16101
16102
16103
16104
16105
16106
16107
16108
16109
16110
16111
16112
16113
16114
16115
16116
16117
16118
16119
16120
16121
16122
16123
16124
16125
16126
16127
16128
16129
16130
16131
16132
16133
16134
16135
16136
16137
16138
16139
16140
16141
16142
16143
16144
16145
16146
16147
16148
16149
16150
16151
16152
16153
16154
16155
16156
16157
16158
16159
16160
16161
16162
16163
16164
16165 --lend;
16166 --z__;
16167 --y;
16168 --x;
16169 --list;
16170 --lptr;
16171
16172
16173
16174
16175
16176
16177
16178
16179
16180
16181
16182
16183
16184
16185
16186
16187
16188
16189
16190
16191
16192
16193
16194
16195
16196
16197
16198
16199
16200
16201
16202
16203
16204
16205
16206
16207
16208
16209
16210
16211
16212
16213
16214
16215
16216
16217
16218
16219
16220
16221
16222
16223
16224
16225
16226 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16227 goto L11;
16228 }
16229 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16230 goto L12;
16231 }
16232
16233
16234
16235
16236 cf = atan(1.) / 45.;
16237 wr = sin(cf * *a);
16238 wrs = wr * wr;
16239
16240
16241
16242
16243
16244
16245
16246
16247
16248
16249 d__1 = *pltsiz * 36.;
16250 ir = i_dnnt(&d__1);
16251 ipx1 = 306 - ir;
16252 ipx2 = ir + 306;
16253 ipy1 = 396 - ir;
16254 ipy2 = ir + 396;
16255
16256
16257
16258
16259
16260
16261
16262
16263
16264
16265
16266
16267
16268
16269 d__1 = (double) ir * .88;
16270 ir = i_dnnt(&d__1);
16271 ipx1 = 306 - ir;
16272 ipx2 = ir + 306;
16273 ipy1 = 396 - ir;
16274 ipy2 = ir + 396;
16275
16276
16277
16278
16279 t = 2.;
16280
16281
16282
16283
16284
16285
16286
16287
16288
16289
16290 sf = (double) ir / wr;
16291 tx = ipx1 + sf * wr;
16292 ty = ipy1 + sf * wr;
16293
16294
16295
16296
16297
16298
16299
16300
16301 t = 1. / sf;
16302
16303
16304
16305
16306
16307
16308
16309
16310
16311
16312
16313
16314
16315
16316
16317
16318
16319
16320
16321
16322
16323
16324
16325 t = cf * *elon;
16326 ct = cos(cf * *elat);
16327 ex = ct * cos(t);
16328 ey = ct * sin(t);
16329 ez = sin(cf * *elat);
16330 if (ct != 0.) {
16331 r11 = -ey / ct;
16332 r12 = ex / ct;
16333 } else {
16334 r11 = 0.;
16335 r12 = 1.;
16336 }
16337 r21 = -ez * r12;
16338 r22 = ez * r11;
16339 r23 = ct;
16340
16341
16342
16343
16344 i__1 = *n;
16345 for (n0 = 1; n0 <= i__1; ++n0) {
16346 p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16347 if (p0[2] < 0.) {
16348 goto L3;
16349 }
16350 p0[0] = r11 * x[n0] + r12 * y[n0];
16351 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16352 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16353 goto L3;
16354 }
16355 lpl = lend[n0];
16356 lp = lpl;
16357
16358
16359
16360
16361 L1:
16362 lp = lptr[lp];
16363 n1 = (i__2 = list[lp], abs(i__2));
16364 p1[0] = r11 * x[n1] + r12 * y[n1];
16365 p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16366 p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16367 if (p1[2] < 0.) {
16368
16369
16370
16371
16372
16373 p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16374 p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16375 t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16376 p1[0] /= t;
16377 p1[1] /= t;
16378 }
16379
16380
16381
16382
16383 if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16384 goto L2;
16385 }
16386
16387
16388
16389
16390 if (p1[2] < 0.) {
16391 p1[2] = 0.;
16392 }
16393 d__1 = tol / sf;
16394 drwarc_(lun, p0, p1, &d__1, &nseg);
16395
16396
16397
16398 L2:
16399 if (lp != lpl) {
16400 goto L1;
16401 }
16402 L3:
16403 ;
16404 }
16405
16406
16407
16408
16409
16410
16411
16412 if (*numbr) {
16413
16414
16415
16416
16417
16418 t = fsizn / sf;
16419
16420
16421
16422
16423
16424
16425
16426 i__1 = *n;
16427 for (n0 = 1; n0 <= i__1; ++n0) {
16428 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16429 goto L4;
16430 }
16431 p0[0] = r11 * x[n0] + r12 * y[n0];
16432 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16433 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16434 goto L4;
16435 }
16436
16437
16438
16439
16440
16441
16442
16443
16444
16445 L4:
16446 ;
16447 }
16448 }
16449
16450
16451
16452
16453 t = fsizt / sf;
16454
16455
16456
16457
16458 p0[1] = wr + t * 3.;
16459
16460
16461
16462
16463
16464 if (annot) {
16465
16466
16467
16468 p0[0] = -wr;
16469 p0[1] = -wr - 50. / sf;
16470
16471
16472 p0[1] -= t * 2.;
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 *ier = 0;
16498 return 0;
16499
16500
16501
16502 L11:
16503 *ier = 1;
16504 return 0;
16505
16506
16507
16508 L12:
16509 *ier = 2;
16510 return 0;
16511
16512
16513
16514
16515 *ier = 3;
16516 return 0;
16517 }
16518
16519 int trprnt_(int *n, double *x, double *y,
16520 double *z__, int *iflag, int *list, int *lptr,
16521 int *lend, int *lout)
16522 {
16523
16524
16525 static int nmax = 9999;
16526 static int nlmax = 58;
16527
16528
16529 int i__1;
16530
16531
16532 static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16533 400];
16534
16535
16536
16537
16538
16539
16540
16541
16542
16543
16544
16545
16546
16547
16548
16549
16550
16551
16552
16553
16554
16555
16556
16557
16558
16559
16560
16561
16562
16563
16564
16565
16566
16567
16568
16569
16570
16571
16572
16573
16574
16575
16576
16577
16578
16579
16580
16581
16582
16583
16584
16585
16586
16587
16588
16589
16590
16591
16592
16593
16594
16595
16596 --lend;
16597 --z__;
16598 --y;
16599 --x;
16600 --list;
16601 --lptr;
16602
16603
16604
16605
16606
16607
16608
16609
16610
16611
16612
16613
16614
16615
16616
16617
16618
16619
16620
16621
16622
16623
16624
16625
16626
16627
16628
16629 nn = *n;
16630 lun = *lout;
16631 if (lun < 0 || lun > 99) {
16632 lun = 6;
16633 }
16634
16635
16636
16637
16638 if (nn < 3 || nn > nmax) {
16639
16640
16641
16642
16643 return 0;
16644 }
16645
16646
16647
16648
16649 nl = 6;
16650 nb = 0;
16651 if (*iflag < 0) {
16652
16653
16654
16655
16656
16657 i__1 = nn;
16658 for (node = 1; node <= i__1; ++node) {
16659 lpl = lend[node];
16660 lp = lpl;
16661 k = 0;
16662
16663 L1:
16664 ++k;
16665 lp = lptr[lp];
16666 nd = list[lp];
16667 nabor[k - 1] = nd;
16668 if (lp != lpl) {
16669 goto L1;
16670 }
16671 if (nd <= 0) {
16672
16673
16674
16675
16676
16677 nabor[k - 1] = -nd;
16678 ++k;
16679 nabor[k - 1] = 0;
16680 ++nb;
16681 }
16682
16683
16684
16685 inc = (k - 1) / 14 + 2;
16686 nl += inc;
16687 if (nl > nlmax) {
16688
16689 nl = inc;
16690 }
16691
16692
16693
16694
16695 }
16696 } else if (*iflag > 0) {
16697
16698
16699
16700
16701 i__1 = nn;
16702 for (node = 1; node <= i__1; ++node) {
16703 lpl = lend[node];
16704 lp = lpl;
16705 k = 0;
16706
16707 L3:
16708 ++k;
16709 lp = lptr[lp];
16710 nd = list[lp];
16711 nabor[k - 1] = nd;
16712 if (lp != lpl) {
16713 goto L3;
16714 }
16715 if (nd <= 0) {
16716
16717
16718
16719 nabor[k - 1] = -nd;
16720 ++k;
16721 nabor[k - 1] = 0;
16722 ++nb;
16723 }
16724
16725
16726
16727 inc = (k - 1) / 8 + 2;
16728 nl += inc;
16729 if (nl > nlmax) {
16730
16731 nl = inc;
16732 }
16733
16734
16735
16736
16737
16738 }
16739 } else {
16740
16741
16742
16743
16744 i__1 = nn;
16745 for (node = 1; node <= i__1; ++node) {
16746 lpl = lend[node];
16747 lp = lpl;
16748 k = 0;
16749
16750 L5:
16751 ++k;
16752 lp = lptr[lp];
16753 nd = list[lp];
16754 nabor[k - 1] = nd;
16755 if (lp != lpl) {
16756 goto L5;
16757 }
16758 if (nd <= 0) {
16759
16760
16761
16762 nabor[k - 1] = -nd;
16763 ++k;
16764 nabor[k - 1] = 0;
16765 ++nb;
16766 }
16767
16768
16769
16770 inc = (k - 1) / 5 + 2;
16771 nl += inc;
16772 if (nl > nlmax) {
16773
16774 nl = inc;
16775 }
16776
16777
16778
16779
16780
16781 }
16782 }
16783
16784
16785
16786
16787 if (nb != 0) {
16788 na = nn * 3 - nb - 3;
16789 nt = (nn << 1) - nb - 2;
16790 } else {
16791 na = nn * 3 - 6;
16792 nt = (nn << 1) - 4;
16793 }
16794
16795 return 0;
16796
16797
16798
16799
16800
16801
16802
16803
16804
16805
16806
16807
16808
16809
16810
16811
16812
16813
16814
16815
16816 }
16817
16818 int vrplot_(int *lun, double *pltsiz, double *
16819 elat, double *elon, double *a, int *n, double *x,
16820 double *y, double *z__, int *nt, int *listc, int *
16821 lptr, int *lend, double *xc, double *yc, double *zc,
16822 char *, long int *numbr, int *ier, short)
16823 {
16824
16825
16826 static long int annot = TRUE_;
16827 static double fsizn = 10.;
16828 static double fsizt = 16.;
16829 static double tol = .5;
16830
16831
16832 int i__1;
16833 double d__1;
16834
16835
16836
16837
16838
16839
16840
16841 static double t;
16842 static int n0;
16843 static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16844 sf;
16845 static int ir, lp;
16846 static double ex, ey, ez, wr, tx, ty;
16847 static long int in1, in2;
16848 static int kv1, kv2, lpl;
16849 static double wrs;
16850 static int ipx1, ipx2, ipy1, ipy2, nseg;
16851 int drwarc_(int *, double *, double *,
16852 double *, int *);
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
16914
16915
16916
16917
16918
16919
16920
16921
16922
16923
16924
16925
16926
16927
16928
16929
16930
16931
16932
16933
16934
16935
16936
16937
16938
16939
16940
16941
16942
16943
16944
16945
16946
16947
16948
16949
16950
16951
16952
16953
16954
16955
16956
16957
16958
16959
16960
16961
16962
16963
16964
16965
16966
16967
16968
16969
16970
16971
16972
16973
16974
16975
16976
16977
16978
16979
16980
16981
16982
16983
16984
16985
16986
16987
16988
16989
16990
16991
16992
16993
16994
16995 --lend;
16996 --z__;
16997 --y;
16998 --x;
16999 --zc;
17000 --yc;
17001 --xc;
17002 --listc;
17003 --lptr;
17004
17005
17006
17007
17008
17009
17010
17011
17012
17013
17014
17015
17016
17017
17018
17019
17020
17021
17022
17023
17024
17025
17026
17027
17028
17029
17030
17031
17032
17033
17034
17035
17036
17037
17038
17039
17040
17041
17042
17043
17044
17045
17046
17047
17048
17049
17050
17051
17052
17053
17054
17055
17056
17057
17058
17059
17060
17061
17062 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
17063 nt != 2 * *n - 4) {
17064 goto L11;
17065 }
17066 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
17067 goto L12;
17068 }
17069
17070
17071
17072
17073 cf = atan(1.) / 45.;
17074 wr = sin(cf * *a);
17075 wrs = wr * wr;
17076
17077
17078
17079
17080
17081
17082
17083
17084
17085
17086 d__1 = *pltsiz * 36.;
17087 ir = i_dnnt(&d__1);
17088 ipx1 = 306 - ir;
17089 ipx2 = ir + 306;
17090 ipy1 = 396 - ir;
17091 ipy2 = ir + 396;
17092
17093
17094
17095
17096
17097
17098
17099
17100
17101
17102
17103
17104
17105 d__1 = (double) ir * .88;
17106 ir = i_dnnt(&d__1);
17107 ipx1 = 306 - ir;
17108 ipx2 = ir + 306;
17109 ipy1 = 396 - ir;
17110 ipy2 = ir + 396;
17111
17112
17113
17114
17115 t = 2.;
17116
17117
17118
17119
17120
17121
17122
17123
17124
17125
17126 sf = (double) ir / wr;
17127 tx = ipx1 + sf * wr;
17128 ty = ipy1 + sf * wr;
17129
17130
17131
17132
17133
17134
17135
17136
17137 t = 1. / sf;
17138
17139
17140
17141
17142
17143
17144
17145
17146
17147
17148
17149
17150
17151
17152
17153
17154
17155
17156
17157
17158
17159
17160
17161 t = cf * *elon;
17162 ct = cos(cf * *elat);
17163 ex = ct * cos(t);
17164 ey = ct * sin(t);
17165 ez = sin(cf * *elat);
17166 if (ct != 0.) {
17167 r11 = -ey / ct;
17168 r12 = ex / ct;
17169 } else {
17170 r11 = 0.;
17171 r12 = 1.;
17172 }
17173 r21 = -ez * r12;
17174 r22 = ez * r11;
17175 r23 = ct;
17176
17177
17178
17179
17180 i__1 = *n;
17181 for (n0 = 1; n0 <= i__1; ++n0) {
17182 lpl = lend[n0];
17183
17184
17185
17186
17187 kv2 = listc[lpl];
17188 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17189 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17190 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17191
17192
17193
17194 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17195
17196
17197
17198
17199 lp = lpl;
17200 L1:
17201 lp = lptr[lp];
17202 kv1 = kv2;
17203 p1[0] = p2[0];
17204 p1[1] = p2[1];
17205 p1[2] = p2[2];
17206 in1 = in2;
17207 kv2 = listc[lp];
17208
17209
17210
17211 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17212 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17213 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17214 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17215
17216
17217
17218
17219
17220 if (! in1 || (in2 && kv2 <= kv1)) {
17221 goto L2;
17222 }
17223 if (p2[2] < 0.) {
17224
17225
17226
17227
17228
17229 p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17230 p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17231 t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17232 p2[0] /= t;
17233 p2[1] /= t;
17234 }
17235
17236
17237
17238
17239 if (p2[2] < 0.) {
17240 p2[2] = 0.f;
17241 }
17242 d__1 = tol / sf;
17243 drwarc_(lun, p1, p2, &d__1, &nseg);
17244
17245
17246
17247 L2:
17248 if (lp != lpl) {
17249 goto L1;
17250 }
17251
17252 }
17253
17254
17255
17256
17257
17258
17259
17260 if (*numbr) {
17261
17262
17263
17264
17265
17266 t = fsizn / sf;
17267
17268
17269
17270
17271
17272
17273
17274 i__1 = *n;
17275 for (n0 = 1; n0 <= i__1; ++n0) {
17276 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17277 goto L4;
17278 }
17279 x0 = r11 * x[n0] + r12 * y[n0];
17280 y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17281 if (x0 * x0 + y0 * y0 > wrs) {
17282 goto L4;
17283 }
17284
17285
17286
17287
17288
17289
17290
17291
17292 L4:
17293 ;
17294 }
17295 }
17296
17297
17298
17299
17300 t = fsizt / sf;
17301
17302
17303
17304
17305 y0 = wr + t * 3.;
17306
17307
17308
17309
17310
17311 if (annot) {
17312
17313
17314
17315 x0 = -wr;
17316 y0 = -wr - 50. / sf;
17317
17318
17319 y0 -= t * 2.;
17320
17321
17322
17323
17324
17325 }
17326
17327
17328
17329
17330
17331
17332
17333
17334
17335
17336
17337
17338
17339
17340
17341
17342
17343
17344 *ier = 0;
17345 return 0;
17346
17347
17348
17349 L11:
17350 *ier = 1;
17351 return 0;
17352
17353
17354
17355 L12:
17356 *ier = 2;
17357 return 0;
17358
17359
17360
17361
17362 *ier = 3;
17363 return 0;
17364 }
17365
17366 int random_(int *ix, int *iy, int *iz,
17367 double *rannum)
17368 {
17369 static double x;
17370
17371
17372
17373
17374
17375
17376
17377
17378
17379
17380
17381
17382
17383
17384
17385 *ix = *ix * 171 % 30269;
17386 *iy = *iy * 172 % 30307;
17387 *iz = *iz * 170 % 30323;
17388 x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17389 double) (*iz) / 30323.;
17390 *rannum = x - (int) x;
17391 return 0;
17392 }
17393
17394 #undef TRUE_
17395 #undef FALSE_
17396 #undef abs
17397
17398
17399
17400
17401
17402
17403
17404
17405
17406 EMData* Util::mult_scalar(EMData* img, float scalar)
17407 {
17408 ENTERFUNC;
17409
17410 if (!img) {
17411 throw NullPointerException("NULL input image");
17412 }
17413
17414
17415 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17416 size_t size = (size_t)nx*ny*nz;
17417 EMData * img2 = img->copy_head();
17418 float *img_ptr =img->get_data();
17419 float *img2_ptr = img2->get_data();
17420 for (size_t i=0;i<size;++i)img2_ptr[i] = img_ptr[i]*scalar;
17421 img2->update();
17422
17423 if(img->is_complex()) {
17424 img2->set_complex(true);
17425 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17426 }
17427 EXITFUNC;
17428 return img2;
17429 }
17430
17431 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17432 {
17433 ENTERFUNC;
17434
17435 if (!img) {
17436 throw NullPointerException("NULL input image");
17437 }
17438
17439
17440 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17441 size_t size = (size_t)nx*ny*nz;
17442 EMData * img2 = img->copy_head();
17443 float *img_ptr =img->get_data();
17444 float *img2_ptr = img2->get_data();
17445 float *img1_ptr = img1->get_data();
17446 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17447 img2->update();
17448 if(img->is_complex()) {
17449 img2->set_complex(true);
17450 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17451 }
17452
17453 EXITFUNC;
17454 return img2;
17455 }
17456
17457 EMData* Util::addn_img(EMData* img, EMData* img1)
17458 {
17459 ENTERFUNC;
17460
17461 if (!img) {
17462 throw NullPointerException("NULL input image");
17463 }
17464
17465
17466 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17467 size_t size = (size_t)nx*ny*nz;
17468 EMData * img2 = img->copy_head();
17469 float *img_ptr =img->get_data();
17470 float *img2_ptr = img2->get_data();
17471 float *img1_ptr = img1->get_data();
17472 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17473 img2->update();
17474 if(img->is_complex()) {
17475 img2->set_complex(true);
17476 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17477 }
17478
17479 EXITFUNC;
17480 return img2;
17481 }
17482
17483 EMData* Util::subn_img(EMData* img, EMData* img1)
17484 {
17485 ENTERFUNC;
17486
17487 if (!img) {
17488 throw NullPointerException("NULL input image");
17489 }
17490
17491
17492 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17493 size_t size = (size_t)nx*ny*nz;
17494 EMData * img2 = img->copy_head();
17495 float *img_ptr =img->get_data();
17496 float *img2_ptr = img2->get_data();
17497 float *img1_ptr = img1->get_data();
17498 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17499 img2->update();
17500 if(img->is_complex()) {
17501 img2->set_complex(true);
17502 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17503 }
17504
17505 EXITFUNC;
17506 return img2;
17507 }
17508
17509 EMData* Util::muln_img(EMData* img, EMData* img1)
17510 {
17511 ENTERFUNC;
17512
17513 if (!img) {
17514 throw NullPointerException("NULL input image");
17515 }
17516
17517
17518 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17519 size_t size = (size_t)nx*ny*nz;
17520 EMData * img2 = img->copy_head();
17521 float *img_ptr =img->get_data();
17522 float *img2_ptr = img2->get_data();
17523 float *img1_ptr = img1->get_data();
17524 if(img->is_complex()) {
17525 for (size_t i=0; i<size; i+=2) {
17526 img2_ptr[i] = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17527 img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17528 }
17529 img2->set_complex(true);
17530 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17531 } else {
17532 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17533 img2->update();
17534 }
17535
17536 EXITFUNC;
17537 return img2;
17538 }
17539
17540 EMData* Util::divn_img(EMData* img, EMData* img1)
17541 {
17542 ENTERFUNC;
17543
17544 if (!img) {
17545 throw NullPointerException("NULL input image");
17546 }
17547
17548
17549 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17550 size_t size = (size_t)nx*ny*nz;
17551 EMData * img2 = img->copy_head();
17552 float *img_ptr =img->get_data();
17553 float *img2_ptr = img2->get_data();
17554 float *img1_ptr = img1->get_data();
17555 if(img->is_complex()) {
17556 float sq2;
17557 for (size_t i=0; i<size; i+=2) {
17558 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17559 img2_ptr[i] = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17560 img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17561 }
17562 img2->set_complex(true);
17563 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17564 } else {
17565 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17566 img2->update();
17567 }
17568
17569 EXITFUNC;
17570 return img2;
17571 }
17572
17573 EMData* Util::divn_filter(EMData* img, EMData* img1)
17574 {
17575 ENTERFUNC;
17576
17577 if (!img) {
17578 throw NullPointerException("NULL input image");
17579 }
17580
17581
17582 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17583 size_t size = (size_t)nx*ny*nz;
17584 EMData * img2 = img->copy_head();
17585 float *img_ptr =img->get_data();
17586 float *img1_ptr = img1->get_data();
17587 float *img2_ptr = img2->get_data();
17588 if(img->is_complex()) {
17589 for (size_t i=0; i<size; i+=2) {
17590 if(img1_ptr[i] > 1.e-10f) {
17591 img2_ptr[i] = img_ptr[i] /img1_ptr[i];
17592 img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17593 } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17594 }
17595 } else throw ImageFormatException("Only Fourier image allowed");
17596
17597 img->update();
17598
17599 EXITFUNC;
17600 return img2;
17601 }
17602
17603 void Util::mul_scalar(EMData* img, float scalar)
17604 {
17605 ENTERFUNC;
17606
17607 if (!img) {
17608 throw NullPointerException("NULL input image");
17609 }
17610
17611
17612 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17613 size_t size = (size_t)nx*ny*nz;
17614 float *img_ptr =img->get_data();
17615 for (size_t i=0;i<size;++i) img_ptr[i] *= scalar;
17616 img->update();
17617
17618 EXITFUNC;
17619 }
17620
17621 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17622 {
17623 ENTERFUNC;
17624
17625 if (!img) {
17626 throw NullPointerException("NULL input image");
17627 }
17628
17629
17630 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17631 size_t size = (size_t)nx*ny*nz;
17632 float *img_ptr =img->get_data();
17633 float *img1_ptr = img1->get_data();
17634 for (size_t i=0;i<size;++i)img_ptr[i] += img1_ptr[i]*scalar;
17635 img1->update();
17636
17637 EXITFUNC;
17638 }
17639
17640 void Util::add_img(EMData* img, EMData* img1)
17641 {
17642 ENTERFUNC;
17643
17644 if (!img || !img1) {
17645 throw NullPointerException("NULL input image");
17646 }
17647
17648
17649 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17650 size_t size = (size_t)nx*ny*nz;
17651 float *img_ptr = img->get_data();
17652 float *img1_ptr = img1->get_data();
17653 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i];
17654 img->update();
17655
17656 EXITFUNC;
17657 }
17658
17659 void Util::add_img_abs(EMData* img, EMData* img1)
17660 {
17661 ENTERFUNC;
17662
17663 if (!img) {
17664 throw NullPointerException("NULL input image");
17665 }
17666
17667
17668 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17669 size_t size = (size_t)nx*ny*nz;
17670 float *img_ptr = img->get_data();
17671 float *img1_ptr = img1->get_data();
17672 for (size_t i=0;i<size;++i) img_ptr[i] += abs(img1_ptr[i]);
17673 img->update();
17674
17675 EXITFUNC;
17676 }
17677
17678 void Util::add_img2(EMData* img, EMData* img1)
17679 {
17680 ENTERFUNC;
17681
17682 if (!img) {
17683 throw NullPointerException("NULL input image");
17684 }
17685
17686
17687 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17688 size_t size = (size_t)nx*ny*nz;
17689 float *img_ptr = img->get_data();
17690 float *img1_ptr = img1->get_data();
17691 if(img->is_complex()) {
17692 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] ;
17693 } else {
17694 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17695 }
17696 img->update();
17697
17698 EXITFUNC;
17699 }
17700
17701 void Util::sub_img(EMData* img, EMData* img1)
17702 {
17703 ENTERFUNC;
17704
17705 if (!img) {
17706 throw NullPointerException("NULL input image");
17707 }
17708
17709
17710 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17711 size_t size = (size_t)nx*ny*nz;
17712 float *img_ptr = img->get_data();
17713 float *img1_ptr = img1->get_data();
17714 for (size_t i=0;i<size;++i) img_ptr[i] -= img1_ptr[i];
17715 img->update();
17716
17717 EXITFUNC;
17718 }
17719
17720 void Util::mul_img(EMData* img, EMData* img1)
17721 {
17722 ENTERFUNC;
17723
17724 if (!img) {
17725 throw NullPointerException("NULL input image");
17726 }
17727
17728
17729 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17730 size_t size = (size_t)nx*ny*nz;
17731 float *img_ptr = img->get_data();
17732 float *img1_ptr = img1->get_data();
17733 if(img->is_complex()) {
17734 for (size_t i=0; i<size; i+=2) {
17735 float tmp = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17736 img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17737 img_ptr[i] = tmp;
17738
17739 }
17740 } else {
17741 for (size_t i=0;i<size;++i) img_ptr[i] *= img1_ptr[i];
17742 }
17743 img->update();
17744
17745 EXITFUNC;
17746 }
17747
17748 void Util::div_img(EMData* img, EMData* img1)
17749 {
17750 ENTERFUNC;
17751
17752 if (!img) {
17753 throw NullPointerException("NULL input image");
17754 }
17755
17756
17757 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17758 size_t size = (size_t)nx*ny*nz;
17759 float *img_ptr = img->get_data();
17760 float *img1_ptr = img1->get_data();
17761 if(img->is_complex()) {
17762 float sq2;
17763 for (size_t i=0; i<size; i+=2) {
17764 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17765 float tmp = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17766 img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17767 img_ptr[i] = tmp;
17768 }
17769 } else {
17770 for (size_t i=0; i<size; ++i) img_ptr[i] /= img1_ptr[i];
17771 }
17772 img->update();
17773
17774 EXITFUNC;
17775 }
17776
17777 void Util::div_filter(EMData* img, EMData* img1)
17778 {
17779 ENTERFUNC;
17780
17781 if (!img) {
17782 throw NullPointerException("NULL input image");
17783 }
17784
17785
17786 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17787 size_t size = (size_t)nx*ny*nz;
17788 float *img_ptr = img->get_data();
17789 float *img1_ptr = img1->get_data();
17790 if(img->is_complex()) {
17791 for (size_t i=0; i<size; i+=2) {
17792 if(img1_ptr[i] > 1.e-10f) {
17793 img_ptr[i] /= img1_ptr[i];
17794 img_ptr[i+1] /= img1_ptr[i];
17795 } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17796 }
17797 } else throw ImageFormatException("Only Fourier image allowed");
17798
17799 img->update();
17800
17801 EXITFUNC;
17802 }
17803
17804 #define img_ptr(i,j,k) img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*(size_t)nxo]
17805
17806 EMData* Util::pack_complex_to_real(EMData* img)
17807 {
17808 ENTERFUNC;
17809
17810 if (!img) {
17811 throw NullPointerException("NULL input image");
17812 }
17813
17814
17815
17816 int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17817 int nx = nxo - 2 + img->is_fftodd();
17818 int lsd2 = (nx + 2 - nx%2) / 2;
17819 int nyt, nzt;
17820 int nx2 = nx/2;
17821 int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17822 int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17823 int nx2p = nx2+nx%2;
17824 int ny2p = ny2+ny%2;
17825 int nz2p = nz2+nz%2;
17826 EMData& power = *(new EMData());
17827 power.set_size(nx, ny, nz);
17828 power.set_array_offsets(-nx2,-ny2,-nz2);
17829
17830 float *img_ptr = img->get_data();
17831 for (int iz = 1; iz <= nz; iz++) {
17832 int jz=iz-1;
17833 if(jz>=nz2p) jz=jz-nzt;
17834 for (int iy = 1; iy <= ny; iy++) {
17835 int jy=iy-1;
17836 if(jy>=ny2p) jy=jy-nyt;
17837 for (int ix = 1; ix <= lsd2; ix++) {
17838 int jx=ix-1;
17839 if(jx>=nx2p) jx=jx-nx;
17840 power(jx,jy,jz) = img_ptr(ix,iy,iz);
17841 }
17842 }
17843 }
17844
17845 int nzb, nze, nyb, nye, nxb, nxe;
17846 nxb =-nx2+(nx+1)%2;
17847 nxe = nx2-(nx+1)%2;
17848 if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17849 if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17850 for (int iz = nzb; iz <= nze; iz++) {
17851 for (int iy = nyb; iy <= nye; iy++) {
17852 for (int ix = 1; ix <= nxe; ix++) {
17853 power(-ix,-iy,-iz) = power(ix,iy,iz);
17854 }
17855 }
17856 }
17857 if(ny2 != 0) {
17858 if(nz2 != 0) {
17859 if(nz%2 == 0) {
17860 for (int iy = nyb; iy <= nye; iy++) {
17861 for (int ix = nxb; ix <= -1; ix++) {
17862 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17863 }
17864 }
17865 if(ny%2 == 0) {
17866 for (int ix = nxb; ix <= -1; ix++) {
17867 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17868 }
17869 }
17870 }
17871 }
17872 if(ny%2 == 0) {
17873 for (int iz = nzb; iz <= nze; iz++) {
17874 for (int ix = nxb; ix <= -1; ix++) {
17875 power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17876 }
17877 }
17878 }
17879
17880 }
17881 power.update();
17882 power.set_array_offsets(0,0,0);
17883 return &power;
17884 }
17885 #undef img_ptr
17886
17887 float Util::ang_n(float peakp, string mode, int maxrin)
17888 {
17889 if (mode == "f" || mode == "F")
17890 return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17891 else
17892 return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17893 }
17894
17895
17896 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17897 {
17898 float* data = ring->get_data();
17899 float av=0.0;
17900 float sq=0.0;
17901 float nn=0.0;
17902 int nring = numr.size()/3;
17903 for( int i=0; i < nring; ++i )
17904 {
17905 int numr3i = numr[3*i+2];
17906 int numr2i = numr[3*i+1]-1;
17907 float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17908 for( int j=0; j < numr3i; ++j )
17909 {
17910 int jc = numr2i+j;
17911 av += data[jc] * w;
17912 sq += data[jc] * data[jc] * w;
17913 nn += w;
17914 }
17915 }
17916
17917 float avg = av/nn;
17918 float sgm = sqrt( (sq-av*av/nn)/nn );
17919 size_t n = (size_t)ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17920 for( size_t i=0; i < n; ++i )
17921 {
17922 data[i] -= avg;
17923 data[i] /= sgm;
17924 }
17925
17926 ring->update();
17927 }
17928
17929 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17930 float xrng, float yrng, float step, string mode,
17931 vector<int>numr, float cnx, float cny) {
17932
17933
17934
17935
17936
17937
17938
17939
17940
17941
17942
17943
17944 size_t crefim_len = crefim.size();
17945
17946 int ky = int(2*yrng/step+0.5)/2;
17947 int kx = int(2*xrng/step+0.5)/2;
17948 int iref, nref=0, mirror=0;
17949 float iy, ix, sx=0, sy=0;
17950 float peak = -1.0E23f;
17951 float ang=0.0f;
17952 for (int i = -ky; i <= ky; i++) {
17953 iy = i * step ;
17954 for (int j = -kx; j <= kx; j++) {
17955 ix = j*step ;
17956 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17957
17958 Normalize_ring( cimage, numr );
17959
17960 Frngs(cimage, numr);
17961
17962
17963 for ( iref = 0; iref < (int)crefim_len; iref++) {
17964 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17965 double qn = retvals["qn"];
17966 double qm = retvals["qm"];
17967 if(qn >= peak || qm >= peak) {
17968 sx = -ix;
17969 sy = -iy;
17970 nref = iref;
17971 if (qn >= qm) {
17972 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17973 peak = static_cast<float>(qn);
17974 mirror = 0;
17975 } else {
17976 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17977 peak = static_cast<float>(qm);
17978 mirror = 1;
17979 }
17980 }
17981 } delete cimage; cimage = 0;
17982 }
17983 }
17984 float co, so, sxs, sys;
17985 co = static_cast<float>( cos(ang*pi/180.0) );
17986 so = static_cast<float>( -sin(ang*pi/180.0) );
17987 sxs = sx*co - sy*so;
17988 sys = sx*so + sy*co;
17989 vector<float> res;
17990 res.push_back(ang);
17991 res.push_back(sxs);
17992 res.push_back(sys);
17993 res.push_back(static_cast<float>(mirror));
17994 res.push_back(static_cast<float>(nref));
17995 res.push_back(peak);
17996 return res;
17997 }
17998
17999 vector<float> Util::multiref_polar_ali_2d_peaklist(EMData* image, const vector< EMData* >& crefim,
18000 float xrng, float yrng, float step, string mode,
18001 vector<int>numr, float cnx, float cny) {
18002
18003 size_t crefim_len = crefim.size();
18004
18005 int ky = int(2*yrng/step+0.5)/2;
18006 int kx = int(2*xrng/step+0.5)/2;
18007 float iy, ix;
18008 vector<float> peak(crefim_len*5, -1.0e23f);
18009 for (int i = -ky; i <= ky; i++) {
18010 iy = i * step ;
18011 for (int j = -kx; j <= kx; j++) {
18012 ix = j*step ;
18013 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18014 Normalize_ring( cimage, numr );
18015 Frngs(cimage, numr);
18016 for (int iref = 0; iref < (int)crefim_len; iref++) {
18017 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18018 double qn = retvals["qn"];
18019 double qm = retvals["qm"];
18020 if(qn >= peak[iref*5] || qm >= peak[iref*5]) {
18021 if (qn >= qm) {
18022 peak[iref*5] = static_cast<float>(qn);
18023 peak[iref*5+1] = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18024 peak[iref*5+2] = -ix;
18025 peak[iref*5+3] = -iy;
18026 peak[iref*5+4] = 0;
18027 } else {
18028 peak[iref*5] = static_cast<float>(qm);
18029 peak[iref*5+1] = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18030 peak[iref*5+2] = -ix;
18031 peak[iref*5+3] = -iy;
18032 peak[iref*5+4] = 1;
18033 }
18034 }
18035 } delete cimage; cimage = 0;
18036 }
18037 }
18038 for (int iref = 0; iref < (int)crefim_len; iref++) {
18039 float ang = peak[iref*5+1];
18040 float sx = peak[iref*5+2];
18041 float sy = peak[iref*5+3];
18042 float co = cos(ang*pi/180.0);
18043 float so = -sin(ang*pi/180.0);
18044 float sxs = sx*co - sy*so;
18045 float sys = sx*so + sy*co;
18046 peak[iref*5+2] = sxs;
18047 peak[iref*5+3] = sys;
18048 }
18049 return peak;
18050 }
18051
18052 struct peak_table {
18053 float value;
18054 int index;
18055 bool operator<(const peak_table& b) const { return value > b.value; }
18056 };
18057
18058 vector<int> Util::assign_groups(const vector<float>& d, int nref, int nima) {
18059
18060 int kt = nref;
18061 unsigned int maxasi = nima/nref;
18062 vector< vector<int> > id_list;
18063 id_list.resize(nref);
18064 int group, ima;
18065
18066 peak_table* dd = new peak_table[nref*nima];
18067 for (int i=0; i<nref*nima; i++) {
18068 dd[i].value = d[i];
18069 dd[i].index = i;
18070 }
18071 sort(dd, dd+nref*nima);
18072 int begin = 0;
18073
18074 bool* del_row = new bool[nref];
18075 for (int i=0; i<nref; i++) del_row[i] = false;
18076 bool* del_column = new bool[nima];
18077 for (int i=0; i<nima; i++) del_column[i] = false;
18078 while (kt > 0) {
18079 bool flag = true;
18080 while (flag) {
18081 int l = dd[begin].index;
18082 group = l/nima;
18083 ima = l%nima;
18084 if (del_column[ima] || del_row[group]) begin++;
18085 else flag = false;
18086 }
18087
18088 id_list[group].push_back(ima);
18089 if (kt > 1) {
18090 if (id_list[group].size() < maxasi) group = -1;
18091 else kt -= 1;
18092 } else {
18093 if (id_list[group].size() < maxasi+nima%nref) group = -1;
18094 else kt -= 1;
18095 }
18096 del_column[ima] = true;
18097 if (group != -1) {
18098 del_row[group] = true;
18099 }
18100 }
18101
18102 vector<int> id_list_1;
18103 for (int iref=0; iref<nref; iref++)
18104 for (unsigned int im=0; im<maxasi; im++)
18105 id_list_1.push_back(id_list[iref][im]);
18106 for (unsigned int im=maxasi; im<maxasi+nima%nref; im++)
18107 id_list_1.push_back(id_list[group][im]);
18108 id_list_1.push_back(group);
18109
18110 delete[] del_row;
18111 delete[] del_column;
18112 delete[] dd;
18113 return id_list_1;
18114 }
18115
18116 int Util::nearest_ang(const vector<float>& vecref, float x, float y, float z) {
18117 float best_v = -1.0f;
18118 int best_i = -1;
18119
18120 for (unsigned int i=0; i<vecref.size()/3; i++) {
18121 float v = abs(vecref[i*3]*x+vecref[i*3+1]*y+vecref[i*3+2]*z);
18122 if (v > best_v) {
18123 best_v = v;
18124 best_i = i;
18125 }
18126 }
18127 return best_i;
18128 }
18129
18130 struct d_ang {
18131 float d;
18132 int i;
18133 int mirror;
18134 d_ang(float _d, int _i, int _m):d(_d), i(_i), mirror(_m) {}
18135 bool operator<(const d_ang& a) const { return d < a.d || (d == a.d && i < a.i); }
18136 };
18137
18138 vector<int> Util::assign_projangles(const vector<float>& projangles, const vector<float>& refangles) {
18139 int nref = refangles.size()/2;
18140 int nproj = projangles.size()/2;
18141 vector<int> asg(nproj);
18142 vector<float> vecref(nref*3);
18143 for (int i=0; i<nref; i++)
18144 getvec(refangles[i*2], refangles[i*2+1], vecref[i*3], vecref[i*3+1], vecref[i*3+2]);
18145 for (int i=0; i<nproj; i++) {
18146 float x, y, z;
18147 getvec(projangles[i*2], projangles[i*2+1], x, y, z);
18148 asg[i] = nearest_ang(vecref, x, y, z);
18149 }
18150 return asg;
18151 }
18152
18153
18154 vector<int> Util::nearestk_to_refdir(const vector<float>& projangles, const vector<float>& refangles, const int howmany) {
18155 int nref = refangles.size()/2;
18156 int nproj = projangles.size()/2;
18157 vector<int> asg(howmany*nref);
18158 vector<float> vecproj(nproj*3);
18159 for (int i=0; i<nproj; i++)
18160 getvec(projangles[i*2], projangles[i*2+1], vecproj[i*3], vecproj[i*3+1], vecproj[i*3+2]);
18161
18162
18163 vector<bool> taken(nproj);
18164 for (int k=0; k<nref; k++) {
18165 for (int i=0; i<nproj; i++) taken[i] = true;
18166 float x, y, z;
18167 getvec(refangles[k*2], refangles[k*2+1], x, y, z);
18168 for (int h=0; h<howmany; h++) {
18169 float best_v = -1.0f;
18170 int best_i = -1;
18171 for (int i=0; i<nproj; i++) {
18172 if( taken[i] ) {
18173 float v = abs(vecproj[i*3]*x+vecproj[i*3+1]*y+vecproj[i*3+2]*z);
18174 if (v > best_v) {
18175 best_v = v;
18176 best_i = i;
18177 }
18178 }
18179 }
18180 asg[k*howmany + h] = best_i;
18181 taken[best_i] = false;
18182 }
18183 }
18184 return asg;
18185 }
18186
18187
18188 vector<int> Util::group_proj_by_phitheta(const vector<float>& projangles, const vector<float>& ref_ang, const int img_per_grp) {
18189 float c = 100.0;
18190 int L = max(100, img_per_grp);
18191 int N = projangles.size()/2;
18192
18193 int sz = ref_ang.size();
18194 int nref1 = ref_ang[sz-4];
18195 int nref2 = ref_ang[sz-3];
18196 int nref3 = ref_ang[sz-2];
18197 int nref4 = ref_ang[sz-1];
18198 int nref;
18199
18200 set<int> pt;
18201 for (int i=0; i<N; i++) pt.insert(i);
18202 vector<float> v(N*3, 0.0f);
18203 for (int i=0; i<N; i++)
18204 getvec(projangles[i*2], projangles[i*2+1], v[i*3], v[i*3+1], v[i*3+2], 1);
18205
18206 int previous_group = -1;
18207 int previous_zone = 5;
18208 int max_group = 0;
18209 vector<float> ref_ang_list;
18210 vector<float> diff_table;
18211 map<int, int> diff_table_index;
18212 vector<int> proj_list;
18213 vector<int> sg;
18214 vector<int> remain_index;
18215 vector<int> asg;
18216 int mirror;
18217 for (int grp=0; grp<N/img_per_grp; grp++) {
18218 int N_remain = N-grp*img_per_grp;
18219 assert(N_remain == static_cast<int>(pt.size()));
18220 if (N_remain >= nref4*L) {
18221 if (previous_zone > 4) {
18222 ref_ang_list.resize(nref4*2);
18223 for (int i=0; i<nref4*2; i++) ref_ang_list[i] = ref_ang[(nref1+nref2+nref3)*2+i];
18224 nref = nref4;
18225 previous_group = -1;
18226 previous_zone = 4;
18227 }
18228 } else if (N_remain >= nref3*L) {
18229 if (previous_zone > 3) {
18230 ref_ang_list.resize(nref3*2);
18231 for (int i=0; i<nref3*2; i++) ref_ang_list[i] = ref_ang[(nref1+nref2)*2+i];
18232 nref = nref3;
18233 previous_group = -1;
18234 previous_zone = 3;
18235 }
18236 } else if (N_remain >= nref2*L) {
18237 if (previous_zone > 2) {
18238 ref_ang_list.resize(nref2*2);
18239 for (int i=0; i<nref2*2; i++) ref_ang_list[i] = ref_ang[nref1*2+i];
18240 nref = nref2;
18241 previous_group = -1;
18242 previous_zone = 2;
18243 }
18244 } else if (N_remain >= nref1*L) {
18245 if (previous_zone > 1) {
18246 ref_ang_list.resize(nref1*2);
18247 for (int i=0; i<nref1*2; i++) ref_ang_list[i] = ref_ang[i];
18248 nref = nref1;
18249 previous_group = -1;
18250 previous_zone = 1;
18251 }
18252 } else if (previous_zone > 0) {
18253 previous_group = -1;
18254 previous_zone = 0;
18255 }
18256
18257 vector<int> index;
18258 if (N_remain >= nref1*L) {
18259 if (previous_group == -1) {
18260 vector<float> proj_ang_list(N_remain*2, 0.0f);
18261 remain_index.resize(N_remain);
18262 int l = 0;
18263 for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si) {
18264 remain_index[l] = (*si);
18265 proj_ang_list[l*2] = projangles[(*si)*2];
18266 proj_ang_list[l*2+1] = projangles[(*si)*2+1];
18267 l++;
18268 }
18269 assert(N_remain == l);
18270 asg = assign_projangles(proj_ang_list, ref_ang_list);
18271 sg.resize(nref);
18272 for (int i=0; i<nref; i++) sg[i] = 0;
18273 for (int i=0; i<N_remain; i++) sg[asg[i]]++;
18274 }
18275 int max_group_size = 0;
18276 for (int i=0; i<nref; i++)
18277 if (sg[i] > max_group_size) { max_group_size = sg[i]; max_group = i; }
18278 for (unsigned int i=0; i<remain_index.size(); i++)
18279 if (asg[i] == max_group) index.push_back(remain_index[i]);
18280 } else {
18281 for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si)
18282 index.push_back(*si);
18283 max_group = 0;
18284 }
18285
18286 int Nn = index.size();
18287 if (max_group != previous_group) {
18288 diff_table.resize(Nn*Nn);
18289 diff_table_index.clear();
18290 for (int i=0; i<Nn-1; i++)
18291 for (int j=i+1; j<Nn; j++) {
18292 float diff = ang_diff(v[index[i]*3], v[index[i]*3+1], v[index[i]*3+2], v[index[j]*3], v[index[j]*3+1], v[index[j]*3+2], mirror);
18293 float q = exp(-c*pow(diff/180.0f*static_cast<float>(M_PI), 2.0f));
18294 diff_table[i*Nn+j] = q;
18295 diff_table[j*Nn+i] = q;
18296 }
18297 for (int i=0; i<Nn; i++) {
18298 diff_table[i*Nn+i] = 0.0f;
18299 diff_table_index[index[i]] = i;
18300 }
18301 previous_group = max_group;
18302 }
18303
18304 int diff_table_size = static_cast<int>(sqrt((float)diff_table.size())+0.5f);
18305 float max_density = -1;
18306 int max_density_i = -1;
18307 for (int i=0; i<Nn; i++) {
18308 float s = 0.0f;
18309 int z = diff_table_index[index[i]];
18310 for (int j=0; j<diff_table_size; j++) s += diff_table[z*diff_table_size+j];
18311 if (s > max_density) {
18312 max_density = s;
18313 max_density_i = i;
18314 }
18315 }
18316
18317 vector<d_ang> dang(Nn, d_ang(0.0, 0, 0));
18318 for (int i=0; i<Nn; i++) {
18319 dang[i].d = ang_diff(v[index[i]*3], v[index[i]*3+1], v[index[i]*3+2],
18320 v[index[max_density_i]*3], v[index[max_density_i]*3+1], v[index[max_density_i]*3+2], mirror);
18321 dang[i].mirror = mirror;
18322 dang[i].i = i;
18323 }
18324 dang[max_density_i].d = -1;
18325 sort(dang.begin(), dang.end());
18326
18327 for (int i=0; i<img_per_grp; i++) {
18328 int idd = index[dang[i].i];
18329 mirror = dang[i].mirror;
18330 for (unsigned int j=0; j<remain_index.size(); j++)
18331 if (idd == remain_index[j]) asg[j] = -1;
18332 for (int j=0; j<diff_table_size; j++) {
18333 diff_table[diff_table_index[idd]*diff_table_size+j] = 0.0f;
18334 diff_table[diff_table_index[idd]+diff_table_size*j] = 0.0f;
18335 }
18336 proj_list.push_back(mirror*idd);
18337 pt.erase(idd);
18338 }
18339 sg[max_group] -= img_per_grp;
18340 }
18341 for (set<int>::const_iterator si = pt.begin(); si != pt.end(); ++si) {
18342 proj_list.push_back(*si);
18343 }
18344 return proj_list;
18345 }
18346
18347 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
18348 float xrng, float yrng, float step, string mode,
18349 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
18350
18351
18352
18353
18354
18355
18356
18357
18358
18359
18360
18361
18362 size_t crefim_len = crefim.size();
18363
18364 int ky = int(2*yrng/step+0.5)/2;
18365 int kx = int(2*xrng/step+0.5)/2;
18366 int iref, nref=0, mirror=0;
18367 float iy, ix, sx=0, sy=0;
18368 float peak = -1.0E23f;
18369 float ang=0.0f;
18370 for (int i = -ky; i <= ky; i++) {
18371 iy = i * step ;
18372 for (int j = -kx; j <= kx; j++) {
18373 ix = j*step ;
18374 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18375
18376 Normalize_ring( cimage, numr );
18377
18378 Frngs(cimage, numr);
18379
18380
18381 for ( iref = 0; iref < (int)crefim_len; iref++) {
18382 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
18383 double qn = retvals["qn"];
18384 double qm = retvals["qm"];
18385 if(qn >= peak || qm >= peak) {
18386 sx = -ix;
18387 sy = -iy;
18388 nref = iref;
18389 if (qn >= qm) {
18390 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18391 peak = static_cast<float>(qn);
18392 mirror = 0;
18393 } else {
18394 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18395 peak = static_cast<float>(qm);
18396 mirror = 1;
18397 }
18398 }
18399 } delete cimage; cimage = 0;
18400 }
18401 }
18402 float co, so, sxs, sys;
18403 co = static_cast<float>( cos(ang*pi/180.0) );
18404 so = static_cast<float>( -sin(ang*pi/180.0) );
18405 sxs = sx*co - sy*so;
18406 sys = sx*so + sy*co;
18407 vector<float> res;
18408 res.push_back(ang);
18409 res.push_back(sxs);
18410 res.push_back(sys);
18411 res.push_back(static_cast<float>(mirror));
18412 res.push_back(static_cast<float>(nref));
18413 res.push_back(peak);
18414 return res;
18415 }
18416
18417 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
18418 float xrng, float yrng, float step, string mode,
18419 vector< int >numr, float cnx, float cny) {
18420
18421
18422
18423
18424
18425
18426
18427
18428
18429
18430
18431 size_t crefim_len = crefim.size();
18432
18433 int ky = int(2*yrng/step+0.5)/2;
18434 int kx = int(2*xrng/step+0.5)/2;
18435 int iref, nref=0;
18436 float iy, ix, sx=0, sy=0;
18437 float peak = -1.0E23f;
18438 float ang=0.0f;
18439 for (int i = -ky; i <= ky; i++) {
18440 iy = i * step ;
18441 for (int j = -kx; j <= kx; j++) {
18442 ix = j*step ;
18443 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18444 Frngs(cimage, numr);
18445
18446
18447 for ( iref = 0; iref < (int)crefim_len; iref++) {
18448 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
18449 double qn = retvals["qn"];
18450 if(qn >= peak) {
18451 sx = -ix;
18452 sy = -iy;
18453 nref = iref;
18454 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18455 peak = static_cast<float>(qn);
18456 }
18457 } delete cimage; cimage = 0;
18458 }
18459 }
18460 float co, so, sxs, sys;
18461 co = static_cast<float>( cos(ang*pi/180.0) );
18462 so = static_cast<float>( -sin(ang*pi/180.0) );
18463 sxs = sx*co - sy*so;
18464 sys = sx*so + sy*co;
18465 vector<float> res;
18466 res.push_back(ang);
18467 res.push_back(sxs);
18468 res.push_back(sys);
18469 res.push_back(static_cast<float>(nref));
18470 res.push_back(peak);
18471 return res;
18472 }
18473
18474 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
18475 float xrng, float yrng, float step, float ant, string mode,
18476 vector<int>numr, float cnx, float cny) {
18477
18478
18479
18480
18481
18482
18483
18484
18485
18486
18487
18488 size_t crefim_len = crefim.size();
18489 const float qv = static_cast<float>( pi/180.0 );
18490
18491 Transform * t = image->get_attr("xform.projection");
18492 Dict d = t->get_params("spider");
18493 if(t) {delete t; t=0;}
18494 float phi = d["phi"];
18495 float theta = d["theta"];
18496 int ky = int(2*yrng/step+0.5)/2;
18497 int kx = int(2*xrng/step+0.5)/2;
18498 int iref, nref=0, mirror=0;
18499 float iy, ix, sx=0, sy=0;
18500 float peak = -1.0E23f;
18501 float ang=0.0f;
18502 float imn1 = sin(theta*qv)*cos(phi*qv);
18503 float imn2 = sin(theta*qv)*sin(phi*qv);
18504 float imn3 = cos(theta*qv);
18505 vector<float> n1(crefim_len);
18506 vector<float> n2(crefim_len);
18507 vector<float> n3(crefim_len);
18508 for ( iref = 0; iref < (int)crefim_len; iref++) {
18509 n1[iref] = crefim[iref]->get_attr("n1");
18510 n2[iref] = crefim[iref]->get_attr("n2");
18511 n3[iref] = crefim[iref]->get_attr("n3");
18512 }
18513 for (int i = -ky; i <= ky; i++) {
18514 iy = i * step ;
18515 for (int j = -kx; j <= kx; j++) {
18516 ix = j*step;
18517 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18518
18519 Normalize_ring( cimage, numr );
18520
18521 Frngs(cimage, numr);
18522
18523
18524 for ( iref = 0; iref < (int)crefim_len; iref++) {
18525 if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18526 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18527 double qn = retvals["qn"];
18528 double qm = retvals["qm"];
18529 if(qn >= peak || qm >= peak) {
18530 sx = -ix;
18531 sy = -iy;
18532 nref = iref;
18533 if (qn >= qm) {
18534 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18535 peak = static_cast<float>( qn );
18536 mirror = 0;
18537 } else {
18538 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18539 peak = static_cast<float>( qm );
18540 mirror = 1;
18541 }
18542 }
18543 }
18544 } delete cimage; cimage = 0;
18545 }
18546 }
18547 float co, so, sxs, sys;
18548 if(peak == -1.0E23) {
18549 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18550 nref = -1;
18551 } else {
18552 co = cos(ang*qv);
18553 so = -sin(ang*qv);
18554 sxs = sx*co - sy*so;
18555 sys = sx*so + sy*co;
18556 }
18557 vector<float> res;
18558 res.push_back(ang);
18559 res.push_back(sxs);
18560 res.push_back(sys);
18561 res.push_back(static_cast<float>(mirror));
18562 res.push_back(static_cast<float>(nref));
18563 res.push_back(peak);
18564 return res;
18565 }
18566
18567 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18568 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18569 vector<int>numr, float cnx, float cny) {
18570
18571
18572
18573
18574
18575
18576
18577
18578
18579
18580
18581 size_t crefim_len = crefim.size();
18582 const float qv = static_cast<float>(pi/180.0);
18583
18584 Transform* t = image->get_attr("xform.projection");
18585 Dict d = t->get_params("spider");
18586 if(t) {delete t; t=0;}
18587 float phi = d["phi"];
18588 float theta = d["theta"];
18589 float psi = d["psi"];
18590 int ky = int(2*yrng/step+0.5)/2;
18591 int kx = int(2*xrng/step+0.5)/2;
18592 int iref, nref = 0, mirror = 0;
18593 float iy, ix, sx = 0, sy = 0;
18594 float peak = -1.0E23f;
18595 float ang = 0.0f;
18596 float imn1 = sin(theta*qv)*cos(phi*qv);
18597 float imn2 = sin(theta*qv)*sin(phi*qv);
18598 float imn3 = cos(theta*qv);
18599 vector<float> n1(crefim_len);
18600 vector<float> n2(crefim_len);
18601 vector<float> n3(crefim_len);
18602 for (iref = 0; iref < (int)crefim_len; iref++) {
18603 n1[iref] = crefim[iref]->get_attr("n1");
18604 n2[iref] = crefim[iref]->get_attr("n2");
18605 n3[iref] = crefim[iref]->get_attr("n3");
18606 }
18607 bool nomirror = (theta<90.0) || (theta==90.0);
18608 if (!nomirror) {
18609 phi = fmod(phi+540.0f, 360.0f);
18610 theta = 180-theta;
18611 psi = fmod(540.0f-psi, 360.0f);
18612 } else { psi = fmod(360.0f-psi, 360.0f); }
18613 for (int i = -ky; i <= ky; i++) {
18614 iy = i * step ;
18615 for (int j = -kx; j <= kx; j++) {
18616 ix = j*step;
18617 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18618
18619 Normalize_ring(cimage, numr);
18620
18621 Frngs(cimage, numr);
18622
18623
18624 for (iref = 0; iref < (int)crefim_len; iref++) {
18625 if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18626 float refpsi = crefim[iref]->get_attr("psi");
18627 if (nomirror) {
18628 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, fmod(360.0+psi+refpsi, 360.0), 0, psi_max);
18629 double qn = retvals["qn"];
18630 if (qn >= peak) {
18631 sx = -ix;
18632 sy = -iy;
18633 nref = iref;
18634 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18635 peak = static_cast<float>(qn);
18636 mirror = 0;
18637 }
18638 } else {
18639 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, fmod(360.0+psi-refpsi, 360.0), 1, psi_max);
18640 double qn = retvals["qn"];
18641 if (qn >= peak) {
18642 sx = -ix;
18643 sy = -iy;
18644 nref = iref;
18645 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18646 peak = static_cast<float>(qn);
18647 mirror = 1;
18648 }
18649 }
18650 }
18651 } delete cimage; cimage = 0;
18652 }
18653 }
18654 float co, so, sxs, sys;
18655 if(peak == -1.0E23) {
18656 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18657 nref = -1;
18658 } else {
18659 co = cos(ang*qv);
18660 so = -sin(ang*qv);
18661 sxs = sx*co - sy*so;
18662 sys = sx*so + sy*co;
18663 }
18664 vector<float> res;
18665 res.push_back(ang);
18666 res.push_back(sxs);
18667 res.push_back(sys);
18668 res.push_back(static_cast<float>(mirror));
18669 res.push_back(static_cast<float>(nref));
18670 res.push_back(peak);
18671 return res;
18672 }
18673
18674
18675 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18676 float xrng, float yrng, float step, float psi_max, string mode,
18677 vector<int>numr, float cnx, float cny, int ynumber) {
18678
18679 size_t crefim_len = crefim.size();
18680
18681 int iref, nref=0, mirror=0;
18682 float iy, ix, sx=0, sy=0;
18683 float peak = -1.0E23f;
18684 float ang=0.0f;
18685 int kx = int(2*xrng/step+0.5)/2;
18686
18687
18688
18689 int ky;
18690 float stepy;
18691 int kystart;
18692
18693 if (ynumber == -1){
18694 ky = int(2*yrng/step+0.5)/2;
18695 stepy = step;
18696 kystart = -ky;
18697 }
18698 else if(ynumber == 0){
18699 ky = 0;
18700 stepy = 0.0f;
18701 kystart = ky;
18702 }
18703 else {
18704 ky = int(ynumber/2);
18705 stepy=2*yrng/ynumber;
18706 kystart = -ky + 1;
18707 }
18708
18709 for (int i = kystart; i <= ky; i++) {
18710 iy = i * stepy ;
18711 for (int j = -kx; j <= kx; j++) {
18712 ix = j*step ;
18713 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18714
18715 Normalize_ring( cimage, numr );
18716
18717 Frngs(cimage, numr);
18718
18719
18720 for ( iref = 0; iref < (int)crefim_len; iref++) {
18721 Dict retvals_0 = Crosrng_psi(crefim[iref], cimage, numr, 0, psi_max);
18722 Dict retvals_180 = Crosrng_psi(crefim[iref], cimage, numr, 180, psi_max);
18723 double qn_0 = retvals_0["qn"];
18724 double qn_180 = retvals_180["qn"];
18725 double qm_0 = retvals_0["qm"];
18726 double qm_180 = retvals_180["qm"];
18727 double qn;
18728 double qm;
18729 bool qn_is_zero = false;
18730 bool qm_is_zero = false;
18731
18732 if (qn_0 >= qn_180){
18733 qn = qn_0;
18734 qn_is_zero = true;
18735 }
18736 else{
18737 qn = qn_180;
18738 qn_is_zero = false;
18739 }
18740
18741 if (qm_0 >= qm_180){
18742 qm = qm_0;
18743 qm_is_zero = true;
18744 }
18745 else{
18746 qm = qm_180;
18747 qm_is_zero = false;
18748 }
18749
18750 if(qn >= peak || qm >= peak) {
18751 sx = -ix;
18752 sy = -iy;
18753 nref = iref;
18754 if (qn >= qm) {
18755 if (qn_is_zero){
18756 ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
18757 }
18758 else{
18759 ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
18760 }
18761 peak = static_cast<float>(qn);
18762 mirror = 0;
18763 } else {
18764 if (qm_is_zero){
18765 ang = ang_n(retvals_0["tmt"], mode, numr[numr.size()-1]);
18766 }
18767 else{
18768 ang = ang_n(retvals_180["tmt"], mode, numr[numr.size()-1]);
18769 }
18770 peak = static_cast<float>(qm);
18771 mirror = 1;
18772 }
18773 }
18774 }
18775 delete cimage; cimage = 0;
18776 }
18777 }
18778 float co, so, sxs, sys;
18779 co = static_cast<float>( cos(ang*pi/180.0) );
18780 so = static_cast<float>( -sin(ang*pi/180.0) );
18781 sxs = sx*co - sy*so;
18782 sys = sx*so + sy*co;
18783 vector<float> res;
18784 res.push_back(ang);
18785 res.push_back(sxs);
18786 res.push_back(sys);
18787 res.push_back(static_cast<float>(mirror));
18788 res.push_back(static_cast<float>(nref));
18789 res.push_back(peak);
18790 return res;
18791 }
18792
18793 vector<float> Util::multiref_polar_ali_helical_local(EMData* image, const vector< EMData* >& crefim,
18794 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18795 vector<int>numr, float cnx, float cny, int ynumber, bool mirror_only, float yrnglocal, bool CONS) {
18796
18797 size_t crefim_len = crefim.size();
18798
18799 float phi_lhs=1000.0;
18800 float phi_rhs=1000.0;
18801 float y_lhs=1000.0;
18802 float y_rhs=1000.0;
18803
18804 if (CONS){
18805 phi_lhs = image->get_attr("phi_lhs");
18806 phi_rhs = image->get_attr("phi_rhs");
18807 y_lhs = image->get_attr("y_lhs");
18808 y_rhs = image->get_attr("y_rhs");
18809 }
18810 int iref, nref=-1, mirror=0;
18811 float iy, ix, sx=0, sy=0;
18812 float peak = -1.0E23f;
18813 float ang=0.0f;
18814 const float qv = static_cast<float>( pi/180.0 );
18815 Transform * t = image->get_attr("xform.projection");
18816 Dict d = t->get_params("spider");
18817 if(t) {delete t; t=0;}
18818 float phi = d["phi"];
18819 float phi_upper = phi+phi_rhs;
18820 float phi_lower = phi+phi_lhs;
18821 float theta = d["theta"];
18822 float psi = d["psi"];
18823 float imn1 = sin(theta*qv)*cos(phi*qv);
18824 float imn2 = sin(theta*qv)*sin(phi*qv);
18825 float imn3 = cos(theta*qv);
18826 vector<float> n1(crefim_len);
18827 vector<float> n2(crefim_len);
18828 vector<float> n3(crefim_len);
18829 vector<float> ref_phi(crefim_len);
18830 for ( iref = 0; iref < (int)crefim_len; iref++) {
18831 n1[iref] = crefim[iref]->get_attr("n1");
18832 n2[iref] = crefim[iref]->get_attr("n2");
18833 n3[iref] = crefim[iref]->get_attr("n3");
18834
18835 ref_phi[iref] = crefim[iref]->get_attr("phi");
18836 }
18837 float nbrinp;
18838 bool use_ref;
18839 int kx = int(2*xrng/step+0.5)/2;
18840 int ychoice = 0;
18841 int phichoice = 0;
18842
18843 if(ynumber==-1) {
18844 int ky = int(2*yrng/step+0.5)/2;
18845 for (int i = -ky; i <= ky; i++) {
18846 iy = i * step ;
18847 for (int j = -kx; j <= kx; j++) {
18848 ix = j*step ;
18849 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18850
18851 Normalize_ring( cimage, numr );
18852
18853 Frngs(cimage, numr);
18854
18855
18856 for ( iref = 0; iref < (int)crefim_len; iref++) {
18857
18858 use_ref = false;
18859 if (!mirror_only){
18860
18861 nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18862 if (nbrinp >= ant){
18863 use_ref = true;
18864 }
18865 }
18866 else if (mirror_only) {
18867
18868 nbrinp = (-1.0*n1[iref]*imn1) + (-1.0*n2[iref]*imn2) + n3[iref]*imn3;
18869 if (nbrinp >= ant){
18870 use_ref = true;
18871 }
18872 }
18873
18874
18875 if(use_ref) {
18876 Dict retvals;
18877 if (mirror_only == true){
18878 if ((psi-90) < 90)
18879 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
18880 else
18881 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max);
18882 }
18883 else{
18884 if ((psi-90) < 90)
18885 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18886 else
18887 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18888 }
18889 double qn = retvals["qn"];
18890
18891 if(qn >= peak) {
18892 sx = -ix;
18893 sy = -iy;
18894 nref = iref;
18895 if (!mirror_only) {
18896 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18897 peak = static_cast<float>(qn);
18898 mirror = 0;
18899 } else {
18900 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18901 peak = static_cast<float>(qn);
18902 mirror = 1;
18903 }
18904 }
18905 }
18906 }
18907 delete cimage; cimage = 0;
18908 }
18909 }
18910 }
18911
18912 else if(ynumber==0) {
18913 sy = 0.0f;
18914 for (int j = -kx; j <= kx; j++) {
18915 ix = j*step ;
18916 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18917
18918 Normalize_ring( cimage, numr );
18919
18920 Frngs(cimage, numr);
18921
18922
18923 for ( iref = 0; iref < (int)crefim_len; iref++) {
18924
18925 use_ref = false;
18926 if (!mirror_only){
18927
18928 nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
18929 if (nbrinp >= ant){
18930 use_ref = true;
18931 }
18932 }
18933 else if (mirror_only) {
18934
18935 nbrinp = (-1.0f*n1[iref]*imn1) + (-1.0f*n2[iref]*imn2) + n3[iref]*imn3;
18936 if (nbrinp >= ant){
18937 use_ref = true;
18938 }
18939 }
18940
18941 if(use_ref) {
18942 Dict retvals;
18943 if (mirror_only == true){
18944 if ((psi-90) < 90)
18945 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
18946 else
18947 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max);
18948 }
18949 else{
18950 if ((psi-90) < 90)
18951 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
18952 else
18953 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
18954 }
18955 double qn = retvals["qn"];
18956
18957 if(qn >= peak) {
18958 sx = -ix;
18959 sy = -iy;
18960 nref = iref;
18961 if (!mirror_only) {
18962 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18963 peak = static_cast<float>(qn);
18964 mirror = 0;
18965 } else {
18966 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18967 peak = static_cast<float>(qn);
18968 mirror = 1;
18969 }
18970 }
18971 }
18972 }
18973 delete cimage; cimage = 0;
18974 }
18975 } else {
18976 int ky = int(ynumber/2);
18977 float stepy=2*yrng/ynumber;
18978 int ky_rhs = ky;
18979 int ky_lhs = -ky + 1;
18980
18981
18982
18983
18984 if (CONS){
18985
18986 ky_rhs = floor((abs(y_lhs))/stepy);
18987 ky_lhs = -1.0*floor((abs(y_rhs))/stepy);
18988
18989 }
18990 else{
18991 if (yrnglocal >= 0.0){
18992 ky_rhs = int(yrnglocal/stepy);
18993 ky_lhs = -ky_rhs + 1;
18994 }
18995 }
18996
18997
18998
18999 for (int i = ky_lhs; i <= ky_rhs; i++) {
19000 iy = i * stepy ;
19001 ychoice = ychoice+1;
19002 for (int j = -kx; j <= kx; j++) {
19003 ix = j*step ;
19004 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19005
19006 Normalize_ring( cimage, numr );
19007
19008 Frngs(cimage, numr);
19009
19010
19011 for ( iref = 0; iref < (int)crefim_len; iref++) {
19012
19013 use_ref = false;
19014 if (!mirror_only){
19015
19016 nbrinp = n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3;
19017 if (CONS){
19018
19019 if ((ref_phi[iref] <= phi_upper) && (ref_phi[iref] >= phi_lower)){
19020 use_ref = true;
19021 }
19022 }
19023 else{
19024 if (nbrinp >= ant){
19025 use_ref = true;
19026 }
19027 }
19028 }
19029 else if (mirror_only) {
19030
19031 nbrinp = (-1.0*n1[iref]*imn1) + (-1.0*n2[iref]*imn2) + n3[iref]*imn3;
19032 if (CONS){
19033 if ((ref_phi[iref] + 180. <= phi_upper) && (ref_phi[iref]+180. >= phi_lower)){
19034 use_ref = true;
19035 }
19036 }
19037 else{
19038 if (nbrinp >= ant){
19039 use_ref = true;
19040 }
19041 }
19042
19043 }
19044 if(use_ref) {
19045 phichoice = phichoice + 1;
19046 Dict retvals;
19047 if (mirror_only == true){
19048 if ((psi-90) < 90)
19049 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 1, psi_max);
19050 else
19051 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 1, psi_max);
19052 }
19053 else{
19054 if ((psi-90) < 90)
19055 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
19056 else
19057 retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
19058 }
19059 double qn = retvals["qn"];
19060
19061 if(qn >= peak) {
19062 sx = -ix;
19063 sy = -iy;
19064 nref = iref;
19065 if (!mirror_only) {
19066 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19067 peak = static_cast<float>(qn);
19068 mirror = 0;
19069 } else {
19070 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19071 peak = static_cast<float>(qn);
19072 mirror = 1;
19073 }
19074 }
19075 }
19076 }
19077 delete cimage; cimage = 0;
19078 }
19079 }
19080 }
19081 if ((phichoice < 1) || (ychoice < 1))
19082 cout<<ychoice<<", "<<phichoice<<", ..."<<mirror_only<<"...,"<<phi<<","<<phi_lhs<<","<<phi_rhs<<endl;
19083
19084 float co, so, sxs, sys;
19085 co = static_cast<float>( cos(ang*pi/180.0) );
19086 so = static_cast<float>( -sin(ang*pi/180.0) );
19087 sxs = sx*co - sy*so;
19088 sys = sx*so + sy*co;
19089 vector<float> res;
19090 res.push_back(ang);
19091 res.push_back(sxs);
19092 res.push_back(sys);
19093 res.push_back(static_cast<float>(mirror));
19094 res.push_back(static_cast<float>(nref));
19095 res.push_back(peak);
19096 return res;
19097 }
19098
19099
19100 vector<float> Util::multiref_polar_ali_helical_90(EMData* image, const vector< EMData* >& crefim,
19101 float xrng, float yrng, float step, float psi_max, string mode,
19102 vector<int>numr, float cnx, float cny, int ynumber) {
19103
19104 size_t crefim_len = crefim.size();
19105
19106 int iref, nref=0, mirror=0;
19107 float iy, ix, sx=0, sy=0;
19108 float peak = -1.0E23f;
19109 float ang=0.0f;
19110 int kx = int(2*xrng/step+0.5)/2;
19111
19112
19113 int ky;
19114 float stepy;
19115 int kystart;
19116
19117 if (ynumber == -1){
19118 ky = int(2*yrng/step+0.5)/2;
19119 stepy = step;
19120 kystart = -ky;
19121 }
19122 else if(ynumber == 0){
19123 ky = 0;
19124 stepy = 0.0f;
19125 kystart = ky;
19126 }
19127 else {
19128 ky = int(ynumber/2);
19129 stepy=2*yrng/ynumber;
19130 kystart = -ky + 1;
19131 }
19132
19133
19134 for (int i = kystart; i <= ky; i++) {
19135 iy = i * stepy ;
19136 for (int j = -kx; j <= kx; j++) {
19137 ix = j*step ;
19138 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19139
19140 Normalize_ring( cimage, numr );
19141
19142 Frngs(cimage, numr);
19143
19144
19145 for ( iref = 0; iref < (int)crefim_len; iref++) {
19146 Dict retvals_0 = Crosrng_sm_psi(crefim[iref], cimage, numr, 0, 0, psi_max);
19147 Dict retvals_180 = Crosrng_sm_psi(crefim[iref], cimage, numr, 180, 0, psi_max);
19148 double qn_0 = retvals_0["qn"];
19149 double qn_180 = retvals_180["qn"];
19150 double qn;
19151 bool qn_is_zero = false;
19152
19153 if (qn_0 >= qn_180){
19154 qn = qn_0;
19155 qn_is_zero = true;
19156 }
19157 else{
19158 qn = qn_180;
19159 qn_is_zero = false;
19160 }
19161
19162 if(qn >= peak) {
19163 sx = -ix;
19164 sy = -iy;
19165 nref = iref;
19166
19167 if (qn_is_zero){
19168 ang = ang_n(retvals_0["tot"], mode, numr[numr.size()-1]);
19169 }
19170 else{
19171 ang = ang_n(retvals_180["tot"], mode, numr[numr.size()-1]);
19172 }
19173 peak = static_cast<float>(qn);
19174 mirror = 0;
19175
19176 }
19177 }
19178 delete cimage; cimage = 0;
19179 }
19180 }
19181 float co, so, sxs, sys;
19182 co = static_cast<float>( cos(ang*pi/180.0) );
19183 so = static_cast<float>( -sin(ang*pi/180.0) );
19184 sxs = sx*co - sy*so;
19185 sys = sx*so + sy*co;
19186 vector<float> res;
19187 res.push_back(ang);
19188 res.push_back(sxs);
19189 res.push_back(sys);
19190 res.push_back(static_cast<float>(mirror));
19191 res.push_back(static_cast<float>(nref));
19192 res.push_back(peak);
19193 return res;
19194 }
19195
19196
19197 vector<float> Util::multiref_polar_ali_helical_90_local(EMData* image, const vector< EMData* >& crefim,
19198 float xrng, float yrng, float step, float ant, float psi_max, string mode,
19199 vector<int>numr, float cnx, float cny, int ynumber) {
19200
19201 size_t crefim_len = crefim.size();
19202 const float qv = static_cast<float>( pi/180.0 );
19203 Transform * t = image->get_attr("xform.projection");
19204 Dict d = t->get_params("spider");
19205 if(t) {delete t; t=0;}
19206 float phi = d["phi"];
19207 float theta = d["theta"];
19208 float imn1 = sin(theta*qv)*cos(phi*qv);
19209 float imn2 = sin(theta*qv)*sin(phi*qv);
19210 float imn3 = cos(theta*qv);
19211 vector<float> n1(crefim_len);
19212 vector<float> n2(crefim_len);
19213 vector<float> n3(crefim_len);
19214 int iref, nref=-1, mirror=0;
19215 float iy, ix, sx=0, sy=0;
19216 float peak = -1.0E23f;
19217 float ang=0.0f;
19218 int kx = int(2*xrng/step+0.5)/2;
19219
19220 for ( iref = 0; iref < (int)crefim_len; iref++) {
19221 n1[iref] = crefim[iref]->get_attr("n1");
19222 n2[iref] = crefim[iref]->get_attr("n2");
19223 n3[iref] = crefim[iref]->get_attr("n3");
19224 }
19225
19226
19227 if(ynumber==-1) {
19228 int ky = int(2*yrng/step+0.5)/2;
19229 for (int i = -ky; i <= ky; i++) {
19230 iy = i * step ;
19231 for (int j = -kx; j <= kx; j++) {
19232 ix = j*step ;
19233 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19234
19235 Normalize_ring( cimage, numr );
19236
19237 Frngs(cimage, numr);
19238
19239
19240 for ( iref = 0; iref < (int)crefim_len; iref++) {
19241 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
19242 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
19243 double qn = retvals["qn"];
19244 if( qn >= peak) {
19245 sx = -ix;
19246 sy = -iy;
19247 nref = iref;
19248 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19249 peak = static_cast<float>(qn);
19250 mirror = 0;
19251 }
19252 }
19253 }
19254 delete cimage; cimage = 0;
19255 }
19256 }
19257 }
19258
19259 else if(ynumber==0) {
19260 sy = 0.0f;
19261 for (int j = -kx; j <= kx; j++) {
19262 ix = j*step ;
19263 iy = 0.0f ;
19264 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
19265
19266 Normalize_ring( cimage, numr );
19267
19268 Frngs(cimage, numr);
19269
19270
19271 for ( iref = 0; iref < (int)crefim_len; iref++) {
19272 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
19273 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
19274 double qn = retvals["qn"];
19275 if( qn >= peak ) {
19276 sx = -ix;
19277 nref = iref;
19278 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19279 peak = static_cast<float>(qn);
19280 mirror = 0;
19281 }
19282 }
19283 }
19284 delete cimage; cimage = 0;
19285 }
19286 } else {
19287 int ky = int(ynumber/2);
19288 float stepy=2*yrng/ynumber;
19289
19290 for (int i = -ky+1; i <= ky; i++) {
19291 iy = i * stepy ;
19292 for (int j = -kx; j <= kx; j++) {
19293 ix = j*step ;
19294 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19295
19296 Normalize_ring( cimage, numr );
19297
19298 Frngs(cimage, numr);
19299
19300
19301 for ( iref = 0; iref < (int)crefim_len; iref++) {
19302 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
19303 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
19304 double qn = retvals["qn"];
19305 if( qn >= peak) {
19306 sx = -ix;
19307 sy = -iy;
19308 nref = iref;
19309 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
19310 peak = static_cast<float>(qn);
19311 mirror = 0;
19312 }
19313 }
19314 }
19315 delete cimage; cimage = 0;
19316 }
19317 }
19318 }
19319 float co, so, sxs, sys;
19320 co = static_cast<float>( cos(ang*pi/180.0) );
19321 so = static_cast<float>( -sin(ang*pi/180.0) );
19322 sxs = sx*co - sy*so;
19323 sys = sx*so + sy*co;
19324 vector<float> res;
19325 res.push_back(ang);
19326 res.push_back(sxs);
19327 res.push_back(sys);
19328 res.push_back(static_cast<float>(mirror));
19329 res.push_back(static_cast<float>(nref));
19330 res.push_back(peak);
19331 return res;
19332 }
19333
19334
19335 void Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
19336 float xrng, float yrng, float step, string mode,
19337 vector< int >numr, float cnx, float cny,
19338 EMData *peaks, EMData *peakm) {
19339
19340 int maxrin = numr[numr.size()-1];
19341
19342 int ky = int(2*yrng/step+0.5)/2;
19343 int kx = int(2*xrng/step+0.5)/2;
19344
19345 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19346 float *p_ccf1ds = peaks->get_data();
19347
19348 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19349 float *p_ccf1dm = peakm->get_data();
19350
19351 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19352 p_ccf1ds[i] = -1.e20f;
19353 p_ccf1dm[i] = -1.e20f;
19354 }
19355
19356 for (int i = -ky; i <= ky; i++) {
19357 float iy = i * step;
19358 for (int j = -kx; j <= kx; j++) {
19359 float ix = j*step;
19360 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19361 Frngs(cimage, numr);
19362 Crosrng_msg_vec(crefim, cimage, numr,
19363 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19364 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19365 delete cimage; cimage = 0;
19366 }
19367 }
19368 return;
19369 }
19370
19371 void Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
19372 float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
19373 EMData *peaks_compress, EMData *peakm_compress) {
19374
19375 int maxrin = numr[numr.size()-1];
19376
19377 int ky = int(2*yrng/step+0.5)/2;
19378 int kx = int(2*xrng/step+0.5)/2;
19379
19380 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19381 float *p_ccf1ds = peaks->get_data();
19382
19383 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19384 float *p_ccf1dm = peakm->get_data();
19385
19386 peaks_compress->set_size(maxrin, 1, 1);
19387 float *p_ccf1ds_compress = peaks_compress->get_data();
19388
19389 peakm_compress->set_size(maxrin, 1, 1);
19390 float *p_ccf1dm_compress = peakm_compress->get_data();
19391
19392 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19393 p_ccf1ds[i] = -1.e20f;
19394 p_ccf1dm[i] = -1.e20f;
19395 }
19396
19397 for (int i = -ky; i <= ky; i++) {
19398 float iy = i * step;
19399 for (int j = -kx; j <= kx; j++) {
19400 float ix = j*step;
19401 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19402 Frngs(cimage, numr);
19403 Crosrng_msg_vec(crefim, cimage, numr,
19404 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19405 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19406 delete cimage; cimage = 0;
19407 }
19408 }
19409 for (int x=0; x<maxrin; x++) {
19410 float maxs = -1.0e22f;
19411 float maxm = -1.0e22f;
19412 for (int i=1; i<=2*ky+1; i++) {
19413 for (int j=1; j<=2*kx+1; j++) {
19414 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
19415 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
19416 }
19417 }
19418 p_ccf1ds_compress[x] = maxs;
19419 p_ccf1dm_compress[x] = maxm;
19420 }
19421 return;
19422 }
19423
19424 struct ccf_point
19425 {
19426 float value;
19427 int i;
19428 int j;
19429 int k;
19430 int mirror;
19431 };
19432
19433
19434 struct ccf_value
19435 {
19436 bool operator()( const ccf_point& a, const ccf_point& b )
19437 {
19438 return a.value > b.value;
19439 }
19440 };
19441
19442
19443 vector<float> Util::ali2d_ccf_list(EMData* image, EMData* crefim,
19444 float xrng, float yrng, float step, string mode,
19445 vector< int >numr, float cnx, float cny, double T) {
19446
19447 int maxrin = numr[numr.size()-1];
19448
19449 int ky = int(2*yrng/step+0.5)/2;
19450 int kx = int(2*xrng/step+0.5)/2;
19451
19452 float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
19453 float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
19454 int vol = maxrin*(2*kx+1)*(2*ky+1);
19455 vector<ccf_point> ccf(2*vol);
19456 ccf_point temp;
19457
19458 int index = 0;
19459 for (int i = -ky; i <= ky; i++) {
19460 float iy = i * step;
19461 for (int j = -kx; j <= kx; j++) {
19462 float ix = j*step;
19463 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19464 Frngs(cimage, numr);
19465 Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
19466 for (int k=0; k<maxrin; k++) {
19467 temp.value = p_ccf1ds[k];
19468 temp.i = k;
19469 temp.j = j;
19470 temp.k = i;
19471 temp.mirror = 0;
19472 ccf[index] = temp;
19473 index++;
19474 temp.value = p_ccf1dm[k];
19475 temp.mirror = 1;
19476 ccf[index] = temp;
19477 index++;
19478 }
19479 delete cimage; cimage = 0;
19480 }
19481 }
19482
19483 delete p_ccf1ds;
19484 delete p_ccf1dm;
19485 std::sort(ccf.begin(), ccf.end(), ccf_value());
19486
19487 double qt = (double)ccf[0].value;
19488 vector <double> p(2*vol), cp(2*vol);
19489
19490 double sump = 0.0;
19491 for (int i=0; i<2*vol; i++) {
19492 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
19493 sump += p[i];
19494 }
19495 for (int i=0; i<2*vol; i++) {
19496 p[i] /= sump;
19497 }
19498 for (int i=1; i<2*vol; i++) {
19499 p[i] += p[i-1];
19500 }
19501 p[2*vol-1] = 2.0;
19502
19503 float t = get_frand(0.0f, 1.0f);
19504 int select = 0;
19505 while (p[select] < t) select += 1;
19506
19507 vector<float> a(6);
19508 a[0] = ccf[select].value;
19509 a[1] = (float)ccf[select].i;
19510 a[2] = (float)ccf[select].j;
19511 a[3] = (float)ccf[select].k;
19512 a[4] = (float)ccf[select].mirror;
19513 a[5] = (float)select;
19514 return a;
19515 }
19516
19517
19518
19519
19520
19521
19522
19523
19524
19525
19526
19527
19528
19529
19530
19531
19532
19533
19534
19535
19536
19537
19538
19539
19540
19541
19542
19543
19544
19545
19546
19547
19548
19549
19550
19551
19552
19553
19554
19555
19556
19557
19558
19559
19560
19561
19562
19563
19564
19565
19566
19567
19568
19569
19570
19571
19572
19573
19574
19575
19576
19577
19578
19579
19580
19581
19582
19583
19584 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19585
19586 EMData *rot;
19587
19588 const int nmax=3, mmax=3;
19589 char task[60], csave[60];
19590 long int lsave[4];
19591 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19592 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];
19593 long int SIXTY=60;
19594
19595
19596 iprint = -1;
19597
19598
19599 factr=1.0e1;
19600 pgtol=1.0e-5;
19601
19602
19603
19604
19605 n=3;
19606 m=3;
19607
19608
19609
19610
19611
19612 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19613 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19614 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19615
19616
19617
19618
19619 strcpy(task,"START");
19620 for (int i=5;i<60;i++) task[i]=' ';
19621
19622
19623
19624 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19625
19626
19627
19628 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19629
19630 if (strncmp(task,"FG",2)==0) {
19631
19632
19633
19634
19635 rot = new EMData();
19636 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
19637 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19638
19639 delete rot;
19640
19641
19642 float dt = 1.0e-3f;
19643 rot = new EMData();
19644 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
19645 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19646
19647 g[0] = (f1-f)/dt;
19648 delete rot;
19649
19650 dt = 1.0e-2f;
19651 rot = new EMData();
19652 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
19653 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19654
19655 g[1] = (f2-f)/dt;
19656 delete rot;
19657
19658 rot = new EMData();
19659 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
19660 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19661
19662 g[2] = (f3-f)/dt;
19663 delete rot;
19664 }
19665
19666
19667 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19668
19669 }
19670
19671
19672 vector<float> res;
19673 res.push_back(static_cast<float>(x[0]));
19674 res.push_back(static_cast<float>(x[1]));
19675 res.push_back(static_cast<float>(x[2]));
19676
19677 return res;
19678 }
19679
19680 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19681
19682 EMData *rot;
19683
19684 const int nmax=3, mmax=3;
19685 char task[60], csave[60];
19686 long int lsave[4];
19687 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19688 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];
19689 long int SIXTY=60;
19690
19691
19692 iprint = -1;
19693
19694
19695 factr=1.0e1;
19696 pgtol=1.0e-5;
19697
19698
19699
19700
19701 n=3;
19702 m=3;
19703
19704
19705
19706
19707
19708 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19709 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19710 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19711
19712
19713
19714
19715 strcpy(task,"START");
19716 for (int i=5;i<60;i++) task[i]=' ';
19717
19718
19719
19720 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19721
19722
19723
19724 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19725
19726 if (strncmp(task,"FG",2)==0) {
19727
19728
19729
19730
19731 rot = new EMData();
19732 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19733 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19734
19735 delete rot;
19736
19737
19738 float dt = 1.0e-3f;
19739 rot = new EMData();
19740 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19741 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19742
19743 g[0] = (f1-f)/dt;
19744 delete rot;
19745
19746 rot = new EMData();
19747 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
19748 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19749
19750 g[1] = (f2-f)/dt;
19751 delete rot;
19752
19753 rot = new EMData();
19754 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
19755 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19756
19757 g[2] = (f3-f)/dt;
19758 delete rot;
19759 }
19760
19761
19762 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19763
19764 }
19765
19766
19767 vector<float> res;
19768 res.push_back(static_cast<float>(x[0]));
19769 res.push_back(static_cast<float>(x[1]));
19770 res.push_back(static_cast<float>(x[2]));
19771
19772 return res;
19773 }
19774
19775 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) {
19776
19777 EMData *proj, *proj2;
19778
19779 const int nmax=5, mmax=5;
19780 char task[60], csave[60];
19781 long int lsave[4];
19782 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19783 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];
19784 long int SIXTY=60;
19785
19786
19787 iprint = -1;
19788
19789
19790 factr=1.0e1;
19791 pgtol=1.0e-5;
19792
19793
19794
19795
19796 n=5;
19797 m=5;
19798
19799
19800
19801
19802
19803 x[0] = phi; nbd[0] = 2; l[0] = phi-2.0; u[0] = phi+2.0;
19804 x[1] = theta; nbd[1] = 2; l[1] = theta-2.0; u[1] = theta+2.0;
19805 x[2] = psi; nbd[2] = 2; l[2] = psi-2.0; u[2] = psi+2.0;
19806 x[3] = sxs; nbd[3] = 2; l[3] = sxs-2.0; u[3] = sxs+2.0;
19807 x[4] = sys; nbd[4] = 2; l[4] = sys-2.0; u[4] = sys+2.0;
19808
19809
19810
19811
19812 strcpy(task,"START");
19813 for (int i=5;i<60;i++) task[i]=' ';
19814
19815
19816
19817 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19818 int step = 1;
19819
19820
19821 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19822
19823 if (strncmp(task,"FG",2)==0) {
19824
19825
19826
19827
19828 proj = new EMData();
19829 proj2 = new EMData();
19830 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19831 proj->fft_shuffle();
19832 proj->center_origin_fft();
19833 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19834 proj->do_ift_inplace();
19835 int M = proj->get_ysize()/2;
19836 proj2 = proj->window_center(M);
19837 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19838
19839 delete proj;
19840 delete proj2;
19841
19842
19843 float dt = 1.0e-3f;
19844 proj = new EMData();
19845 proj2 = new EMData();
19846 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
19847 proj->fft_shuffle();
19848 proj->center_origin_fft();
19849 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19850 proj->do_ift_inplace();
19851 proj2 = proj->window_center(M);
19852 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19853
19854 delete proj;
19855 delete proj2;
19856 g[0] = (ft-f)/dt;
19857
19858 proj = new EMData();
19859 proj2 = new EMData();
19860 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
19861 proj->fft_shuffle();
19862 proj->center_origin_fft();
19863 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19864 proj->do_ift_inplace();
19865 proj2 = proj->window_center(M);
19866 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19867
19868 delete proj;
19869 delete proj2;
19870 g[1] = (ft-f)/dt;
19871
19872 proj = new EMData();
19873 proj2 = new EMData();
19874 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
19875 proj->fft_shuffle();
19876 proj->center_origin_fft();
19877 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19878 proj->do_ift_inplace();
19879 proj2 = proj->window_center(M);
19880 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19881
19882 delete proj;
19883 delete proj2;
19884 g[2] = (ft-f)/dt;
19885
19886 proj = new EMData();
19887 proj2 = new EMData();
19888 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19889 proj->fft_shuffle();
19890 proj->center_origin_fft();
19891 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
19892 proj->do_ift_inplace();
19893 proj2 = proj->window_center(M);
19894 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19895
19896 delete proj;
19897 delete proj2;
19898 g[3] = (ft-f)/dt;
19899
19900 proj = new EMData();
19901 proj2 = new EMData();
19902 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19903 proj->fft_shuffle();
19904 proj->center_origin_fft();
19905 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
19906 proj->do_ift_inplace();
19907 proj2 = proj->window_center(M);
19908 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19909
19910 delete proj;
19911 delete proj2;
19912 g[4] = (ft-f)/dt;
19913 }
19914
19915
19916 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19917 step++;
19918 }
19919
19920
19921 vector<float> res;
19922 res.push_back(static_cast<float>(x[0]));
19923 res.push_back(static_cast<float>(x[1]));
19924 res.push_back(static_cast<float>(x[2]));
19925 res.push_back(static_cast<float>(x[3]));
19926 res.push_back(static_cast<float>(x[4]));
19927
19928 return res;
19929 }
19930
19931
19932 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19933
19934 double x[4];
19935 int n;
19936 int l = 3;
19937 int m = 200;
19938 double e = 1e-9;
19939 double step = 0.01;
19940 float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
19941
19942 x[1] = ang;
19943 x[2] = sxs;
19944 x[3] = sys;
19945
19946 Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);
19947
19948
19949 vector<float> res;
19950 res.push_back(static_cast<float>(x[1]));
19951 res.push_back(static_cast<float>(x[2]));
19952 res.push_back(static_cast<float>(x[3]));
19953 res.push_back(static_cast<float>(n));
19954 return res;
19955 }
19956
19957 vector<float> Util::multi_align_error(vector<float> args, vector<float> all_ali_params, int d) {
19958
19959 const int nmax=args.size(), mmax=nmax;
19960 char task[60], csave[60];
19961 long int lsave[4];
19962 long int n, m, iprint, isave[44];
19963 long int* nbd = new long int[nmax];
19964 long int* iwa = new long int[3*nmax];
19965 double f, factr, pgtol;
19966 double* x = new double[nmax];
19967 double* l = new double[nmax];
19968 double* u = new double[nmax];
19969 double* g = new double[nmax];
19970 double dsave[29];
19971 double* wa = new double[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
19972 long int SIXTY=60;
19973
19974 int num_ali = nmax/3+1;
19975 int nima = all_ali_params.size()/(num_ali*4);
19976
19977
19978 iprint = -1;
19979
19980
19981 factr=1.0e1;
19982 pgtol=1.0e-9;
19983
19984
19985
19986
19987 n=nmax;
19988 m=mmax;
19989
19990
19991
19992
19993
19994 for (int i=0; i<nmax; i++) {
19995 x[i] = args[i];
19996 nbd[i] = 0;
19997 }
19998
19999
20000
20001 strcpy(task,"START");
20002 for (int i=5;i<60;i++) task[i]=' ';
20003
20004
20005
20006 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
20007 int step = 1;
20008
20009
20010 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
20011
20012 if (strncmp(task,"FG",2)==0) {
20013
20014
20015
20016
20017 f = multi_align_error_func(x, all_ali_params, nima, num_ali, d);
20018
20019
20020 multi_align_error_dfunc(x, all_ali_params, nima, num_ali, g, d);
20021
20022 }
20023
20024 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
20025 step++;
20026 }
20027
20028
20029 vector<float> res;
20030 for (int i=0; i<nmax; i++) res.push_back(static_cast<float>(x[i]));
20031 res.push_back(static_cast<float>(f));
20032
20033 delete[] nbd;
20034 delete[] iwa;
20035 delete[] x;
20036 delete[] l;
20037 delete[] u;
20038 delete[] g;
20039 delete[] wa;
20040
20041 return res;
20042
20043 }
20044
20045 double Util::multi_align_error_func(double* x, vector<float> all_ali_params, int nima, int num_ali, int d) {
20046
20047 vector<double> sqr_pixel_error = multi_align_error_func2(x, all_ali_params, nima, num_ali, d);
20048 double sum_sqr_pixel_error = 0.0;
20049 for (int i=0; i<nima; i++) sum_sqr_pixel_error += sqr_pixel_error[i];
20050 return sum_sqr_pixel_error/static_cast<float>(nima);
20051 }
20052
20053
20054 vector<double> Util::multi_align_error_func2(double* x, vector<float> ali_params, int nima, int num_ali, int d) {
20055
20056 double* args = new double[num_ali*3];
20057 for (int i=0; i<3*num_ali-3; i++) args[i] = x[i];
20058 args[3*num_ali-3] = 0.0;
20059 args[3*num_ali-2] = 0.0;
20060 args[3*num_ali-1] = 0.0;
20061 double* cosa = new double[num_ali];
20062 double* sina = new double[num_ali];
20063 for (int i=0; i<num_ali; i++) {
20064 cosa[i] = cos(args[i*3]*M_PI/180.0);
20065 sina[i] = sin(args[i*3]*M_PI/180.0);
20066 }
20067 double* sx = new double[num_ali];
20068 double* sy = new double[num_ali];
20069
20070 vector<double> sqr_pixel_error(nima);
20071
20072 for (int i=0; i<nima; i++) {
20073 double sum_cosa = 0.0;
20074 double sum_sina = 0.0;
20075 for (int j=0; j<num_ali; j++) {
20076 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
20077 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20078 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20079 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];
20080 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];
20081 } else {
20082 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20083 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20084 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];
20085 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];
20086 }
20087 }
20088 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
20089 sum_cosa /= P;
20090 sum_sina /= P;
20091 sqr_pixel_error[i] = d*d/4.0*(1.0-P/num_ali)+var(sx, num_ali)+var(sy, num_ali);
20092 }
20093
20094 delete[] args;
20095 delete[] cosa;
20096 delete[] sina;
20097 delete[] sx;
20098 delete[] sy;
20099
20100 return sqr_pixel_error;
20101 }
20102
20103 void Util::multi_align_error_dfunc(double* x, vector<float> ali_params, int nima, int num_ali, double* g, int d) {
20104
20105 for (int i=0; i<num_ali*3-3; i++) g[i] = 0.0;
20106
20107 double* args = new double[num_ali*3];
20108 for (int i=0; i<3*num_ali-3; i++) args[i] = x[i];
20109 args[3*num_ali-3] = 0.0;
20110 args[3*num_ali-2] = 0.0;
20111 args[3*num_ali-1] = 0.0;
20112 double* cosa = new double[num_ali];
20113 double* sina = new double[num_ali];
20114 for (int i=0; i<num_ali; i++) {
20115 cosa[i] = cos(args[i*3]*M_PI/180.0);
20116 sina[i] = sin(args[i*3]*M_PI/180.0);
20117 }
20118 double* sx = new double[num_ali];
20119 double* sy = new double[num_ali];
20120
20121 vector<float> sqr_pixel_error(nima);
20122
20123 for (int i=0; i<nima; i++) {
20124 double sum_cosa = 0.0;
20125 double sum_sina = 0.0;
20126 for (int j=0; j<num_ali; j++) {
20127 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
20128 sum_cosa += cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20129 sum_sina += sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20130 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];
20131 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];
20132 } else {
20133 sum_cosa += cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20134 sum_sina += sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0);
20135 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];
20136 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];
20137 }
20138 }
20139 double P = sqrt(sum_cosa*sum_cosa+sum_sina*sum_sina);
20140 sum_cosa /= P;
20141 sum_sina /= P;
20142 for (int j=0; j<num_ali-1; j++) {
20143 double dx = 2.0*(sx[j]-mean(sx, num_ali));
20144 double dy = 2.0*(sy[j]-mean(sy, num_ali));
20145 if (static_cast<int>(ali_params[j*nima*4+i*4+3]) == 0) {
20146 g[j*3] += (d*d/4.0*(sum_cosa*sin((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) -
20147 sum_sina*cos((args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
20148 dx*(-ali_params[j*nima*4+i*4+1]*sina[j]-ali_params[j*nima*4+i*4+2]*cosa[j])+
20149 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;
20150 g[j*3+1] += dx;
20151 g[j*3+2] += dy;
20152 } else {
20153 g[j*3] += (d*d/4.0*(-sum_cosa*sin((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0) +
20154 sum_sina*cos((-args[j*3]+ali_params[j*nima*4+i*4])*M_PI/180.0)) +
20155 dx*(-ali_params[j*nima*4+i*4+1]*sina[j]+ali_params[j*nima*4+i*4+2]*cosa[j])+
20156 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;
20157 g[j*3+1] += -dx;
20158 g[j*3+2] += dy;
20159 }
20160 }
20161 }
20162
20163 for (int i=0; i<3*num_ali-3; i++) g[i] /= (num_ali*nima);
20164
20165 delete[] args;
20166 delete[] cosa;
20167 delete[] sina;
20168 delete[] sx;
20169 delete[] sy;
20170 }
20171
20172 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
20173
20174 EMData *rot= new EMData();
20175 float ccc;
20176
20177 rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
20178 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
20179 delete rot;
20180 return ccc;
20181 }
20182
20183 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
20184
20185 double x[4];
20186 int n;
20187 int l = 3;
20188 int m = 200;
20189 double e = 1e-9;
20190 double step = 0.001;
20191 float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
20192
20193 x[1] = ang;
20194 x[2] = sxs;
20195 x[3] = sys;
20196
20197 Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);
20198
20199
20200 vector<float> res;
20201 res.push_back(static_cast<float>(x[1]));
20202 res.push_back(static_cast<float>(x[2]));
20203 res.push_back(static_cast<float>(x[3]));
20204 res.push_back(static_cast<float>(n));
20205 return res;
20206 }
20207
20208
20209 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
20210
20211 EMData *rot= new EMData();
20212 float ccc;
20213
20214 rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
20215 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
20216 delete rot;
20217 return ccc;
20218 }
20219
20220 #define img_ptr(i,j,k) img_ptr[i+(j+(k*ny))*(size_t)nx]
20221 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*(size_t)nx]
20222 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
20223 {
20224 ENTERFUNC;
20225
20226 if (!img) {
20227 throw NullPointerException("NULL input image");
20228 }
20229
20230 int newx, newy, newz;
20231 bool keep_going;
20232 cout << " entered " <<endl;
20233 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
20234
20235 EMData * img2 = new EMData();
20236 img2->set_size(nx,ny,nz);
20237 img2->to_zero();
20238 float *img_ptr =img->get_data();
20239 float *img2_ptr = img2->get_data();
20240 int r2 = ro*ro;
20241 int r3 = r2*ro;
20242 int ri2 = ri*ri;
20243 int ri3 = ri2*ri;
20244
20245 int n2 = nx/2;
20246
20247 for (int k=-n2; k<=n2; k++) {
20248 float z2 = static_cast<float>(k*k);
20249 for (int j=-n2; j<=n2; j++) {
20250 float y2 = z2 + j*j;
20251 if(y2 <= r2) {
20252
20253
20254 for (int i=-n2; i<=n2; i++) {
20255 float x2 = y2 + i*i;
20256 if(x2 <= r3) {
20257
20258 int ib = i+n2; int jb = j+n2; int kb = k+n2;
20259 if(x2 >= ri3) {
20260
20261 if(img_ptr(ib,jb,kb) == 1.0f) {
20262
20263 if(Util::get_frand(0.0f, 1.0f) > qprob){
20264 img2_ptr(ib,jb,kb) = 0.0f;
20265 keep_going = true;
20266
20267 while(keep_going) {
20268 newx = Util::get_irand(-ro,ro);
20269 newy = Util::get_irand(-ro,ro);
20270 newz = Util::get_irand(-ro,ro);
20271 if(newx*newx+newy*newy+newz*newz <= r3) {
20272 newx += n2; newy += n2; newz += n2;
20273 if( img_ptr(newx,newy,newz) == 0.0f) {
20274 img2_ptr(newx,newy,newz) = 1.0f;
20275 keep_going = false; }
20276 }
20277 }
20278 } else img2_ptr(ib,jb,kb) = 1.0f;
20279 }
20280 } else {
20281
20282 if(img_ptr(ib,jb,kb) == 1.0) {
20283 if(Util::get_frand(0.0f,1.0f) > qprob) {
20284
20285 float numn = -1.0f;
20286 for (newz = -1; newz <= 1; newz++)
20287 for (newy = -1; newy <= 1; newy++)
20288 for (newx = -1; newx <= 1; newx++)
20289 numn += img_ptr(ib+newx,jb+newy,kb+newz);
20290 img2_ptr(ib,jb,kb) = 0.0;
20291 if(numn == 26.0f) {
20292
20293 keep_going = true;
20294 while(keep_going) {
20295 newx = Util::get_irand(-ro,ro);
20296 newy = Util::get_irand(-ro,ro);
20297 newz = Util::get_irand(-ro,ro);
20298 if(newx*newx+newy*newy+newz*newz < r3) {
20299 newx += n2; newy += n2; newz += n2;
20300 if( img_ptr(newx,newy,newz) == 0.0f) {
20301 if(newx*newx+newy*newy+newz*newz < r3) {
20302 if(newx*newx+newy*newy+newz*newz < r3) {
20303 newx += n2; newy += n2; newz += n2;
20304 if( img_ptr(newx,newy,newz) == 0.0f) {
20305 img2_ptr(newx,newy,newz) = 1.0f;
20306 keep_going = false; }
20307 }
20308 }
20309 }
20310 }
20311 }
20312 } else if(numn == 25.0f) {
20313
20314 for (newz = -1; newz <= 1; newz++) {
20315 for (newy = -1; newy <= 1; newy++) {
20316 for (newx = -1; newx <= 1; newx++) {
20317 if( newx != 0 && newy != 0 && newz != 0) {
20318 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
20319 img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
20320 }
20321 }
20322 }
20323 }
20324 }
20325 } else {
20326
20327 keep_going = true;
20328 while(keep_going) {
20329 newx = Util::get_irand(-1,1);
20330 newy = Util::get_irand(-1,1);
20331 newz = Util::get_irand(-1,1);
20332 if(newx != 0 && newy != 0 && newz != 0) {
20333 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
20334 img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;
20335 keep_going = false;
20336 }
20337 }
20338 }
20339 }
20340 } else img2_ptr(ib,jb,kb) = 1.0f;
20341 }
20342 }
20343 }
20344 }
20345 }
20346 }
20347 }
20348
20349 img2->update();
20350
20351 EXITFUNC;
20352 return img2;
20353 }
20354 #undef img_ptr
20355 #undef img2_ptr
20356
20357 struct point3d_t
20358 {
20359 point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
20360
20361 int x;
20362 int y;
20363 int z;
20364 };
20365
20366
20367 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
20368 {
20369 int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
20370 int noff = 6;
20371
20372 int nx = visited->get_xsize();
20373 int ny = visited->get_ysize();
20374 int nz = visited->get_zsize();
20375
20376 vector< point3d_t > pts;
20377 pts.push_back( point3d_t(ix, iy, iz) );
20378 visited->set_value_at( ix, iy, iz, (float)grpid );
20379
20380 int start = 0;
20381 int end = pts.size();
20382
20383 while( end > start ) {
20384 for(int i=start; i < end; ++i ) {
20385 int ix = pts[i].x;
20386 int iy = pts[i].y;
20387 int iz = pts[i].z;
20388
20389 for( int j=0; j < noff; ++j ) {
20390 int jx = ix + offs[j][0];
20391 int jy = iy + offs[j][1];
20392 int jz = iz + offs[j][2];
20393
20394 if( jx < 0 || jx >= nx ) continue;
20395 if( jy < 0 || jy >= ny ) continue;
20396 if( jz < 0 || jz >= nz ) continue;
20397
20398
20399 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
20400 pts.push_back( point3d_t(jx, jy, jz) );
20401 visited->set_value_at( jx, jy, jz, (float)grpid );
20402 }
20403
20404 }
20405 }
20406
20407 start = end;
20408 end = pts.size();
20409 }
20410 return pts.size();
20411 }
20412
20413
20414 EMData* Util::get_biggest_cluster( EMData* mg )
20415 {
20416 int nx = mg->get_xsize();
20417 int ny = mg->get_ysize();
20418 int nz = mg->get_zsize();
20419
20420 EMData* visited = new EMData();
20421 visited->set_size( nx, ny, nz );
20422 visited->to_zero();
20423 int grpid = 0;
20424 int maxgrp = 0;
20425 int maxsize = 0;
20426 for( int iz=0; iz < nz; ++iz ) {
20427 for( int iy=0; iy < ny; ++iy ) {
20428 for( int ix=0; ix < nx; ++ix ) {
20429 if( (*mg)(ix, iy, iz)==0.0 ) continue;
20430
20431 if( (*visited)(ix, iy, iz) > 0.0 ) {
20432
20433 continue;
20434 }
20435
20436 grpid++;
20437 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
20438 if( grpsize > maxsize ) {
20439 maxsize = grpsize;
20440 maxgrp = grpid;
20441 }
20442 }
20443 }
20444 }
20445
20446 Assert( maxgrp > 0 );
20447
20448 int npoint = 0;
20449 EMData* result = new EMData();
20450 result->set_size( nx, ny, nz );
20451 result->to_zero();
20452
20453 for( int iz=0; iz < nz; ++iz ) {
20454 for( int iy=0; iy < ny; ++iy ) {
20455 for( int ix=0; ix < nx; ++ix ) {
20456 if( (*visited)(ix, iy, iz)==maxgrp ) {
20457 (*result)(ix,iy,iz) = 1.0;
20458 npoint++;
20459 }
20460 }
20461 }
20462 }
20463
20464 Assert( npoint==maxsize );
20465 delete visited;
20466 return result;
20467
20468 }
20469
20470 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)
20471 {
20472 int ix, iy, iz;
20473 int i, j, k;
20474 int nr2, nl2;
20475 float az, ak;
20476 float scx, scy, scz;
20477 int offset = 2 - nx%2;
20478 int lsm = nx + offset;
20479 EMData* ctf_img1 = new EMData();
20480 ctf_img1->set_size(lsm, ny, nz);
20481 float freq = 1.0f/(2.0f*ps);
20482 scx = 2.0f/float(nx);
20483 if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20484 if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20485 nr2 = ny/2 ;
20486 nl2 = nz/2 ;
20487 float pihalf = M_PI/2.0f;
20488 for ( k=0; k<nz;k++) {
20489 iz = k; if(k>nl2) iz=k-nz;
20490 float oz2 = iz*scz*iz*scz;
20491 for ( j=0; j<ny;j++) {
20492 iy = j; if(j>nr2) iy=j - ny;
20493 float oy = iy*scy;
20494 float oy2 = oy*oy;
20495 for ( i=0; i<lsm/2; i++) {
20496 ix=i;
20497 if( dza == 0.0f) {
20498 ak=pow(ix*ix*scx*scx + oy2 + oz2, 0.5f)*freq;
20499 (*ctf_img1) (i*2,j,k) = Util::tf(dz, ak, voltage, cs, wgh, b_factor, sign);
20500 } else {
20501 float ox = ix*scx;
20502 ak=pow(ox*ox + oy2 + oz2, 0.5f)*freq;
20503 az = atan2(oy, ox);
20504 float dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f-pihalf));
20505 (*ctf_img1) (i*2,j,k) = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
20506 }
20507
20508 }
20509 }
20510 }
20511 ctf_img1->update();
20512 ctf_img1->set_complex(true);
20513 ctf_img1->set_ri(true);
20514
20515
20516 if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
20517 return ctf_img1;
20518 }
20519
20520
20521
20522
20523
20524
20525
20526
20527
20528
20529
20530
20531
20532
20533
20534
20535
20536
20537
20538
20539
20540
20541
20542
20543
20544
20545
20546
20547
20548
20549
20550
20551
20552
20553
20554
20555
20556
20557
20558
20559
20560
20561
20562
20563
20564
20565
20566
20567
20568
20569
20570
20571
20572
20573
20574
20575
20576
20577
20578
20579
20580
20581
20582
20583
20584
20585
20586
20587
20588
20589
20590
20591
20592
20593
20594
20595
20596
20597
20598
20599
20600
20601
20602
20603
20604
20605
20606 #define cent(i) out[i+N]
20607 #define assign(i) out[i]
20608 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
20609 int nx = d->get_xsize();
20610 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20611 vector<float> out(N+K+2);
20612 if(N*(N-1)/2 != nx) {
20613
20614 return out;}
20615
20616 for(int i=0; i<N; i++) assign(i) = float(i);
20617
20618 for(int i=0; i<N; i++) {
20619 int j = Util::get_irand(0,N-1);
20620 float temp = assign(i);
20621 assign(i) = assign(j);
20622 assign(j) = temp;
20623 }
20624 for(int k=0; k<K; k++) cent(k) = float(assign(k));
20625
20626
20627 for(int i=0; i<N; i++) assign(i) = 0.0f;
20628 float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
20629 bool change = true;
20630 int it = -1;
20631 int ct = -1;
20632 while ((change && disp < dispold) || ct > 0) {
20633
20634 change = false;
20635 dispold = disp;
20636 it++;
20637
20638
20639 disp = 0.0f;
20640 ct = 0;
20641 for(int i=0; i<N; i++) {
20642 qm = 1.0e23f;
20643 for(int k=0; k<K; k++) {
20644 if(float(i) == cent(k)) {
20645 qm = 0.0f;
20646 na = (float)k;
20647 } else {
20648 float dt = (*d)(mono(i,int(cent(k))));
20649 if(dt < qm) {
20650 qm = dt;
20651 na = (float)k;
20652 }
20653 }
20654 }
20655
20656
20657
20658 if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
20659 na = (float)(Util::get_irand(0, K));
20660 qm = (*d)(mono(i,int(na)));
20661 ct++;
20662 }
20663
20664 disp += qm;
20665
20666 if(na != assign(i)) {
20667 assign(i) = na;
20668 change = true;
20669 }
20670 }
20671
20672
20673 T = T*F;
20674
20675
20676
20677
20678
20679 for(int k=0; k<K; k++) {
20680 qm = 1.0e23f;
20681 for(int i=0; i<N; i++) {
20682 if(assign(i) == float(k)) {
20683 float q = 0.0;
20684 for(int j=0; j<N; j++) {
20685 if(assign(j) == float(k)) {
20686
20687 if(i != j) q += (*d)(mono(i,j));
20688
20689 }
20690 }
20691 if(q < qm) {
20692
20693 qm = q;
20694 cent(k) = float(i);
20695 }
20696 }
20697 }
20698 }
20699
20700 }
20701 out[N+K] = disp;
20702 out[N+K+1] = float(it);
20703 return out;
20704 }
20705 #undef cent
20706 #undef assign
20707
20708
20709
20710
20711
20712
20713
20714
20715
20716
20717
20718
20719
20720
20721
20722
20723
20724
20725
20726
20727
20728
20729
20730
20731
20732
20733
20734
20735
20736
20737
20738
20739
20740
20741
20742
20743
20744
20745
20746
20747
20748
20749
20750
20751
20752
20753
20754
20755
20756
20757
20758
20759
20760
20761
20762
20763
20764
20765
20766
20767
20768
20769
20770
20771
20772
20773
20774
20775
20776
20777
20778
20779
20780
20781
20782
20783
20784
20785
20786
20787
20788
20789
20790
20791
20792
20793
20794
20795
20796
20797
20798
20799
20800
20801
20802
20803
20804
20805
20806
20807
20808
20809
20810
20811
20812
20813
20814
20815
20816 vector<float> Util::cluster_equalsize(EMData* d) {
20817
20818 int nx = d->get_xsize();
20819 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20820 int K = N/2;
20821 vector<float> group(N);
20822 if(N*(N-1)/2 != nx) {
20823
20824 return group;}
20825
20826 bool * active = new bool[N];
20827 for(int i=0; i<N; i++) active[i] = true;
20828
20829 float dm, qd;
20830 int ppi = 0, ppj = 0;
20831 for(int k=0; k<K; k++) {
20832
20833
20834 dm = 1.0e23f;
20835 for(int i=1; i<N; i++) {
20836 if(active[i]) {
20837 for(int j=0; j<i; j++) {
20838 if(active[j]) {
20839 qd = (*d)(i*(i - 1)/2 + j);
20840 if(qd < dm) {
20841 dm = qd;
20842 ppi = i;
20843 ppj = j;
20844 }
20845 }
20846 }
20847 }
20848 }
20849 group[2*k] = float(ppi);
20850 group[1+2*k] = float(ppj);
20851 active[ppi] = false;
20852 active[ppj] = false;
20853 }
20854
20855 delete [] active;
20856 active = NULL;
20857 return group;
20858 }
20859
20860
20861
20862
20863
20864
20865
20866
20867
20868
20869
20870
20871
20872
20873
20874
20875
20876
20877
20878
20879
20880
20881
20882
20883
20884
20885
20886
20887
20888
20889
20890
20891
20892
20893
20894
20895
20896
20897
20898
20899
20900
20901
20902
20903
20904
20905
20906
20907
20908
20909
20910
20911
20912
20913 #define data(i,j) group[i*ny+j]
20914 vector<float> Util::vareas(EMData* d) {
20915 const float step=0.001f;
20916 int ny = d->get_ysize();
20917
20918
20919
20920 vector<float> group(2*ny);
20921 for(int i=0; i<2*ny; i++) group[i] = 0.0f;
20922 int K = int(1.0f/step) +1;
20923 int hit = 0;
20924 for(int kx=0; kx<=K; kx++) {
20925 float tx = kx*step;
20926 for(int ky=0; ky<=K; ky++) {
20927 float ty = ky*step;
20928 float dm = 1.0e23f;
20929 for(int i=0; i<ny; i++) {
20930 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
20931 if( qd < dm) {
20932 dm = qd;
20933 hit = i;
20934 }
20935 }
20936 data(0,hit) += 1.0f;
20937 if(kx == 0 || ky == 0 || kx == K || ky == K) data(1,hit) = 1.0f;
20938 }
20939 }
20940 return group;
20941 }
20942 #undef data
20943
20944 EMData* Util::get_slice(EMData *vol, int dim, int index) {
20945
20946 int nx = vol->get_xsize();
20947 int ny = vol->get_ysize();
20948 int nz = vol->get_zsize();
20949 float *vol_data = vol->get_data();
20950 int new_nx, new_ny;
20951
20952 if (nz == 1)
20953 throw ImageDimensionException("Error: Input must be a 3-D object");
20954 if ((dim < 1) || (dim > 3))
20955 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
20956 if (((dim == 1) && (index < 0 || index > nx-1)) ||
20957 ((dim == 1) && (index < 0 || index > nx-1)) ||
20958 ((dim == 1) && (index < 0 || index > nx-1)))
20959 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
20960
20961 if (dim == 1) {
20962 new_nx = ny;
20963 new_ny = nz;
20964 } else if (dim == 2) {
20965 new_nx = nx;
20966 new_ny = nz;
20967 } else {
20968 new_nx = nx;
20969 new_ny = ny;
20970 }
20971
20972 EMData *slice = new EMData();
20973 slice->set_size(new_nx, new_ny, 1);
20974 float *slice_data = slice->get_data();
20975
20976 if (dim == 1) {
20977 for (int x=0; x<new_nx; x++)
20978 for (int y=0; y<new_ny; y++)
20979 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
20980 } else if (dim == 2) {
20981 for (int x=0; x<new_nx; x++)
20982 for (int y=0; y<new_ny; y++)
20983 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
20984 } else {
20985 for (int x=0; x<new_nx; x++)
20986 for (int y=0; y<new_ny; y++)
20987 slice_data[y*new_nx+x] = vol_data[((size_t)index*ny+y)*nx+x];
20988 }
20989
20990 return slice;
20991 }
20992
20993 void Util::image_mutation(EMData *img, float mutation_rate) {
20994 int nx = img->get_xsize();
20995 float min = img->get_attr("minimum");
20996 float max = img->get_attr("maximum");
20997 float* img_data = img->get_data();
20998 array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
20999 return;
21000 }
21001
21002
21003 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
21004
21005 if (is_mirror != 0) {
21006 for (int i=0; i<len_list; i++) {
21007 int r = rand()%10000;
21008 float f = r/10000.0f;
21009 if (f < mutation_rate) list[i] = 1-list[i];
21010 }
21011 } else {
21012 map<int, vector<int> > graycode;
21013 map<vector<int>, int> rev_graycode;
21014 vector <int> gray;
21015
21016 int K=1;
21017 for (int i=0; i<L; i++) K*=2;
21018
21019 for (int k=0; k<K; k++) {
21020 int shift = 0;
21021 vector <int> gray;
21022 for (int i=L-1; i>-1; i--) {
21023 int t = ((k>>i)%2-shift)%2;
21024 gray.push_back(t);
21025 shift += t-2;
21026 }
21027 graycode[k] = gray;
21028 rev_graycode[gray] = k;
21029 }
21030
21031 float gap = (K-1)/(max_val-min_val);
21032 for (int i=0; i<len_list; i++) {
21033 float val = list[i];
21034 if (val < min_val) { val = min_val; }
21035 else if (val > max_val) { val = max_val; }
21036 int k = int((val-min_val)*gap+0.5);
21037 vector<int> gray = graycode[k];
21038 bool changed = false;
21039 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
21040 int r = rand()%10000;
21041 float f = r/10000.0f;
21042 if (f < mutation_rate) {
21043 *p = 1-*p;
21044 changed = true;
21045 }
21046 }
21047 if (changed) {
21048 k = rev_graycode[gray];
21049 list[i] = k/gap+min_val;
21050 }
21051 }
21052 }
21053
21054 }
21055
21056 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
21057
21058 if (is_mirror != 0) {
21059 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
21060 int r = rand()%10000;
21061 float f = r/10000.0f;
21062 if (f < mutation_rate) *q = 1-*q;
21063 }
21064 } else {
21065 map<int, vector<int> > graycode;
21066 map<vector<int>, int> rev_graycode;
21067 vector <int> gray;
21068
21069 int K=1;
21070 for (int i=0; i<L; i++) K*=2;
21071
21072 for (int k=0; k<K; k++) {
21073 int shift = 0;
21074 vector <int> gray;
21075 for (int i=L-1; i>-1; i--) {
21076 int t = ((k>>i)%2-shift)%2;
21077 gray.push_back(t);
21078 shift += t-2;
21079 }
21080 graycode[k] = gray;
21081 rev_graycode[gray] = k;
21082 }
21083
21084 float gap = (K-1)/(max_val-min_val);
21085 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
21086 float val = *q;
21087 if (val < min_val) { val = min_val; }
21088 else if (val > max_val) { val = max_val; }
21089 int k = int((val-min_val)*gap+0.5);
21090 vector<int> gray = graycode[k];
21091 bool changed = false;
21092 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
21093 int r = rand()%10000;
21094 float f = r/10000.0f;
21095 if (f < mutation_rate) {
21096 *p = 1-*p;
21097 changed = true;
21098 }
21099 }
21100 if (changed) {
21101 k = rev_graycode[gray];
21102 *q = k/gap+min_val;
21103 }
21104 }
21105 }
21106 return list;
21107 }
21108
21109
21110 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
21111
21112 int total_cost = *output;
21113 int num_matches = *(output+1);
21114
21115 int cost=0;
21116 int* intx;
21117 int intx_size;
21118 int* intx_next(0);
21119 int intx_next_size = 0;
21120 int curclass;
21121 int curclass_size;
21122
21123 for(int i = 0; i < num_matches; i++){
21124 curclass = *(output+2+ i*nParts);
21125
21126 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
21127 *(argParts + Indices[curclass]+1) = -5;
21128
21129 curclass_size = *(dimClasses+curclass)-2;
21130 intx = new int[curclass_size];
21131 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
21132 intx_size = curclass_size;
21133
21134 for (int j=1; j < nParts; j++){
21135 curclass = *(output+2+ i*nParts+j);
21136 if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
21137 *(argParts + Indices[j*K+curclass]+1)=-5;
21138
21139 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);
21140 intx_next = new int[intx_next_size];
21141 Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
21142 delete[] intx;
21143 intx=intx_next;
21144 intx_size= intx_next_size;
21145 }
21146 delete[] intx_next;
21147
21148 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
21149
21150 cost = cost + intx_next_size;
21151 }
21152
21153 if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
21154
21155 return 1;
21156
21157 }
21158
21159
21160
21161
21162
21163
21164 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int* costlist, int J){
21165
21166
21167 bool flag = 0;
21168 int nintx;
21169 int* dummy(0);
21170
21171 int* curbranch = new int[nParts];
21172
21173
21174 for(int jit= 0; jit< J; jit++) *(costlist+jit) = 0;
21175
21176
21177 for(int a=0; a<K; a++)
21178 {
21179
21180
21181 if (*(argParts + Indices[a] + 1) < 1) continue;
21182 if (*(dimClasses + a)-2 <= T) continue;
21183
21184
21185
21186 for( int i=1; i < nParts; i++){
21187 flag = 0;
21188 for(int j=0; j < K; j++){
21189 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
21190 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);
21191 if (nintx > T) flag=1;
21192 else *(argParts + Indices[i*K+j] + 1) =-4;
21193 }
21194 if (flag==0) {break;}
21195 }
21196
21197
21198 *curbranch = a;
21199
21200 if (flag > 0)
21201 Util::explore2(argParts, Indices, dimClasses, nParts, K, T, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2,
21202 *(dimClasses+a)-2,0, J, matchlist, costlist, curbranch);
21203
21204
21205 for( int i=1; i < nParts; i++){
21206 for(int j=0; j < K; j++){
21207 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
21208
21209 }
21210 }
21211 }
21212
21213 delete[] curbranch;
21214 }
21215
21216
21217 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){
21218
21219
21220 int* curintx2(0);
21221 int nintx = size_curintx;
21222
21223
21224
21225 if (depth >0){
21226 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
21227 if (nintx <= T) return;
21228 }
21229
21230
21231 if (depth == (nParts-1)) {
21232
21233 int replace = 0;
21234 int ind_smallest = -1;
21235 int smallest_cost = -1;
21236
21237 for (int jit = 0; jit < J; jit++){
21238 if (*(costlist+jit) < nintx){
21239 replace = 1;
21240 if (ind_smallest == -1) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
21241 if (*(costlist+jit) < smallest_cost) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
21242 }
21243 }
21244
21245 if (replace > 0){
21246
21247 *(costlist + ind_smallest) = nintx;
21248 for (int xit = 0; xit < nParts; xit++)
21249 *(matchlist + ind_smallest*nParts + xit) = *(curbranch+xit);
21250
21251 }
21252
21253 return;
21254 }
21255
21256
21257
21258
21259 if (depth > 0){
21260 curintx2 = new int[nintx];
21261 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
21262 }
21263
21264 if (depth == 0){
21265
21266 curintx2 = new int[size_curintx];
21267 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
21268 }
21269
21270
21271
21272 depth=depth+1;
21273
21274 for (int i=0; i < K; i++){
21275
21276 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue;
21277 size_next = (*(dimClasses + depth*K+i ))-2;
21278 if (size_next <= T) continue;
21279 *(curbranch+depth) = i;
21280 Util::explore2(argParts,Indices, dimClasses, nParts, K, T, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth,J, matchlist,
21281 costlist, curbranch);
21282
21283 }
21284
21285 delete[] curintx2;
21286 }
21287
21288 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
21289
21290
21291
21292
21293
21294
21295
21296
21297
21298
21299 int* dummy(0);
21300 int* cref;
21301 int cref_size;
21302 int* ccomp;
21303 int ccomp_size;
21304 int nintx;
21305 for (int i=0; i < nParts; i++){
21306 for (int j =0; j < K; j++){
21307
21308
21309 cref = Parts[i][j];
21310 cref_size = dimClasses[i*K+cref[0]]-2;
21311
21312
21313 if (cref_size <= T){
21314 cref[0] = -1;
21315 continue;
21316 }
21317 bool done = 0;
21318 for (int a = 0; a < nParts; a++){
21319 if (a == i) continue;
21320 bool hasActive=0;
21321 for (unsigned int b=0; b < Parts[a].size(); b++){
21322
21323
21324 ccomp = Parts[a][b];
21325 ccomp_size= dimClasses[a*K+ccomp[0]]-2;
21326 nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
21327
21328
21329 if (nintx <= T)
21330 ccomp[1] = 0;
21331 else{
21332 ccomp[1] = 1;
21333 hasActive=1;
21334 }
21335 }
21336
21337 if (hasActive < 1){
21338 done=1;
21339 break;
21340 }
21341
21342 }
21343
21344 if (done > 0){
21345
21346
21347 cref[0] = -1;
21348 continue;
21349 }
21350
21351
21352
21353
21354
21355
21356
21357
21358
21359
21360 bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
21361
21362 if (found<1){
21363
21364 cref[0] = -1;
21365 }
21366 }
21367
21368
21369
21370 for (int d = K-1; d > -1; d--){
21371 if (Parts[i][d][0] < 0) Parts[i].erase(Parts[i].begin()+d);
21372 }
21373
21374 }
21375
21376
21377
21378
21379
21380 }
21381
21382
21383 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) {
21384
21385
21386 if (size_next <= T) return 0;
21387
21388
21389 int* curintx2(0);
21390 int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
21391 if (nintx <= T) return 0;
21392
21393 int old_depth=depth;
21394 if (depth == partref) depth = depth + 1;
21395 if (depth == nParts && old_depth>0) return 1;
21396
21397
21398
21399 curintx2 = new int[nintx];
21400 Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
21401
21402
21403
21404
21405 bool gt_thresh;
21406 int num_classes = Parts[depth].size();
21407
21408 for (int i=0; i < num_classes; i++){
21409 if (Parts[depth][i][1] < 1) continue;
21410 size_next = dimClasses[depth*K + Parts[depth][i][0] ]-2;
21411 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
21412 if (gt_thresh) { delete[] curintx2; return 1; }
21413 }
21414 delete[] curintx2;
21415 return 0;
21416 }
21417
21418
21419
21420
21421 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int n_guesses, int LARGEST_CLASS, int J,
21422 int max_branching, float stmult, int branchfunc, int LIM) {
21423
21424
21425
21426
21427
21428 int* Indices = new int[nParts*K];
21429 int ind_c = 0;
21430 for (int i=0; i < nParts; i++){
21431 for(int j = 0; j < K; j++){
21432 Indices[i*K + j] = ind_c;
21433 ind_c = ind_c + dimClasses[i*K + j];
21434 }
21435 }
21436
21437
21438
21439
21440 vector <vector <int*> > Parts(nParts,vector<int*>(K));
21441 ind_c = 0;
21442 int argParts_size=0;
21443 for (int i=0; i < nParts; i++){
21444 for(int j = 0; j < K; j++){
21445 Parts[i][j] = argParts + ind_c;
21446 ind_c = ind_c + dimClasses[i*K + j];
21447 argParts_size = argParts_size + dimClasses[i*K + j];
21448 }
21449 }
21450
21451
21452
21453
21454
21455
21456 Util::initial_prune(Parts, dimClasses, nParts, K, T);
21457 for(int i = 0; i < nParts; i++){
21458 for(int j=0; j < K; j++){
21459 argParts[Indices[i*K + j]+1] = -1;
21460 }
21461 }
21462
21463 int num_classes;
21464 int old_index;
21465 for(int i=0; i<nParts; i++){
21466 num_classes = Parts[i].size();
21467 for (int j=0; j < num_classes; j++){
21468 old_index = Parts[i][j][0];
21469
21470 argParts[Indices[i*K + old_index]+1] = 1;
21471 }
21472 }
21473
21474
21475
21476
21477
21478 int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T, 0, n_guesses, LARGEST_CLASS, J, max_branching, stmult, branchfunc, LIM);
21479
21480
21481
21482
21483 bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
21484
21485 delete[] Indices;
21486
21487
21488 if (correct < 1){
21489 cout << "something is wrong with output of branchMPI!\n";
21490 vector<int> ret(1);
21491 ret[0] = -1;
21492 if (output != 0) { delete[] output; output = 0; }
21493 return ret;
21494 }
21495
21496
21497
21498
21499
21500 int output_size = 2 + output[1] * nParts;
21501 vector<int> ret(output_size);
21502 for (int i = 0; i < output_size; i++) {
21503 ret[i]= output[i];
21504 }
21505 if (output != 0) { delete[] output; output = 0; }
21506 return ret;
21507
21508 }
21509
21510
21511 int branch_all=0;
21512 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int curlevel,int n_guesses, int
21513 LARGEST_CLASS, int J, int max_branching, float stmult, int branchfunc, int LIM) {
21514
21515
21516
21517 if (1 == 0){
21518 cout <<"begin test search2\n";
21519 int* matchlist = new int[J*nParts];
21520 int* costlist = new int[J];
21521 for (int jit = 0; jit < nParts; jit++) *(costlist+jit) = 0;
21522 Util::search2(argParts,Indices, dimClasses, nParts, K, T, matchlist,costlist,J);
21523
21524 for (int jit = 0; jit < J; jit++) {
21525 cout << *(costlist +jit)<<": ";
21526 for (int yit = 0; yit < nParts; yit++)
21527 cout << *(matchlist + jit*nParts + yit)<<",";
21528 cout <<"\n";
21529
21530 }
21531 cout <<"end test search2\n";
21532 int* output = new int[1];
21533 output[0] = 1;
21534 delete [] matchlist;
21535 delete [] costlist;
21536 return output;
21537 }
21538
21539
21540
21541 if (curlevel > K -1){
21542 int* output = new int[2];
21543 output[0] = 0;
21544 output[1] = 0;
21545 return output;
21546 }
21547
21548
21549
21550 int* matchlist = new int[J*nParts];
21551 int* costlist = new int[J];
21552 Util::search2(argParts, Indices, dimClasses, nParts, K, T, matchlist, costlist, J);
21553
21554
21555
21556
21557
21558
21559 for (int jit = 0; jit < J ; jit++){
21560
21561 if (costlist[jit] > T) break;
21562 if (jit == J-1){
21563 int* output = new int[2];
21564 output[0] = 0;
21565 output[1] = 0;
21566 delete[] matchlist;
21567 delete[] costlist;
21568 return output;
21569 }
21570 }
21571
21572
21573
21574
21575 if (curlevel==0) branch_all = 0;
21576
21577 int nBranches = -1;
21578
21579 if (branchfunc == 0)
21580 nBranches = branch_factor_0(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM);
21581
21582 if (branchfunc == 2)
21583 nBranches = branch_factor_2(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM);
21584
21585 if (branchfunc == 3)
21586 nBranches = branch_factor_3(costlist,matchlist,J, T, nParts, curlevel, max_branching, K, LIM);
21587
21588 if (branchfunc == 4)
21589 nBranches = branch_factor_4(costlist,matchlist,J, T, nParts, curlevel, max_branching, stmult);
21590
21591 int* newcostlist= new int[nBranches];
21592 int* newmatchlist = new int[nBranches*nParts];
21593 for (int i=0; i<nBranches; i++){
21594 newcostlist[i] = costlist[i];
21595 for (int j=0; j< nParts; j++)
21596 newmatchlist[i*nParts + j] = matchlist[i*nParts + j];
21597 }
21598
21599 delete[] costlist;
21600 delete[] matchlist;
21601
21602
21603 int* output = new int[2+K*nParts];
21604 output[0] = 0;
21605 output[1] = 0;
21606
21607 int old_index;
21608 int totalcost;
21609 int nmatches;
21610
21611
21612 for(int i=0; i < nBranches ; i++){
21613
21614
21615
21616
21617
21618
21619
21620 for(int j=0; j < nParts; j++){
21621
21622 old_index = newmatchlist[i*nParts + j];
21623 argParts[Indices[j*K+old_index] + 1] = -2;
21624 }
21625
21626
21627 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, curlevel+1, n_guesses, LARGEST_CLASS,
21628 J, max_branching, stmult,branchfunc, LIM);
21629
21630
21631 totalcost = newcostlist[i] + ret[0];
21632
21633
21634
21635
21636
21637 if (totalcost > output[0])
21638 {
21639 nmatches = 1 + ret[1];
21640
21641
21642 output[0] = totalcost;
21643 output[1] = nmatches;
21644 int nret = 2+(nmatches-1)*nParts;
21645 for(int iret=2; iret < nret; iret++) output[iret] = ret[iret];
21646 for(int imax=0; imax < nParts; imax++) output[nret+imax] = newmatchlist[i*nParts + imax];
21647 }
21648
21649
21650 delete[] ret;
21651
21652
21653
21654 for(int j=0; j < nParts; j++){
21655 old_index = newmatchlist[i*nParts + j];
21656 argParts[Indices[j*K+old_index] + 1] = 1;
21657 }
21658
21659 }
21660
21661 delete[] newmatchlist;
21662 delete[] newcostlist;
21663
21664 return output;
21665 }
21666
21667 int* costlist_global;
21668
21669 bool jiafunc(int i, int j){
21670 return (costlist_global[j] < costlist_global[i]) ;
21671
21672 }
21673
21674
21675
21676
21677
21678 int Util::branch_factor_2(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21679
21680 int ntot=0;
21681 for (int jit=0; jit < J; jit++){
21682 if (*(costlist+jit) > T) ntot++;
21683 }
21684
21685 int cur;
21686
21687 int* indx = new int[J];
21688 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21689 vector<int> myindx (indx, indx+J);
21690 vector<int>::iterator it;
21691 costlist_global=costlist;
21692 sort(myindx.begin(), myindx.end(), jiafunc);
21693
21694
21695 int* templist = new int[J];
21696 int* temp2list = new int[J*nParts];
21697 int next = 0;
21698
21699 for (it=myindx.begin(); it!=myindx.end();++it){
21700 cur = *(costlist + *it);
21701 if (cur > T){
21702
21703 templist[next] = cur;
21704 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21705 next = next + 1;
21706 }
21707 }
21708
21709 for (int jit=0; jit < ntot; jit++){
21710 *(costlist+jit)=*(templist + jit);
21711
21712 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21713 }
21714
21715
21716 delete [] indx;
21717
21718
21719
21720
21721
21722
21723 int B=1;
21724 int B_init=B;
21725 int infeasible=0;
21726
21727 for (int i=B_init; i<ntot; i++){
21728 if (i==ntot) continue;
21729
21730
21731 infeasible = 0;
21732 if (LIM < 0) LIM = B;
21733 for (int j=0; j<B; j++){
21734
21735 for (int vit=0; vit<nParts; vit++){
21736 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21737 }
21738 if (infeasible >= LIM) break;
21739 }
21740
21741 if (infeasible >= LIM){
21742 *(costlist+B)=*(templist+i);
21743 for (int vit=0; vit < nParts; vit++)
21744 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21745 B=B+1;
21746 }
21747 }
21748
21749 delete [] templist;
21750 delete [] temp2list;
21751
21752
21753 if (branch_all < max_branching){
21754 if (B>1)
21755 {branch_all = branch_all + B -1 ; }
21756 }
21757 else B=1;
21758
21759 return B;
21760
21761
21762 }
21763
21764
21765
21766 int Util::branch_factor_3(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int, int LIM){
21767
21768 int ntot=0;
21769 for (int jit=0; jit < J; jit++){
21770 if (*(costlist+jit) > T) ntot++;
21771 }
21772
21773 int cur;
21774
21775 int* indx = new int[J];
21776 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21777 vector<int> myindx (indx, indx+J);
21778 vector<int>::iterator it;
21779 costlist_global=costlist;
21780 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21781
21782
21783 int* templist = new int[J];
21784 int* temp2list = new int[J*nParts];
21785 int next = 0;
21786
21787 for (it=myindx.begin(); it!=myindx.end();++it){
21788 cur = *(costlist + *it);
21789 if (cur > T){
21790
21791 templist[next] = cur;
21792 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21793 next = next + 1;
21794 }
21795 }
21796
21797 for (int jit=0; jit < ntot; jit++){
21798 *(costlist+jit)=*(templist + jit);
21799
21800 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21801 }
21802
21803
21804 delete [] indx;
21805
21806
21807
21808
21809
21810
21811 int B=1;
21812 int B_init=B;
21813 int infeasible=0;
21814
21815
21816 if (LIM < 0) LIM = ntot-1;
21817 for (int i=B_init; i<ntot; i++){
21818 if (i==ntot) continue;
21819
21820
21821 infeasible = 0;
21822
21823 for (int j=0; j<ntot; j++){
21824 if (j == i) continue;
21825 for (int vit=0; vit<nParts; vit++){
21826 if (temp2list[i*nParts+vit] == temp2list[j*nParts+vit]) {infeasible++; break;}
21827 }
21828 if (infeasible >= LIM) break;
21829 }
21830
21831 if (infeasible >= LIM){
21832 *(costlist+B)=*(templist+i);
21833 for (int vit=0; vit < nParts; vit++)
21834 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21835 B=B+1;
21836 }
21837 }
21838
21839 delete [] templist;
21840 delete [] temp2list;
21841
21842
21843
21844 if (branch_all < max_branching){
21845 if (B>1)
21846 {branch_all = branch_all + B-1;}
21847 }
21848 else B=1;
21849
21850 return B;
21851
21852
21853 }
21854
21855
21856
21857
21858
21859 int Util::branch_factor_4(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, float stmult){
21860 int sum=0;
21861 float average =0;
21862 int ntot=0;
21863 for (int jit=0; jit < J; jit++){
21864 if (*(costlist+jit) > T) {ntot++; sum = sum +*(costlist+jit);}
21865 }
21866 average = ((float)sum)/((float)ntot);
21867 int cur;
21868
21869 int* indx = new int[J];
21870 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21871 vector<int> myindx (indx, indx+J);
21872 vector<int>::iterator it;
21873 costlist_global=costlist;
21874 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21875
21876
21877 int* templist = new int[J];
21878 int* temp2list = new int[J*nParts];
21879 int next = 0;
21880
21881 for (it=myindx.begin(); it!=myindx.end();++it){
21882 cur = *(costlist + *it);
21883 if (cur > T){
21884
21885 templist[next] = cur;
21886 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21887 next = next + 1;
21888 }
21889 }
21890
21891 for (int jit=0; jit < ntot; jit++){
21892 *(costlist+jit)=*(templist + jit);
21893
21894 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21895 }
21896
21897
21898 delete [] indx;
21899 delete [] templist;
21900 delete [] temp2list;
21901
21902 if (ntot == 1) return 1;
21903
21904
21905
21906 float sq_sum=0.0;
21907
21908 for (int i=0; i< ntot; i++){
21909 sq_sum = sq_sum + (float) pow((float) *(costlist+i) - average, (float)2.0);
21910
21911 }
21912
21913
21914 float variance = sq_sum/ntot;
21915 float stdev = (float)pow((float)variance,(float)0.5);
21916
21917
21918
21919 int B=1;
21920 int largest = *costlist;
21921
21922 for (int i=1; i<ntot; i++){
21923 int cur = *(costlist+i);
21924 if (largest-cur < (float)(stdev*stmult)) B++;
21925 else break;
21926
21927 }
21928
21929 if (branch_all < max_branching){
21930 if (B>1)
21931 {branch_all = branch_all + B-1;}
21932 }
21933 else B=1;
21934
21935 return B;
21936
21937
21938 }
21939
21940 int Util::branch_factor_0(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21941
21942 int ntot=0;
21943 for (int jit=0; jit < J; jit++){
21944 if (*(costlist+jit) > T) ntot++;
21945 }
21946
21947 int cur;
21948
21949 int* indx = new int[J];
21950 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21951 vector<int> myindx (indx, indx+J);
21952 vector<int>::iterator it;
21953 costlist_global=costlist;
21954 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21955
21956
21957 int* templist = new int[J];
21958 int* temp2list = new int[J*nParts];
21959 int next = 0;
21960
21961 for (it=myindx.begin(); it!=myindx.end();++it){
21962 cur = *(costlist + *it);
21963 if (cur > T){
21964
21965 templist[next] = cur;
21966 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21967 next = next + 1;
21968 }
21969 }
21970
21971 for (int jit=0; jit < ntot; jit++){
21972 *(costlist+jit)=*(templist + jit);
21973
21974 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21975 }
21976
21977
21978 for (int jit=1; jit < ntot; jit++){
21979
21980 if ((costlist[jit] == costlist[0]) && costlist[jit] > T){
21981
21982 for (int vit=0; vit < nParts; vit++){
21983 if ( matchlist[jit*nParts + vit] > matchlist[vit])
21984 break;
21985 if ( matchlist[jit*nParts + vit] == matchlist[vit])
21986 continue;
21987 if ( matchlist[jit*nParts + vit] < matchlist[vit])
21988 {
21989
21990 for (int swp=0; swp < nParts; swp++){
21991 int tmp = matchlist[swp];
21992 matchlist[swp]= matchlist[jit*nParts + swp];
21993 matchlist[jit*nParts + swp] = tmp;
21994 }
21995 break;
21996
21997 }
21998 }
21999 }
22000
22001 }
22002
22003
22004 delete [] indx;
22005
22006
22007
22008
22009
22010
22011 int B=1;
22012 int B_init=B;
22013 int infeasible=0;
22014
22015 for (int i=B_init; i<ntot; i++){
22016 if (i==ntot) continue;
22017
22018
22019 infeasible = 0;
22020 if (LIM < 0) LIM = B;
22021 for (int j=0; j<B; j++){
22022
22023 for (int vit=0; vit<nParts; vit++){
22024 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
22025 }
22026 if (infeasible >= LIM) break;
22027 }
22028
22029 if (infeasible >= LIM){
22030 *(costlist+B)=*(templist+i);
22031 for (int vit=0; vit < nParts; vit++)
22032 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
22033 B=B+1;
22034 }
22035 }
22036
22037 delete [] templist;
22038 delete [] temp2list;
22039
22040
22041 if (branch_all < max_branching){
22042 if (B>1)
22043 {branch_all = branch_all + B -1 ; }
22044 }
22045 else B=1;
22046
22047 return B;
22048
22049
22050 }