00001
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifdef _WIN32
00037 #pragma warning(disable:4819)
00038 #endif //_WIN32
00039
00040 #include <cstring>
00041 #include <ctime>
00042 #include <iostream>
00043 #include <cstdio>
00044 #include <cstdlib>
00045 #include <boost/format.hpp>
00046 #include "emdata.h"
00047 #include "util.h"
00048 #include "fundamentals.h"
00049 #include "lapackblas.h"
00050 #include "lbfgsb.h"
00051 using namespace EMAN;
00052 #include "steepest.h"
00053 #include "emassert.h"
00054 #include "randnum.h"
00055
00056 #include <gsl/gsl_sf_bessel.h>
00057 #include <gsl/gsl_sf_bessel.h>
00058 #include <cmath>
00059 using namespace std;
00060 using std::complex;
00061
00062 vector<float> Util::infomask(EMData* Vol, EMData* mask, bool flip = false)
00063
00064
00065 {
00066 ENTERFUNC;
00067 vector<float> stats;
00068 float *Volptr, *maskptr,MAX,MIN;
00069 long double Sum1,Sum2;
00070 long count;
00071
00072 MAX = -FLT_MAX;
00073 MIN = FLT_MAX;
00074 count = 0L;
00075 Sum1 = 0.0L;
00076 Sum2 = 0.0L;
00077
00078 if (mask == NULL) {
00079
00080 stats.push_back(Vol->get_attr("mean"));
00081 stats.push_back(Vol->get_attr("sigma"));
00082 stats.push_back(Vol->get_attr("minimum"));
00083 stats.push_back(Vol->get_attr("maximum"));
00084 return stats;
00085 }
00086
00087
00088
00089 size_t nx = Vol->get_xsize();
00090 size_t ny = Vol->get_ysize();
00091 size_t nz = Vol->get_zsize();
00092
00093 size_t mask_nx = mask->get_xsize();
00094 size_t mask_ny = mask->get_ysize();
00095 size_t mask_nz = mask->get_zsize();
00096
00097 if (nx != mask_nx || ny != mask_ny || nz != mask_nz )
00098 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111 Volptr = Vol->get_data();
00112 maskptr = mask->get_data();
00113
00114 for (size_t i = 0; i < (size_t)nx*ny*nz; ++i) {
00115 if ((maskptr[i]>0.5f) == flip) {
00116 Sum1 += Volptr[i];
00117 Sum2 += Volptr[i]*double(Volptr[i]);
00118 MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00119 MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00120 count++;
00121 }
00122 }
00123
00124 if (count == 0) {
00125 LOGERR("Invalid mask");
00126 throw ImageFormatException( "Invalid mask");
00127 }
00128
00129 float avg = static_cast<float>(Sum1/count);
00130 float sig = static_cast<float>(sqrt((Sum2 - Sum1*Sum1/count)/(count-1)));
00131
00132 stats.push_back(avg);
00133 stats.push_back(sig);
00134 stats.push_back(MIN);
00135 stats.push_back(MAX);
00136
00137 return stats;
00138 }
00139
00140
00141
00142
00143 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00144 {
00145 ENTERFUNC;
00146
00147 if (!EMUtil::is_same_size(V1, V2)) {
00148 LOGERR("images not same size");
00149 throw ImageFormatException( "images not same size");
00150 }
00151
00152 size_t nx = V1->get_xsize();
00153 size_t ny = V1->get_ysize();
00154 size_t nz = V1->get_zsize();
00155 size_t size = (size_t)nx*ny*nz;
00156
00157 EMData *BD = new EMData();
00158 BD->set_size(nx, ny, nz);
00159
00160 float *params = new float[2];
00161
00162 float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00163 long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00164 int nvox = 0L;
00165
00166 V1ptr = V1->get_data();
00167 V2ptr = V2->get_data();
00168 BDptr = BD->get_data();
00169
00170
00171 if(!mask){
00172 EMData * Mask = new EMData();
00173 Mask->set_size(nx,ny,nz);
00174 Mask->to_one();
00175 MASKptr = Mask->get_data();
00176 } else {
00177 if (!EMUtil::is_same_size(V1, mask)) {
00178 LOGERR("mask not same size");
00179 throw ImageFormatException( "mask not same size");
00180 }
00181
00182 MASKptr = mask->get_data();
00183 }
00184
00185
00186
00187
00188
00189 for (size_t i = 0L;i < size; i++) {
00190 if (MASKptr[i]>0.5f) {
00191 S1 += V1ptr[i]*V2ptr[i];
00192 S2 += V1ptr[i]*V1ptr[i];
00193 S3 += V2ptr[i];
00194 S4 += V1ptr[i];
00195 nvox ++;
00196 }
00197 }
00198
00199 if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00200 A =1.0f ;
00201 } else {
00202 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00203 }
00204 B = static_cast<float> (A*S4 - S3)/nvox;
00205
00206
00207
00208 for (size_t i = 0L;i < size; i++) {
00209 if (MASKptr[i]>0.5f) {
00210 BDptr[i] = A*V1ptr[i] - B - V2ptr[i];
00211 } else {
00212 BDptr[i] = 0.f;
00213 }
00214 }
00215
00216 BD->update();
00217
00218 params[0] = A;
00219 params[1] = B;
00220
00221 Dict BDnParams;
00222 BDnParams["imdiff"] = BD;
00223 BDnParams["A"] = params[0];
00224 BDnParams["B"] = params[1];
00225
00226 EXITFUNC;
00227 return BDnParams;
00228 }
00229
00230
00231
00232
00233
00234 EMData *Util::TwoDTestFunc(int Size, float p, float q, float a, float b, int flag, float alphaDeg)
00235 {
00236 ENTERFUNC;
00237 int Mid= (Size+1)/2;
00238
00239 if (flag==0) {
00240 EMData* ImBW = new EMData();
00241 ImBW->set_size(Size,Size,1);
00242 ImBW->to_zero();
00243
00244 float tempIm;
00245 float x,y;
00246
00247 for (int ix=(1-Mid); ix<Mid; ix++){
00248 for (int iy=(1-Mid); iy<Mid; iy++){
00249 x = (float)ix;
00250 y = (float)iy;
00251 tempIm= static_cast<float>( (1/(2*M_PI)) * cos(p*x)* cos(q*y) * exp(-.5*x*x/(a*a))* exp(-.5*y*y/(b*b)) );
00252 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00253 }
00254 }
00255 ImBW->update();
00256 ImBW->set_complex(false);
00257 ImBW->set_ri(true);
00258
00259 return ImBW;
00260 }
00261 else if (flag==1) {
00262 EMData* ImBWFFT = new EMData();
00263 ImBWFFT ->set_size(2*Size,Size,1);
00264 ImBWFFT ->to_zero();
00265
00266 float r,s;
00267
00268 for (int ir=(1-Mid); ir<Mid; ir++){
00269 for (int is=(1-Mid); is<Mid; is++){
00270 r = (float)ir;
00271 s = (float)is;
00272 (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00273 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00274 }
00275 }
00276 ImBWFFT->update();
00277 ImBWFFT->set_complex(true);
00278 ImBWFFT->set_ri(true);
00279 ImBWFFT->set_shuffled(true);
00280 ImBWFFT->set_fftodd(true);
00281
00282 return ImBWFFT;
00283 }
00284 else if (flag==2 || flag==3) {
00285 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00286 float C=cos(alpha);
00287 float S=sin(alpha);
00288 float D= sqrt(S*S*b*b + C*C*a*a);
00289
00290
00291 float P = p * C *a*a/D ;
00292 float Q = q * S *b*b/D ;
00293
00294 if (flag==2) {
00295 EMData* pofalpha = new EMData();
00296 pofalpha ->set_size(Size,1,1);
00297 pofalpha ->to_zero();
00298
00299 float Norm0 = D*(float)sqrt(2*pi);
00300 float Norm1 = exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00301 float Norm2 = exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00302 float sD;
00303
00304 for (int is=(1-Mid); is<Mid; is++){
00305 sD = is/D ;
00306 (*pofalpha)(is+Mid-1) = Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00307 + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00308 }
00309 pofalpha-> update();
00310 pofalpha-> set_complex(false);
00311 pofalpha-> set_ri(true);
00312
00313 return pofalpha;
00314 }
00315 if (flag==3) {
00316 float vD;
00317
00318 EMData* pofalphak = new EMData();
00319 pofalphak ->set_size(2*Size,1,1);
00320 pofalphak ->to_zero();
00321
00322 for (int iv=(1-Mid); iv<Mid; iv++){
00323 vD = iv*D ;
00324 (*pofalphak)(2*(iv+Mid-1)) = exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00325 }
00326 pofalphak-> update();
00327 pofalphak-> set_complex(false);
00328 pofalphak-> set_ri(true);
00329
00330 return pofalphak;
00331 }
00332 }
00333 else if (flag==4) {
00334 cout <<" FH under construction";
00335 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00336 EMData* TryFH= OutFT -> real2FH(4.0);
00337 return TryFH;
00338 } else {
00339 cout <<" flag must be 0,1,2,3, or 4";
00340 }
00341
00342 EXITFUNC;
00343 return 0;
00344 }
00345
00346
00347 void Util::spline_mat(float *x, float *y, int n, float *xq, float *yq, int m)
00348 {
00349
00350 float x0= x[0];
00351 float x1= x[1];
00352 float x2= x[2];
00353 float y0= y[0];
00354 float y1= y[1];
00355 float y2= y[2];
00356 float yp1 = (y1-y0)/(x1-x0) + (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1) ;
00357 float xn = x[n];
00358 float xnm1= x[n-1];
00359 float xnm2= x[n-2];
00360 float yn = y[n];
00361 float ynm1= y[n-1];
00362 float ynm2= y[n-2];
00363 float ypn= (yn-ynm1)/(xn-xnm1) + (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00364 float *y2d = new float[n];
00365 Util::spline(x,y,n,yp1,ypn,y2d);
00366 Util::splint(x,y,y2d,n,xq,yq,m);
00367 delete [] y2d;
00368 return;
00369 }
00370
00371
00372 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2)
00373 {
00374 int i,k;
00375 float p, qn, sig, un, *u;
00376 u = new float[n-1];
00377
00378 if (yp1 > .99e30){
00379 y2[0]=u[0]=0.0;
00380 } else {
00381 y2[0]=-.5f;
00382 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00383 }
00384
00385 for (i=1; i < n-1; i++) {
00386 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00387 p = sig*y2[i-1] + 2.0f;
00388 y2[i] = (sig-1.0f)/p;
00389 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) - (y[i] - y[i-1] )/(x[i] -x[i-1]);
00390 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00391 }
00392
00393 if (ypn>.99e30){
00394 qn=0; un=0;
00395 } else {
00396 qn= .5f;
00397 un= (3.0f/(x[n-1] -x[n-2])) * (ypn - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00398 }
00399 y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00400 for (k=n-2; k>=0; k--){
00401 y2[k]=y2[k]*y2[k+1]+u[k];
00402 }
00403 delete [] u;
00404 }
00405
00406
00407 void Util::splint( float *xa, float *ya, float *y2a, int n, float *xq, float *yq, int m)
00408 {
00409 int klo, khi, k;
00410 float h, b, a;
00411
00412
00413 for (int j=0; j<m;j++){
00414 klo=0;
00415 khi=n-1;
00416 while (khi-klo >1) {
00417 k=(khi+klo) >>1;
00418 if (xa[k]>xq[j]){ khi=k;}
00419 else { klo=k;}
00420 }
00421 h=xa[khi]- xa[klo];
00422 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00423 a =(xa[khi]-xq[j])/h;
00424 b=(xq[j]-xa[klo])/h;
00425 yq[j]=a*ya[klo] + b*ya[khi]
00426 + ((a*a*a-a)*y2a[klo]
00427 +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00428 }
00429
00430 }
00431
00432
00433 void Util::Radialize(int *PermMatTr, float *kValsSorted,
00434 float *weightofkValsSorted, int Size, int *SizeReturned)
00435 {
00436 int iMax = (int) floor( (Size-1.0)/2 +.01);
00437 int CountMax = (iMax+2)*(iMax+1)/2;
00438 int Count=-1;
00439 float *kVals = new float[CountMax];
00440 float *weightMat = new float[CountMax];
00441 int *PermMat = new int[CountMax];
00442 SizeReturned[0] = CountMax;
00443
00444
00445 for (int jkx=0; jkx< iMax+1; jkx++) {
00446 for (int jky=0; jky< jkx+1; jky++) {
00447 Count++;
00448 kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00449 weightMat[Count]= 1.0;
00450 if (jkx!=0) { weightMat[Count] *=2;}
00451 if (jky!=0) { weightMat[Count] *=2;}
00452 if (jkx!=jky){ weightMat[Count] *=2;}
00453 PermMat[Count]=Count+1;
00454 }
00455 }
00456
00457 int lkVals = Count+1;
00458
00459
00460 sort_mat(&kVals[0],&kVals[Count],
00461 &PermMat[0], &PermMat[Count]);
00462
00463 fflush(stdout);
00464
00465 int newInd;
00466
00467 for (int iP=0; iP < lkVals ; iP++ ) {
00468 newInd = PermMat[iP];
00469 PermMatTr[newInd-1] = iP+1;
00470 }
00471
00472
00473
00474 int CountA=-1;
00475 int CountB=-1;
00476
00477 while (CountB< (CountMax-1)) {
00478 CountA++;
00479 CountB++;
00480
00481 kValsSorted[CountA] = kVals[CountB] ;
00482 if (CountB<(CountMax-1) ) {
00483 while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001 ) {
00484 SizeReturned[0]--;
00485 for (int iP=0; iP < lkVals; iP++){
00486
00487 if (PermMatTr[iP]>CountA+1) {
00488 PermMatTr[iP]--;
00489 }
00490 }
00491 CountB++;
00492 }
00493 }
00494 }
00495
00496
00497 for (int CountD=0; CountD < CountMax; CountD++) {
00498 newInd = PermMatTr[CountD];
00499 weightofkValsSorted[newInd-1] += weightMat[CountD];
00500 }
00501
00502 }
00503
00504
00505 vector<float>
00506 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00507 {
00508 vector<float> angles;
00509 float psi = 0.0;
00510 if ((0.0 == t1 && 0.0 == t2)||(t1 >= t2)) {
00511 t1 = 0.0f;
00512 t2 = 90.0f;
00513 }
00514 if ((0.0 == p1 && 0.0 == p2)||(p1 >= p2)) {
00515 p1 = 0.0f;
00516 p2 = 359.9f;
00517 }
00518 bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00519 for (float theta = t1; theta <= t2; theta += delta) {
00520 float detphi;
00521 int lt;
00522 if ((0.0 == theta)||(180.0 == theta)) {
00523 detphi = 360.0f;
00524 lt = 1;
00525 } else {
00526 detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00527 lt = int((p2 - p1)/detphi)-1;
00528 if (lt < 1) lt = 1;
00529 detphi = (p2 - p1)/lt;
00530 }
00531 for (int i = 0; i < lt; i++) {
00532 float phi = p1 + i*detphi;
00533 if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00534 angles.push_back(phi);
00535 angles.push_back(theta);
00536 angles.push_back(psi);
00537 }
00538 }
00539 return angles;
00540 }
00541
00542
00543 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00646 {
00647
00648
00649 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00650 float quadri;
00651 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00652
00653 x = xx;
00654 y = yy;
00655
00656
00657 while ( x < 1.0 ) x += nxdata;
00658 while ( x >= (float)(nxdata+1) ) x -= nxdata;
00659 while ( y < 1.0 ) y += nydata;
00660 while ( y >= (float)(nydata+1) ) y -= nydata;
00661
00662 i = (int) x;
00663 j = (int) y;
00664
00665 dx0 = x - i;
00666 dy0 = y - j;
00667
00668 ip1 = i + 1;
00669 im1 = i - 1;
00670 jp1 = j + 1;
00671 jm1 = j - 1;
00672
00673 if (ip1 > nxdata) ip1 -= nxdata;
00674 if (im1 < 1) im1 += nxdata;
00675 if (jp1 > nydata) jp1 -= nydata;
00676 if (jm1 < 1) jm1 += nydata;
00677
00678 f0 = fdata(i,j);
00679 c1 = fdata(ip1,j) - f0;
00680 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00681 c3 = fdata(i,jp1) - f0;
00682 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00683
00684 dxb = dx0 - 1;
00685 dyb = dy0 - 1;
00686
00687
00688 if (dx0 >= 0) hxc = 1; else hxc = -1;
00689 if (dy0 >= 0) hyc = 1; else hyc = -1;
00690
00691 ic = i + hxc;
00692 jc = j + hyc;
00693
00694 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00695 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00696
00697 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00698 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00699
00700
00701 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00702
00703 return quadri;
00704 }
00705
00706 #undef fdata
00707
00708 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00709 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00710 {
00711
00712
00713 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00714 float quadri;
00715 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00716
00717 x = xx;
00718 y = yy;
00719
00720
00721 if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00722 x = (float)xnew;
00723 y = (float)ynew;
00724 }
00725
00726
00727 i = (int) x;
00728 j = (int) y;
00729
00730 dx0 = x - i;
00731 dy0 = y - j;
00732
00733 ip1 = i + 1;
00734 im1 = i - 1;
00735 jp1 = j + 1;
00736 jm1 = j - 1;
00737
00738 if (ip1 > nxdata) ip1 -= nxdata;
00739 if (im1 < 1) im1 += nxdata;
00740 if (jp1 > nydata) jp1 -= nydata;
00741 if (jm1 < 1) jm1 += nydata;
00742
00743 f0 = fdata(i,j);
00744 c1 = fdata(ip1,j) - f0;
00745 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00746 c3 = fdata(i,jp1) - f0;
00747 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00748
00749 dxb = dx0 - 1;
00750 dyb = dy0 - 1;
00751
00752
00753 if (dx0 >= 0) hxc = 1; else hxc = -1;
00754 if (dy0 >= 0) hyc = 1; else hyc = -1;
00755
00756 ic = i + hxc;
00757 jc = j + hyc;
00758
00759 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00760 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00761
00762 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00763 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00764
00765
00766 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00767
00768 return quadri;
00769 }
00770
00771 #undef fdata
00772
00773
00774 float Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00775 int K = kb.get_window_size();
00776 int kbmin = -K/2;
00777 int kbmax = -kbmin;
00778 int kbc = kbmax+1;
00779
00780 float pixel =0.0f;
00781 float w=0.0f;
00782
00783 delx = restrict1(delx, nx);
00784 int inxold = int(round(delx));
00785 if ( ny < 2 ) {
00786 float tablex1 = kb.i0win_tab(delx-inxold+3);
00787 float tablex2 = kb.i0win_tab(delx-inxold+2);
00788 float tablex3 = kb.i0win_tab(delx-inxold+1);
00789 float tablex4 = kb.i0win_tab(delx-inxold);
00790 float tablex5 = kb.i0win_tab(delx-inxold-1);
00791 float tablex6 = kb.i0win_tab(delx-inxold-2);
00792 float tablex7 = kb.i0win_tab(delx-inxold-3);
00793
00794 int x1, x2, x3, x4, x5, x6, x7;
00795
00796 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
00797 x1 = (inxold-3+nx)%nx;
00798 x2 = (inxold-2+nx)%nx;
00799 x3 = (inxold-1+nx)%nx;
00800 x4 = (inxold +nx)%nx;
00801 x5 = (inxold+1+nx)%nx;
00802 x6 = (inxold+2+nx)%nx;
00803 x7 = (inxold+3+nx)%nx;
00804 } else {
00805 x1 = inxold-3;
00806 x2 = inxold-2;
00807 x3 = inxold-1;
00808 x4 = inxold;
00809 x5 = inxold+1;
00810 x6 = inxold+2;
00811 x7 = inxold+3;
00812 }
00813
00814 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00815 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00816 data[x7]*tablex7 ;
00817
00818 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00819 } else if ( nz < 2 ) {
00820 dely = restrict1(dely, ny);
00821 int inyold = int(round(dely));
00822 float tablex1 = kb.i0win_tab(delx-inxold+3);
00823 float tablex2 = kb.i0win_tab(delx-inxold+2);
00824 float tablex3 = kb.i0win_tab(delx-inxold+1);
00825 float tablex4 = kb.i0win_tab(delx-inxold);
00826 float tablex5 = kb.i0win_tab(delx-inxold-1);
00827 float tablex6 = kb.i0win_tab(delx-inxold-2);
00828 float tablex7 = kb.i0win_tab(delx-inxold-3);
00829
00830 float tabley1 = kb.i0win_tab(dely-inyold+3);
00831 float tabley2 = kb.i0win_tab(dely-inyold+2);
00832 float tabley3 = kb.i0win_tab(dely-inyold+1);
00833 float tabley4 = kb.i0win_tab(dely-inyold);
00834 float tabley5 = kb.i0win_tab(dely-inyold-1);
00835 float tabley6 = kb.i0win_tab(dely-inyold-2);
00836 float tabley7 = kb.i0win_tab(dely-inyold-3);
00837
00838 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00839
00840 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
00841 x1 = (inxold-3+nx)%nx;
00842 x2 = (inxold-2+nx)%nx;
00843 x3 = (inxold-1+nx)%nx;
00844 x4 = (inxold +nx)%nx;
00845 x5 = (inxold+1+nx)%nx;
00846 x6 = (inxold+2+nx)%nx;
00847 x7 = (inxold+3+nx)%nx;
00848
00849 y1 = ((inyold-3+ny)%ny)*nx;
00850 y2 = ((inyold-2+ny)%ny)*nx;
00851 y3 = ((inyold-1+ny)%ny)*nx;
00852 y4 = ((inyold +ny)%ny)*nx;
00853 y5 = ((inyold+1+ny)%ny)*nx;
00854 y6 = ((inyold+2+ny)%ny)*nx;
00855 y7 = ((inyold+3+ny)%ny)*nx;
00856 } else {
00857 x1 = inxold-3;
00858 x2 = inxold-2;
00859 x3 = inxold-1;
00860 x4 = inxold;
00861 x5 = inxold+1;
00862 x6 = inxold+2;
00863 x7 = inxold+3;
00864
00865 y1 = (inyold-3)*nx;
00866 y2 = (inyold-2)*nx;
00867 y3 = (inyold-1)*nx;
00868 y4 = inyold*nx;
00869 y5 = (inyold+1)*nx;
00870 y6 = (inyold+2)*nx;
00871 y7 = (inyold+3)*nx;
00872 }
00873
00874 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00875 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00876 data[x7+y1]*tablex7 ) * tabley1 +
00877 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00878 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00879 data[x7+y2]*tablex7 ) * tabley2 +
00880 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00881 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00882 data[x7+y3]*tablex7 ) * tabley3 +
00883 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00884 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00885 data[x7+y4]*tablex7 ) * tabley4 +
00886 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00887 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00888 data[x7+y5]*tablex7 ) * tabley5 +
00889 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00890 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00891 data[x7+y6]*tablex7 ) * tabley6 +
00892 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00893 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00894 data[x7+y7]*tablex7 ) * tabley7;
00895
00896 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00897 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00898 } else {
00899 dely = restrict1(dely, ny);
00900 int inyold = int(Util::round(dely));
00901 delz = restrict1(delz, nz);
00902 int inzold = int(Util::round(delz));
00903
00904 float tablex1 = kb.i0win_tab(delx-inxold+3);
00905 float tablex2 = kb.i0win_tab(delx-inxold+2);
00906 float tablex3 = kb.i0win_tab(delx-inxold+1);
00907 float tablex4 = kb.i0win_tab(delx-inxold);
00908 float tablex5 = kb.i0win_tab(delx-inxold-1);
00909 float tablex6 = kb.i0win_tab(delx-inxold-2);
00910 float tablex7 = kb.i0win_tab(delx-inxold-3);
00911
00912 float tabley1 = kb.i0win_tab(dely-inyold+3);
00913 float tabley2 = kb.i0win_tab(dely-inyold+2);
00914 float tabley3 = kb.i0win_tab(dely-inyold+1);
00915 float tabley4 = kb.i0win_tab(dely-inyold);
00916 float tabley5 = kb.i0win_tab(dely-inyold-1);
00917 float tabley6 = kb.i0win_tab(dely-inyold-2);
00918 float tabley7 = kb.i0win_tab(dely-inyold-3);
00919
00920 float tablez1 = kb.i0win_tab(delz-inzold+3);
00921 float tablez2 = kb.i0win_tab(delz-inzold+2);
00922 float tablez3 = kb.i0win_tab(delz-inzold+1);
00923 float tablez4 = kb.i0win_tab(delz-inzold);
00924 float tablez5 = kb.i0win_tab(delz-inzold-1);
00925 float tablez6 = kb.i0win_tab(delz-inzold-2);
00926 float tablez7 = kb.i0win_tab(delz-inzold-3);
00927
00928 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00929
00930 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
00931 x1 = (inxold-3+nx)%nx;
00932 x2 = (inxold-2+nx)%nx;
00933 x3 = (inxold-1+nx)%nx;
00934 x4 = (inxold +nx)%nx;
00935 x5 = (inxold+1+nx)%nx;
00936 x6 = (inxold+2+nx)%nx;
00937 x7 = (inxold+3+nx)%nx;
00938
00939 y1 = ((inyold-3+ny)%ny)*nx;
00940 y2 = ((inyold-2+ny)%ny)*nx;
00941 y3 = ((inyold-1+ny)%ny)*nx;
00942 y4 = ((inyold +ny)%ny)*nx;
00943 y5 = ((inyold+1+ny)%ny)*nx;
00944 y6 = ((inyold+2+ny)%ny)*nx;
00945 y7 = ((inyold+3+ny)%ny)*nx;
00946
00947 z1 = ((inzold-3+nz)%nz)*nx*ny;
00948 z2 = ((inzold-2+nz)%nz)*nx*ny;
00949 z3 = ((inzold-1+nz)%nz)*nx*ny;
00950 z4 = ((inzold +nz)%nz)*nx*ny;
00951 z5 = ((inzold+1+nz)%nz)*nx*ny;
00952 z6 = ((inzold+2+nz)%nz)*nx*ny;
00953 z7 = ((inzold+3+nz)%nz)*nx*ny;
00954 } else {
00955 x1 = inxold-3;
00956 x2 = inxold-2;
00957 x3 = inxold-1;
00958 x4 = inxold;
00959 x5 = inxold+1;
00960 x6 = inxold+2;
00961 x7 = inxold+3;
00962
00963 y1 = (inyold-3)*nx;
00964 y2 = (inyold-2)*nx;
00965 y3 = (inyold-1)*nx;
00966 y4 = inyold*nx;
00967 y5 = (inyold+1)*nx;
00968 y6 = (inyold+2)*nx;
00969 y7 = (inyold+3)*nx;
00970
00971 z1 = (inzold-3)*nx*ny;
00972 z2 = (inzold-2)*nx*ny;
00973 z3 = (inzold-1)*nx*ny;
00974 z4 = inzold*nx*ny;
00975 z5 = (inzold+1)*nx*ny;
00976 z6 = (inzold+2)*nx*ny;
00977 z7 = (inzold+3)*nx*ny;
00978 }
00979
00980 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00981 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00982 data[x7+y1+z1]*tablex7 ) * tabley1 +
00983 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00984 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00985 data[x7+y2+z1]*tablex7 ) * tabley2 +
00986 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00987 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00988 data[x7+y3+z1]*tablex7 ) * tabley3 +
00989 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00990 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00991 data[x7+y4+z1]*tablex7 ) * tabley4 +
00992 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
00993 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
00994 data[x7+y5+z1]*tablex7 ) * tabley5 +
00995 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
00996 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
00997 data[x7+y6+z1]*tablex7 ) * tabley6 +
00998 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
00999 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01000 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01001 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01002 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01003 data[x7+y1+z2]*tablex7 ) * tabley1 +
01004 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01005 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01006 data[x7+y2+z2]*tablex7 ) * tabley2 +
01007 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01008 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01009 data[x7+y3+z2]*tablex7 ) * tabley3 +
01010 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01011 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01012 data[x7+y4+z2]*tablex7 ) * tabley4 +
01013 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01014 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01015 data[x7+y5+z2]*tablex7 ) * tabley5 +
01016 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01017 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01018 data[x7+y6+z2]*tablex7 ) * tabley6 +
01019 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01020 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01021 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01022 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01023 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01024 data[x7+y1+z3]*tablex7 ) * tabley1 +
01025 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01026 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01027 data[x7+y2+z3]*tablex7 ) * tabley2 +
01028 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01029 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01030 data[x7+y3+z3]*tablex7 ) * tabley3 +
01031 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01032 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01033 data[x7+y4+z3]*tablex7 ) * tabley4 +
01034 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01035 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01036 data[x7+y5+z3]*tablex7 ) * tabley5 +
01037 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01038 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01039 data[x7+y6+z3]*tablex7 ) * tabley6 +
01040 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01041 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01042 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01043 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01044 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01045 data[x7+y1+z4]*tablex7 ) * tabley1 +
01046 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01047 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01048 data[x7+y2+z4]*tablex7 ) * tabley2 +
01049 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01050 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01051 data[x7+y3+z4]*tablex7 ) * tabley3 +
01052 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01053 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01054 data[x7+y4+z4]*tablex7 ) * tabley4 +
01055 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01056 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01057 data[x7+y5+z4]*tablex7 ) * tabley5 +
01058 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01059 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01060 data[x7+y6+z4]*tablex7 ) * tabley6 +
01061 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01062 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01063 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01064 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01065 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01066 data[x7+y1+z5]*tablex7 ) * tabley1 +
01067 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01068 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01069 data[x7+y2+z5]*tablex7 ) * tabley2 +
01070 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01071 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01072 data[x7+y3+z5]*tablex7 ) * tabley3 +
01073 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01074 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01075 data[x7+y4+z5]*tablex7 ) * tabley4 +
01076 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01077 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01078 data[x7+y5+z5]*tablex7 ) * tabley5 +
01079 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01080 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01081 data[x7+y6+z5]*tablex7 ) * tabley6 +
01082 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01083 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01084 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01085 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01086 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01087 data[x7+y1+z6]*tablex7 ) * tabley1 +
01088 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01089 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01090 data[x7+y2+z6]*tablex7 ) * tabley2 +
01091 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01092 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01093 data[x7+y3+z6]*tablex7 ) * tabley3 +
01094 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01095 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01096 data[x7+y4+z6]*tablex7 ) * tabley4 +
01097 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01098 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01099 data[x7+y5+z6]*tablex7 ) * tabley5 +
01100 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01101 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01102 data[x7+y6+z6]*tablex7 ) * tabley6 +
01103 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01104 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01105 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01106 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01107 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01108 data[x7+y1+z7]*tablex7 ) * tabley1 +
01109 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01110 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01111 data[x7+y2+z7]*tablex7 ) * tabley2 +
01112 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01113 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01114 data[x7+y3+z7]*tablex7 ) * tabley3 +
01115 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01116 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01117 data[x7+y4+z7]*tablex7 ) * tabley4 +
01118 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01119 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01120 data[x7+y5+z7]*tablex7 ) * tabley5 +
01121 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01122 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01123 data[x7+y6+z7]*tablex7 ) * tabley6 +
01124 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01125 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01126 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01127
01128 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01129 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01130 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01131 }
01132 return pixel/w;
01133 }
01134
01135 float Util::get_pixel_conv_new_background(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb, int xnew, int ynew) {
01136 int K = kb.get_window_size();
01137 int kbmin = -K/2;
01138 int kbmax = -kbmin;
01139 int kbc = kbmax+1;
01140
01141 float pixel =0.0f;
01142 float w=0.0f;
01143
01144 float argdelx = delx;
01145 delx = restrict1(delx, nx);
01146 int inxold = int(round(delx));
01147 if ( ny < 2 ) {
01148 float tablex1 = kb.i0win_tab(delx-inxold+3);
01149 float tablex2 = kb.i0win_tab(delx-inxold+2);
01150 float tablex3 = kb.i0win_tab(delx-inxold+1);
01151 float tablex4 = kb.i0win_tab(delx-inxold);
01152 float tablex5 = kb.i0win_tab(delx-inxold-1);
01153 float tablex6 = kb.i0win_tab(delx-inxold-2);
01154 float tablex7 = kb.i0win_tab(delx-inxold-3);
01155
01156 int x1, x2, x3, x4, x5, x6, x7;
01157
01158 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
01159 x1 = (inxold-3+nx)%nx;
01160 x2 = (inxold-2+nx)%nx;
01161 x3 = (inxold-1+nx)%nx;
01162 x4 = (inxold +nx)%nx;
01163 x5 = (inxold+1+nx)%nx;
01164 x6 = (inxold+2+nx)%nx;
01165 x7 = (inxold+3+nx)%nx;
01166 } else {
01167 x1 = inxold-3;
01168 x2 = inxold-2;
01169 x3 = inxold-1;
01170 x4 = inxold;
01171 x5 = inxold+1;
01172 x6 = inxold+2;
01173 x7 = inxold+3;
01174 }
01175
01176 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01177 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01178 data[x7]*tablex7 ;
01179
01180 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01181 } else if ( nz < 2 ) {
01182
01183 delx = argdelx;
01184
01185 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01186 delx = (float)xnew*2.0f;
01187 dely = (float)ynew*2.0f;
01188 }
01189
01190 int inxold = int(round(delx));
01191 int inyold = int(round(dely));
01192
01193 float tablex1 = kb.i0win_tab(delx-inxold+3);
01194 float tablex2 = kb.i0win_tab(delx-inxold+2);
01195 float tablex3 = kb.i0win_tab(delx-inxold+1);
01196 float tablex4 = kb.i0win_tab(delx-inxold);
01197 float tablex5 = kb.i0win_tab(delx-inxold-1);
01198 float tablex6 = kb.i0win_tab(delx-inxold-2);
01199 float tablex7 = kb.i0win_tab(delx-inxold-3);
01200
01201 float tabley1 = kb.i0win_tab(dely-inyold+3);
01202 float tabley2 = kb.i0win_tab(dely-inyold+2);
01203 float tabley3 = kb.i0win_tab(dely-inyold+1);
01204 float tabley4 = kb.i0win_tab(dely-inyold);
01205 float tabley5 = kb.i0win_tab(dely-inyold-1);
01206 float tabley6 = kb.i0win_tab(dely-inyold-2);
01207 float tabley7 = kb.i0win_tab(dely-inyold-3);
01208
01209 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01210
01211 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
01212 x1 = (inxold-3+nx)%nx;
01213 x2 = (inxold-2+nx)%nx;
01214 x3 = (inxold-1+nx)%nx;
01215 x4 = (inxold +nx)%nx;
01216 x5 = (inxold+1+nx)%nx;
01217 x6 = (inxold+2+nx)%nx;
01218 x7 = (inxold+3+nx)%nx;
01219
01220 y1 = ((inyold-3+ny)%ny)*nx;
01221 y2 = ((inyold-2+ny)%ny)*nx;
01222 y3 = ((inyold-1+ny)%ny)*nx;
01223 y4 = ((inyold +ny)%ny)*nx;
01224 y5 = ((inyold+1+ny)%ny)*nx;
01225 y6 = ((inyold+2+ny)%ny)*nx;
01226 y7 = ((inyold+3+ny)%ny)*nx;
01227 } else {
01228 x1 = inxold-3;
01229 x2 = inxold-2;
01230 x3 = inxold-1;
01231 x4 = inxold;
01232 x5 = inxold+1;
01233 x6 = inxold+2;
01234 x7 = inxold+3;
01235
01236 y1 = (inyold-3)*nx;
01237 y2 = (inyold-2)*nx;
01238 y3 = (inyold-1)*nx;
01239 y4 = inyold*nx;
01240 y5 = (inyold+1)*nx;
01241 y6 = (inyold+2)*nx;
01242 y7 = (inyold+3)*nx;
01243 }
01244
01245 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01246 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01247 data[x7+y1]*tablex7 ) * tabley1 +
01248 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01249 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01250 data[x7+y2]*tablex7 ) * tabley2 +
01251 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01252 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01253 data[x7+y3]*tablex7 ) * tabley3 +
01254 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01255 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01256 data[x7+y4]*tablex7 ) * tabley4 +
01257 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01258 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01259 data[x7+y5]*tablex7 ) * tabley5 +
01260 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01261 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01262 data[x7+y6]*tablex7 ) * tabley6 +
01263 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01264 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01265 data[x7+y7]*tablex7 ) * tabley7;
01266
01267 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01268 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01269 } else {
01270 dely = restrict1(dely, ny);
01271 int inyold = int(Util::round(dely));
01272 delz = restrict1(delz, nz);
01273 int inzold = int(Util::round(delz));
01274
01275 float tablex1 = kb.i0win_tab(delx-inxold+3);
01276 float tablex2 = kb.i0win_tab(delx-inxold+2);
01277 float tablex3 = kb.i0win_tab(delx-inxold+1);
01278 float tablex4 = kb.i0win_tab(delx-inxold);
01279 float tablex5 = kb.i0win_tab(delx-inxold-1);
01280 float tablex6 = kb.i0win_tab(delx-inxold-2);
01281 float tablex7 = kb.i0win_tab(delx-inxold-3);
01282
01283 float tabley1 = kb.i0win_tab(dely-inyold+3);
01284 float tabley2 = kb.i0win_tab(dely-inyold+2);
01285 float tabley3 = kb.i0win_tab(dely-inyold+1);
01286 float tabley4 = kb.i0win_tab(dely-inyold);
01287 float tabley5 = kb.i0win_tab(dely-inyold-1);
01288 float tabley6 = kb.i0win_tab(dely-inyold-2);
01289 float tabley7 = kb.i0win_tab(dely-inyold-3);
01290
01291 float tablez1 = kb.i0win_tab(delz-inzold+3);
01292 float tablez2 = kb.i0win_tab(delz-inzold+2);
01293 float tablez3 = kb.i0win_tab(delz-inzold+1);
01294 float tablez4 = kb.i0win_tab(delz-inzold);
01295 float tablez5 = kb.i0win_tab(delz-inzold-1);
01296 float tablez6 = kb.i0win_tab(delz-inzold-2);
01297 float tablez7 = kb.i0win_tab(delz-inzold-3);
01298
01299 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01300
01301 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
01302 x1 = (inxold-3+nx)%nx;
01303 x2 = (inxold-2+nx)%nx;
01304 x3 = (inxold-1+nx)%nx;
01305 x4 = (inxold +nx)%nx;
01306 x5 = (inxold+1+nx)%nx;
01307 x6 = (inxold+2+nx)%nx;
01308 x7 = (inxold+3+nx)%nx;
01309
01310 y1 = ((inyold-3+ny)%ny)*nx;
01311 y2 = ((inyold-2+ny)%ny)*nx;
01312 y3 = ((inyold-1+ny)%ny)*nx;
01313 y4 = ((inyold +ny)%ny)*nx;
01314 y5 = ((inyold+1+ny)%ny)*nx;
01315 y6 = ((inyold+2+ny)%ny)*nx;
01316 y7 = ((inyold+3+ny)%ny)*nx;
01317
01318 z1 = ((inzold-3+nz)%nz)*nx*ny;
01319 z2 = ((inzold-2+nz)%nz)*nx*ny;
01320 z3 = ((inzold-1+nz)%nz)*nx*ny;
01321 z4 = ((inzold +nz)%nz)*nx*ny;
01322 z5 = ((inzold+1+nz)%nz)*nx*ny;
01323 z6 = ((inzold+2+nz)%nz)*nx*ny;
01324 z7 = ((inzold+3+nz)%nz)*nx*ny;
01325 } else {
01326 x1 = inxold-3;
01327 x2 = inxold-2;
01328 x3 = inxold-1;
01329 x4 = inxold;
01330 x5 = inxold+1;
01331 x6 = inxold+2;
01332 x7 = inxold+3;
01333
01334 y1 = (inyold-3)*nx;
01335 y2 = (inyold-2)*nx;
01336 y3 = (inyold-1)*nx;
01337 y4 = inyold*nx;
01338 y5 = (inyold+1)*nx;
01339 y6 = (inyold+2)*nx;
01340 y7 = (inyold+3)*nx;
01341
01342 z1 = (inzold-3)*nx*ny;
01343 z2 = (inzold-2)*nx*ny;
01344 z3 = (inzold-1)*nx*ny;
01345 z4 = inzold*nx*ny;
01346 z5 = (inzold+1)*nx*ny;
01347 z6 = (inzold+2)*nx*ny;
01348 z7 = (inzold+3)*nx*ny;
01349 }
01350
01351 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01352 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01353 data[x7+y1+z1]*tablex7 ) * tabley1 +
01354 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01355 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01356 data[x7+y2+z1]*tablex7 ) * tabley2 +
01357 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01358 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01359 data[x7+y3+z1]*tablex7 ) * tabley3 +
01360 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01361 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01362 data[x7+y4+z1]*tablex7 ) * tabley4 +
01363 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01364 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01365 data[x7+y5+z1]*tablex7 ) * tabley5 +
01366 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01367 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01368 data[x7+y6+z1]*tablex7 ) * tabley6 +
01369 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01370 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01371 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01372 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01373 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01374 data[x7+y1+z2]*tablex7 ) * tabley1 +
01375 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01376 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01377 data[x7+y2+z2]*tablex7 ) * tabley2 +
01378 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01379 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01380 data[x7+y3+z2]*tablex7 ) * tabley3 +
01381 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01382 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01383 data[x7+y4+z2]*tablex7 ) * tabley4 +
01384 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01385 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01386 data[x7+y5+z2]*tablex7 ) * tabley5 +
01387 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01388 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01389 data[x7+y6+z2]*tablex7 ) * tabley6 +
01390 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01391 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01392 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01393 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01394 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01395 data[x7+y1+z3]*tablex7 ) * tabley1 +
01396 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01397 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01398 data[x7+y2+z3]*tablex7 ) * tabley2 +
01399 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01400 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01401 data[x7+y3+z3]*tablex7 ) * tabley3 +
01402 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01403 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01404 data[x7+y4+z3]*tablex7 ) * tabley4 +
01405 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01406 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01407 data[x7+y5+z3]*tablex7 ) * tabley5 +
01408 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01409 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01410 data[x7+y6+z3]*tablex7 ) * tabley6 +
01411 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01412 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01413 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01414 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01415 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01416 data[x7+y1+z4]*tablex7 ) * tabley1 +
01417 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01418 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01419 data[x7+y2+z4]*tablex7 ) * tabley2 +
01420 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01421 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01422 data[x7+y3+z4]*tablex7 ) * tabley3 +
01423 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01424 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01425 data[x7+y4+z4]*tablex7 ) * tabley4 +
01426 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01427 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01428 data[x7+y5+z4]*tablex7 ) * tabley5 +
01429 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01430 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01431 data[x7+y6+z4]*tablex7 ) * tabley6 +
01432 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01433 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01434 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01435 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01436 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01437 data[x7+y1+z5]*tablex7 ) * tabley1 +
01438 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01439 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01440 data[x7+y2+z5]*tablex7 ) * tabley2 +
01441 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01442 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01443 data[x7+y3+z5]*tablex7 ) * tabley3 +
01444 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01445 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01446 data[x7+y4+z5]*tablex7 ) * tabley4 +
01447 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01448 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01449 data[x7+y5+z5]*tablex7 ) * tabley5 +
01450 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01451 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01452 data[x7+y6+z5]*tablex7 ) * tabley6 +
01453 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01454 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01455 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01456 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01457 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01458 data[x7+y1+z6]*tablex7 ) * tabley1 +
01459 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01460 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01461 data[x7+y2+z6]*tablex7 ) * tabley2 +
01462 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01463 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01464 data[x7+y3+z6]*tablex7 ) * tabley3 +
01465 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01466 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01467 data[x7+y4+z6]*tablex7 ) * tabley4 +
01468 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01469 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01470 data[x7+y5+z6]*tablex7 ) * tabley5 +
01471 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01472 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01473 data[x7+y6+z6]*tablex7 ) * tabley6 +
01474 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01475 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01476 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01477 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01478 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01479 data[x7+y1+z7]*tablex7 ) * tabley1 +
01480 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01481 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01482 data[x7+y2+z7]*tablex7 ) * tabley2 +
01483 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01484 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01485 data[x7+y3+z7]*tablex7 ) * tabley3 +
01486 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01487 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01488 data[x7+y4+z7]*tablex7 ) * tabley4 +
01489 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01490 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01491 data[x7+y5+z7]*tablex7 ) * tabley5 +
01492 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01493 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01494 data[x7+y6+z7]*tablex7 ) * tabley6 +
01495 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01496 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01497 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01498
01499 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01500 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01501 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01502 }
01503 return pixel/w;
01504 }
01505
01506
01507
01508
01509
01510
01511
01512
01513
01514
01515
01516
01517
01518
01519
01520
01521
01522
01523
01524
01525
01526
01527
01528
01529
01530
01531
01532
01533
01534
01535
01536
01537
01538
01539
01540
01541
01542
01543
01544
01545
01546
01547
01548
01549
01550
01551
01552
01553
01554
01555
01556
01557
01558
01559
01560
01561
01562
01563
01564
01565
01566
01567
01568
01569
01570
01571
01572
01573
01574
01575
01576
01577
01578
01579
01580
01581
01582
01583
01584
01585
01586
01587
01588
01589
01590
01591
01592
01593
01594
01595
01596
01597
01598
01599
01600
01601
01602
01603
01604
01605
01606
01607
01608
01609
01610
01611
01612
01613
01614
01615
01616
01617
01618
01619
01620
01621
01622
01623
01624
01625
01626
01627
01628
01629
01630
01631
01632
01633
01634
01635
01636
01637
01638
01639
01640
01641
01642
01643
01644
01645
01646
01647
01648
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672
01673
01674
01675
01676
01677
01678
01679
01680
01681
01682
01683
01684 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01685
01686 int nxreal = nx - 2;
01687 if (nxreal != ny)
01688 throw ImageDimensionException("extractpoint requires ny == nx");
01689 int nhalf = nxreal/2;
01690 bool flip = (nuxnew < 0.f);
01691 if (flip) {
01692 nuxnew *= -1;
01693 nuynew *= -1;
01694 }
01695 if (nuynew >= nhalf-0.5) {
01696 nuynew -= nxreal;
01697 } else if (nuynew < -nhalf-0.5) {
01698 nuynew += nxreal;
01699 }
01700
01701
01702
01703
01704 int ixn = int(Util::round(nuxnew));
01705 int iyn = int(Util::round(nuynew));
01706
01707
01708 static float wy[7];
01709 static float wx[7];
01710
01711 float iynn = nuynew - iyn;
01712 wy[0] = kb.i0win_tab(iynn+3);
01713 wy[1] = kb.i0win_tab(iynn+2);
01714 wy[2] = kb.i0win_tab(iynn+1);
01715 wy[3] = kb.i0win_tab(iynn);
01716 wy[4] = kb.i0win_tab(iynn-1);
01717 wy[5] = kb.i0win_tab(iynn-2);
01718 wy[6] = kb.i0win_tab(iynn-3);
01719
01720 float ixnn = nuxnew - ixn;
01721 wx[0] = kb.i0win_tab(ixnn+3);
01722 wx[1] = kb.i0win_tab(ixnn+2);
01723 wx[2] = kb.i0win_tab(ixnn+1);
01724 wx[3] = kb.i0win_tab(ixnn);
01725 wx[4] = kb.i0win_tab(ixnn-1);
01726 wx[5] = kb.i0win_tab(ixnn-2);
01727 wx[6] = kb.i0win_tab(ixnn-3);
01728
01729 float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01730
01731 complex<float> result(0.f,0.f);
01732 if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01733
01734 for (int iy = 0; iy < 7; iy++) {
01735 int iyp = iyn + iy - 3 ;
01736 for (int ix = 0; ix < 7; ix++) {
01737 int ixp = ixn + ix - 3;
01738 float w = wx[ix]*wy[iy];
01739 complex<float> val = fimage->cmplx(ixp,iyp);
01740 result += val*w;
01741 }
01742 }
01743 } else {
01744
01745 for (int iy = 0; iy < 7; iy++) {
01746 int iyp = iyn + iy - 3;
01747 for (int ix = 0; ix < 7; ix++) {
01748 int ixp = ixn + ix - 3;
01749 bool mirror = false;
01750 int ixt = ixp, iyt = iyp;
01751 if (ixt < 0) {
01752 ixt = -ixt;
01753 iyt = -iyt;
01754 mirror = !mirror;
01755 }
01756 if (ixt > nhalf) {
01757 ixt = nxreal - ixt;
01758 iyt = -iyt;
01759 mirror = !mirror;
01760 }
01761 if (iyt > nhalf-1) iyt -= nxreal;
01762 if (iyt < -nhalf) iyt += nxreal;
01763 float w = wx[ix]*wy[iy];
01764 complex<float> val = fimage->cmplx(ixt,iyt);
01765 if (mirror) result += conj(val)*w;
01766 else result += val*w;
01767 }
01768 }
01769 }
01770 if (flip) result = conj(result)/wsum;
01771 else result /= wsum;
01772 return result;
01773 }
01774
01775
01776
01777
01778
01779
01780
01781
01782
01783
01784
01785
01786
01787
01788
01789
01790
01791
01792
01793
01794
01795
01796
01797
01798
01799
01800
01801
01802
01803
01804
01805
01806
01807
01808
01809
01810
01811
01812
01813
01814
01815
01816
01817
01818
01819
01820
01821
01822
01823
01824
01825
01826
01827
01828
01829
01830
01831
01832
01833
01834
01835
01836
01837
01838
01839
01840
01841
01842
01843
01844
01845
01846
01847
01848
01849
01850
01851
01852
01853
01854
01855
01856
01857
01858
01859
01860
01861
01862
01863
01864
01865
01866
01867
01868
01869
01870
01871
01872
01873
01874
01875
01876
01877
01878
01879
01880
01881
01882
01883
01884
01885
01886
01887
01888
01889
01890
01891
01892
01893
01894
01895
01896
01897
01898
01899
01900
01901
01902
01903 float Util::triquad(float R, float S, float T, float* fdata)
01904 {
01905
01906 const float C2 = 0.5f;
01907 const float C4 = 0.25f;
01908 const float C8 = 0.125f;
01909
01910 float RS = R * S;
01911 float ST = S * T;
01912 float RT = R * T;
01913 float RST = R * ST;
01914
01915 float RSQ = 1-R*R;
01916 float SSQ = 1-S*S;
01917 float TSQ = 1-T*T;
01918
01919 float RM1 = (1-R);
01920 float SM1 = (1-S);
01921 float TM1 = (1-T);
01922
01923 float RP1 = (1+R);
01924 float SP1 = (1+S);
01925 float TP1 = (1+T);
01926
01927 float triquad =
01928 (-C8) * RST * RM1 * SM1 * TM1 * fdata[0] +
01929 ( C4) * ST * RSQ * SM1 * TM1 * fdata[1] +
01930 ( C8) * RST * RP1 * SM1 * TM1 * fdata[2] +
01931 ( C4) * RT * RM1 * SSQ * TM1 * fdata[3] +
01932 (-C2) * T * RSQ * SSQ * TM1 * fdata[4] +
01933 (-C4) * RT * RP1 * SSQ * TM1 * fdata[5] +
01934 ( C8) * RST * RM1 * SP1 * TM1 * fdata[6] +
01935 (-C4) * ST * RSQ * SP1 * TM1 * fdata[7] +
01936 (-C8) * RST * RP1 * SP1 * TM1 * fdata[8] +
01937
01938 ( C4) * RS * RM1 * SM1 * TSQ * fdata[9] +
01939 (-C2) * S * RSQ * SM1 * TSQ * fdata[10] +
01940 (-C4) * RS * RP1 * SM1 * TSQ * fdata[11] +
01941 (-C2) * R * RM1 * SSQ * TSQ * fdata[12] +
01942 RSQ * SSQ * TSQ * fdata[13] +
01943 ( C2) * R * RP1 * SSQ * TSQ * fdata[14] +
01944 (-C4) * RS * RM1 * SP1 * TSQ * fdata[15] +
01945 ( C2) * S * RSQ * SP1 * TSQ * fdata[16] +
01946 ( C4) * RS * RP1 * SP1 * TSQ * fdata[17] +
01947
01948 ( C8) * RST * RM1 * SM1 * TP1 * fdata[18] +
01949 (-C4) * ST * RSQ * SM1 * TP1 * fdata[19] +
01950 (-C8) * RST * RP1 * SM1 * TP1 * fdata[20] +
01951 (-C4) * RT * RM1 * SSQ * TP1 * fdata[21] +
01952 ( C2) * T * RSQ * SSQ * TP1 * fdata[22] +
01953 ( C4) * RT * RP1 * SSQ * TP1 * fdata[23] +
01954 (-C8) * RST * RM1 * SP1 * TP1 * fdata[24] +
01955 ( C4) * ST * RSQ * SP1 * TP1 * fdata[25] +
01956 ( C8) * RST * RP1 * SP1 * TP1 * fdata[26] ;
01957 return triquad;
01958 }
01959
01960 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01961 : M(M_), fc(fc_), ntable(ntable_) {
01962
01963 build_sBtable();
01964 }
01965
01966 void Util::sincBlackman::build_sBtable() {
01967 sBtable.resize(ntable+1);
01968 int ltab = int(round(float(ntable)/1.25f));
01969 int M2 = M/2;
01970 fltb = float(ltab)/M2;
01971 for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01972 float x = 1.0e-7f;
01973 sBtable[0] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01974 for (int i=1; i <= ltab; i++) {
01975 x = float(i)/fltb;
01976 sBtable[i] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01977
01978 }
01979 }
01980
01981 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01982 int N_, float vtable_, int ntable_)
01983 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01984 ntable(ntable_) {
01985
01986 if (0.f == v) v = float(K)/2;
01987 if (0.f == vtable) vtable = v;
01988 alphar = alpha*r;
01989 fac = static_cast<float>(twopi)*alphar*v;
01990 vadjust = 1.0f*v;
01991 facadj = static_cast<float>(twopi)*alphar*vadjust;
01992 build_I0table();
01993 }
01994
01995 float Util::KaiserBessel::i0win(float x) const {
01996 float val0 = float(gsl_sf_bessel_I0(facadj));
01997 float absx = fabs(x);
01998 if (absx > vadjust) return 0.f;
01999 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02000 float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02001 return res;
02002 }
02003
02004 void Util::KaiserBessel::build_I0table() {
02005 i0table.resize(ntable+1);
02006 int ltab = int(round(float(ntable)/1.25f));
02007 fltb = float(ltab)/(K/2);
02008 float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02009 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02010 for (int i=0; i <= ltab; i++) {
02011 float s = float(i)/fltb/N;
02012 if (s < vadjust) {
02013 float rt = sqrt(1.f - pow(s/vadjust, 2));
02014 i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02015 } else {
02016 i0table[i] = 0.f;
02017 }
02018
02019 }
02020 }
02021
02022 float Util::KaiserBessel::I0table_maxerror() {
02023 float maxdiff = 0.f;
02024 for (int i = 1; i <= ntable; i++) {
02025 float diff = fabs(i0table[i] - i0table[i-1]);
02026 if (diff > maxdiff) maxdiff = diff;
02027 }
02028 return maxdiff;
02029 }
02030
02031 float Util::KaiserBessel::sinhwin(float x) const {
02032 float val0 = sinh(fac)/fac;
02033 float absx = fabs(x);
02034 if (0.0 == x) {
02035 float res = 1.0f;
02036 return res;
02037 } else if (absx == alphar) {
02038 return 1.0f/val0;
02039 } else if (absx < alphar) {
02040 float rt = sqrt(1.0f - pow((x/alphar), 2));
02041 float facrt = fac*rt;
02042 float res = (sinh(facrt)/facrt)/val0;
02043 return res;
02044 } else {
02045 float rt = sqrt(pow((x/alphar),2) - 1.f);
02046 float facrt = fac*rt;
02047 float res = (sin(facrt)/facrt)/val0;
02048 return res;
02049 }
02050 }
02051
02052 float Util::FakeKaiserBessel::i0win(float x) const {
02053 float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02054 float absx = fabs(x);
02055 if (absx > vadjust) return 0.f;
02056 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02057 float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02058 return res;
02059 }
02060
02061 void Util::FakeKaiserBessel::build_I0table() {
02062 i0table.resize(ntable+1);
02063 int ltab = int(round(float(ntable)/1.1f));
02064 fltb = float(ltab)/(K/2);
02065 float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02066 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02067 for (int i=0; i <= ltab; i++) {
02068 float s = float(i)/fltb/N;
02069 if (s < vadjust) {
02070 float rt = sqrt(1.f - pow(s/vadjust, 2));
02071 i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02072 } else {
02073 i0table[i] = 0.f;
02074 }
02075 }
02076 }
02077
02078 float Util::FakeKaiserBessel::sinhwin(float x) const {
02079 float val0 = sinh(fac)/fac;
02080 float absx = fabs(x);
02081 if (0.0 == x) {
02082 float res = 1.0f;
02083 return res;
02084 } else if (absx == alphar) {
02085 return 1.0f/val0;
02086 } else if (absx < alphar) {
02087 float rt = sqrt(1.0f - pow((x/alphar), 2));
02088 float facrt = fac*rt;
02089 float res = (sinh(facrt)/facrt)/val0;
02090 return res;
02091 } else {
02092 float rt = sqrt(pow((x/alphar),2) - 1.f);
02093 float facrt = fac*rt;
02094 float res = (sin(facrt)/facrt)/val0;
02095 return res;
02096 }
02097 }
02098
02099 #if 0 // 1-st order KB window
02100 float Util::FakeKaiserBessel::sinhwin(float x) const {
02101
02102 float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02103 float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02104 float absx = fabs(x);
02105 if (0.0 == x) {
02106
02107 float res = val0;
02108 return res;
02109 } else if (absx == alphar) {
02110
02111 return prefix;
02112 } else if (absx < alphar) {
02113 float rt = sqrt(1.0f - pow((x/alphar), 2));
02114
02115 float facrt = facadj*rt;
02116
02117 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02118 return res;
02119 } else {
02120 float rt = sqrt(pow((x/alphar),2) - 1.f);
02121
02122 float facrt = facadj*rt;
02123
02124 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02125 return res;
02126 }
02127 }
02128 #endif // 0
02129
02130
02131
02132 #define circ(i) circ[i-1]
02133 #define numr(i,j) numr[(j-1)*3 + i-1]
02134 #define xim(i,j) xim[(j-1)*nsam + i-1]
02135
02136 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02137 int nsam = image->get_xsize();
02138 int nrow = image->get_ysize();
02139 int nring = numr.size()/3;
02140 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02141 EMData* out = new EMData();
02142 out->set_size(lcirc,1,1);
02143 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02144 float *xim = image->get_data();
02145 float *circ = out->get_data();
02146
02147
02148
02149
02150
02151
02152
02153
02154
02155
02156
02157
02158
02159
02160
02161
02162 double dfi, dpi;
02163 int ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02164 float yq, xold, yold, fi, x, y;
02165
02166 ns2 = nsam/2+1;
02167 nr2 = nrow/2+1;
02168 dpi = 2.0*atan(1.0);
02169
02170 for (i=1;i<=nring;i++) {
02171
02172 inr = numr(1,i);
02173 yq = static_cast<float>(inr);
02174 l = numr(3,i);
02175 if (mode == 'h' || mode == 'H') lt = l/2;
02176 else lt = l/4;
02177
02178 nsim = lt-1;
02179 dfi = dpi/(nsim+1);
02180 kcirc = numr(2,i);
02181 xold = 0.0f;
02182 yold = static_cast<float>(inr);
02183 circ(kcirc) = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02184 xold = static_cast<float>(inr);
02185 yold = 0.0f;
02186 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02187
02188 if (mode == 'f' || mode == 'F') {
02189 xold = 0.0f;
02190 yold = static_cast<float>(-inr);
02191 circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02192 xold = static_cast<float>(-inr);
02193 yold = 0.0f;
02194 circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02195 }
02196
02197 for (j=1;j<=nsim;j++) {
02198 fi = static_cast<float>(dfi*j);
02199 x = sin(fi)*yq;
02200 y = cos(fi)*yq;
02201 xold = x;
02202 yold = y;
02203 circ(j+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02204 xold = y;
02205 yold = -x;
02206 circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02207
02208 if (mode == 'f' || mode == 'F') {
02209 xold = -x;
02210 yold = -y;
02211 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02212 xold = -y;
02213 yold = x;
02214 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02215 }
02216 }
02217 }
02218 return out;
02219 }
02220
02221 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02222 int nsam = image->get_xsize();
02223 int nrow = image->get_ysize();
02224 int nring = numr.size()/3;
02225 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02226 EMData* out = new EMData();
02227 out->set_size(lcirc,1,1);
02228 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02229 float *xim = image->get_data();
02230 float *circ = out->get_data();
02231 double dpi, dfi;
02232 int it, jt, inr, l, nsim, kcirc, lt;
02233 float xold, yold, fi, x, y;
02234
02235
02236
02237 dpi = 2*atan(1.0);
02238 for (it=1; it<=nring; it++) {
02239
02240 inr = numr(1,it);
02241
02242
02243
02244
02245 l = numr(3,it);
02246 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02247 else lt = l / 4;
02248
02249 nsim = lt - 1;
02250 dfi = dpi / (nsim+1);
02251 kcirc = numr(2,it);
02252 xold = 0.0f+cns2;
02253 yold = inr+cnr2;
02254
02255 Assert( kcirc <= lcirc );
02256 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02257
02258 xold = inr+cns2;
02259 yold = 0.0f+cnr2;
02260 Assert( lt+kcirc <= lcirc );
02261 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02262
02263 if ( mode == 'f' || mode == 'F' ) {
02264 xold = 0.0f+cns2;
02265 yold = -inr+cnr2;
02266 Assert( lt+lt+kcirc <= lcirc );
02267 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02268
02269 xold = -inr+cns2;
02270 yold = 0.0f+cnr2;
02271 Assert(lt+lt+lt+kcirc <= lcirc );
02272 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02273 }
02274
02275 for (jt=1; jt<=nsim; jt++) {
02276 fi = static_cast<float>(dfi * jt);
02277 x = sin(fi) * inr;
02278 y = cos(fi) * inr;
02279
02280 xold = x+cns2;
02281 yold = y+cnr2;
02282
02283 Assert( jt+kcirc <= lcirc );
02284 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02285
02286 xold = y+cns2;
02287 yold = -x+cnr2;
02288
02289 Assert( jt+lt+kcirc <= lcirc );
02290 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02291
02292 if ( mode == 'f' || mode == 'F' ) {
02293 xold = -x+cns2;
02294 yold = -y+cnr2;
02295
02296 Assert( jt+lt+lt+kcirc <= lcirc );
02297 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02298
02299 xold = -y+cns2;
02300 yold = x+cnr2;
02301
02302 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02303 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02304 }
02305 }
02306 }
02307 return out;
02308 }
02309
02310 float Util::bilinear(float xold, float yold, int nsam, int, float* xim)
02311 {
02312
02313
02314
02315
02316 float bilinear;
02317 int ixold, iyold;
02318
02319
02320
02321
02322
02323
02324
02325
02326
02327
02328
02329
02330
02331
02332
02333
02334
02335
02336
02337
02338
02339
02340
02341 float xdif, ydif;
02342
02343 ixold = (int) xold;
02344 iyold = (int) yold;
02345 ydif = yold - iyold;
02346
02347
02348
02349
02350
02351 xdif = xold - ixold;
02352 bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02353 xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02354 ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02355
02356 return bilinear;
02357 }
02358
02359 void Util::alrl_ms(float *xim, int nsam, int nrow, float cns2, float cnr2,
02360 int *numr, float *circ, int , int nring, char mode) {
02361 double dpi, dfi;
02362 int it, jt, inr, l, nsim, kcirc, lt;
02363 float xold, yold, fi, x, y;
02364
02365
02366
02367
02368 dpi = 2*atan(1.0);
02369 for (it=1; it<=nring; it++) {
02370
02371 inr = numr(1,it);
02372
02373 l = numr(3,it);
02374 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02375 else lt = l / 4;
02376
02377 nsim = lt - 1;
02378 dfi = dpi / (nsim+1);
02379 kcirc = numr(2,it);
02380
02381
02382 xold = 0.0f+cns2;
02383 yold = inr+cnr2;
02384
02385 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02386
02387 xold = inr+cns2;
02388 yold = 0.0f+cnr2;
02389 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02390
02391 if ( mode == 'f' || mode == 'F' ) {
02392 xold = 0.0f+cns2;
02393 yold = -inr+cnr2;
02394 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02395
02396 xold = -inr+cns2;
02397 yold = 0.0f+cnr2;
02398 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02399 }
02400
02401 for (jt=1; jt<=nsim; jt++) {
02402 fi = static_cast<float>(dfi * jt);
02403 x = sin(fi) * inr;
02404 y = cos(fi) * inr;
02405
02406 xold = x+cns2;
02407 yold = y+cnr2;
02408 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02409
02410 xold = y+cns2;
02411 yold = -x+cnr2;
02412 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02413
02414 if ( mode == 'f' || mode == 'F' ) {
02415 xold = -x+cns2;
02416 yold = -y+cnr2;
02417 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02418
02419 xold = -y+cns2;
02420 yold = x+cnr2;
02421 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02422 }
02423 }
02424 }
02425 }
02426
02427
02428
02429
02430
02431
02432
02433
02434
02435
02436
02437
02438
02439
02440
02441
02442
02443
02444
02445
02446
02447
02448
02449
02450
02451
02452
02453
02454
02455
02456
02457
02458
02459
02460
02461
02462
02463
02464
02465
02466
02467
02468
02469
02470
02471
02472
02473
02474
02475
02476
02477
02478
02479
02480
02481
02482
02483
02484
02485
02486
02487
02488
02489
02490
02491
02492
02493
02494
02495
02496
02497
02498
02499
02500
02501
02502 #undef xim
02503
02504 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02505
02506 int nring = numr.size()/3;
02507 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02508 EMData* out = new EMData();
02509 out->set_size(lcirc,1,1);
02510 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02511 float *circ = out->get_data();
02512 float *fimage = image->get_data();
02513 int nx = image->get_xsize();
02514 int ny = image->get_ysize();
02515 int nz = image->get_zsize();
02516 double dpi, dfi;
02517 int it, jt, inr, l, nsim, kcirc, lt;
02518 float yq, xold, yold, fi, x, y;
02519
02520
02521
02522
02523 dpi = 2*atan(1.0);
02524 for (it=1;it<=nring;it++) {
02525
02526 inr = numr(1,it);
02527 yq = static_cast<float>(inr);
02528
02529 l = numr(3,it);
02530 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02531 else lt = l / 4;
02532
02533 nsim = lt - 1;
02534 dfi = dpi / (nsim+1);
02535 kcirc = numr(2,it);
02536 xold = 0.0f;
02537 yold = static_cast<float>(inr);
02538 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02539
02540
02541 xold = static_cast<float>(inr);
02542 yold = 0.0f;
02543 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02544
02545
02546 if ( mode == 'f' || mode == 'F' ) {
02547 xold = 0.0f;
02548 yold = static_cast<float>(-inr);
02549 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02550
02551
02552 xold = static_cast<float>(-inr);
02553 yold = 0.0f;
02554 circ(lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02555
02556 }
02557
02558 for (jt=1;jt<=nsim;jt++) {
02559 fi = static_cast<float>(dfi * jt);
02560 x = sin(fi) * yq;
02561 y = cos(fi) * yq;
02562
02563 xold = x;
02564 yold = y;
02565 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02566
02567
02568 xold = y;
02569 yold = -x;
02570 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02571
02572
02573 if ( mode == 'f' || mode == 'F' ) {
02574 xold = -x;
02575 yold = -y;
02576 circ(jt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02577
02578
02579 xold = -y;
02580 yold = x;
02581 circ(jt+lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02582
02583 }
02584 }
02585 }
02586 return out;
02587 }
02588
02589
02590
02591
02592
02593
02594
02595
02596
02597
02598
02599
02600
02601
02602
02603
02604
02605
02606
02607
02608
02609
02610
02611
02612 #define tab1(i) tab1[i-1]
02613 #define xcmplx(i,j) xcmplx [(j-1)*2 + i-1]
02614 #define br(i) br[i-1]
02615 #define bi(i) bi[i-1]
02616
02617 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02618 {
02619 double rni,sgn,tr1,tr2,ti1,ti2;
02620 double cc,c,ss,s,t,x2,x3,x4,x5;
02621 int b3,b4,b5,b6,b7,b56;
02622 int n, k, l, j, i, ix0, ix1, status=0;
02623
02624 const double tab1[] = {
02625 9.58737990959775e-5,
02626 1.91747597310703e-4,
02627 3.83495187571395e-4,
02628 7.66990318742704e-4,
02629 1.53398018628476e-3,
02630 3.06795676296598e-3,
02631 6.13588464915449e-3,
02632 1.22715382857199e-2,
02633 2.45412285229123e-2,
02634 4.90676743274181e-2,
02635 9.80171403295604e-2,
02636 1.95090322016128e-1,
02637 3.82683432365090e-1,
02638 7.07106781186546e-1,
02639 1.00000000000000,
02640 };
02641
02642 n=(int)pow(2.0f,ln);
02643
02644 k=abs(ks);
02645 l=16-ln;
02646 b3=n*k;
02647 b6=b3;
02648 b7=k;
02649 if (ks > 0) {
02650 sgn=1.0f;
02651 } else {
02652 sgn=-1.0f;
02653 rni=1.0f/(float)(n);
02654 j=1;
02655 for (i=1; i<=n; i++) {
02656 br(j)=br(j)*rni;
02657 bi(j)=bi(j)*rni;
02658 j=j+k;
02659 }
02660 }
02661
02662 L12:
02663 b6=b6/2;
02664 b5=b6;
02665 b4=2*b6;
02666 b56=b5-b6;
02667
02668 L14:
02669 tr1=br(b5+1);
02670 ti1=bi(b5+1);
02671 tr2=br(b56+1);
02672 ti2=bi(b56+1);
02673
02674 br(b5+1)=tr2-tr1;
02675 bi(b5+1)=ti2-ti1;
02676 br(b56+1)=tr1+tr2;
02677 bi(b56+1)=ti1+ti2;
02678
02679 b5=b5+b4;
02680 b56=b5-b6;
02681 if ( b5 <= b3 ) goto L14;
02682 if ( b6 == b7 ) goto L20;
02683
02684 b4=b7;
02685 cc=2.0f*pow(tab1(l),2);
02686 c=1.0f-cc;
02687 l++;
02688 ss=sgn*tab1(l);
02689 s=ss;
02690
02691 L16:
02692 b5=b6+b4;
02693 b4=2*b6;
02694 b56=b5-b6;
02695
02696 L18:
02697 tr1=br(b5+1);
02698 ti1=bi(b5+1);
02699 tr2=br(b56+1);
02700 ti2=bi(b56+1);
02701 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02702 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02703 br(b56+1)=tr1+tr2;
02704 bi(b56+1)=ti1+ti2;
02705
02706 b5=b5+b4;
02707 b56=b5-b6;
02708 if ( b5 <= b3 ) goto L18;
02709 b4=b5-b6;
02710 b5=b4-b3;
02711 c=-c;
02712 b4=b6-b5;
02713 if ( b5 < b4 ) goto L16;
02714 b4=b4+b7;
02715 if ( b4 >= b5 ) goto L12;
02716
02717 t=c-cc*c-ss*s;
02718 s=s+ss*c-cc*s;
02719 c=t;
02720 goto L16;
02721
02722 L20:
02723 ix0=b3/2;
02724 b3=b3-b7;
02725 b4=0;
02726 b5=0;
02727 b6=ix0;
02728 ix1=0;
02729 if (b6 == b7) goto EXIT;
02730
02731 L22:
02732 b4=b3-b4;
02733 b5=b3-b5;
02734 x2=br(b4+1);
02735 x3=br(b5+1);
02736 x4=bi(b4+1);
02737 x5=bi(b5+1);
02738 br(b4+1)=x3;
02739 br(b5+1)=x2;
02740 bi(b4+1)=x5;
02741 bi(b5+1)=x4;
02742 if(b6 < b4) goto L22;
02743
02744 L24:
02745 b4=b4+b7;
02746 b5=b6+b5;
02747 x2=br(b4+1);
02748 x3=br(b5+1);
02749 x4=bi(b4+1);
02750 x5=bi(b5+1);
02751 br(b4+1)=x3;
02752 br(b5+1)=x2;
02753 bi(b4+1)=x5;
02754 bi(b5+1)=x4;
02755 ix0=b6;
02756
02757 L26:
02758 ix0=ix0/2;
02759 ix1=ix1-ix0;
02760 if( ix1 >= 0) goto L26;
02761
02762 ix0=2*ix0;
02763 b4=b4+b7;
02764 ix1=ix1+ix0;
02765 b5=ix1;
02766 if ( b5 >= b4) goto L22;
02767 if ( b4 < b6) goto L24;
02768
02769 EXIT:
02770 status = 0;
02771 }
02772
02773
02774 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02775 {
02776
02777
02778 int b3,b4,b5,b6,b7,b56;
02779 int n, k, l, j, i, ix0, ix1;
02780 float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02781 int status=0;
02782
02783 const float tab1[] = {
02784 9.58737990959775e-5f,
02785 1.91747597310703e-4f,
02786 3.83495187571395e-4f,
02787 7.66990318742704e-4f,
02788 1.53398018628476e-3f,
02789 3.06795676296598e-3f,
02790 6.13588464915449e-3f,
02791 1.22715382857199e-2f,
02792 2.45412285229123e-2f,
02793 4.90676743274181e-2f,
02794 9.80171403295604e-2f,
02795 1.95090322016128e-1f,
02796 3.82683432365090e-1f,
02797 7.07106781186546e-1f,
02798 1.00000000000000f,
02799 };
02800
02801 n=(int)pow(2.0f,ln);
02802
02803 k=abs(ks);
02804 l=16-ln;
02805 b3=n*k;
02806 b6=b3;
02807 b7=k;
02808 if( ks > 0 ) {
02809 sgn=1.0f;
02810 } else {
02811 sgn=-1.0f;
02812 rni=1.0f/(float)n;
02813 j=1;
02814 for (i=1; i<=n; i++) {
02815 br(j)=br(j)*rni;
02816 bi(j)=bi(j)*rni;
02817 j=j+k;
02818 }
02819 }
02820 L12:
02821 b6=b6/2;
02822 b5=b6;
02823 b4=2*b6;
02824 b56=b5-b6;
02825 L14:
02826 tr1=br(b5+1);
02827 ti1=bi(b5+1);
02828
02829 tr2=br(b56+1);
02830 ti2=bi(b56+1);
02831
02832 br(b5+1)=tr2-tr1;
02833 bi(b5+1)=ti2-ti1;
02834 br(b56+1)=tr1+tr2;
02835 bi(b56+1)=ti1+ti2;
02836
02837 b5=b5+b4;
02838 b56=b5-b6;
02839 if ( b5 <= b3 ) goto L14;
02840 if ( b6 == b7 ) goto L20;
02841
02842 b4=b7;
02843 cc=2.0f*pow(tab1(l),2);
02844 c=1.0f-cc;
02845 l++;
02846 ss=sgn*tab1(l);
02847 s=ss;
02848 L16:
02849 b5=b6+b4;
02850 b4=2*b6;
02851 b56=b5-b6;
02852 L18:
02853 tr1=br(b5+1);
02854 ti1=bi(b5+1);
02855 tr2=br(b56+1);
02856 ti2=bi(b56+1);
02857 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02858 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02859 br(b56+1)=tr1+tr2;
02860 bi(b56+1)=ti1+ti2;
02861
02862 b5=b5+b4;
02863 b56=b5-b6;
02864 if(b5 <= b3) goto L18;
02865 b4=b5-b6;
02866 b5=b4-b3;
02867 c=-c;
02868 b4=b6-b5;
02869 if(b5 < b4) goto L16;
02870 b4=b4+b7;
02871 if(b4 >= b5) goto L12;
02872
02873 t=c-cc*c-ss*s;
02874 s=s+ss*c-cc*s;
02875 c=t;
02876 goto L16;
02877 L20:
02878 ix0=b3/2;
02879 b3=b3-b7;
02880 b4=0;
02881 b5=0;
02882 b6=ix0;
02883 ix1=0;
02884 if ( b6 == b7) goto EXIT;
02885 L22:
02886 b4=b3-b4;
02887 b5=b3-b5;
02888 x2=br(b4+1);
02889 x3=br(b5+1);
02890 x4=bi(b4+1);
02891 x5=bi(b5+1);
02892 br(b4+1)=x3;
02893 br(b5+1)=x2;
02894 bi(b4+1)=x5;
02895 bi(b5+1)=x4;
02896 if (b6 < b4) goto L22;
02897 L24:
02898 b4=b4+b7;
02899 b5=b6+b5;
02900 x2=br(b4+1);
02901 x3=br(b5+1);
02902 x4=bi(b4+1);
02903 x5=bi(b5+1);
02904 br(b4+1)=x3;
02905 br(b5+1)=x2;
02906 bi(b4+1)=x5;
02907 bi(b5+1)=x4;
02908 ix0=b6;
02909 L26:
02910 ix0=ix0/2;
02911 ix1=ix1-ix0;
02912 if(ix1 >= 0) goto L26;
02913
02914 ix0=2*ix0;
02915 b4=b4+b7;
02916 ix1=ix1+ix0;
02917 b5=ix1;
02918 if (b5 >= b4) goto L22;
02919 if (b4 < b6) goto L24;
02920 EXIT:
02921 status = 0;
02922 }
02923
02924 void Util::fftr_q(float *xcmplx, int nv)
02925 {
02926
02927
02928 int nu, inv, nu1, n, isub, n2, i1, i2, i;
02929 float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02930
02931 const float tab1[] = {
02932 9.58737990959775e-5f,
02933 1.91747597310703e-4f,
02934 3.83495187571395e-4f,
02935 7.66990318742704e-4f,
02936 1.53398018628476e-3f,
02937 3.06795676296598e-3f,
02938 6.13588464915449e-3f,
02939 1.22715382857199e-2f,
02940 2.45412285229123e-2f,
02941 4.90676743274181e-2f,
02942 9.80171403295604e-2f,
02943 1.95090322016128e-1f,
02944 3.82683432365090e-1f,
02945 7.07106781186546e-1f,
02946 1.00000000000000f,
02947 };
02948
02949 nu=abs(nv);
02950 inv=nv/nu;
02951 nu1=nu-1;
02952 n=(int)pow(2.f,nu1);
02953 isub=16-nu1;
02954
02955 ss=-tab1(isub);
02956 cc=-2.0f*pow(tab1(isub-1),2.f);
02957 c=1.0f;
02958 s=0.0f;
02959 n2=n/2;
02960 if ( inv > 0) {
02961 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02962 tr=xcmplx(1,1);
02963 ti=xcmplx(2,1);
02964 xcmplx(1,1)=tr+ti;
02965 xcmplx(2,1)=tr-ti;
02966 for (i=1;i<=n2;i++) {
02967 i1=i+1;
02968 i2=n-i+1;
02969 tr1=xcmplx(1,i1);
02970 tr2=xcmplx(1,i2);
02971 ti1=xcmplx(2,i1);
02972 ti2=xcmplx(2,i2);
02973 t=(cc*c-ss*s)+c;
02974 s=(cc*s+ss*c)+s;
02975 c=t;
02976 xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02977 xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02978 xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02979 xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02980 }
02981 } else {
02982 tr=xcmplx(1,1);
02983 ti=xcmplx(2,1);
02984 xcmplx(1,1)=0.5f*(tr+ti);
02985 xcmplx(2,1)=0.5f*(tr-ti);
02986 for (i=1; i<=n2; i++) {
02987 i1=i+1;
02988 i2=n-i+1;
02989 tr1=xcmplx(1,i1);
02990 tr2=xcmplx(1,i2);
02991 ti1=xcmplx(2,i1);
02992 ti2=xcmplx(2,i2);
02993 t=(cc*c-ss*s)+c;
02994 s=(cc*s+ss*c)+s;
02995 c=t;
02996 xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
02997 xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
02998 xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
02999 xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03000 }
03001 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03002 }
03003 }
03004
03005
03006 void Util::fftr_d(double *xcmplx, int nv)
03007 {
03008
03009 int i1, i2, nu, inv, nu1, n, isub, n2, i;
03010 double tr1,tr2,ti1,ti2,tr,ti;
03011 double cc,c,ss,s,t;
03012 const double tab1[] = {
03013 9.58737990959775e-5,
03014 1.91747597310703e-4,
03015 3.83495187571395e-4,
03016 7.66990318742704e-4,
03017 1.53398018628476e-3,
03018 3.06795676296598e-3,
03019 6.13588464915449e-3,
03020 1.22715382857199e-2,
03021 2.45412285229123e-2,
03022 4.90676743274181e-2,
03023 9.80171403295604e-2,
03024 1.95090322016128e-1,
03025 3.82683432365090e-1,
03026 7.07106781186546e-1,
03027 1.00000000000000,
03028 };
03029
03030 nu=abs(nv);
03031 inv=nv/nu;
03032 nu1=nu-1;
03033 n=(int)pow(2.0f,nu1);
03034 isub=16-nu1;
03035 ss=-tab1(isub);
03036 cc=-2.0*pow(tab1(isub-1),2);
03037 c=1.0f;
03038 s=0.0f;
03039 n2=n/2;
03040
03041 if ( inv > 0 ) {
03042 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03043 tr=xcmplx(1,1);
03044 ti=xcmplx(2,1);
03045 xcmplx(1,1)=tr+ti;
03046 xcmplx(2,1)=tr-ti;
03047 for (i=1;i<=n2;i++) {
03048 i1=i+1;
03049 i2=n-i+1;
03050 tr1=xcmplx(1,i1);
03051 tr2=xcmplx(1,i2);
03052 ti1=xcmplx(2,i1);
03053 ti2=xcmplx(2,i2);
03054 t=(cc*c-ss*s)+c;
03055 s=(cc*s+ss*c)+s;
03056 c=t;
03057 xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03058 xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03059 xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03060 xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03061 }
03062 } else {
03063 tr=xcmplx(1,1);
03064 ti=xcmplx(2,1);
03065 xcmplx(1,1)=0.5*(tr+ti);
03066 xcmplx(2,1)=0.5*(tr-ti);
03067 for (i=1; i<=n2; i++) {
03068 i1=i+1;
03069 i2=n-i+1;
03070 tr1=xcmplx(1,i1);
03071 tr2=xcmplx(1,i2);
03072 ti1=xcmplx(2,i1);
03073 ti2=xcmplx(2,i2);
03074 t=(cc*c-ss*s)+c;
03075 s=(cc*s+ss*c)+s;
03076 c=t;
03077 xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03078 xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03079 xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03080 xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03081 }
03082 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03083 }
03084 }
03085 #undef tab1
03086 #undef xcmplx
03087 #undef br
03088 #undef bi
03089
03090
03091 void Util::Frngs(EMData* circp, vector<int> numr){
03092 int nring = numr.size()/3;
03093 float *circ = circp->get_data();
03094 int i, l;
03095 for (i=1; i<=nring;i++) {
03096
03097 #ifdef _WIN32
03098 l = (int)( log((float)numr(3,i))/log(2.0f) );
03099 #else
03100 l=(int)(log2(numr(3,i)));
03101 #endif //_WIN32
03102
03103 fftr_q(&circ(numr(2,i)),l);
03104 }
03105 }
03106
03107 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03108 int nring = numr.size()/3;
03109 float *circ = circp->get_data();
03110 int i, l;
03111 for (i=1; i<=nring;i++) {
03112
03113 #ifdef _WIN32
03114 l = (int)( log((float)numr(3,i))/log(2.0f) );
03115 #else
03116 l=(int)(log2(numr(3,i)));
03117 #endif //_WIN32
03118
03119 fftr_q(&circ(numr(2,i)),-l);
03120 }
03121 }
03122 #undef circ
03123
03124 #define b(i) b[i-1]
03125 void Util::prb1d(double *b, int npoint, float *pos) {
03126 double c2,c3;
03127 int nhalf;
03128
03129 nhalf = npoint/2 + 1;
03130 *pos = 0.0;
03131
03132 if (npoint == 7) {
03133 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03134 - 6.*b(6) + 31.*b(7);
03135 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03136 }
03137 else if (npoint == 5) {
03138 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03139 + 46.*b(5) ) / (-70.);
03140 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03141 }
03142 else if (npoint == 3) {
03143 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03144 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03145 }
03146
03147 else {
03148 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03149 - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03150 + 1092.*b(9) ) / (-4620.);
03151 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03152 - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03153 }
03154 if (c3 != 0.0) *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03155 }
03156 #undef b
03157
03158 #define circ1(i) circ1[i-1]
03159 #define circ2(i) circ2[i-1]
03160 #define t(i) t[i-1]
03161 #define q(i) q[i-1]
03162 #define b(i) b[i-1]
03163 #define t7(i) t7[i-1]
03164 Dict Util::Crosrng_e(EMData* circ1p, EMData* circ2p, vector<int> numr, int neg) {
03165
03166 int nring = numr.size()/3;
03167
03168 int maxrin = numr[numr.size()-1];
03169 double qn; float tot;
03170 float *circ1 = circ1p->get_data();
03171 float *circ2 = circ2p->get_data();
03172
03173
03174
03175
03176
03177
03178
03179
03180
03181
03182
03183 float *t;
03184 double t7[7], *q;
03185 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03186 float pos;
03187
03188 #ifdef _WIN32
03189 ip = -(int)(log((float)maxrin)/log(2.0f));
03190 #else
03191 ip = -(int) (log2(maxrin));
03192 #endif //_WIN32
03193
03194 q = (double*)calloc(maxrin, sizeof(double));
03195 t = (float*)calloc(maxrin, sizeof(float));
03196
03197
03198 for (i=1; i<=nring; i++) {
03199 numr3i = numr(3,i);
03200 numr2i = numr(2,i);
03201
03202 t(1) = (circ1(numr2i)) * circ2(numr2i);
03203
03204 if (numr3i != maxrin) {
03205
03206 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03207 t(2) = 0.0;
03208
03209 if (neg) {
03210
03211 for (j=3;j<=numr3i;j=j+2) {
03212 jc = j+numr2i-1;
03213 t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03214 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03215 }
03216 } else {
03217 for (j=3;j<=numr3i;j=j+2) {
03218 jc = j+numr2i-1;
03219 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03220 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03221 }
03222 }
03223 for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03224 } else {
03225 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03226 if (neg) {
03227
03228 for (j=3;j<=maxrin;j=j+2) {
03229 jc = j+numr2i-1;
03230 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03231 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03232 }
03233 } else {
03234 for (j=3;j<=maxrin;j=j+2) {
03235 jc = j+numr2i-1;
03236 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03237 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03238 }
03239 }
03240 for (j = 1; j <= maxrin; j++) q(j) += t(j);
03241 }
03242 }
03243
03244 fftr_d(q,ip);
03245
03246 qn = -1.0e20;
03247 for (j=1;j<=maxrin;j++) {
03248 if (q(j) >= qn) {
03249 qn = q(j); jtot = j;
03250 }
03251 }
03252
03253 for (k=-3; k<=3; k++) {
03254 j = (jtot+k+maxrin-1)%maxrin + 1;
03255 t7(k+4) = q(j);
03256 }
03257
03258 prb1d(t7,7,&pos);
03259
03260 tot = (float)jtot + pos;
03261
03262 if (q) free(q);
03263 if (t) free(t);
03264
03265 Dict retvals;
03266 retvals["qn"] = qn;
03267 retvals["tot"] = tot;
03268 return retvals;
03269 }
03270
03271 Dict Util::Crosrng_ew(EMData* circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03272
03273 int nring = numr.size()/3;
03274
03275 int maxrin = numr[numr.size()-1];
03276 double qn; float tot;
03277 float *circ1 = circ1p->get_data();
03278 float *circ2 = circ2p->get_data();
03279
03280
03281
03282
03283
03284
03285
03286
03287
03288
03289
03290 float *t;
03291 double t7[7], *q;
03292 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03293 float pos;
03294
03295 #ifdef _WIN32
03296 ip = -(int)(log((float)maxrin)/log(2.0f));
03297 #else
03298 ip = -(int) (log2(maxrin));
03299 #endif //_WIN32
03300
03301 q = (double*)calloc(maxrin, sizeof(double));
03302 t = (float*)calloc(maxrin, sizeof(float));
03303
03304
03305 for (i=1;i<=nring;i++) {
03306 numr3i = numr(3,i);
03307 numr2i = numr(2,i);
03308
03309 t(1) = circ1(numr2i) * circ2(numr2i);
03310
03311 if (numr3i != maxrin) {
03312
03313 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03314 t(2) = 0.0;
03315
03316 if (neg) {
03317
03318 for (j=3; j<=numr3i; j=j+2) {
03319 jc = j+numr2i-1;
03320 t(j) = (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03321 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03322 }
03323 } else {
03324 for (j=3; j<=numr3i; j=j+2) {
03325 jc = j+numr2i-1;
03326 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03327 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03328 }
03329 }
03330 for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03331 } else {
03332 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03333 if (neg) {
03334
03335 for (j=3; j<=maxrin; j=j+2) {
03336 jc = j+numr2i-1;
03337 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03338 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03339 }
03340 } else {
03341 for (j=3; j<=maxrin; j=j+2) {
03342 jc = j+numr2i-1;
03343 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03344 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03345 }
03346 }
03347 for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03348 }
03349 }
03350
03351 fftr_d(q,ip);
03352
03353 qn = -1.0e20;
03354 for (j=1;j<=maxrin;j++) {
03355
03356 if (q(j) >= qn) {
03357 qn = q(j);
03358 jtot = j;
03359 }
03360 }
03361
03362 for (k=-3; k<=3; k++) {
03363 j = (jtot+k+maxrin-1)%maxrin + 1;
03364 t7(k+4) = q(j);
03365 }
03366
03367 prb1d(t7,7,&pos);
03368
03369 tot = (float)jtot + pos;
03370
03371
03372 if (t) free(t);
03373
03374 Dict retvals;
03375
03376
03377 retvals["qn"] = qn;
03378 retvals["tot"] = tot;
03379
03380 if (q) free(q);
03381
03382 return retvals;
03383 }
03384
03385 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03386 int nring = numr.size()/3;
03387
03388 int maxrin = numr[numr.size()-1];
03389 double qn; float tot; double qm; float tmt;
03390 float *circ1 = circ1p->get_data();
03391 float *circ2 = circ2p->get_data();
03392
03393
03394
03395
03396
03397
03398
03399
03400
03401
03402
03403
03404 double *t, *q, t7[7];
03405
03406 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03407 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03408
03409 qn = 0.0f;
03410 qm = 0.0f;
03411 tot = 0.0f;
03412 tmt = 0.0f;
03413 #ifdef _WIN32
03414 ip = -(int)(log((float)maxrin)/log(2.0f));
03415 #else
03416 ip = -(int)(log2(maxrin));
03417 #endif //_WIN32
03418
03419
03420
03421
03422
03423 q = (double*)calloc(maxrin,sizeof(double));
03424
03425
03426
03427 t = (double*)calloc(maxrin,sizeof(double));
03428
03429
03430 for (i=1; i<=nring; i++) {
03431
03432 numr3i = numr(3,i);
03433 numr2i = numr(2,i);
03434
03435 t1 = circ1(numr2i) * circ2(numr2i);
03436 q(1) += t1;
03437 t(1) += t1;
03438
03439 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03440 if (numr3i == maxrin) {
03441 q(2) += t1;
03442 t(2) += t1;
03443 } else {
03444 q(numr3i+1) += t1;
03445 t(numr3i+1) += t1;
03446 }
03447
03448 for (j=3; j<=numr3i; j += 2) {
03449 jc = j+numr2i-1;
03450
03451
03452
03453
03454
03455
03456
03457
03458 c1 = circ1(jc);
03459 c2 = circ1(jc+1);
03460 d1 = circ2(jc);
03461 d2 = circ2(jc+1);
03462
03463 t1 = c1 * d1;
03464 t2 = c2 * d2;
03465 t3 = c1 * d2;
03466 t4 = c2 * d1;
03467
03468 q(j) += t1 + t2;
03469 q(j+1) += -t3 + t4;
03470 t(j) += t1 - t2;
03471 t(j+1) += -t3 - t4;
03472 }
03473 }
03474
03475 fftr_d(q,ip);
03476
03477 qn = -1.0e20;
03478 for (j=1; j<=maxrin; j++) {
03479 if (q(j) >= qn) {
03480 qn = q(j);
03481 jtot = j;
03482 }
03483 }
03484
03485 for (k=-3; k<=3; k++) {
03486 j = ((jtot+k+maxrin-1)%maxrin)+1;
03487 t7(k+4) = q(j);
03488 }
03489
03490
03491 prb1d(t7,7,&pos);
03492 tot = (float)(jtot)+pos;
03493
03494
03495
03496
03497 fftr_d(t,ip);
03498
03499
03500 qm = -1.0e20;
03501 for (j=1; j<=maxrin;j++) {
03502 if ( t(j) >= qm ) {
03503 qm = t(j);
03504 jtot = j;
03505 }
03506 }
03507
03508 for (k=-3; k<=3; k++) {
03509 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03510 t7(k+4) = t(j);
03511 }
03512
03513
03514
03515 prb1d(t7,7,&pos);
03516 tmt = float(jtot) + pos;
03517
03518
03519
03520 free(t);
03521 free(q);
03522
03523 Dict retvals;
03524 retvals["qn"] = qn;
03525 retvals["tot"] = tot;
03526 retvals["qm"] = qm;
03527 retvals["tmt"] = tmt;
03528 return retvals;
03529 }
03530
03531 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03532 int nring = numr.size()/3;
03533
03534 int maxrin = numr[numr.size()-1];
03535 double qn; float tot; double qm; float tmt;
03536 float *circ1 = circ1p->get_data();
03537 float *circ2 = circ2p->get_data();
03538
03539
03540
03541
03542
03543
03544
03545
03546
03547
03548
03549
03550 double *t, *q;
03551
03552 int ip, jc, numr3i, numr2i, i, j, jtot = 0;
03553 float t1, t2, t3, t4, c1, c2, d1, d2;
03554
03555 qn = 0.0f;
03556 qm = 0.0f;
03557 tot = 0.0f;
03558 tmt = 0.0f;
03559 #ifdef _WIN32
03560 ip = -(int)(log((float)maxrin)/log(2.0f));
03561 #else
03562 ip = -(int)(log2(maxrin));
03563 #endif //_WIN32
03564
03565
03566
03567
03568
03569 q = (double*)calloc(maxrin,sizeof(double));
03570
03571
03572
03573 t = (double*)calloc(maxrin,sizeof(double));
03574
03575
03576 for (i=1; i<=nring; i++) {
03577
03578 numr3i = numr(3,i);
03579 numr2i = numr(2,i);
03580
03581 t1 = circ1(numr2i) * circ2(numr2i);
03582 q(1) += t1;
03583 t(1) += t1;
03584
03585 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03586 if (numr3i == maxrin) {
03587 q(2) += t1;
03588 t(2) += t1;
03589 } else {
03590 q(numr3i+1) += t1;
03591 t(numr3i+1) += t1;
03592 }
03593
03594 for (j=3; j<=numr3i; j += 2) {
03595 jc = j+numr2i-1;
03596
03597
03598
03599
03600
03601
03602
03603
03604 c1 = circ1(jc);
03605 c2 = circ1(jc+1);
03606 d1 = circ2(jc);
03607 d2 = circ2(jc+1);
03608
03609 t1 = c1 * d1;
03610 t2 = c2 * d2;
03611 t3 = c1 * d2;
03612 t4 = c2 * d1;
03613
03614 q(j) += t1 + t2;
03615 q(j+1) += -t3 + t4;
03616 t(j) += t1 - t2;
03617 t(j+1) += -t3 - t4;
03618 }
03619 }
03620
03621 fftr_d(q,ip);
03622
03623 qn = -1.0e20;
03624
03625 int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03626 int jstep = static_cast<int>(delta/360.0*maxrin);
03627 if (jstep < 1) { jstep = 1; }
03628
03629 for (j=jstart; j<=maxrin; j+=jstep) {
03630 if (q(j) >= qn) {
03631 qn = q(j);
03632 jtot = j;
03633 }
03634 }
03635
03636
03637
03638
03639
03640
03641
03642
03643
03644
03645 tot = (float)(jtot);
03646
03647
03648 fftr_d(t,ip);
03649
03650
03651 qm = -1.0e20;
03652 for (j=jstart; j<=maxrin;j+=jstep) {
03653 if ( t(j) >= qm ) {
03654 qm = t(j);
03655 jtot = j;
03656 }
03657 }
03658
03659
03660
03661
03662
03663
03664
03665
03666
03667
03668
03669 tmt = float(jtot);
03670
03671 free(t);
03672 free(q);
03673
03674 Dict retvals;
03675 retvals["qn"] = qn;
03676 retvals["tot"] = tot;
03677 retvals["qm"] = qm;
03678 retvals["tmt"] = tmt;
03679 return retvals;
03680 }
03681
03682
03683 Dict Util::Crosrng_psi_0_180(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03684 int nring = numr.size()/3;
03685
03686 int maxrin = numr[numr.size()-1];
03687 double qn; float tot; double qm; float tmt;
03688 float *circ1 = circ1p->get_data();
03689 float *circ2 = circ2p->get_data();
03690
03691
03692
03693
03694 double *t, *q, t7[7];
03695
03696 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03697 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03698
03699 qn = 0.0f;
03700 qm = 0.0f;
03701 tot = 0.0f;
03702 tmt = 0.0f;
03703 #ifdef _WIN32
03704 ip = -(int)(log((float)maxrin)/log(2.0f));
03705 #else
03706 ip = -(int)(log2(maxrin));
03707 #endif //_WIN32
03708
03709
03710
03711
03712
03713 q = (double*)calloc(maxrin,sizeof(double));
03714
03715
03716
03717 t = (double*)calloc(maxrin,sizeof(double));
03718
03719
03720 for (i=1; i<=nring; i++) {
03721
03722 numr3i = numr(3,i);
03723 numr2i = numr(2,i);
03724
03725 t1 = circ1(numr2i) * circ2(numr2i);
03726 q(1) += t1;
03727 t(1) += t1;
03728
03729 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03730 if (numr3i == maxrin) {
03731 q(2) += t1;
03732 t(2) += t1;
03733 } else {
03734 q(numr3i+1) += t1;
03735 t(numr3i+1) += t1;
03736 }
03737
03738 for (j=3; j<=numr3i; j += 2) {
03739 jc = j+numr2i-1;
03740
03741
03742
03743
03744
03745
03746
03747
03748 c1 = circ1(jc);
03749 c2 = circ1(jc+1);
03750 d1 = circ2(jc);
03751 d2 = circ2(jc+1);
03752
03753 t1 = c1 * d1;
03754 t2 = c2 * d2;
03755 t3 = c1 * d2;
03756 t4 = c2 * d1;
03757
03758 q(j) += t1 + t2;
03759 q(j+1) += -t3 + t4;
03760 t(j) += t1 - t2;
03761 t(j+1) += -t3 - t4;
03762 }
03763 }
03764
03765 fftr_d(q,ip);
03766
03767 int psi_range = int(psi_max/360.0*maxrin+0.5);
03768 const int psi_0 = 0;
03769 int psi_180 = int( 180.0/360.0*maxrin+0.5);
03770
03771 qn = -1.0e20;
03772 for (k=-psi_range; k<=psi_range; k++) {
03773 j = (k+psi_0+maxrin-1)%maxrin+1;
03774 if (q(j) >= qn) {
03775 qn = q(j);
03776 jtot = j;
03777 }
03778 }
03779
03780 for (k=-psi_range; k<=psi_range; k++) {
03781 j = (k+psi_180+maxrin-1)%maxrin+1;
03782 if (q(j) >= qn) {
03783 qn = q(j);
03784 jtot = j;
03785 }
03786 }
03787
03788 for (k=-3; k<=3; k++) {
03789 j = ((jtot+k+maxrin-1)%maxrin)+1;
03790 t7(k+4) = q(j);
03791 }
03792
03793
03794 prb1d(t7,7,&pos);
03795 tot = (float)(jtot)+pos;
03796
03797
03798
03799
03800 fftr_d(t,ip);
03801
03802
03803 qm = -1.0e20;
03804 for (k=-psi_range; k<=psi_range; k++) {
03805 j = (k+psi_0+maxrin-1)%maxrin+1;
03806 if (t(j) >= qm) {
03807 qm = t(j);
03808 jtot = j;
03809 }
03810 }
03811
03812 for (k=-psi_range; k<=psi_range; k++) {
03813 j = (k+psi_180+maxrin-1)%maxrin+1;
03814 if (t(j) >= qm) {
03815 qm = t(j);
03816 jtot = j;
03817 }
03818 }
03819
03820 for (k=-3; k<=3; k++) {
03821 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03822 t7(k+4) = t(j);
03823 }
03824
03825
03826
03827 prb1d(t7,7,&pos);
03828 tmt = float(jtot) + pos;
03829
03830
03831
03832 free(t);
03833 free(q);
03834
03835 Dict retvals;
03836 retvals["qn"] = qn;
03837 retvals["tot"] = tot;
03838 retvals["qm"] = qm;
03839 retvals["tmt"] = tmt;
03840 return retvals;
03841 }
03842
03843 Dict Util::Crosrng_psi_0_180_no_mirror(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03844 int nring = numr.size()/3;
03845
03846 int maxrin = numr[numr.size()-1];
03847 double qn; float tot;
03848 float *circ1 = circ1p->get_data();
03849 float *circ2 = circ2p->get_data();
03850
03851
03852
03853
03854 double *q, t7[7];
03855
03856 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03857 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03858
03859 qn = 0.0f;
03860 tot = 0.0f;
03861 #ifdef _WIN32
03862 ip = -(int)(log((float)maxrin)/log(2.0f));
03863 #else
03864 ip = -(int)(log2(maxrin));
03865 #endif //_WIN32
03866
03867
03868
03869
03870
03871 q = (double*)calloc(maxrin,sizeof(double));
03872
03873
03874 for (i=1; i<=nring; i++) {
03875
03876 numr3i = numr(3,i);
03877 numr2i = numr(2,i);
03878
03879 t1 = circ1(numr2i) * circ2(numr2i);
03880 q(1) += t1;
03881
03882
03883 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03884 if (numr3i == maxrin) {
03885 q(2) += t1;
03886
03887 } else {
03888 q(numr3i+1) += t1;
03889 }
03890
03891 for (j=3; j<=numr3i; j += 2) {
03892 jc = j+numr2i-1;
03893
03894
03895
03896
03897
03898
03899
03900
03901 c1 = circ1(jc);
03902 c2 = circ1(jc+1);
03903 d1 = circ2(jc);
03904 d2 = circ2(jc+1);
03905
03906 t1 = c1 * d1;
03907 t2 = c2 * d2;
03908 t3 = c1 * d2;
03909 t4 = c2 * d1;
03910
03911 q(j) += t1 + t2;
03912 q(j+1) += -t3 + t4;
03913
03914 }
03915 }
03916
03917 fftr_d(q,ip);
03918
03919 int psi_range = int(psi_max/360.0*maxrin+0.5);
03920 const int psi_0 = 0;
03921 int psi_180 = int( 180.0/360.0*maxrin+0.5);
03922
03923 qn = -1.0e20;
03924 for (k=-psi_range; k<=psi_range; k++) {
03925 j = (k+psi_0+maxrin-1)%maxrin+1;
03926 if (q(j) >= qn) {
03927 qn = q(j);
03928 jtot = j;
03929 }
03930 }
03931
03932 for (k=-psi_range; k<=psi_range; k++) {
03933 j = (k+psi_180+maxrin-1)%maxrin+1;
03934 if (q(j) >= qn) {
03935 qn = q(j);
03936 jtot = j;
03937 }
03938 }
03939
03940 for (k=-3; k<=3; k++) {
03941 j = ((jtot+k+maxrin-1)%maxrin)+1;
03942 t7(k+4) = q(j);
03943 }
03944
03945
03946 prb1d(t7,7,&pos);
03947 tot = (float)(jtot)+pos;
03948
03949
03950
03951 free(q);
03952
03953 Dict retvals;
03954 retvals["qn"] = qn;
03955 retvals["tot"] = tot;
03956
03957 return retvals;
03958 }
03959
03960
03961
03962 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag) {
03963
03964
03965 int nring = numr.size()/3;
03966 int maxrin = numr[numr.size()-1];
03967 double qn; float tot; double qm; float tmt;
03968 float *circ1 = circ1p->get_data();
03969 float *circ2 = circ2p->get_data();
03970
03971 double *q, t7[7];
03972
03973 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03974 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03975
03976 qn = 0.0f;
03977 qm = 0.0f;
03978 tot = 0.0f;
03979 tmt = 0.0f;
03980 #ifdef _WIN32
03981 ip = -(int)(log((float)maxrin)/log(2.0f));
03982 #else
03983 ip = -(int)(log2(maxrin));
03984 #endif //_WIN32
03985
03986
03987
03988
03989 q = (double*)calloc(maxrin,sizeof(double));
03990
03991
03992 if (flag==0) {
03993 for (i=1; i<=nring; i++) {
03994
03995 numr3i = numr(3,i);
03996 numr2i = numr(2,i);
03997
03998 t1 = circ1(numr2i) * circ2(numr2i);
03999 q(1) += t1;
04000
04001 t1 = circ1(numr2i+1) * circ2(numr2i+1);
04002 if (numr3i == maxrin) {
04003 q(2) += t1;
04004 } else {
04005 q(numr3i+1) += t1;
04006 }
04007
04008 for (j=3; j<=numr3i; j += 2) {
04009 jc = j+numr2i-1;
04010
04011
04012
04013
04014
04015 c1 = circ1(jc);
04016 c2 = circ1(jc+1);
04017 d1 = circ2(jc);
04018 d2 = circ2(jc+1);
04019
04020 t1 = c1 * d1;
04021 t3 = c1 * d2;
04022 t2 = c2 * d2;
04023 t4 = c2 * d1;
04024
04025 q(j) += t1 + t2;
04026 q(j+1) += -t3 + t4;
04027 }
04028 }
04029 } else {
04030 for (i=1; i<=nring; i++) {
04031
04032 numr3i = numr(3,i);
04033 numr2i = numr(2,i);
04034
04035 t1 = circ1(numr2i) * circ2(numr2i);
04036 q(1) += t1;
04037
04038 t1 = circ1(numr2i+1) * circ2(numr2i+1);
04039 if (numr3i == maxrin) {
04040 q(2) += t1;
04041 } else {
04042 q(numr3i+1) += t1;
04043 }
04044
04045 for (j=3; j<=numr3i; j += 2) {
04046 jc = j+numr2i-1;
04047
04048
04049
04050
04051
04052 c1 = circ1(jc);
04053 c2 = circ1(jc+1);
04054 d1 = circ2(jc);
04055 d2 = circ2(jc+1);
04056
04057 t1 = c1 * d1;
04058 t3 = c1 * d2;
04059 t2 = c2 * d2;
04060 t4 = c2 * d1;
04061
04062 q(j) += t1 - t2;
04063 q(j+1) += -t3 - t4;
04064 }
04065 }
04066 }
04067 fftr_d(q,ip);
04068
04069 qn = -1.0e20;
04070 int psi_pos = int(psi/360.0*maxrin+0.5);
04071
04072 for (k=-5; k<=5; k++) {
04073 j = (psi_pos+maxrin-1)%maxrin+1;
04074 if (q(j) >= qn) {
04075 qn = q(j);
04076 jtot = j;
04077 }
04078 }
04079
04080 for (k=-3; k<=3; k++) {
04081 j = ((jtot+k+maxrin-1)%maxrin)+1;
04082 t7(k+4) = q(j);
04083 }
04084
04085
04086 prb1d(t7,7,&pos);
04087 tot = (float)(jtot)+pos;
04088 free(q);
04089
04090 Dict retvals;
04091 retvals["qn"] = qn;
04092 retvals["tot"] = tot;
04093 return retvals;
04094 }
04095
04096 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
04097 int nring = numr.size()/3;
04098
04099 int maxrin = numr[numr.size()-1];
04100 double qn; float tot;
04101 float *circ1 = circ1p->get_data();
04102 float *circ2 = circ2p->get_data();
04103
04104
04105
04106
04107
04108
04109
04110
04111
04112
04113
04114
04115 double *q, t7[7];
04116
04117 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04118 float c1, c2, d1, d2, pos;
04119
04120 qn = 0.0;
04121 tot = 0.0;
04122 #ifdef _WIN32
04123 ip = -(int)(log((float)maxrin)/log(2.0f));
04124 #else
04125 ip = -(int)(log2(maxrin));
04126 #endif //_WIN32
04127
04128
04129
04130
04131
04132 q = (double*)calloc(maxrin,sizeof(double));
04133
04134
04135 for (i=1; i<=nring; i++) {
04136
04137 numr3i = numr(3,i);
04138 numr2i = numr(2,i);
04139
04140 q(1) += circ1(numr2i) * circ2(numr2i);
04141
04142 if (numr3i == maxrin) q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04143 else q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04144
04145 for (j=3; j<=numr3i; j += 2) {
04146 jc = j+numr2i-1;
04147
04148
04149
04150
04151
04152 c1 = circ1(jc);
04153 c2 = circ1(jc+1);
04154 d1 = circ2(jc);
04155 d2 = circ2(jc+1);
04156
04157 q(j) += c1 * d1 + c2 * d2;
04158 q(j+1) += -c1 * d2 + c2 * d1;
04159 }
04160 }
04161
04162 fftr_d(q,ip);
04163
04164 qn = -1.0e20;
04165 for (j=1; j<=maxrin; j++) {
04166 if (q(j) >= qn) {
04167 qn = q(j);
04168 jtot = j;
04169 }
04170 }
04171
04172 for (k=-3; k<=3; k++) {
04173 j = ((jtot+k+maxrin-1)%maxrin)+1;
04174 t7(k+4) = q(j);
04175 }
04176
04177
04178 prb1d(t7,7,&pos);
04179 tot = (float)(jtot)+pos;
04180
04181
04182
04183 free(q);
04184
04185 Dict retvals;
04186 retvals["qn"] = qn;
04187 retvals["tot"] = tot;
04188 return retvals;
04189 }
04190
04191 #define dout(i,j) dout[i+maxrin*j]
04192 #define circ1b(i) circ1b[i-1]
04193 #define circ2b(i) circ2b[i-1]
04194
04195 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04196
04197
04198
04199 int ip, jc, numr3i, numr2i, i, j;
04200 float t1, t2, t3, t4, c1, c2, d1, d2;
04201
04202 int nring = numr.size()/3;
04203
04204 int maxrin = numr[numr.size()-1];
04205
04206 float* circ1b = circ1->get_data();
04207 float* circ2b = circ2->get_data();
04208
04209
04210 double *t, *q;
04211
04212 q = (double*)calloc(maxrin,sizeof(double));
04213 t = (double*)calloc(maxrin,sizeof(double));
04214
04215 #ifdef _WIN32
04216 ip = -(int)(log((float)maxrin)/log(2.0f));
04217 #else
04218 ip = -(int)(log2(maxrin));
04219 #endif //_WIN32
04220
04221
04222
04223
04224
04225
04226
04227 for (i=1; i<=nring; i++) {
04228
04229 numr3i = numr(3,i);
04230 numr2i = numr(2,i);
04231
04232 t1 = circ1b(numr2i) * circ2b(numr2i);
04233 q(1) = q(1)+t1;
04234 t(1) = t(1)+t1;
04235
04236 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04237 if (numr3i == maxrin) {
04238 q(2) += t1;
04239 t(2) += t1;
04240 } else {
04241 q(numr3i+1) += t1;
04242 t(numr3i+1) += t1;
04243 }
04244
04245 for (j=3; j<=numr3i; j=j+2) {
04246 jc = j+numr2i-1;
04247
04248 c1 = circ1b(jc);
04249 c2 = circ1b(jc+1);
04250 d1 = circ2b(jc);
04251 d2 = circ2b(jc+1);
04252
04253 t1 = c1 * d1;
04254 t3 = c1 * d2;
04255 t2 = c2 * d2;
04256 t4 = c2 * d1;
04257
04258 q(j) += t1 + t2;
04259 q(j+1) += - t3 + t4;
04260 t(j) += t1 - t2;
04261 t(j+1) += - t3 - t4;
04262 }
04263 }
04264
04265
04266 fftr_d(q,ip);
04267
04268
04269 fftr_d(t,ip);
04270
04271 EMData* out = new EMData();
04272 out->set_size(maxrin,2,1);
04273 float *dout = out->get_data();
04274 for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04275
04276
04277
04278 free(t);
04279 free(q);
04280 return out;
04281 }
04282
04283
04284 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04285
04286 int maxrin = numr[numr.size()-1];
04287
04288 vector<float> r(2*maxrin);
04289
04290 Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04291
04292 return r;
04293 }
04294
04295 #define dout(i,j) dout[i+maxrin*j]
04296 #define circ1b(i) circ1b[i-1]
04297 #define circ2b(i) circ2b[i-1]
04298
04299 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04300
04301
04302
04303 int ip, jc, numr3i, numr2i, i, j;
04304 float t1, t2, t3, t4, c1, c2, d1, d2;
04305
04306 int nring = numr.size()/3;
04307
04308 int maxrin = numr[numr.size()-1];
04309
04310 float* circ1b = circ1->get_data();
04311 float* circ2b = circ2->get_data();
04312
04313 #ifdef _WIN32
04314 ip = -(int)(log((float)maxrin)/log(2.0f));
04315 #else
04316 ip = -(int)(log2(maxrin));
04317 #endif //_WIN32
04318 for (int i=1; i<=maxrin; i++) {q(i) = 0.0f; t(i) = 0.0f;}
04319
04320
04321
04322
04323
04324 for (i=1; i<=nring; i++) {
04325
04326 numr3i = numr(3,i);
04327 numr2i = numr(2,i);
04328
04329 t1 = circ1b(numr2i) * circ2b(numr2i);
04330 q(1) += t1;
04331 t(1) += t1;
04332
04333 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04334 if (numr3i == maxrin) {
04335 q(2) += t1;
04336 t(2) += t1;
04337 } else {
04338 q(numr3i+1) += t1;
04339 t(numr3i+1) += t1;
04340 }
04341
04342 for (j=3; j<=numr3i; j=j+2) {
04343 jc = j+numr2i-1;
04344
04345 c1 = circ1b(jc);
04346 c2 = circ1b(jc+1);
04347 d1 = circ2b(jc);
04348 d2 = circ2b(jc+1);
04349
04350 t1 = c1 * d1;
04351 t3 = c1 * d2;
04352 t2 = c2 * d2;
04353 t4 = c2 * d1;
04354
04355 q(j) += t1 + t2;
04356 q(j+1) += -t3 + t4;
04357 t(j) += t1 - t2;
04358 t(j+1) += -t3 - t4;
04359 }
04360 }
04361
04362 fftr_q(q,ip);
04363
04364
04365
04366 fftr_q(t,ip);
04367 }
04368
04369
04370
04371 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04372 {
04373
04374 int ip, jc, numr3i, numr2i, i, j;
04375 float t1, t2, t3, t4, c1, c2, d1, d2;
04376
04377 int nring = numr.size()/3;
04378 int maxrin = numr[numr.size()-1];
04379
04380 float* circ1b = circ1->get_data();
04381 float* circ2b = circ2->get_data();
04382
04383 double *q;
04384
04385 q = (double*)calloc(maxrin,sizeof(double));
04386
04387 #ifdef _WIN32
04388 ip = -(int)(log((float)maxrin)/log(2.0f));
04389 #else
04390 ip = -(int)(log2(maxrin));
04391 #endif //_WIN32
04392
04393
04394
04395 for (i=1;i<=nring;i++) {
04396
04397 numr3i = numr(3,i);
04398 numr2i = numr(2,i);
04399
04400 t1 = circ1b(numr2i) * circ2b(numr2i);
04401 q(1) = q(1)+t1;
04402
04403 if (numr3i == maxrin) {
04404 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04405 q(2) = q(2)+t1;
04406 } else {
04407 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04408 q(numr3i+1) = q(numr3i+1)+t1;
04409 }
04410
04411 for (j=3;j<=numr3i;j=j+2) {
04412 jc = j+numr2i-1;
04413
04414 c1 = circ1b(jc);
04415 c2 = circ1b(jc+1);
04416 d1 = circ2b(jc);
04417 d2 = circ2b(jc+1);
04418
04419 t1 = c1 * d1;
04420 t3 = c1 * d2;
04421 t2 = c2 * d2;
04422 t4 = c2 * d1;
04423
04424 q(j) = q(j) + t1 + t2;
04425 q(j+1) = q(j+1) - t3 + t4;
04426 }
04427 }
04428
04429
04430 fftr_d(q,ip);
04431
04432 EMData* out = new EMData();
04433 out->set_size(maxrin,1,1);
04434 float *dout = out->get_data();
04435 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04436 free(q);
04437 return out;
04438
04439 }
04440
04441
04442 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04443 {
04444
04445 int ip, jc, numr3i, numr2i, i, j;
04446 float t1, t2, t3, t4, c1, c2, d1, d2;
04447
04448 int nring = numr.size()/3;
04449 int maxrin = numr[numr.size()-1];
04450
04451 float* circ1b = circ1->get_data();
04452 float* circ2b = circ2->get_data();
04453
04454 double *t;
04455
04456 t = (double*)calloc(maxrin,sizeof(double));
04457
04458 #ifdef _WIN32
04459 ip = -(int)(log((float)maxrin)/log(2.0f));
04460 #else
04461 ip = -(int)(log2(maxrin));
04462 #endif //_WIN32
04463
04464
04465
04466 for (i=1;i<=nring;i++) {
04467
04468 numr3i = numr(3,i);
04469 numr2i = numr(2,i);
04470
04471 t1 = circ1b(numr2i) * circ2b(numr2i);
04472 t(1) = t(1)+t1;
04473
04474 if (numr3i == maxrin) {
04475 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04476 t(2) = t(2)+t1;
04477 }
04478
04479 for (j=3;j<=numr3i;j=j+2) {
04480 jc = j+numr2i-1;
04481
04482 c1 = circ1b(jc);
04483 c2 = circ1b(jc+1);
04484 d1 = circ2b(jc);
04485 d2 = circ2b(jc+1);
04486
04487 t1 = c1 * d1;
04488 t3 = c1 * d2;
04489 t2 = c2 * d2;
04490 t4 = c2 * d1;
04491
04492 t(j) = t(j) + t1 - t2;
04493 t(j+1) = t(j+1) - t3 - t4;
04494 }
04495 }
04496
04497
04498 fftr_d(t,ip);
04499
04500 EMData* out = new EMData();
04501 out->set_size(maxrin,1,1);
04502 float *dout = out->get_data();
04503 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04504 free(t);
04505 return out;
04506
04507 }
04508
04509 #undef circ1b
04510 #undef circ2b
04511 #undef dout
04512
04513 #undef circ1
04514 #undef circ2
04515 #undef t
04516 #undef q
04517 #undef b
04518 #undef t7
04519
04520
04521 #define QUADPI 3.141592653589793238462643383279502884197
04522 #define PI2 2*QUADPI
04523
04524 float Util::ener(EMData* ave, vector<int> numr) {
04525 ENTERFUNC;
04526 long double ener,en;
04527
04528 int nring = numr.size()/3;
04529 float *aveptr = ave->get_data();
04530
04531 ener = 0.0;
04532 for (int i=1; i<=nring; i++) {
04533 int numr3i = numr(3,i);
04534 int np = numr(2,i)-1;
04535 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04536 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04537 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04538 ener += en/numr3i;
04539 }
04540 EXITFUNC;
04541 return static_cast<float>(ener);
04542 }
04543
04544 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04545 ENTERFUNC;
04546 long double ener, en;
04547 float arg, cs, si;
04548
04549 int nima = data.size();
04550 int nring = numr.size()/3;
04551 int maxrin = numr(3,nring);
04552
04553 ener = 0.0;
04554 for (int i=1; i<=nring; i++) {
04555 int numr3i = numr(3,i);
04556 int np = numr(2,i)-1;
04557 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04558 float temp1 = 0.0, temp2 = 0.0;
04559 for (int kk=0; kk<nima; kk++) {
04560 float *ptr = data[kk]->get_data();
04561 temp1 += ptr[np];
04562 temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04563 }
04564 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04565 for (int j=2; j<numr3i; j+=2) {
04566 float tempr = 0.0, tempi = 0.0;
04567 for (int kk=0; kk<nima; kk++) {
04568 float *ptr = data[kk]->get_data();
04569 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04570 cs = cos(arg);
04571 si = sin(arg);
04572 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04573 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04574 }
04575 en += tq*(tempr*tempr+tempi*tempi);
04576 }
04577 ener += en/numr3i;
04578 }
04579 EXITFUNC;
04580 return static_cast<float>(ener);
04581 }
04582
04583 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04584 int nring = numr.size()/3;
04585 float *ave = avep->get_data();
04586 float *dat = datp->get_data();
04587 int i, j, numr3i, np;
04588 float arg, cs, si;
04589 int maxrin = numr(3,nring);
04590 if(mirror == 1) {
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 } else {
04606 for (i=1; i<=nring; i++) {
04607 numr3i = numr(3,i);
04608 np = numr(2,i)-1;
04609 ave[np] += dat[np];
04610 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04611 for (j=2; j<numr3i; j=j+2) {
04612 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04613 cs = cos(arg);
04614 si = sin(arg);
04615
04616 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04617 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04618 }
04619 }
04620 }
04621 avep->update();
04622 EXITFUNC;
04623 }
04624
04625 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04626 int nring = numr.size()/3;
04627 float *ave = avep->get_data();
04628 float *dat = datp->get_data();
04629 int i, j, numr3i, np;
04630 float arg, cs, si;
04631 int maxrin = numr(3,nring);
04632 if(mirror == 1) {
04633 for (i=1; i<=nring; i++) {
04634 numr3i = numr(3,i);
04635 np = numr(2,i)-1;
04636 ave[np] -= dat[np];
04637 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04638 for (j=2; j<numr3i; j=j+2) {
04639 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04640 cs = cos(arg);
04641 si = sin(arg);
04642
04643 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04644 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04645 }
04646 }
04647 } else {
04648 for (i=1; i<=nring; i++) {
04649 numr3i = numr(3,i);
04650 np = numr(2,i)-1;
04651 ave[np] -= dat[np];
04652 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04653 for (j=2; j<numr3i; j=j+2) {
04654 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04655 cs = cos(arg);
04656 si = sin(arg);
04657
04658 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04659 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04660 }
04661 }
04662 }
04663 avep->update();
04664 EXITFUNC;
04665 }
04666
04667
04668 #undef QUADPI
04669 #undef PI2
04670
04671 #undef numr
04672 #undef circ
04673
04674
04675 #define QUADPI 3.141592653589793238462643383279502884197
04676 #define PI2 QUADPI*2
04677 #define deg_rad QUADPI/180.0
04678 #define rad_deg 180.0/QUADPI
04679
04680 struct ori_t
04681 {
04682 int iphi;
04683 int itht;
04684 int id;
04685 };
04686
04687
04688 struct cmpang
04689 {
04690 bool operator()( const ori_t& a, const ori_t& b )
04691 {
04692 if( a.itht != b.itht )
04693 {
04694 return a.itht < b.itht;
04695 }
04696
04697 return a.iphi < b.iphi;
04698 }
04699 };
04700
04701
04702 vector<double> Util::cml_weights(const vector<float>& cml){
04703 static const int NBIN = 100;
04704 int nline=cml.size()/2;
04705 vector<double> weights(nline);
04706
04707 vector<ori_t> angs(nline);
04708 for( int i=0; i < nline; ++i ) {
04709 angs[i].iphi = int( NBIN*cml[2*i] );
04710 angs[i].itht = int( NBIN*cml[2*i+1] );
04711 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04712 angs[i].id = i;
04713 }
04714
04715
04716
04717 std::sort( angs.begin(), angs.end(), cmpang() );
04718
04719 vector<float> newphi;
04720 vector<float> newtht;
04721 vector< vector<int> > indices;
04722
04723 int curt_iphi = -1;
04724 int curt_itht = -1;
04725 for(unsigned int i=0 ;i < angs.size(); ++i ) {
04726 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04727 Assert( indices.size() > 0 );
04728 indices.back().push_back(angs[i].id);
04729 } else {
04730 curt_iphi = angs[i].iphi;
04731 curt_itht = angs[i].itht;
04732
04733 newphi.push_back( float(curt_iphi)/NBIN );
04734 newtht.push_back( float(curt_itht)/NBIN );
04735 indices.push_back( vector<int>(1,angs[i].id) );
04736 }
04737 }
04738
04739
04740
04741
04742 int num_agl = newphi.size();
04743
04744 if(num_agl>2) {
04745 vector<double> w=Util::vrdg(newphi, newtht);
04746
04747 Assert( w.size()==newphi.size() );
04748 Assert( indices.size()==newphi.size() );
04749
04750 for(unsigned int i=0; i < newphi.size(); ++i ) {
04751
04752
04753
04754
04755
04756
04757
04758
04759 for(unsigned int j=0; j < indices[i].size(); ++j ) {
04760 int id = indices[i][j];
04761 weights[id] = w[i]/indices[i].size();
04762
04763 }
04764
04765
04766
04767 }
04768 } else {
04769 cout<<"warning in Util.cml_weights"<<endl;
04770 double val = PI2/float(nline);
04771 for(int i=0; i<nline; i++) weights[i]=val;
04772 }
04773
04774 return weights;
04775
04776 }
04777
04778
04779
04780
04781
04782 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04783 {
04784 int i;
04785 int nx=img->get_xsize();
04786 float *img_ptr = img->get_data();
04787 float *line_ptr = line->get_data();
04788 for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04789 img->update();
04790 }
04791
04792 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04793 int j;
04794 int nx = sino->get_xsize();
04795 int i = nx * pos_line;
04796 float r1, r2;
04797 float *line_ptr = line->get_data();
04798 float *sino_ptr = sino->get_data();
04799 for (j=ilf;j<=ihf; j += 2) {
04800 r1 = line_ptr[j];
04801 r2 = line_ptr[j + 1];
04802 sino_ptr[i + j - ilf] = r1;
04803 sino_ptr[i + j - ilf + 1] = r2;
04804 sino_ptr[i + nx * nblines + j - ilf] = r1;
04805 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04806 }
04807 sino->update();
04808 }
04809
04810 vector<double> Util::cml_init_rot(vector<float> Ori){
04811 int nb_ori = Ori.size() / 4;
04812 int i, ind;
04813 float ph, th, ps;
04814 double cph, cth, cps, sph, sth, sps;
04815 vector<double> Rot(nb_ori*9);
04816 for (i=0; i<nb_ori; ++i){
04817 ind = i*4;
04818
04819 ph = Ori[ind+2]-90;
04820 th = Ori[ind+1];
04821 ps = Ori[ind]+90;
04822 ph *= deg_rad;
04823 th *= deg_rad;
04824 ps *= deg_rad;
04825
04826 cph = cos(ph);
04827 cth = cos(th);
04828 cps = cos(ps);
04829 sph = sin(ph);
04830 sth = sin(th);
04831 sps = sin(ps);
04832
04833 ind = i*9;
04834 Rot[ind] = cph*cps-cth*sps*sph;
04835 Rot[ind+1] = cph*sps+cth*cps*sph;
04836 Rot[ind+2] = sth*sph;
04837 Rot[ind+3] = -sph*cps-cth*sps*cph;
04838 Rot[ind+4] = -sph*sps+cth*cps*cph;
04839 Rot[ind+5] = sth*cph;
04840 Rot[ind+6] = sth*sps;
04841 Rot[ind+7] = -sth*cps;
04842 Rot[ind+8] = cth;
04843 }
04844
04845 return Rot;
04846 }
04847
04848 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04849 float ph, ps;
04850 double cph, cth, cps, sph, sth, sps;
04851 int ind = iprj*9;
04852
04853 ph = nps-90;
04854 ps = nph+90;
04855 ph *= deg_rad;
04856 th *= deg_rad;
04857 ps *= deg_rad;
04858
04859 cph = cos(ph);
04860 cth = cos(th);
04861 cps = cos(ps);
04862 sph = sin(ph);
04863 sth = sin(th);
04864 sps = sin(ps);
04865
04866 Rot[ind] = (float)(cph*cps-cth*sps*sph);
04867 Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04868 Rot[ind+2] = (float)(sth*sph);
04869 Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04870 Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04871 Rot[ind+5] = (float)(sth*cph);
04872 Rot[ind+6] = (float)(sth*sps);
04873 Rot[ind+7] = (float)(-sth*cps);
04874 Rot[ind+8] = (float)(cth);
04875
04876 return Rot;
04877 }
04878
04879 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04880 vector<int> com(2*(n_prj - 1));
04881 int a = i_prj*9;
04882 int i, b, c;
04883 int n1=0, n2=0;
04884 float vmax = 1 - 1.0e-6f;
04885 double r11, r12, r13, r23, r31, r32, r33;
04886
04887 c = 0;
04888 for (i=0; i<n_prj; ++i){
04889 if (i!=i_prj){
04890 b = i*9;
04891
04892 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04893 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04894 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04895 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04896 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04897 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04898 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04899 if (r33 > vmax) {
04900 n2 = 270;
04901 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04902 }
04903 else if (r33 < -vmax) {
04904 n2 = 270;
04905 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04906 } else {
04907 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04908 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04909 if (n1 < 0) {n1 += 360;}
04910 if (n2 <= 0) {n2 = abs(n2);}
04911 else {n2 = 360 - n2;}
04912 }
04913
04914 if (n1 >= 360){n1 = n1 % 360;}
04915 if (n2 >= 360){n2 = n2 % 360;}
04916
04917
04918 b = c*2;
04919 com[b] = n1;
04920 com[b+1] = n2;
04921 ++c;
04922 }
04923 }
04924
04925 return com;
04926
04927 }
04928
04929 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int, int n_lines) {
04930 vector<int> com(2*n_lines);
04931 int a=0, b, c, l;
04932 int n1=0, n2=0, mem=-1;
04933 float vmax = 1 - 1.0e-6f;
04934 double r11, r12, r13, r23, r31, r32, r33;
04935 c = 0;
04936 for (l=0; l<n_lines; ++l){
04937 c = 2*l;
04938 if (seq[c]!=mem){
04939 mem = seq[c];
04940 a = seq[c]*9;
04941 }
04942 b = seq[c+1]*9;
04943
04944
04945 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04946 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04947 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04948 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04949 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04950 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04951 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04952 if (r33 > vmax) {
04953 n2 = 270;
04954 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04955 }
04956 else if (r33 < -vmax) {
04957 n2 = 270;
04958 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04959 } else {
04960 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04961 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04962 if (n1 < 0) {n1 += 360;}
04963 if (n2 <= 0) {n2 = abs(n2);}
04964 else {n2 = 360 - n2;}
04965 }
04966 if (n1 >= 360){n1 = n1 % 360;}
04967 if (n2 >= 360){n2 = n2 % 360;}
04968
04969
04970 com[c] = n1;
04971 com[c+1] = n2;
04972 }
04973
04974 return com;
04975
04976 }
04977
04978 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int, int nlines){
04979
04980 vector<double> cml(2*nlines);
04981 float ph1, th1;
04982 float ph2, th2;
04983 double nx, ny, nz;
04984 double norm;
04985 double sth1=0, sph1=0, cth1=0, cph1=0;
04986 double sth2, sph2, cth2, cph2;
04987 int l, ind, c;
04988 int mem = -1;
04989 for (l=0; l<nlines; ++l){
04990 c = 2*l;
04991 if (seq[c]!=mem){
04992 mem = seq[c];
04993 ind = 4*seq[c];
04994 ph1 = Ori[ind]*deg_rad;
04995 th1 = Ori[ind+1]*deg_rad;
04996 sth1 = sin(th1);
04997 sph1 = sin(ph1);
04998 cth1 = cos(th1);
04999 cph1 = cos(ph1);
05000 }
05001 ind = 4*seq[c+1];
05002 ph2 = Ori[ind]*deg_rad;
05003 th2 = Ori[ind+1]*deg_rad;
05004 sth2 = sin(th2);
05005 cth2 = cos(th2);
05006 sph2 = sin(ph2);
05007 cph2 = cos(ph2);
05008
05009 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
05010 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
05011 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
05012 norm = sqrt(nx*nx+ny*ny+nz*nz);
05013 nx /= norm;
05014 ny /= norm;
05015 nz /= norm;
05016
05017 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
05018
05019 cml[c+1] = acos(nz);
05020 if (cml[c+1] == 0) {cml[c] = 0;}
05021 else {
05022 cml[c+1] *= rad_deg;
05023 if (cml[c+1] > 89.99) {cml[c+1] = 89.99;}
05024 cml[c] = rad_deg * atan2(nx, ny);
05025 cml[c] = fmod(360 + cml[c], 360);
05026
05027 }
05028 }
05029
05030 return cml;
05031 }
05032
05033 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
05034 double res = 0;
05035 double buf = 0;
05036 float* line_1;
05037 float* line_2;
05038 int i, n, ind;
05039 int lnlen = data[0]->get_xsize();
05040 for (n=0; n<n_lines; ++n) {
05041 ind = n*2;
05042 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
05043 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
05044 buf = 0;
05045 for (i=0; i<lnlen; ++i) {
05046 buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
05047 }
05048 res += buf * weights[n];
05049 }
05050
05051 return res;
05052
05053 }
05054
05055 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
05056 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
05057
05058
05059
05060 vector<double> res(2);
05061 int lnlen = data[0]->get_xsize();
05062 int end = 2*(n_prj-1);
05063 double disc, buf, bdisc, tmp;
05064 int n, i, ipsi, ind, bipsi, c;
05065 float* line_1;
05066 float* line_2;
05067 bdisc = 1.0e6;
05068 bipsi = -1;
05069
05070 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
05071
05072 disc = 0;
05073 c = 0;
05074 for (n=0; n<n_prj; ++n) {
05075 if(n!=iprj) {
05076 ind = 2*c;
05077 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
05078 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
05079 buf = 0;
05080 for (i=0; i<lnlen; ++i) {
05081 tmp = line_1[i]-line_2[i];
05082 buf += tmp*tmp;
05083 }
05084 disc += buf * weights[iw[c]];
05085 ++c;
05086 }
05087 }
05088
05089 if (disc <= bdisc) {
05090 bdisc = disc;
05091 bipsi = ipsi;
05092 }
05093
05094 for (i=0; i<end; i+=2){
05095 com[i] += d_psi;
05096 if (com[i] >= n_psi) {com[i] = com[i] % n_psi;}
05097 }
05098 }
05099 res[0] = bdisc;
05100 res[1] = float(bipsi);
05101
05102 return res;
05103 }
05104
05105 #undef QUADPI
05106 #undef PI2
05107 #undef deg_rad
05108 #undef rad_deg
05109
05110
05111
05112
05113
05114
05115 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
05116 ENTERFUNC;
05117
05118 int nima = data.size();
05119 vector<float> res(nima);
05120 double result = 0.;
05121 double valmin = 1.0e20;
05122 int valpos = -1;
05123
05124 for (int kk=0; kk<nima; kk++){
05125 result = 0;
05126
05127 float *y_data = data[kk]->get_data();
05128 float *x_data = image->get_data();
05129 long totsize = image->get_xsize()*image->get_ysize();
05130 for (long i = 0; i < totsize; i++) {
05131 double temp = x_data[i]- y_data[i];
05132 result += temp*temp;
05133 }
05134 result /= totsize;
05135 res[kk] = (float)result;
05136
05137 if(result<valmin) {valmin = result; valpos = kk;}
05138
05139 }
05140
05141 Dict retvals;
05142 retvals["dist"] = res;
05143 retvals["pos"] = valpos;
05144
05145 EXITFUNC;
05146 return retvals;
05147
05148 }
05149
05150 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05151 ENTERFUNC;
05152
05153 int nima = data.size();
05154 vector<float> res(nima);
05155 double result = 0.;
05156 double valmin = 1.0e20;
05157 int valpos = -1;
05158
05159 for (int kk=0; kk<nima; kk++){
05160 result = 0;
05161
05162
05163 float *y_data = data[kk]->get_data();
05164 float *x_data = image->get_data();
05165
05166
05167 int nx = data[kk]->get_xsize();
05168 int ny = data[kk]->get_ysize();
05169 nx = (nx - 2 + data[kk]->is_fftodd());
05170 int lsd2 = (nx + 2 - nx%2) ;
05171
05172 int ixb = 2*((nx+1)%2);
05173 int iyb = ny%2;
05174 int iz = 0;
05175
05176 for ( int iy = 0; iy <= ny-1; iy++) {
05177 for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05178 int ii = ix + (iy + iz * ny)* lsd2;
05179 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05180 }
05181 }
05182 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05183 int ii = (iy + iz * ny)* lsd2;
05184 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05185 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05186 }
05187 if(nx%2 == 0) {
05188 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05189 int ii = lsd2 - 2 + (iy + iz * ny)* lsd2;
05190 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05191 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05192 }
05193
05194 }
05195 result *= 2;
05196 result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05197 if(ny%2 == 0) {
05198 int ii = (ny/2 + iz * ny)* lsd2;
05199 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05200 }
05201 if(nx%2 == 0) {
05202 int ii = lsd2 - 2 + (0 + iz * ny)* lsd2;
05203 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05204 if(ny%2 == 0) {
05205 int ii = lsd2 - 2 +(ny/2 + iz * ny)* lsd2;
05206 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05207 }
05208 }
05209
05210 result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05211 res[kk] = (float)result;
05212
05213 if(result<valmin) {valmin = result; valpos = kk;}
05214
05215 }
05216
05217 Dict retvals;
05218 retvals["dist"] = res;
05219 retvals["pos"] = valpos;
05220
05221 EXITFUNC;
05222 return retvals;
05223 }
05224
05225 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05226 long int d2 = group2[s2 - 1] - group2[0];
05227 long int p2 = 0;
05228 long int i1 = 0;
05229 long int i2 = 0;
05230 long int max = 0;
05231 long int cont = 0;
05232 long int i = 0;
05233 int stop1 = 0;
05234 int stop2 = 0;
05235
05236 for (i=0; i<s1; i++) {
05237 p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05238 if (p2 >= s2) {p2 = s2 - 1;}
05239 i1 = p2;
05240 i2 = p2;
05241 max = s2;
05242 if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05243
05244 stop1 = 0;
05245 stop2 = 0;
05246 while (max--) {
05247 if (group1[i] == group2[i1]) {
05248 if (flag) {stb[cont] = group1[i];}
05249 cont++;
05250 break;
05251 }
05252 if (group2[i1] < group1[i]) {stop1=1;}
05253 if (group1[i] == group2[i2]) {
05254 if (flag) {stb[cont] = group1[i];}
05255 cont++;
05256 break;
05257 }
05258 if (group2[i2] > group1[i]) {stop2=1;}
05259
05260
05261 if (stop1 & stop2) {break;}
05262 i1--;
05263 i2++;
05264 if (i1 < 0) {i1 = 0;}
05265 if (i2 >= s2) {i2 = s2 - 1;}
05266 }
05267
05268 }
05269
05270 return cont;
05271 }
05272
05273
05274
05275 #define old_ptr(i,j,k) old_ptr[i+(j+(k*ny))*(size_t)nx]
05276 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*(size_t)new_nx]
05277 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05278 {
05279
05280 if (!img) {
05281 throw NullPointerException("NULL input image");
05282 }
05283
05284
05285
05286 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05287
05288
05289
05290
05291 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)
05292 {
05293 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05294 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05295 }
05296
05297
05298
05299
05300 int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05301
05302
05303
05304
05305 int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05306 int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05307 int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05308 if(r1>1){r1=1;}
05309 if(r2>1){r2=1;}
05310 if(r3>1){r3=1;}
05311 int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05312
05313
05314
05315 EMData* img2 = new EMData();
05316 img2->set_size(new_nx,new_ny,new_nz);
05317 float *new_ptr = img2->get_data();
05318 float *old_ptr = img->get_data();
05319 int iptr, jptr, kptr = 0;
05320 for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05321 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05322 for (int i=new_st_x; i<nx; i+=x_step) {
05323 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05324 iptr++;}
05325 jptr++;}
05326 kptr++;}
05327 img2->update();
05328 return img2;
05329 }
05330 #undef old_ptr
05331 #undef new_ptr
05332
05333 #define inp(i,j,k) inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*(size_t)nx]
05334 #define outp(i,j,k) outp[i+(j+(k*new_ny))*(size_t)new_nx]
05335 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05336 {
05337
05338 if (!img) throw NullPointerException("NULL input image");
05339
05340
05341
05342 int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05343
05344
05345
05346 if(new_nx>nx || new_ny>ny || new_nz>nz)
05347 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size.");
05348 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)
05349 throw ImageDimensionException("The offset inconsistent with the input image size.");
05350 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))))
05351 throw ImageDimensionException("The offset inconsistent with the input image size.");
05352
05353
05354
05355 int new_st_x = nx/2-new_nx/2 + x_offset,
05356 new_st_y = ny/2-new_ny/2 + y_offset,
05357 new_st_z = nz/2-new_nz/2 + z_offset;
05358
05359
05360
05361 if (new_st_x<0 || new_st_y<0 || new_st_z<0)
05362 throw ImageDimensionException("The offset inconsistent with the input image size.");
05363
05364
05365 EMData* wind = img->copy_empty_head();
05366 wind->set_size(new_nx, new_ny, new_nz);
05367 float *outp=wind->get_data();
05368 float *inp=img->get_data();
05369
05370 for (int k=0; k<new_nz; k++)
05371 for(int j=0; j<new_ny; j++)
05372 for(int i=0; i<new_nx; i++)
05373 outp(i,j,k) = inp(i,j,k);
05374 wind->update();
05375 return wind;
05376 }
05377 #undef inp
05378 #undef outp
05379
05380 #define inp(i,j,k) inp[i+(j+(k*ny))*(size_t)nx]
05381 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*(size_t)new_nx]
05382 EMData *Util::pad(EMData* img,int new_nx, int new_ny, int new_nz, int x_offset, int y_offset, int z_offset,char *params)
05383 {
05384
05385 if (!img) throw NullPointerException("NULL input image");
05386
05387
05388
05389 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05390
05391
05392
05393 if(new_nx<nx || new_ny<ny || new_nz<nz)
05394 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05395 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)
05396 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05397 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))))
05398 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05399
05400
05401 EMData* pading = img->copy_head();
05402 pading->set_size(new_nx, new_ny, new_nz);
05403 float *inp = img->get_data();
05404 float *outp = pading->get_data();
05405
05406
05407
05408
05409 float background;
05410
05411 if (strcmp(params,"average")==0) background = img->get_attr("mean");
05412 else if (strcmp(params,"circumference")==0) {
05413 float sum1=0.0f;
05414 size_t cnt=0;
05415 for(int i=0;i<nx;i++) {
05416 sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05417 cnt+=2;
05418 }
05419 if(nz-1 == 0) {
05420 for (int j=1;j<ny-1;j++) {
05421 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05422 cnt+=2;
05423 }
05424 } else {
05425 for (int k=1;k<nz-1;k++) {
05426 for (int j=1;j<ny-1;j++) {
05427 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05428 cnt+=2;
05429 }
05430 }
05431 }
05432 background = sum1/cnt;
05433 } else {
05434 background = static_cast<float>( atof( params ) );
05435 }
05436
05437
05438
05439 int new_st_x=0,new_st_y=0,new_st_z=0;
05440 for (int k=0;k<new_nz;k++)
05441 for(int j=0;j<new_ny;j++)
05442 for (int i=0;i<new_nx;i++)
05443 outp(i,j,k)=background;
05444
05445
05446
05447 new_st_x=int((new_nx/2-nx/2) + x_offset);
05448 new_st_y=int((new_ny/2-ny/2) + y_offset);
05449 new_st_z=int((new_nz/2-nz/2) + z_offset);
05450
05451
05452 for (int k=0;k<nz;k++)
05453 for(int j=0;j<ny;j++)
05454 for(int i=0;i<nx;i++)
05455 outp(i,j,k)=inp(i,j,k);
05456 pading->update();
05457 return pading;
05458 }
05459 #undef inp
05460 #undef outp
05461
05462
05463 void Util::colreverse(float* beg, float* end, int nx) {
05464 float* tmp = new float[nx];
05465 int n = (end - beg)/nx;
05466 int nhalf = n/2;
05467 for (int i = 0; i < nhalf; i++) {
05468
05469 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05470 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05471 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05472 }
05473 delete[] tmp;
05474 }
05475
05476 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05477 {
05478 int nxy = nx*ny;
05479 colreverse(beg, end, nxy);
05480 }
05481
05482
05483 void Util::cyclicshift(EMData *image, Dict params) {
05484
05485 if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05486
05487 int dx = params["dx"];
05488 int dy = params["dy"];
05489 int dz = params["dz"];
05490
05491
05492 int nx = image->get_xsize();
05493 dx %= nx;
05494 if (dx < 0) dx += nx;
05495 int ny = image->get_ysize();
05496 dy %= ny;
05497 if (dy < 0) dy += ny;
05498 int nz = image->get_zsize();
05499 dz %= nz;
05500 if (dz < 0) dz += nz;
05501
05502 int mx = -(dx - nx);
05503 int my = -(dy - ny);
05504 int mz = -(dz - nz);
05505
05506 float* data = image->get_data();
05507
05508 if (mx != 0) {
05509 for (int iz = 0; iz < nz; iz++)
05510 for (int iy = 0; iy < ny; iy++) {
05511
05512 size_t offset = nx*iy + (size_t)nx*ny*iz;
05513 reverse(&data[offset],&data[offset+mx]);
05514 reverse(&data[offset+mx],&data[offset+nx]);
05515 reverse(&data[offset],&data[offset+nx]);
05516 }
05517 }
05518
05519 if (my != 0) {
05520 for (int iz = 0; iz < nz; iz++) {
05521 size_t offset = (size_t)nx*ny*iz;
05522 colreverse(&data[offset], &data[offset + my*nx], nx);
05523 colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05524 colreverse(&data[offset], &data[offset + ny*nx], nx);
05525 }
05526 }
05527 if (mz != 0) {
05528 slicereverse(&data[0], &data[(size_t)mz*ny*nx], nx, ny);
05529 slicereverse(&data[mz*ny*nx], &data[(size_t)nz*ny*nx], nx, ny);
05530 slicereverse(&data[0], &data[(size_t)nz*ny*nx], nx ,ny);
05531 }
05532 image->update();
05533 }
05534
05535
05536
05537
05538 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05539 {
05540 if (image->is_complex())
05541 throw ImageFormatException("Cannot do histogram on Fourier image");
05542
05543 float *imageptr=0, *maskptr=0;
05544 int nx=image->get_xsize();
05545 int ny=image->get_ysize();
05546 int nz=image->get_zsize();
05547
05548 if(mask != NULL){
05549 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05550 throw ImageDimensionException("The size of mask image should be of same size as the input image");
05551 maskptr =mask->get_data();
05552 }
05553 if( nbins == 0) nbins = nx;
05554 vector <float> freq(2*nbins, 0.0);
05555
05556 imageptr=image->get_data();
05557 if( hmin == hmax ) {
05558 if(mask == NULL) {
05559 hmax = image->get_attr("maximum");
05560 hmin = image->get_attr("minimum");
05561 } else {
05562 bool First = true;
05563 for (size_t i = 0;i < (size_t)nx*ny*nz; i++) {
05564 if (maskptr[i]>=0.5f) {
05565 if(First) {
05566 hmax = imageptr[i];
05567 hmin = imageptr[i];
05568 First = false;
05569 } else {
05570 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05571 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05572 }
05573 }
05574 }
05575 }
05576 }
05577 float hdiff = hmax - hmin;
05578 float ff = (nbins-1)/hdiff;
05579 for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05580 if(mask == NULL) {
05581 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05582 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05583 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05584 }
05585 } else {
05586 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05587 if(maskptr[i] >= 0.5) {
05588 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05589 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05590 }
05591 }
05592 }
05593 return freq;
05594 }
05595
05596 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05597 {
05598
05599 if (img->is_complex() || ref->is_complex())
05600 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05601
05602 if(mask != NULL){
05603 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05604 throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05605
05606
05607
05608 size_t size_ref = ((size_t)(ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05609 size_t size_img = ((size_t)(img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05610
05611
05612
05613 float *ref_ptr = ref->get_data();
05614 float ref_h_min = ref->get_attr("minimum");
05615 float ref_h_max = ref->get_attr("maximum");
05616 float ref_h_avg = ref->get_attr("mean");
05617 float ref_h_sig = ref->get_attr("sigma");
05618
05619
05620
05621 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05622
05623 vector<float> img_data = Util::infomask(img, mask);
05624 float img_avg = img_data[0];
05625 float img_sig = img_data[1];
05626
05627
05628 int cnt=0;
05629 for(size_t i=0;i<size_img;++i)
05630 if (mask_ptr[i]>0.5f)
05631 cnt++;
05632
05633
05634
05635 float ref_h_diff = ref_h_max - ref_h_min;
05636
05637 #ifdef _WIN32
05638 int hist_len = _cpp_min((unsigned long)size_ref/16,_cpp_min((unsigned long)size_img/16,256lu));
05639 #else
05640 int hist_len = std::min((unsigned long)size_ref/16,std::min((unsigned long)size_img/16,256lu));
05641 #endif //_WIN32
05642
05643 float *ref_freq_bin = new float[3*hist_len];
05644
05645
05646 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05647
05648 for (size_t i = 0;i < size_ref;++i) {
05649 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05650 ref_freq_bin[L]++;
05651 }
05652 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05653
05654
05655 float A = ref_h_sig/img_sig;
05656 float B = ref_h_avg - (A*img_avg);
05657
05658 vector<float> args;
05659 args.push_back(A);
05660 args.push_back(B);
05661
05662 vector<float> scale;
05663 scale.push_back(1.e-7f*A);
05664 scale.push_back(-1.e-7f*B);
05665
05666 vector<float> ref_freq_hist;
05667 for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05668
05669 vector<float> data;
05670 data.push_back(ref_h_diff);
05671 data.push_back(ref_h_min);
05672
05673 Dict parameter;
05674
05675
05676 parameter["args"] = args;
05677 parameter["scale"]= scale;
05678 parameter["data"] = data;
05679 parameter["ref_freq_bin"] = ref_freq_hist;
05680 parameter["size_img"]=(double)size_img;
05681 parameter["hist_len"]=hist_len;
05682
05683
05684 return parameter;
05685 }
05686
05687
05688 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)
05689 {
05690 float *img_ptr = img->get_data();
05691 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05692
05693 int *img_freq_bin = new int[3*hist_len];
05694 for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05695 for(size_t i = 0;i < size_img;++i) {
05696 if(mask_ptr[i] > 0.5f) {
05697 float img_xn = img_ptr[i]*PA + PB;
05698 int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05699 if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05700 }
05701 }
05702 int freq_hist = 0;
05703
05704 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);
05705 freq_hist = (-freq_hist);
05706 return static_cast<float>(freq_hist);
05707 }
05708
05709 #define QUADPI 3.141592653589793238462643383279502884197
05710 #define DGR_TO_RAD QUADPI/180
05711 #define DM(I) DM [I-1]
05712 #define SS(I) SS [I-1]
05713 Dict Util::CANG(float PHI,float THETA,float PSI)
05714 {
05715 double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05716 vector<float> DM,SS;
05717
05718 for(int i =0;i<9;i++) DM.push_back(0);
05719
05720 for(int i =0;i<6;i++) SS.push_back(0);
05721
05722 CPHI = cos(double(PHI)*DGR_TO_RAD);
05723 SPHI = sin(double(PHI)*DGR_TO_RAD);
05724 CTHE = cos(double(THETA)*DGR_TO_RAD);
05725 STHE = sin(double(THETA)*DGR_TO_RAD);
05726 CPSI = cos(double(PSI)*DGR_TO_RAD);
05727 SPSI = sin(double(PSI)*DGR_TO_RAD);
05728
05729 SS(1) = float(CPHI);
05730 SS(2) = float(SPHI);
05731 SS(3) = float(CTHE);
05732 SS(4) = float(STHE);
05733 SS(5) = float(CPSI);
05734 SS(6) = float(SPSI);
05735
05736 DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05737 DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05738 DM(3) = float(-STHE*CPSI);
05739 DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05740 DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05741 DM(6) = float(STHE*SPSI);
05742 DM(7) = float(STHE*CPHI);
05743 DM(8) = float(STHE*SPHI);
05744 DM(9) = float(CTHE);
05745
05746 Dict DMnSS;
05747 DMnSS["DM"] = DM;
05748 DMnSS["SS"] = SS;
05749
05750 return(DMnSS);
05751 }
05752 #undef SS
05753 #undef DM
05754 #undef QUADPI
05755 #undef DGR_TO_RAD
05756
05757 #define DM(I) DM[I-1]
05758 #define B(i,j) Bptr[i-1+((j-1)*NSAM)]
05759 #define CUBE(i,j,k) CUBEptr[(i-1)+((j-1)+((k-1)*NY3D))*(size_t)NX3D]
05760
05761 void Util::BPCQ(EMData *B,EMData *CUBE, vector<float> DM)
05762 {
05763
05764 float *Bptr = B->get_data();
05765 float *CUBEptr = CUBE->get_data();
05766
05767 int NSAM,NROW,NX3D,NY3D,NZC,KZ,IQX,IQY,LDPX,LDPY,LDPZ,LDPNMX,LDPNMY,NZ1;
05768 float DIPX,DIPY,XB,YB,XBB,YBB;
05769
05770 Transform * t = B->get_attr("xform.projection");
05771 Dict d = t->get_params("spider");
05772 if(t) {delete t; t=0;}
05773
05774 float x_shift = d[ "tx" ];
05775 float y_shift = d[ "ty" ];
05776 x_shift = -x_shift;
05777 y_shift = -y_shift;
05778
05779 NSAM = B->get_xsize();
05780 NROW = B->get_ysize();
05781 NX3D = CUBE->get_xsize();
05782 NY3D = CUBE->get_ysize();
05783 NZC = CUBE->get_zsize();
05784
05785
05786 LDPX = NX3D/2 +1;
05787 LDPY = NY3D/2 +1;
05788 LDPZ = NZC/2 +1;
05789 LDPNMX = NSAM/2 +1;
05790 LDPNMY = NROW/2 +1;
05791 NZ1 = 1;
05792
05793 for(int K=1;K<=NZC;K++) {
05794 KZ=K-1+NZ1;
05795 for(int J=1;J<=NY3D;J++) {
05796 XBB = (1-LDPX)*DM(1)+(J-LDPY)*DM(2)+(KZ-LDPZ)*DM(3);
05797 YBB = (1-LDPX)*DM(4)+(J-LDPY)*DM(5)+(KZ-LDPZ)*DM(6);
05798 for(int I=1;I<=NX3D;I++) {
05799 XB = (I-1)*DM(1)+XBB-x_shift;
05800 IQX = int(XB+float(LDPNMX));
05801 if (IQX <1 || IQX >= NSAM) continue;
05802 YB = (I-1)*DM(4)+YBB-y_shift;
05803 IQY = int(YB+float(LDPNMY));
05804 if (IQY<1 || IQY>=NROW) continue;
05805 DIPX = XB+LDPNMX-IQX;
05806 DIPY = YB+LDPNMY-IQY;
05807
05808 CUBE(I,J,K) = CUBE(I,J,K)+B(IQX,IQY)+DIPY*(B(IQX,IQY+1)-B(IQX,IQY))+DIPX*(B(IQX+1,IQY)-B(IQX,IQY)+DIPY*(B(IQX+1,IQY+1)-B(IQX+1,IQY)-B(IQX,IQY+1)+B(IQX,IQY)));
05809 }
05810 }
05811 }
05812 }
05813
05814 #undef DM
05815 #undef B
05816 #undef CUBE
05817
05818
05819 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
05820 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
05821 #define SS(I,J) SS [I-1 + (J-1)*6]
05822
05823 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05824 {
05825 int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05826 float WW,OX,OY;
05827
05828 NSAM = PROJ->get_xsize();
05829 NROW = PROJ->get_ysize();
05830 int ntotal = NSAM*NROW;
05831 float q = 2.0f;
05832 float qt = 8.0f/q;
05833
05834 int ipad = 1;
05835 NSAM *= ipad;
05836 NROW *= ipad;
05837 NNNN = NSAM+2-(NSAM%2);
05838 int NX2 = NSAM/2;
05839 NR2 = NROW/2;
05840
05841 NANG = int(SS.size())/6;
05842
05843 EMData* W = new EMData();
05844 int Wnx = NNNN/2;
05845 W->set_size(Wnx,NROW,1);
05846 W->to_zero();
05847 float *Wptr = W->get_data();
05848 float *PROJptr = PROJ->get_data();
05849 for (L=1; L<=NANG; L++) {
05850 float tmp1 = SS(3,K)*SS(4,L)*(SS(1,K)*SS(1,L) + SS(2,K)*SS(2,L)) - SS(3,L)*SS(4,K);
05851 float tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) );
05852 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05853 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05854 if(OX < 0.0f) {
05855 OX = -OX;
05856 OY = -OY;
05857 }
05858
05859 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f ) {
05860 for(int J=1;J<=NROW;J++) {
05861 JY = (J-1);
05862 if(JY > NR2) JY -= NROW;
05863 #ifdef _WIN32
05864 int xma = _cpp_min(int(0.5f+(q-JY*OY)/OX),NX2);
05865 int xmi = _cpp_max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05866 #else
05867 int xma = std::min(int(0.5f+(q-JY*OY)/OX),NX2);
05868 int xmi = std::max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05869 #endif //_WIN32
05870 if( xmi <= xma) {
05871 for(int I=xmi;I<=xma;I++) {
05872 float Y = fabs(OX*I + OY*JY);
05873 W(I+1,J) += exp(-qt*Y*Y);
05874
05875 }
05876 }
05877 }
05878 } else {
05879 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J) += 1.0f;
05880 }
05881 }
05882 EMData* proj_in = PROJ;
05883
05884 PROJ = PROJ->norm_pad( false, ipad);
05885 PROJ->do_fft_inplace();
05886 PROJ->update();
05887
05888 PROJptr = PROJ->get_data();
05889
05890 float WNRMinv,temp;
05891 float osnr = 1.0f/SNR;
05892 WNRMinv = 1.0f/W(1,1);
05893 for(int J=1;J<=NROW;J++) {
05894 JY = J-1;
05895 if( JY > NR2) JY -= NROW;
05896 float sy = JY;
05897 sy /= NROW;
05898 sy *= sy;
05899 for(int I=1;I<=NNNN;I+=2) {
05900 KX = (I+1)/2;
05901 temp = W(KX,J)*WNRMinv;
05902 WW = temp/(temp*temp + osnr);
05903
05904 float sx = KX-1;
05905 sx /= NSAM;
05906 WW *= exp(qt*(sy + sx*sx));
05907 PROJ(I,J) *= WW;
05908 PROJ(I+1,J) *= WW;
05909 }
05910 }
05911 delete W; W = 0;
05912 PROJ->do_ift_inplace();
05913 PROJ->depad();
05914
05915 float* data_src = PROJ->get_data();
05916 float* data_dst = proj_in->get_data();
05917
05918 for( int i=0; i < ntotal; ++i ) data_dst[i] = data_src[i];
05919
05920 proj_in->update();
05921
05922 delete PROJ;
05923 }
05924
05925
05926
05927
05928
05929
05930
05931
05932
05933
05934
05935
05936
05937
05938
05939
05940
05941
05942
05943
05944
05945
05946
05947
05948
05949
05950
05951
05952
05953
05954
05955
05956
05957
05958
05959
05960
05961
05962
05963
05964
05965
05966
05967
05968
05969
05970
05971
05972
05973
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 #undef PROJ
06004 #undef W
06005 #undef SS
06006
06007 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
06008 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
06009 #define SS(I,J) SS [I-1 + (J-1)*6]
06010 #define RI(i,j) RI [(i-1) + ((j-1)*3)]
06011 #define CC(i) CC [i-1]
06012 #define CP(i) CP [i-1]
06013 #define VP(i) VP [i-1]
06014 #define VV(i) VV [i-1]
06015 #define AMAX1(i,j) i>j?i:j
06016 #define AMIN1(i,j) i<j?i:j
06017 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
06018 {
06019 float rad2deg =(180.0f/3.1415926f);
06020 float deg2rad = (3.1415926f/180.0f);
06021
06022 int NSAM,NROW,NNNN,NR2,NANG,L,JY;
06023
06024 NSAM = PROJ->get_xsize();
06025 NROW = PROJ->get_ysize();
06026 NNNN = NSAM+2-(NSAM%2);
06027 NR2 = NROW/2;
06028 NANG = int(SS.size())/6;
06029
06030 float RI[9];
06031 RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
06032 RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
06033 RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
06034 RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
06035 RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
06036 RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
06037 RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
06038 RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
06039 RI(3,3)=SS(3,NUMP);
06040
06041 float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
06042
06043 EMData* W = new EMData();
06044 int Wnx = NNNN/2;
06045 W->set_size(NNNN/2,NROW,1);
06046 W->to_one();
06047 float *Wptr = W->get_data();
06048
06049 float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
06050
06051 for (L=1; L<=NANG; L++) {
06052 if (L != NUMP) {
06053 CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
06054 CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
06055 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);
06056
06057 TMP = sqrt(CC(1)*CC(1) + CC(2)*CC(2) + CC(3)*CC(3));
06058 CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
06059 ALPHA=rad2deg*float(asin(CCN));
06060 if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
06061 if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
06062 if(ALPHA<1.0E-6) {
06063 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
06064 } else {
06065 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
06066 CC(1) = CC(1)/CCN;CC(2) = CC(2)/CCN;CC(3) = CC(3)/CCN;
06067 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
06068 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
06069 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
06070 CP(1) = 0.0;CP(2) = 0.0;
06071 VP(1) = 0.0;VP(2) = 0.0;
06072
06073 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
06074 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
06075 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
06076 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
06077
06078 TMP = CP(1)*VP(2)-CP(2)*VP(1);
06079
06080
06081 TMP = AMAX1(1.0E-4f,fabs(TMP));
06082 float tmpinv = 1.0f/TMP;
06083 for(int J=1;J<=NROW;J++) {
06084 JY = (J-1);
06085 if (JY>NR2) JY=JY-NROW;
06086 for(int I=1;I<=NNNN/2;I++) {
06087 FV = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
06088 RT = 1.0f-FV/FM;
06089 W(I,J) += ((RT>0.0f)*RT);
06090 }
06091 }
06092 }
06093 }
06094 }
06095
06096 EMData* proj_in = PROJ;
06097
06098 PROJ = PROJ->norm_pad( false, 1);
06099 PROJ->do_fft_inplace();
06100 PROJ->update();
06101 float *PROJptr = PROJ->get_data();
06102
06103 int KX;
06104 float WW;
06105 for(int J=1; J<=NROW; J++)
06106 for(int I=1; I<=NNNN; I+=2) {
06107 KX = (I+1)/2;
06108 WW = 1.0f/W(KX,J);
06109 PROJ(I,J) = PROJ(I,J)*WW;
06110 PROJ(I+1,J) = PROJ(I+1,J)*WW;
06111 }
06112 delete W; W = 0;
06113 PROJ->do_ift_inplace();
06114 PROJ->depad();
06115
06116 float* data_src = PROJ->get_data();
06117 float* data_dst = proj_in->get_data();
06118
06119 int ntotal = NSAM*NROW;
06120 for( int i=0; i < ntotal; ++i ) data_dst[i] = data_src[i];
06121
06122 proj_in->update();
06123 delete PROJ;
06124 }
06125 #undef AMAX1
06126 #undef AMIN1
06127 #undef RI
06128 #undef CC
06129 #undef CP
06130 #undef VV
06131 #undef VP
06132
06133
06134 #undef W
06135 #undef SS
06136 #undef PROJ
06137
06138 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06139 {
06140 float cst = cs*1.0e7f;
06141
06142 wgh /= 100.0;
06143 float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06144 float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06145 float ak2 = ak*ak;
06146 float g1 = dzz*1.0e4f*lambda*ak2;
06147 float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06148
06149 float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06150 if(b_factor != 0.0f) ctfv *= exp(-b_factor*ak2/4.0f);
06151
06152 return ctfv;
06153 }
06154
06155 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06156 {
06157
06158
06159
06160 int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();
06161
06162
06163
06164 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06165 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06166
06167 size_t i, size = (size_t)nx*ny*nz;
06168
06169 float* img_ptr = image->get_data();
06170 float* mask_ptr = mask->get_data();
06171
06172 int ln=0;
06173 for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06174
06175 EMData* new_image = new EMData();
06176 new_image->set_size(ln,1,1);
06177 float *new_ptr = new_image->get_data();
06178
06179 ln=-1;
06180 for(i = 0;i < size;i++){
06181 if(mask_ptr[i] > 0.5f) {
06182 ln++;
06183 new_ptr[ln]=img_ptr[i];
06184 }
06185 }
06186
06187 return new_image;
06188 }
06189
06190 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06191 {
06192
06193
06194
06195 if(mask == NULL)
06196 throw ImageDimensionException("The mask cannot be an null image");
06197
06198
06199
06200
06201 int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06202
06203 size_t i,size = (size_t)nx*ny*nz;
06204
06205 EMData *new_image = new EMData();
06206 new_image->set_size(nx,ny,nz);
06207 float *new_ptr = new_image->get_data();
06208 float *mask_ptr = mask->get_data();
06209 float *img_ptr = image->get_data();
06210 int count = 0;
06211 float sum_under_mask = 0.0 ;
06212 for(i = 0;i < size;i++){
06213 if(mask_ptr[i] > 0.5f){
06214 new_ptr[i] = img_ptr[count];
06215 sum_under_mask += img_ptr[count];
06216 count++;
06217 if( count > image->get_xsize() ) {
06218 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06219 }
06220 }
06221 }
06222
06223 if( count > image->get_xsize() ) {
06224 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06225 }
06226
06227 float avg_under_mask = sum_under_mask / count;
06228 for(i = 0;i < size;i++) {
06229 if(mask_ptr[i] <= 0.5f) new_ptr[i] = avg_under_mask;
06230 }
06231 new_image->update();
06232 return new_image;
06233 }
06234
06235
06236
06237 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06238 {
06239 vector<float>new_peak;
06240 int n1=peak1.size()/3;
06241 float p_size2=p_size*p_size;
06242 for (int i=0;i<n1;++i) {
06243 vector<float>::iterator it2= peak1.begin()+3*i;
06244 bool push_back1=true;
06245 int n2=peak2.size()/3;
06246
06247
06248 if(n2 ==0) {
06249 new_peak.push_back(*it2);
06250 new_peak.push_back(*(it2+1));
06251 new_peak.push_back(*(it2+2));
06252 } else {
06253 int j=0;
06254 while (j< n2-1 ) {
06255 vector<float>::iterator it3= peak2.begin()+3*j;
06256 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06257 if(d2< p_size2 ) {
06258 if( (*it2)<(*it3) ) {
06259 new_peak.push_back(*it3);
06260 new_peak.push_back(*(it3+1));
06261 new_peak.push_back(*(it3+2));
06262 peak2.erase(it3);
06263 peak2.erase(it3);
06264 peak2.erase(it3);
06265 push_back1=false;
06266 } else {
06267 peak2.erase(it3);
06268 peak2.erase(it3);
06269 peak2.erase(it3);
06270 }
06271 } else j=j+1;
06272 n2=peak2.size()/3;
06273 }
06274 if(push_back1) {
06275 new_peak.push_back(*it2);
06276 new_peak.push_back(*(it2+1));
06277 new_peak.push_back(*(it2+2));
06278 }
06279 }
06280 }
06281 return new_peak;
06282 }
06283
06284 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06285 {
06286
06287
06288
06289
06290
06291 ENTERFUNC;
06292
06293 int i;
06294
06295
06296 for ( i = 0 ; i < n * n ; i++ ) eigvec[i] = covmat[i];
06297
06298 char NEEDV = 'V';
06299 char UPLO = 'U';
06300 int lwork = -1;
06301 int info = 0;
06302 float *work, wsize;
06303
06304
06305 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06306 lwork = (int)wsize;
06307
06308 work = (float *)calloc(lwork, sizeof(float));
06309
06310 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06311 free(work);
06312 EXITFUNC;
06313 return info;
06314 }
06315
06316 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06317 {
06318
06319 ENTERFUNC;
06320 int len = covmatpy.size();
06321 float *eigvec;
06322 float *eigval;
06323 float *covmat;
06324 int status = 0;
06325 eigval = (float*)calloc(ncov,sizeof(float));
06326 eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06327 covmat = (float*)calloc(ncov*ncov, sizeof(float));
06328
06329 const float *covmat_ptr;
06330 covmat_ptr = &covmatpy[0];
06331 for(int i=0;i<len;i++){
06332 covmat[i] = covmat_ptr[i];
06333 }
06334
06335 status = Util::coveig(ncov, covmat, eigval, eigvec);
06336
06337 vector<float> eigval_py(ncov);
06338 const float *eigval_ptr;
06339 eigval_ptr = &eigval[0];
06340 for(int i=0;i<ncov;i++){
06341 eigval_py[i] = eigval_ptr[i];
06342 }
06343
06344 vector<float> eigvec_py(ncov*ncov);
06345 const float *eigvec_ptr;
06346 eigvec_ptr = &eigvec[0];
06347 for(int i=0;i<ncov*ncov;i++){
06348 eigvec_py[i] = eigvec_ptr[i];
06349 }
06350
06351 Dict res;
06352 res["eigval"] = eigval_py;
06353 res["eigvec"] = eigvec_py;
06354
06355 EXITFUNC;
06356 return res;
06357 }
06358
06359 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06360 {
06361 int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06362
06363 k=(int)pw.size();
06364 l=0;
06365 m=k;
06366 n2=n+2;
06367 n1=n+1;
06368 klmd=k+l+m;
06369 klm2d= k+l+m+2;
06370 nklmd=k+l+m+n;
06371 n2d=n+2;
06372
06373 n_larg=klmd*2;
06374 klm2d=n_larg+klm2d;
06375 klmd=n_larg+klmd;
06376 nklmd=n_larg+nklmd;
06377 int size_q=klm2d*n2d;
06378 int size_cu=nklmd*2;
06379 static int i__;
06380
06381 double *q ;
06382 double *x ;
06383 double *res;
06384 double *cu;
06385 float *q2;
06386 float *pw_;
06387 long int *iu;
06388 double *s;
06389 q = (double*)calloc(size_q,sizeof(double));
06390 x = (double*)calloc(n2d,sizeof(double));
06391 res = (double*)calloc(klmd,sizeof(double));
06392 cu =(double*)calloc(size_cu,sizeof(double));
06393 s = (double*)calloc(klmd,sizeof(double));
06394 q2 = (float*)calloc(size_q,sizeof(float));
06395 iu = (long int*)calloc(size_cu,sizeof(long int));
06396 pw_ = (float*)calloc(k,sizeof(float));
06397
06398 for( i__ =0;i__<k;++i__)
06399 {
06400 pw_[i__]=log(pw[i__]); }
06401 long int l_k=k;
06402 long int l_n=n;
06403 long int l_iswi=iswi;
06404 vector<float> cl1_res;
06405 cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06406 free(q);
06407 free(x);
06408 free(res);
06409 free(s);
06410 free(cu);
06411 free(q2);
06412 free(iu);
06413 free(pw_);
06414 return cl1_res;
06415 }
06416 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)
06417 {
06418 long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06419 float r__1;
06420 int tmp__i;
06421 long int i__, j;
06422 --s;
06423 --res;
06424 iu -= 3;
06425 cu -= 3;
06426 --x;
06427 long int klm2d;
06428 klm2d= *k+*k+2;
06429 klm2d=klm2d+klm2d;
06430 q_dim1 = klm2d;
06431 q_offset = 1 + q_dim1;
06432 q -= q_offset;
06433 q2_dim1 = klm2d;
06434 q2_offset = 1 + q2_dim1;
06435 q2 -= q2_offset;
06436 i__2=0;
06437 i__1 = *n - 1;
06438 tmp__i=0;
06439 for (j = 1; j <= i__1; ++j) {
06440 i__2 = *k;
06441 tmp__i+=1;
06442 for (i__ = 1; i__ <= i__2; ++i__) {
06443 r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06444 q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06445 }
06446 }
06447 for (i__ = 1; i__ <= i__2; ++i__)
06448 { q2[i__ + *n * q2_dim1] = 1.f;
06449 q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06450 }
06451 vector<float> fit_res;
06452 fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06453 return fit_res;
06454 }
06455 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)
06456 {
06457
06458 long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06459
06460
06461 long int i__, j, m, n1, ii, jj;
06462 double tmp;
06463 vector<float> p;
06464 --x;
06465 q_dim1 = *klm2d;
06466 q_offset = 1 + q_dim1;
06467 q -= q_offset;
06468 q1_dim1 = *klm2d;
06469 q1_offset = 1 + q1_dim1;
06470 q1 -= q1_offset;
06471 --s;
06472 --res;
06473 iu -= 3;
06474 cu -= 3;
06475
06476
06477 long int l = 0;
06478
06479
06480 m = *ks;
06481 n1 = *n + 1;
06482 if (*iswi == 1) {
06483 i__1 = n1;
06484 for (jj = 1; jj <= i__1; ++jj) {
06485 i__2 = *ks;
06486 for (ii = 1; ii <= i__2; ++ii) {
06487
06488
06489 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06490 ;
06491 }
06492 }
06493 } else if (*iswi == 2) {
06494 i__1 = *ks;
06495 for (ii = 1; ii <= i__1; ++ii) {
06496 i__2 = n1;
06497 for (jj = 1; jj <= i__2; ++jj) {
06498 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06499 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06500 }
06501 }
06502 } else if (*iswi == 3) {
06503 l = 2;
06504 i__1 = n1;
06505 for (jj = 1; jj <= i__1; ++jj) {
06506 i__2 = *ks + 2;
06507 for (ii = 1; ii <= i__2; ++ii) {
06508 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06509 }
06510 i__2 = *ks;
06511 for (ii = 1; ii <= i__2; ++ii) {
06512 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06513 }
06514 }
06515 } else if (*iswi == 4) {
06516 l = 2;
06517 i__1 = n1;
06518 for (jj = 1; jj <= i__1; ++jj) {
06519 i__2 = *ks + 2;
06520 for (ii = 1; ii <= i__2; ++ii) {
06521 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06522 }
06523 i__2 = *ks;
06524 for (ii = 1; ii <= i__2; ++ii) {
06525 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06526 }
06527 }
06528 } else if (*iswi == 5) {
06529 l = 1;
06530 i__1 = n1;
06531 for (jj = 1; jj <= i__1; ++jj) {
06532 i__2 = *ks + 1;
06533 for (ii = 1; ii <= i__2; ++ii) {
06534 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06535 }
06536 i__2 = *ks;
06537 for (ii = 1; ii <= i__2; ++ii) {
06538 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06539 }
06540 }
06541 } else if (*iswi == 6) {
06542 l = 1;
06543 i__1 = n1;
06544 for (jj = 1; jj <= i__1; ++jj) {
06545 i__2 = *ks + 1;
06546 for (ii = 1; ii <= i__2; ++ii) {
06547 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06548 }
06549 i__2 = *ks;
06550 for (ii = 1; ii <= i__2; ++ii) {
06551 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06552 }
06553 }
06554 } else if (*iswi == 7) {
06555 l = 3;
06556 i__1 = n1;
06557 for (jj = 1; jj <= i__1; ++jj) {
06558 i__2 = *ks + 3;
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 + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06565 }
06566 }
06567 } else if (*iswi == 8) {
06568 l = 4;
06569 i__1 = n1;
06570 for (jj = 1; jj <= i__1; ++jj) {
06571 i__2 = *ks + 4;
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 + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06578 }
06579 }
06580 }
06581
06582 Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06583 i__1 = *ks;
06584 int tmp__j=0;
06585 for (i__ = 1; i__ <= i__1; ++i__) {
06586 tmp = 0.f;
06587 i__2 = *n - 1;
06588 for (j = 1; j <= i__2; ++j) {
06589 tmp__j=j;
06590 tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06591 }
06592 tmp += x[*n];
06593 p.push_back(static_cast<float>(exp(tmp)));
06594 p.push_back(q1[i__ + q1_dim1]);
06595 }
06596 i__2=*n;
06597 for (i__=1;i__<=i__2;++i__)
06598 { p.push_back(static_cast<float>(x[i__]));}
06599 return p;
06600 }
06601 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06602 double *q, double *x, double *res, double *cu, long int *iu, double *s)
06603 {
06604
06605 long int q_dim1, q_offset, i__1, i__2;
06606 double d__1;
06607
06608 static long int i__, j;
06609 static double z__;
06610 static long int n1, n2, ia, ii, kk, in, nk, js;
06611 static double sn, zu, zv;
06612 static long int nk1, klm, nkl, jmn, jpn;
06613 static double cuv;
06614 static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06615 static float xmin;
06616 static double xmax;
06617 static long int iout;
06618 static double xsum;
06619 static long int iineg, maxit;
06620 static double toler;
06621 static float error;
06622 static double pivot;
06623 static long int kforce, iphase;
06624 static double tpivot;
06625
06626 --s;
06627 --res;
06628 iu -= 3;
06629 cu -= 3;
06630 --x;
06631 q_dim1 = *klm2d;
06632 q_offset = 1 + q_dim1;
06633 q -= q_offset;
06634
06635
06636 maxit = 500;
06637 kode = 0;
06638 toler = 1e-4f;
06639 iter = 0;
06640 n1 = *n + 1;
06641 n2 = *n + 2;
06642 nk = *n + *k;
06643 nk1 = nk + 1;
06644 nkl = nk + *l;
06645 nkl1 = nkl + 1;
06646 klm = *k + *l + *m;
06647 klm1 = klm + 1;
06648 klm2 = klm + 2;
06649 nklm = *n + klm;
06650 kforce = 1;
06651 iter = 0;
06652 js = 1;
06653 ia = 0;
06654
06655 i__1 = *n;
06656 for (j = 1; j <= i__1; ++j) {
06657 q[klm2 + j * q_dim1] = (double) j;
06658
06659 }
06660 i__1 = klm;
06661 for (i__ = 1; i__ <= i__1; ++i__) {
06662 q[i__ + n2 * q_dim1] = (double) (*n + i__);
06663 if (q[i__ + n1 * q_dim1] >= 0.f) {
06664 goto L30;
06665 }
06666 i__2 = n2;
06667 for (j = 1; j <= i__2; ++j) {
06668 q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06669
06670 }
06671 L30:
06672 ;
06673 }
06674
06675 iphase = 2;
06676 i__1 = nklm;
06677 for (j = 1; j <= i__1; ++j) {
06678 cu[(j << 1) + 1] = 0.f;
06679 cu[(j << 1) + 2] = 0.f;
06680 iu[(j << 1) + 1] = 0;
06681 iu[(j << 1) + 2] = 0;
06682
06683 }
06684 if (*l == 0) {
06685 goto L60;
06686 }
06687 i__1 = nkl;
06688 for (j = nk1; j <= i__1; ++j) {
06689 cu[(j << 1) + 1] = 1.f;
06690 cu[(j << 1) + 2] = 1.f;
06691 iu[(j << 1) + 1] = 1;
06692 iu[(j << 1) + 2] = 1;
06693
06694 }
06695 iphase = 1;
06696 L60:
06697 if (*m == 0) {
06698 goto L80;
06699 }
06700 i__1 = nklm;
06701 for (j = nkl1; j <= i__1; ++j) {
06702 cu[(j << 1) + 2] = 1.f;
06703 iu[(j << 1) + 2] = 1;
06704 jmn = j - *n;
06705 if (q[jmn + n2 * q_dim1] < 0.f) {
06706 iphase = 1;
06707 }
06708
06709 }
06710 L80:
06711 if (kode == 0) {
06712 goto L150;
06713 }
06714 i__1 = *n;
06715 for (j = 1; j <= i__1; ++j) {
06716 if ((d__1 = x[j]) < 0.) {
06717 goto L90;
06718 } else if (d__1 == 0) {
06719 goto L110;
06720 } else {
06721 goto L100;
06722 }
06723 L90:
06724 cu[(j << 1) + 1] = 1.f;
06725 iu[(j << 1) + 1] = 1;
06726 goto L110;
06727 L100:
06728 cu[(j << 1) + 2] = 1.f;
06729 iu[(j << 1) + 2] = 1;
06730 L110:
06731 ;
06732 }
06733 i__1 = *k;
06734 for (j = 1; j <= i__1; ++j) {
06735 jpn = j + *n;
06736 if ((d__1 = res[j]) < 0.) {
06737 goto L120;
06738 } else if (d__1 == 0) {
06739 goto L140;
06740 } else {
06741 goto L130;
06742 }
06743 L120:
06744 cu[(jpn << 1) + 1] = 1.f;
06745 iu[(jpn << 1) + 1] = 1;
06746 if (q[j + n2 * q_dim1] > 0.f) {
06747 iphase = 1;
06748 }
06749 goto L140;
06750 L130:
06751 cu[(jpn << 1) + 2] = 1.f;
06752 iu[(jpn << 1) + 2] = 1;
06753 if (q[j + n2 * q_dim1] < 0.f) {
06754 iphase = 1;
06755 }
06756 L140:
06757 ;
06758 }
06759 L150:
06760 if (iphase == 2) {
06761 goto L500;
06762 }
06763
06764 L160:
06765 i__1 = n1;
06766 for (j = js; j <= i__1; ++j) {
06767 xsum = 0.;
06768 i__2 = klm;
06769 for (i__ = 1; i__ <= i__2; ++i__) {
06770 ii = (long int) q[i__ + n2 * q_dim1];
06771 if (ii < 0) {
06772 goto L170;
06773 }
06774 z__ = cu[(ii << 1) + 1];
06775 goto L180;
06776 L170:
06777 iineg = -ii;
06778 z__ = cu[(iineg << 1) + 2];
06779 L180:
06780 xsum += q[i__ + j * q_dim1] * z__;
06781
06782
06783 }
06784 q[klm1 + j * q_dim1] = xsum;
06785
06786 }
06787 i__1 = *n;
06788 for (j = js; j <= i__1; ++j) {
06789 ii = (long int) q[klm2 + j * q_dim1];
06790 if (ii < 0) {
06791 goto L210;
06792 }
06793 z__ = cu[(ii << 1) + 1];
06794 goto L220;
06795 L210:
06796 iineg = -ii;
06797 z__ = cu[(iineg << 1) + 2];
06798 L220:
06799 q[klm1 + j * q_dim1] -= z__;
06800
06801 }
06802
06803 L240:
06804 xmax = 0.f;
06805 if (js > *n) {
06806 goto L490;
06807 }
06808 i__1 = *n;
06809 for (j = js; j <= i__1; ++j) {
06810 zu = q[klm1 + j * q_dim1];
06811 ii = (long int) q[klm2 + j * q_dim1];
06812 if (ii > 0) {
06813 goto L250;
06814 }
06815 ii = -ii;
06816 zv = zu;
06817 zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06818 goto L260;
06819 L250:
06820 zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06821 L260:
06822 if (kforce == 1 && ii > *n) {
06823 goto L280;
06824 }
06825 if (iu[(ii << 1) + 1] == 1) {
06826 goto L270;
06827 }
06828 if (zu <= xmax) {
06829 goto L270;
06830 }
06831 xmax = zu;
06832 in = j;
06833 L270:
06834 if (iu[(ii << 1) + 2] == 1) {
06835 goto L280;
06836 }
06837 if (zv <= xmax) {
06838 goto L280;
06839 }
06840 xmax = zv;
06841 in = j;
06842 L280:
06843 ;
06844 }
06845 if (xmax <= toler) {
06846 goto L490;
06847 }
06848 if (q[klm1 + in * q_dim1] == xmax) {
06849 goto L300;
06850 }
06851 i__1 = klm2;
06852 for (i__ = 1; i__ <= i__1; ++i__) {
06853 q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06854
06855 }
06856 q[klm1 + in * q_dim1] = xmax;
06857
06858 L300:
06859 if (iphase == 1 || ia == 0) {
06860 goto L330;
06861 }
06862 xmax = 0.f;
06863 i__1 = ia;
06864 for (i__ = 1; i__ <= i__1; ++i__) {
06865 z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06866 if (z__ <= xmax) {
06867 goto L310;
06868 }
06869 xmax = z__;
06870 iout = i__;
06871 L310:
06872 ;
06873 }
06874 if (xmax <= toler) {
06875 goto L330;
06876 }
06877 i__1 = n2;
06878 for (j = 1; j <= i__1; ++j) {
06879 z__ = q[ia + j * q_dim1];
06880 q[ia + j * q_dim1] = q[iout + j * q_dim1];
06881 q[iout + j * q_dim1] = z__;
06882
06883 }
06884 iout = ia;
06885 --ia;
06886 pivot = q[iout + in * q_dim1];
06887 goto L420;
06888 L330:
06889 kk = 0;
06890 i__1 = klm;
06891 for (i__ = 1; i__ <= i__1; ++i__) {
06892 z__ = q[i__ + in * q_dim1];
06893 if (z__ <= toler) {
06894 goto L340;
06895 }
06896 ++kk;
06897 res[kk] = q[i__ + n1 * q_dim1] / z__;
06898 s[kk] = (double) i__;
06899 L340:
06900 ;
06901 }
06902 L350:
06903 if (kk > 0) {
06904 goto L360;
06905 }
06906 kode = 2;
06907 goto L590;
06908 L360:
06909 xmin = static_cast<float>( res[1] );
06910 iout = (long int) s[1];
06911 j = 1;
06912 if (kk == 1) {
06913 goto L380;
06914 }
06915 i__1 = kk;
06916 for (i__ = 2; i__ <= i__1; ++i__) {
06917 if (res[i__] >= xmin) {
06918 goto L370;
06919 }
06920 j = i__;
06921 xmin = static_cast<float>( res[i__] );
06922 iout = (long int) s[i__];
06923 L370:
06924 ;
06925 }
06926 res[j] = res[kk];
06927 s[j] = s[kk];
06928 L380:
06929 --kk;
06930 pivot = q[iout + in * q_dim1];
06931 ii = (long int) q[iout + n2 * q_dim1];
06932 if (iphase == 1) {
06933 goto L400;
06934 }
06935 if (ii < 0) {
06936 goto L390;
06937 }
06938 if (iu[(ii << 1) + 2] == 1) {
06939 goto L420;
06940 }
06941 goto L400;
06942 L390:
06943 iineg = -ii;
06944 if (iu[(iineg << 1) + 1] == 1) {
06945 goto L420;
06946 }
06947
06948 L400:
06949 ii = abs(ii);
06950 cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06951 if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06952 goto L420;
06953 }
06954
06955 i__1 = n1;
06956 for (j = js; j <= i__1; ++j) {
06957 z__ = q[iout + j * q_dim1];
06958 q[klm1 + j * q_dim1] -= z__ * cuv;
06959 q[iout + j * q_dim1] = -z__;
06960
06961 }
06962 q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06963 goto L350;
06964
06965 L420:
06966 if (iter < maxit) {
06967 goto L430;
06968 }
06969 kode = 3;
06970 goto L590;
06971 L430:
06972 ++iter;
06973 i__1 = n1;
06974 for (j = js; j <= i__1; ++j) {
06975 if (j != in) {
06976 q[iout + j * q_dim1] /= pivot;
06977 }
06978
06979 }
06980
06981
06982
06983
06984
06985
06986
06987
06988 i__1 = n1;
06989 for (j = js; j <= i__1; ++j) {
06990 if (j == in) {
06991 goto L460;
06992 }
06993 z__ = -q[iout + j * q_dim1];
06994 i__2 = klm1;
06995 for (i__ = 1; i__ <= i__2; ++i__) {
06996 if (i__ != iout) {
06997 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06998 }
06999
07000 }
07001 L460:
07002 ;
07003 }
07004 tpivot = -pivot;
07005 i__1 = klm1;
07006 for (i__ = 1; i__ <= i__1; ++i__) {
07007 if (i__ != iout) {
07008 q[i__ + in * q_dim1] /= tpivot;
07009 }
07010
07011 }
07012 q[iout + in * q_dim1] = 1.f / pivot;
07013 z__ = q[iout + n2 * q_dim1];
07014 q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
07015 q[klm2 + in * q_dim1] = z__;
07016 ii = (long int) abs(z__);
07017 if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
07018 goto L240;
07019 }
07020 i__1 = klm2;
07021 for (i__ = 1; i__ <= i__1; ++i__) {
07022 z__ = q[i__ + in * q_dim1];
07023 q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
07024 q[i__ + js * q_dim1] = z__;
07025
07026 }
07027 ++js;
07028 goto L240;
07029
07030 L490:
07031 if (kforce == 0) {
07032 goto L580;
07033 }
07034 if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
07035 goto L500;
07036 }
07037 kforce = 0;
07038 goto L240;
07039
07040 L500:
07041 iphase = 2;
07042 i__1 = nklm;
07043 for (j = 1; j <= i__1; ++j) {
07044 cu[(j << 1) + 1] = 0.f;
07045 cu[(j << 1) + 2] = 0.f;
07046
07047 }
07048 i__1 = nk;
07049 for (j = n1; j <= i__1; ++j) {
07050 cu[(j << 1) + 1] = 1.f;
07051 cu[(j << 1) + 2] = 1.f;
07052
07053 }
07054 i__1 = klm;
07055 for (i__ = 1; i__ <= i__1; ++i__) {
07056 ii = (long int) q[i__ + n2 * q_dim1];
07057 if (ii > 0) {
07058 goto L530;
07059 }
07060 ii = -ii;
07061 if (iu[(ii << 1) + 2] == 0) {
07062 goto L560;
07063 }
07064 cu[(ii << 1) + 2] = 0.f;
07065 goto L540;
07066 L530:
07067 if (iu[(ii << 1) + 1] == 0) {
07068 goto L560;
07069 }
07070 cu[(ii << 1) + 1] = 0.f;
07071 L540:
07072 ++ia;
07073 i__2 = n2;
07074 for (j = 1; j <= i__2; ++j) {
07075 z__ = q[ia + j * q_dim1];
07076 q[ia + j * q_dim1] = q[i__ + j * q_dim1];
07077 q[i__ + j * q_dim1] = z__;
07078
07079 }
07080 L560:
07081 ;
07082 }
07083 goto L160;
07084 L570:
07085 if (q[klm1 + n1 * q_dim1] <= toler) {
07086 goto L500;
07087 }
07088 kode = 1;
07089 goto L590;
07090 L580:
07091 if (iphase == 1) {
07092 goto L570;
07093 }
07094
07095 kode = 0;
07096 L590:
07097 xsum = 0.;
07098 i__1 = *n;
07099 for (j = 1; j <= i__1; ++j) {
07100 x[j] = 0.f;
07101
07102 }
07103 i__1 = klm;
07104 for (i__ = 1; i__ <= i__1; ++i__) {
07105 res[i__] = 0.f;
07106
07107 }
07108 i__1 = klm;
07109 for (i__ = 1; i__ <= i__1; ++i__) {
07110 ii = (long int) q[i__ + n2 * q_dim1];
07111 sn = 1.f;
07112 if (ii > 0) {
07113 goto L620;
07114 }
07115 ii = -ii;
07116 sn = -1.f;
07117 L620:
07118 if (ii > *n) {
07119 goto L630;
07120 }
07121 x[ii] = sn * q[i__ + n1 * q_dim1];
07122 goto L640;
07123 L630:
07124 iimn = ii - *n;
07125 res[iimn] = sn * q[i__ + n1 * q_dim1];
07126 if (ii >= n1 && ii <= nk) {
07127 xsum += q[i__ + n1 * q_dim1];
07128 }
07129 L640:
07130 ;
07131 }
07132 error = (float)xsum;
07133 return;
07134 }
07135
07136 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07137 {
07138 int j,d;
07139 EMData * e = new EMData();
07140 float *eptr, *imgptr;
07141 imgptr = img->get_data();
07142 float SSE = 0.f;
07143 for (j = 0 ; j < N ; j++) {
07144 e->read_image(images,S[j]);
07145 eptr = e->get_data();
07146 for (d = 0; d < size; d++) {
07147 SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07148 }
07149 delete e;
07150 return SSE;
07151 }
07152
07153
07154 #define mymax(x,y) (((x)>(y))?(x):(y))
07155 #define mymin(x,y) (((x)<(y))?(x):(y))
07156 #define sign(x,y) (((((y)>0)?(1):(-1))*(y!=0))*(x))
07157
07158
07159 #define quadpi 3.141592653589793238462643383279502884197
07160 #define dgr_to_rad quadpi/180
07161 #define deg_to_rad quadpi/180
07162 #define rad_to_deg 180/quadpi
07163 #define rad_to_dgr 180/quadpi
07164 #define TRUE 1
07165 #define FALSE 0
07166
07167
07168 #define theta(i) theta [i-1]
07169 #define phi(i) phi [i-1]
07170 #define weight(i) weight [i-1]
07171 #define lband(i) lband [i-1]
07172 #define ts(i) ts [i-1]
07173 #define thetast(i) thetast [i-1]
07174 #define key(i) key [i-1]
07175
07176
07177 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07178 {
07179
07180 ENTERFUNC;
07181
07182 if ( th.size() != ph.size() ) {
07183 LOGERR("images not same size");
07184 throw ImageFormatException( "images not same size");
07185 }
07186
07187
07188 srand(10);
07189
07190 int i,*key;
07191 int len = th.size();
07192 double *theta,*phi,*weight;
07193 theta = (double*) calloc(len,sizeof(double));
07194 phi = (double*) calloc(len,sizeof(double));
07195 weight = (double*) calloc(len,sizeof(double));
07196 key = (int*) calloc(len,sizeof(int));
07197 const float *thptr, *phptr;
07198
07199 thptr = &th[0];
07200 phptr = &ph[0];
07201 for(i=1;i<=len;i++){
07202 key(i) = i;
07203 weight(i) = 0.0;
07204 }
07205
07206 for(i = 0;i<len;i++){
07207 theta[i] = thptr[i];
07208 phi[i] = phptr[i];
07209 }
07210
07211
07212 Util::hsortd(theta, phi, key, len, 1);
07213
07214
07215 Util::voronoi(phi, theta, weight, len);
07216
07217
07218 Util::hsortd(weight, weight, key, len, 2);
07219
07220 free(theta);
07221 free(phi);
07222 free(key);
07223 vector<double> wt;
07224 double count = 0;
07225 for(i=1; i<= len; i++)
07226 {
07227 wt.push_back(weight(i));
07228 count += weight(i);
07229 }
07230
07231
07232
07233
07234
07235
07236 free(weight);
07237
07238 EXITFUNC;
07239 return wt;
07240
07241 }
07242
07243 struct tmpstruct{
07244 double theta1,phi1;
07245 int key1;
07246 };
07247
07248 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07249 {
07250 ENTERFUNC;
07251 vector<tmpstruct> tmp(len);
07252 int i;
07253 for(i = 1;i<=len;i++)
07254 {
07255 tmp[i-1].theta1 = theta(i);
07256 tmp[i-1].phi1 = phi(i);
07257 tmp[i-1].key1 = key(i);
07258 }
07259
07260 if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07261 if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07262
07263 for(i = 1;i<=len;i++)
07264 {
07265 theta(i) = tmp[i-1].theta1;
07266 phi(i) = tmp[i-1].phi1;
07267 key(i) = tmp[i-1].key1;
07268 }
07269 EXITFUNC;
07270 }
07271
07272 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07273 {
07274 return(tmp1.theta1 < tmp2.theta1);
07275 }
07276
07277 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07278 {
07279 return(tmp1.key1 < tmp2.key1);
07280 }
07281
07282
07283
07284
07285
07286
07287
07288
07289
07290
07291
07292
07293
07294
07295
07296
07297
07298
07299
07300
07301
07302
07303
07304
07305
07306
07307
07308
07309
07310
07311
07312
07313
07314
07315
07316
07317
07318
07319
07320
07321
07322
07323
07324
07325
07326
07327
07328
07329
07330
07331
07332
07333
07334
07335
07336
07337
07338
07339
07340
07341
07342
07343
07344
07345
07346
07347
07348
07349
07350
07351
07352
07353
07354
07355
07356
07357
07358
07359
07360
07361
07362
07363
07364
07365
07366
07367
07368
07369
07370
07371
07372
07373
07374
07375
07376
07377
07378
07379
07380
07381
07382
07383
07384
07385
07386
07387
07388
07389
07390
07391
07392
07393
07394
07395
07396
07397
07398
07399
07400
07401
07402
07403
07404
07405
07406
07407
07408
07409
07410
07411
07412
07413
07414
07415
07416
07417
07418
07419
07420
07421
07422
07423
07424
07425
07426
07427
07428
07429
07430
07431
07432
07433
07434
07435
07436
07437
07438
07439
07440
07441
07442
07443
07444
07445
07446
07447
07448
07449
07450
07451
07452
07453
07454
07455
07456
07457
07458
07459
07460
07461
07462
07463
07464
07465
07466
07467
07468
07469
07470
07471
07472
07473
07474
07475
07476
07477
07478
07479
07480
07481
07482
07483
07484
07485
07486
07487
07488
07489
07490
07491
07492
07493
07494
07495
07496
07497
07498
07499
07500
07501
07502
07503
07504
07505
07506
07507
07508
07509
07510
07511
07512
07513
07514
07515
07516
07517
07518
07519
07520
07521
07522
07523
07524
07525
07526
07527
07528
07529
07530
07531
07532
07533
07534
07535
07536
07537
07538
07539
07540
07541
07542
07543
07544
07545
07546
07547
07548
07549
07550
07551
07552
07553
07554
07555
07556
07557
07558
07559
07560
07561
07562
07563
07564
07565
07566
07567
07568
07569
07570
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 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07601 {
07602
07603 ENTERFUNC;
07604
07605 int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07606 int nt6, n, ier, nout, lnew, mdup, nd;
07607 int i,k,mt,status;
07608
07609
07610 double *ds, *x, *y, *z;
07611 double tol = 1.0e-8;
07612 double dtol = 15;
07613 double a;
07614
07615
07616
07617
07618
07619
07620
07621
07622
07623 n = nt + nt;
07624
07625 nt6 = n*6;
07626
07627 list = (int*)calloc(nt6,sizeof(int));
07628 lptr = (int*)calloc(nt6,sizeof(int));
07629 lend = (int*)calloc(n ,sizeof(int));
07630 iwk = (int*)calloc(n ,sizeof(int));
07631 good = (int*)calloc(n ,sizeof(int));
07632 key = (int*)calloc(n ,sizeof(int));
07633 indx = (int*)calloc(n ,sizeof(int));
07634 lcnt = (int*)calloc(n ,sizeof(int));
07635
07636 ds = (double*) calloc(n,sizeof(double));
07637 x = (double*) calloc(n,sizeof(double));
07638 y = (double*) calloc(n,sizeof(double));
07639 z = (double*) calloc(n,sizeof(double));
07640
07641 if (list == NULL ||
07642 lptr == NULL ||
07643 lend == NULL ||
07644 iwk == NULL ||
07645 good == NULL ||
07646 key == NULL ||
07647 indx == NULL ||
07648 lcnt == NULL ||
07649 x == NULL ||
07650 y == NULL ||
07651 z == NULL ||
07652 ds == NULL) {
07653 printf("memory allocation failure!\n");
07654 exit(1);
07655 }
07656
07657 bool colinear=true;
07658 while(colinear)
07659 {
07660
07661 L1:
07662 for(i = 0; i<nt; i++){
07663 x[i] = theta[i];
07664 y[i] = phi[i];
07665 x[nt+i] = 180.0 - x[i];
07666 y[nt+i] = 180.0 + y[i];
07667 }
07668
07669 Util::disorder2(x, y, key, n);
07670
07671
07672 double val;
07673 for(k=0; k<2; k++){
07674 for(i=k+1; i<3; i++){
07675 val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07676 if( val < dtol) {
07677 goto L1;
07678 }
07679 }
07680 }
07681
07682 Util::ang_to_xyz(x, y, z, n);
07683
07684
07685 bool dupnode=true;
07686 dupnode=true;
07687 while(dupnode)
07688 {
07689 for(k=0; k<2; k++){
07690 for(i=k+1; i<3; i++){
07691 if( x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07692 Util::flip23(x, y, z, key, k, n);
07693 continue;
07694 }
07695 }
07696 }
07697 dupnode = false;
07698 }
07699
07700
07701 ier = 0;
07702
07703 status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07704
07705 if (status != 0) {
07706 printf(" error in trmsh3 \n");
07707 exit(1);
07708 }
07709
07710 if (ier > 0) {
07711 printf("*** Error in TRMESH: duplicate nodes encountered ***\n");
07712 exit(1);
07713 }
07714
07715 mdup=n-nout;
07716 if (ier == -2) {
07717
07718 }
07719 else
07720 {
07721 colinear=false;
07722 }
07723 }
07724
07725
07726 Assert( ier != -2 );
07727
07728
07729 nd=0;
07730 for (k=1; k<=n; k++){
07731 if (indx[k-1]>0) {
07732 nd++;
07733 good[nd-1]=k;
07734 }
07735 }
07736
07737
07738
07739
07740 for(i = 1; i<=nout; i++) {
07741 k=good[i-1];
07742
07743 if (key[k-1] <= nt) {
07744
07745 a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07746 if (ier != 0){
07747
07748
07749 printf(" *** error in areav: ier = %d ***\n", ier);
07750 weight[key[k-1]-1] =-1.0;
07751 } else {
07752
07753 weight[key[k-1]-1]=a/lcnt[i-1];
07754 }
07755 }
07756 }
07757
07758
07759
07760 for(i = 1; i<=n; i++){
07761 mt =- indx[i-1];
07762 if (mt>0){
07763 k = good[mt-1];
07764
07765
07766
07767 if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07768 }
07769 }
07770
07771 free(list);
07772 free(lend);
07773 free(iwk);
07774 free(good);
07775 free(key);
07776 free(lptr);
07777 free(indx);
07778 free(lcnt);
07779 free(ds);
07780 free(x);
07781 free(y);
07782 free(z);
07783
07784
07785 EXITFUNC;
07786 }
07787
07788 void Util::disorder2(double *x,double *y, int *key, int len)
07789 {
07790 ENTERFUNC;
07791 int k, i;
07792 for(i=0; i<len; i++) key[i]=i+1;
07793
07794 for(i = 0; i<len;i++){
07795 k = rand()%len;
07796 std::swap(key[k], key[i]);
07797 std::swap(x[k], x[i]);
07798 std::swap(y[k], y[i]);
07799 }
07800 EXITFUNC;
07801 }
07802
07803 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07804 {
07805 ENTERFUNC;
07806 double costheta,sintheta,cosphi,sinphi;
07807 for(int i = 0; i<len; i++)
07808 {
07809 cosphi = cos(y[i]*dgr_to_rad);
07810 sinphi = sin(y[i]*dgr_to_rad);
07811 if(fabs(x[i]-90.0)< 1.0e-5){
07812 x[i] = cosphi;
07813 y[i] = sinphi;
07814 z[i] = 0.0;
07815 }
07816 else{
07817 costheta = cos(x[i]*dgr_to_rad);
07818 sintheta = sin(x[i]*dgr_to_rad);
07819 x[i] = cosphi*sintheta;
07820 y[i] = sinphi*sintheta;
07821 z[i] = costheta;
07822 }
07823 }
07824 EXITFUNC;
07825 }
07826
07827 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07828 {
07829 ENTERFUNC;
07830 int i = k;
07831 while( i == k ) i = rand()%len;
07832 std::swap(key[i], key[k]);
07833 std::swap(x[i], x[k]);
07834 std::swap(y[i], y[k]);
07835 std::swap(z[i], z[k]);
07836 EXITFUNC;
07837 }
07838
07839
07840 #undef mymax
07841 #undef mymin
07842 #undef sign
07843 #undef quadpi
07844 #undef dgr_to_rad
07845 #undef deg_to_rad
07846 #undef rad_to_deg
07847 #undef rad_to_dgr
07848 #undef TRUE
07849 #undef FALSE
07850 #undef theta
07851 #undef phi
07852 #undef weight
07853 #undef lband
07854 #undef ts
07855 #undef thetast
07856 #undef key
07857
07858
07859
07860
07861
07862
07863
07864
07865
07866
07867
07868 #define TRUE_ (1)
07869 #define FALSE_ (0)
07870 #define abs(x) ((x) >= 0 ? (x) : -(x))
07871
07872 struct stcom_{
07873 double y;
07874 };
07875 stcom_ stcom_1;
07876 #ifdef KR_headers
07877 double floor();
07878 int i_dnnt(x) double *x;
07879 #else
07880 int i_dnnt(double *x)
07881 #endif
07882 {
07883 return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07884 }
07885
07886
07887
07888
07889
07890 int Util::trmsh3_(int *n0, double *tol, double *x,
07891 double *y, double *z__, int *n, int *list, int *
07892 lptr, int *lend, int *lnew, int *indx, int *lcnt,
07893 int *near__, int *next, double *dist, int *ier)
07894 {
07895
07896 int i__1, i__2;
07897
07898
07899 static double d__;
07900 static int i__, j;
07901 static double d1, d2, d3;
07902 static int i0, lp, kt, ku, lpl, nku;
07903 extern long int left_(double *, double *, double *, double
07904 *, double *, double *, double *, double *,
07905 double *);
07906 static int nexti;
07907 extern int addnod_(int *, int *, double *,
07908 double *, double *, int *, int *, int *,
07909 int *, int *);
07910
07911
07912
07913
07914
07915
07916
07917
07918
07919
07920
07921
07922
07923
07924
07925
07926
07927
07928
07929
07930
07931
07932
07933
07934
07935
07936
07937
07938
07939
07940
07941
07942
07943
07944
07945
07946
07947
07948
07949
07950
07951
07952
07953
07954
07955
07956
07957
07958
07959
07960
07961
07962
07963
07964
07965
07966
07967
07968
07969
07970
07971
07972
07973
07974
07975
07976
07977
07978
07979
07980
07981
07982
07983
07984
07985
07986
07987
07988
07989
07990
07991
07992
07993
07994
07995
07996
07997
07998
07999
08000
08001
08002
08003
08004
08005
08006
08007
08008
08009
08010
08011
08012
08013
08014
08015
08016
08017
08018
08019
08020
08021
08022
08023
08024
08025
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 --dist;
08062 --next;
08063 --near__;
08064 --indx;
08065 --lend;
08066 --z__;
08067 --y;
08068 --x;
08069 --list;
08070 --lptr;
08071 --lcnt;
08072
08073
08074 if (*n0 < 3) {
08075 *n = 0;
08076 *ier = -1;
08077 return 0;
08078 }
08079
08080
08081
08082 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
08083 z__[3])) {
08084
08085
08086
08087 list[1] = 3;
08088 lptr[1] = 2;
08089 list[2] = -2;
08090 lptr[2] = 1;
08091 lend[1] = 2;
08092
08093 list[3] = 1;
08094 lptr[3] = 4;
08095 list[4] = -3;
08096 lptr[4] = 3;
08097 lend[2] = 4;
08098
08099 list[5] = 2;
08100 lptr[5] = 6;
08101 list[6] = -1;
08102 lptr[6] = 5;
08103 lend[3] = 6;
08104
08105 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
08106 y[3], &z__[3])) {
08107
08108
08109
08110
08111
08112 list[1] = 2;
08113 lptr[1] = 2;
08114 list[2] = -3;
08115 lptr[2] = 1;
08116 lend[1] = 2;
08117
08118 list[3] = 3;
08119 lptr[3] = 4;
08120 list[4] = -1;
08121 lptr[4] = 3;
08122 lend[2] = 4;
08123
08124 list[5] = 1;
08125 lptr[5] = 6;
08126 list[6] = -2;
08127 lptr[6] = 5;
08128 lend[3] = 6;
08129
08130
08131 } else {
08132
08133
08134
08135 *n = 0;
08136 *ier = -2;
08137 return 0;
08138 }
08139
08140
08141
08142
08143
08144 *lnew = 7;
08145 indx[1] = 1;
08146 indx[2] = 2;
08147 indx[3] = 3;
08148 lcnt[1] = 1;
08149 lcnt[2] = 1;
08150 lcnt[3] = 1;
08151 if (*n0 == 3) {
08152 *n = 3;
08153 *ier = 0;
08154 return 0;
08155 }
08156
08157
08158
08159
08160
08161
08162
08163
08164
08165
08166
08167
08168
08169
08170
08171
08172
08173
08174
08175
08176
08177
08178
08179
08180
08181
08182
08183 near__[1] = 0;
08184 near__[2] = 0;
08185 near__[3] = 0;
08186 for (ku = *n0; ku >= 4; --ku) {
08187 d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08188 d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08189 d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08190 if (d1 <= d2 && d1 <= d3) {
08191 near__[ku] = 1;
08192 dist[ku] = d1;
08193 next[ku] = near__[1];
08194 near__[1] = ku;
08195 } else if (d2 <= d1 && d2 <= d3) {
08196 near__[ku] = 2;
08197 dist[ku] = d2;
08198 next[ku] = near__[2];
08199 near__[2] = ku;
08200 } else {
08201 near__[ku] = 3;
08202 dist[ku] = d3;
08203 next[ku] = near__[3];
08204 near__[3] = ku;
08205 }
08206
08207 }
08208
08209
08210
08211
08212 kt = 3;
08213 i__1 = *n0;
08214 for (ku = 4; ku <= i__1; ++ku) {
08215 nku = near__[ku];
08216
08217
08218
08219 i__ = nku;
08220 if (near__[i__] == ku) {
08221 near__[i__] = next[ku];
08222 } else {
08223 i__ = near__[i__];
08224 L2:
08225 i0 = i__;
08226 i__ = next[i0];
08227 if (i__ != ku) {
08228 goto L2;
08229 }
08230 next[i0] = next[ku];
08231 }
08232 near__[ku] = 0;
08233
08234
08235
08236 if (dist[ku] <= *tol - 1.) {
08237 indx[ku] = -nku;
08238 ++lcnt[nku];
08239 goto L6;
08240 }
08241
08242
08243
08244 ++kt;
08245 x[kt] = x[ku];
08246 y[kt] = y[ku];
08247 z__[kt] = z__[ku];
08248 indx[ku] = kt;
08249 lcnt[kt] = 1;
08250 addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08251 , lnew, ier);
08252 if (*ier != 0) {
08253 *n = 0;
08254 *ier = -3;
08255 return 0;
08256 }
08257
08258
08259
08260 lpl = lend[kt];
08261 lp = lpl;
08262 L3:
08263 lp = lptr[lp];
08264 j = (i__2 = list[lp], abs(i__2));
08265
08266
08267
08268
08269
08270
08271
08272 i__ = near__[j];
08273 L4:
08274 if (i__ == 0) {
08275 goto L5;
08276 }
08277 nexti = next[i__];
08278
08279
08280
08281
08282 d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08283 if (d__ < dist[i__]) {
08284
08285
08286
08287
08288
08289 near__[i__] = kt;
08290 dist[i__] = d__;
08291 if (i__ == near__[j]) {
08292 near__[j] = nexti;
08293 } else {
08294 next[i0] = nexti;
08295 }
08296 next[i__] = near__[kt];
08297 near__[kt] = i__;
08298 } else {
08299 i0 = i__;
08300 }
08301
08302
08303
08304 i__ = nexti;
08305 goto L4;
08306
08307
08308
08309 L5:
08310 if (lp != lpl) {
08311 goto L3;
08312 }
08313 L6:
08314 ;
08315 }
08316 *n = kt;
08317 *ier = 0;
08318 return 0;
08319 }
08320
08321
08322 int addnod_(int *nst, int *k, double *x,
08323 double *y, double *z__, int *list, int *lptr, int
08324 *lend, int *lnew, int *ier)
08325 {
08326
08327
08328 static double tol = 0.;
08329
08330
08331 int i__1;
08332
08333
08334 static int l;
08335 static double p[3], b1, b2, b3;
08336 static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08337 extern int swap_(int *, int *, int *,
08338 int *, int *, int *, int *, int *);
08339 static int lpo1s;
08340 extern int bdyadd_(int *, int *, int *,
08341 int *, int *, int *, int *), intadd_(int *,
08342 int *, int *, int *, int *, int *, int *,
08343 int *), trfind_(int *, double *, int *,
08344 double *, double *, double *, int *, int *,
08345 int *, double *, double *, double *, int *,
08346 int *, int *), covsph_(int *, int *, int *,
08347 int *, int *, int *);
08348 extern int lstptr_(int *, int *, int *, int *);
08349 extern long int swptst_(int *, int *, int *, int *,
08350 double *, double *, double *);
08351
08352
08353
08354
08355
08356
08357
08358
08359
08360
08361
08362
08363
08364
08365
08366
08367
08368
08369
08370
08371
08372
08373
08374
08375
08376
08377
08378
08379
08380
08381
08382
08383
08384
08385
08386
08387
08388
08389
08390
08391
08392
08393
08394
08395
08396
08397
08398
08399
08400
08401
08402
08403
08404
08405
08406
08407
08408
08409
08410
08411
08412
08413
08414
08415
08416
08417
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 --lend;
08454 --z__;
08455 --y;
08456 --x;
08457 --list;
08458 --lptr;
08459
08460
08461
08462 kk = *k;
08463 if (kk < 4) {
08464 goto L3;
08465 }
08466
08467
08468 km1 = kk - 1;
08469 ist = *nst;
08470 if (ist < 1) {
08471 ist = km1;
08472 }
08473 p[0] = x[kk];
08474 p[1] = y[kk];
08475 p[2] = z__[kk];
08476
08477
08478
08479
08480 trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08481 , &b1, &b2, &b3, &i1, &i2, &i3);
08482
08483
08484
08485 if (i1 == 0) {
08486 goto L4;
08487 }
08488 l = i1;
08489 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08490 goto L5;
08491 }
08492 l = i2;
08493 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08494 goto L5;
08495 }
08496 if (i3 != 0) {
08497 l = i3;
08498 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08499 goto L5;
08500 }
08501 intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08502 } else {
08503 if (i1 != i2) {
08504 bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08505 } else {
08506 covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08507 }
08508 }
08509 *ier = 0;
08510
08511
08512
08513 lp = lend[kk];
08514 lpf = lptr[lp];
08515 io2 = list[lpf];
08516 lpo1 = lptr[lpf];
08517 io1 = (i__1 = list[lpo1], abs(i__1));
08518
08519
08520
08521 L1:
08522 lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08523 if (list[lp] < 0) {
08524 goto L2;
08525 }
08526 lp = lptr[lp];
08527 in1 = (i__1 = list[lp], abs(i__1));
08528
08529
08530
08531
08532 lpo1s = lpo1;
08533 if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08534 goto L2;
08535 }
08536 swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08537 if (lpo1 == 0) {
08538
08539
08540
08541
08542
08543
08544 lpo1 = lpo1s;
08545 goto L2;
08546 }
08547 io1 = in1;
08548 goto L1;
08549
08550
08551
08552
08553 L2:
08554 if (lpo1 == lpf || list[lpo1] < 0) {
08555 return 0;
08556 }
08557 io2 = io1;
08558 lpo1 = lptr[lpo1];
08559 io1 = (i__1 = list[lpo1], abs(i__1));
08560 goto L1;
08561
08562
08563
08564 L3:
08565 *ier = -1;
08566 return 0;
08567
08568
08569
08570 L4:
08571 *ier = -2;
08572 return 0;
08573
08574
08575
08576 L5:
08577 *ier = l;
08578 return 0;
08579 }
08580
08581 double angle_(double *v1, double *v2, double *v3)
08582 {
08583
08584 double ret_val;
08585
08586
08587
08588
08589
08590 static double a;
08591 static int i__;
08592 static double ca, s21, s23, u21[3], u23[3];
08593 extern long int left_(double *, double *, double *, double
08594 *, double *, double *, double *, double *,
08595 double *);
08596
08597
08598
08599
08600
08601
08602
08603
08604
08605
08606
08607
08608
08609
08610
08611
08612
08613
08614
08615
08616
08617
08618
08619
08620
08621
08622
08623
08624
08625
08626
08627
08628
08629
08630
08631
08632
08633
08634
08635
08636
08637
08638
08639
08640
08641
08642
08643
08644
08645
08646
08647
08648
08649
08650
08651
08652
08653
08654
08655 --v3;
08656 --v2;
08657 --v1;
08658
08659
08660 u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08661 u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08662 u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08663
08664 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08665 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08666 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08667
08668
08669
08670 s21 = 0.;
08671 s23 = 0.;
08672 for (i__ = 1; i__ <= 3; ++i__) {
08673 s21 += u21[i__ - 1] * u21[i__ - 1];
08674 s23 += u23[i__ - 1] * u23[i__ - 1];
08675
08676 }
08677
08678
08679
08680
08681 if (s21 == 0. || s23 == 0.) {
08682 ret_val = 0.;
08683 return ret_val;
08684 }
08685 s21 = sqrt(s21);
08686 s23 = sqrt(s23);
08687 for (i__ = 1; i__ <= 3; ++i__) {
08688 u21[i__ - 1] /= s21;
08689 u23[i__ - 1] /= s23;
08690
08691 }
08692
08693
08694
08695
08696
08697 ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08698 if (ca < -1.) {
08699 ca = -1.;
08700 }
08701 if (ca > 1.) {
08702 ca = 1.;
08703 }
08704 a = acos(ca);
08705
08706
08707
08708
08709 if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08710 , &v3[3])) {
08711 a = acos(-1.) * 2. - a;
08712 }
08713 ret_val = a;
08714 return ret_val;
08715 }
08716
08717 double areas_(double *v1, double *v2, double *v3)
08718 {
08719
08720 double ret_val;
08721
08722
08723
08724
08725
08726 static int i__;
08727 static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08728 ca2, ca3;
08729
08730
08731
08732
08733
08734
08735
08736
08737
08738
08739
08740
08741
08742
08743
08744
08745
08746
08747
08748
08749
08750
08751
08752
08753
08754
08755
08756
08757
08758
08759
08760
08761
08762
08763
08764
08765
08766
08767
08768
08769
08770
08771
08772
08773
08774
08775
08776
08777
08778
08779
08780
08781
08782 --v3;
08783 --v2;
08784 --v1;
08785
08786
08787 u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08788 u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08789 u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08790
08791 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08792 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08793 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08794
08795 u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08796 u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08797 u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08798
08799
08800
08801 s12 = 0.;
08802 s23 = 0.;
08803 s31 = 0.;
08804 for (i__ = 1; i__ <= 3; ++i__) {
08805 s12 += u12[i__ - 1] * u12[i__ - 1];
08806 s23 += u23[i__ - 1] * u23[i__ - 1];
08807 s31 += u31[i__ - 1] * u31[i__ - 1];
08808
08809 }
08810
08811
08812
08813
08814 if (s12 == 0. || s23 == 0. || s31 == 0.) {
08815 ret_val = 0.;
08816 return ret_val;
08817 }
08818 s12 = sqrt(s12);
08819 s23 = sqrt(s23);
08820 s31 = sqrt(s31);
08821 for (i__ = 1; i__ <= 3; ++i__) {
08822 u12[i__ - 1] /= s12;
08823 u23[i__ - 1] /= s23;
08824 u31[i__ - 1] /= s31;
08825
08826 }
08827
08828
08829
08830
08831
08832
08833
08834 ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08835 ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08836 ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08837 if (ca1 < -1.) {
08838 ca1 = -1.;
08839 }
08840 if (ca1 > 1.) {
08841 ca1 = 1.;
08842 }
08843 if (ca2 < -1.) {
08844 ca2 = -1.;
08845 }
08846 if (ca2 > 1.) {
08847 ca2 = 1.;
08848 }
08849 if (ca3 < -1.) {
08850 ca3 = -1.;
08851 }
08852 if (ca3 > 1.) {
08853 ca3 = 1.;
08854 }
08855 a1 = acos(ca1);
08856 a2 = acos(ca2);
08857 a3 = acos(ca3);
08858
08859
08860
08861 ret_val = a1 + a2 + a3 - acos(-1.);
08862 if (ret_val < 0.) {
08863 ret_val = 0.;
08864 }
08865 return ret_val;
08866 }
08867
08868 double Util::areav_(int *k, int *n, double *x, double *y,
08869 double *z__, int *list, int *lptr, int *lend, int
08870 *ier)
08871 {
08872
08873
08874 static double amax = 6.28;
08875
08876
08877 double ret_val;
08878
08879
08880 static double a, c0[3], c2[3], c3[3];
08881 static int n1, n2, n3;
08882 static double v1[3], v2[3], v3[3];
08883 static int lp, lpl, ierr;
08884 static double asum;
08885 extern double areas_(double *, double *, double *);
08886 static long int first;
08887 extern int circum_(double *, double *,
08888 double *, double *, int *);
08889
08890
08891
08892
08893
08894
08895
08896
08897
08898
08899
08900
08901
08902
08903
08904
08905
08906
08907
08908
08909
08910
08911
08912
08913
08914
08915
08916
08917
08918
08919
08920
08921
08922
08923
08924
08925
08926
08927
08928
08929
08930
08931
08932
08933
08934
08935
08936
08937
08938
08939
08940
08941
08942
08943
08944
08945
08946
08947 --lend;
08948 --z__;
08949 --y;
08950 --x;
08951 --list;
08952 --lptr;
08953
08954
08955
08956
08957
08958 if (*k < 1 || *k > *n || *n <= 3) {
08959 goto L11;
08960 }
08961
08962
08963
08964
08965
08966 n1 = *k;
08967 v1[0] = x[n1];
08968 v1[1] = y[n1];
08969 v1[2] = z__[n1];
08970 lpl = lend[n1];
08971 n3 = list[lpl];
08972 if (n3 < 0) {
08973 goto L12;
08974 }
08975 lp = lpl;
08976 first = TRUE_;
08977 asum = 0.;
08978
08979
08980
08981 L1:
08982 n2 = n3;
08983 lp = lptr[lp];
08984 n3 = list[lp];
08985 v2[0] = x[n2];
08986 v2[1] = y[n2];
08987 v2[2] = z__[n2];
08988 v3[0] = x[n3];
08989 v3[1] = y[n3];
08990 v3[2] = z__[n3];
08991 if (first) {
08992
08993
08994
08995
08996 circum_(v1, v2, v3, c3, &ierr);
08997 if (ierr != 0) {
08998 goto L13;
08999 }
09000 c0[0] = c3[0];
09001 c0[1] = c3[1];
09002 c0[2] = c3[2];
09003 first = FALSE_;
09004 } else {
09005
09006
09007
09008
09009 c2[0] = c3[0];
09010 c2[1] = c3[1];
09011 c2[2] = c3[2];
09012 circum_(v1, v2, v3, c3, &ierr);
09013 if (ierr != 0) {
09014 goto L13;
09015 }
09016 a = areas_(v1, c2, c3);
09017 if (a > amax) {
09018 goto L14;
09019 }
09020 asum += a;
09021 }
09022
09023
09024
09025 if (lp != lpl) {
09026 goto L1;
09027 }
09028
09029
09030
09031 a = areas_(v1, c3, c0);
09032 if (a > amax) {
09033 goto L14;
09034 }
09035 asum += a;
09036
09037
09038
09039 *ier = 0;
09040 ret_val = asum;
09041 return ret_val;
09042
09043
09044
09045 L11:
09046 *ier = 1;
09047 ret_val = 0.;
09048 return ret_val;
09049
09050
09051
09052 L12:
09053 *ier = 2;
09054 ret_val = 0.;
09055 return ret_val;
09056
09057
09058
09059 L13:
09060 *ier = 3;
09061 ret_val = 0.;
09062 return ret_val;
09063
09064
09065
09066 L14:
09067 *ier = 4;
09068 ret_val = 0.;
09069 return ret_val;
09070 }
09071
09072 double areav_new__(int *k, int *n, double *x, double *y,
09073 double *z__, int *list, int *lptr, int *lend, int
09074 *ier)
09075 {
09076
09077 double ret_val = 0;
09078
09079
09080
09081
09082
09083 static int m;
09084 static double c1[3], c2[3], c3[3];
09085 static int n1, n2, n3;
09086 static double v1[3], v2[3], v3[3];
09087 static int lp;
09088 static double c1s[3], c2s[3];
09089 static int lpl, ierr;
09090 static double asum;
09091 extern double angle_(double *, double *, double *);
09092 static float areav;
09093 extern int circum_(double *, double *,
09094 double *, double *, int *);
09095
09096
09097
09098
09099
09100
09101
09102
09103
09104
09105
09106
09107
09108
09109
09110
09111
09112
09113
09114
09115
09116
09117
09118
09119
09120
09121
09122
09123
09124
09125
09126
09127
09128
09129
09130
09131
09132
09133
09134
09135
09136
09137
09138
09139
09140
09141
09142
09143
09144
09145
09146
09147
09148
09149
09150
09151
09152
09153
09154
09155
09156 --lend;
09157 --z__;
09158 --y;
09159 --x;
09160 --list;
09161 --lptr;
09162
09163
09164 if (*k < 1 || *k > *n || *n <= 3) {
09165 goto L11;
09166 }
09167
09168
09169
09170
09171
09172 n1 = *k;
09173 v1[0] = x[n1];
09174 v1[1] = y[n1];
09175 v1[2] = z__[n1];
09176 lpl = lend[n1];
09177 n3 = list[lpl];
09178 if (n3 < 0) {
09179 goto L12;
09180 }
09181 lp = lpl;
09182 m = 0;
09183 asum = 0.;
09184
09185
09186
09187 L1:
09188 ++m;
09189 n2 = n3;
09190 lp = lptr[lp];
09191 n3 = list[lp];
09192 v2[0] = x[n2];
09193 v2[1] = y[n2];
09194 v2[2] = z__[n2];
09195 v3[0] = x[n3];
09196 v3[1] = y[n3];
09197 v3[2] = z__[n3];
09198 if (m == 1) {
09199
09200
09201
09202
09203 circum_(v1, v2, v3, c2, &ierr);
09204 if (ierr != 0) {
09205 goto L13;
09206 }
09207 c1s[0] = c2[0];
09208 c1s[1] = c2[1];
09209 c1s[2] = c2[2];
09210 } else if (m == 2) {
09211
09212
09213
09214
09215 circum_(v1, v2, v3, c3, &ierr);
09216 if (ierr != 0) {
09217 goto L13;
09218 }
09219 c2s[0] = c3[0];
09220 c2s[1] = c3[1];
09221 c2s[2] = c3[2];
09222 } else {
09223
09224
09225
09226
09227
09228 c1[0] = c2[0];
09229 c1[1] = c2[1];
09230 c1[2] = c2[2];
09231 c2[0] = c3[0];
09232 c2[1] = c3[1];
09233 c2[2] = c3[2];
09234 circum_(v1, v2, v3, c3, &ierr);
09235 if (ierr != 0) {
09236 goto L13;
09237 }
09238 asum += angle_(c1, c2, c3);
09239 }
09240
09241
09242
09243 if (lp != lpl) {
09244 goto L1;
09245 }
09246
09247
09248
09249
09250 asum += angle_(c2, c3, c1s);
09251
09252
09253
09254
09255 asum += angle_(c3, c1s, c2s);
09256
09257
09258
09259 *ier = 0;
09260 ret_val = asum - (double) (m - 2) * acos(-1.);
09261 return ret_val;
09262
09263
09264
09265 L11:
09266 *ier = 1;
09267 areav = 0.f;
09268 return ret_val;
09269
09270
09271
09272 L12:
09273 *ier = 2;
09274 areav = 0.f;
09275 return ret_val;
09276
09277
09278
09279 L13:
09280 *ier = 3;
09281 areav = 0.f;
09282 return ret_val;
09283 }
09284
09285 int bdyadd_(int *kk, int *i1, int *i2, int *
09286 list, int *lptr, int *lend, int *lnew)
09287 {
09288 static int k, n1, n2, lp, lsav, nsav, next;
09289 extern int insert_(int *, int *, int *,
09290 int *, int *);
09291
09292
09293
09294
09295
09296
09297
09298
09299
09300
09301
09302
09303
09304
09305
09306
09307
09308
09309
09310
09311
09312
09313
09314
09315
09316
09317
09318
09319
09320
09321
09322
09323
09324
09325
09326
09327
09328
09329
09330
09331
09332
09333
09334
09335
09336
09337
09338
09339
09340
09341
09342
09343
09344
09345
09346
09347
09348
09349
09350
09351
09352
09353
09354
09355 --lend;
09356 --lptr;
09357 --list;
09358
09359
09360 k = *kk;
09361 n1 = *i1;
09362 n2 = *i2;
09363
09364
09365
09366 lp = lend[n1];
09367 lsav = lptr[lp];
09368 lptr[lp] = *lnew;
09369 list[*lnew] = -k;
09370 lptr[*lnew] = lsav;
09371 lend[n1] = *lnew;
09372 ++(*lnew);
09373 next = -list[lp];
09374 list[lp] = next;
09375 nsav = next;
09376
09377
09378
09379
09380 L1:
09381 lp = lend[next];
09382 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09383 if (next == n2) {
09384 goto L2;
09385 }
09386 next = -list[lp];
09387 list[lp] = next;
09388 goto L1;
09389
09390
09391
09392
09393 L2:
09394 lsav = *lnew;
09395 list[*lnew] = n1;
09396 lptr[*lnew] = *lnew + 1;
09397 ++(*lnew);
09398 next = nsav;
09399
09400 L3:
09401 if (next == n2) {
09402 goto L4;
09403 }
09404 list[*lnew] = next;
09405 lptr[*lnew] = *lnew + 1;
09406 ++(*lnew);
09407 lp = lend[next];
09408 next = list[lp];
09409 goto L3;
09410
09411 L4:
09412 list[*lnew] = -n2;
09413 lptr[*lnew] = lsav;
09414 lend[k] = *lnew;
09415 ++(*lnew);
09416 return 0;
09417 }
09418
09419 int bnodes_(int *n, int *list, int *lptr,
09420 int *lend, int *nodes, int *nb, int *na, int *nt)
09421 {
09422
09423 int i__1;
09424
09425
09426 static int k, n0, lp, nn, nst;
09427
09428
09429
09430
09431
09432
09433
09434
09435
09436
09437
09438
09439
09440
09441
09442
09443
09444
09445
09446
09447
09448
09449
09450
09451
09452
09453
09454
09455
09456
09457
09458
09459
09460
09461
09462
09463
09464
09465
09466
09467
09468
09469
09470
09471
09472
09473
09474
09475
09476
09477
09478
09479
09480
09481
09482
09483
09484
09485
09486
09487 --lend;
09488 --list;
09489 --lptr;
09490 --nodes;
09491
09492
09493 nn = *n;
09494
09495
09496
09497 i__1 = nn;
09498 for (nst = 1; nst <= i__1; ++nst) {
09499 lp = lend[nst];
09500 if (list[lp] < 0) {
09501 goto L2;
09502 }
09503
09504 }
09505
09506
09507
09508 *nb = 0;
09509 *na = (nn - 2) * 3;
09510 *nt = nn - (2<<1);
09511 return 0;
09512
09513
09514
09515
09516 L2:
09517 nodes[1] = nst;
09518 k = 1;
09519 n0 = nst;
09520
09521
09522
09523 L3:
09524 lp = lend[n0];
09525 lp = lptr[lp];
09526 n0 = list[lp];
09527 if (n0 == nst) {
09528 goto L4;
09529 }
09530 ++k;
09531 nodes[k] = n0;
09532 goto L3;
09533
09534
09535
09536 L4:
09537 *nb = k;
09538 *nt = (*n << 1) - *nb - 2;
09539 *na = *nt + *n - 1;
09540 return 0;
09541 }
09542
09543 int circle_(int *k, double *xc, double *yc,
09544 int *ier)
09545 {
09546
09547 int i__1;
09548
09549
09550
09551
09552
09553 static double a, c__;
09554 static int i__;
09555 static double s;
09556 static int k2, k3;
09557 static double x0, y0;
09558 static int kk, np1;
09559
09560
09561
09562
09563
09564
09565
09566
09567
09568
09569
09570
09571
09572
09573
09574
09575
09576
09577
09578
09579
09580
09581
09582
09583
09584
09585
09586
09587
09588
09589
09590
09591
09592
09593
09594
09595
09596
09597
09598
09599
09600
09601
09602
09603
09604
09605
09606
09607
09608
09609
09610
09611
09612
09613
09614
09615
09616
09617
09618
09619
09620
09621
09622
09623 --yc;
09624 --xc;
09625
09626
09627 kk = *k;
09628 k2 = kk << 1;
09629 k3 = kk * 3;
09630 np1 = (kk << 2) + 1;
09631
09632
09633
09634
09635 if (kk < 1) {
09636 goto L2;
09637 }
09638 a = atan(1.) * 2. / (double) kk;
09639 c__ = cos(a);
09640 s = sin(a);
09641 x0 = 1.;
09642 y0 = 0.;
09643
09644
09645
09646
09647
09648 i__1 = kk;
09649 for (i__ = 1; i__ <= i__1; ++i__) {
09650 xc[i__] = x0;
09651 yc[i__] = y0;
09652 xc[i__ + kk] = -y0;
09653 yc[i__ + kk] = x0;
09654 xc[i__ + k2] = -x0;
09655 yc[i__ + k2] = -y0;
09656 xc[i__ + k3] = y0;
09657 yc[i__ + k3] = -x0;
09658
09659
09660
09661 x0 = c__ * x0 - s * y0;
09662 y0 = s * x0 + c__ * y0;
09663
09664 }
09665
09666
09667
09668
09669 xc[np1] = xc[1];
09670 yc[np1] = yc[1];
09671 *ier = 0;
09672 return 0;
09673
09674
09675
09676 L2:
09677 *ier = 1;
09678 return 0;
09679 }
09680
09681 int circum_(double *v1, double *v2, double *v3,
09682 double *c__, int *ier)
09683 {
09684
09685
09686
09687
09688 static int i__;
09689 static double e1[3], e2[3], cu[3], cnorm;
09690
09691
09692
09693
09694
09695
09696
09697
09698
09699
09700
09701
09702
09703
09704
09705
09706
09707
09708
09709
09710
09711
09712
09713
09714
09715
09716
09717
09718
09719
09720
09721
09722
09723
09724
09725
09726
09727
09728
09729
09730
09731
09732
09733
09734
09735
09736
09737
09738
09739
09740
09741
09742
09743
09744
09745
09746 --c__;
09747 --v3;
09748 --v2;
09749 --v1;
09750
09751
09752 for (i__ = 1; i__ <= 3; ++i__) {
09753 e1[i__ - 1] = v2[i__] - v1[i__];
09754 e2[i__ - 1] = v3[i__] - v1[i__];
09755
09756 }
09757
09758
09759
09760 cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09761 cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09762 cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09763 cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09764
09765
09766
09767
09768 if (cnorm != 0.) {
09769
09770
09771
09772 cnorm = sqrt(cnorm);
09773 for (i__ = 1; i__ <= 3; ++i__) {
09774 c__[i__] = cu[i__ - 1] / cnorm;
09775
09776 }
09777
09778
09779
09780
09781
09782
09783 if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09784 c__[1] = -c__[1];
09785 c__[2] = -c__[2];
09786 c__[3] = -c__[3];
09787 }
09788 *ier = 0;
09789 } else {
09790
09791
09792
09793 *ier = 1;
09794 }
09795 return 0;
09796 }
09797
09798 int covsph_(int *kk, int *n0, int *list, int
09799 *lptr, int *lend, int *lnew)
09800 {
09801 static int k, lp, nst, lsav, next;
09802 extern int insert_(int *, int *, int *,
09803 int *, int *);
09804
09805
09806
09807
09808
09809
09810
09811
09812
09813
09814
09815
09816
09817
09818
09819
09820
09821
09822
09823
09824
09825
09826
09827
09828
09829
09830
09831
09832
09833
09834
09835
09836
09837
09838
09839
09840
09841
09842
09843
09844
09845
09846
09847
09848
09849
09850
09851
09852
09853
09854
09855
09856
09857
09858
09859
09860
09861 --lend;
09862 --lptr;
09863 --list;
09864
09865
09866 k = *kk;
09867 nst = *n0;
09868
09869
09870
09871
09872
09873 next = nst;
09874 L1:
09875 lp = lend[next];
09876 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09877 next = -list[lp];
09878 list[lp] = next;
09879 if (next != nst) {
09880 goto L1;
09881 }
09882
09883
09884
09885
09886 lsav = *lnew;
09887 L2:
09888 lp = lend[next];
09889 list[*lnew] = next;
09890 lptr[*lnew] = *lnew + 1;
09891 ++(*lnew);
09892 next = list[lp];
09893 if (next != nst) {
09894 goto L2;
09895 }
09896
09897 lptr[*lnew - 1] = lsav;
09898 lend[k] = *lnew - 1;
09899 return 0;
09900 }
09901
09902 int crlist_(int *n, int *ncol, double *x,
09903 double *y, double *z__, int *list, int *lend, int
09904 *lptr, int *lnew, int *ltri, int *listc, int *nb,
09905 double *xc, double *yc, double *zc, double *rc,
09906 int *ier)
09907 {
09908
09909 int i__1, i__2;
09910
09911
09912
09913
09914
09915 static double c__[3], t;
09916 static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09917 static double v1[3], v2[3], v3[3];
09918 static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09919 lpn;
09920 static long int swp;
09921 static int ierr;
09922 extern int circum_(double *, double *,
09923 double *, double *, int *);
09924 extern int lstptr_(int *, int *, int *, int *);
09925 extern long int swptst_(int *, int *, int *, int *,
09926 double *, double *, double *);
09927
09928
09929
09930
09931
09932
09933
09934
09935
09936
09937
09938
09939
09940
09941
09942
09943
09944
09945
09946
09947
09948
09949
09950
09951
09952
09953
09954
09955
09956
09957
09958
09959
09960
09961
09962
09963
09964
09965
09966
09967
09968
09969
09970
09971
09972
09973
09974
09975
09976
09977
09978
09979
09980
09981
09982
09983
09984
09985
09986
09987
09988
09989
09990
09991
09992
09993
09994
09995
09996
09997
09998
09999
10000
10001
10002
10003
10004
10005
10006
10007
10008
10009
10010
10011
10012
10013
10014
10015
10016
10017
10018
10019
10020
10021
10022
10023
10024
10025
10026
10027
10028
10029
10030
10031
10032
10033
10034
10035
10036
10037
10038
10039
10040
10041
10042
10043
10044
10045
10046
10047
10048
10049
10050
10051
10052
10053
10054
10055
10056
10057
10058
10059
10060
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 --lend;
10104 --z__;
10105 --y;
10106 --x;
10107 ltri -= 7;
10108 --list;
10109 --lptr;
10110 --listc;
10111 --xc;
10112 --yc;
10113 --zc;
10114 --rc;
10115
10116
10117 nn = *n;
10118 *nb = 0;
10119 nt = 0;
10120 if (nn < 3) {
10121 goto L21;
10122 }
10123
10124
10125
10126 i__1 = nn;
10127 for (n1 = 1; n1 <= i__1; ++n1) {
10128 lp = lend[n1];
10129 if (list[lp] < 0) {
10130 goto L2;
10131 }
10132
10133 }
10134
10135
10136
10137 goto L9;
10138
10139
10140
10141
10142
10143
10144
10145
10146 L2:
10147 n2 = -list[lp];
10148 lp = lptr[lp];
10149 n3 = list[lp];
10150
10151
10152
10153
10154
10155
10156 L3:
10157 ++nt;
10158 if (nt <= *ncol) {
10159 ltri[nt * 6 + 1] = n1;
10160 ltri[nt * 6 + 2] = n2;
10161 ltri[nt * 6 + 3] = n3;
10162 ltri[nt * 6 + 4] = nt + 1;
10163 ltri[nt * 6 + 5] = nt - 1;
10164 ltri[nt * 6 + 6] = 0;
10165 }
10166 n1 = n2;
10167 lp = lend[n1];
10168 n2 = -list[lp];
10169 if (n2 != n3) {
10170 goto L3;
10171 }
10172
10173 *nb = nt + 2;
10174 if (*ncol < nt) {
10175 goto L22;
10176 }
10177 ltri[nt * 6 + 4] = 0;
10178 if (nt == 1) {
10179 goto L7;
10180 }
10181
10182
10183
10184
10185
10186
10187
10188 L4:
10189 swp = FALSE_;
10190 i__1 = nt - 1;
10191 for (kt1 = 1; kt1 <= i__1; ++kt1) {
10192 for (i3 = 1; i3 <= 3; ++i3) {
10193 kt2 = ltri[i3 + 3 + kt1 * 6];
10194 if (kt2 <= kt1) {
10195 goto L5;
10196 }
10197
10198
10199
10200
10201 if (i3 == 1) {
10202 i1 = 2;
10203 i2 = 3;
10204 } else if (i3 == 2) {
10205 i1 = 3;
10206 i2 = 1;
10207 } else {
10208 i1 = 1;
10209 i2 = 2;
10210 }
10211 n1 = ltri[i1 + kt1 * 6];
10212 n2 = ltri[i2 + kt1 * 6];
10213 n3 = ltri[i3 + kt1 * 6];
10214
10215
10216
10217
10218 if (ltri[kt2 * 6 + 4] == kt1) {
10219 i4 = 1;
10220 } else if (ltri[kt2 * 6 + 5] == kt1) {
10221 i4 = 2;
10222 } else {
10223 i4 = 3;
10224 }
10225 n4 = ltri[i4 + kt2 * 6];
10226
10227
10228
10229
10230
10231 if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10232 goto L5;
10233 }
10234
10235
10236
10237
10238 swp = TRUE_;
10239 kt11 = ltri[i1 + 3 + kt1 * 6];
10240 kt12 = ltri[i2 + 3 + kt1 * 6];
10241 if (i4 == 1) {
10242 i2 = 2;
10243 i1 = 3;
10244 } else if (i4 == 2) {
10245 i2 = 3;
10246 i1 = 1;
10247 } else {
10248 i2 = 1;
10249 i1 = 2;
10250 }
10251 kt21 = ltri[i1 + 3 + kt2 * 6];
10252 kt22 = ltri[i2 + 3 + kt2 * 6];
10253 ltri[kt1 * 6 + 1] = n4;
10254 ltri[kt1 * 6 + 2] = n3;
10255 ltri[kt1 * 6 + 3] = n1;
10256 ltri[kt1 * 6 + 4] = kt12;
10257 ltri[kt1 * 6 + 5] = kt22;
10258 ltri[kt1 * 6 + 6] = kt2;
10259 ltri[kt2 * 6 + 1] = n3;
10260 ltri[kt2 * 6 + 2] = n4;
10261 ltri[kt2 * 6 + 3] = n2;
10262 ltri[kt2 * 6 + 4] = kt21;
10263 ltri[kt2 * 6 + 5] = kt11;
10264 ltri[kt2 * 6 + 6] = kt1;
10265
10266
10267
10268 if (kt11 != 0) {
10269 i4 = 4;
10270 if (ltri[kt11 * 6 + 4] != kt1) {
10271 i4 = 5;
10272 if (ltri[kt11 * 6 + 5] != kt1) {
10273 i4 = 6;
10274 }
10275 }
10276 ltri[i4 + kt11 * 6] = kt2;
10277 }
10278 if (kt22 != 0) {
10279 i4 = 4;
10280 if (ltri[kt22 * 6 + 4] != kt2) {
10281 i4 = 5;
10282 if (ltri[kt22 * 6 + 5] != kt2) {
10283 i4 = 6;
10284 }
10285 }
10286 ltri[i4 + kt22 * 6] = kt1;
10287 }
10288 L5:
10289 ;
10290 }
10291
10292 }
10293 if (swp) {
10294 goto L4;
10295 }
10296
10297
10298
10299
10300 L7:
10301 i__1 = nt;
10302 for (kt = 1; kt <= i__1; ++kt) {
10303 n1 = ltri[kt * 6 + 1];
10304 n2 = ltri[kt * 6 + 2];
10305 n3 = ltri[kt * 6 + 3];
10306 v1[0] = x[n1];
10307 v1[1] = y[n1];
10308 v1[2] = z__[n1];
10309 v2[0] = x[n2];
10310 v2[1] = y[n2];
10311 v2[2] = z__[n2];
10312 v3[0] = x[n3];
10313 v3[1] = y[n3];
10314 v3[2] = z__[n3];
10315 circum_(v2, v1, v3, c__, &ierr);
10316 if (ierr != 0) {
10317 goto L23;
10318 }
10319
10320
10321
10322
10323 xc[kt] = -c__[0];
10324 yc[kt] = -c__[1];
10325 zc[kt] = -c__[2];
10326 t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10327 if (t < -1.) {
10328 t = -1.;
10329 }
10330 if (t > 1.) {
10331 t = 1.;
10332 }
10333 rc[kt] = acos(t);
10334
10335 }
10336
10337
10338
10339
10340
10341
10342 L9:
10343 kt = nt;
10344
10345
10346
10347 nm2 = nn - 2;
10348 i__1 = nm2;
10349 for (n1 = 1; n1 <= i__1; ++n1) {
10350 lpl = lend[n1];
10351 lp = lpl;
10352 n3 = list[lp];
10353
10354
10355
10356
10357 L10:
10358 lp = lptr[lp];
10359 n2 = n3;
10360 n3 = (i__2 = list[lp], abs(i__2));
10361 if (n2 <= n1 || n3 <= n1) {
10362 goto L11;
10363 }
10364 ++kt;
10365
10366
10367
10368 v1[0] = x[n1];
10369 v1[1] = y[n1];
10370 v1[2] = z__[n1];
10371 v2[0] = x[n2];
10372 v2[1] = y[n2];
10373 v2[2] = z__[n2];
10374 v3[0] = x[n3];
10375 v3[1] = y[n3];
10376 v3[2] = z__[n3];
10377 circum_(v1, v2, v3, c__, &ierr);
10378 if (ierr != 0) {
10379 goto L23;
10380 }
10381
10382
10383
10384 xc[kt] = c__[0];
10385 yc[kt] = c__[1];
10386 zc[kt] = c__[2];
10387 t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10388 if (t < -1.) {
10389 t = -1.;
10390 }
10391 if (t > 1.) {
10392 t = 1.;
10393 }
10394 rc[kt] = acos(t);
10395
10396
10397
10398
10399
10400 lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10401 listc[lpn] = kt;
10402 lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10403 listc[lpn] = kt;
10404 lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10405 listc[lpn] = kt;
10406 L11:
10407 if (lp != lpl) {
10408 goto L10;
10409 }
10410
10411 }
10412 if (nt == 0) {
10413 goto L20;
10414 }
10415
10416
10417
10418
10419
10420
10421 kt1 = 0;
10422 L13:
10423 ++kt1;
10424 if (ltri[kt1 * 6 + 4] == 0) {
10425 i1 = 2;
10426 i2 = 3;
10427 i3 = 1;
10428 goto L14;
10429 } else if (ltri[kt1 * 6 + 5] == 0) {
10430 i1 = 3;
10431 i2 = 1;
10432 i3 = 2;
10433 goto L14;
10434 } else if (ltri[kt1 * 6 + 6] == 0) {
10435 i1 = 1;
10436 i2 = 2;
10437 i3 = 3;
10438 goto L14;
10439 }
10440 goto L13;
10441 L14:
10442 n1 = ltri[i1 + kt1 * 6];
10443 n0 = n1;
10444
10445
10446
10447
10448
10449
10450
10451
10452
10453 L15:
10454 lp = lend[n1];
10455 lpn = lptr[lp];
10456 listc[lp] = kt1;
10457
10458
10459
10460 L16:
10461 kt2 = ltri[i2 + 3 + kt1 * 6];
10462 if (kt2 != 0) {
10463
10464
10465
10466 lptr[lp] = *lnew;
10467 lp = *lnew;
10468 listc[lp] = kt2;
10469 ++(*lnew);
10470
10471
10472
10473
10474 kt1 = kt2;
10475 if (ltri[kt1 * 6 + 1] == n1) {
10476 i1 = 1;
10477 i2 = 2;
10478 i3 = 3;
10479 } else if (ltri[kt1 * 6 + 2] == n1) {
10480 i1 = 2;
10481 i2 = 3;
10482 i3 = 1;
10483 } else {
10484 i1 = 3;
10485 i2 = 1;
10486 i3 = 2;
10487 }
10488 goto L16;
10489 }
10490
10491
10492
10493
10494
10495
10496
10497 lptr[lp] = lpn;
10498 n1 = ltri[i3 + kt1 * 6];
10499 if (n1 != n0) {
10500 i4 = i3;
10501 i3 = i2;
10502 i2 = i1;
10503 i1 = i4;
10504 goto L15;
10505 }
10506
10507
10508
10509 L20:
10510 *ier = 0;
10511 return 0;
10512
10513
10514
10515 L21:
10516 *ier = 1;
10517 return 0;
10518
10519
10520
10521 L22:
10522 *ier = 2;
10523 return 0;
10524
10525
10526
10527 L23:
10528 *ier = 3;
10529 return 0;
10530 }
10531
10532 int delarc_(int *n, int *io1, int *io2, int *
10533 list, int *lptr, int *lend, int *lnew, int *ier)
10534 {
10535
10536 int i__1;
10537
10538
10539 static int n1, n2, n3, lp, lph, lpl;
10540 extern int delnb_(int *, int *, int *,
10541 int *, int *, int *, int *, int *);
10542 extern int lstptr_(int *, int *, int *, int *);
10543
10544
10545
10546
10547
10548
10549
10550
10551
10552
10553
10554
10555
10556
10557
10558
10559
10560
10561
10562
10563
10564
10565
10566
10567
10568
10569
10570
10571
10572
10573
10574
10575
10576
10577
10578
10579
10580
10581
10582
10583
10584
10585
10586
10587
10588
10589
10590
10591
10592
10593
10594
10595
10596
10597
10598
10599
10600
10601
10602
10603
10604
10605
10606
10607
10608
10609
10610
10611
10612
10613
10614
10615
10616 --lend;
10617 --list;
10618 --lptr;
10619
10620
10621 n1 = *io1;
10622 n2 = *io2;
10623
10624
10625
10626
10627
10628 if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10629 *ier = 1;
10630 return 0;
10631 }
10632
10633 lpl = lend[n2];
10634 if (-list[lpl] != n1) {
10635 n1 = n2;
10636 n2 = *io1;
10637 lpl = lend[n2];
10638 if (-list[lpl] != n1) {
10639 *ier = 2;
10640 return 0;
10641 }
10642 }
10643
10644
10645
10646
10647
10648 lpl = lend[n1];
10649 lp = lptr[lpl];
10650 lp = lptr[lp];
10651 n3 = (i__1 = list[lp], abs(i__1));
10652 lpl = lend[n3];
10653 if (list[lpl] <= 0) {
10654 *ier = 3;
10655 return 0;
10656 }
10657
10658
10659
10660
10661
10662
10663 delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10664 if (lph < 0) {
10665 *ier = 4;
10666 return 0;
10667 }
10668
10669
10670
10671
10672 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10673
10674
10675
10676
10677 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10678 lend[n3] = lp;
10679 list[lp] = -n1;
10680
10681
10682
10683 *ier = 0;
10684 return 0;
10685 }
10686
10687 int delnb_(int *n0, int *nb, int *n, int *
10688 list, int *lptr, int *lend, int *lnew, int *lph)
10689 {
10690
10691 int i__1;
10692
10693
10694 static int i__, lp, nn, lpb, lpl, lpp, lnw;
10695
10696
10697
10698
10699
10700
10701
10702
10703
10704
10705
10706
10707
10708
10709
10710
10711
10712
10713
10714
10715
10716
10717
10718
10719
10720
10721
10722
10723
10724
10725
10726
10727
10728
10729
10730
10731
10732
10733
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 --lend;
10767 --list;
10768 --lptr;
10769
10770
10771 nn = *n;
10772
10773
10774
10775 if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10776 *lph = -1;
10777 return 0;
10778 }
10779
10780
10781
10782
10783
10784
10785
10786 lpl = lend[*n0];
10787 lpp = lpl;
10788 lpb = lptr[lpp];
10789 L1:
10790 if (list[lpb] == *nb) {
10791 goto L2;
10792 }
10793 lpp = lpb;
10794 lpb = lptr[lpp];
10795 if (lpb != lpl) {
10796 goto L1;
10797 }
10798
10799
10800
10801 if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10802 *lph = -2;
10803 return 0;
10804 }
10805
10806
10807
10808
10809
10810 lend[*n0] = lpp;
10811 lp = lend[*nb];
10812 if (list[lp] < 0) {
10813 list[lpp] = -list[lpp];
10814 }
10815 goto L3;
10816
10817
10818
10819
10820
10821 L2:
10822 lp = lend[*nb];
10823 if (list[lp] < 0 && list[lpl] > 0) {
10824 lend[*n0] = lpp;
10825 list[lpp] = -list[lpp];
10826 }
10827
10828
10829
10830
10831 L3:
10832 lptr[lpp] = lptr[lpb];
10833 lnw = *lnew - 1;
10834 list[lpb] = list[lnw];
10835 lptr[lpb] = lptr[lnw];
10836 for (i__ = nn; i__ >= 1; --i__) {
10837 if (lend[i__] == lnw) {
10838 lend[i__] = lpb;
10839 goto L5;
10840 }
10841
10842 }
10843
10844 L5:
10845 i__1 = lnw - 1;
10846 for (i__ = 1; i__ <= i__1; ++i__) {
10847 if (lptr[i__] == lnw) {
10848 lptr[i__] = lpb;
10849 }
10850
10851 }
10852
10853
10854
10855 *lnew = lnw;
10856 *lph = lpb;
10857 return 0;
10858 }
10859
10860 int delnod_(int *k, int *n, double *x,
10861 double *y, double *z__, int *list, int *lptr, int
10862 *lend, int *lnew, int *lwk, int *iwk, int *ier)
10863 {
10864
10865 int i__1;
10866
10867
10868 static int i__, j, n1, n2;
10869 static double x1, x2, y1, y2, z1, z2;
10870 static int nl, lp, nn, nr;
10871 static double xl, yl, zl, xr, yr, zr;
10872 static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10873 extern long int left_(double *, double *, double *, double
10874 *, double *, double *, double *, double *,
10875 double *);
10876 static long int bdry;
10877 static int ierr, lwkl;
10878 extern int swap_(int *, int *, int *,
10879 int *, int *, int *, int *, int *), delnb_(
10880 int *, int *, int *, int *, int *, int *,
10881 int *, int *);
10882 extern int nbcnt_(int *, int *);
10883 extern int optim_(double *, double *, double
10884 *, int *, int *, int *, int *, int *, int
10885 *, int *);
10886 static int nfrst;
10887 extern int lstptr_(int *, int *, int *, int *);
10888
10889
10890
10891
10892
10893
10894
10895
10896
10897
10898
10899
10900
10901
10902
10903
10904
10905
10906
10907
10908
10909
10910
10911
10912
10913
10914
10915
10916
10917
10918
10919
10920
10921
10922
10923
10924
10925
10926
10927
10928
10929
10930
10931
10932
10933
10934
10935
10936
10937
10938
10939
10940
10941
10942
10943
10944
10945
10946
10947
10948
10949
10950
10951
10952
10953
10954
10955
10956
10957
10958
10959
10960
10961
10962
10963
10964
10965
10966
10967
10968
10969
10970
10971
10972
10973
10974
10975
10976
10977
10978
10979
10980
10981
10982
10983
10984
10985
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 iwk -= 3;
11032 --lend;
11033 --lptr;
11034 --list;
11035 --z__;
11036 --y;
11037 --x;
11038
11039
11040 n1 = *k;
11041 nn = *n;
11042 if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
11043 goto L21;
11044 }
11045 lpl = lend[n1];
11046 lpf = lptr[lpl];
11047 nnb = nbcnt_(&lpl, &lptr[1]);
11048 bdry = list[lpl] < 0;
11049 if (bdry) {
11050 ++nnb;
11051 }
11052 if (nnb < 3) {
11053 goto L23;
11054 }
11055 lwkl = *lwk;
11056 *lwk = nnb - 3;
11057 if (lwkl < *lwk) {
11058 goto L22;
11059 }
11060 iwl = 0;
11061 if (nnb == 3) {
11062 goto L3;
11063 }
11064
11065
11066
11067
11068
11069
11070
11071 x1 = x[n1];
11072 y1 = y[n1];
11073 z1 = z__[n1];
11074 nfrst = list[lpf];
11075 nr = nfrst;
11076 xr = x[nr];
11077 yr = y[nr];
11078 zr = z__[nr];
11079 lp = lptr[lpf];
11080 n2 = list[lp];
11081 x2 = x[n2];
11082 y2 = y[n2];
11083 z2 = z__[n2];
11084 lp = lptr[lp];
11085
11086
11087
11088 L1:
11089 nl = (i__1 = list[lp], abs(i__1));
11090 if (nl == nfrst && bdry) {
11091 goto L3;
11092 }
11093 xl = x[nl];
11094 yl = y[nl];
11095 zl = z__[nl];
11096
11097
11098
11099
11100
11101
11102 lpl2 = lend[n2];
11103 if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
11104 list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
11105 z2)))) {
11106
11107
11108
11109 nr = n2;
11110 xr = x2;
11111 yr = y2;
11112 zr = z2;
11113 goto L2;
11114 }
11115
11116
11117
11118
11119
11120
11121
11122
11123 swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11124 if (lp21 == 0) {
11125 nr = n2;
11126 xr = x2;
11127 yr = y2;
11128 zr = z2;
11129 goto L2;
11130 }
11131 ++iwl;
11132 if (nl <= n1) {
11133 iwk[(iwl << 1) + 1] = nl;
11134 } else {
11135 iwk[(iwl << 1) + 1] = nl - 1;
11136 }
11137 if (nr <= n1) {
11138 iwk[(iwl << 1) + 2] = nr;
11139 } else {
11140 iwk[(iwl << 1) + 2] = nr - 1;
11141 }
11142
11143
11144
11145 lpl = lend[n1];
11146 --nnb;
11147 if (nnb == 3) {
11148 goto L3;
11149 }
11150 lpf = lptr[lpl];
11151 nfrst = list[lpf];
11152 lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11153 if (nr == nfrst) {
11154 goto L2;
11155 }
11156
11157
11158
11159
11160
11161
11162
11163 n2 = nr;
11164 x2 = xr;
11165 y2 = yr;
11166 z2 = zr;
11167 lp21 = lptr[lp21];
11168 lp21 = lptr[lp21];
11169 nr = (i__1 = list[lp21], abs(i__1));
11170 xr = x[nr];
11171 yr = y[nr];
11172 zr = z__[nr];
11173 goto L1;
11174
11175
11176
11177 L2:
11178 if (n2 == nfrst) {
11179 goto L3;
11180 }
11181 n2 = nl;
11182 x2 = xl;
11183 y2 = yl;
11184 z2 = zl;
11185 lp = lptr[lp];
11186 goto L1;
11187
11188
11189
11190
11191
11192
11193
11194
11195
11196
11197 L3:
11198 if (! bdry) {
11199 if (nnb > 3) {
11200 bdry = TRUE_;
11201 } else {
11202 lpf = lptr[lpl];
11203 nr = list[lpf];
11204 lp = lptr[lpf];
11205 n2 = list[lp];
11206 nl = list[lpl];
11207 bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11208 x[n2], &y[n2], &z__[n2]);
11209 }
11210 if (bdry) {
11211
11212
11213
11214
11215
11216
11217 i__1 = nn;
11218 for (i__ = 1; i__ <= i__1; ++i__) {
11219 if (list[lend[i__]] < 0) {
11220 bdry = FALSE_;
11221 goto L5;
11222 }
11223
11224 }
11225 list[lpl] = -list[lpl];
11226 ++nnb;
11227 }
11228 }
11229 L5:
11230 if (! bdry && nnb > 3) {
11231 goto L24;
11232 }
11233
11234
11235
11236
11237 lp = lpl;
11238 lnw = *lnew;
11239
11240
11241
11242 L6:
11243 lp = lptr[lp];
11244 n2 = (i__1 = list[lp], abs(i__1));
11245 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11246 if (lph < 0) {
11247 goto L23;
11248 }
11249
11250
11251
11252 if (lpl == lnw) {
11253 lpl = lph;
11254 }
11255 if (lp == lnw) {
11256 lp = lph;
11257 }
11258 if (lp != lpl) {
11259 goto L6;
11260 }
11261
11262
11263
11264
11265
11266 --nn;
11267 if (n1 > nn) {
11268 goto L9;
11269 }
11270 i__1 = nn;
11271 for (i__ = n1; i__ <= i__1; ++i__) {
11272 x[i__] = x[i__ + 1];
11273 y[i__] = y[i__ + 1];
11274 z__[i__] = z__[i__ + 1];
11275 lend[i__] = lend[i__ + 1];
11276
11277 }
11278
11279 i__1 = lnw - 1;
11280 for (i__ = 1; i__ <= i__1; ++i__) {
11281 if (list[i__] > n1) {
11282 --list[i__];
11283 }
11284 if (list[i__] < -n1) {
11285 ++list[i__];
11286 }
11287
11288 }
11289
11290
11291
11292
11293
11294
11295
11296
11297
11298
11299
11300 L9:
11301 if (bdry) {
11302 --nnb;
11303 }
11304 lpn = lpl;
11305 i__1 = nnb;
11306 for (j = 1; j <= i__1; ++j) {
11307 --lnw;
11308 lp = lpn;
11309 lpn = lptr[lp];
11310 list[lp] = list[lnw];
11311 lptr[lp] = lptr[lnw];
11312 if (lptr[lpn] == lnw) {
11313 lptr[lpn] = lp;
11314 }
11315 if (lpn == lnw) {
11316 lpn = lp;
11317 }
11318 for (i__ = nn; i__ >= 1; --i__) {
11319 if (lend[i__] == lnw) {
11320 lend[i__] = lp;
11321 goto L11;
11322 }
11323
11324 }
11325
11326 L11:
11327 for (i__ = lnw - 1; i__ >= 1; --i__) {
11328 if (lptr[i__] == lnw) {
11329 lptr[i__] = lp;
11330 }
11331
11332 }
11333
11334 }
11335
11336
11337
11338
11339
11340 *n = nn;
11341 *lnew = lnw;
11342 if (iwl > 0) {
11343 nit = iwl << 2;
11344 optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11345 nit, &iwk[3], &ierr);
11346 if (ierr != 0 && ierr != 1) {
11347 goto L25;
11348 }
11349 if (ierr == 1) {
11350 goto L26;
11351 }
11352 }
11353
11354
11355
11356 *ier = 0;
11357 return 0;
11358
11359
11360
11361 L21:
11362 *ier = 1;
11363 return 0;
11364
11365
11366
11367 L22:
11368 *ier = 2;
11369 return 0;
11370
11371
11372
11373
11374 L23:
11375 *ier = 3;
11376 return 0;
11377
11378
11379
11380 L24:
11381 *ier = 4;
11382 return 0;
11383
11384
11385
11386 L25:
11387 *ier = 5;
11388
11389
11390
11391 return 0;
11392
11393
11394
11395 L26:
11396 *ier = 6;
11397 return 0;
11398 }
11399
11400 int drwarc_(int *, double *p, double *q,
11401 double *tol, int *nseg)
11402 {
11403
11404 int i__1;
11405 double d__1;
11406
11407
11408
11409
11410
11411 static int i__, k;
11412 static double s, p1[3], p2[3], u1, u2, v1, v2;
11413 static int na;
11414 static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11415
11416
11417
11418
11419
11420
11421
11422
11423
11424
11425
11426
11427
11428
11429
11430
11431
11432
11433
11434
11435
11436
11437
11438
11439
11440
11441
11442
11443
11444
11445
11446
11447
11448
11449
11450
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 --q;
11493 --p;
11494
11495
11496 enrm = 0.;
11497 for (i__ = 1; i__ <= 3; ++i__) {
11498 pm[i__ - 1] = p[i__] + q[i__];
11499 enrm += pm[i__ - 1] * pm[i__ - 1];
11500
11501 }
11502 if (enrm == 0.) {
11503 goto L5;
11504 }
11505 enrm = sqrt(enrm);
11506 pm[0] /= enrm;
11507 pm[1] /= enrm;
11508 pm[2] /= enrm;
11509
11510
11511
11512
11513 u1 = p[1];
11514 v1 = p[2];
11515 u2 = q[1];
11516 v2 = q[2];
11517 um = pm[0];
11518 vm = pm[1];
11519
11520
11521
11522
11523
11524
11525 du = u2 - u1;
11526 dv = v2 - v1;
11527 enrm = du * du + dv * dv;
11528 if (enrm == 0.) {
11529 goto L5;
11530 }
11531 err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11532
11533
11534
11535
11536
11537 na = (int) (err / *tol + 1.);
11538
11539
11540
11541
11542
11543 s = 1. / (double) na;
11544 for (i__ = 1; i__ <= 3; ++i__) {
11545 dp[i__ - 1] = s * (q[i__] - p[i__]);
11546 pm[i__ - 1] = p[i__];
11547 p1[i__ - 1] = p[i__];
11548
11549 }
11550
11551
11552
11553
11554 i__1 = na - 1;
11555 for (k = 1; k <= i__1; ++k) {
11556 enrm = 0.;
11557 for (i__ = 1; i__ <= 3; ++i__) {
11558 pm[i__ - 1] += dp[i__ - 1];
11559 enrm += pm[i__ - 1] * pm[i__ - 1];
11560
11561 }
11562 if (enrm == 0.) {
11563 goto L5;
11564 }
11565 enrm = sqrt(enrm);
11566 p2[0] = pm[0] / enrm;
11567 p2[1] = pm[1] / enrm;
11568 p2[2] = pm[2] / enrm;
11569
11570
11571 p1[0] = p2[0];
11572 p1[1] = p2[1];
11573 p1[2] = p2[2];
11574
11575 }
11576
11577
11578
11579
11580 *nseg = na;
11581 return 0;
11582
11583
11584
11585 L5:
11586 *nseg = 0;
11587 return 0;
11588 }
11589
11590 int edge_(int *in1, int *in2, double *x,
11591 double *y, double *z__, int *lwk, int *iwk, int *
11592 list, int *lptr, int *lend, int *ier)
11593 {
11594
11595 int i__1;
11596
11597
11598 static int i__, n0, n1, n2;
11599 static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11600 static int nl, lp, nr;
11601 static double dp12;
11602 static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11603 static double dp1l, dp2l, dp1r, dp2r;
11604 extern long int left_(double *, double *, double *, double
11605 *, double *, double *, double *, double *,
11606 double *);
11607 static int ierr;
11608 extern int swap_(int *, int *, int *,
11609 int *, int *, int *, int *, int *);
11610 static int next, iwcp1, n1lst, iwend;
11611 extern int optim_(double *, double *, double
11612 *, int *, int *, int *, int *, int *, int
11613 *, int *);
11614 static int n1frst;
11615
11616
11617
11618
11619
11620
11621
11622
11623
11624
11625
11626
11627
11628
11629
11630
11631
11632
11633
11634
11635
11636
11637
11638
11639
11640
11641
11642
11643
11644
11645
11646
11647
11648
11649
11650
11651
11652
11653
11654
11655
11656
11657
11658
11659
11660
11661
11662
11663
11664
11665
11666
11667
11668
11669
11670
11671
11672
11673
11674
11675
11676
11677
11678
11679
11680
11681
11682
11683
11684
11685
11686
11687
11688
11689
11690
11691
11692
11693
11694
11695
11696
11697
11698
11699
11700
11701
11702
11703
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 --lend;
11753 --lptr;
11754 --list;
11755 iwk -= 3;
11756 --z__;
11757 --y;
11758 --x;
11759
11760
11761 n1 = *in1;
11762 n2 = *in2;
11763 iwend = *lwk;
11764 if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11765 goto L31;
11766 }
11767
11768
11769
11770
11771 lpl = lend[n1];
11772 n0 = (i__1 = list[lpl], abs(i__1));
11773 lp = lpl;
11774 L1:
11775 if (n0 == n2) {
11776 goto L30;
11777 }
11778 lp = lptr[lp];
11779 n0 = list[lp];
11780 if (lp != lpl) {
11781 goto L1;
11782 }
11783
11784
11785
11786 iwl = 0;
11787 nit = 0;
11788
11789
11790
11791 L2:
11792 x1 = x[n1];
11793 y1 = y[n1];
11794 z1 = z__[n1];
11795 x2 = x[n2];
11796 y2 = y[n2];
11797 z2 = z__[n2];
11798
11799
11800
11801
11802
11803
11804
11805
11806
11807
11808 lpl = lend[n1];
11809 n1lst = list[lpl];
11810 lp = lptr[lpl];
11811 n1frst = list[lp];
11812 nl = n1frst;
11813 if (n1lst < 0) {
11814 goto L4;
11815 }
11816
11817
11818
11819
11820 L3:
11821 if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11822 goto L4;
11823 }
11824 lp = lptr[lp];
11825 nl = list[lp];
11826 if (nl != n1frst) {
11827 goto L3;
11828 }
11829
11830
11831
11832 goto L5;
11833
11834
11835
11836
11837 L4:
11838 nr = nl;
11839 lp = lptr[lp];
11840 nl = (i__1 = list[lp], abs(i__1));
11841 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11842
11843
11844
11845
11846
11847 dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11848 dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11849 dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11850 dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11851 dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11852 if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11853 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11854 goto L6;
11855 }
11856
11857
11858
11859
11860
11861 if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11862 goto L5;
11863 }
11864 }
11865
11866
11867
11868 if (nl != n1frst) {
11869 goto L4;
11870 }
11871
11872
11873
11874
11875
11876
11877
11878 L5:
11879 if (nit > 0) {
11880 goto L33;
11881 }
11882 nit = 1;
11883 n1 = n2;
11884 n2 = *in1;
11885 goto L2;
11886
11887
11888
11889
11890 L6:
11891 ++iwl;
11892 if (iwl > iwend) {
11893 goto L32;
11894 }
11895 iwk[(iwl << 1) + 1] = nl;
11896 iwk[(iwl << 1) + 2] = nr;
11897
11898
11899
11900 lpl = lend[nl];
11901 lp = lptr[lpl];
11902
11903
11904
11905
11906 L7:
11907 if (list[lp] == nr) {
11908 goto L8;
11909 }
11910 lp = lptr[lp];
11911 if (lp != lpl) {
11912 goto L7;
11913 }
11914
11915
11916
11917
11918 if (list[lp] != nr) {
11919 goto L33;
11920 }
11921
11922
11923
11924
11925 L8:
11926 lp = lptr[lp];
11927 next = (i__1 = list[lp], abs(i__1));
11928 if (next == n2) {
11929 goto L9;
11930 }
11931
11932
11933
11934 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11935 nl = next;
11936 } else {
11937 nr = next;
11938 }
11939 goto L6;
11940
11941
11942
11943
11944 L9:
11945 *lwk = iwl;
11946 iwend = iwl;
11947
11948
11949
11950
11951
11952
11953
11954
11955 iwf = 1;
11956
11957
11958
11959
11960
11961 L10:
11962 lft = 0;
11963 n0 = n1;
11964 x0 = x1;
11965 y0 = y1;
11966 z0 = z1;
11967 nl = iwk[(iwf << 1) + 1];
11968 nr = iwk[(iwf << 1) + 2];
11969 iwc = iwf;
11970
11971
11972
11973
11974 L11:
11975 if (iwc == iwl) {
11976 goto L21;
11977 }
11978 iwcp1 = iwc + 1;
11979 next = iwk[(iwcp1 << 1) + 1];
11980 if (next != nl) {
11981 goto L16;
11982 }
11983 next = iwk[(iwcp1 << 1) + 2];
11984
11985
11986
11987
11988 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11989 z__[next])) {
11990 goto L14;
11991 }
11992 if (lft >= 0) {
11993 goto L12;
11994 }
11995 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11996 z__[next])) {
11997 goto L14;
11998 }
11999
12000
12001
12002 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12003 iwk[(iwc << 1) + 1] = n0;
12004 iwk[(iwc << 1) + 2] = next;
12005 goto L15;
12006
12007
12008
12009
12010
12011 L12:
12012 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12013 i__1 = iwl;
12014 for (i__ = iwcp1; i__ <= i__1; ++i__) {
12015 iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
12016 iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
12017
12018 }
12019 iwk[(iwl << 1) + 1] = n0;
12020 iwk[(iwl << 1) + 2] = next;
12021 --iwl;
12022 nr = next;
12023 goto L11;
12024
12025
12026
12027 L14:
12028 n0 = nr;
12029 x0 = x[n0];
12030 y0 = y[n0];
12031 z0 = z__[n0];
12032 lft = 1;
12033
12034
12035
12036 L15:
12037 nr = next;
12038 ++iwc;
12039 goto L11;
12040
12041
12042
12043
12044 L16:
12045 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
12046 z__[next])) {
12047 goto L19;
12048 }
12049 if (lft <= 0) {
12050 goto L17;
12051 }
12052 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
12053 z__[next])) {
12054 goto L19;
12055 }
12056
12057
12058
12059 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12060 iwk[(iwc << 1) + 1] = next;
12061 iwk[(iwc << 1) + 2] = n0;
12062 goto L20;
12063
12064
12065
12066
12067
12068 L17:
12069 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12070 i__1 = iwf;
12071 for (i__ = iwc - 1; i__ >= i__1; --i__) {
12072 iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
12073 iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
12074
12075 }
12076 iwk[(iwf << 1) + 1] = n0;
12077 iwk[(iwf << 1) + 2] = next;
12078 ++iwf;
12079 goto L20;
12080
12081
12082
12083 L19:
12084 n0 = nl;
12085 x0 = x[n0];
12086 y0 = y[n0];
12087 z0 = z__[n0];
12088 lft = -1;
12089
12090
12091
12092 L20:
12093 nl = next;
12094 ++iwc;
12095 goto L11;
12096
12097
12098
12099 L21:
12100 if (n0 == n1) {
12101 goto L24;
12102 }
12103 if (lft < 0) {
12104 goto L22;
12105 }
12106
12107
12108
12109 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
12110 goto L10;
12111 }
12112
12113
12114
12115
12116 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12117 iwk[(iwl << 1) + 1] = n0;
12118 iwk[(iwl << 1) + 2] = n2;
12119 --iwl;
12120 goto L10;
12121
12122
12123
12124 L22:
12125 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12126 goto L10;
12127 }
12128
12129
12130
12131
12132 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12133 i__ = iwl;
12134 L23:
12135 iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12136 iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12137 --i__;
12138 if (i__ > iwf) {
12139 goto L23;
12140 }
12141 iwk[(iwf << 1) + 1] = n0;
12142 iwk[(iwf << 1) + 2] = n2;
12143 ++iwf;
12144 goto L10;
12145
12146
12147
12148
12149 L24:
12150 swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12151 iwk[(iwc << 1) + 1] = 0;
12152 iwk[(iwc << 1) + 2] = 0;
12153
12154
12155
12156 *ier = 0;
12157 if (iwc > 1) {
12158
12159
12160
12161 nit = iwc - (1<<2);
12162 i__1 = iwc - 1;
12163 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12164 nit, &iwk[3], &ierr);
12165 if (ierr != 0 && ierr != 1) {
12166 goto L34;
12167 }
12168 if (ierr == 1) {
12169 *ier = 5;
12170 }
12171 }
12172 if (iwc < iwend) {
12173
12174
12175
12176 nit = iwend - (iwc<<2);
12177 i__1 = iwend - iwc;
12178 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12179 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12180 if (ierr != 0 && ierr != 1) {
12181 goto L34;
12182 }
12183 if (ierr == 1) {
12184 goto L35;
12185 }
12186 }
12187 if (*ier == 5) {
12188 goto L35;
12189 }
12190
12191
12192
12193 return 0;
12194
12195
12196
12197 L30:
12198 *ier = 0;
12199 return 0;
12200
12201
12202
12203 L31:
12204 *ier = 1;
12205 return 0;
12206
12207
12208
12209 L32:
12210 *ier = 2;
12211 return 0;
12212
12213
12214
12215
12216 L33:
12217 *ier = 3;
12218
12219
12220
12221
12222 return 0;
12223
12224
12225
12226 L34:
12227 *ier = 4;
12228
12229
12230
12231 return 0;
12232
12233
12234
12235 L35:
12236 *ier = 5;
12237 return 0;
12238 }
12239
12240 int getnp_(double *x, double *y, double *z__,
12241 int *list, int *lptr, int *lend, int *l, int *
12242 npts, double *df, int *ier)
12243 {
12244
12245 int i__1, i__2;
12246
12247
12248 static int i__, n1;
12249 static double x1, y1, z1;
12250 static int nb, ni, lp, np, lm1;
12251 static double dnb, dnp;
12252 static int lpl;
12253
12254
12255
12256
12257
12258
12259
12260
12261
12262
12263
12264
12265
12266
12267
12268
12269
12270
12271
12272
12273
12274
12275
12276
12277
12278
12279
12280
12281
12282
12283
12284
12285
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 --x;
12333 --y;
12334 --z__;
12335 --list;
12336 --lptr;
12337 --lend;
12338 --npts;
12339
12340
12341 lm1 = *l - 1;
12342 if (lm1 < 1) {
12343 goto L6;
12344 }
12345 *ier = 0;
12346
12347
12348
12349 n1 = npts[1];
12350 x1 = x[n1];
12351 y1 = y[n1];
12352 z1 = z__[n1];
12353 i__1 = lm1;
12354 for (i__ = 1; i__ <= i__1; ++i__) {
12355 ni = npts[i__];
12356 lend[ni] = -lend[ni];
12357
12358 }
12359
12360
12361
12362
12363
12364 dnp = 2.;
12365
12366
12367
12368 i__1 = lm1;
12369 for (i__ = 1; i__ <= i__1; ++i__) {
12370 ni = npts[i__];
12371 lpl = -lend[ni];
12372 lp = lpl;
12373
12374
12375
12376 L2:
12377 nb = (i__2 = list[lp], abs(i__2));
12378 if (lend[nb] < 0) {
12379 goto L3;
12380 }
12381
12382
12383
12384
12385 dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12386 if (dnb >= dnp) {
12387 goto L3;
12388 }
12389 np = nb;
12390 dnp = dnb;
12391 L3:
12392 lp = lptr[lp];
12393 if (lp != lpl) {
12394 goto L2;
12395 }
12396
12397 }
12398 npts[*l] = np;
12399 *df = dnp;
12400
12401
12402
12403 i__1 = lm1;
12404 for (i__ = 1; i__ <= i__1; ++i__) {
12405 ni = npts[i__];
12406 lend[ni] = -lend[ni];
12407
12408 }
12409 return 0;
12410
12411
12412
12413 L6:
12414 *ier = 1;
12415 return 0;
12416 }
12417
12418 int insert_(int *k, int *lp, int *list, int *
12419 lptr, int *lnew)
12420 {
12421 static int lsav;
12422
12423
12424
12425
12426
12427
12428
12429
12430
12431
12432
12433
12434
12435
12436
12437
12438
12439
12440
12441
12442
12443
12444
12445
12446
12447
12448
12449
12450
12451
12452
12453
12454
12455
12456
12457
12458
12459
12460
12461
12462
12463
12464
12465 --lptr;
12466 --list;
12467
12468
12469 lsav = lptr[*lp];
12470 lptr[*lp] = *lnew;
12471 list[*lnew] = *k;
12472 lptr[*lnew] = lsav;
12473 ++(*lnew);
12474 return 0;
12475 }
12476
12477 long int inside_(double *p, int *lv, double *xv, double *yv,
12478 double *zv, int *nv, int *listv, int *ier)
12479 {
12480
12481
12482 static double eps = .001;
12483
12484
12485 int i__1;
12486 long int ret_val = 0;
12487
12488
12489
12490
12491
12492 static double b[3], d__;
12493 static int k, n;
12494 static double q[3];
12495 static int i1, i2, k0;
12496 static double v1[3], v2[3], cn[3], bp, bq;
12497 static int ni;
12498 static double pn[3], qn[3], vn[3];
12499 static int imx;
12500 static long int lft1, lft2, even;
12501 static int ierr;
12502 static long int pinr, qinr;
12503 static double qnrm, vnrm;
12504 extern int intrsc_(double *, double *,
12505 double *, double *, int *);
12506
12507
12508
12509
12510
12511
12512
12513
12514
12515
12516
12517
12518
12519
12520
12521
12522
12523
12524
12525
12526
12527
12528
12529
12530
12531
12532
12533
12534
12535
12536
12537
12538
12539
12540
12541
12542
12543
12544
12545
12546
12547
12548
12549
12550
12551
12552
12553
12554
12555
12556
12557
12558
12559
12560
12561
12562
12563
12564
12565
12566
12567
12568
12569
12570
12571
12572
12573
12574
12575
12576
12577
12578
12579
12580
12581
12582
12583
12584
12585
12586
12587
12588
12589
12590
12591
12592
12593
12594
12595
12596
12597
12598
12599
12600
12601
12602
12603
12604
12605
12606
12607
12608
12609
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 --p;
12659 --zv;
12660 --yv;
12661 --xv;
12662 --listv;
12663
12664
12665
12666
12667
12668
12669 imx = *lv;
12670 n = *nv;
12671 if (n < 3 || n > imx) {
12672 goto L11;
12673 }
12674 k0 = 0;
12675 i1 = listv[1];
12676 if (i1 < 1 || i1 > imx) {
12677 goto L12;
12678 }
12679
12680
12681
12682
12683
12684 L1:
12685 ++k0;
12686 if (k0 > n) {
12687 goto L14;
12688 }
12689 i1 = listv[k0];
12690 if (k0 < n) {
12691 i2 = listv[k0 + 1];
12692 } else {
12693 i2 = listv[1];
12694 }
12695 if (i2 < 1 || i2 > imx) {
12696 goto L12;
12697 }
12698 vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12699 vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12700 vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12701 vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12702 if (vnrm == 0.) {
12703 goto L1;
12704 }
12705 q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12706 q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12707 q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12708 qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12709 q[0] /= qnrm;
12710 q[1] /= qnrm;
12711 q[2] /= qnrm;
12712
12713
12714
12715 cn[0] = q[1] * p[3] - q[2] * p[2];
12716 cn[1] = q[2] * p[1] - q[0] * p[3];
12717 cn[2] = q[0] * p[2] - q[1] * p[1];
12718 if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12719 goto L1;
12720 }
12721 pn[0] = p[2] * cn[2] - p[3] * cn[1];
12722 pn[1] = p[3] * cn[0] - p[1] * cn[2];
12723 pn[2] = p[1] * cn[1] - p[2] * cn[0];
12724 qn[0] = cn[1] * q[2] - cn[2] * q[1];
12725 qn[1] = cn[2] * q[0] - cn[0] * q[2];
12726 qn[2] = cn[0] * q[1] - cn[1] * q[0];
12727
12728
12729
12730 ni = 0;
12731 even = TRUE_;
12732 bp = -2.;
12733 bq = -2.;
12734 pinr = TRUE_;
12735 qinr = TRUE_;
12736 i2 = listv[n];
12737 if (i2 < 1 || i2 > imx) {
12738 goto L12;
12739 }
12740 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12741
12742
12743
12744 i__1 = n;
12745 for (k = 1; k <= i__1; ++k) {
12746 i1 = i2;
12747 lft1 = lft2;
12748 i2 = listv[k];
12749 if (i2 < 1 || i2 > imx) {
12750 goto L12;
12751 }
12752 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12753 if (lft1 == lft2) {
12754 goto L2;
12755 }
12756
12757
12758
12759
12760 ++ni;
12761 v1[0] = xv[i1];
12762 v1[1] = yv[i1];
12763 v1[2] = zv[i1];
12764 v2[0] = xv[i2];
12765 v2[1] = yv[i2];
12766 v2[2] = zv[i2];
12767 intrsc_(v1, v2, cn, b, &ierr);
12768
12769
12770
12771
12772
12773 if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12774 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12775
12776
12777
12778 even = ! even;
12779 d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12780 if (d__ > bq) {
12781 bq = d__;
12782 qinr = lft2;
12783 }
12784 d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12785 if (d__ > bp) {
12786 bp = d__;
12787 pinr = lft1;
12788 }
12789 }
12790 L2:
12791 ;
12792 }
12793
12794
12795
12796
12797 if (ni != ni / 2 << 1 || ! qinr) {
12798 goto L1;
12799 }
12800
12801
12802
12803 if (pinr != even) {
12804 goto L13;
12805 }
12806
12807
12808
12809 *ier = 0;
12810 ret_val = even;
12811 return ret_val;
12812
12813
12814
12815 L11:
12816 *ier = 1;
12817 return ret_val;
12818
12819
12820
12821 L12:
12822 *ier = 2;
12823 return ret_val;
12824
12825
12826
12827 L13:
12828 *ier = 3;
12829 return ret_val;
12830
12831
12832
12833 L14:
12834 *ier = 4;
12835 return ret_val;
12836 }
12837
12838 int intadd_(int *kk, int *i1, int *i2, int *
12839 i3, int *list, int *lptr, int *lend, int *lnew)
12840 {
12841 static int k, n1, n2, n3, lp;
12842 extern int insert_(int *, int *, int *,
12843 int *, int *);
12844 extern int lstptr_(int *, int *, int *, int *);
12845
12846
12847
12848
12849
12850
12851
12852
12853
12854
12855
12856
12857
12858
12859
12860
12861
12862
12863
12864
12865
12866
12867
12868
12869
12870
12871
12872
12873
12874
12875
12876
12877
12878
12879
12880
12881
12882
12883
12884
12885
12886
12887
12888
12889
12890
12891
12892
12893
12894
12895
12896
12897
12898
12899
12900
12901
12902 --lend;
12903 --lptr;
12904 --list;
12905
12906
12907 k = *kk;
12908
12909
12910
12911 n1 = *i1;
12912 n2 = *i2;
12913 n3 = *i3;
12914
12915
12916
12917 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12918 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12919 lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12920 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12921 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12922 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12923
12924
12925
12926 list[*lnew] = n1;
12927 list[*lnew + 1] = n2;
12928 list[*lnew + 2] = n3;
12929 lptr[*lnew] = *lnew + 1;
12930 lptr[*lnew + 1] = *lnew + 2;
12931 lptr[*lnew + 2] = *lnew;
12932 lend[k] = *lnew + 2;
12933 *lnew += 3;
12934 return 0;
12935 }
12936
12937 int intrsc_(double *p1, double *p2, double *cn,
12938 double *p, int *ier)
12939 {
12940
12941
12942
12943
12944 static int i__;
12945 static double t, d1, d2, pp[3], ppn;
12946
12947
12948
12949
12950
12951
12952
12953
12954
12955
12956
12957
12958
12959
12960
12961
12962
12963
12964
12965
12966
12967
12968
12969
12970
12971
12972
12973
12974
12975
12976
12977
12978
12979
12980
12981
12982
12983
12984
12985
12986
12987
12988
12989
12990
12991
12992
12993
12994
12995
12996
12997
12998
12999
13000
13001
13002
13003
13004
13005
13006
13007
13008
13009
13010
13011
13012
13013
13014
13015 --p;
13016 --cn;
13017 --p2;
13018 --p1;
13019
13020
13021 d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
13022 d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
13023
13024 if (d1 == d2) {
13025 *ier = 1;
13026 return 0;
13027 }
13028
13029
13030
13031 t = d1 / (d1 - d2);
13032 ppn = 0.;
13033 for (i__ = 1; i__ <= 3; ++i__) {
13034 pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
13035 ppn += pp[i__ - 1] * pp[i__ - 1];
13036
13037 }
13038
13039
13040
13041 if (ppn == 0.) {
13042 *ier = 2;
13043 return 0;
13044 }
13045 ppn = sqrt(ppn);
13046
13047
13048
13049 for (i__ = 1; i__ <= 3; ++i__) {
13050 p[i__] = pp[i__ - 1] / ppn;
13051
13052 }
13053 *ier = 0;
13054 return 0;
13055 }
13056
13057 int jrand_(int *n, int *ix, int *iy, int *iz)
13058 {
13059
13060 int ret_val;
13061
13062
13063 static float u, x;
13064
13065
13066
13067
13068
13069
13070
13071
13072
13073
13074
13075
13076
13077
13078
13079
13080
13081
13082
13083
13084
13085
13086
13087
13088
13089
13090
13091
13092
13093
13094
13095
13096
13097
13098
13099
13100
13101
13102
13103
13104
13105
13106
13107
13108
13109
13110
13111
13112
13113
13114
13115
13116
13117 *ix = *ix * 171 % 30269;
13118 *iy = *iy * 172 % 30307;
13119 *iz = *iz * 170 % 30323;
13120 x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13121 30323.f;
13122 u = x - (int) x;
13123 ret_val = (int) ((float) (*n) * u + 1.f);
13124 return ret_val;
13125 }
13126
13127 long int left_(double *x1, double *y1, double *z1, double *x2,
13128 double *y2, double *z2, double *x0, double *y0,
13129 double *z0)
13130 {
13131
13132 long int ret_val;
13133
13134
13135
13136
13137
13138
13139
13140
13141
13142
13143
13144
13145
13146
13147
13148
13149
13150
13151
13152
13153
13154
13155
13156
13157
13158
13159
13160
13161
13162
13163
13164
13165
13166
13167
13168
13169
13170
13171 ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13172 *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13173
13174
13175 return ret_val;
13176 }
13177
13178 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13179 {
13180
13181 int ret_val;
13182
13183
13184 static int nd, lp;
13185
13186
13187
13188
13189
13190
13191
13192
13193
13194
13195
13196
13197
13198
13199
13200
13201
13202
13203
13204
13205
13206
13207
13208
13209
13210
13211
13212
13213
13214
13215
13216
13217
13218
13219
13220
13221
13222
13223
13224
13225
13226
13227
13228
13229
13230
13231
13232 --lptr;
13233 --list;
13234
13235
13236 lp = lptr[*lpl];
13237 L1:
13238 nd = list[lp];
13239 if (nd == *nb) {
13240 goto L2;
13241 }
13242 lp = lptr[lp];
13243 if (lp != *lpl) {
13244 goto L1;
13245 }
13246
13247 L2:
13248 ret_val = lp;
13249 return ret_val;
13250 }
13251
13252 int nbcnt_(int *lpl, int *lptr)
13253 {
13254
13255 int ret_val;
13256
13257
13258 static int k, lp;
13259
13260
13261
13262
13263
13264
13265
13266
13267
13268
13269
13270
13271
13272
13273
13274
13275
13276
13277
13278
13279
13280
13281
13282
13283
13284
13285
13286
13287
13288
13289
13290
13291
13292
13293
13294
13295
13296
13297
13298
13299
13300
13301 --lptr;
13302
13303
13304 lp = *lpl;
13305 k = 1;
13306
13307 L1:
13308 lp = lptr[lp];
13309 if (lp == *lpl) {
13310 goto L2;
13311 }
13312 ++k;
13313 goto L1;
13314
13315 L2:
13316 ret_val = k;
13317 return ret_val;
13318 }
13319
13320 int nearnd_(double *p, int *ist, int *n, double *x,
13321 double *y, double *z__, int *list, int *lptr, int
13322 *lend, double *al)
13323 {
13324
13325 int ret_val, i__1;
13326
13327
13328
13329
13330
13331 static int l;
13332 static double b1, b2, b3;
13333 static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13334 static double ds1;
13335 static int lp1, lp2;
13336 static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13337 static int lpl;
13338 static double dsr;
13339 static int nst, listp[25], lptrp[25];
13340 extern int trfind_(int *, double *, int *,
13341 double *, double *, double *, int *, int *,
13342 int *, double *, double *, double *, int *,
13343 int *, int *);
13344 extern int lstptr_(int *, int *, int *, int *);
13345
13346
13347
13348
13349
13350
13351
13352
13353
13354
13355
13356
13357
13358
13359
13360
13361
13362
13363
13364
13365
13366
13367
13368
13369
13370
13371
13372
13373
13374
13375
13376
13377
13378
13379
13380
13381
13382
13383
13384
13385
13386
13387
13388
13389
13390
13391
13392
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 --p;
13442 --lend;
13443 --z__;
13444 --y;
13445 --x;
13446 --list;
13447 --lptr;
13448
13449
13450 nn = *n;
13451 if (nn < 3) {
13452 goto L6;
13453 }
13454 nst = *ist;
13455 if (nst < 1 || nst > nn) {
13456 nst = 1;
13457 }
13458
13459
13460
13461
13462
13463 trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13464 1], &b1, &b2, &b3, &i1, &i2, &i3);
13465
13466
13467
13468 if (i1 == 0) {
13469 goto L6;
13470 }
13471
13472
13473
13474
13475
13476
13477
13478 if (i3 != 0) {
13479 listp[0] = i1;
13480 lptrp[0] = 2;
13481 listp[1] = i2;
13482 lptrp[1] = 3;
13483 listp[2] = i3;
13484 lptrp[2] = 1;
13485 l = 3;
13486 } else {
13487 n1 = i1;
13488 l = 1;
13489 lp1 = 2;
13490 listp[l - 1] = n1;
13491 lptrp[l - 1] = lp1;
13492
13493
13494
13495
13496 L1:
13497 lpl = lend[n1];
13498 n1 = -list[lpl];
13499 l = lp1;
13500 lp1 = l + 1;
13501 listp[l - 1] = n1;
13502 lptrp[l - 1] = lp1;
13503 if (n1 != i2 && lp1 < 25) {
13504 goto L1;
13505 }
13506 l = lp1;
13507 listp[l - 1] = 0;
13508 lptrp[l - 1] = 1;
13509 }
13510
13511
13512
13513
13514
13515
13516 lp2 = 1;
13517 n2 = i1;
13518 lp1 = lptrp[0];
13519 n1 = listp[lp1 - 1];
13520
13521
13522
13523 L2:
13524 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13525 if (list[lp] < 0) {
13526 goto L3;
13527 }
13528 lp = lptr[lp];
13529 n3 = (i__1 = list[lp], abs(i__1));
13530
13531
13532
13533 if (l == 25) {
13534 goto L4;
13535 }
13536 dx1 = x[n1] - p[1];
13537 dy1 = y[n1] - p[2];
13538 dz1 = z__[n1] - p[3];
13539
13540 dx2 = x[n2] - p[1];
13541 dy2 = y[n2] - p[2];
13542 dz2 = z__[n2] - p[3];
13543
13544 dx3 = x[n3] - p[1];
13545 dy3 = y[n3] - p[2];
13546 dz3 = z__[n3] - p[3];
13547 if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13548 (dx2 * dy1 - dx1 * dy2) <= 0.) {
13549 goto L3;
13550 }
13551
13552
13553
13554
13555 ++l;
13556 lptrp[lp2 - 1] = l;
13557 listp[l - 1] = n3;
13558 lptrp[l - 1] = lp1;
13559 lp1 = l;
13560 n1 = n3;
13561 goto L2;
13562
13563
13564
13565
13566 L3:
13567 if (lp1 == 1) {
13568 goto L4;
13569 }
13570 lp2 = lp1;
13571 n2 = n1;
13572 lp1 = lptrp[lp1 - 1];
13573 n1 = listp[lp1 - 1];
13574 if (n1 == 0) {
13575 goto L4;
13576 }
13577 goto L2;
13578
13579
13580
13581
13582
13583 L4:
13584 nr = i1;
13585 dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13586 i__1 = l;
13587 for (lp = 2; lp <= i__1; ++lp) {
13588 n1 = listp[lp - 1];
13589 if (n1 == 0) {
13590 goto L5;
13591 }
13592 ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13593 if (ds1 < dsr) {
13594 nr = n1;
13595 dsr = ds1;
13596 }
13597 L5:
13598 ;
13599 }
13600 dsr = -dsr;
13601 if (dsr > 1.) {
13602 dsr = 1.;
13603 }
13604 *al = acos(dsr);
13605 ret_val = nr;
13606 return ret_val;
13607
13608
13609
13610 L6:
13611 ret_val = 0;
13612 return ret_val;
13613 }
13614
13615 int optim_(double *x, double *y, double *z__,
13616 int *na, int *list, int *lptr, int *lend, int *
13617 nit, int *iwk, int *ier)
13618 {
13619
13620 int i__1, i__2;
13621
13622
13623 static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13624 static long int swp;
13625 static int iter;
13626 extern int swap_(int *, int *, int *,
13627 int *, int *, int *, int *, int *);
13628 static int maxit;
13629 extern long int swptst_(int *, int *, int *, int *,
13630 double *, double *, double *);
13631
13632
13633
13634
13635
13636
13637
13638
13639
13640
13641
13642
13643
13644
13645
13646
13647
13648
13649
13650
13651
13652
13653
13654
13655
13656
13657
13658
13659
13660
13661
13662
13663
13664
13665
13666
13667
13668
13669
13670
13671
13672
13673
13674
13675
13676
13677
13678
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 --x;
13728 --y;
13729 --z__;
13730 iwk -= 3;
13731 --list;
13732 --lptr;
13733 --lend;
13734
13735
13736 nna = *na;
13737 maxit = *nit;
13738 if (nna < 0 || maxit < 1) {
13739 goto L7;
13740 }
13741
13742
13743
13744 iter = 0;
13745 if (nna == 0) {
13746 goto L5;
13747 }
13748
13749
13750
13751
13752 L1:
13753 if (iter == maxit) {
13754 goto L6;
13755 }
13756 ++iter;
13757 swp = FALSE_;
13758
13759
13760
13761 i__1 = nna;
13762 for (i__ = 1; i__ <= i__1; ++i__) {
13763 io1 = iwk[(i__ << 1) + 1];
13764 io2 = iwk[(i__ << 1) + 2];
13765
13766
13767
13768
13769
13770
13771
13772
13773 lpl = lend[io1];
13774 lpp = lpl;
13775 lp = lptr[lpp];
13776 L2:
13777 if (list[lp] == io2) {
13778 goto L3;
13779 }
13780 lpp = lp;
13781 lp = lptr[lpp];
13782 if (lp != lpl) {
13783 goto L2;
13784 }
13785
13786
13787
13788
13789
13790 if ((i__2 = list[lp], abs(i__2)) != io2) {
13791 goto L8;
13792 }
13793 if (list[lp] < 0) {
13794 goto L4;
13795 }
13796
13797
13798
13799
13800 L3:
13801 n2 = list[lpp];
13802 if (n2 < 0) {
13803 goto L4;
13804 }
13805 lp = lptr[lp];
13806 n1 = (i__2 = list[lp], abs(i__2));
13807
13808
13809
13810 if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13811 goto L4;
13812 }
13813 swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13814 if (lp21 == 0) {
13815 goto L9;
13816 }
13817 swp = TRUE_;
13818 iwk[(i__ << 1) + 1] = n1;
13819 iwk[(i__ << 1) + 2] = n2;
13820 L4:
13821 ;
13822 }
13823 if (swp) {
13824 goto L1;
13825 }
13826
13827
13828
13829 L5:
13830 *nit = iter;
13831 *ier = 0;
13832 return 0;
13833
13834
13835
13836 L6:
13837 *nit = maxit;
13838 *ier = 1;
13839 return 0;
13840
13841
13842
13843 L7:
13844 *nit = 0;
13845 *ier = 2;
13846 return 0;
13847
13848
13849
13850 L8:
13851 *nit = iter;
13852 *ier = 3;
13853 return 0;
13854
13855
13856
13857 L9:
13858 *nit = iter;
13859 *ier = 4;
13860 return 0;
13861 }
13862
13863 int projct_(double *px, double *py, double *pz,
13864 double *ox, double *oy, double *oz, double *ex,
13865 double *ey, double *ez, double *vx, double *vy,
13866 double *vz, long int *init, double *x, double *y,
13867 double *z__, int *ier)
13868 {
13869
13870
13871
13872
13873 static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13874 oes, xoe, yoe, zoe, xep, yep, zep;
13875
13876
13877
13878
13879
13880
13881
13882
13883
13884
13885
13886
13887
13888
13889
13890
13891
13892
13893
13894
13895
13896
13897
13898
13899
13900
13901
13902
13903
13904
13905
13906
13907
13908
13909
13910
13911
13912
13913
13914
13915
13916
13917
13918
13919
13920
13921
13922
13923
13924
13925
13926
13927
13928
13929
13930
13931
13932
13933
13934
13935
13936
13937
13938
13939
13940
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 if (*init) {
13990
13991
13992
13993
13994
13995
13996
13997
13998 xe = *ex;
13999 ye = *ey;
14000 ze = *ez;
14001 xoe = xe - *ox;
14002 yoe = ye - *oy;
14003 zoe = ze - *oz;
14004 oes = xoe * xoe + yoe * yoe + zoe * zoe;
14005 if (oes == 0.) {
14006 goto L1;
14007 }
14008
14009
14010
14011 s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
14012 xv = *vx - s * xoe;
14013 yv = *vy - s * yoe;
14014 zv = *vz - s * zoe;
14015
14016
14017
14018 sc = xv * xv + yv * yv + zv * zv;
14019 if (sc == 0.) {
14020 goto L2;
14021 }
14022 sc = 1. / sqrt(sc);
14023 xv = sc * xv;
14024 yv = sc * yv;
14025 zv = sc * zv;
14026
14027
14028
14029 xh = yv * zoe - yoe * zv;
14030 yh = xoe * zv - xv * zoe;
14031 zh = xv * yoe - xoe * yv;
14032 sc = sqrt(xh * xh + yh * yh + zh * zh);
14033 if (sc == 0.) {
14034 goto L2;
14035 }
14036 sc = 1. / sc;
14037 xh = sc * xh;
14038 yh = sc * yh;
14039 zh = sc * zh;
14040 }
14041
14042
14043
14044
14045
14046
14047 xep = *px - xe;
14048 yep = *py - ye;
14049 zep = *pz - ze;
14050 s = xoe * xep + yoe * yep + zoe * zep;
14051 if (s >= 0.) {
14052 goto L1;
14053 }
14054 s = oes / s;
14055 xw = xoe - s * xep;
14056 yw = yoe - s * yep;
14057 zw = zoe - s * zep;
14058
14059
14060
14061
14062 *x = xw * xh + yw * yh + zw * zh;
14063 *y = xw * xv + yw * yv + zw * zv;
14064 *z__ = s + 1.;
14065 *init = FALSE_;
14066 *ier = 0;
14067 return 0;
14068
14069
14070
14071 L1:
14072 *ier = 1;
14073 return 0;
14074
14075
14076
14077 L2:
14078 *ier = 2;
14079 return 0;
14080 }
14081
14082 int scoord_(double *px, double *py, double *pz,
14083 double *plat, double *plon, double *pnrm)
14084 {
14085
14086
14087
14088
14089
14090
14091
14092
14093
14094
14095
14096
14097
14098
14099
14100
14101
14102
14103
14104
14105
14106
14107
14108
14109
14110
14111
14112
14113
14114
14115
14116
14117
14118
14119
14120
14121
14122
14123
14124
14125
14126
14127 *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14128 if (*px != 0. || *py != 0.) {
14129 *plon = atan2(*py, *px);
14130 } else {
14131 *plon = 0.;
14132 }
14133 if (*pnrm != 0.) {
14134 *plat = asin(*pz / *pnrm);
14135 } else {
14136 *plat = 0.;
14137 }
14138 return 0;
14139 }
14140
14141 double store_(double *x)
14142 {
14143
14144 double ret_val;
14145
14146
14147
14148
14149
14150
14151
14152
14153
14154
14155
14156
14157
14158
14159
14160
14161
14162
14163
14164
14165
14166
14167
14168
14169
14170
14171
14172
14173
14174
14175
14176
14177
14178
14179 stcom_1.y = *x;
14180 ret_val = stcom_1.y;
14181 return ret_val;
14182 }
14183
14184 int swap_(int *in1, int *in2, int *io1, int *
14185 io2, int *list, int *lptr, int *lend, int *lp21)
14186 {
14187
14188 int i__1;
14189
14190
14191 static int lp, lph, lpsav;
14192 extern int lstptr_(int *, int *, int *, int *);
14193
14194
14195
14196
14197
14198
14199
14200
14201
14202
14203
14204
14205
14206
14207
14208
14209
14210
14211
14212
14213
14214
14215
14216
14217
14218
14219
14220
14221
14222
14223
14224
14225
14226
14227
14228
14229
14230
14231
14232
14233
14234
14235
14236
14237
14238
14239
14240
14241
14242
14243
14244
14245
14246
14247
14248
14249
14250
14251
14252
14253 --lend;
14254 --lptr;
14255 --list;
14256
14257
14258 lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14259 if ((i__1 = list[lp], abs(i__1)) == *in2) {
14260 *lp21 = 0;
14261 return 0;
14262 }
14263
14264
14265
14266 lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14267 lph = lptr[lp];
14268 lptr[lp] = lptr[lph];
14269
14270
14271
14272
14273 if (lend[*io1] == lph) {
14274 lend[*io1] = lp;
14275 }
14276
14277
14278
14279
14280 lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14281 lpsav = lptr[lp];
14282 lptr[lp] = lph;
14283 list[lph] = *in2;
14284 lptr[lph] = lpsav;
14285
14286
14287
14288 lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14289 lph = lptr[lp];
14290 lptr[lp] = lptr[lph];
14291
14292
14293
14294
14295 if (lend[*io2] == lph) {
14296 lend[*io2] = lp;
14297 }
14298
14299
14300
14301 lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14302 lpsav = lptr[lp];
14303 lptr[lp] = lph;
14304 list[lph] = *in1;
14305 lptr[lph] = lpsav;
14306 *lp21 = lph;
14307 return 0;
14308 }
14309
14310 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14311 double *x, double *y, double *z__)
14312 {
14313
14314 long int ret_val;
14315
14316
14317 static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14318
14319
14320
14321
14322
14323
14324
14325
14326
14327
14328
14329
14330
14331
14332
14333
14334
14335
14336
14337
14338
14339
14340
14341
14342
14343
14344
14345
14346
14347
14348
14349
14350
14351
14352
14353
14354
14355
14356
14357
14358
14359
14360
14361
14362
14363
14364
14365
14366
14367
14368
14369
14370
14371
14372
14373
14374 --z__;
14375 --y;
14376 --x;
14377
14378
14379 x4 = x[*n4];
14380 y4 = y[*n4];
14381 z4 = z__[*n4];
14382 dx1 = x[*n1] - x4;
14383 dx2 = x[*n2] - x4;
14384 dx3 = x[*n3] - x4;
14385 dy1 = y[*n1] - y4;
14386 dy2 = y[*n2] - y4;
14387 dy3 = y[*n3] - y4;
14388 dz1 = z__[*n1] - z4;
14389 dz2 = z__[*n2] - z4;
14390 dz3 = z__[*n3] - z4;
14391
14392
14393
14394
14395
14396 ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14397 dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14398 return ret_val;
14399 }
14400
14401 int trans_(int *n, double *rlat, double *rlon,
14402 double *x, double *y, double *z__)
14403 {
14404
14405 int i__1;
14406
14407
14408
14409
14410
14411 static int i__, nn;
14412 static double phi, theta, cosphi;
14413
14414
14415
14416
14417
14418
14419
14420
14421
14422
14423
14424
14425
14426
14427
14428
14429
14430
14431
14432
14433
14434
14435
14436
14437
14438
14439
14440
14441
14442
14443
14444
14445
14446
14447
14448
14449
14450
14451
14452
14453
14454
14455
14456
14457
14458
14459
14460
14461
14462
14463
14464
14465
14466
14467 --z__;
14468 --y;
14469 --x;
14470 --rlon;
14471 --rlat;
14472
14473
14474 nn = *n;
14475 i__1 = nn;
14476 for (i__ = 1; i__ <= i__1; ++i__) {
14477 phi = rlat[i__];
14478 theta = rlon[i__];
14479 cosphi = cos(phi);
14480 x[i__] = cosphi * cos(theta);
14481 y[i__] = cosphi * sin(theta);
14482 z__[i__] = sin(phi);
14483
14484 }
14485 return 0;
14486 }
14487
14488 int trfind_(int *nst, double *p, int *n,
14489 double *x, double *y, double *z__, int *list, int
14490 *lptr, int *lend, double *b1, double *b2, double *b3,
14491 int *i1, int *i2, int *i3)
14492 {
14493
14494
14495 static int ix = 1;
14496 static int iy = 2;
14497 static int iz = 3;
14498
14499
14500 int i__1;
14501 double d__1, d__2;
14502
14503
14504 static double q[3];
14505 static int n0, n1, n2, n3, n4, nf;
14506 static double s12;
14507 static int nl, lp;
14508 static double xp, yp, zp;
14509 static int n1s, n2s;
14510 static double eps, tol, ptn1, ptn2;
14511 static int next;
14512 extern int jrand_(int *, int *, int *, int *);
14513 extern double store_(double *);
14514 extern int lstptr_(int *, int *, int *, int *);
14515
14516
14517
14518
14519
14520
14521
14522
14523
14524
14525
14526
14527
14528
14529
14530
14531
14532
14533
14534
14535
14536
14537
14538
14539
14540
14541
14542
14543
14544
14545
14546
14547
14548
14549
14550
14551
14552
14553
14554
14555
14556
14557
14558
14559
14560
14561
14562
14563
14564
14565
14566
14567
14568
14569
14570
14571
14572
14573
14574
14575
14576
14577
14578
14579
14580
14581
14582
14583
14584 --p;
14585 --lend;
14586 --z__;
14587 --y;
14588 --x;
14589 --list;
14590 --lptr;
14591
14592
14593
14594
14595
14596
14597
14598
14599
14600
14601
14602
14603
14604
14605
14606
14607
14608
14609
14610
14611
14612
14613
14614
14615
14616
14617
14618
14619
14620
14621
14622
14623
14624
14625
14626
14627
14628
14629
14630
14631
14632
14633
14634
14635
14636
14637
14638
14639 xp = p[1];
14640 yp = p[2];
14641 zp = p[3];
14642 n0 = *nst;
14643 if (n0 < 1 || n0 > *n) {
14644 n0 = jrand_(n, &ix, &iy, &iz);
14645 }
14646
14647
14648
14649 eps = 1.;
14650 L1:
14651 eps /= 2.;
14652 d__1 = eps + 1.;
14653 if (store_(&d__1) > 1.) {
14654 goto L1;
14655 }
14656 eps *= 2.;
14657 tol = eps * 4.;
14658
14659
14660
14661
14662 L2:
14663 lp = lend[n0];
14664 nl = list[lp];
14665 lp = lptr[lp];
14666 nf = list[lp];
14667 n1 = nf;
14668
14669
14670
14671
14672 if (nl > 0) {
14673
14674
14675
14676 L3:
14677 if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14678 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14679 -1e-10) {
14680 lp = lptr[lp];
14681 n1 = list[lp];
14682 if (n1 == nl) {
14683 goto L6;
14684 }
14685 goto L3;
14686 }
14687 } else {
14688
14689
14690
14691 nl = -nl;
14692 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14693 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14694 -1e-10) {
14695
14696
14697
14698 n1 = n0;
14699 n2 = nf;
14700 goto L9;
14701 }
14702 if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14703 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14704 -1e-10) {
14705
14706
14707
14708 n1 = nl;
14709 n2 = n0;
14710 goto L9;
14711 }
14712 }
14713
14714
14715
14716
14717 L4:
14718 lp = lptr[lp];
14719 n2 = (i__1 = list[lp], abs(i__1));
14720 if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14721 n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14722 goto L7;
14723 }
14724 n1 = n2;
14725 if (n1 != nl) {
14726 goto L4;
14727 }
14728 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14729 nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14730 goto L6;
14731 }
14732
14733
14734
14735
14736 d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14737 if (store_(&d__2) < 1. - eps * 4.) {
14738
14739
14740
14741
14742
14743 L5:
14744 if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14745 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14746 -1e-10) {
14747 lp = lptr[lp];
14748 n1 = (i__1 = list[lp], abs(i__1));
14749 if (n1 == nl) {
14750 goto L14;
14751 }
14752 goto L5;
14753 }
14754 }
14755
14756
14757
14758
14759 n0 = n1;
14760 goto L2;
14761
14762
14763
14764 L6:
14765 n2 = nf;
14766
14767
14768
14769
14770
14771 L7:
14772 n3 = n0;
14773 n1s = n1;
14774 n2s = n2;
14775
14776
14777
14778 L8:
14779
14780 *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14781 x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14782 if (*b3 < -1e-10) {
14783
14784
14785
14786
14787 lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14788 if (list[lp] < 0) {
14789 goto L9;
14790 }
14791 lp = lptr[lp];
14792 n4 = (i__1 = list[lp], abs(i__1));
14793
14794
14795
14796 if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14797 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14798 -1e-10) {
14799 n3 = n2;
14800 n2 = n4;
14801 n1s = n1;
14802 if (n2 != n2s && n2 != n0) {
14803 goto L8;
14804 }
14805 } else {
14806 n3 = n1;
14807 n1 = n4;
14808 n2s = n2;
14809 if (n1 != n1s && n1 != n0) {
14810 goto L8;
14811 }
14812 }
14813
14814
14815
14816
14817
14818 n0 = jrand_(n, &ix, &iy, &iz);
14819 goto L2;
14820 }
14821
14822
14823
14824
14825 if (*b3 >= eps) {
14826
14827
14828
14829 *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14830 - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14831 *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14832 - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14833 if (*b1 < -tol || *b2 < -tol) {
14834
14835
14836
14837 n0 = jrand_(n, &ix, &iy, &iz);
14838 goto L2;
14839 }
14840 } else {
14841
14842
14843
14844
14845 *b3 = 0.;
14846 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14847 ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14848 ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14849 *b1 = ptn1 - s12 * ptn2;
14850 *b2 = ptn2 - s12 * ptn1;
14851 if (*b1 < -tol || *b2 < -tol) {
14852
14853
14854
14855 n0 = jrand_(n, &ix, &iy, &iz);
14856 goto L2;
14857 }
14858 }
14859
14860
14861
14862 *i1 = n1;
14863 *i2 = n2;
14864 *i3 = n3;
14865 if (*b1 < 0.f) {
14866 *b1 = 0.f;
14867 }
14868 if (*b2 < 0.f) {
14869 *b2 = 0.f;
14870 }
14871 return 0;
14872
14873
14874
14875
14876
14877 L9:
14878 n1s = n1;
14879 n2s = n2;
14880 nl = 0;
14881
14882
14883
14884 L10:
14885
14886 lp = lend[n2];
14887 lp = lptr[lp];
14888 next = list[lp];
14889 if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14890 - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14891 >= -1e-10) {
14892
14893
14894
14895
14896 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14897 q[0] = x[n1] - s12 * x[n2];
14898 q[1] = y[n1] - s12 * y[n2];
14899 q[2] = z__[n1] - s12 * z__[n2];
14900 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14901 goto L11;
14902 }
14903 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14904 goto L11;
14905 }
14906
14907
14908
14909
14910 nl = n2;
14911 }
14912
14913
14914
14915 n1 = n2;
14916 n2 = next;
14917 if (n2 != n1s) {
14918 goto L10;
14919 }
14920
14921
14922
14923 *i1 = n1s;
14924 *i2 = n1s;
14925 *i3 = 0;
14926 return 0;
14927
14928
14929
14930 L11:
14931 nf = n2;
14932 if (nl == 0) {
14933
14934
14935
14936
14937 n2 = n2s;
14938 n1 = n1s;
14939
14940
14941
14942 L12:
14943 lp = lend[n1];
14944 next = -list[lp];
14945 if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14946 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14947 y[next]) >= -1e-10) {
14948
14949
14950
14951
14952 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14953 q[0] = x[n2] - s12 * x[n1];
14954 q[1] = y[n2] - s12 * y[n1];
14955 q[2] = z__[n2] - s12 * z__[n1];
14956 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14957 goto L13;
14958 }
14959 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14960 goto L13;
14961 }
14962
14963
14964
14965
14966 nf = n1;
14967 }
14968
14969
14970
14971 n2 = n1;
14972 n1 = next;
14973 if (n1 != n1s) {
14974 goto L12;
14975 }
14976
14977
14978
14979 *i1 = n1;
14980 *i2 = n1;
14981 *i3 = 0;
14982 return 0;
14983
14984
14985
14986 L13:
14987 nl = n1;
14988 }
14989
14990
14991
14992 *i1 = nf;
14993 *i2 = nl;
14994 *i3 = 0;
14995 return 0;
14996
14997
14998
14999 L14:
15000 *i1 = 0;
15001 *i2 = 0;
15002 *i3 = 0;
15003 return 0;
15004 }
15005
15006 int trlist_(int *n, int *list, int *lptr,
15007 int *lend, int *nrow, int *nt, int *ltri, int *
15008 ier)
15009 {
15010
15011 int ltri_dim1, ltri_offset, i__1, i__2;
15012
15013
15014 static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
15015 lpl, isv;
15016 static long int arcs;
15017 static int lpln1;
15018
15019
15020
15021
15022
15023
15024
15025
15026
15027
15028
15029
15030
15031
15032
15033
15034
15035
15036
15037
15038
15039
15040
15041
15042
15043
15044
15045
15046
15047
15048
15049
15050
15051
15052
15053
15054
15055
15056
15057
15058
15059
15060
15061
15062
15063
15064
15065
15066
15067
15068
15069
15070
15071
15072
15073
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 --lend;
15123 --list;
15124 --lptr;
15125 ltri_dim1 = *nrow;
15126 ltri_offset = 1 + ltri_dim1;
15127 ltri -= ltri_offset;
15128
15129
15130 if (*n < 3 || (*nrow != 6 && *nrow != 9)) {
15131 goto L11;
15132 }
15133
15134
15135
15136
15137
15138
15139
15140
15141 arcs = *nrow == 9;
15142 ka = 0;
15143 kt = 0;
15144 nm2 = *n - 2;
15145
15146
15147
15148 i__1 = nm2;
15149 for (n1 = 1; n1 <= i__1; ++n1) {
15150
15151
15152
15153
15154 lpln1 = lend[n1];
15155 lp2 = lpln1;
15156 L1:
15157 lp2 = lptr[lp2];
15158 n2 = list[lp2];
15159 lp = lptr[lp2];
15160 n3 = (i__2 = list[lp], abs(i__2));
15161 if (n2 < n1 || n3 < n1) {
15162 goto L8;
15163 }
15164
15165
15166
15167 ++kt;
15168 ltri[kt * ltri_dim1 + 1] = n1;
15169 ltri[kt * ltri_dim1 + 2] = n2;
15170 ltri[kt * ltri_dim1 + 3] = n3;
15171
15172
15173
15174
15175 for (i__ = 1; i__ <= 3; ++i__) {
15176 if (i__ == 1) {
15177 i1 = n3;
15178 i2 = n2;
15179 } else if (i__ == 2) {
15180 i1 = n1;
15181 i2 = n3;
15182 } else {
15183 i1 = n2;
15184 i2 = n1;
15185 }
15186
15187
15188
15189
15190 lpl = lend[i1];
15191 lp = lptr[lpl];
15192 L2:
15193 if (list[lp] == i2) {
15194 goto L3;
15195 }
15196 lp = lptr[lp];
15197 if (lp != lpl) {
15198 goto L2;
15199 }
15200
15201
15202
15203
15204
15205 if ((i__2 = list[lp], abs(i__2)) != i2) {
15206 goto L12;
15207 }
15208 kn = 0;
15209 if (list[lp] < 0) {
15210 goto L6;
15211 }
15212
15213
15214
15215
15216 L3:
15217 lp = lptr[lp];
15218 i3 = (i__2 = list[lp], abs(i__2));
15219
15220
15221
15222
15223
15224 if (i1 < i2 && i1 < i3) {
15225 j = 3;
15226 } else if (i2 < i3) {
15227 j = 2;
15228 isv = i1;
15229 i1 = i2;
15230 i2 = i3;
15231 i3 = isv;
15232 } else {
15233 j = 1;
15234 isv = i1;
15235 i1 = i3;
15236 i3 = i2;
15237 i2 = isv;
15238 }
15239
15240
15241
15242 if (i1 > n1) {
15243 goto L7;
15244 }
15245
15246
15247
15248
15249 for (kn = kt - 1; kn >= 1; --kn) {
15250 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15251 == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15252 goto L5;
15253 }
15254
15255 }
15256 goto L7;
15257
15258
15259
15260 L5:
15261 ltri[j + 3 + kn * ltri_dim1] = kt;
15262
15263
15264
15265 L6:
15266 ltri[i__ + 3 + kt * ltri_dim1] = kn;
15267 if (arcs) {
15268 ++ka;
15269 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15270 if (kn != 0) {
15271 ltri[j + 6 + kn * ltri_dim1] = ka;
15272 }
15273 }
15274 L7:
15275 ;
15276 }
15277
15278
15279
15280 L8:
15281 if (lp2 != lpln1) {
15282 goto L1;
15283 }
15284
15285 }
15286
15287
15288
15289 *nt = kt;
15290 *ier = 0;
15291 return 0;
15292
15293
15294
15295 L11:
15296 *nt = 0;
15297 *ier = 1;
15298 return 0;
15299
15300
15301
15302
15303 L12:
15304 *nt = 0;
15305 *ier = 2;
15306 return 0;
15307 }
15308
15309 int trlprt_(int *n, double *x, double *y,
15310 double *z__, int *iflag, int *nrow, int *nt, int *
15311 ltri, int *lout)
15312 {
15313
15314
15315 static int nmax = 9999;
15316 static int nlmax = 58;
15317
15318
15319 int ltri_dim1, ltri_offset, i__1;
15320
15321
15322 static int i__, k, na, nb, nl, lun;
15323
15324
15325
15326
15327
15328
15329
15330
15331
15332
15333
15334
15335
15336
15337
15338
15339
15340
15341
15342
15343
15344
15345
15346
15347
15348
15349
15350
15351
15352
15353
15354
15355
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 --z__;
15397 --y;
15398 --x;
15399 ltri_dim1 = *nrow;
15400 ltri_offset = 1 + ltri_dim1;
15401 ltri -= ltri_offset;
15402
15403
15404
15405
15406
15407
15408
15409
15410
15411
15412
15413
15414
15415
15416
15417
15418 lun = *lout;
15419 if (lun < 0 || lun > 99) {
15420 lun = 6;
15421 }
15422
15423
15424
15425
15426 nl = 3;
15427 if (*n < 3 || *n > nmax || (*nrow != 6 && *nrow != 9) || *nt < 1 || *nt >
15428 nmax) {
15429
15430
15431
15432
15433 return 0;
15434 }
15435 if (*iflag == 0) {
15436
15437
15438
15439
15440 nl = 6;
15441 i__1 = *n;
15442 for (i__ = 1; i__ <= i__1; ++i__) {
15443 if (nl >= nlmax) {
15444
15445 nl = 0;
15446 }
15447
15448 ++nl;
15449
15450 }
15451 } else if (*iflag > 0) {
15452
15453
15454
15455
15456 nl = 6;
15457 i__1 = *n;
15458 for (i__ = 1; i__ <= i__1; ++i__) {
15459 if (nl >= nlmax) {
15460
15461 nl = 0;
15462 }
15463
15464 ++nl;
15465
15466 }
15467 }
15468
15469
15470
15471 if (nl > nlmax / 2) {
15472
15473 nl = 0;
15474 }
15475 if (*nrow == 6) {
15476
15477 } else {
15478
15479 }
15480 nl += 5;
15481 i__1 = *nt;
15482 for (k = 1; k <= i__1; ++k) {
15483 if (nl >= nlmax) {
15484
15485 nl = 0;
15486 }
15487
15488 ++nl;
15489
15490 }
15491
15492
15493
15494
15495 nb = (*n << 1) - *nt - 2;
15496 if (nb < 3) {
15497 nb = 0;
15498 na = *n * 3 - 6;
15499 } else {
15500 na = *nt + *n - 1;
15501 }
15502
15503 return 0;
15504
15505
15506
15507
15508
15509
15510
15511
15512
15513
15514
15515
15516
15517
15518
15519
15520
15521
15522
15523
15524
15525
15526
15527 }
15528
15529 int trmesh_(int *n, double *x, double *y,
15530 double *z__, int *list, int *lptr, int *lend, int
15531 *lnew, int *near__, int *next, double *dist, int *ier)
15532 {
15533
15534 int i__1, i__2;
15535
15536
15537 static double d__;
15538 static int i__, j, k;
15539 static double d1, d2, d3;
15540 static int i0, lp, nn, lpl;
15541 extern long int left_(double *, double *, double *, double
15542 *, double *, double *, double *, double *,
15543 double *);
15544 static int nexti;
15545 extern int addnod_(int *, int *, double *,
15546 double *, double *, int *, int *, int *,
15547 int *, int *);
15548
15549
15550
15551
15552
15553
15554
15555
15556
15557
15558
15559
15560
15561
15562
15563
15564
15565
15566
15567
15568
15569
15570
15571
15572
15573
15574
15575
15576
15577
15578
15579
15580
15581
15582
15583
15584
15585
15586
15587
15588
15589
15590
15591
15592
15593
15594
15595
15596
15597
15598
15599
15600
15601
15602
15603
15604
15605
15606
15607
15608
15609
15610
15611
15612
15613
15614
15615
15616
15617
15618
15619
15620
15621
15622
15623
15624
15625
15626
15627
15628
15629
15630
15631
15632
15633
15634
15635
15636
15637
15638
15639
15640
15641
15642
15643
15644
15645
15646
15647
15648
15649
15650
15651
15652
15653
15654
15655
15656
15657
15658
15659
15660
15661
15662
15663
15664
15665
15666
15667
15668
15669
15670
15671
15672
15673
15674
15675
15676
15677
15678
15679
15680
15681
15682
15683
15684
15685
15686
15687
15688
15689
15690
15691
15692
15693
15694
15695
15696
15697
15698
15699
15700
15701
15702
15703
15704
15705
15706
15707
15708
15709
15710
15711
15712
15713
15714
15715
15716
15717
15718
15719
15720
15721
15722
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 --dist;
15775 --next;
15776 --near__;
15777 --lend;
15778 --z__;
15779 --y;
15780 --x;
15781 --list;
15782 --lptr;
15783
15784
15785 nn = *n;
15786 if (nn < 3) {
15787 *ier = -1;
15788 return 0;
15789 }
15790
15791
15792
15793 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15794 z__[3])) {
15795
15796
15797
15798 list[1] = 3;
15799 lptr[1] = 2;
15800 list[2] = -2;
15801 lptr[2] = 1;
15802 lend[1] = 2;
15803
15804 list[3] = 1;
15805 lptr[3] = 4;
15806 list[4] = -3;
15807 lptr[4] = 3;
15808 lend[2] = 4;
15809
15810 list[5] = 2;
15811 lptr[5] = 6;
15812 list[6] = -1;
15813 lptr[6] = 5;
15814 lend[3] = 6;
15815
15816 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15817 y[3], &z__[3])) {
15818
15819
15820
15821
15822
15823 list[1] = 2;
15824 lptr[1] = 2;
15825 list[2] = -3;
15826 lptr[2] = 1;
15827 lend[1] = 2;
15828
15829 list[3] = 3;
15830 lptr[3] = 4;
15831 list[4] = -1;
15832 lptr[4] = 3;
15833 lend[2] = 4;
15834
15835 list[5] = 1;
15836 lptr[5] = 6;
15837 list[6] = -2;
15838 lptr[6] = 5;
15839 lend[3] = 6;
15840
15841 } else {
15842
15843
15844
15845 *ier = -2;
15846 return 0;
15847 }
15848
15849
15850
15851 *lnew = 7;
15852 if (nn == 3) {
15853 *ier = 0;
15854 return 0;
15855 }
15856
15857
15858
15859
15860
15861
15862
15863
15864
15865
15866
15867
15868
15869
15870
15871
15872
15873
15874
15875
15876
15877
15878
15879
15880
15881
15882
15883 near__[1] = 0;
15884 near__[2] = 0;
15885 near__[3] = 0;
15886 for (k = nn; k >= 4; --k) {
15887 d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15888 d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15889 d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15890 if (d1 <= d2 && d1 <= d3) {
15891 near__[k] = 1;
15892 dist[k] = d1;
15893 next[k] = near__[1];
15894 near__[1] = k;
15895 } else if (d2 <= d1 && d2 <= d3) {
15896 near__[k] = 2;
15897 dist[k] = d2;
15898 next[k] = near__[2];
15899 near__[2] = k;
15900 } else {
15901 near__[k] = 3;
15902 dist[k] = d3;
15903 next[k] = near__[3];
15904 near__[3] = k;
15905 }
15906
15907 }
15908
15909
15910
15911 i__1 = nn;
15912 for (k = 4; k <= i__1; ++k) {
15913 addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15914 lend[1], lnew, ier);
15915 if (*ier != 0) {
15916 return 0;
15917 }
15918
15919
15920
15921
15922 i__ = near__[k];
15923 if (near__[i__] == k) {
15924 near__[i__] = next[k];
15925 } else {
15926 i__ = near__[i__];
15927 L2:
15928 i0 = i__;
15929 i__ = next[i0];
15930 if (i__ != k) {
15931 goto L2;
15932 }
15933 next[i0] = next[k];
15934 }
15935 near__[k] = 0;
15936
15937
15938
15939 lpl = lend[k];
15940 lp = lpl;
15941 L3:
15942 lp = lptr[lp];
15943 j = (i__2 = list[lp], abs(i__2));
15944
15945
15946
15947
15948
15949
15950
15951 i__ = near__[j];
15952 L4:
15953 if (i__ == 0) {
15954 goto L5;
15955 }
15956 nexti = next[i__];
15957
15958
15959
15960
15961 d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15962 if (d__ < dist[i__]) {
15963
15964
15965
15966
15967
15968 near__[i__] = k;
15969 dist[i__] = d__;
15970 if (i__ == near__[j]) {
15971 near__[j] = nexti;
15972 } else {
15973 next[i0] = nexti;
15974 }
15975 next[i__] = near__[k];
15976 near__[k] = i__;
15977 } else {
15978 i0 = i__;
15979 }
15980
15981
15982
15983 i__ = nexti;
15984 goto L4;
15985
15986
15987
15988 L5:
15989 if (lp != lpl) {
15990 goto L3;
15991 }
15992
15993 }
15994 return 0;
15995 }
15996
15997 int trplot_(int *lun, double *pltsiz, double *
15998 elat, double *elon, double *a, int *n, double *x,
15999 double *y, double *z__, int *list, int *lptr, int
16000 *lend, char *, long int *numbr, int *ier, short )
16001 {
16002
16003
16004 static long int annot = TRUE_;
16005 static double fsizn = 10.;
16006 static double fsizt = 16.;
16007 static double tol = .5;
16008
16009
16010 int i__1, i__2;
16011 double d__1;
16012
16013
16014
16015
16016
16017
16018
16019 static double t;
16020 static int n0, n1;
16021 static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
16022 static int ir, lp;
16023 static double ex, ey, ez, wr, tx, ty;
16024 static int lpl;
16025 static double wrs;
16026 static int ipx1, ipx2, ipy1, ipy2, nseg;
16027 extern int drwarc_(int *, double *, double *,
16028 double *, int *);
16029
16030
16031
16032
16033
16034
16035
16036
16037
16038
16039
16040
16041
16042
16043
16044
16045
16046
16047
16048
16049
16050
16051
16052
16053
16054
16055
16056
16057
16058
16059
16060
16061
16062
16063
16064
16065
16066
16067
16068
16069
16070
16071
16072
16073
16074
16075
16076
16077
16078
16079
16080
16081
16082
16083
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 --lend;
16136 --z__;
16137 --y;
16138 --x;
16139 --list;
16140 --lptr;
16141
16142
16143
16144
16145
16146
16147
16148
16149
16150
16151
16152
16153
16154
16155
16156
16157
16158
16159
16160
16161
16162
16163
16164
16165
16166
16167
16168
16169
16170
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 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16197 goto L11;
16198 }
16199 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16200 goto L12;
16201 }
16202
16203
16204
16205
16206 cf = atan(1.) / 45.;
16207 wr = sin(cf * *a);
16208 wrs = wr * wr;
16209
16210
16211
16212
16213
16214
16215
16216
16217
16218
16219 d__1 = *pltsiz * 36.;
16220 ir = i_dnnt(&d__1);
16221 ipx1 = 306 - ir;
16222 ipx2 = ir + 306;
16223 ipy1 = 396 - ir;
16224 ipy2 = ir + 396;
16225
16226
16227
16228
16229
16230
16231
16232
16233
16234
16235
16236
16237
16238
16239 d__1 = (double) ir * .88;
16240 ir = i_dnnt(&d__1);
16241 ipx1 = 306 - ir;
16242 ipx2 = ir + 306;
16243 ipy1 = 396 - ir;
16244 ipy2 = ir + 396;
16245
16246
16247
16248
16249 t = 2.;
16250
16251
16252
16253
16254
16255
16256
16257
16258
16259
16260 sf = (double) ir / wr;
16261 tx = ipx1 + sf * wr;
16262 ty = ipy1 + sf * wr;
16263
16264
16265
16266
16267
16268
16269
16270
16271 t = 1. / sf;
16272
16273
16274
16275
16276
16277
16278
16279
16280
16281
16282
16283
16284
16285
16286
16287
16288
16289
16290
16291
16292
16293
16294
16295 t = cf * *elon;
16296 ct = cos(cf * *elat);
16297 ex = ct * cos(t);
16298 ey = ct * sin(t);
16299 ez = sin(cf * *elat);
16300 if (ct != 0.) {
16301 r11 = -ey / ct;
16302 r12 = ex / ct;
16303 } else {
16304 r11 = 0.;
16305 r12 = 1.;
16306 }
16307 r21 = -ez * r12;
16308 r22 = ez * r11;
16309 r23 = ct;
16310
16311
16312
16313
16314 i__1 = *n;
16315 for (n0 = 1; n0 <= i__1; ++n0) {
16316 p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16317 if (p0[2] < 0.) {
16318 goto L3;
16319 }
16320 p0[0] = r11 * x[n0] + r12 * y[n0];
16321 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16322 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16323 goto L3;
16324 }
16325 lpl = lend[n0];
16326 lp = lpl;
16327
16328
16329
16330
16331 L1:
16332 lp = lptr[lp];
16333 n1 = (i__2 = list[lp], abs(i__2));
16334 p1[0] = r11 * x[n1] + r12 * y[n1];
16335 p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16336 p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16337 if (p1[2] < 0.) {
16338
16339
16340
16341
16342
16343 p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16344 p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16345 t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16346 p1[0] /= t;
16347 p1[1] /= t;
16348 }
16349
16350
16351
16352
16353 if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16354 goto L2;
16355 }
16356
16357
16358
16359
16360 if (p1[2] < 0.) {
16361 p1[2] = 0.;
16362 }
16363 d__1 = tol / sf;
16364 drwarc_(lun, p0, p1, &d__1, &nseg);
16365
16366
16367
16368 L2:
16369 if (lp != lpl) {
16370 goto L1;
16371 }
16372 L3:
16373 ;
16374 }
16375
16376
16377
16378
16379
16380
16381
16382 if (*numbr) {
16383
16384
16385
16386
16387
16388 t = fsizn / sf;
16389
16390
16391
16392
16393
16394
16395
16396 i__1 = *n;
16397 for (n0 = 1; n0 <= i__1; ++n0) {
16398 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16399 goto L4;
16400 }
16401 p0[0] = r11 * x[n0] + r12 * y[n0];
16402 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16403 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16404 goto L4;
16405 }
16406
16407
16408
16409
16410
16411
16412
16413
16414
16415 L4:
16416 ;
16417 }
16418 }
16419
16420
16421
16422
16423 t = fsizt / sf;
16424
16425
16426
16427
16428 p0[1] = wr + t * 3.;
16429
16430
16431
16432
16433
16434 if (annot) {
16435
16436
16437
16438 p0[0] = -wr;
16439 p0[1] = -wr - 50. / sf;
16440
16441
16442 p0[1] -= t * 2.;
16443
16444
16445
16446
16447
16448 }
16449
16450
16451
16452
16453
16454
16455
16456
16457
16458
16459
16460
16461
16462
16463
16464
16465
16466
16467 *ier = 0;
16468 return 0;
16469
16470
16471
16472 L11:
16473 *ier = 1;
16474 return 0;
16475
16476
16477
16478 L12:
16479 *ier = 2;
16480 return 0;
16481
16482
16483
16484
16485 *ier = 3;
16486 return 0;
16487 }
16488
16489 int trprnt_(int *n, double *x, double *y,
16490 double *z__, int *iflag, int *list, int *lptr,
16491 int *lend, int *lout)
16492 {
16493
16494
16495 static int nmax = 9999;
16496 static int nlmax = 58;
16497
16498
16499 int i__1;
16500
16501
16502 static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16503 400];
16504
16505
16506
16507
16508
16509
16510
16511
16512
16513
16514
16515
16516
16517
16518
16519
16520
16521
16522
16523
16524
16525
16526
16527
16528
16529
16530
16531
16532
16533
16534
16535
16536
16537
16538
16539
16540
16541
16542
16543
16544
16545
16546
16547
16548
16549
16550
16551
16552
16553
16554
16555
16556
16557
16558
16559
16560
16561
16562
16563
16564
16565
16566 --lend;
16567 --z__;
16568 --y;
16569 --x;
16570 --list;
16571 --lptr;
16572
16573
16574
16575
16576
16577
16578
16579
16580
16581
16582
16583
16584
16585
16586
16587
16588
16589
16590
16591
16592
16593
16594
16595
16596
16597
16598
16599 nn = *n;
16600 lun = *lout;
16601 if (lun < 0 || lun > 99) {
16602 lun = 6;
16603 }
16604
16605
16606
16607
16608 if (nn < 3 || nn > nmax) {
16609
16610
16611
16612
16613 return 0;
16614 }
16615
16616
16617
16618
16619 nl = 6;
16620 nb = 0;
16621 if (*iflag < 0) {
16622
16623
16624
16625
16626
16627 i__1 = nn;
16628 for (node = 1; node <= i__1; ++node) {
16629 lpl = lend[node];
16630 lp = lpl;
16631 k = 0;
16632
16633 L1:
16634 ++k;
16635 lp = lptr[lp];
16636 nd = list[lp];
16637 nabor[k - 1] = nd;
16638 if (lp != lpl) {
16639 goto L1;
16640 }
16641 if (nd <= 0) {
16642
16643
16644
16645
16646
16647 nabor[k - 1] = -nd;
16648 ++k;
16649 nabor[k - 1] = 0;
16650 ++nb;
16651 }
16652
16653
16654
16655 inc = (k - 1) / 14 + 2;
16656 nl += inc;
16657 if (nl > nlmax) {
16658
16659 nl = inc;
16660 }
16661
16662
16663
16664
16665 }
16666 } else if (*iflag > 0) {
16667
16668
16669
16670
16671 i__1 = nn;
16672 for (node = 1; node <= i__1; ++node) {
16673 lpl = lend[node];
16674 lp = lpl;
16675 k = 0;
16676
16677 L3:
16678 ++k;
16679 lp = lptr[lp];
16680 nd = list[lp];
16681 nabor[k - 1] = nd;
16682 if (lp != lpl) {
16683 goto L3;
16684 }
16685 if (nd <= 0) {
16686
16687
16688
16689 nabor[k - 1] = -nd;
16690 ++k;
16691 nabor[k - 1] = 0;
16692 ++nb;
16693 }
16694
16695
16696
16697 inc = (k - 1) / 8 + 2;
16698 nl += inc;
16699 if (nl > nlmax) {
16700
16701 nl = inc;
16702 }
16703
16704
16705
16706
16707
16708 }
16709 } else {
16710
16711
16712
16713
16714 i__1 = nn;
16715 for (node = 1; node <= i__1; ++node) {
16716 lpl = lend[node];
16717 lp = lpl;
16718 k = 0;
16719
16720 L5:
16721 ++k;
16722 lp = lptr[lp];
16723 nd = list[lp];
16724 nabor[k - 1] = nd;
16725 if (lp != lpl) {
16726 goto L5;
16727 }
16728 if (nd <= 0) {
16729
16730
16731
16732 nabor[k - 1] = -nd;
16733 ++k;
16734 nabor[k - 1] = 0;
16735 ++nb;
16736 }
16737
16738
16739
16740 inc = (k - 1) / 5 + 2;
16741 nl += inc;
16742 if (nl > nlmax) {
16743
16744 nl = inc;
16745 }
16746
16747
16748
16749
16750
16751 }
16752 }
16753
16754
16755
16756
16757 if (nb != 0) {
16758 na = nn * 3 - nb - 3;
16759 nt = (nn << 1) - nb - 2;
16760 } else {
16761 na = nn * 3 - 6;
16762 nt = (nn << 1) - 4;
16763 }
16764
16765 return 0;
16766
16767
16768
16769
16770
16771
16772
16773
16774
16775
16776
16777
16778
16779
16780
16781
16782
16783
16784
16785
16786 }
16787
16788 int vrplot_(int *lun, double *pltsiz, double *
16789 elat, double *elon, double *a, int *n, double *x,
16790 double *y, double *z__, int *nt, int *listc, int *
16791 lptr, int *lend, double *xc, double *yc, double *zc,
16792 char *, long int *numbr, int *ier, short)
16793 {
16794
16795
16796 static long int annot = TRUE_;
16797 static double fsizn = 10.;
16798 static double fsizt = 16.;
16799 static double tol = .5;
16800
16801
16802 int i__1;
16803 double d__1;
16804
16805
16806
16807
16808
16809
16810
16811 static double t;
16812 static int n0;
16813 static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16814 sf;
16815 static int ir, lp;
16816 static double ex, ey, ez, wr, tx, ty;
16817 static long int in1, in2;
16818 static int kv1, kv2, lpl;
16819 static double wrs;
16820 static int ipx1, ipx2, ipy1, ipy2, nseg;
16821 extern int drwarc_(int *, double *, double *,
16822 double *, int *);
16823
16824
16825
16826
16827
16828
16829
16830
16831
16832
16833
16834
16835
16836
16837
16838
16839
16840
16841
16842
16843
16844
16845
16846
16847
16848
16849
16850
16851
16852
16853
16854
16855
16856
16857
16858
16859
16860
16861
16862
16863
16864
16865
16866
16867
16868
16869
16870
16871
16872
16873
16874
16875
16876
16877
16878
16879
16880
16881
16882
16883
16884
16885
16886
16887
16888
16889
16890
16891
16892
16893
16894
16895
16896
16897
16898
16899
16900
16901
16902
16903
16904
16905
16906
16907
16908
16909
16910
16911
16912
16913
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 --lend;
16966 --z__;
16967 --y;
16968 --x;
16969 --zc;
16970 --yc;
16971 --xc;
16972 --listc;
16973 --lptr;
16974
16975
16976
16977
16978
16979
16980
16981
16982
16983
16984
16985
16986
16987
16988
16989
16990
16991
16992
16993
16994
16995
16996
16997
16998
16999
17000
17001
17002
17003
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 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
17033 nt != 2 * *n - 4) {
17034 goto L11;
17035 }
17036 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
17037 goto L12;
17038 }
17039
17040
17041
17042
17043 cf = atan(1.) / 45.;
17044 wr = sin(cf * *a);
17045 wrs = wr * wr;
17046
17047
17048
17049
17050
17051
17052
17053
17054
17055
17056 d__1 = *pltsiz * 36.;
17057 ir = i_dnnt(&d__1);
17058 ipx1 = 306 - ir;
17059 ipx2 = ir + 306;
17060 ipy1 = 396 - ir;
17061 ipy2 = ir + 396;
17062
17063
17064
17065
17066
17067
17068
17069
17070
17071
17072
17073
17074
17075 d__1 = (double) ir * .88;
17076 ir = i_dnnt(&d__1);
17077 ipx1 = 306 - ir;
17078 ipx2 = ir + 306;
17079 ipy1 = 396 - ir;
17080 ipy2 = ir + 396;
17081
17082
17083
17084
17085 t = 2.;
17086
17087
17088
17089
17090
17091
17092
17093
17094
17095
17096 sf = (double) ir / wr;
17097 tx = ipx1 + sf * wr;
17098 ty = ipy1 + sf * wr;
17099
17100
17101
17102
17103
17104
17105
17106
17107 t = 1. / sf;
17108
17109
17110
17111
17112
17113
17114
17115
17116
17117
17118
17119
17120
17121
17122
17123
17124
17125
17126
17127
17128
17129
17130
17131 t = cf * *elon;
17132 ct = cos(cf * *elat);
17133 ex = ct * cos(t);
17134 ey = ct * sin(t);
17135 ez = sin(cf * *elat);
17136 if (ct != 0.) {
17137 r11 = -ey / ct;
17138 r12 = ex / ct;
17139 } else {
17140 r11 = 0.;
17141 r12 = 1.;
17142 }
17143 r21 = -ez * r12;
17144 r22 = ez * r11;
17145 r23 = ct;
17146
17147
17148
17149
17150 i__1 = *n;
17151 for (n0 = 1; n0 <= i__1; ++n0) {
17152 lpl = lend[n0];
17153
17154
17155
17156
17157 kv2 = listc[lpl];
17158 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17159 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17160 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17161
17162
17163
17164 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17165
17166
17167
17168
17169 lp = lpl;
17170 L1:
17171 lp = lptr[lp];
17172 kv1 = kv2;
17173 p1[0] = p2[0];
17174 p1[1] = p2[1];
17175 p1[2] = p2[2];
17176 in1 = in2;
17177 kv2 = listc[lp];
17178
17179
17180
17181 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17182 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17183 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17184 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17185
17186
17187
17188
17189
17190 if (! in1 || (in2 && kv2 <= kv1)) {
17191 goto L2;
17192 }
17193 if (p2[2] < 0.) {
17194
17195
17196
17197
17198
17199 p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17200 p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17201 t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17202 p2[0] /= t;
17203 p2[1] /= t;
17204 }
17205
17206
17207
17208
17209 if (p2[2] < 0.) {
17210 p2[2] = 0.f;
17211 }
17212 d__1 = tol / sf;
17213 drwarc_(lun, p1, p2, &d__1, &nseg);
17214
17215
17216
17217 L2:
17218 if (lp != lpl) {
17219 goto L1;
17220 }
17221
17222 }
17223
17224
17225
17226
17227
17228
17229
17230 if (*numbr) {
17231
17232
17233
17234
17235
17236 t = fsizn / sf;
17237
17238
17239
17240
17241
17242
17243
17244 i__1 = *n;
17245 for (n0 = 1; n0 <= i__1; ++n0) {
17246 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17247 goto L4;
17248 }
17249 x0 = r11 * x[n0] + r12 * y[n0];
17250 y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17251 if (x0 * x0 + y0 * y0 > wrs) {
17252 goto L4;
17253 }
17254
17255
17256
17257
17258
17259
17260
17261
17262 L4:
17263 ;
17264 }
17265 }
17266
17267
17268
17269
17270 t = fsizt / sf;
17271
17272
17273
17274
17275 y0 = wr + t * 3.;
17276
17277
17278
17279
17280
17281 if (annot) {
17282
17283
17284
17285 x0 = -wr;
17286 y0 = -wr - 50. / sf;
17287
17288
17289 y0 -= t * 2.;
17290
17291
17292
17293
17294
17295 }
17296
17297
17298
17299
17300
17301
17302
17303
17304
17305
17306
17307
17308
17309
17310
17311
17312
17313
17314 *ier = 0;
17315 return 0;
17316
17317
17318
17319 L11:
17320 *ier = 1;
17321 return 0;
17322
17323
17324
17325 L12:
17326 *ier = 2;
17327 return 0;
17328
17329
17330
17331
17332 *ier = 3;
17333 return 0;
17334 }
17335
17336 int random_(int *ix, int *iy, int *iz,
17337 double *rannum)
17338 {
17339 static double x;
17340
17341
17342
17343
17344
17345
17346
17347
17348
17349
17350
17351
17352
17353
17354
17355 *ix = *ix * 171 % 30269;
17356 *iy = *iy * 172 % 30307;
17357 *iz = *iz * 170 % 30323;
17358 x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17359 double) (*iz) / 30323.;
17360 *rannum = x - (int) x;
17361 return 0;
17362 }
17363
17364 #undef TRUE_
17365 #undef FALSE_
17366 #undef abs
17367
17368
17369
17370
17371
17372
17373
17374
17375
17376 EMData* Util::mult_scalar(EMData* img, float scalar)
17377 {
17378 ENTERFUNC;
17379
17380 if (!img) {
17381 throw NullPointerException("NULL input image");
17382 }
17383
17384
17385 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17386 size_t size = (size_t)nx*ny*nz;
17387 EMData * img2 = img->copy_head();
17388 float *img_ptr =img->get_data();
17389 float *img2_ptr = img2->get_data();
17390 for (size_t i=0;i<size;++i)img2_ptr[i] = img_ptr[i]*scalar;
17391 img2->update();
17392
17393 if(img->is_complex()) {
17394 img2->set_complex(true);
17395 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17396 }
17397 EXITFUNC;
17398 return img2;
17399 }
17400
17401 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17402 {
17403 ENTERFUNC;
17404
17405 if (!img) {
17406 throw NullPointerException("NULL input image");
17407 }
17408
17409
17410 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17411 size_t size = (size_t)nx*ny*nz;
17412 EMData * img2 = img->copy_head();
17413 float *img_ptr =img->get_data();
17414 float *img2_ptr = img2->get_data();
17415 float *img1_ptr = img1->get_data();
17416 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17417 img2->update();
17418 if(img->is_complex()) {
17419 img2->set_complex(true);
17420 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17421 }
17422
17423 EXITFUNC;
17424 return img2;
17425 }
17426
17427 EMData* Util::addn_img(EMData* img, EMData* img1)
17428 {
17429 ENTERFUNC;
17430
17431 if (!img) {
17432 throw NullPointerException("NULL input image");
17433 }
17434
17435
17436 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17437 size_t size = (size_t)nx*ny*nz;
17438 EMData * img2 = img->copy_head();
17439 float *img_ptr =img->get_data();
17440 float *img2_ptr = img2->get_data();
17441 float *img1_ptr = img1->get_data();
17442 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17443 img2->update();
17444 if(img->is_complex()) {
17445 img2->set_complex(true);
17446 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17447 }
17448
17449 EXITFUNC;
17450 return img2;
17451 }
17452
17453 EMData* Util::subn_img(EMData* img, EMData* img1)
17454 {
17455 ENTERFUNC;
17456
17457 if (!img) {
17458 throw NullPointerException("NULL input image");
17459 }
17460
17461
17462 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17463 size_t size = (size_t)nx*ny*nz;
17464 EMData * img2 = img->copy_head();
17465 float *img_ptr =img->get_data();
17466 float *img2_ptr = img2->get_data();
17467 float *img1_ptr = img1->get_data();
17468 for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17469 img2->update();
17470 if(img->is_complex()) {
17471 img2->set_complex(true);
17472 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17473 }
17474
17475 EXITFUNC;
17476 return img2;
17477 }
17478
17479 EMData* Util::muln_img(EMData* img, EMData* img1)
17480 {
17481 ENTERFUNC;
17482
17483 if (!img) {
17484 throw NullPointerException("NULL input image");
17485 }
17486
17487
17488 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17489 size_t size = (size_t)nx*ny*nz;
17490 EMData * img2 = img->copy_head();
17491 float *img_ptr =img->get_data();
17492 float *img2_ptr = img2->get_data();
17493 float *img1_ptr = img1->get_data();
17494 if(img->is_complex()) {
17495 for (size_t i=0; i<size; i+=2) {
17496 img2_ptr[i] = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17497 img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17498 }
17499 img2->set_complex(true);
17500 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17501 } else {
17502 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17503 img2->update();
17504 }
17505
17506 EXITFUNC;
17507 return img2;
17508 }
17509
17510 EMData* Util::divn_img(EMData* img, EMData* img1)
17511 {
17512 ENTERFUNC;
17513
17514 if (!img) {
17515 throw NullPointerException("NULL input image");
17516 }
17517
17518
17519 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17520 size_t size = (size_t)nx*ny*nz;
17521 EMData * img2 = img->copy_head();
17522 float *img_ptr =img->get_data();
17523 float *img2_ptr = img2->get_data();
17524 float *img1_ptr = img1->get_data();
17525 if(img->is_complex()) {
17526 float sq2;
17527 for (size_t i=0; i<size; i+=2) {
17528 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17529 img2_ptr[i] = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17530 img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17531 }
17532 img2->set_complex(true);
17533 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17534 } else {
17535 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17536 img2->update();
17537 }
17538
17539 EXITFUNC;
17540 return img2;
17541 }
17542
17543 EMData* Util::divn_filter(EMData* img, EMData* img1)
17544 {
17545 ENTERFUNC;
17546
17547 if (!img) {
17548 throw NullPointerException("NULL input image");
17549 }
17550
17551
17552 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17553 size_t size = (size_t)nx*ny*nz;
17554 EMData * img2 = img->copy_head();
17555 float *img_ptr =img->get_data();
17556 float *img1_ptr = img1->get_data();
17557 float *img2_ptr = img2->get_data();
17558 if(img->is_complex()) {
17559 for (size_t i=0; i<size; i+=2) {
17560 if(img1_ptr[i] > 1.e-10f) {
17561 img2_ptr[i] = img_ptr[i] /img1_ptr[i];
17562 img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17563 } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17564 }
17565 } else throw ImageFormatException("Only Fourier image allowed");
17566
17567 img->update();
17568
17569 EXITFUNC;
17570 return img2;
17571 }
17572
17573 void Util::mul_scalar(EMData* img, float scalar)
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 float *img_ptr =img->get_data();
17585 for (size_t i=0;i<size;++i) img_ptr[i] *= scalar;
17586 img->update();
17587
17588 EXITFUNC;
17589 }
17590
17591 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17592 {
17593 ENTERFUNC;
17594
17595 if (!img) {
17596 throw NullPointerException("NULL input image");
17597 }
17598
17599
17600 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17601 size_t size = (size_t)nx*ny*nz;
17602 float *img_ptr =img->get_data();
17603 float *img1_ptr = img1->get_data();
17604 for (size_t i=0;i<size;++i)img_ptr[i] += img1_ptr[i]*scalar;
17605 img1->update();
17606
17607 EXITFUNC;
17608 }
17609
17610 void Util::add_img(EMData* img, EMData* img1)
17611 {
17612 ENTERFUNC;
17613
17614 if (!img) {
17615 throw NullPointerException("NULL input image");
17616 }
17617
17618
17619 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17620 size_t size = (size_t)nx*ny*nz;
17621 float *img_ptr = img->get_data();
17622 float *img1_ptr = img1->get_data();
17623 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i];
17624 img->update();
17625
17626 EXITFUNC;
17627 }
17628
17629 void Util::add_img_abs(EMData* img, EMData* img1)
17630 {
17631 ENTERFUNC;
17632
17633 if (!img) {
17634 throw NullPointerException("NULL input image");
17635 }
17636
17637
17638 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17639 size_t size = (size_t)nx*ny*nz;
17640 float *img_ptr = img->get_data();
17641 float *img1_ptr = img1->get_data();
17642 for (size_t i=0;i<size;++i) img_ptr[i] += abs(img1_ptr[i]);
17643 img->update();
17644
17645 EXITFUNC;
17646 }
17647
17648 void Util::add_img2(EMData* img, EMData* img1)
17649 {
17650 ENTERFUNC;
17651
17652 if (!img) {
17653 throw NullPointerException("NULL input image");
17654 }
17655
17656
17657 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17658 size_t size = (size_t)nx*ny*nz;
17659 float *img_ptr = img->get_data();
17660 float *img1_ptr = img1->get_data();
17661 if(img->is_complex()) {
17662 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] ;
17663 } else {
17664 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17665 }
17666 img->update();
17667
17668 EXITFUNC;
17669 }
17670
17671 void Util::sub_img(EMData* img, EMData* img1)
17672 {
17673 ENTERFUNC;
17674
17675 if (!img) {
17676 throw NullPointerException("NULL input image");
17677 }
17678
17679
17680 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17681 size_t size = (size_t)nx*ny*nz;
17682 float *img_ptr = img->get_data();
17683 float *img1_ptr = img1->get_data();
17684 for (size_t i=0;i<size;++i) img_ptr[i] -= img1_ptr[i];
17685 img->update();
17686
17687 EXITFUNC;
17688 }
17689
17690 void Util::mul_img(EMData* img, EMData* img1)
17691 {
17692 ENTERFUNC;
17693
17694 if (!img) {
17695 throw NullPointerException("NULL input image");
17696 }
17697
17698
17699 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17700 size_t size = (size_t)nx*ny*nz;
17701 float *img_ptr = img->get_data();
17702 float *img1_ptr = img1->get_data();
17703 if(img->is_complex()) {
17704 for (size_t i=0; i<size; i+=2) {
17705 float tmp = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17706 img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17707 img_ptr[i] = tmp;
17708
17709 }
17710 } else {
17711 for (size_t i=0;i<size;++i) img_ptr[i] *= img1_ptr[i];
17712 }
17713 img->update();
17714
17715 EXITFUNC;
17716 }
17717
17718 void Util::div_img(EMData* img, EMData* img1)
17719 {
17720 ENTERFUNC;
17721
17722 if (!img) {
17723 throw NullPointerException("NULL input image");
17724 }
17725
17726
17727 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17728 size_t size = (size_t)nx*ny*nz;
17729 float *img_ptr = img->get_data();
17730 float *img1_ptr = img1->get_data();
17731 if(img->is_complex()) {
17732 float sq2;
17733 for (size_t i=0; i<size; i+=2) {
17734 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17735 float tmp = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17736 img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17737 img_ptr[i] = tmp;
17738 }
17739 } else {
17740 for (size_t i=0; i<size; ++i) img_ptr[i] /= img1_ptr[i];
17741 }
17742 img->update();
17743
17744 EXITFUNC;
17745 }
17746
17747 void Util::div_filter(EMData* img, EMData* img1)
17748 {
17749 ENTERFUNC;
17750
17751 if (!img) {
17752 throw NullPointerException("NULL input image");
17753 }
17754
17755
17756 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17757 size_t size = (size_t)nx*ny*nz;
17758 float *img_ptr = img->get_data();
17759 float *img1_ptr = img1->get_data();
17760 if(img->is_complex()) {
17761 for (size_t i=0; i<size; i+=2) {
17762 if(img1_ptr[i] > 1.e-10f) {
17763 img_ptr[i] /= img1_ptr[i];
17764 img_ptr[i+1] /= img1_ptr[i];
17765 } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17766 }
17767 } else throw ImageFormatException("Only Fourier image allowed");
17768
17769 img->update();
17770
17771 EXITFUNC;
17772 }
17773
17774 #define img_ptr(i,j,k) img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*(size_t)nxo]
17775
17776 EMData* Util::pack_complex_to_real(EMData* img)
17777 {
17778 ENTERFUNC;
17779
17780 if (!img) {
17781 throw NullPointerException("NULL input image");
17782 }
17783
17784
17785
17786 int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17787 int nx = nxo - 2 + img->is_fftodd();
17788 int lsd2 = (nx + 2 - nx%2) / 2;
17789 int nyt, nzt;
17790 int nx2 = nx/2;
17791 int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17792 int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17793 int nx2p = nx2+nx%2;
17794 int ny2p = ny2+ny%2;
17795 int nz2p = nz2+nz%2;
17796 EMData& power = *(new EMData());
17797 power.set_size(nx, ny, nz);
17798 power.set_array_offsets(-nx2,-ny2,-nz2);
17799
17800 float *img_ptr = img->get_data();
17801 for (int iz = 1; iz <= nz; iz++) {
17802 int jz=iz-1;
17803 if(jz>=nz2p) jz=jz-nzt;
17804 for (int iy = 1; iy <= ny; iy++) {
17805 int jy=iy-1;
17806 if(jy>=ny2p) jy=jy-nyt;
17807 for (int ix = 1; ix <= lsd2; ix++) {
17808 int jx=ix-1;
17809 if(jx>=nx2p) jx=jx-nx;
17810 power(jx,jy,jz) = img_ptr(ix,iy,iz);
17811 }
17812 }
17813 }
17814
17815 int nzb, nze, nyb, nye, nxb, nxe;
17816 nxb =-nx2+(nx+1)%2;
17817 nxe = nx2-(nx+1)%2;
17818 if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17819 if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17820 for (int iz = nzb; iz <= nze; iz++) {
17821 for (int iy = nyb; iy <= nye; iy++) {
17822 for (int ix = 1; ix <= nxe; ix++) {
17823 power(-ix,-iy,-iz) = power(ix,iy,iz);
17824 }
17825 }
17826 }
17827 if(ny2 != 0) {
17828 if(nz2 != 0) {
17829 if(nz%2 == 0) {
17830 for (int iy = nyb; iy <= nye; iy++) {
17831 for (int ix = nxb; ix <= -1; ix++) {
17832 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17833 }
17834 }
17835 if(ny%2 == 0) {
17836 for (int ix = nxb; ix <= -1; ix++) {
17837 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17838 }
17839 }
17840 }
17841 }
17842 if(ny%2 == 0) {
17843 for (int iz = nzb; iz <= nze; iz++) {
17844 for (int ix = nxb; ix <= -1; ix++) {
17845 power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17846 }
17847 }
17848 }
17849
17850 }
17851 power.update();
17852 power.set_array_offsets(0,0,0);
17853 return &power;
17854 }
17855 #undef img_ptr
17856
17857 float Util::ang_n(float peakp, string mode, int maxrin)
17858 {
17859 if (mode == "f" || mode == "F")
17860 return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17861 else
17862 return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17863 }
17864
17865
17866 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17867 {
17868 float* data = ring->get_data();
17869 float av=0.0;
17870 float sq=0.0;
17871 float nn=0.0;
17872 int nring = numr.size()/3;
17873 for( int i=0; i < nring; ++i )
17874 {
17875 int numr3i = numr[3*i+2];
17876 int numr2i = numr[3*i+1]-1;
17877 float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17878 for( int j=0; j < numr3i; ++j )
17879 {
17880 int jc = numr2i+j;
17881 av += data[jc] * w;
17882 sq += data[jc] * data[jc] * w;
17883 nn += w;
17884 }
17885 }
17886
17887 float avg = av/nn;
17888 float sgm = sqrt( (sq-av*av/nn)/nn );
17889 size_t n = (size_t)ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17890 for( size_t i=0; i < n; ++i )
17891 {
17892 data[i] -= avg;
17893 data[i] /= sgm;
17894 }
17895
17896 ring->update();
17897 }
17898
17899 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17900 float xrng, float yrng, float step, string mode,
17901 vector<int>numr, float cnx, float cny) {
17902
17903
17904
17905
17906
17907
17908
17909
17910
17911
17912
17913
17914 size_t crefim_len = crefim.size();
17915
17916 int ky = int(2*yrng/step+0.5)/2;
17917 int kx = int(2*xrng/step+0.5)/2;
17918 int iref, nref=0, mirror=0;
17919 float iy, ix, sx=0, sy=0;
17920 float peak = -1.0E23f;
17921 float ang=0.0f;
17922 for (int i = -ky; i <= ky; i++) {
17923 iy = i * step ;
17924 for (int j = -kx; j <= kx; j++) {
17925 ix = j*step ;
17926 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17927
17928 Normalize_ring( cimage, numr );
17929
17930 Frngs(cimage, numr);
17931
17932
17933 for ( iref = 0; iref < (int)crefim_len; iref++) {
17934 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17935 double qn = retvals["qn"];
17936 double qm = retvals["qm"];
17937 if(qn >= peak || qm >= peak) {
17938 sx = -ix;
17939 sy = -iy;
17940 nref = iref;
17941 if (qn >= qm) {
17942 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17943 peak = static_cast<float>(qn);
17944 mirror = 0;
17945 } else {
17946 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17947 peak = static_cast<float>(qm);
17948 mirror = 1;
17949 }
17950 }
17951 } delete cimage; cimage = 0;
17952 }
17953 }
17954 float co, so, sxs, sys;
17955 co = static_cast<float>( cos(ang*pi/180.0) );
17956 so = static_cast<float>( -sin(ang*pi/180.0) );
17957 sxs = sx*co - sy*so;
17958 sys = sx*so + sy*co;
17959 vector<float> res;
17960 res.push_back(ang);
17961 res.push_back(sxs);
17962 res.push_back(sys);
17963 res.push_back(static_cast<float>(mirror));
17964 res.push_back(static_cast<float>(nref));
17965 res.push_back(peak);
17966 return res;
17967 }
17968
17969 vector<float> Util::multiref_polar_ali_2d_peaklist(EMData* image, const vector< EMData* >& crefim,
17970 float xrng, float yrng, float step, string mode,
17971 vector<int>numr, float cnx, float cny) {
17972
17973 size_t crefim_len = crefim.size();
17974
17975 int ky = int(2*yrng/step+0.5)/2;
17976 int kx = int(2*xrng/step+0.5)/2;
17977 float iy, ix;
17978 vector<float> peak(crefim_len*5, -1.0e23f);
17979 for (int i = -ky; i <= ky; i++) {
17980 iy = i * step ;
17981 for (int j = -kx; j <= kx; j++) {
17982 ix = j*step ;
17983 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17984 Normalize_ring( cimage, numr );
17985 Frngs(cimage, numr);
17986 for (int iref = 0; iref < (int)crefim_len; iref++) {
17987 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17988 double qn = retvals["qn"];
17989 double qm = retvals["qm"];
17990 if(qn >= peak[iref*5] || qm >= peak[iref*5]) {
17991 if (qn >= qm) {
17992 peak[iref*5] = static_cast<float>(qn);
17993 peak[iref*5+1] = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17994 peak[iref*5+2] = -ix;
17995 peak[iref*5+3] = -iy;
17996 peak[iref*5+4] = 0;
17997 } else {
17998 peak[iref*5] = static_cast<float>(qm);
17999 peak[iref*5+1] = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18000 peak[iref*5+2] = -ix;
18001 peak[iref*5+3] = -iy;
18002 peak[iref*5+4] = 1;
18003 }
18004 }
18005 } delete cimage; cimage = 0;
18006 }
18007 }
18008 for (int iref = 0; iref < (int)crefim_len; iref++) {
18009 float ang = peak[iref*5+1];
18010 float sx = peak[iref*5+2];
18011 float sy = peak[iref*5+3];
18012 float co = cos(ang*pi/180.0);
18013 float so = -sin(ang*pi/180.0);
18014 float sxs = sx*co - sy*so;
18015 float sys = sx*so + sy*co;
18016 peak[iref*5+2] = sxs;
18017 peak[iref*5+3] = sys;
18018 }
18019 return peak;
18020 }
18021
18022 struct peak_table {
18023 float value;
18024 int index;
18025 bool operator<(const peak_table& b) const { return value > b.value; }
18026 };
18027
18028 vector<int> Util::assign_groups(const vector<float>& d, int nref, int nima) {
18029
18030 int kt = nref;
18031 unsigned int maxasi = nima/nref;
18032 vector< vector<int> > id_list;
18033 id_list.resize(nref);
18034 int group, ima;
18035
18036 peak_table* dd = new peak_table[nref*nima];
18037 for (int i=0; i<nref*nima; i++) {
18038 dd[i].value = d[i];
18039 dd[i].index = i;
18040 }
18041 sort(dd, dd+nref*nima);
18042 int begin = 0;
18043
18044 bool* del_row = new bool[nref];
18045 for (int i=0; i<nref; i++) del_row[i] = false;
18046 bool* del_column = new bool[nima];
18047 for (int i=0; i<nima; i++) del_column[i] = false;
18048 while (kt > 0) {
18049 bool flag = true;
18050 while (flag) {
18051 int l = dd[begin].index;
18052 group = l/nima;
18053 ima = l%nima;
18054 if (del_column[ima] || del_row[group]) begin++;
18055 else flag = false;
18056 }
18057
18058 id_list[group].push_back(ima);
18059 if (kt > 1) {
18060 if (id_list[group].size() < maxasi) group = -1;
18061 else kt -= 1;
18062 } else {
18063 if (id_list[group].size() < maxasi+nima%nref) group = -1;
18064 else kt -= 1;
18065 }
18066 del_column[ima] = true;
18067 if (group != -1) {
18068 del_row[group] = true;
18069 }
18070 }
18071
18072 vector<int> id_list_1;
18073 for (int iref=0; iref<nref; iref++)
18074 for (unsigned int im=0; im<maxasi; im++)
18075 id_list_1.push_back(id_list[iref][im]);
18076 for (unsigned int im=maxasi; im<maxasi+nima%nref; im++)
18077 id_list_1.push_back(id_list[group][im]);
18078 id_list_1.push_back(group);
18079
18080 delete[] del_row;
18081 delete[] del_column;
18082 delete[] dd;
18083 return id_list_1;
18084 }
18085
18086
18087 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
18088 float xrng, float yrng, float step, string mode,
18089 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
18090
18091
18092
18093
18094
18095
18096
18097
18098
18099
18100
18101
18102 size_t crefim_len = crefim.size();
18103
18104 int ky = int(2*yrng/step+0.5)/2;
18105 int kx = int(2*xrng/step+0.5)/2;
18106 int iref, nref=0, mirror=0;
18107 float iy, ix, sx=0, sy=0;
18108 float peak = -1.0E23f;
18109 float ang=0.0f;
18110 for (int i = -ky; i <= ky; i++) {
18111 iy = i * step ;
18112 for (int j = -kx; j <= kx; j++) {
18113 ix = j*step ;
18114 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18115
18116 Normalize_ring( cimage, numr );
18117
18118 Frngs(cimage, numr);
18119
18120
18121 for ( iref = 0; iref < (int)crefim_len; iref++) {
18122 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
18123 double qn = retvals["qn"];
18124 double qm = retvals["qm"];
18125 if(qn >= peak || qm >= peak) {
18126 sx = -ix;
18127 sy = -iy;
18128 nref = iref;
18129 if (qn >= qm) {
18130 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18131 peak = static_cast<float>(qn);
18132 mirror = 0;
18133 } else {
18134 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18135 peak = static_cast<float>(qm);
18136 mirror = 1;
18137 }
18138 }
18139 } delete cimage; cimage = 0;
18140 }
18141 }
18142 float co, so, sxs, sys;
18143 co = static_cast<float>( cos(ang*pi/180.0) );
18144 so = static_cast<float>( -sin(ang*pi/180.0) );
18145 sxs = sx*co - sy*so;
18146 sys = sx*so + sy*co;
18147 vector<float> res;
18148 res.push_back(ang);
18149 res.push_back(sxs);
18150 res.push_back(sys);
18151 res.push_back(static_cast<float>(mirror));
18152 res.push_back(static_cast<float>(nref));
18153 res.push_back(peak);
18154 return res;
18155 }
18156
18157 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
18158 float xrng, float yrng, float step, string mode,
18159 vector< int >numr, float cnx, float cny) {
18160
18161
18162
18163
18164
18165
18166
18167
18168
18169
18170
18171 size_t crefim_len = crefim.size();
18172
18173 int ky = int(2*yrng/step+0.5)/2;
18174 int kx = int(2*xrng/step+0.5)/2;
18175 int iref, nref=0;
18176 float iy, ix, sx=0, sy=0;
18177 float peak = -1.0E23f;
18178 float ang=0.0f;
18179 for (int i = -ky; i <= ky; i++) {
18180 iy = i * step ;
18181 for (int j = -kx; j <= kx; j++) {
18182 ix = j*step ;
18183 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18184 Frngs(cimage, numr);
18185
18186
18187 for ( iref = 0; iref < (int)crefim_len; iref++) {
18188 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
18189 double qn = retvals["qn"];
18190 if(qn >= peak) {
18191 sx = -ix;
18192 sy = -iy;
18193 nref = iref;
18194 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18195 peak = static_cast<float>(qn);
18196 }
18197 } delete cimage; cimage = 0;
18198 }
18199 }
18200 float co, so, sxs, sys;
18201 co = static_cast<float>( cos(ang*pi/180.0) );
18202 so = static_cast<float>( -sin(ang*pi/180.0) );
18203 sxs = sx*co - sy*so;
18204 sys = sx*so + sy*co;
18205 vector<float> res;
18206 res.push_back(ang);
18207 res.push_back(sxs);
18208 res.push_back(sys);
18209 res.push_back(static_cast<float>(nref));
18210 res.push_back(peak);
18211 return res;
18212 }
18213
18214 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
18215 float xrng, float yrng, float step, float ant, string mode,
18216 vector<int>numr, float cnx, float cny) {
18217
18218
18219
18220
18221
18222
18223
18224
18225
18226
18227
18228 size_t crefim_len = crefim.size();
18229 const float qv = static_cast<float>( pi/180.0 );
18230
18231 Transform * t = image->get_attr("xform.projection");
18232 Dict d = t->get_params("spider");
18233 if(t) {delete t; t=0;}
18234 float phi = d["phi"];
18235 float theta = d["theta"];
18236 int ky = int(2*yrng/step+0.5)/2;
18237 int kx = int(2*xrng/step+0.5)/2;
18238 int iref, nref=0, mirror=0;
18239 float iy, ix, sx=0, sy=0;
18240 float peak = -1.0E23f;
18241 float ang=0.0f;
18242 float imn1 = sin(theta*qv)*cos(phi*qv);
18243 float imn2 = sin(theta*qv)*sin(phi*qv);
18244 float imn3 = cos(theta*qv);
18245 vector<float> n1(crefim_len);
18246 vector<float> n2(crefim_len);
18247 vector<float> n3(crefim_len);
18248 for ( iref = 0; iref < (int)crefim_len; iref++) {
18249 n1[iref] = crefim[iref]->get_attr("n1");
18250 n2[iref] = crefim[iref]->get_attr("n2");
18251 n3[iref] = crefim[iref]->get_attr("n3");
18252 }
18253 for (int i = -ky; i <= ky; i++) {
18254 iy = i * step ;
18255 for (int j = -kx; j <= kx; j++) {
18256 ix = j*step;
18257 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18258
18259 Normalize_ring( cimage, numr );
18260
18261 Frngs(cimage, numr);
18262
18263
18264 for ( iref = 0; iref < (int)crefim_len; iref++) {
18265 if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18266 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18267 double qn = retvals["qn"];
18268 double qm = retvals["qm"];
18269 if(qn >= peak || qm >= peak) {
18270 sx = -ix;
18271 sy = -iy;
18272 nref = iref;
18273 if (qn >= qm) {
18274 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18275 peak = static_cast<float>( qn );
18276 mirror = 0;
18277 } else {
18278 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18279 peak = static_cast<float>( qm );
18280 mirror = 1;
18281 }
18282 }
18283 }
18284 } delete cimage; cimage = 0;
18285 }
18286 }
18287 float co, so, sxs, sys;
18288 if(peak == -1.0E23) {
18289 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18290 nref = -1;
18291 } else {
18292 co = cos(ang*qv);
18293 so = -sin(ang*qv);
18294 sxs = sx*co - sy*so;
18295 sys = sx*so + sy*co;
18296 }
18297 vector<float> res;
18298 res.push_back(ang);
18299 res.push_back(sxs);
18300 res.push_back(sys);
18301 res.push_back(static_cast<float>(mirror));
18302 res.push_back(static_cast<float>(nref));
18303 res.push_back(peak);
18304 return res;
18305 }
18306
18307 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18308 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18309 vector<int>numr, float cnx, float cny) {
18310
18311
18312
18313
18314
18315
18316
18317
18318
18319
18320
18321 size_t crefim_len = crefim.size();
18322 const float qv = static_cast<float>(pi/180.0);
18323
18324 Transform* t = image->get_attr("xform.projection");
18325 Dict d = t->get_params("spider");
18326 if(t) {delete t; t=0;}
18327 float phi = d["phi"];
18328 float theta = d["theta"];
18329 float psi = d["psi"];
18330 int ky = int(2*yrng/step+0.5)/2;
18331 int kx = int(2*xrng/step+0.5)/2;
18332 int iref, nref = 0, mirror = 0;
18333 float iy, ix, sx = 0, sy = 0;
18334 float peak = -1.0E23f;
18335 float ang = 0.0f;
18336 float imn1 = sin(theta*qv)*cos(phi*qv);
18337 float imn2 = sin(theta*qv)*sin(phi*qv);
18338 float imn3 = cos(theta*qv);
18339 vector<float> n1(crefim_len);
18340 vector<float> n2(crefim_len);
18341 vector<float> n3(crefim_len);
18342 for (iref = 0; iref < (int)crefim_len; iref++) {
18343 n1[iref] = crefim[iref]->get_attr("n1");
18344 n2[iref] = crefim[iref]->get_attr("n2");
18345 n3[iref] = crefim[iref]->get_attr("n3");
18346 }
18347 bool nomirror = (theta<90.0) || ((theta==90.0) && (psi<psi_max));
18348 if (!nomirror) {
18349 phi = fmod(phi+540.0f, 360.0f);
18350 theta = 180-theta;
18351 psi = fmod(540.0f-psi, 360.0f);
18352 }
18353 for (int i = -ky; i <= ky; i++) {
18354 iy = i * step ;
18355 for (int j = -kx; j <= kx; j++) {
18356 ix = j*step;
18357 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18358
18359 Normalize_ring(cimage, numr);
18360
18361 Frngs(cimage, numr);
18362
18363
18364 for (iref = 0; iref < (int)crefim_len; iref++) {
18365 if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18366 if (nomirror) {
18367 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 0);
18368 double qn = retvals["qn"];
18369 if (qn >= peak) {
18370 sx = -ix;
18371 sy = -iy;
18372 nref = iref;
18373 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18374 peak = static_cast<float>(qn);
18375 mirror = 0;
18376 }
18377 } else {
18378 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 1);
18379 double qn = retvals["qn"];
18380 if (qn >= peak) {
18381 sx = -ix;
18382 sy = -iy;
18383 nref = iref;
18384 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18385 peak = static_cast<float>(qn);
18386 mirror = 1;
18387 }
18388 }
18389 }
18390 } delete cimage; cimage = 0;
18391 }
18392 }
18393 float co, so, sxs, sys;
18394 if(peak == -1.0E23) {
18395 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18396 nref = -1;
18397 } else {
18398 co = cos(ang*qv);
18399 so = -sin(ang*qv);
18400 sxs = sx*co - sy*so;
18401 sys = sx*so + sy*co;
18402 }
18403 vector<float> res;
18404 res.push_back(ang);
18405 res.push_back(sxs);
18406 res.push_back(sys);
18407 res.push_back(static_cast<float>(mirror));
18408 res.push_back(static_cast<float>(nref));
18409 res.push_back(peak);
18410 return res;
18411 }
18412
18413
18414 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18415 float xrng, float yrng, float step, float psi_max, string mode,
18416 vector<int>numr, float cnx, float cny, int ynumber) {
18417
18418 size_t crefim_len = crefim.size();
18419
18420 int iref, nref=0, mirror=0;
18421 float iy, ix, sx=0, sy=0;
18422 float peak = -1.0E23f;
18423 float ang=0.0f;
18424 int kx = int(2*xrng/step+0.5)/2;
18425
18426 if(ynumber==-1) {
18427 int ky = int(2*yrng/step+0.5)/2;
18428 for (int i = -ky; i <= ky; i++) {
18429 iy = i * step ;
18430 for (int j = -kx; j <= kx; j++) {
18431 ix = j*step ;
18432 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18433
18434 Normalize_ring( cimage, numr );
18435
18436 Frngs(cimage, numr);
18437
18438
18439 for ( iref = 0; iref < (int)crefim_len; iref++) {
18440 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18441 double qn = retvals["qn"];
18442 double qm = retvals["qm"];
18443 if(qn >= peak || qm >= peak) {
18444 sx = -ix;
18445 sy = -iy;
18446 nref = iref;
18447 if (qn >= qm) {
18448 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18449 peak = static_cast<float>(qn);
18450 mirror = 0;
18451 } else {
18452 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18453 peak = static_cast<float>(qm);
18454 mirror = 1;
18455 }
18456 }
18457 }
18458 delete cimage; cimage = 0;
18459 }
18460 }
18461 }
18462
18463 else if(ynumber==0) {
18464 sy = 0.0f;
18465 for (int j = -kx; j <= kx; j++) {
18466 ix = j*step ;
18467 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18468
18469 Normalize_ring( cimage, numr );
18470
18471 Frngs(cimage, numr);
18472
18473
18474 for ( iref = 0; iref < (int)crefim_len; iref++) {
18475 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18476 double qn = retvals["qn"];
18477 double qm = retvals["qm"];
18478 if(qn >= peak || qm >= peak) {
18479 sx = -ix;
18480 nref = iref;
18481 if (qn >= qm) {
18482 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18483 peak = static_cast<float>(qn);
18484 mirror = 0;
18485 } else {
18486 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18487 peak = static_cast<float>(qm);
18488 mirror = 1;
18489 }
18490 }
18491 }
18492 delete cimage; cimage = 0;
18493 }
18494 } else {
18495 int ky = int(ynumber/2);
18496 float stepy=2*yrng/ynumber;
18497
18498 for (int i = -ky+1; i <= ky; i++) {
18499 iy = i * stepy ;
18500 for (int j = -kx; j <= kx; j++) {
18501 ix = j*step ;
18502 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18503
18504 Normalize_ring( cimage, numr );
18505
18506 Frngs(cimage, numr);
18507
18508
18509 for ( iref = 0; iref < (int)crefim_len; iref++) {
18510 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18511 double qn = retvals["qn"];
18512 double qm = retvals["qm"];
18513 if(qn >= peak || qm >= peak) {
18514 sx = -ix;
18515 sy = -iy;
18516 nref = iref;
18517 if (qn >= qm) {
18518 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18519 peak = static_cast<float>(qn);
18520 mirror = 0;
18521 } else {
18522 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18523 peak = static_cast<float>(qm);
18524 mirror = 1;
18525 }
18526 }
18527 }
18528 delete cimage; cimage = 0;
18529 }
18530 }
18531 }
18532 float co, so, sxs, sys;
18533 co = static_cast<float>( cos(ang*pi/180.0) );
18534 so = static_cast<float>( -sin(ang*pi/180.0) );
18535 sxs = sx*co - sy*so;
18536 sys = sx*so + sy*co;
18537 vector<float> res;
18538 res.push_back(ang);
18539 res.push_back(sxs);
18540 res.push_back(sys);
18541 res.push_back(static_cast<float>(mirror));
18542 res.push_back(static_cast<float>(nref));
18543 res.push_back(peak);
18544 return res;
18545 }
18546
18547 vector<float> Util::multiref_polar_ali_helical_local(EMData* image, const vector< EMData* >& crefim,
18548 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18549 vector<int>numr, float cnx, float cny, int ynumber) {
18550
18551 size_t crefim_len = crefim.size();
18552
18553 int iref, nref=-1, mirror=0;
18554 float iy, ix, sx=0, sy=0;
18555 float peak = -1.0E23f;
18556 float ang=0.0f;
18557 const float qv = static_cast<float>( pi/180.0 );
18558 Transform * t = image->get_attr("xform.projection");
18559 Dict d = t->get_params("spider");
18560 if(t) {delete t; t=0;}
18561 float phi = d["phi"];
18562 float theta = d["theta"];
18563 float imn1 = sin(theta*qv)*cos(phi*qv);
18564 float imn2 = sin(theta*qv)*sin(phi*qv);
18565 float imn3 = cos(theta*qv);
18566 vector<float> n1(crefim_len);
18567 vector<float> n2(crefim_len);
18568 vector<float> n3(crefim_len);
18569 for ( iref = 0; iref < (int)crefim_len; iref++) {
18570 n1[iref] = crefim[iref]->get_attr("n1");
18571 n2[iref] = crefim[iref]->get_attr("n2");
18572 n3[iref] = crefim[iref]->get_attr("n3");
18573 }
18574
18575 int kx = int(2*xrng/step+0.5)/2;
18576
18577 if(ynumber==-1) {
18578 int ky = int(2*yrng/step+0.5)/2;
18579 for (int i = -ky; i <= ky; i++) {
18580 iy = i * step ;
18581 for (int j = -kx; j <= kx; j++) {
18582 ix = j*step ;
18583 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18584
18585 Normalize_ring( cimage, numr );
18586
18587 Frngs(cimage, numr);
18588
18589
18590 for ( iref = 0; iref < (int)crefim_len; iref++) {
18591 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18592 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18593 double qn = retvals["qn"];
18594 double qm = retvals["qm"];
18595 if(qn >= peak || qm >= peak) {
18596 sx = -ix;
18597 sy = -iy;
18598 nref = iref;
18599 if (qn >= qm) {
18600 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18601 peak = static_cast<float>(qn);
18602 mirror = 0;
18603 } else {
18604 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18605 peak = static_cast<float>(qm);
18606 mirror = 1;
18607 }
18608 }
18609 }
18610 }
18611 delete cimage; cimage = 0;
18612 }
18613 }
18614 }
18615
18616 else if(ynumber==0) {
18617 sy = 0.0f;
18618 for (int j = -kx; j <= kx; j++) {
18619 ix = j*step ;
18620 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18621
18622 Normalize_ring( cimage, numr );
18623
18624 Frngs(cimage, numr);
18625
18626
18627 for ( iref = 0; iref < (int)crefim_len; iref++) {
18628 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18629 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18630 double qn = retvals["qn"];
18631 double qm = retvals["qm"];
18632 if(qn >= peak || qm >= peak) {
18633 sx = -ix;
18634 nref = iref;
18635 if (qn >= qm) {
18636 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18637 peak = static_cast<float>(qn);
18638 mirror = 0;
18639 } else {
18640 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18641 peak = static_cast<float>(qm);
18642 mirror = 1;
18643 }
18644 }
18645 }
18646 }
18647 delete cimage; cimage = 0;
18648 }
18649 } else {
18650 int ky = int(ynumber/2);
18651 float stepy=2*yrng/ynumber;
18652
18653 for (int i = -ky+1; i <= ky; i++) {
18654 iy = i * stepy ;
18655 for (int j = -kx; j <= kx; j++) {
18656 ix = j*step ;
18657 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18658
18659 Normalize_ring( cimage, numr );
18660
18661 Frngs(cimage, numr);
18662
18663
18664 for ( iref = 0; iref < (int)crefim_len; iref++) {
18665 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18666 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18667 double qn = retvals["qn"];
18668 double qm = retvals["qm"];
18669 if(qn >= peak || qm >= peak) {
18670 sx = -ix;
18671 sy = -iy;
18672 nref = iref;
18673 if (qn >= qm) {
18674 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18675 peak = static_cast<float>(qn);
18676 mirror = 0;
18677 } else {
18678 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18679 peak = static_cast<float>(qm);
18680 mirror = 1;
18681 }
18682 }
18683 }
18684 }
18685 delete cimage; cimage = 0;
18686 }
18687 }
18688 }
18689 float co, so, sxs, sys;
18690 co = static_cast<float>( cos(ang*pi/180.0) );
18691 so = static_cast<float>( -sin(ang*pi/180.0) );
18692 sxs = sx*co - sy*so;
18693 sys = sx*so + sy*co;
18694 vector<float> res;
18695 res.push_back(ang);
18696 res.push_back(sxs);
18697 res.push_back(sys);
18698 res.push_back(static_cast<float>(mirror));
18699 res.push_back(static_cast<float>(nref));
18700 res.push_back(peak);
18701 return res;
18702 }
18703
18704 vector<float> Util::multiref_polar_ali_helical_90(EMData* image, const vector< EMData* >& crefim,
18705 float xrng, float yrng, float step, float psi_max, string mode,
18706 vector<int>numr, float cnx, float cny, int ynumber) {
18707
18708 size_t crefim_len = crefim.size();
18709
18710 int iref, nref=0, mirror=0;
18711 float iy, ix, sx=0, sy=0;
18712 float peak = -1.0E23f;
18713 float ang=0.0f;
18714 int kx = int(2*xrng/step+0.5)/2;
18715
18716 if(ynumber==-1) {
18717 int ky = int(2*yrng/step+0.5)/2;
18718 for (int i = -ky; i <= ky; i++) {
18719 iy = i * step ;
18720 for (int j = -kx; j <= kx; j++) {
18721 ix = j*step ;
18722 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18723
18724 Normalize_ring( cimage, numr );
18725
18726 Frngs(cimage, numr);
18727
18728
18729 for ( iref = 0; iref < (int)crefim_len; iref++) {
18730 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18731 double qn = retvals["qn"];
18732 if( qn >= peak) {
18733 sx = -ix;
18734 sy = -iy;
18735 nref = iref;
18736 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18737 peak = static_cast<float>(qn);
18738 mirror = 0;
18739 }
18740 }
18741 delete cimage; cimage = 0;
18742 }
18743 }
18744 }
18745
18746 else if(ynumber==0) {
18747 sy = 0.0f;
18748 for (int j = -kx; j <= kx; j++) {
18749 ix = j*step ;
18750 iy = 0.0f ;
18751 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18752
18753 Normalize_ring( cimage, numr );
18754
18755 Frngs(cimage, numr);
18756
18757
18758 for ( iref = 0; iref < (int)crefim_len; iref++) {
18759 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18760 double qn = retvals["qn"];
18761 if( qn >= peak ) {
18762 sx = -ix;
18763 nref = iref;
18764 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18765 peak = static_cast<float>(qn);
18766 mirror = 0;
18767 }
18768 }
18769 delete cimage; cimage = 0;
18770 }
18771 } else {
18772 int ky = int(ynumber/2);
18773 float stepy=2*yrng/ynumber;
18774
18775 for (int i = -ky+1; i <= ky; i++) {
18776 iy = i * stepy ;
18777 for (int j = -kx; j <= kx; j++) {
18778 ix = j*step ;
18779 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18780
18781 Normalize_ring( cimage, numr );
18782
18783 Frngs(cimage, numr);
18784
18785
18786 for ( iref = 0; iref < (int)crefim_len; iref++) {
18787 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18788 double qn = retvals["qn"];
18789 if( qn >= peak) {
18790 sx = -ix;
18791 sy = -iy;
18792 nref = iref;
18793 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18794 peak = static_cast<float>(qn);
18795 mirror = 0;
18796 }
18797 }
18798 delete cimage; cimage = 0;
18799 }
18800 }
18801 }
18802 float co, so, sxs, sys;
18803 co = static_cast<float>( cos(ang*pi/180.0) );
18804 so = static_cast<float>( -sin(ang*pi/180.0) );
18805 sxs = sx*co - sy*so;
18806 sys = sx*so + sy*co;
18807 vector<float> res;
18808 res.push_back(ang);
18809 res.push_back(sxs);
18810 res.push_back(sys);
18811 res.push_back(static_cast<float>(mirror));
18812 res.push_back(static_cast<float>(nref));
18813 res.push_back(peak);
18814 return res;
18815 }
18816
18817
18818
18819 vector<float> Util::multiref_polar_ali_helical_90_local(EMData* image, const vector< EMData* >& crefim,
18820 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18821 vector<int>numr, float cnx, float cny, int ynumber) {
18822
18823 size_t crefim_len = crefim.size();
18824 const float qv = static_cast<float>( pi/180.0 );
18825 Transform * t = image->get_attr("xform.projection");
18826 Dict d = t->get_params("spider");
18827 if(t) {delete t; t=0;}
18828 float phi = d["phi"];
18829 float theta = d["theta"];
18830 float imn1 = sin(theta*qv)*cos(phi*qv);
18831 float imn2 = sin(theta*qv)*sin(phi*qv);
18832 float imn3 = cos(theta*qv);
18833 vector<float> n1(crefim_len);
18834 vector<float> n2(crefim_len);
18835 vector<float> n3(crefim_len);
18836 int iref, nref=-1, mirror=0;
18837 float iy, ix, sx=0, sy=0;
18838 float peak = -1.0E23f;
18839 float ang=0.0f;
18840 int kx = int(2*xrng/step+0.5)/2;
18841
18842 for ( iref = 0; iref < (int)crefim_len; iref++) {
18843 n1[iref] = crefim[iref]->get_attr("n1");
18844 n2[iref] = crefim[iref]->get_attr("n2");
18845 n3[iref] = crefim[iref]->get_attr("n3");
18846 }
18847
18848
18849 if(ynumber==-1) {
18850 int ky = int(2*yrng/step+0.5)/2;
18851 for (int i = -ky; i <= ky; i++) {
18852 iy = i * step ;
18853 for (int j = -kx; j <= kx; j++) {
18854 ix = j*step ;
18855 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18856
18857 Normalize_ring( cimage, numr );
18858
18859 Frngs(cimage, numr);
18860
18861
18862 for ( iref = 0; iref < (int)crefim_len; iref++) {
18863 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18864 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18865 double qn = retvals["qn"];
18866 if( qn >= peak) {
18867 sx = -ix;
18868 sy = -iy;
18869 nref = iref;
18870 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18871 peak = static_cast<float>(qn);
18872 mirror = 0;
18873 }
18874 }
18875 }
18876 delete cimage; cimage = 0;
18877 }
18878 }
18879 }
18880
18881 else if(ynumber==0) {
18882 sy = 0.0f;
18883 for (int j = -kx; j <= kx; j++) {
18884 ix = j*step ;
18885 iy = 0.0f ;
18886 EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18887
18888 Normalize_ring( cimage, numr );
18889
18890 Frngs(cimage, numr);
18891
18892
18893 for ( iref = 0; iref < (int)crefim_len; iref++) {
18894 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18895 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18896 double qn = retvals["qn"];
18897 if( qn >= peak ) {
18898 sx = -ix;
18899 nref = iref;
18900 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18901 peak = static_cast<float>(qn);
18902 mirror = 0;
18903 }
18904 }
18905 }
18906 delete cimage; cimage = 0;
18907 }
18908 } else {
18909 int ky = int(ynumber/2);
18910 float stepy=2*yrng/ynumber;
18911
18912 for (int i = -ky+1; i <= ky; i++) {
18913 iy = i * stepy ;
18914 for (int j = -kx; j <= kx; j++) {
18915 ix = j*step ;
18916 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, 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 if(fabs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18925 Dict retvals = Crosrng_psi_0_180_no_mirror(crefim[iref], cimage, numr, psi_max);
18926 double qn = retvals["qn"];
18927 if( qn >= peak) {
18928 sx = -ix;
18929 sy = -iy;
18930 nref = iref;
18931 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18932 peak = static_cast<float>(qn);
18933 mirror = 0;
18934 }
18935 }
18936 }
18937 delete cimage; cimage = 0;
18938 }
18939 }
18940 }
18941 float co, so, sxs, sys;
18942 co = static_cast<float>( cos(ang*pi/180.0) );
18943 so = static_cast<float>( -sin(ang*pi/180.0) );
18944 sxs = sx*co - sy*so;
18945 sys = sx*so + sy*co;
18946 vector<float> res;
18947 res.push_back(ang);
18948 res.push_back(sxs);
18949 res.push_back(sys);
18950 res.push_back(static_cast<float>(mirror));
18951 res.push_back(static_cast<float>(nref));
18952 res.push_back(peak);
18953 return res;
18954 }
18955
18956
18957 void Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
18958 float xrng, float yrng, float step, string mode,
18959 vector< int >numr, float cnx, float cny,
18960 EMData *peaks, EMData *peakm) {
18961
18962 int maxrin = numr[numr.size()-1];
18963
18964 int ky = int(2*yrng/step+0.5)/2;
18965 int kx = int(2*xrng/step+0.5)/2;
18966
18967 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18968 float *p_ccf1ds = peaks->get_data();
18969
18970 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18971 float *p_ccf1dm = peakm->get_data();
18972
18973 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18974 p_ccf1ds[i] = -1.e20f;
18975 p_ccf1dm[i] = -1.e20f;
18976 }
18977
18978 for (int i = -ky; i <= ky; i++) {
18979 float iy = i * step;
18980 for (int j = -kx; j <= kx; j++) {
18981 float ix = j*step;
18982 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18983 Frngs(cimage, numr);
18984 Crosrng_msg_vec(crefim, cimage, numr,
18985 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18986 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18987 delete cimage; cimage = 0;
18988 }
18989 }
18990 return;
18991 }
18992
18993 void Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
18994 float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
18995 EMData *peaks_compress, EMData *peakm_compress) {
18996
18997 int maxrin = numr[numr.size()-1];
18998
18999 int ky = int(2*yrng/step+0.5)/2;
19000 int kx = int(2*xrng/step+0.5)/2;
19001
19002 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
19003 float *p_ccf1ds = peaks->get_data();
19004
19005 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
19006 float *p_ccf1dm = peakm->get_data();
19007
19008 peaks_compress->set_size(maxrin, 1, 1);
19009 float *p_ccf1ds_compress = peaks_compress->get_data();
19010
19011 peakm_compress->set_size(maxrin, 1, 1);
19012 float *p_ccf1dm_compress = peakm_compress->get_data();
19013
19014 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
19015 p_ccf1ds[i] = -1.e20f;
19016 p_ccf1dm[i] = -1.e20f;
19017 }
19018
19019 for (int i = -ky; i <= ky; i++) {
19020 float iy = i * step;
19021 for (int j = -kx; j <= kx; j++) {
19022 float ix = j*step;
19023 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19024 Frngs(cimage, numr);
19025 Crosrng_msg_vec(crefim, cimage, numr,
19026 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
19027 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
19028 delete cimage; cimage = 0;
19029 }
19030 }
19031 for (int x=0; x<maxrin; x++) {
19032 float maxs = -1.0e22f;
19033 float maxm = -1.0e22f;
19034 for (int i=1; i<=2*ky+1; i++) {
19035 for (int j=1; j<=2*kx+1; j++) {
19036 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
19037 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
19038 }
19039 }
19040 p_ccf1ds_compress[x] = maxs;
19041 p_ccf1dm_compress[x] = maxm;
19042 }
19043 return;
19044 }
19045
19046 struct ccf_point
19047 {
19048 float value;
19049 int i;
19050 int j;
19051 int k;
19052 int mirror;
19053 };
19054
19055
19056 struct ccf_value
19057 {
19058 bool operator()( const ccf_point& a, const ccf_point& b )
19059 {
19060 return a.value > b.value;
19061 }
19062 };
19063
19064
19065 vector<float> Util::ali2d_ccf_list(EMData* image, EMData* crefim,
19066 float xrng, float yrng, float step, string mode,
19067 vector< int >numr, float cnx, float cny, double T) {
19068
19069 int maxrin = numr[numr.size()-1];
19070
19071 int ky = int(2*yrng/step+0.5)/2;
19072 int kx = int(2*xrng/step+0.5)/2;
19073
19074 float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
19075 float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
19076 int vol = maxrin*(2*kx+1)*(2*ky+1);
19077 vector<ccf_point> ccf(2*vol);
19078 ccf_point temp;
19079
19080 int index = 0;
19081 for (int i = -ky; i <= ky; i++) {
19082 float iy = i * step;
19083 for (int j = -kx; j <= kx; j++) {
19084 float ix = j*step;
19085 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
19086 Frngs(cimage, numr);
19087 Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
19088 for (int k=0; k<maxrin; k++) {
19089 temp.value = p_ccf1ds[k];
19090 temp.i = k;
19091 temp.j = j;
19092 temp.k = i;
19093 temp.mirror = 0;
19094 ccf[index] = temp;
19095 index++;
19096 temp.value = p_ccf1dm[k];
19097 temp.mirror = 1;
19098 ccf[index] = temp;
19099 index++;
19100 }
19101 delete cimage; cimage = 0;
19102 }
19103 }
19104
19105 delete p_ccf1ds;
19106 delete p_ccf1dm;
19107 std::sort(ccf.begin(), ccf.end(), ccf_value());
19108
19109 double qt = (double)ccf[0].value;
19110 vector <double> p(2*vol), cp(2*vol);
19111
19112 double sump = 0.0;
19113 for (int i=0; i<2*vol; i++) {
19114 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
19115 sump += p[i];
19116 }
19117 for (int i=0; i<2*vol; i++) {
19118 p[i] /= sump;
19119 }
19120 for (int i=1; i<2*vol; i++) {
19121 p[i] += p[i-1];
19122 }
19123 p[2*vol-1] = 2.0;
19124
19125 float t = get_frand(0.0f, 1.0f);
19126 int select = 0;
19127 while (p[select] < t) select += 1;
19128
19129 vector<float> a(6);
19130 a[0] = ccf[select].value;
19131 a[1] = (float)ccf[select].i;
19132 a[2] = (float)ccf[select].j;
19133 a[3] = (float)ccf[select].k;
19134 a[4] = (float)ccf[select].mirror;
19135 a[5] = (float)select;
19136 return a;
19137 }
19138
19139
19140
19141
19142
19143
19144
19145
19146
19147
19148
19149
19150
19151
19152
19153
19154
19155
19156
19157
19158
19159
19160
19161
19162
19163
19164
19165
19166
19167
19168
19169
19170
19171
19172
19173
19174
19175
19176
19177
19178
19179
19180
19181
19182
19183
19184
19185
19186
19187
19188
19189
19190
19191
19192
19193
19194
19195
19196
19197
19198
19199
19200
19201
19202
19203
19204
19205
19206 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19207
19208 EMData *rot;
19209
19210 const int nmax=3, mmax=3;
19211 char task[60], csave[60];
19212 long int lsave[4];
19213 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19214 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];
19215 long int SIXTY=60;
19216
19217
19218 iprint = -1;
19219
19220
19221 factr=1.0e1;
19222 pgtol=1.0e-5;
19223
19224
19225
19226
19227 n=3;
19228 m=3;
19229
19230
19231
19232
19233
19234 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19235 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19236 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19237
19238
19239
19240
19241 strcpy(task,"START");
19242 for (int i=5;i<60;i++) task[i]=' ';
19243
19244
19245
19246 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19247
19248
19249
19250 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19251
19252 if (strncmp(task,"FG",2)==0) {
19253
19254
19255
19256
19257 rot = new EMData();
19258 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
19259 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19260
19261 delete rot;
19262
19263
19264 float dt = 1.0e-3f;
19265 rot = new EMData();
19266 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
19267 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19268
19269 g[0] = (f1-f)/dt;
19270 delete rot;
19271
19272 dt = 1.0e-2f;
19273 rot = new EMData();
19274 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
19275 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19276
19277 g[1] = (f2-f)/dt;
19278 delete rot;
19279
19280 rot = new EMData();
19281 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
19282 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19283
19284 g[2] = (f3-f)/dt;
19285 delete rot;
19286 }
19287
19288
19289 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19290
19291 }
19292
19293
19294 vector<float> res;
19295 res.push_back(static_cast<float>(x[0]));
19296 res.push_back(static_cast<float>(x[1]));
19297 res.push_back(static_cast<float>(x[2]));
19298
19299 return res;
19300 }
19301
19302 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19303
19304 EMData *rot;
19305
19306 const int nmax=3, mmax=3;
19307 char task[60], csave[60];
19308 long int lsave[4];
19309 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19310 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];
19311 long int SIXTY=60;
19312
19313
19314 iprint = -1;
19315
19316
19317 factr=1.0e1;
19318 pgtol=1.0e-5;
19319
19320
19321
19322
19323 n=3;
19324 m=3;
19325
19326
19327
19328
19329
19330 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
19331 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
19332 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
19333
19334
19335
19336
19337 strcpy(task,"START");
19338 for (int i=5;i<60;i++) task[i]=' ';
19339
19340
19341
19342 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19343
19344
19345
19346 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19347
19348 if (strncmp(task,"FG",2)==0) {
19349
19350
19351
19352
19353 rot = new EMData();
19354 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19355 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19356
19357 delete rot;
19358
19359
19360 float dt = 1.0e-3f;
19361 rot = new EMData();
19362 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
19363 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19364
19365 g[0] = (f1-f)/dt;
19366 delete rot;
19367
19368 rot = new EMData();
19369 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
19370 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19371
19372 g[1] = (f2-f)/dt;
19373 delete rot;
19374
19375 rot = new EMData();
19376 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
19377 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19378
19379 g[2] = (f3-f)/dt;
19380 delete rot;
19381 }
19382
19383
19384 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19385
19386 }
19387
19388
19389 vector<float> res;
19390 res.push_back(static_cast<float>(x[0]));
19391 res.push_back(static_cast<float>(x[1]));
19392 res.push_back(static_cast<float>(x[2]));
19393
19394 return res;
19395 }
19396
19397 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) {
19398
19399 EMData *proj, *proj2;
19400
19401 const int nmax=5, mmax=5;
19402 char task[60], csave[60];
19403 long int lsave[4];
19404 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
19405 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];
19406 long int SIXTY=60;
19407
19408
19409 iprint = -1;
19410
19411
19412 factr=1.0e1;
19413 pgtol=1.0e-5;
19414
19415
19416
19417
19418 n=5;
19419 m=5;
19420
19421
19422
19423
19424
19425 x[0] = phi; nbd[0] = 2; l[0] = phi-2.0; u[0] = phi+2.0;
19426 x[1] = theta; nbd[1] = 2; l[1] = theta-2.0; u[1] = theta+2.0;
19427 x[2] = psi; nbd[2] = 2; l[2] = psi-2.0; u[2] = psi+2.0;
19428 x[3] = sxs; nbd[3] = 2; l[3] = sxs-2.0; u[3] = sxs+2.0;
19429 x[4] = sys; nbd[4] = 2; l[4] = sys-2.0; u[4] = sys+2.0;
19430
19431
19432
19433
19434 strcpy(task,"START");
19435 for (int i=5;i<60;i++) task[i]=' ';
19436
19437
19438
19439 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19440 int step = 1;
19441
19442
19443 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19444
19445 if (strncmp(task,"FG",2)==0) {
19446
19447
19448
19449
19450 proj = new EMData();
19451 proj2 = new EMData();
19452 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19453 proj->fft_shuffle();
19454 proj->center_origin_fft();
19455 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19456 proj->do_ift_inplace();
19457 int M = proj->get_ysize()/2;
19458 proj2 = proj->window_center(M);
19459 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19460
19461 delete proj;
19462 delete proj2;
19463
19464
19465 float dt = 1.0e-3f;
19466 proj = new EMData();
19467 proj2 = new EMData();
19468 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
19469 proj->fft_shuffle();
19470 proj->center_origin_fft();
19471 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19472 proj->do_ift_inplace();
19473 proj2 = proj->window_center(M);
19474 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19475
19476 delete proj;
19477 delete proj2;
19478 g[0] = (ft-f)/dt;
19479
19480 proj = new EMData();
19481 proj2 = new EMData();
19482 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
19483 proj->fft_shuffle();
19484 proj->center_origin_fft();
19485 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19486 proj->do_ift_inplace();
19487 proj2 = proj->window_center(M);
19488 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19489
19490 delete proj;
19491 delete proj2;
19492 g[1] = (ft-f)/dt;
19493
19494 proj = new EMData();
19495 proj2 = new EMData();
19496 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
19497 proj->fft_shuffle();
19498 proj->center_origin_fft();
19499 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
19500 proj->do_ift_inplace();
19501 proj2 = proj->window_center(M);
19502 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19503
19504 delete proj;
19505 delete proj2;
19506 g[2] = (ft-f)/dt;
19507
19508 proj = new EMData();
19509 proj2 = new EMData();
19510 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19511 proj->fft_shuffle();
19512 proj->center_origin_fft();
19513 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
19514 proj->do_ift_inplace();
19515 proj2 = proj->window_center(M);
19516 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19517
19518 delete proj;
19519 delete proj2;
19520 g[3] = (ft-f)/dt;
19521
19522 proj = new EMData();
19523 proj2 = new EMData();
19524 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
19525 proj->fft_shuffle();
19526 proj->center_origin_fft();
19527 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
19528 proj->do_ift_inplace();
19529 proj2 = proj->window_center(M);
19530 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
19531
19532 delete proj;
19533 delete proj2;
19534 g[4] = (ft-f)/dt;
19535 }
19536
19537
19538 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19539 step++;
19540 }
19541
19542
19543 vector<float> res;
19544 res.push_back(static_cast<float>(x[0]));
19545 res.push_back(static_cast<float>(x[1]));
19546 res.push_back(static_cast<float>(x[2]));
19547 res.push_back(static_cast<float>(x[3]));
19548 res.push_back(static_cast<float>(x[4]));
19549
19550 return res;
19551 }
19552
19553
19554 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19555
19556 double x[4];
19557 int n;
19558 int l = 3;
19559 int m = 200;
19560 double e = 1e-9;
19561 double step = 0.01;
19562 float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
19563
19564 x[1] = ang;
19565 x[2] = sxs;
19566 x[3] = sys;
19567
19568 Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);
19569
19570
19571 vector<float> res;
19572 res.push_back(static_cast<float>(x[1]));
19573 res.push_back(static_cast<float>(x[2]));
19574 res.push_back(static_cast<float>(x[3]));
19575 res.push_back(static_cast<float>(n));
19576 return res;
19577 }
19578
19579 vector<float> Util::multi_align_error(vector<float> args, vector<float> all_ali_params) {
19580
19581 const int nmax=args.size(), mmax=nmax;
19582 char task[60], csave[60];
19583 long int lsave[4];
19584 long int n, m, iprint, isave[44];
19585 long int* nbd = new long int[nmax];
19586 long int* iwa = new long int[3*nmax];
19587 double f, factr, pgtol;
19588 double* x = new double[nmax];
19589 double* l = new double[nmax];
19590 double* u = new double[nmax];
19591 double* g = new double[nmax];
19592 double dsave[29];
19593 double* wa = new double[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
19594 long int SIXTY=60;
19595
19596 int num_ali = nmax/3+1;
19597 int nima = all_ali_params.size()/(num_ali*4);
19598
19599
19600 iprint = -1;
19601
19602
19603 factr=1.0e1;
19604 pgtol=1.0e-9;
19605
19606
19607
19608
19609 n=nmax;
19610 m=mmax;
19611
19612
19613
19614
19615
19616 for (int i=0; i<nmax; i++) {
19617 x[i] = args[i];
19618 nbd[i] = 0;
19619 }
19620
19621
19622
19623 strcpy(task,"START");
19624 for (int i=5;i<60;i++) task[i]=' ';
19625
19626
19627
19628 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19629 int step = 1;
19630
19631
19632 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19633
19634 if (strncmp(task,"FG",2)==0) {
19635
19636
19637
19638
19639 f = multi_align_error_func(x, all_ali_params, nima, num_ali);
19640
19641
19642 multi_align_error_dfunc(x, all_ali_params, nima, num_ali, g);
19643 }
19644
19645
19646 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19647 step++;
19648 }
19649
19650
19651 vector<float> res;
19652 for (int i=0; i<nmax; i++) res.push_back(static_cast<float>(x[i]));
19653 res.push_back((float)f);
19654
19655 delete[] nbd;
19656 delete[] iwa;
19657 delete[] x;
19658 delete[] l;
19659 delete[] u;
19660 delete[] g;
19661 delete[] wa;
19662
19663 return res;
19664
19665 }
19666
19667 float Util::multi_align_error_func(double* x, vector<float> all_ali_params, int nima, int num_ali) {
19668
19669 float x1 = 1.0;
19670 float y1 = 0.0;
19671 float x2 = 0.0;
19672 float y2 = 1.0;
19673
19674 float all_var = 0;
19675 float* x1_new = new float[num_ali];
19676 float* y1_new = new float[num_ali];
19677 float* x2_new = new float[num_ali];
19678 float* y2_new = new float[num_ali];
19679
19680 for (int i=0; i<nima; i++) {
19681 float alpha2 = all_ali_params[(num_ali-1)*(nima*4)+i*4];
19682 float sx2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+1];
19683 float sy2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+2];
19684
19685 rot_shift(x1, y1, alpha2, sx2, sy2, x1_new+num_ali-1, y1_new+num_ali-1);
19686 rot_shift(x2, y2, alpha2, sx2, sy2, x2_new+num_ali-1, y2_new+num_ali-1);
19687 for (int j=0; j<num_ali-1; j++) {
19688 float alpha1 = all_ali_params[j*(nima*4)+i*4];
19689 float sx1 = all_ali_params[j*(nima*4)+i*4+1];
19690 float sy1 = all_ali_params[j*(nima*4)+i*4+2];
19691 int mirror1 = static_cast<int>(all_ali_params[j*(nima*4)+i*4+3]);
19692
19693 float alphai = x[j*3];
19694 float sxi = x[j*3+1];
19695 float syi = x[j*3+2];
19696
19697 float alpha12, sx12, sy12;
19698 int mirror12;
19699 if (mirror1 == 0) {
19700 alpha12 = fmod(alpha1+alphai, 360.0f);
19701 rot_shift(sx1, sy1, alphai, sxi, syi, &sx12, &sy12);
19702 mirror12 = 0;
19703 } else {
19704 alpha12 = fmod(alpha1-alphai, 360.0f);
19705 rot_shift(sx1, sy1, -alphai, -sxi, syi, &sx12, &sy12);
19706 mirror12 = 1;
19707 }
19708
19709 rot_shift(x1, y1, alpha12, sx12, sy12, x1_new+j, y1_new+j);
19710 rot_shift(x2, y2, alpha12, sx12, sy12, x2_new+j, y2_new+j);
19711 }
19712
19713 all_var += var(x1_new, num_ali)+var(y1_new, num_ali)+var(x2_new, num_ali)+var(y2_new, num_ali);
19714 }
19715 delete[] x1_new;
19716 delete[] y1_new;
19717 delete[] x2_new;
19718 delete[] y2_new;
19719 return all_var/static_cast<float>(nima);
19720 }
19721
19722 void Util::multi_align_error_dfunc(double* x, vector<float> all_ali_params, int nima, int num_ali, double* g) {
19723
19724
19725 for (int i=0; i<num_ali*3-3; i++) g[i] = 0.0;
19726
19727 float x1 = 1.0;
19728 float y1 = 0.0;
19729 float x2 = 0.0;
19730 float y2 = 1.0;
19731
19732 float* x1_new = new float[num_ali];
19733 float* y1_new = new float[num_ali];
19734 float* x2_new = new float[num_ali];
19735 float* y2_new = new float[num_ali];
19736
19737 float* alpha12_0 = new float[num_ali-1];
19738 float* dalpha12 = new float[num_ali-1];
19739 float* dsx12 = new float[num_ali-1];
19740 float* dsy12 = new float[num_ali-1];
19741 float* mirror1_0 = new float[num_ali-1];
19742
19743 for (int i=0; i<nima; i++) {
19744
19745 float alpha2 = all_ali_params[(num_ali-1)*(nima*4)+i*4];
19746 float sx2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+1];
19747 float sy2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+2];
19748
19749 rot_shift(x1, y1, alpha2, sx2, sy2, x1_new+num_ali-1, y1_new+num_ali-1);
19750 rot_shift(x2, y2, alpha2, sx2, sy2, x2_new+num_ali-1, y2_new+num_ali-1);
19751
19752 for (int j=0; j<num_ali-1; j++) {
19753 float alpha1 = all_ali_params[j*(nima*4)+i*4];
19754 float sx1 = all_ali_params[j*(nima*4)+i*4+1];
19755 float sy1 = all_ali_params[j*(nima*4)+i*4+2];
19756 int mirror1 = static_cast<int>(all_ali_params[j*(nima*4)+i*4+3]);
19757
19758 float alphai = x[j*3];
19759 float sxi = x[j*3+1];
19760 float syi = x[j*3+2];
19761
19762 float cosi = cos(alphai/180.0f*M_PI);
19763 float sini = sin(alphai/180.0f*M_PI);
19764
19765 float alpha12, sx12, sy12;
19766 int mirror12;
19767 if (mirror1 == 0) {
19768 alpha12 = fmod(alpha1+alphai, 360.0f);
19769 rot_shift(sx1, sy1, alphai, sxi, syi, &sx12, &sy12);
19770 mirror12 = 0;
19771 } else {
19772 alpha12 = fmod(alpha1-alphai, 360.0f);
19773 rot_shift(sx1, sy1, -alphai, -sxi, syi, &sx12, &sy12);
19774 mirror12 = 1;
19775 }
19776
19777 rot_shift(x1, y1, alpha12, sx12, sy12, x1_new+j, y1_new+j);
19778 rot_shift(x2, y2, alpha12, sx12, sy12, x2_new+j, y2_new+j);
19779
19780 alpha12_0[j] = alpha12;
19781 mirror1_0[j] = mirror1;
19782 if (mirror1 == 0) {
19783 dalpha12[j] = M_PI/180.0f;
19784 dsx12[j] = (-sini*sx1+cosi*sy1)/180.0f*M_PI;
19785 dsy12[j] = (-cosi*sx1-sini*sy1)/180.0f*M_PI;
19786 } else {
19787 dalpha12[j] = -M_PI/180.0f;
19788 dsx12[j] = (sini*(-sx1)-cosi*sy1)/180.0f*M_PI;
19789 dsy12[j] = (-cosi*(-sx1)-sini*sy1)/180.0f*M_PI;
19790 }
19791 }
19792
19793 for (int j=0; j<num_ali-1; j++) {
19794 float cosa = cos(alpha12_0[j]/180.0f*M_PI);
19795 float sina = sin(alpha12_0[j]/180.0f*M_PI);
19796 float diffx1 = x1_new[j]-mean(x1_new, num_ali);
19797 float diffx2 = x2_new[j]-mean(x2_new, num_ali);
19798 float diffy1 = y1_new[j]-mean(y1_new, num_ali);
19799 float diffy2 = y2_new[j]-mean(y2_new, num_ali);
19800
19801 float p = diffx1*((-x1*sina+y1*cosa)*dalpha12[j]+dsx12[j])+diffx2*((-x2*sina+y2*cosa)*dalpha12[j]+dsx12[j])+diffy1*((-x1*cosa-y1*sina)*dalpha12[j]+dsy12[j])+diffy2*((-x2*cosa-y2*sina)*dalpha12[j]+dsy12[j]);
19802 g[j*3] += p;
19803
19804 p = diffx1+diffx2;
19805 if (mirror1_0[j] == 0) g[j*3+1] += p;
19806 else g[j*3+1] -= p;
19807
19808 p = diffy1+diffy2;
19809 g[j*3+2] += p;
19810 }
19811 }
19812
19813 delete[] x1_new;
19814 delete[] y1_new;
19815 delete[] x2_new;
19816 delete[] y2_new;
19817 delete[] alpha12_0;
19818 delete[] dalpha12;
19819 delete[] dsx12;
19820 delete[] dsy12;
19821 delete[] mirror1_0;
19822
19823 }
19824
19825 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
19826
19827 EMData *rot= new EMData();
19828 float ccc;
19829
19830 rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
19831 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19832 delete rot;
19833 return ccc;
19834 }
19835
19836 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19837
19838 double x[4];
19839 int n;
19840 int l = 3;
19841 int m = 200;
19842 double e = 1e-9;
19843 double step = 0.001;
19844 float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
19845
19846 x[1] = ang;
19847 x[2] = sxs;
19848 x[3] = sys;
19849
19850 Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);
19851
19852
19853 vector<float> res;
19854 res.push_back(static_cast<float>(x[1]));
19855 res.push_back(static_cast<float>(x[2]));
19856 res.push_back(static_cast<float>(x[3]));
19857 res.push_back(static_cast<float>(n));
19858 return res;
19859 }
19860
19861
19862 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
19863
19864 EMData *rot= new EMData();
19865 float ccc;
19866
19867 rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
19868 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19869 delete rot;
19870 return ccc;
19871 }
19872
19873 #define img_ptr(i,j,k) img_ptr[i+(j+(k*ny))*(size_t)nx]
19874 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*(size_t)nx]
19875 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
19876 {
19877 ENTERFUNC;
19878
19879 if (!img) {
19880 throw NullPointerException("NULL input image");
19881 }
19882
19883 int newx, newy, newz;
19884 bool keep_going;
19885 cout << " entered " <<endl;
19886 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
19887
19888 EMData * img2 = new EMData();
19889 img2->set_size(nx,ny,nz);
19890 img2->to_zero();
19891 float *img_ptr =img->get_data();
19892 float *img2_ptr = img2->get_data();
19893 int r2 = ro*ro;
19894 int r3 = r2*ro;
19895 int ri2 = ri*ri;
19896 int ri3 = ri2*ri;
19897
19898 int n2 = nx/2;
19899
19900 for (int k=-n2; k<=n2; k++) {
19901 float z2 = static_cast<float>(k*k);
19902 for (int j=-n2; j<=n2; j++) {
19903 float y2 = z2 + j*j;
19904 if(y2 <= r2) {
19905
19906
19907 for (int i=-n2; i<=n2; i++) {
19908 float x2 = y2 + i*i;
19909 if(x2 <= r3) {
19910
19911 int ib = i+n2; int jb = j+n2; int kb = k+n2;
19912 if(x2 >= ri3) {
19913
19914 if(img_ptr(ib,jb,kb) == 1.0f) {
19915
19916 if(Util::get_frand(0.0f, 1.0f) > qprob){
19917 img2_ptr(ib,jb,kb) = 0.0f;
19918 keep_going = true;
19919
19920 while(keep_going) {
19921 newx = Util::get_irand(-ro,ro);
19922 newy = Util::get_irand(-ro,ro);
19923 newz = Util::get_irand(-ro,ro);
19924 if(newx*newx+newy*newy+newz*newz <= r3) {
19925 newx += n2; newy += n2; newz += n2;
19926 if( img_ptr(newx,newy,newz) == 0.0f) {
19927 img2_ptr(newx,newy,newz) = 1.0f;
19928 keep_going = false; }
19929 }
19930 }
19931 } else img2_ptr(ib,jb,kb) = 1.0f;
19932 }
19933 } else {
19934
19935 if(img_ptr(ib,jb,kb) == 1.0) {
19936 if(Util::get_frand(0.0f,1.0f) > qprob) {
19937
19938 float numn = -1.0f;
19939 for (newz = -1; newz <= 1; newz++)
19940 for (newy = -1; newy <= 1; newy++)
19941 for (newx = -1; newx <= 1; newx++)
19942 numn += img_ptr(ib+newx,jb+newy,kb+newz);
19943 img2_ptr(ib,jb,kb) = 0.0;
19944 if(numn == 26.0f) {
19945
19946 keep_going = true;
19947 while(keep_going) {
19948 newx = Util::get_irand(-ro,ro);
19949 newy = Util::get_irand(-ro,ro);
19950 newz = Util::get_irand(-ro,ro);
19951 if(newx*newx+newy*newy+newz*newz < r3) {
19952 newx += n2; newy += n2; newz += n2;
19953 if( img_ptr(newx,newy,newz) == 0.0f) {
19954 if(newx*newx+newy*newy+newz*newz < r3) {
19955 if(newx*newx+newy*newy+newz*newz < r3) {
19956 newx += n2; newy += n2; newz += n2;
19957 if( img_ptr(newx,newy,newz) == 0.0f) {
19958 img2_ptr(newx,newy,newz) = 1.0f;
19959 keep_going = false; }
19960 }
19961 }
19962 }
19963 }
19964 }
19965 } else if(numn == 25.0f) {
19966
19967 for (newz = -1; newz <= 1; newz++) {
19968 for (newy = -1; newy <= 1; newy++) {
19969 for (newx = -1; newx <= 1; newx++) {
19970 if( newx != 0 && newy != 0 && newz != 0) {
19971 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
19972 img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
19973 }
19974 }
19975 }
19976 }
19977 }
19978 } else {
19979
19980 keep_going = true;
19981 while(keep_going) {
19982 newx = Util::get_irand(-1,1);
19983 newy = Util::get_irand(-1,1);
19984 newz = Util::get_irand(-1,1);
19985 if(newx != 0 && newy != 0 && newz != 0) {
19986 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
19987 img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;
19988 keep_going = false;
19989 }
19990 }
19991 }
19992 }
19993 } else img2_ptr(ib,jb,kb) = 1.0f;
19994 }
19995 }
19996 }
19997 }
19998 }
19999 }
20000 }
20001
20002 img2->update();
20003
20004 EXITFUNC;
20005 return img2;
20006 }
20007 #undef img_ptr
20008 #undef img2_ptr
20009
20010 struct point3d_t
20011 {
20012 point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
20013
20014 int x;
20015 int y;
20016 int z;
20017 };
20018
20019
20020 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
20021 {
20022 int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
20023 int noff = 6;
20024
20025 int nx = visited->get_xsize();
20026 int ny = visited->get_ysize();
20027 int nz = visited->get_zsize();
20028
20029 vector< point3d_t > pts;
20030 pts.push_back( point3d_t(ix, iy, iz) );
20031 visited->set_value_at( ix, iy, iz, (float)grpid );
20032
20033 int start = 0;
20034 int end = pts.size();
20035
20036 while( end > start ) {
20037 for(int i=start; i < end; ++i ) {
20038 int ix = pts[i].x;
20039 int iy = pts[i].y;
20040 int iz = pts[i].z;
20041
20042 for( int j=0; j < noff; ++j ) {
20043 int jx = ix + offs[j][0];
20044 int jy = iy + offs[j][1];
20045 int jz = iz + offs[j][2];
20046
20047 if( jx < 0 || jx >= nx ) continue;
20048 if( jy < 0 || jy >= ny ) continue;
20049 if( jz < 0 || jz >= nz ) continue;
20050
20051
20052 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
20053 pts.push_back( point3d_t(jx, jy, jz) );
20054 visited->set_value_at( jx, jy, jz, (float)grpid );
20055 }
20056
20057 }
20058 }
20059
20060 start = end;
20061 end = pts.size();
20062 }
20063 return pts.size();
20064 }
20065
20066
20067 EMData* Util::get_biggest_cluster( EMData* mg )
20068 {
20069 int nx = mg->get_xsize();
20070 int ny = mg->get_ysize();
20071 int nz = mg->get_zsize();
20072
20073 EMData* visited = new EMData();
20074 visited->set_size( nx, ny, nz );
20075 visited->to_zero();
20076 int grpid = 0;
20077 int maxgrp = 0;
20078 int maxsize = 0;
20079 for( int iz=0; iz < nz; ++iz ) {
20080 for( int iy=0; iy < ny; ++iy ) {
20081 for( int ix=0; ix < nx; ++ix ) {
20082 if( (*mg)(ix, iy, iz)==0.0 ) continue;
20083
20084 if( (*visited)(ix, iy, iz) > 0.0 ) {
20085
20086 continue;
20087 }
20088
20089 grpid++;
20090 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
20091 if( grpsize > maxsize ) {
20092 maxsize = grpsize;
20093 maxgrp = grpid;
20094 }
20095 }
20096 }
20097 }
20098
20099 Assert( maxgrp > 0 );
20100
20101 int npoint = 0;
20102 EMData* result = new EMData();
20103 result->set_size( nx, ny, nz );
20104 result->to_zero();
20105
20106 for( int iz=0; iz < nz; ++iz ) {
20107 for( int iy=0; iy < ny; ++iy ) {
20108 for( int ix=0; ix < nx; ++ix ) {
20109 if( (*visited)(ix, iy, iz)==maxgrp ) {
20110 (*result)(ix,iy,iz) = 1.0;
20111 npoint++;
20112 }
20113 }
20114 }
20115 }
20116
20117 Assert( npoint==maxsize );
20118 delete visited;
20119 return result;
20120
20121 }
20122
20123 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)
20124 {
20125 int ix, iy, iz;
20126 int i, j, k;
20127 int nr2, nl2;
20128 float dzz, az, ak;
20129 float scx, scy, scz;
20130 int offset = 2 - nx%2;
20131 int lsm = nx + offset;
20132 EMData* ctf_img1 = new EMData();
20133 ctf_img1->set_size(lsm, ny, nz);
20134 float freq = 1.0f/(2.0f*ps);
20135 scx = 2.0f/float(nx);
20136 if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
20137 if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
20138 nr2 = ny/2 ;
20139 nl2 = nz/2 ;
20140 for ( k=0; k<nz;k++) {
20141 iz = k; if(k>nl2) iz=k-nz;
20142 for ( j=0; j<ny;j++) {
20143 iy = j; if(j>nr2) iy=j - ny;
20144 for ( i=0; i<lsm/2; i++) {
20145 ix=i;
20146 ak=pow(ix*ix*scx*scx+iy*scy*iy*scy+iz*scz*iz*scz, 0.5f)*freq;
20147 if(ak!=0) az=0.0; else az=M_PI;
20148 dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f));
20149 (*ctf_img1) (i*2,j,k) = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
20150 (*ctf_img1) (i*2+1,j,k) = 0.0f;
20151 }
20152 }
20153 }
20154 ctf_img1->update();
20155 ctf_img1->set_complex(true);
20156 ctf_img1->set_ri(true);
20157
20158
20159 if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
20160 return ctf_img1;
20161 }
20162
20163
20164
20165
20166
20167
20168
20169
20170
20171
20172
20173
20174
20175
20176
20177
20178
20179
20180
20181
20182
20183
20184
20185
20186
20187
20188
20189
20190
20191
20192
20193
20194
20195
20196
20197
20198
20199
20200
20201
20202
20203
20204
20205
20206
20207
20208
20209
20210
20211
20212
20213
20214
20215
20216
20217
20218
20219
20220
20221
20222
20223
20224
20225
20226
20227
20228
20229
20230
20231
20232
20233
20234
20235
20236
20237
20238
20239
20240
20241
20242
20243
20244
20245
20246
20247
20248
20249 #define cent(i) out[i+N]
20250 #define assign(i) out[i]
20251 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
20252 int nx = d->get_xsize();
20253 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20254 vector<float> out(N+K+2);
20255 if(N*(N-1)/2 != nx) {
20256
20257 return out;}
20258
20259 for(int i=0; i<N; i++) assign(i) = float(i);
20260
20261 for(int i=0; i<N; i++) {
20262 int j = Util::get_irand(0,N-1);
20263 float temp = assign(i);
20264 assign(i) = assign(j);
20265 assign(j) = temp;
20266 }
20267 for(int k=0; k<K; k++) cent(k) = float(assign(k));
20268
20269
20270 for(int i=0; i<N; i++) assign(i) = 0.0f;
20271 float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
20272 bool change = true;
20273 int it = -1;
20274 int ct = -1;
20275 while ((change && disp < dispold) || ct > 0) {
20276
20277 change = false;
20278 dispold = disp;
20279 it++;
20280
20281
20282 disp = 0.0f;
20283 ct = 0;
20284 for(int i=0; i<N; i++) {
20285 qm = 1.0e23f;
20286 for(int k=0; k<K; k++) {
20287 if(float(i) == cent(k)) {
20288 qm = 0.0f;
20289 na = (float)k;
20290 } else {
20291 float dt = (*d)(mono(i,int(cent(k))));
20292 if(dt < qm) {
20293 qm = dt;
20294 na = (float)k;
20295 }
20296 }
20297 }
20298
20299
20300
20301 if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
20302 na = (float)(Util::get_irand(0, K));
20303 qm = (*d)(mono(i,int(na)));
20304 ct++;
20305 }
20306
20307 disp += qm;
20308
20309 if(na != assign(i)) {
20310 assign(i) = na;
20311 change = true;
20312 }
20313 }
20314
20315
20316 T = T*F;
20317
20318
20319
20320
20321
20322 for(int k=0; k<K; k++) {
20323 qm = 1.0e23f;
20324 for(int i=0; i<N; i++) {
20325 if(assign(i) == float(k)) {
20326 float q = 0.0;
20327 for(int j=0; j<N; j++) {
20328 if(assign(j) == float(k)) {
20329
20330 if(i != j) q += (*d)(mono(i,j));
20331
20332 }
20333 }
20334 if(q < qm) {
20335
20336 qm = q;
20337 cent(k) = float(i);
20338 }
20339 }
20340 }
20341 }
20342
20343 }
20344 out[N+K] = disp;
20345 out[N+K+1] = float(it);
20346 return out;
20347 }
20348 #undef cent
20349 #undef assign
20350
20351
20352
20353
20354
20355
20356
20357
20358
20359
20360
20361
20362
20363
20364
20365
20366
20367
20368
20369
20370
20371
20372
20373
20374
20375
20376
20377
20378
20379
20380
20381
20382
20383
20384
20385
20386
20387
20388
20389
20390
20391
20392
20393
20394
20395
20396
20397
20398
20399
20400
20401
20402
20403
20404
20405
20406
20407
20408
20409
20410
20411
20412
20413
20414
20415
20416
20417
20418
20419
20420
20421
20422
20423
20424
20425
20426
20427
20428
20429
20430
20431
20432
20433
20434
20435
20436
20437
20438
20439
20440
20441
20442
20443
20444
20445
20446
20447
20448
20449
20450
20451
20452
20453
20454
20455
20456
20457
20458
20459 vector<float> Util::cluster_equalsize(EMData* d) {
20460
20461 int nx = d->get_xsize();
20462 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
20463 int K = N/2;
20464 vector<float> group(N);
20465 if(N*(N-1)/2 != nx) {
20466
20467 return group;}
20468
20469 bool * active = new bool[N];
20470 for(int i=0; i<N; i++) active[i] = true;
20471
20472 float dm, qd;
20473 int ppi = 0, ppj = 0;
20474 for(int k=0; k<K; k++) {
20475
20476
20477 dm = 1.0e23f;
20478 for(int i=1; i<N; i++) {
20479 if(active[i]) {
20480 for(int j=0; j<i; j++) {
20481 if(active[j]) {
20482 qd = (*d)(i*(i - 1)/2 + j);
20483 if(qd < dm) {
20484 dm = qd;
20485 ppi = i;
20486 ppj = j;
20487 }
20488 }
20489 }
20490 }
20491 }
20492 group[2*k] = float(ppi);
20493 group[1+2*k] = float(ppj);
20494 active[ppi] = false;
20495 active[ppj] = false;
20496 }
20497
20498 delete [] active;
20499 active = NULL;
20500 return group;
20501 }
20502
20503
20504
20505
20506
20507
20508
20509
20510
20511
20512
20513
20514
20515
20516
20517
20518
20519
20520
20521
20522
20523
20524
20525
20526
20527
20528
20529
20530
20531
20532
20533
20534
20535
20536
20537
20538
20539
20540
20541
20542
20543
20544
20545
20546
20547
20548
20549
20550
20551
20552
20553
20554
20555
20556 #define data(i,j) group[i*ny+j]
20557 vector<float> Util::vareas(EMData* d) {
20558 const float step=0.001f;
20559 int ny = d->get_ysize();
20560
20561
20562
20563 vector<float> group(2*ny);
20564 for(int i=0; i<2*ny; i++) group[i] = 0.0f;
20565 int K = int(1.0f/step) +1;
20566 int hit = 0;
20567 for(int kx=0; kx<=K; kx++) {
20568 float tx = kx*step;
20569 for(int ky=0; ky<=K; ky++) {
20570 float ty = ky*step;
20571 float dm = 1.0e23f;
20572 for(int i=0; i<ny; i++) {
20573 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
20574 if( qd < dm) {
20575 dm = qd;
20576 hit = i;
20577 }
20578 }
20579 data(0,hit) += 1.0f;
20580 if(kx == 0 || ky == 0 || kx == K || ky == K) data(1,hit) = 1.0f;
20581 }
20582 }
20583 return group;
20584 }
20585 #undef data
20586
20587 EMData* Util::get_slice(EMData *vol, int dim, int index) {
20588
20589 int nx = vol->get_xsize();
20590 int ny = vol->get_ysize();
20591 int nz = vol->get_zsize();
20592 float *vol_data = vol->get_data();
20593 int new_nx, new_ny;
20594
20595 if (nz == 1)
20596 throw ImageDimensionException("Error: Input must be a 3-D object");
20597 if ((dim < 1) || (dim > 3))
20598 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
20599 if (((dim == 1) && (index < 0 || index > nx-1)) ||
20600 ((dim == 1) && (index < 0 || index > nx-1)) ||
20601 ((dim == 1) && (index < 0 || index > nx-1)))
20602 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
20603
20604 if (dim == 1) {
20605 new_nx = ny;
20606 new_ny = nz;
20607 } else if (dim == 2) {
20608 new_nx = nx;
20609 new_ny = nz;
20610 } else {
20611 new_nx = nx;
20612 new_ny = ny;
20613 }
20614
20615 EMData *slice = new EMData();
20616 slice->set_size(new_nx, new_ny, 1);
20617 float *slice_data = slice->get_data();
20618
20619 if (dim == 1) {
20620 for (int x=0; x<new_nx; x++)
20621 for (int y=0; y<new_ny; y++)
20622 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
20623 } else if (dim == 2) {
20624 for (int x=0; x<new_nx; x++)
20625 for (int y=0; y<new_ny; y++)
20626 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
20627 } else {
20628 for (int x=0; x<new_nx; x++)
20629 for (int y=0; y<new_ny; y++)
20630 slice_data[y*new_nx+x] = vol_data[((size_t)index*ny+y)*nx+x];
20631 }
20632
20633 return slice;
20634 }
20635
20636 void Util::image_mutation(EMData *img, float mutation_rate) {
20637 int nx = img->get_xsize();
20638 float min = img->get_attr("minimum");
20639 float max = img->get_attr("maximum");
20640 float* img_data = img->get_data();
20641 array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
20642 return;
20643 }
20644
20645
20646 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20647
20648 if (is_mirror != 0) {
20649 for (int i=0; i<len_list; i++) {
20650 int r = rand()%10000;
20651 float f = r/10000.0f;
20652 if (f < mutation_rate) list[i] = 1-list[i];
20653 }
20654 } else {
20655 map<int, vector<int> > graycode;
20656 map<vector<int>, int> rev_graycode;
20657 vector <int> gray;
20658
20659 int K=1;
20660 for (int i=0; i<L; i++) K*=2;
20661
20662 for (int k=0; k<K; k++) {
20663 int shift = 0;
20664 vector <int> gray;
20665 for (int i=L-1; i>-1; i--) {
20666 int t = ((k>>i)%2-shift)%2;
20667 gray.push_back(t);
20668 shift += t-2;
20669 }
20670 graycode[k] = gray;
20671 rev_graycode[gray] = k;
20672 }
20673
20674 float gap = (K-1)/(max_val-min_val);
20675 for (int i=0; i<len_list; i++) {
20676 float val = list[i];
20677 if (val < min_val) { val = min_val; }
20678 else if (val > max_val) { val = max_val; }
20679 int k = int((val-min_val)*gap+0.5);
20680 vector<int> gray = graycode[k];
20681 bool changed = false;
20682 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20683 int r = rand()%10000;
20684 float f = r/10000.0f;
20685 if (f < mutation_rate) {
20686 *p = 1-*p;
20687 changed = true;
20688 }
20689 }
20690 if (changed) {
20691 k = rev_graycode[gray];
20692 list[i] = k/gap+min_val;
20693 }
20694 }
20695 }
20696
20697 }
20698
20699 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20700
20701 if (is_mirror != 0) {
20702 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20703 int r = rand()%10000;
20704 float f = r/10000.0f;
20705 if (f < mutation_rate) *q = 1-*q;
20706 }
20707 } else {
20708 map<int, vector<int> > graycode;
20709 map<vector<int>, int> rev_graycode;
20710 vector <int> gray;
20711
20712 int K=1;
20713 for (int i=0; i<L; i++) K*=2;
20714
20715 for (int k=0; k<K; k++) {
20716 int shift = 0;
20717 vector <int> gray;
20718 for (int i=L-1; i>-1; i--) {
20719 int t = ((k>>i)%2-shift)%2;
20720 gray.push_back(t);
20721 shift += t-2;
20722 }
20723 graycode[k] = gray;
20724 rev_graycode[gray] = k;
20725 }
20726
20727 float gap = (K-1)/(max_val-min_val);
20728 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20729 float val = *q;
20730 if (val < min_val) { val = min_val; }
20731 else if (val > max_val) { val = max_val; }
20732 int k = int((val-min_val)*gap+0.5);
20733 vector<int> gray = graycode[k];
20734 bool changed = false;
20735 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20736 int r = rand()%10000;
20737 float f = r/10000.0f;
20738 if (f < mutation_rate) {
20739 *p = 1-*p;
20740 changed = true;
20741 }
20742 }
20743 if (changed) {
20744 k = rev_graycode[gray];
20745 *q = k/gap+min_val;
20746 }
20747 }
20748 }
20749 return list;
20750 }
20751
20752
20753 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
20754
20755 int total_cost = *output;
20756 int num_matches = *(output+1);
20757
20758 int cost=0;
20759 int* intx;
20760 int intx_size;
20761 int* intx_next(0);
20762 int intx_next_size = 0;
20763 int curclass;
20764 int curclass_size;
20765
20766 for(int i = 0; i < num_matches; i++){
20767 curclass = *(output+2+ i*nParts);
20768
20769 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
20770 *(argParts + Indices[curclass]+1) = -5;
20771
20772 curclass_size = *(dimClasses+curclass)-2;
20773 intx = new int[curclass_size];
20774 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
20775 intx_size = curclass_size;
20776
20777 for (int j=1; j < nParts; j++){
20778 curclass = *(output+2+ i*nParts+j);
20779 if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
20780 *(argParts + Indices[j*K+curclass]+1)=-5;
20781
20782 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);
20783 intx_next = new int[intx_next_size];
20784 Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
20785 delete[] intx;
20786 intx=intx_next;
20787 intx_size= intx_next_size;
20788 }
20789 delete[] intx_next;
20790
20791 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
20792
20793 cost = cost + intx_next_size;
20794 }
20795
20796 if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
20797
20798 return 1;
20799
20800 }
20801
20802
20803
20804
20805
20806
20807 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int* costlist, int J){
20808
20809
20810 bool flag = 0;
20811 int nintx;
20812 int* dummy(0);
20813
20814 int* curbranch = new int[nParts];
20815
20816
20817 for(int jit= 0; jit< J; jit++) *(costlist+jit) = 0;
20818
20819
20820 for(int a=0; a<K; a++)
20821 {
20822
20823
20824 if (*(argParts + Indices[a] + 1) < 1) continue;
20825 if (*(dimClasses + a)-2 <= T) continue;
20826
20827
20828
20829 for( int i=1; i < nParts; i++){
20830 flag = 0;
20831 for(int j=0; j < K; j++){
20832 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
20833 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);
20834 if (nintx > T) flag=1;
20835 else *(argParts + Indices[i*K+j] + 1) =-4;
20836 }
20837 if (flag==0) {break;}
20838 }
20839
20840
20841 *curbranch = a;
20842
20843 if (flag > 0)
20844 Util::explore2(argParts, Indices, dimClasses, nParts, K, T, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2,
20845 *(dimClasses+a)-2,0, J, matchlist, costlist, curbranch);
20846
20847
20848 for( int i=1; i < nParts; i++){
20849 for(int j=0; j < K; j++){
20850 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
20851
20852 }
20853 }
20854 }
20855
20856 delete[] curbranch;
20857 }
20858
20859
20860 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){
20861
20862
20863 int* curintx2(0);
20864 int nintx = size_curintx;
20865
20866
20867
20868 if (depth >0){
20869 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
20870 if (nintx <= T) return;
20871 }
20872
20873
20874 if (depth == (nParts-1)) {
20875
20876 int replace = 0;
20877 int ind_smallest = -1;
20878 int smallest_cost = -1;
20879
20880 for (int jit = 0; jit < J; jit++){
20881 if (*(costlist+jit) < nintx){
20882 replace = 1;
20883 if (ind_smallest == -1) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20884 if (*(costlist+jit) < smallest_cost) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20885 }
20886 }
20887
20888 if (replace > 0){
20889
20890 *(costlist + ind_smallest) = nintx;
20891 for (int xit = 0; xit < nParts; xit++)
20892 *(matchlist + ind_smallest*nParts + xit) = *(curbranch+xit);
20893
20894 }
20895
20896 return;
20897 }
20898
20899
20900
20901
20902 if (depth > 0){
20903 curintx2 = new int[nintx];
20904 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
20905 }
20906
20907 if (depth == 0){
20908
20909 curintx2 = new int[size_curintx];
20910 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
20911 }
20912
20913
20914
20915 depth=depth+1;
20916
20917 for (int i=0; i < K; i++){
20918
20919 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue;
20920 size_next = (*(dimClasses + depth*K+i ))-2;
20921 if (size_next <= T) continue;
20922 *(curbranch+depth) = i;
20923 Util::explore2(argParts,Indices, dimClasses, nParts, K, T, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth,J, matchlist,
20924 costlist, curbranch);
20925
20926 }
20927
20928 delete[] curintx2;
20929 }
20930
20931 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
20932
20933
20934
20935
20936
20937
20938
20939
20940
20941
20942 int* dummy(0);
20943 int* cref;
20944 int cref_size;
20945 int* ccomp;
20946 int ccomp_size;
20947 int nintx;
20948 for (int i=0; i < nParts; i++){
20949 for (int j =0; j < K; j++){
20950
20951
20952 cref = Parts[i][j];
20953 cref_size = dimClasses[i*K+cref[0]]-2;
20954
20955
20956 if (cref_size <= T){
20957 cref[0] = -1;
20958 continue;
20959 }
20960 bool done = 0;
20961 for (int a = 0; a < nParts; a++){
20962 if (a == i) continue;
20963 bool hasActive=0;
20964 for (unsigned int b=0; b < Parts[a].size(); b++){
20965
20966
20967 ccomp = Parts[a][b];
20968 ccomp_size= dimClasses[a*K+ccomp[0]]-2;
20969 nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
20970
20971
20972 if (nintx <= T)
20973 ccomp[1] = 0;
20974 else{
20975 ccomp[1] = 1;
20976 hasActive=1;
20977 }
20978 }
20979
20980 if (hasActive < 1){
20981 done=1;
20982 break;
20983 }
20984
20985 }
20986
20987 if (done > 0){
20988
20989
20990 cref[0] = -1;
20991 continue;
20992 }
20993
20994
20995
20996
20997
20998
20999
21000
21001
21002
21003 bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
21004
21005 if (found<1){
21006
21007 cref[0] = -1;
21008 }
21009 }
21010
21011
21012
21013 for (int d = K-1; d > -1; d--){
21014 if (Parts[i][d][0] < 0) Parts[i].erase(Parts[i].begin()+d);
21015 }
21016
21017 }
21018
21019
21020
21021
21022
21023 }
21024
21025
21026 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) {
21027
21028
21029 if (size_next <= T) return 0;
21030
21031
21032 int* curintx2(0);
21033 int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
21034 if (nintx <= T) return 0;
21035
21036 int old_depth=depth;
21037 if (depth == partref) depth = depth + 1;
21038 if (depth == nParts && old_depth>0) return 1;
21039
21040
21041
21042 curintx2 = new int[nintx];
21043 Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
21044
21045
21046
21047
21048 bool gt_thresh;
21049 int num_classes = Parts[depth].size();
21050
21051 for (int i=0; i < num_classes; i++){
21052 if (Parts[depth][i][1] < 1) continue;
21053 size_next = dimClasses[depth*K + Parts[depth][i][0] ]-2;
21054 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
21055 if (gt_thresh) { delete[] curintx2; return 1; }
21056 }
21057 delete[] curintx2;
21058 return 0;
21059 }
21060
21061
21062
21063
21064 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int n_guesses, int LARGEST_CLASS, int J,
21065 int max_branching, float stmult, int branchfunc, int LIM) {
21066
21067
21068
21069
21070
21071 int* Indices = new int[nParts*K];
21072 int ind_c = 0;
21073 for (int i=0; i < nParts; i++){
21074 for(int j = 0; j < K; j++){
21075 Indices[i*K + j] = ind_c;
21076 ind_c = ind_c + dimClasses[i*K + j];
21077 }
21078 }
21079
21080
21081
21082
21083 vector <vector <int*> > Parts(nParts,vector<int*>(K));
21084 ind_c = 0;
21085 int argParts_size=0;
21086 for (int i=0; i < nParts; i++){
21087 for(int j = 0; j < K; j++){
21088 Parts[i][j] = argParts + ind_c;
21089 ind_c = ind_c + dimClasses[i*K + j];
21090 argParts_size = argParts_size + dimClasses[i*K + j];
21091 }
21092 }
21093
21094
21095
21096
21097
21098
21099 Util::initial_prune(Parts, dimClasses, nParts, K, T);
21100 for(int i = 0; i < nParts; i++){
21101 for(int j=0; j < K; j++){
21102 argParts[Indices[i*K + j]+1] = -1;
21103 }
21104 }
21105
21106 int num_classes;
21107 int old_index;
21108 for(int i=0; i<nParts; i++){
21109 num_classes = Parts[i].size();
21110 for (int j=0; j < num_classes; j++){
21111 old_index = Parts[i][j][0];
21112
21113 argParts[Indices[i*K + old_index]+1] = 1;
21114 }
21115 }
21116
21117
21118
21119
21120
21121 int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T, 0, n_guesses, LARGEST_CLASS, J, max_branching, stmult, branchfunc, LIM);
21122
21123
21124
21125
21126 bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
21127
21128 delete[] Indices;
21129
21130
21131 if (correct < 1){
21132 cout << "something is wrong with output of branchMPI!\n";
21133 vector<int> ret(1);
21134 ret[0] = -1;
21135 if (output != 0) { delete[] output; output = 0; }
21136 return ret;
21137 }
21138
21139
21140
21141
21142
21143 int output_size = 2 + output[1] * nParts;
21144 vector<int> ret(output_size);
21145 for (int i = 0; i < output_size; i++) {
21146 ret[i]= output[i];
21147 }
21148 if (output != 0) { delete[] output; output = 0; }
21149 return ret;
21150
21151 }
21152
21153
21154 int branch_all=0;
21155 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int curlevel,int n_guesses, int
21156 LARGEST_CLASS, int J, int max_branching, float stmult, int branchfunc, int LIM) {
21157
21158
21159
21160 if (1 == 0){
21161 cout <<"begin test search2\n";
21162 int* matchlist = new int[J*nParts];
21163 int* costlist = new int[J];
21164 for (int jit = 0; jit < nParts; jit++) *(costlist+jit) = 0;
21165 Util::search2(argParts,Indices, dimClasses, nParts, K, T, matchlist,costlist,J);
21166
21167 for (int jit = 0; jit < J; jit++) {
21168 cout << *(costlist +jit)<<": ";
21169 for (int yit = 0; yit < nParts; yit++)
21170 cout << *(matchlist + jit*nParts + yit)<<",";
21171 cout <<"\n";
21172
21173 }
21174 cout <<"end test search2\n";
21175 int* output = new int[1];
21176 output[0] = 1;
21177 delete [] matchlist;
21178 delete [] costlist;
21179 return output;
21180 }
21181
21182
21183
21184 if (curlevel > K -1){
21185 int* output = new int[2];
21186 output[0] = 0;
21187 output[1] = 0;
21188 return output;
21189 }
21190
21191
21192
21193 int* matchlist = new int[J*nParts];
21194 int* costlist = new int[J];
21195 Util::search2(argParts, Indices, dimClasses, nParts, K, T, matchlist, costlist, J);
21196
21197
21198
21199
21200
21201
21202 for (int jit = 0; jit < J ; jit++){
21203
21204 if (costlist[jit] > T) break;
21205 if (jit == J-1){
21206 int* output = new int[2];
21207 output[0] = 0;
21208 output[1] = 0;
21209 delete[] matchlist;
21210 delete[] costlist;
21211 return output;
21212 }
21213 }
21214
21215
21216
21217
21218 if (curlevel==0) branch_all = 0;
21219
21220 int nBranches = -1;
21221
21222 if (branchfunc == 2)
21223 nBranches = branch_factor_2(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM);
21224
21225 if (branchfunc == 3)
21226 nBranches = branch_factor_3(costlist,matchlist,J, T, nParts, curlevel, max_branching, K, LIM);
21227
21228 if (branchfunc == 4)
21229 nBranches = branch_factor_4(costlist,matchlist,J, T, nParts, curlevel, max_branching, stmult);
21230
21231 int* newcostlist= new int[nBranches];
21232 int* newmatchlist = new int[nBranches*nParts];
21233 for (int i=0; i<nBranches; i++){
21234 newcostlist[i] = costlist[i];
21235 for (int j=0; j< nParts; j++)
21236 newmatchlist[i*nParts + j] = matchlist[i*nParts + j];
21237 }
21238
21239 delete[] costlist;
21240 delete[] matchlist;
21241
21242
21243 int* output = new int[2+K*nParts];
21244 output[0] = 0;
21245 output[1] = 0;
21246
21247 int old_index;
21248 int totalcost;
21249 int nmatches;
21250
21251
21252 for(int i=0; i < nBranches ; i++){
21253
21254
21255
21256
21257
21258
21259
21260 for(int j=0; j < nParts; j++){
21261
21262 old_index = newmatchlist[i*nParts + j];
21263 argParts[Indices[j*K+old_index] + 1] = -2;
21264 }
21265
21266
21267 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, curlevel+1, n_guesses, LARGEST_CLASS,
21268 J, max_branching, stmult,branchfunc, LIM);
21269
21270
21271 totalcost = newcostlist[i] + ret[0];
21272
21273
21274
21275
21276
21277 if (totalcost > output[0])
21278 {
21279 nmatches = 1 + ret[1];
21280
21281
21282 output[0] = totalcost;
21283 output[1] = nmatches;
21284 int nret = 2+(nmatches-1)*nParts;
21285 for(int iret=2; iret < nret; iret++) output[iret] = ret[iret];
21286 for(int imax=0; imax < nParts; imax++) output[nret+imax] = newmatchlist[i*nParts + imax];
21287 }
21288
21289
21290 delete[] ret;
21291
21292
21293
21294 for(int j=0; j < nParts; j++){
21295 old_index = newmatchlist[i*nParts + j];
21296 argParts[Indices[j*K+old_index] + 1] = 1;
21297 }
21298
21299 }
21300
21301 delete[] newmatchlist;
21302 delete[] newcostlist;
21303
21304 return output;
21305 }
21306
21307 int* costlist_global;
21308
21309
21310 bool jiafunc(int i, int j){
21311 return (costlist_global[j] < costlist_global[i]) ;
21312
21313 }
21314
21315
21316
21317
21318
21319
21320 int Util::branch_factor_2(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
21321
21322 int ntot=0;
21323 for (int jit=0; jit < J; jit++){
21324 if (*(costlist+jit) > T) ntot++;
21325 }
21326
21327 int cur;
21328
21329 int* indx = new int[J];
21330 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21331 vector<int> myindx (indx, indx+J);
21332 vector<int>::iterator it;
21333 costlist_global=costlist;
21334 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21335
21336
21337 int* templist = new int[J];
21338 int* temp2list = new int[J*nParts];
21339 int next = 0;
21340
21341 for (it=myindx.begin(); it!=myindx.end();++it){
21342 cur = *(costlist + *it);
21343 if (cur > T){
21344
21345 templist[next] = cur;
21346 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21347 next = next + 1;
21348 }
21349 }
21350
21351 for (int jit=0; jit < ntot; jit++){
21352 *(costlist+jit)=*(templist + jit);
21353
21354 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21355 }
21356
21357
21358 delete [] indx;
21359
21360
21361
21362
21363
21364
21365 int B=1;
21366 int B_init=B;
21367 int infeasible=0;
21368
21369 for (int i=B_init; i<ntot; i++){
21370 if (i==ntot) continue;
21371
21372
21373 infeasible = 0;
21374 if (LIM < 0) LIM = B;
21375 for (int j=0; j<B; j++){
21376
21377 for (int vit=0; vit<nParts; vit++){
21378 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
21379 }
21380 if (infeasible >= LIM) break;
21381 }
21382
21383 if (infeasible >= LIM){
21384 *(costlist+B)=*(templist+i);
21385 for (int vit=0; vit < nParts; vit++)
21386 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21387 B=B+1;
21388 }
21389 }
21390
21391 delete [] templist;
21392 delete [] temp2list;
21393
21394
21395 if (branch_all < max_branching){
21396 if (B>1)
21397 {branch_all = branch_all + B -1 ; }
21398 }
21399 else B=1;
21400
21401 return B;
21402
21403
21404 }
21405
21406
21407
21408 int Util::branch_factor_3(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int, int LIM){
21409
21410 int ntot=0;
21411 for (int jit=0; jit < J; jit++){
21412 if (*(costlist+jit) > T) ntot++;
21413 }
21414
21415 int cur;
21416
21417 int* indx = new int[J];
21418 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21419 vector<int> myindx (indx, indx+J);
21420 vector<int>::iterator it;
21421 costlist_global=costlist;
21422 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21423
21424
21425 int* templist = new int[J];
21426 int* temp2list = new int[J*nParts];
21427 int next = 0;
21428
21429 for (it=myindx.begin(); it!=myindx.end();++it){
21430 cur = *(costlist + *it);
21431 if (cur > T){
21432
21433 templist[next] = cur;
21434 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21435 next = next + 1;
21436 }
21437 }
21438
21439 for (int jit=0; jit < ntot; jit++){
21440 *(costlist+jit)=*(templist + jit);
21441
21442 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21443 }
21444
21445
21446 delete [] indx;
21447
21448
21449
21450
21451
21452
21453 int B=1;
21454 int B_init=B;
21455 int infeasible=0;
21456
21457
21458 if (LIM < 0) LIM = ntot-1;
21459 for (int i=B_init; i<ntot; i++){
21460 if (i==ntot) continue;
21461
21462
21463 infeasible = 0;
21464
21465 for (int j=0; j<ntot; j++){
21466 if (j == i) continue;
21467 for (int vit=0; vit<nParts; vit++){
21468 if (temp2list[i*nParts+vit] == temp2list[j*nParts+vit]) {infeasible++; break;}
21469 }
21470 if (infeasible >= LIM) break;
21471 }
21472
21473 if (infeasible >= LIM){
21474 *(costlist+B)=*(templist+i);
21475 for (int vit=0; vit < nParts; vit++)
21476 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
21477 B=B+1;
21478 }
21479 }
21480
21481 delete [] templist;
21482 delete [] temp2list;
21483
21484
21485
21486 if (branch_all < max_branching){
21487 if (B>1)
21488 {branch_all = branch_all + B-1;}
21489 }
21490 else B=1;
21491
21492 return B;
21493
21494
21495 }
21496
21497
21498
21499
21500
21501 int Util::branch_factor_4(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, float stmult){
21502 int sum=0;
21503 float average =0;
21504 int ntot=0;
21505 for (int jit=0; jit < J; jit++){
21506 if (*(costlist+jit) > T) {ntot++; sum = sum +*(costlist+jit);}
21507 }
21508 average = ((float)sum)/((float)ntot);
21509 int cur;
21510
21511 int* indx = new int[J];
21512 for (int jit=0; jit < J; jit++) indx[jit]=jit;
21513 vector<int> myindx (indx, indx+J);
21514 vector<int>::iterator it;
21515 costlist_global=costlist;
21516 sort(myindx.begin(), myindx.begin()+J, jiafunc);
21517
21518
21519 int* templist = new int[J];
21520 int* temp2list = new int[J*nParts];
21521 int next = 0;
21522
21523 for (it=myindx.begin(); it!=myindx.end();++it){
21524 cur = *(costlist + *it);
21525 if (cur > T){
21526
21527 templist[next] = cur;
21528 for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
21529 next = next + 1;
21530 }
21531 }
21532
21533 for (int jit=0; jit < ntot; jit++){
21534 *(costlist+jit)=*(templist + jit);
21535
21536 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
21537 }
21538
21539
21540 delete [] indx;
21541 delete [] templist;
21542 delete [] temp2list;
21543
21544 if (ntot == 1) return 1;
21545
21546
21547
21548 float sq_sum=0.0;
21549
21550 for (int i=0; i< ntot; i++){
21551 sq_sum = sq_sum + (float) pow((float) *(costlist+i) - average, (float)2.0);
21552
21553 }
21554
21555
21556 float variance = sq_sum/ntot;
21557 float stdev = (float)pow((float)variance,(float)0.5);
21558
21559
21560
21561 int B=1;
21562 int largest = *costlist;
21563
21564 for (int i=1; i<ntot; i++){
21565 int cur = *(costlist+i);
21566 if (largest-cur < (float)(stdev*stmult)) B++;
21567 else break;
21568
21569 }
21570
21571 if (branch_all < max_branching){
21572 if (B>1)
21573 {branch_all = branch_all + B-1;}
21574 }
21575 else B=1;
21576
21577 return B;
21578
21579
21580 }