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 < nx*ny*nz; i++) {
00115 if (maskptr[i]>0.5f == flip) {
00116 Sum1 += Volptr[i];
00117 Sum2 += Volptr[i]*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 sig2 = static_cast<float>(Sum2 - count*avg*avg)/(count-1);
00131 float sig = sqrt(sig2);
00132
00133 stats.push_back(avg);
00134 stats.push_back(sig);
00135 stats.push_back(MIN);
00136 stats.push_back(MAX);
00137
00138 return stats;
00139 }
00140
00141
00142
00143
00144 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00145 {
00146 ENTERFUNC;
00147
00148 if (!EMUtil::is_same_size(V1, V2)) {
00149 LOGERR("images not same size");
00150 throw ImageFormatException( "images not same size");
00151 }
00152
00153 size_t nx = V1->get_xsize();
00154 size_t ny = V1->get_ysize();
00155 size_t nz = V1->get_zsize();
00156 size_t size = nx*ny*nz;
00157
00158 EMData *BD = new EMData();
00159 BD->set_size(nx, ny, nz);
00160
00161 float *params = new float[2];
00162
00163 float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00164 long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00165 int nvox = 0L;
00166
00167 V1ptr = V1->get_data();
00168 V2ptr = V2->get_data();
00169 BDptr = BD->get_data();
00170
00171
00172 if(!mask){
00173 EMData * Mask = new EMData();
00174 Mask->set_size(nx,ny,nz);
00175 Mask->to_one();
00176 MASKptr = Mask->get_data();
00177 } else {
00178 if (!EMUtil::is_same_size(V1, mask)) {
00179 LOGERR("mask not same size");
00180 throw ImageFormatException( "mask not same size");
00181 }
00182
00183 MASKptr = mask->get_data();
00184 }
00185
00186
00187
00188
00189
00190 for (size_t i = 0L;i < size; i++) {
00191 if (MASKptr[i]>0.5f) {
00192 S1 += V1ptr[i]*V2ptr[i];
00193 S2 += V1ptr[i]*V1ptr[i];
00194 S3 += V2ptr[i];
00195 S4 += V1ptr[i];
00196 nvox ++;
00197 }
00198 }
00199
00200 if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00201 A =1.0f ;
00202 } else {
00203 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00204 }
00205 B = static_cast<float> (A*S4 - S3)/nvox;
00206
00207
00208
00209 for (size_t i = 0L;i < size; i++) {
00210 if (MASKptr[i]>0.5f) {
00211 BDptr[i] = A*V1ptr[i] - B - V2ptr[i];
00212 } else {
00213 BDptr[i] = 0.f;
00214 }
00215 }
00216
00217 BD->update();
00218
00219 params[0] = A;
00220 params[1] = B;
00221
00222 Dict BDnParams;
00223 BDnParams["imdiff"] = BD;
00224 BDnParams["A"] = params[0];
00225 BDnParams["B"] = params[1];
00226
00227 EXITFUNC;
00228 return BDnParams;
00229 }
00230
00231
00232
00233
00234
00235 EMData *Util::TwoDTestFunc(int Size, float p, float q, float a, float b, int flag, float alphaDeg)
00236 {
00237 ENTERFUNC;
00238 int Mid= (Size+1)/2;
00239
00240 if (flag==0) {
00241 EMData* ImBW = new EMData();
00242 ImBW->set_size(Size,Size,1);
00243 ImBW->to_zero();
00244
00245 float tempIm;
00246 float x,y;
00247
00248 for (int ix=(1-Mid); ix<Mid; ix++){
00249 for (int iy=(1-Mid); iy<Mid; iy++){
00250 x = (float)ix;
00251 y = (float)iy;
00252 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)) );
00253 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00254 }
00255 }
00256 ImBW->update();
00257 ImBW->set_complex(false);
00258 ImBW->set_ri(true);
00259
00260 return ImBW;
00261 }
00262 else if (flag==1) {
00263 EMData* ImBWFFT = new EMData();
00264 ImBWFFT ->set_size(2*Size,Size,1);
00265 ImBWFFT ->to_zero();
00266
00267 float r,s;
00268
00269 for (int ir=(1-Mid); ir<Mid; ir++){
00270 for (int is=(1-Mid); is<Mid; is++){
00271 r = (float)ir;
00272 s = (float)is;
00273 (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00274 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00275 }
00276 }
00277 ImBWFFT->update();
00278 ImBWFFT->set_complex(true);
00279 ImBWFFT->set_ri(true);
00280 ImBWFFT->set_shuffled(true);
00281 ImBWFFT->set_fftodd(true);
00282
00283 return ImBWFFT;
00284 }
00285 else if (flag==2 || flag==3) {
00286 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00287 float C=cos(alpha);
00288 float S=sin(alpha);
00289 float D= sqrt(S*S*b*b + C*C*a*a);
00290
00291
00292 float P = p * C *a*a/D ;
00293 float Q = q * S *b*b/D ;
00294
00295 if (flag==2) {
00296 EMData* pofalpha = new EMData();
00297 pofalpha ->set_size(Size,1,1);
00298 pofalpha ->to_zero();
00299
00300 float Norm0 = D*(float)sqrt(2*pi);
00301 float Norm1 = exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00302 float Norm2 = exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00303 float sD;
00304
00305 for (int is=(1-Mid); is<Mid; is++){
00306 sD = is/D ;
00307 (*pofalpha)(is+Mid-1) = Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00308 + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00309 }
00310 pofalpha-> update();
00311 pofalpha-> set_complex(false);
00312 pofalpha-> set_ri(true);
00313
00314 return pofalpha;
00315 }
00316 if (flag==3) {
00317 float vD;
00318
00319 EMData* pofalphak = new EMData();
00320 pofalphak ->set_size(2*Size,1,1);
00321 pofalphak ->to_zero();
00322
00323 for (int iv=(1-Mid); iv<Mid; iv++){
00324 vD = iv*D ;
00325 (*pofalphak)(2*(iv+Mid-1)) = exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00326 }
00327 pofalphak-> update();
00328 pofalphak-> set_complex(false);
00329 pofalphak-> set_ri(true);
00330
00331 return pofalphak;
00332 }
00333 }
00334 else if (flag==4) {
00335 cout <<" FH under construction";
00336 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00337 EMData* TryFH= OutFT -> real2FH(4.0);
00338 return TryFH;
00339 } else {
00340 cout <<" flag must be 0,1,2,3, or 4";
00341 }
00342
00343 EXITFUNC;
00344 return 0;
00345 }
00346
00347
00348 void Util::spline_mat(float *x, float *y, int n, float *xq, float *yq, int m)
00349 {
00350
00351 float x0= x[0];
00352 float x1= x[1];
00353 float x2= x[2];
00354 float y0= y[0];
00355 float y1= y[1];
00356 float y2= y[2];
00357 float yp1 = (y1-y0)/(x1-x0) + (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1) ;
00358 float xn = x[n];
00359 float xnm1= x[n-1];
00360 float xnm2= x[n-2];
00361 float yn = y[n];
00362 float ynm1= y[n-1];
00363 float ynm2= y[n-2];
00364 float ypn= (yn-ynm1)/(xn-xnm1) + (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00365 float *y2d = new float[n];
00366 Util::spline(x,y,n,yp1,ypn,y2d);
00367 Util::splint(x,y,y2d,n,xq,yq,m);
00368 delete [] y2d;
00369 return;
00370 }
00371
00372
00373 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2)
00374 {
00375 int i,k;
00376 float p, qn, sig, un, *u;
00377 u = new float[n-1];
00378
00379 if (yp1 > .99e30){
00380 y2[0]=u[0]=0.0;
00381 } else {
00382 y2[0]=-.5f;
00383 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00384 }
00385
00386 for (i=1; i < n-1; i++) {
00387 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00388 p = sig*y2[i-1] + 2.0f;
00389 y2[i] = (sig-1.0f)/p;
00390 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) - (y[i] - y[i-1] )/(x[i] -x[i-1]);
00391 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00392 }
00393
00394 if (ypn>.99e30){
00395 qn=0; un=0;
00396 } else {
00397 qn= .5f;
00398 un= (3.0f/(x[n-1] -x[n-2])) * (ypn - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00399 }
00400 y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00401 for (k=n-2; k>=0; k--){
00402 y2[k]=y2[k]*y2[k+1]+u[k];
00403 }
00404 delete [] u;
00405 }
00406
00407
00408 void Util::splint( float *xa, float *ya, float *y2a, int n, float *xq, float *yq, int m)
00409 {
00410 int klo, khi, k;
00411 float h, b, a;
00412
00413
00414 for (int j=0; j<m;j++){
00415 klo=0;
00416 khi=n-1;
00417 while (khi-klo >1) {
00418 k=(khi+klo) >>1;
00419 if (xa[k]>xq[j]){ khi=k;}
00420 else { klo=k;}
00421 }
00422 h=xa[khi]- xa[klo];
00423 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00424 a =(xa[khi]-xq[j])/h;
00425 b=(xq[j]-xa[klo])/h;
00426 yq[j]=a*ya[klo] + b*ya[khi]
00427 + ((a*a*a-a)*y2a[klo]
00428 +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00429 }
00430
00431 }
00432
00433
00434 void Util::Radialize(int *PermMatTr, float *kValsSorted,
00435 float *weightofkValsSorted, int Size, int *SizeReturned)
00436 {
00437 int iMax = (int) floor( (Size-1.0)/2 +.01);
00438 int CountMax = (iMax+2)*(iMax+1)/2;
00439 int Count=-1;
00440 float *kVals = new float[CountMax];
00441 float *weightMat = new float[CountMax];
00442 int *PermMat = new int[CountMax];
00443 SizeReturned[0] = CountMax;
00444
00445
00446 for (int jkx=0; jkx< iMax+1; jkx++) {
00447 for (int jky=0; jky< jkx+1; jky++) {
00448 Count++;
00449 kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00450 weightMat[Count]= 1.0;
00451 if (jkx!=0) { weightMat[Count] *=2;}
00452 if (jky!=0) { weightMat[Count] *=2;}
00453 if (jkx!=jky){ weightMat[Count] *=2;}
00454 PermMat[Count]=Count+1;
00455 }
00456 }
00457
00458 int lkVals = Count+1;
00459
00460
00461 sort_mat(&kVals[0],&kVals[Count],
00462 &PermMat[0], &PermMat[Count]);
00463
00464 fflush(stdout);
00465
00466 int newInd;
00467
00468 for (int iP=0; iP < lkVals ; iP++ ) {
00469 newInd = PermMat[iP];
00470 PermMatTr[newInd-1] = iP+1;
00471 }
00472
00473
00474
00475 int CountA=-1;
00476 int CountB=-1;
00477
00478 while (CountB< (CountMax-1)) {
00479 CountA++;
00480 CountB++;
00481
00482 kValsSorted[CountA] = kVals[CountB] ;
00483 if (CountB<(CountMax-1) ) {
00484 while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001 ) {
00485 SizeReturned[0]--;
00486 for (int iP=0; iP < lkVals; iP++){
00487
00488 if (PermMatTr[iP]>CountA+1) {
00489 PermMatTr[iP]--;
00490 }
00491 }
00492 CountB++;
00493 }
00494 }
00495 }
00496
00497
00498 for (int CountD=0; CountD < CountMax; CountD++) {
00499 newInd = PermMatTr[CountD];
00500 weightofkValsSorted[newInd-1] += weightMat[CountD];
00501 }
00502
00503 }
00504
00505
00506 vector<float>
00507 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00508 {
00509 vector<float> angles;
00510 float psi = 0.0;
00511 if ((0.0 == t1)&&(0.0 == t2)||(t1 >= t2)) {
00512 t1 = 0.0f;
00513 t2 = 90.0f;
00514 }
00515 if ((0.0 == p1)&&(0.0 == p2)||(p1 >= p2)) {
00516 p1 = 0.0f;
00517 p2 = 359.9f;
00518 }
00519 bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00520 for (float theta = t1; theta <= t2; theta += delta) {
00521 float detphi;
00522 int lt;
00523 if ((0.0 == theta)||(180.0 == theta)) {
00524 detphi = 360.0f;
00525 lt = 1;
00526 } else {
00527 detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00528 lt = int((p2 - p1)/detphi)-1;
00529 if (lt < 1) lt = 1;
00530 detphi = (p2 - p1)/lt;
00531 }
00532 for (int i = 0; i < lt; i++) {
00533 float phi = p1 + i*detphi;
00534 if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00535 angles.push_back(phi);
00536 angles.push_back(theta);
00537 angles.push_back(psi);
00538 }
00539 }
00540 return angles;
00541 }
00542
00543
00544 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
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
00646 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00647 {
00648
00649
00650 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00651 float quadri;
00652 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00653
00654 x = xx;
00655 y = yy;
00656
00657
00658 while ( x < 1.0 ) x += nxdata;
00659 while ( x >= (float)(nxdata+1) ) x -= nxdata;
00660 while ( y < 1.0 ) y += nydata;
00661 while ( y >= (float)(nydata+1) ) y -= nydata;
00662
00663 i = (int) x;
00664 j = (int) y;
00665
00666 dx0 = x - i;
00667 dy0 = y - j;
00668
00669 ip1 = i + 1;
00670 im1 = i - 1;
00671 jp1 = j + 1;
00672 jm1 = j - 1;
00673
00674 if (ip1 > nxdata) ip1 -= nxdata;
00675 if (im1 < 1) im1 += nxdata;
00676 if (jp1 > nydata) jp1 -= nydata;
00677 if (jm1 < 1) jm1 += nydata;
00678
00679 f0 = fdata(i,j);
00680 c1 = fdata(ip1,j) - f0;
00681 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00682 c3 = fdata(i,jp1) - f0;
00683 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00684
00685 dxb = dx0 - 1;
00686 dyb = dy0 - 1;
00687
00688
00689 if (dx0 >= 0) hxc = 1; else hxc = -1;
00690 if (dy0 >= 0) hyc = 1; else hyc = -1;
00691
00692 ic = i + hxc;
00693 jc = j + hyc;
00694
00695 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00696 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00697
00698 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00699 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00700
00701
00702 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00703
00704 return quadri;
00705 }
00706
00707 #undef fdata
00708
00709 #define fdata(i,j) fdata[ i-1 + (j-1)*nxdata ]
00710 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00711 {
00712
00713
00714 float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00715 float quadri;
00716 int i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00717
00718 x = xx;
00719 y = yy;
00720
00721
00722 if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00723 x = (float)xnew;
00724 y = (float)ynew;
00725 }
00726
00727
00728 i = (int) x;
00729 j = (int) y;
00730
00731 dx0 = x - i;
00732 dy0 = y - j;
00733
00734 ip1 = i + 1;
00735 im1 = i - 1;
00736 jp1 = j + 1;
00737 jm1 = j - 1;
00738
00739 if (ip1 > nxdata) ip1 -= nxdata;
00740 if (im1 < 1) im1 += nxdata;
00741 if (jp1 > nydata) jp1 -= nydata;
00742 if (jm1 < 1) jm1 += nydata;
00743
00744 f0 = fdata(i,j);
00745 c1 = fdata(ip1,j) - f0;
00746 c2 = (c1 - f0 + fdata(im1,j)) * 0.5f;
00747 c3 = fdata(i,jp1) - f0;
00748 c4 = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00749
00750 dxb = dx0 - 1;
00751 dyb = dy0 - 1;
00752
00753
00754 if (dx0 >= 0) hxc = 1; else hxc = -1;
00755 if (dy0 >= 0) hyc = 1; else hyc = -1;
00756
00757 ic = i + hxc;
00758 jc = j + hyc;
00759
00760 if (ic > nxdata) ic -= nxdata; else if (ic < 1) ic += nxdata;
00761 if (jc > nydata) jc -= nydata; else if (jc < 1) jc += nydata;
00762
00763 c5 = ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00764 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00765
00766
00767 quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00768
00769 return quadri;
00770 }
00771
00772 #undef fdata
00773
00774
00775 float Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00776 int K = kb.get_window_size();
00777 int kbmin = -K/2;
00778 int kbmax = -kbmin;
00779 int kbc = kbmax+1;
00780
00781 float pixel =0.0f;
00782 float w=0.0f;
00783
00784 delx = restrict1(delx, nx);
00785 int inxold = int(round(delx));
00786 if ( ny < 2 ) {
00787 float tablex1 = kb.i0win_tab(delx-inxold+3);
00788 float tablex2 = kb.i0win_tab(delx-inxold+2);
00789 float tablex3 = kb.i0win_tab(delx-inxold+1);
00790 float tablex4 = kb.i0win_tab(delx-inxold);
00791 float tablex5 = kb.i0win_tab(delx-inxold-1);
00792 float tablex6 = kb.i0win_tab(delx-inxold-2);
00793 float tablex7 = kb.i0win_tab(delx-inxold-3);
00794
00795 int x1, x2, x3, x4, x5, x6, x7;
00796
00797 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
00798 x1 = (inxold-3+nx)%nx;
00799 x2 = (inxold-2+nx)%nx;
00800 x3 = (inxold-1+nx)%nx;
00801 x4 = (inxold +nx)%nx;
00802 x5 = (inxold+1+nx)%nx;
00803 x6 = (inxold+2+nx)%nx;
00804 x7 = (inxold+3+nx)%nx;
00805 } else {
00806 x1 = inxold-3;
00807 x2 = inxold-2;
00808 x3 = inxold-1;
00809 x4 = inxold;
00810 x5 = inxold+1;
00811 x6 = inxold+2;
00812 x7 = inxold+3;
00813 }
00814
00815 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00816 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00817 data[x7]*tablex7 ;
00818
00819 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00820 } else if ( nz < 2 ) {
00821 dely = restrict1(dely, ny);
00822 int inyold = int(round(dely));
00823 float tablex1 = kb.i0win_tab(delx-inxold+3);
00824 float tablex2 = kb.i0win_tab(delx-inxold+2);
00825 float tablex3 = kb.i0win_tab(delx-inxold+1);
00826 float tablex4 = kb.i0win_tab(delx-inxold);
00827 float tablex5 = kb.i0win_tab(delx-inxold-1);
00828 float tablex6 = kb.i0win_tab(delx-inxold-2);
00829 float tablex7 = kb.i0win_tab(delx-inxold-3);
00830
00831 float tabley1 = kb.i0win_tab(dely-inyold+3);
00832 float tabley2 = kb.i0win_tab(dely-inyold+2);
00833 float tabley3 = kb.i0win_tab(dely-inyold+1);
00834 float tabley4 = kb.i0win_tab(dely-inyold);
00835 float tabley5 = kb.i0win_tab(dely-inyold-1);
00836 float tabley6 = kb.i0win_tab(dely-inyold-2);
00837 float tabley7 = kb.i0win_tab(dely-inyold-3);
00838
00839 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00840
00841 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
00842 x1 = (inxold-3+nx)%nx;
00843 x2 = (inxold-2+nx)%nx;
00844 x3 = (inxold-1+nx)%nx;
00845 x4 = (inxold +nx)%nx;
00846 x5 = (inxold+1+nx)%nx;
00847 x6 = (inxold+2+nx)%nx;
00848 x7 = (inxold+3+nx)%nx;
00849
00850 y1 = ((inyold-3+ny)%ny)*nx;
00851 y2 = ((inyold-2+ny)%ny)*nx;
00852 y3 = ((inyold-1+ny)%ny)*nx;
00853 y4 = ((inyold +ny)%ny)*nx;
00854 y5 = ((inyold+1+ny)%ny)*nx;
00855 y6 = ((inyold+2+ny)%ny)*nx;
00856 y7 = ((inyold+3+ny)%ny)*nx;
00857 } else {
00858 x1 = inxold-3;
00859 x2 = inxold-2;
00860 x3 = inxold-1;
00861 x4 = inxold;
00862 x5 = inxold+1;
00863 x6 = inxold+2;
00864 x7 = inxold+3;
00865
00866 y1 = (inyold-3)*nx;
00867 y2 = (inyold-2)*nx;
00868 y3 = (inyold-1)*nx;
00869 y4 = inyold*nx;
00870 y5 = (inyold+1)*nx;
00871 y6 = (inyold+2)*nx;
00872 y7 = (inyold+3)*nx;
00873 }
00874
00875 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00876 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00877 data[x7+y1]*tablex7 ) * tabley1 +
00878 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00879 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00880 data[x7+y2]*tablex7 ) * tabley2 +
00881 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00882 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00883 data[x7+y3]*tablex7 ) * tabley3 +
00884 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00885 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00886 data[x7+y4]*tablex7 ) * tabley4 +
00887 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00888 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00889 data[x7+y5]*tablex7 ) * tabley5 +
00890 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00891 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00892 data[x7+y6]*tablex7 ) * tabley6 +
00893 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00894 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00895 data[x7+y7]*tablex7 ) * tabley7;
00896
00897 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00898 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00899 } else {
00900 dely = restrict1(dely, ny);
00901 int inyold = int(Util::round(dely));
00902 delz = restrict1(delz, nz);
00903 int inzold = int(Util::round(delz));
00904
00905 float tablex1 = kb.i0win_tab(delx-inxold+3);
00906 float tablex2 = kb.i0win_tab(delx-inxold+2);
00907 float tablex3 = kb.i0win_tab(delx-inxold+1);
00908 float tablex4 = kb.i0win_tab(delx-inxold);
00909 float tablex5 = kb.i0win_tab(delx-inxold-1);
00910 float tablex6 = kb.i0win_tab(delx-inxold-2);
00911 float tablex7 = kb.i0win_tab(delx-inxold-3);
00912
00913 float tabley1 = kb.i0win_tab(dely-inyold+3);
00914 float tabley2 = kb.i0win_tab(dely-inyold+2);
00915 float tabley3 = kb.i0win_tab(dely-inyold+1);
00916 float tabley4 = kb.i0win_tab(dely-inyold);
00917 float tabley5 = kb.i0win_tab(dely-inyold-1);
00918 float tabley6 = kb.i0win_tab(dely-inyold-2);
00919 float tabley7 = kb.i0win_tab(dely-inyold-3);
00920
00921 float tablez1 = kb.i0win_tab(delz-inzold+3);
00922 float tablez2 = kb.i0win_tab(delz-inzold+2);
00923 float tablez3 = kb.i0win_tab(delz-inzold+1);
00924 float tablez4 = kb.i0win_tab(delz-inzold);
00925 float tablez5 = kb.i0win_tab(delz-inzold-1);
00926 float tablez6 = kb.i0win_tab(delz-inzold-2);
00927 float tablez7 = kb.i0win_tab(delz-inzold-3);
00928
00929 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00930
00931 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
00932 x1 = (inxold-3+nx)%nx;
00933 x2 = (inxold-2+nx)%nx;
00934 x3 = (inxold-1+nx)%nx;
00935 x4 = (inxold +nx)%nx;
00936 x5 = (inxold+1+nx)%nx;
00937 x6 = (inxold+2+nx)%nx;
00938 x7 = (inxold+3+nx)%nx;
00939
00940 y1 = ((inyold-3+ny)%ny)*nx;
00941 y2 = ((inyold-2+ny)%ny)*nx;
00942 y3 = ((inyold-1+ny)%ny)*nx;
00943 y4 = ((inyold +ny)%ny)*nx;
00944 y5 = ((inyold+1+ny)%ny)*nx;
00945 y6 = ((inyold+2+ny)%ny)*nx;
00946 y7 = ((inyold+3+ny)%ny)*nx;
00947
00948 z1 = ((inzold-3+nz)%nz)*nx*ny;
00949 z2 = ((inzold-2+nz)%nz)*nx*ny;
00950 z3 = ((inzold-1+nz)%nz)*nx*ny;
00951 z4 = ((inzold +nz)%nz)*nx*ny;
00952 z5 = ((inzold+1+nz)%nz)*nx*ny;
00953 z6 = ((inzold+2+nz)%nz)*nx*ny;
00954 z7 = ((inzold+3+nz)%nz)*nx*ny;
00955 } else {
00956 x1 = inxold-3;
00957 x2 = inxold-2;
00958 x3 = inxold-1;
00959 x4 = inxold;
00960 x5 = inxold+1;
00961 x6 = inxold+2;
00962 x7 = inxold+3;
00963
00964 y1 = (inyold-3)*nx;
00965 y2 = (inyold-2)*nx;
00966 y3 = (inyold-1)*nx;
00967 y4 = inyold*nx;
00968 y5 = (inyold+1)*nx;
00969 y6 = (inyold+2)*nx;
00970 y7 = (inyold+3)*nx;
00971
00972 z1 = (inzold-3)*nx*ny;
00973 z2 = (inzold-2)*nx*ny;
00974 z3 = (inzold-1)*nx*ny;
00975 z4 = inzold*nx*ny;
00976 z5 = (inzold+1)*nx*ny;
00977 z6 = (inzold+2)*nx*ny;
00978 z7 = (inzold+3)*nx*ny;
00979 }
00980
00981 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00982 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00983 data[x7+y1+z1]*tablex7 ) * tabley1 +
00984 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00985 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00986 data[x7+y2+z1]*tablex7 ) * tabley2 +
00987 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00988 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00989 data[x7+y3+z1]*tablex7 ) * tabley3 +
00990 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00991 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00992 data[x7+y4+z1]*tablex7 ) * tabley4 +
00993 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
00994 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
00995 data[x7+y5+z1]*tablex7 ) * tabley5 +
00996 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
00997 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
00998 data[x7+y6+z1]*tablex7 ) * tabley6 +
00999 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01000 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01001 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01002 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01003 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01004 data[x7+y1+z2]*tablex7 ) * tabley1 +
01005 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01006 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01007 data[x7+y2+z2]*tablex7 ) * tabley2 +
01008 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01009 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01010 data[x7+y3+z2]*tablex7 ) * tabley3 +
01011 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01012 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01013 data[x7+y4+z2]*tablex7 ) * tabley4 +
01014 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01015 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01016 data[x7+y5+z2]*tablex7 ) * tabley5 +
01017 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01018 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01019 data[x7+y6+z2]*tablex7 ) * tabley6 +
01020 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01021 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01022 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01023 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01024 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01025 data[x7+y1+z3]*tablex7 ) * tabley1 +
01026 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01027 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01028 data[x7+y2+z3]*tablex7 ) * tabley2 +
01029 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01030 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01031 data[x7+y3+z3]*tablex7 ) * tabley3 +
01032 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01033 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01034 data[x7+y4+z3]*tablex7 ) * tabley4 +
01035 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01036 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01037 data[x7+y5+z3]*tablex7 ) * tabley5 +
01038 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01039 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01040 data[x7+y6+z3]*tablex7 ) * tabley6 +
01041 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01042 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01043 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01044 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01045 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01046 data[x7+y1+z4]*tablex7 ) * tabley1 +
01047 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01048 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01049 data[x7+y2+z4]*tablex7 ) * tabley2 +
01050 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01051 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01052 data[x7+y3+z4]*tablex7 ) * tabley3 +
01053 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01054 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01055 data[x7+y4+z4]*tablex7 ) * tabley4 +
01056 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01057 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01058 data[x7+y5+z4]*tablex7 ) * tabley5 +
01059 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01060 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01061 data[x7+y6+z4]*tablex7 ) * tabley6 +
01062 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01063 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01064 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01065 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01066 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01067 data[x7+y1+z5]*tablex7 ) * tabley1 +
01068 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01069 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01070 data[x7+y2+z5]*tablex7 ) * tabley2 +
01071 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01072 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01073 data[x7+y3+z5]*tablex7 ) * tabley3 +
01074 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01075 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01076 data[x7+y4+z5]*tablex7 ) * tabley4 +
01077 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01078 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01079 data[x7+y5+z5]*tablex7 ) * tabley5 +
01080 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01081 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01082 data[x7+y6+z5]*tablex7 ) * tabley6 +
01083 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01084 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01085 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01086 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01087 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01088 data[x7+y1+z6]*tablex7 ) * tabley1 +
01089 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01090 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01091 data[x7+y2+z6]*tablex7 ) * tabley2 +
01092 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01093 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01094 data[x7+y3+z6]*tablex7 ) * tabley3 +
01095 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01096 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01097 data[x7+y4+z6]*tablex7 ) * tabley4 +
01098 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01099 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01100 data[x7+y5+z6]*tablex7 ) * tabley5 +
01101 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01102 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01103 data[x7+y6+z6]*tablex7 ) * tabley6 +
01104 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01105 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01106 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01107 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01108 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01109 data[x7+y1+z7]*tablex7 ) * tabley1 +
01110 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01111 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01112 data[x7+y2+z7]*tablex7 ) * tabley2 +
01113 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01114 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01115 data[x7+y3+z7]*tablex7 ) * tabley3 +
01116 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01117 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01118 data[x7+y4+z7]*tablex7 ) * tabley4 +
01119 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01120 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01121 data[x7+y5+z7]*tablex7 ) * tabley5 +
01122 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01123 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01124 data[x7+y6+z7]*tablex7 ) * tabley6 +
01125 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01126 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01127 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01128
01129 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01130 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01131 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01132 }
01133 return pixel/w;
01134 }
01135
01136 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) {
01137 int K = kb.get_window_size();
01138 int kbmin = -K/2;
01139 int kbmax = -kbmin;
01140 int kbc = kbmax+1;
01141
01142 float pixel =0.0f;
01143 float w=0.0f;
01144
01145 float argdelx = delx;
01146 delx = restrict1(delx, nx);
01147 int inxold = int(round(delx));
01148 if ( ny < 2 ) {
01149 float tablex1 = kb.i0win_tab(delx-inxold+3);
01150 float tablex2 = kb.i0win_tab(delx-inxold+2);
01151 float tablex3 = kb.i0win_tab(delx-inxold+1);
01152 float tablex4 = kb.i0win_tab(delx-inxold);
01153 float tablex5 = kb.i0win_tab(delx-inxold-1);
01154 float tablex6 = kb.i0win_tab(delx-inxold-2);
01155 float tablex7 = kb.i0win_tab(delx-inxold-3);
01156
01157 int x1, x2, x3, x4, x5, x6, x7;
01158
01159 if ( inxold <= kbc || inxold >=nx-kbc-2 ) {
01160 x1 = (inxold-3+nx)%nx;
01161 x2 = (inxold-2+nx)%nx;
01162 x3 = (inxold-1+nx)%nx;
01163 x4 = (inxold +nx)%nx;
01164 x5 = (inxold+1+nx)%nx;
01165 x6 = (inxold+2+nx)%nx;
01166 x7 = (inxold+3+nx)%nx;
01167 } else {
01168 x1 = inxold-3;
01169 x2 = inxold-2;
01170 x3 = inxold-1;
01171 x4 = inxold;
01172 x5 = inxold+1;
01173 x6 = inxold+2;
01174 x7 = inxold+3;
01175 }
01176
01177 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01178 data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01179 data[x7]*tablex7 ;
01180
01181 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01182 } else if ( nz < 2 ) {
01183
01184 delx = argdelx;
01185
01186 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01187 delx = (float)xnew*2.0f;
01188 dely = (float)ynew*2.0f;
01189 }
01190
01191 int inxold = int(round(delx));
01192 int inyold = int(round(dely));
01193
01194 float tablex1 = kb.i0win_tab(delx-inxold+3);
01195 float tablex2 = kb.i0win_tab(delx-inxold+2);
01196 float tablex3 = kb.i0win_tab(delx-inxold+1);
01197 float tablex4 = kb.i0win_tab(delx-inxold);
01198 float tablex5 = kb.i0win_tab(delx-inxold-1);
01199 float tablex6 = kb.i0win_tab(delx-inxold-2);
01200 float tablex7 = kb.i0win_tab(delx-inxold-3);
01201
01202 float tabley1 = kb.i0win_tab(dely-inyold+3);
01203 float tabley2 = kb.i0win_tab(dely-inyold+2);
01204 float tabley3 = kb.i0win_tab(dely-inyold+1);
01205 float tabley4 = kb.i0win_tab(dely-inyold);
01206 float tabley5 = kb.i0win_tab(dely-inyold-1);
01207 float tabley6 = kb.i0win_tab(dely-inyold-2);
01208 float tabley7 = kb.i0win_tab(dely-inyold-3);
01209
01210 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01211
01212 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 ) {
01213 x1 = (inxold-3+nx)%nx;
01214 x2 = (inxold-2+nx)%nx;
01215 x3 = (inxold-1+nx)%nx;
01216 x4 = (inxold +nx)%nx;
01217 x5 = (inxold+1+nx)%nx;
01218 x6 = (inxold+2+nx)%nx;
01219 x7 = (inxold+3+nx)%nx;
01220
01221 y1 = ((inyold-3+ny)%ny)*nx;
01222 y2 = ((inyold-2+ny)%ny)*nx;
01223 y3 = ((inyold-1+ny)%ny)*nx;
01224 y4 = ((inyold +ny)%ny)*nx;
01225 y5 = ((inyold+1+ny)%ny)*nx;
01226 y6 = ((inyold+2+ny)%ny)*nx;
01227 y7 = ((inyold+3+ny)%ny)*nx;
01228 } else {
01229 x1 = inxold-3;
01230 x2 = inxold-2;
01231 x3 = inxold-1;
01232 x4 = inxold;
01233 x5 = inxold+1;
01234 x6 = inxold+2;
01235 x7 = inxold+3;
01236
01237 y1 = (inyold-3)*nx;
01238 y2 = (inyold-2)*nx;
01239 y3 = (inyold-1)*nx;
01240 y4 = inyold*nx;
01241 y5 = (inyold+1)*nx;
01242 y6 = (inyold+2)*nx;
01243 y7 = (inyold+3)*nx;
01244 }
01245
01246 pixel = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01247 data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01248 data[x7+y1]*tablex7 ) * tabley1 +
01249 ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01250 data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01251 data[x7+y2]*tablex7 ) * tabley2 +
01252 ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01253 data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01254 data[x7+y3]*tablex7 ) * tabley3 +
01255 ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01256 data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01257 data[x7+y4]*tablex7 ) * tabley4 +
01258 ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01259 data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01260 data[x7+y5]*tablex7 ) * tabley5 +
01261 ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01262 data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01263 data[x7+y6]*tablex7 ) * tabley6 +
01264 ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01265 data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01266 data[x7+y7]*tablex7 ) * tabley7;
01267
01268 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01269 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01270 } else {
01271 dely = restrict1(dely, ny);
01272 int inyold = int(Util::round(dely));
01273 delz = restrict1(delz, nz);
01274 int inzold = int(Util::round(delz));
01275
01276 float tablex1 = kb.i0win_tab(delx-inxold+3);
01277 float tablex2 = kb.i0win_tab(delx-inxold+2);
01278 float tablex3 = kb.i0win_tab(delx-inxold+1);
01279 float tablex4 = kb.i0win_tab(delx-inxold);
01280 float tablex5 = kb.i0win_tab(delx-inxold-1);
01281 float tablex6 = kb.i0win_tab(delx-inxold-2);
01282 float tablex7 = kb.i0win_tab(delx-inxold-3);
01283
01284 float tabley1 = kb.i0win_tab(dely-inyold+3);
01285 float tabley2 = kb.i0win_tab(dely-inyold+2);
01286 float tabley3 = kb.i0win_tab(dely-inyold+1);
01287 float tabley4 = kb.i0win_tab(dely-inyold);
01288 float tabley5 = kb.i0win_tab(dely-inyold-1);
01289 float tabley6 = kb.i0win_tab(dely-inyold-2);
01290 float tabley7 = kb.i0win_tab(dely-inyold-3);
01291
01292 float tablez1 = kb.i0win_tab(delz-inzold+3);
01293 float tablez2 = kb.i0win_tab(delz-inzold+2);
01294 float tablez3 = kb.i0win_tab(delz-inzold+1);
01295 float tablez4 = kb.i0win_tab(delz-inzold);
01296 float tablez5 = kb.i0win_tab(delz-inzold-1);
01297 float tablez6 = kb.i0win_tab(delz-inzold-2);
01298 float tablez7 = kb.i0win_tab(delz-inzold-3);
01299
01300 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01301
01302 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 ) {
01303 x1 = (inxold-3+nx)%nx;
01304 x2 = (inxold-2+nx)%nx;
01305 x3 = (inxold-1+nx)%nx;
01306 x4 = (inxold +nx)%nx;
01307 x5 = (inxold+1+nx)%nx;
01308 x6 = (inxold+2+nx)%nx;
01309 x7 = (inxold+3+nx)%nx;
01310
01311 y1 = ((inyold-3+ny)%ny)*nx;
01312 y2 = ((inyold-2+ny)%ny)*nx;
01313 y3 = ((inyold-1+ny)%ny)*nx;
01314 y4 = ((inyold +ny)%ny)*nx;
01315 y5 = ((inyold+1+ny)%ny)*nx;
01316 y6 = ((inyold+2+ny)%ny)*nx;
01317 y7 = ((inyold+3+ny)%ny)*nx;
01318
01319 z1 = ((inzold-3+nz)%nz)*nx*ny;
01320 z2 = ((inzold-2+nz)%nz)*nx*ny;
01321 z3 = ((inzold-1+nz)%nz)*nx*ny;
01322 z4 = ((inzold +nz)%nz)*nx*ny;
01323 z5 = ((inzold+1+nz)%nz)*nx*ny;
01324 z6 = ((inzold+2+nz)%nz)*nx*ny;
01325 z7 = ((inzold+3+nz)%nz)*nx*ny;
01326 } else {
01327 x1 = inxold-3;
01328 x2 = inxold-2;
01329 x3 = inxold-1;
01330 x4 = inxold;
01331 x5 = inxold+1;
01332 x6 = inxold+2;
01333 x7 = inxold+3;
01334
01335 y1 = (inyold-3)*nx;
01336 y2 = (inyold-2)*nx;
01337 y3 = (inyold-1)*nx;
01338 y4 = inyold*nx;
01339 y5 = (inyold+1)*nx;
01340 y6 = (inyold+2)*nx;
01341 y7 = (inyold+3)*nx;
01342
01343 z1 = (inzold-3)*nx*ny;
01344 z2 = (inzold-2)*nx*ny;
01345 z3 = (inzold-1)*nx*ny;
01346 z4 = inzold*nx*ny;
01347 z5 = (inzold+1)*nx*ny;
01348 z6 = (inzold+2)*nx*ny;
01349 z7 = (inzold+3)*nx*ny;
01350 }
01351
01352 pixel = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01353 data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01354 data[x7+y1+z1]*tablex7 ) * tabley1 +
01355 ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01356 data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01357 data[x7+y2+z1]*tablex7 ) * tabley2 +
01358 ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01359 data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01360 data[x7+y3+z1]*tablex7 ) * tabley3 +
01361 ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01362 data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01363 data[x7+y4+z1]*tablex7 ) * tabley4 +
01364 ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01365 data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01366 data[x7+y5+z1]*tablex7 ) * tabley5 +
01367 ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01368 data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01369 data[x7+y6+z1]*tablex7 ) * tabley6 +
01370 ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01371 data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01372 data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01373 ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01374 data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01375 data[x7+y1+z2]*tablex7 ) * tabley1 +
01376 ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01377 data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01378 data[x7+y2+z2]*tablex7 ) * tabley2 +
01379 ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01380 data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01381 data[x7+y3+z2]*tablex7 ) * tabley3 +
01382 ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01383 data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01384 data[x7+y4+z2]*tablex7 ) * tabley4 +
01385 ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01386 data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01387 data[x7+y5+z2]*tablex7 ) * tabley5 +
01388 ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01389 data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01390 data[x7+y6+z2]*tablex7 ) * tabley6 +
01391 ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01392 data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01393 data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01394 ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01395 data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01396 data[x7+y1+z3]*tablex7 ) * tabley1 +
01397 ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01398 data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01399 data[x7+y2+z3]*tablex7 ) * tabley2 +
01400 ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01401 data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01402 data[x7+y3+z3]*tablex7 ) * tabley3 +
01403 ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01404 data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01405 data[x7+y4+z3]*tablex7 ) * tabley4 +
01406 ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01407 data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01408 data[x7+y5+z3]*tablex7 ) * tabley5 +
01409 ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01410 data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01411 data[x7+y6+z3]*tablex7 ) * tabley6 +
01412 ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01413 data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01414 data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01415 ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01416 data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01417 data[x7+y1+z4]*tablex7 ) * tabley1 +
01418 ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01419 data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01420 data[x7+y2+z4]*tablex7 ) * tabley2 +
01421 ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01422 data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01423 data[x7+y3+z4]*tablex7 ) * tabley3 +
01424 ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01425 data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01426 data[x7+y4+z4]*tablex7 ) * tabley4 +
01427 ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01428 data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01429 data[x7+y5+z4]*tablex7 ) * tabley5 +
01430 ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01431 data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01432 data[x7+y6+z4]*tablex7 ) * tabley6 +
01433 ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01434 data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01435 data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01436 ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01437 data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01438 data[x7+y1+z5]*tablex7 ) * tabley1 +
01439 ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01440 data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01441 data[x7+y2+z5]*tablex7 ) * tabley2 +
01442 ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01443 data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01444 data[x7+y3+z5]*tablex7 ) * tabley3 +
01445 ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01446 data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01447 data[x7+y4+z5]*tablex7 ) * tabley4 +
01448 ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01449 data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01450 data[x7+y5+z5]*tablex7 ) * tabley5 +
01451 ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01452 data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01453 data[x7+y6+z5]*tablex7 ) * tabley6 +
01454 ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01455 data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01456 data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01457 ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01458 data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01459 data[x7+y1+z6]*tablex7 ) * tabley1 +
01460 ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01461 data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01462 data[x7+y2+z6]*tablex7 ) * tabley2 +
01463 ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01464 data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01465 data[x7+y3+z6]*tablex7 ) * tabley3 +
01466 ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01467 data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01468 data[x7+y4+z6]*tablex7 ) * tabley4 +
01469 ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01470 data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01471 data[x7+y5+z6]*tablex7 ) * tabley5 +
01472 ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01473 data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01474 data[x7+y6+z6]*tablex7 ) * tabley6 +
01475 ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01476 data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01477 data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01478 ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01479 data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01480 data[x7+y1+z7]*tablex7 ) * tabley1 +
01481 ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01482 data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01483 data[x7+y2+z7]*tablex7 ) * tabley2 +
01484 ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01485 data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01486 data[x7+y3+z7]*tablex7 ) * tabley3 +
01487 ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01488 data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01489 data[x7+y4+z7]*tablex7 ) * tabley4 +
01490 ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01491 data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01492 data[x7+y5+z7]*tablex7 ) * tabley5 +
01493 ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01494 data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01495 data[x7+y6+z7]*tablex7 ) * tabley6 +
01496 ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01497 data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01498 data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01499
01500 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01501 (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01502 (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01503 }
01504 return pixel/w;
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
01685 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01686
01687 int nxreal = nx - 2;
01688 if (nxreal != ny)
01689 throw ImageDimensionException("extractpoint requires ny == nx");
01690 int nhalf = nxreal/2;
01691 bool flip = (nuxnew < 0.f);
01692 if (flip) {
01693 nuxnew *= -1;
01694 nuynew *= -1;
01695 }
01696 if (nuynew >= nhalf-0.5) {
01697 nuynew -= nxreal;
01698 } else if (nuynew < -nhalf-0.5) {
01699 nuynew += nxreal;
01700 }
01701
01702
01703
01704
01705 int ixn = int(Util::round(nuxnew));
01706 int iyn = int(Util::round(nuynew));
01707
01708
01709 static float wy[7];
01710 static float wx[7];
01711
01712 float iynn = nuynew - iyn;
01713 wy[0] = kb.i0win_tab(iynn+3);
01714 wy[1] = kb.i0win_tab(iynn+2);
01715 wy[2] = kb.i0win_tab(iynn+1);
01716 wy[3] = kb.i0win_tab(iynn);
01717 wy[4] = kb.i0win_tab(iynn-1);
01718 wy[5] = kb.i0win_tab(iynn-2);
01719 wy[6] = kb.i0win_tab(iynn-3);
01720
01721 float ixnn = nuxnew - ixn;
01722 wx[0] = kb.i0win_tab(ixnn+3);
01723 wx[1] = kb.i0win_tab(ixnn+2);
01724 wx[2] = kb.i0win_tab(ixnn+1);
01725 wx[3] = kb.i0win_tab(ixnn);
01726 wx[4] = kb.i0win_tab(ixnn-1);
01727 wx[5] = kb.i0win_tab(ixnn-2);
01728 wx[6] = kb.i0win_tab(ixnn-3);
01729
01730 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]);
01731
01732 complex<float> result(0.f,0.f);
01733 if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01734
01735 for (int iy = 0; iy < 7; iy++) {
01736 int iyp = iyn + iy - 3 ;
01737 for (int ix = 0; ix < 7; ix++) {
01738 int ixp = ixn + ix - 3;
01739 float w = wx[ix]*wy[iy];
01740 complex<float> val = fimage->cmplx(ixp,iyp);
01741 result += val*w;
01742 }
01743 }
01744 } else {
01745
01746 for (int iy = 0; iy < 7; iy++) {
01747 int iyp = iyn + iy - 3;
01748 for (int ix = 0; ix < 7; ix++) {
01749 int ixp = ixn + ix - 3;
01750 bool mirror = false;
01751 int ixt = ixp, iyt = iyp;
01752 if (ixt < 0) {
01753 ixt = -ixt;
01754 iyt = -iyt;
01755 mirror = !mirror;
01756 }
01757 if (ixt > nhalf) {
01758 ixt = nxreal - ixt;
01759 iyt = -iyt;
01760 mirror = !mirror;
01761 }
01762 if (iyt > nhalf-1) iyt -= nxreal;
01763 if (iyt < -nhalf) iyt += nxreal;
01764 float w = wx[ix]*wy[iy];
01765 complex<float> val = fimage->cmplx(ixt,iyt);
01766 if (mirror) result += conj(val)*w;
01767 else result += val*w;
01768 }
01769 }
01770 }
01771 if (flip) result = conj(result)/wsum;
01772 else result /= wsum;
01773 return result;
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
01904 float Util::triquad(float R, float S, float T, float* fdata)
01905 {
01906
01907 const float C2 = 0.5f;
01908 const float C4 = 0.25f;
01909 const float C8 = 0.125f;
01910
01911 float RS = R * S;
01912 float ST = S * T;
01913 float RT = R * T;
01914 float RST = R * ST;
01915
01916 float RSQ = 1-R*R;
01917 float SSQ = 1-S*S;
01918 float TSQ = 1-T*T;
01919
01920 float RM1 = (1-R);
01921 float SM1 = (1-S);
01922 float TM1 = (1-T);
01923
01924 float RP1 = (1+R);
01925 float SP1 = (1+S);
01926 float TP1 = (1+T);
01927
01928 float triquad =
01929 (-C8) * RST * RM1 * SM1 * TM1 * fdata[0] +
01930 ( C4) * ST * RSQ * SM1 * TM1 * fdata[1] +
01931 ( C8) * RST * RP1 * SM1 * TM1 * fdata[2] +
01932 ( C4) * RT * RM1 * SSQ * TM1 * fdata[3] +
01933 (-C2) * T * RSQ * SSQ * TM1 * fdata[4] +
01934 (-C4) * RT * RP1 * SSQ * TM1 * fdata[5] +
01935 ( C8) * RST * RM1 * SP1 * TM1 * fdata[6] +
01936 (-C4) * ST * RSQ * SP1 * TM1 * fdata[7] +
01937 (-C8) * RST * RP1 * SP1 * TM1 * fdata[8] +
01938
01939 ( C4) * RS * RM1 * SM1 * TSQ * fdata[9] +
01940 (-C2) * S * RSQ * SM1 * TSQ * fdata[10] +
01941 (-C4) * RS * RP1 * SM1 * TSQ * fdata[11] +
01942 (-C2) * R * RM1 * SSQ * TSQ * fdata[12] +
01943 RSQ * SSQ * TSQ * fdata[13] +
01944 ( C2) * R * RP1 * SSQ * TSQ * fdata[14] +
01945 (-C4) * RS * RM1 * SP1 * TSQ * fdata[15] +
01946 ( C2) * S * RSQ * SP1 * TSQ * fdata[16] +
01947 ( C4) * RS * RP1 * SP1 * TSQ * fdata[17] +
01948
01949 ( C8) * RST * RM1 * SM1 * TP1 * fdata[18] +
01950 (-C4) * ST * RSQ * SM1 * TP1 * fdata[19] +
01951 (-C8) * RST * RP1 * SM1 * TP1 * fdata[20] +
01952 (-C4) * RT * RM1 * SSQ * TP1 * fdata[21] +
01953 ( C2) * T * RSQ * SSQ * TP1 * fdata[22] +
01954 ( C4) * RT * RP1 * SSQ * TP1 * fdata[23] +
01955 (-C8) * RST * RM1 * SP1 * TP1 * fdata[24] +
01956 ( C4) * ST * RSQ * SP1 * TP1 * fdata[25] +
01957 ( C8) * RST * RP1 * SP1 * TP1 * fdata[26] ;
01958 return triquad;
01959 }
01960
01961 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01962 : M(M_), fc(fc_), ntable(ntable_) {
01963
01964 build_sBtable();
01965 }
01966
01967 void Util::sincBlackman::build_sBtable() {
01968 sBtable.resize(ntable+1);
01969 int ltab = int(round(float(ntable)/1.25f));
01970 int M2 = M/2;
01971 fltb = float(ltab)/M2;
01972 for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01973 float x = 1.0e-7f;
01974 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)));
01975 for (int i=1; i <= ltab; i++) {
01976 x = float(i)/fltb;
01977 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)));
01978
01979 }
01980 }
01981
01982 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01983 int N_, float vtable_, int ntable_)
01984 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01985 ntable(ntable_) {
01986
01987 if (0.f == v) v = float(K)/2;
01988 if (0.f == vtable) vtable = v;
01989 alphar = alpha*r;
01990 fac = static_cast<float>(twopi)*alphar*v;
01991 vadjust = 1.0f*v;
01992 facadj = static_cast<float>(twopi)*alphar*vadjust;
01993 build_I0table();
01994 }
01995
01996 float Util::KaiserBessel::i0win(float x) const {
01997 float val0 = float(gsl_sf_bessel_I0(facadj));
01998 float absx = fabs(x);
01999 if (absx > vadjust) return 0.f;
02000 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02001 float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02002 return res;
02003 }
02004
02005 void Util::KaiserBessel::build_I0table() {
02006 i0table.resize(ntable+1);
02007 int ltab = int(round(float(ntable)/1.25f));
02008 fltb = float(ltab)/(K/2);
02009 float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02010 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02011 for (int i=0; i <= ltab; i++) {
02012 float s = float(i)/fltb/N;
02013 if (s < vadjust) {
02014 float rt = sqrt(1.f - pow(s/vadjust, 2));
02015 i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02016 } else {
02017 i0table[i] = 0.f;
02018 }
02019
02020 }
02021 }
02022
02023 float Util::KaiserBessel::I0table_maxerror() {
02024 float maxdiff = 0.f;
02025 for (int i = 1; i <= ntable; i++) {
02026 float diff = fabs(i0table[i] - i0table[i-1]);
02027 if (diff > maxdiff) maxdiff = diff;
02028 }
02029 return maxdiff;
02030 }
02031
02032 float Util::KaiserBessel::sinhwin(float x) const {
02033 float val0 = sinh(fac)/fac;
02034 float absx = fabs(x);
02035 if (0.0 == x) {
02036 float res = 1.0f;
02037 return res;
02038 } else if (absx == alphar) {
02039 return 1.0f/val0;
02040 } else if (absx < alphar) {
02041 float rt = sqrt(1.0f - pow((x/alphar), 2));
02042 float facrt = fac*rt;
02043 float res = (sinh(facrt)/facrt)/val0;
02044 return res;
02045 } else {
02046 float rt = sqrt(pow((x/alphar),2) - 1.f);
02047 float facrt = fac*rt;
02048 float res = (sin(facrt)/facrt)/val0;
02049 return res;
02050 }
02051 }
02052
02053 float Util::FakeKaiserBessel::i0win(float x) const {
02054 float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02055 float absx = fabs(x);
02056 if (absx > vadjust) return 0.f;
02057 float rt = sqrt(1.f - pow(absx/vadjust, 2));
02058 float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02059 return res;
02060 }
02061
02062 void Util::FakeKaiserBessel::build_I0table() {
02063 i0table.resize(ntable+1);
02064 int ltab = int(round(float(ntable)/1.1f));
02065 fltb = float(ltab)/(K/2);
02066 float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02067 for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02068 for (int i=0; i <= ltab; i++) {
02069 float s = float(i)/fltb/N;
02070 if (s < vadjust) {
02071 float rt = sqrt(1.f - pow(s/vadjust, 2));
02072 i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02073 } else {
02074 i0table[i] = 0.f;
02075 }
02076 }
02077 }
02078
02079 float Util::FakeKaiserBessel::sinhwin(float x) const {
02080 float val0 = sinh(fac)/fac;
02081 float absx = fabs(x);
02082 if (0.0 == x) {
02083 float res = 1.0f;
02084 return res;
02085 } else if (absx == alphar) {
02086 return 1.0f/val0;
02087 } else if (absx < alphar) {
02088 float rt = sqrt(1.0f - pow((x/alphar), 2));
02089 float facrt = fac*rt;
02090 float res = (sinh(facrt)/facrt)/val0;
02091 return res;
02092 } else {
02093 float rt = sqrt(pow((x/alphar),2) - 1.f);
02094 float facrt = fac*rt;
02095 float res = (sin(facrt)/facrt)/val0;
02096 return res;
02097 }
02098 }
02099
02100 #if 0 // 1-st order KB window
02101 float Util::FakeKaiserBessel::sinhwin(float x) const {
02102
02103 float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02104 float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02105 float absx = fabs(x);
02106 if (0.0 == x) {
02107
02108 float res = val0;
02109 return res;
02110 } else if (absx == alphar) {
02111
02112 return prefix;
02113 } else if (absx < alphar) {
02114 float rt = sqrt(1.0f - pow((x/alphar), 2));
02115
02116 float facrt = facadj*rt;
02117
02118 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02119 return res;
02120 } else {
02121 float rt = sqrt(pow((x/alphar),2) - 1.f);
02122
02123 float facrt = facadj*rt;
02124
02125 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02126 return res;
02127 }
02128 }
02129 #endif // 0
02130
02131
02132
02133 #define circ(i) circ[i-1]
02134 #define numr(i,j) numr[(j-1)*3 + i-1]
02135 #define xim(i,j) xim[(j-1)*nsam + i-1]
02136
02137 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02138 int nsam = image->get_xsize();
02139 int nrow = image->get_ysize();
02140 int nring = numr.size()/3;
02141 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02142 EMData* out = new EMData();
02143 out->set_size(lcirc,1,1);
02144 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02145 float *xim = image->get_data();
02146 float *circ = out->get_data();
02147
02148
02149
02150
02151
02152
02153
02154
02155
02156
02157
02158
02159
02160
02161
02162
02163 double dfi, dpi;
02164 int ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02165 float yq, xold, yold, fi, x, y;
02166
02167 ns2 = nsam/2+1;
02168 nr2 = nrow/2+1;
02169 dpi = 2.0*atan(1.0);
02170
02171 for (i=1;i<=nring;i++) {
02172
02173 inr = numr(1,i);
02174 yq = static_cast<float>(inr);
02175 l = numr(3,i);
02176 if (mode == 'h' || mode == 'H') lt = l/2;
02177 else lt = l/4;
02178
02179 nsim = lt-1;
02180 dfi = dpi/(nsim+1);
02181 kcirc = numr(2,i);
02182 xold = 0.0f;
02183 yold = static_cast<float>(inr);
02184 circ(kcirc) = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02185 xold = static_cast<float>(inr);
02186 yold = 0.0f;
02187 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02188
02189 if (mode == 'f' || mode == 'F') {
02190 xold = 0.0f;
02191 yold = static_cast<float>(-inr);
02192 circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02193 xold = static_cast<float>(-inr);
02194 yold = 0.0f;
02195 circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02196 }
02197
02198 for (j=1;j<=nsim;j++) {
02199 fi = static_cast<float>(dfi*j);
02200 x = sin(fi)*yq;
02201 y = cos(fi)*yq;
02202 xold = x;
02203 yold = y;
02204 circ(j+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02205 xold = y;
02206 yold = -x;
02207 circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02208
02209 if (mode == 'f' || mode == 'F') {
02210 xold = -x;
02211 yold = -y;
02212 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02213 xold = -y;
02214 yold = x;
02215 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02216 }
02217 }
02218 }
02219 return out;
02220 }
02221
02222 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02223 int nsam = image->get_xsize();
02224 int nrow = image->get_ysize();
02225 int nring = numr.size()/3;
02226 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02227 EMData* out = new EMData();
02228 out->set_size(lcirc,1,1);
02229 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02230 float *xim = image->get_data();
02231 float *circ = out->get_data();
02232 double dpi, dfi;
02233 int it, jt, inr, l, nsim, kcirc, lt;
02234 float xold, yold, fi, x, y;
02235
02236
02237
02238 dpi = 2*atan(1.0);
02239 for (it=1; it<=nring; it++) {
02240
02241 inr = numr(1,it);
02242
02243
02244
02245
02246 l = numr(3,it);
02247 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02248 else lt = l / 4;
02249
02250 nsim = lt - 1;
02251 dfi = dpi / (nsim+1);
02252 kcirc = numr(2,it);
02253 xold = 0.0f+cns2;
02254 yold = inr+cnr2;
02255
02256 Assert( kcirc <= lcirc );
02257 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02258
02259 xold = inr+cns2;
02260 yold = 0.0f+cnr2;
02261 Assert( lt+kcirc <= lcirc );
02262 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02263
02264 if ( mode == 'f' || mode == 'F' ) {
02265 xold = 0.0f+cns2;
02266 yold = -inr+cnr2;
02267 Assert( lt+lt+kcirc <= lcirc );
02268 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02269
02270 xold = -inr+cns2;
02271 yold = 0.0f+cnr2;
02272 Assert(lt+lt+lt+kcirc <= lcirc );
02273 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02274 }
02275
02276 for (jt=1; jt<=nsim; jt++) {
02277 fi = static_cast<float>(dfi * jt);
02278 x = sin(fi) * inr;
02279 y = cos(fi) * inr;
02280
02281 xold = x+cns2;
02282 yold = y+cnr2;
02283
02284 Assert( jt+kcirc <= lcirc );
02285 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02286
02287 xold = y+cns2;
02288 yold = -x+cnr2;
02289
02290 Assert( jt+lt+kcirc <= lcirc );
02291 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02292
02293 if ( mode == 'f' || mode == 'F' ) {
02294 xold = -x+cns2;
02295 yold = -y+cnr2;
02296
02297 Assert( jt+lt+lt+kcirc <= lcirc );
02298 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02299
02300 xold = -y+cns2;
02301 yold = x+cnr2;
02302
02303 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02304 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02305 }
02306 }
02307 }
02308 return out;
02309 }
02310
02311 float Util::bilinear(float xold, float yold, int nsam, int nrow, float* xim)
02312 {
02313
02314
02315
02316
02317 float bilinear;
02318 int ixold, iyold;
02319
02320
02321
02322
02323
02324
02325
02326
02327
02328
02329
02330
02331
02332
02333
02334
02335
02336
02337
02338
02339
02340
02341
02342 float xdif, ydif;
02343
02344 ixold = (int) xold;
02345 iyold = (int) yold;
02346 ydif = yold - iyold;
02347
02348
02349
02350
02351
02352 xdif = xold - ixold;
02353 bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02354 xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02355 ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02356
02357 return bilinear;
02358 }
02359
02360 void Util::alrl_ms(float *xim, int nsam, int nrow, float cns2, float cnr2,
02361 int *numr, float *circ, int , int nring, char mode) {
02362 double dpi, dfi;
02363 int it, jt, inr, l, nsim, kcirc, lt;
02364 float xold, yold, fi, x, y;
02365
02366
02367
02368
02369 dpi = 2*atan(1.0);
02370 for (it=1; it<=nring; it++) {
02371
02372 inr = numr(1,it);
02373
02374 l = numr(3,it);
02375 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02376 else lt = l / 4;
02377
02378 nsim = lt - 1;
02379 dfi = dpi / (nsim+1);
02380 kcirc = numr(2,it);
02381
02382
02383 xold = 0.0f+cns2;
02384 yold = inr+cnr2;
02385
02386 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02387
02388 xold = inr+cns2;
02389 yold = 0.0f+cnr2;
02390 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02391
02392 if ( mode == 'f' || mode == 'F' ) {
02393 xold = 0.0f+cns2;
02394 yold = -inr+cnr2;
02395 circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02396
02397 xold = -inr+cns2;
02398 yold = 0.0f+cnr2;
02399 circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02400 }
02401
02402 for (jt=1; jt<=nsim; jt++) {
02403 fi = static_cast<float>(dfi * jt);
02404 x = sin(fi) * inr;
02405 y = cos(fi) * inr;
02406
02407 xold = x+cns2;
02408 yold = y+cnr2;
02409 circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02410
02411 xold = y+cns2;
02412 yold = -x+cnr2;
02413 circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02414
02415 if ( mode == 'f' || mode == 'F' ) {
02416 xold = -x+cns2;
02417 yold = -y+cnr2;
02418 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02419
02420 xold = -y+cns2;
02421 yold = x+cnr2;
02422 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
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
02503 #undef xim
02504
02505 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02506
02507 int nring = numr.size()/3;
02508 int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02509 EMData* out = new EMData();
02510 out->set_size(lcirc,1,1);
02511 char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02512 float *circ = out->get_data();
02513 float *fimage = image->get_data();
02514 int nx = image->get_xsize();
02515 int ny = image->get_ysize();
02516 int nz = image->get_zsize();
02517 double dpi, dfi;
02518 int it, jt, inr, l, nsim, kcirc, lt;
02519 float yq, xold, yold, fi, x, y;
02520
02521
02522
02523
02524 dpi = 2*atan(1.0);
02525 for (it=1;it<=nring;it++) {
02526
02527 inr = numr(1,it);
02528 yq = static_cast<float>(inr);
02529
02530 l = numr(3,it);
02531 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02532 else lt = l / 4;
02533
02534 nsim = lt - 1;
02535 dfi = dpi / (nsim+1);
02536 kcirc = numr(2,it);
02537 xold = 0.0f;
02538 yold = static_cast<float>(inr);
02539 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02540
02541
02542 xold = static_cast<float>(inr);
02543 yold = 0.0f;
02544 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02545
02546
02547 if ( mode == 'f' || mode == 'F' ) {
02548 xold = 0.0f;
02549 yold = static_cast<float>(-inr);
02550 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02551
02552
02553 xold = static_cast<float>(-inr);
02554 yold = 0.0f;
02555 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);
02556
02557 }
02558
02559 for (jt=1;jt<=nsim;jt++) {
02560 fi = static_cast<float>(dfi * jt);
02561 x = sin(fi) * yq;
02562 y = cos(fi) * yq;
02563
02564 xold = x;
02565 yold = y;
02566 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02567
02568
02569 xold = y;
02570 yold = -x;
02571 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02572
02573
02574 if ( mode == 'f' || mode == 'F' ) {
02575 xold = -x;
02576 yold = -y;
02577 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);
02578
02579
02580 xold = -y;
02581 yold = x;
02582 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);
02583
02584 }
02585 }
02586 }
02587 return out;
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
02613 #define tab1(i) tab1[i-1]
02614 #define xcmplx(i,j) xcmplx [(j-1)*2 + i-1]
02615 #define br(i) br[i-1]
02616 #define bi(i) bi[i-1]
02617
02618 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02619 {
02620 double rni,sgn,tr1,tr2,ti1,ti2;
02621 double cc,c,ss,s,t,x2,x3,x4,x5;
02622 int b3,b4,b5,b6,b7,b56;
02623 int n, k, l, j, i, ix0, ix1, status=0;
02624
02625 const double tab1[] = {
02626 9.58737990959775e-5,
02627 1.91747597310703e-4,
02628 3.83495187571395e-4,
02629 7.66990318742704e-4,
02630 1.53398018628476e-3,
02631 3.06795676296598e-3,
02632 6.13588464915449e-3,
02633 1.22715382857199e-2,
02634 2.45412285229123e-2,
02635 4.90676743274181e-2,
02636 9.80171403295604e-2,
02637 1.95090322016128e-1,
02638 3.82683432365090e-1,
02639 7.07106781186546e-1,
02640 1.00000000000000,
02641 };
02642
02643 n=(int)pow(2.0f,ln);
02644
02645 k=abs(ks);
02646 l=16-ln;
02647 b3=n*k;
02648 b6=b3;
02649 b7=k;
02650 if (ks > 0) {
02651 sgn=1.0f;
02652 } else {
02653 sgn=-1.0f;
02654 rni=1.0f/(float)(n);
02655 j=1;
02656 for (i=1; i<=n; i++) {
02657 br(j)=br(j)*rni;
02658 bi(j)=bi(j)*rni;
02659 j=j+k;
02660 }
02661 }
02662
02663 L12:
02664 b6=b6/2;
02665 b5=b6;
02666 b4=2*b6;
02667 b56=b5-b6;
02668
02669 L14:
02670 tr1=br(b5+1);
02671 ti1=bi(b5+1);
02672 tr2=br(b56+1);
02673 ti2=bi(b56+1);
02674
02675 br(b5+1)=tr2-tr1;
02676 bi(b5+1)=ti2-ti1;
02677 br(b56+1)=tr1+tr2;
02678 bi(b56+1)=ti1+ti2;
02679
02680 b5=b5+b4;
02681 b56=b5-b6;
02682 if ( b5 <= b3 ) goto L14;
02683 if ( b6 == b7 ) goto L20;
02684
02685 b4=b7;
02686 cc=2.0f*pow(tab1(l),2);
02687 c=1.0f-cc;
02688 l++;
02689 ss=sgn*tab1(l);
02690 s=ss;
02691
02692 L16:
02693 b5=b6+b4;
02694 b4=2*b6;
02695 b56=b5-b6;
02696
02697 L18:
02698 tr1=br(b5+1);
02699 ti1=bi(b5+1);
02700 tr2=br(b56+1);
02701 ti2=bi(b56+1);
02702 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02703 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02704 br(b56+1)=tr1+tr2;
02705 bi(b56+1)=ti1+ti2;
02706
02707 b5=b5+b4;
02708 b56=b5-b6;
02709 if ( b5 <= b3 ) goto L18;
02710 b4=b5-b6;
02711 b5=b4-b3;
02712 c=-c;
02713 b4=b6-b5;
02714 if ( b5 < b4 ) goto L16;
02715 b4=b4+b7;
02716 if ( b4 >= b5 ) goto L12;
02717
02718 t=c-cc*c-ss*s;
02719 s=s+ss*c-cc*s;
02720 c=t;
02721 goto L16;
02722
02723 L20:
02724 ix0=b3/2;
02725 b3=b3-b7;
02726 b4=0;
02727 b5=0;
02728 b6=ix0;
02729 ix1=0;
02730 if (b6 == b7) goto EXIT;
02731
02732 L22:
02733 b4=b3-b4;
02734 b5=b3-b5;
02735 x2=br(b4+1);
02736 x3=br(b5+1);
02737 x4=bi(b4+1);
02738 x5=bi(b5+1);
02739 br(b4+1)=x3;
02740 br(b5+1)=x2;
02741 bi(b4+1)=x5;
02742 bi(b5+1)=x4;
02743 if(b6 < b4) goto L22;
02744
02745 L24:
02746 b4=b4+b7;
02747 b5=b6+b5;
02748 x2=br(b4+1);
02749 x3=br(b5+1);
02750 x4=bi(b4+1);
02751 x5=bi(b5+1);
02752 br(b4+1)=x3;
02753 br(b5+1)=x2;
02754 bi(b4+1)=x5;
02755 bi(b5+1)=x4;
02756 ix0=b6;
02757
02758 L26:
02759 ix0=ix0/2;
02760 ix1=ix1-ix0;
02761 if( ix1 >= 0) goto L26;
02762
02763 ix0=2*ix0;
02764 b4=b4+b7;
02765 ix1=ix1+ix0;
02766 b5=ix1;
02767 if ( b5 >= b4) goto L22;
02768 if ( b4 < b6) goto L24;
02769
02770 EXIT:
02771 status = 0;
02772 }
02773
02774
02775 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02776 {
02777
02778
02779 int b3,b4,b5,b6,b7,b56;
02780 int n, k, l, j, i, ix0, ix1;
02781 float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02782 int status=0;
02783
02784 const float tab1[] = {
02785 9.58737990959775e-5f,
02786 1.91747597310703e-4f,
02787 3.83495187571395e-4f,
02788 7.66990318742704e-4f,
02789 1.53398018628476e-3f,
02790 3.06795676296598e-3f,
02791 6.13588464915449e-3f,
02792 1.22715382857199e-2f,
02793 2.45412285229123e-2f,
02794 4.90676743274181e-2f,
02795 9.80171403295604e-2f,
02796 1.95090322016128e-1f,
02797 3.82683432365090e-1f,
02798 7.07106781186546e-1f,
02799 1.00000000000000f,
02800 };
02801
02802 n=(int)pow(2.0f,ln);
02803
02804 k=abs(ks);
02805 l=16-ln;
02806 b3=n*k;
02807 b6=b3;
02808 b7=k;
02809 if( ks > 0 ) {
02810 sgn=1.0f;
02811 } else {
02812 sgn=-1.0f;
02813 rni=1.0f/(float)n;
02814 j=1;
02815 for (i=1; i<=n; i++) {
02816 br(j)=br(j)*rni;
02817 bi(j)=bi(j)*rni;
02818 j=j+k;
02819 }
02820 }
02821 L12:
02822 b6=b6/2;
02823 b5=b6;
02824 b4=2*b6;
02825 b56=b5-b6;
02826 L14:
02827 tr1=br(b5+1);
02828 ti1=bi(b5+1);
02829
02830 tr2=br(b56+1);
02831 ti2=bi(b56+1);
02832
02833 br(b5+1)=tr2-tr1;
02834 bi(b5+1)=ti2-ti1;
02835 br(b56+1)=tr1+tr2;
02836 bi(b56+1)=ti1+ti2;
02837
02838 b5=b5+b4;
02839 b56=b5-b6;
02840 if ( b5 <= b3 ) goto L14;
02841 if ( b6 == b7 ) goto L20;
02842
02843 b4=b7;
02844 cc=2.0f*pow(tab1(l),2);
02845 c=1.0f-cc;
02846 l++;
02847 ss=sgn*tab1(l);
02848 s=ss;
02849 L16:
02850 b5=b6+b4;
02851 b4=2*b6;
02852 b56=b5-b6;
02853 L18:
02854 tr1=br(b5+1);
02855 ti1=bi(b5+1);
02856 tr2=br(b56+1);
02857 ti2=bi(b56+1);
02858 br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02859 bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02860 br(b56+1)=tr1+tr2;
02861 bi(b56+1)=ti1+ti2;
02862
02863 b5=b5+b4;
02864 b56=b5-b6;
02865 if(b5 <= b3) goto L18;
02866 b4=b5-b6;
02867 b5=b4-b3;
02868 c=-c;
02869 b4=b6-b5;
02870 if(b5 < b4) goto L16;
02871 b4=b4+b7;
02872 if(b4 >= b5) goto L12;
02873
02874 t=c-cc*c-ss*s;
02875 s=s+ss*c-cc*s;
02876 c=t;
02877 goto L16;
02878 L20:
02879 ix0=b3/2;
02880 b3=b3-b7;
02881 b4=0;
02882 b5=0;
02883 b6=ix0;
02884 ix1=0;
02885 if ( b6 == b7) goto EXIT;
02886 L22:
02887 b4=b3-b4;
02888 b5=b3-b5;
02889 x2=br(b4+1);
02890 x3=br(b5+1);
02891 x4=bi(b4+1);
02892 x5=bi(b5+1);
02893 br(b4+1)=x3;
02894 br(b5+1)=x2;
02895 bi(b4+1)=x5;
02896 bi(b5+1)=x4;
02897 if (b6 < b4) goto L22;
02898 L24:
02899 b4=b4+b7;
02900 b5=b6+b5;
02901 x2=br(b4+1);
02902 x3=br(b5+1);
02903 x4=bi(b4+1);
02904 x5=bi(b5+1);
02905 br(b4+1)=x3;
02906 br(b5+1)=x2;
02907 bi(b4+1)=x5;
02908 bi(b5+1)=x4;
02909 ix0=b6;
02910 L26:
02911 ix0=ix0/2;
02912 ix1=ix1-ix0;
02913 if(ix1 >= 0) goto L26;
02914
02915 ix0=2*ix0;
02916 b4=b4+b7;
02917 ix1=ix1+ix0;
02918 b5=ix1;
02919 if (b5 >= b4) goto L22;
02920 if (b4 < b6) goto L24;
02921 EXIT:
02922 status = 0;
02923 }
02924
02925 void Util::fftr_q(float *xcmplx, int nv)
02926 {
02927
02928
02929 int nu, inv, nu1, n, isub, n2, i1, i2, i;
02930 float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02931
02932 const float tab1[] = {
02933 9.58737990959775e-5f,
02934 1.91747597310703e-4f,
02935 3.83495187571395e-4f,
02936 7.66990318742704e-4f,
02937 1.53398018628476e-3f,
02938 3.06795676296598e-3f,
02939 6.13588464915449e-3f,
02940 1.22715382857199e-2f,
02941 2.45412285229123e-2f,
02942 4.90676743274181e-2f,
02943 9.80171403295604e-2f,
02944 1.95090322016128e-1f,
02945 3.82683432365090e-1f,
02946 7.07106781186546e-1f,
02947 1.00000000000000f,
02948 };
02949
02950 nu=abs(nv);
02951 inv=nv/nu;
02952 nu1=nu-1;
02953 n=(int)pow(2.f,nu1);
02954 isub=16-nu1;
02955
02956 ss=-tab1(isub);
02957 cc=-2.0f*pow(tab1(isub-1),2.f);
02958 c=1.0f;
02959 s=0.0f;
02960 n2=n/2;
02961 if ( inv > 0) {
02962 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02963 tr=xcmplx(1,1);
02964 ti=xcmplx(2,1);
02965 xcmplx(1,1)=tr+ti;
02966 xcmplx(2,1)=tr-ti;
02967 for (i=1;i<=n2;i++) {
02968 i1=i+1;
02969 i2=n-i+1;
02970 tr1=xcmplx(1,i1);
02971 tr2=xcmplx(1,i2);
02972 ti1=xcmplx(2,i1);
02973 ti2=xcmplx(2,i2);
02974 t=(cc*c-ss*s)+c;
02975 s=(cc*s+ss*c)+s;
02976 c=t;
02977 xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02978 xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02979 xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02980 xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02981 }
02982 } else {
02983 tr=xcmplx(1,1);
02984 ti=xcmplx(2,1);
02985 xcmplx(1,1)=0.5f*(tr+ti);
02986 xcmplx(2,1)=0.5f*(tr-ti);
02987 for (i=1; i<=n2; i++) {
02988 i1=i+1;
02989 i2=n-i+1;
02990 tr1=xcmplx(1,i1);
02991 tr2=xcmplx(1,i2);
02992 ti1=xcmplx(2,i1);
02993 ti2=xcmplx(2,i2);
02994 t=(cc*c-ss*s)+c;
02995 s=(cc*s+ss*c)+s;
02996 c=t;
02997 xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
02998 xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
02999 xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03000 xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03001 }
03002 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03003 }
03004 }
03005
03006
03007 void Util::fftr_d(double *xcmplx, int nv)
03008 {
03009
03010 int i1, i2, nu, inv, nu1, n, isub, n2, i;
03011 double tr1,tr2,ti1,ti2,tr,ti;
03012 double cc,c,ss,s,t;
03013 const double tab1[] = {
03014 9.58737990959775e-5,
03015 1.91747597310703e-4,
03016 3.83495187571395e-4,
03017 7.66990318742704e-4,
03018 1.53398018628476e-3,
03019 3.06795676296598e-3,
03020 6.13588464915449e-3,
03021 1.22715382857199e-2,
03022 2.45412285229123e-2,
03023 4.90676743274181e-2,
03024 9.80171403295604e-2,
03025 1.95090322016128e-1,
03026 3.82683432365090e-1,
03027 7.07106781186546e-1,
03028 1.00000000000000,
03029 };
03030
03031 nu=abs(nv);
03032 inv=nv/nu;
03033 nu1=nu-1;
03034 n=(int)pow(2.0f,nu1);
03035 isub=16-nu1;
03036 ss=-tab1(isub);
03037 cc=-2.0*pow(tab1(isub-1),2);
03038 c=1.0f;
03039 s=0.0f;
03040 n2=n/2;
03041
03042 if ( inv > 0 ) {
03043 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03044 tr=xcmplx(1,1);
03045 ti=xcmplx(2,1);
03046 xcmplx(1,1)=tr+ti;
03047 xcmplx(2,1)=tr-ti;
03048 for (i=1;i<=n2;i++) {
03049 i1=i+1;
03050 i2=n-i+1;
03051 tr1=xcmplx(1,i1);
03052 tr2=xcmplx(1,i2);
03053 ti1=xcmplx(2,i1);
03054 ti2=xcmplx(2,i2);
03055 t=(cc*c-ss*s)+c;
03056 s=(cc*s+ss*c)+s;
03057 c=t;
03058 xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03059 xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03060 xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03061 xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03062 }
03063 } else {
03064 tr=xcmplx(1,1);
03065 ti=xcmplx(2,1);
03066 xcmplx(1,1)=0.5*(tr+ti);
03067 xcmplx(2,1)=0.5*(tr-ti);
03068 for (i=1; i<=n2; i++) {
03069 i1=i+1;
03070 i2=n-i+1;
03071 tr1=xcmplx(1,i1);
03072 tr2=xcmplx(1,i2);
03073 ti1=xcmplx(2,i1);
03074 ti2=xcmplx(2,i2);
03075 t=(cc*c-ss*s)+c;
03076 s=(cc*s+ss*c)+s;
03077 c=t;
03078 xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03079 xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03080 xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03081 xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03082 }
03083 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03084 }
03085 }
03086 #undef tab1
03087 #undef xcmplx
03088 #undef br
03089 #undef bi
03090
03091
03092 void Util::Frngs(EMData* circp, vector<int> numr){
03093 int nring = numr.size()/3;
03094 float *circ = circp->get_data();
03095 int i, l;
03096 for (i=1; i<=nring;i++) {
03097
03098 #ifdef _WIN32
03099 l = (int)( log((float)numr(3,i))/log(2.0f) );
03100 #else
03101 l=(int)(log2(numr(3,i)));
03102 #endif //_WIN32
03103
03104 fftr_q(&circ(numr(2,i)),l);
03105 }
03106 }
03107
03108 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03109 int nring = numr.size()/3;
03110 float *circ = circp->get_data();
03111 int i, l;
03112 for (i=1; i<=nring;i++) {
03113
03114 #ifdef _WIN32
03115 l = (int)( log((float)numr(3,i))/log(2.0f) );
03116 #else
03117 l=(int)(log2(numr(3,i)));
03118 #endif //_WIN32
03119
03120 fftr_q(&circ(numr(2,i)),-l);
03121 }
03122 }
03123 #undef circ
03124
03125 #define b(i) b[i-1]
03126 void Util::prb1d(double *b, int npoint, float *pos) {
03127 double c2,c3;
03128 int nhalf;
03129
03130 nhalf = npoint/2 + 1;
03131 *pos = 0.0;
03132
03133 if (npoint == 7) {
03134 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03135 - 6.*b(6) + 31.*b(7);
03136 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03137 }
03138 else if (npoint == 5) {
03139 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03140 + 46.*b(5) ) / (-70.);
03141 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03142 }
03143 else if (npoint == 3) {
03144 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03145 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03146 }
03147
03148 else {
03149 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03150 - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03151 + 1092.*b(9) ) / (-4620.);
03152 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03153 - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03154 }
03155 if (c3 != 0.0) *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03156 }
03157 #undef b
03158
03159 #define circ1(i) circ1[i-1]
03160 #define circ2(i) circ2[i-1]
03161 #define t(i) t[i-1]
03162 #define q(i) q[i-1]
03163 #define b(i) b[i-1]
03164 #define t7(i) t7[i-1]
03165 Dict Util::Crosrng_e(EMData* circ1p, EMData* circ2p, vector<int> numr, int neg) {
03166
03167 int nring = numr.size()/3;
03168
03169 int maxrin = numr[numr.size()-1];
03170 double qn; float tot;
03171 float *circ1 = circ1p->get_data();
03172 float *circ2 = circ2p->get_data();
03173
03174
03175
03176
03177
03178
03179
03180
03181
03182
03183
03184 float *t;
03185 double t7[7], *q;
03186 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03187 float pos;
03188
03189 #ifdef _WIN32
03190 ip = -(int)(log((float)maxrin)/log(2.0f));
03191 #else
03192 ip = -(int) (log2(maxrin));
03193 #endif //_WIN32
03194
03195 q = (double*)calloc(maxrin, sizeof(double));
03196 t = (float*)calloc(maxrin, sizeof(float));
03197
03198
03199 for (i=1; i<=nring; i++) {
03200 numr3i = numr(3,i);
03201 numr2i = numr(2,i);
03202
03203 t(1) = (circ1(numr2i)) * circ2(numr2i);
03204
03205 if (numr3i != maxrin) {
03206
03207 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03208 t(2) = 0.0;
03209
03210 if (neg) {
03211
03212 for (j=3;j<=numr3i;j=j+2) {
03213 jc = j+numr2i-1;
03214 t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03215 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03216 }
03217 } else {
03218 for (j=3;j<=numr3i;j=j+2) {
03219 jc = j+numr2i-1;
03220 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03221 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03222 }
03223 }
03224 for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03225 } else {
03226 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03227 if (neg) {
03228
03229 for (j=3;j<=maxrin;j=j+2) {
03230 jc = j+numr2i-1;
03231 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03232 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03233 }
03234 } else {
03235 for (j=3;j<=maxrin;j=j+2) {
03236 jc = j+numr2i-1;
03237 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03238 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03239 }
03240 }
03241 for (j = 1; j <= maxrin; j++) q(j) += t(j);
03242 }
03243 }
03244
03245 fftr_d(q,ip);
03246
03247 qn = -1.0e20;
03248 for (j=1;j<=maxrin;j++) {
03249 if (q(j) >= qn) {
03250 qn = q(j); jtot = j;
03251 }
03252 }
03253
03254 for (k=-3; k<=3; k++) {
03255 j = (jtot+k+maxrin-1)%maxrin + 1;
03256 t7(k+4) = q(j);
03257 }
03258
03259 prb1d(t7,7,&pos);
03260
03261 tot = (float)jtot + pos;
03262
03263 if (q) free(q);
03264 if (t) free(t);
03265
03266 Dict retvals;
03267 retvals["qn"] = qn;
03268 retvals["tot"] = tot;
03269 return retvals;
03270 }
03271
03272 Dict Util::Crosrng_ew(EMData* circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03273
03274 int nring = numr.size()/3;
03275
03276 int maxrin = numr[numr.size()-1];
03277 double qn; float tot;
03278 float *circ1 = circ1p->get_data();
03279 float *circ2 = circ2p->get_data();
03280
03281
03282
03283
03284
03285
03286
03287
03288
03289
03290
03291 float *t;
03292 double t7[7], *q;
03293 int i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03294 float pos;
03295
03296 #ifdef _WIN32
03297 ip = -(int)(log((float)maxrin)/log(2.0f));
03298 #else
03299 ip = -(int) (log2(maxrin));
03300 #endif //_WIN32
03301
03302 q = (double*)calloc(maxrin, sizeof(double));
03303 t = (float*)calloc(maxrin, sizeof(float));
03304
03305
03306 for (i=1;i<=nring;i++) {
03307 numr3i = numr(3,i);
03308 numr2i = numr(2,i);
03309
03310 t(1) = circ1(numr2i) * circ2(numr2i);
03311
03312 if (numr3i != maxrin) {
03313
03314 t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03315 t(2) = 0.0;
03316
03317 if (neg) {
03318
03319 for (j=3; j<=numr3i; j=j+2) {
03320 jc = j+numr2i-1;
03321 t(j) = (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03322 t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03323 }
03324 } else {
03325 for (j=3; j<=numr3i; j=j+2) {
03326 jc = j+numr2i-1;
03327 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03328 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03329 }
03330 }
03331 for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03332 } else {
03333 t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03334 if (neg) {
03335
03336 for (j=3; j<=maxrin; j=j+2) {
03337 jc = j+numr2i-1;
03338 t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03339 t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03340 }
03341 } else {
03342 for (j=3; j<=maxrin; j=j+2) {
03343 jc = j+numr2i-1;
03344 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03345 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03346 }
03347 }
03348 for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03349 }
03350 }
03351
03352 fftr_d(q,ip);
03353
03354 qn = -1.0e20;
03355 for (j=1;j<=maxrin;j++) {
03356
03357 if (q(j) >= qn) {
03358 qn = q(j);
03359 jtot = j;
03360 }
03361 }
03362
03363 for (k=-3; k<=3; k++) {
03364 j = (jtot+k+maxrin-1)%maxrin + 1;
03365 t7(k+4) = q(j);
03366 }
03367
03368 prb1d(t7,7,&pos);
03369
03370 tot = (float)jtot + pos;
03371
03372
03373 if (t) free(t);
03374
03375 Dict retvals;
03376
03377
03378 retvals["qn"] = qn;
03379 retvals["tot"] = tot;
03380
03381 if (q) free(q);
03382
03383 return retvals;
03384 }
03385
03386 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03387 int nring = numr.size()/3;
03388
03389 int maxrin = numr[numr.size()-1];
03390 double qn; float tot; double qm; float tmt;
03391 float *circ1 = circ1p->get_data();
03392 float *circ2 = circ2p->get_data();
03393
03394
03395
03396
03397
03398
03399
03400
03401
03402
03403
03404
03405 double *t, *q, t7[7];
03406
03407 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03408 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03409
03410 qn = 0.0f;
03411 qm = 0.0f;
03412 tot = 0.0f;
03413 tmt = 0.0f;
03414 #ifdef _WIN32
03415 ip = -(int)(log((float)maxrin)/log(2.0f));
03416 #else
03417 ip = -(int)(log2(maxrin));
03418 #endif //_WIN32
03419
03420
03421
03422
03423
03424 q = (double*)calloc(maxrin,sizeof(double));
03425
03426
03427
03428 t = (double*)calloc(maxrin,sizeof(double));
03429
03430
03431 for (i=1; i<=nring; i++) {
03432
03433 numr3i = numr(3,i);
03434 numr2i = numr(2,i);
03435
03436 t1 = circ1(numr2i) * circ2(numr2i);
03437 q(1) += t1;
03438 t(1) += t1;
03439
03440 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03441 if (numr3i == maxrin) {
03442 q(2) += t1;
03443 t(2) += t1;
03444 } else {
03445 q(numr3i+1) += t1;
03446 t(numr3i+1) += t1;
03447 }
03448
03449 for (j=3; j<=numr3i; j += 2) {
03450 jc = j+numr2i-1;
03451
03452
03453
03454
03455
03456
03457
03458
03459 c1 = circ1(jc);
03460 c2 = circ1(jc+1);
03461 d1 = circ2(jc);
03462 d2 = circ2(jc+1);
03463
03464 t1 = c1 * d1;
03465 t2 = c2 * d2;
03466 t3 = c1 * d2;
03467 t4 = c2 * d1;
03468
03469 q(j) += t1 + t2;
03470 q(j+1) += -t3 + t4;
03471 t(j) += t1 - t2;
03472 t(j+1) += -t3 - t4;
03473 }
03474 }
03475
03476 fftr_d(q,ip);
03477
03478 qn = -1.0e20;
03479 for (j=1; j<=maxrin; j++) {
03480 if (q(j) >= qn) {
03481 qn = q(j);
03482 jtot = j;
03483 }
03484 }
03485
03486 for (k=-3; k<=3; k++) {
03487 j = ((jtot+k+maxrin-1)%maxrin)+1;
03488 t7(k+4) = q(j);
03489 }
03490
03491
03492 prb1d(t7,7,&pos);
03493 tot = (float)(jtot)+pos;
03494
03495
03496
03497
03498 fftr_d(t,ip);
03499
03500
03501 qm = -1.0e20;
03502 for (j=1; j<=maxrin;j++) {
03503 if ( t(j) >= qm ) {
03504 qm = t(j);
03505 jtot = j;
03506 }
03507 }
03508
03509 for (k=-3; k<=3; k++) {
03510 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03511 t7(k+4) = t(j);
03512 }
03513
03514
03515
03516 prb1d(t7,7,&pos);
03517 tmt = float(jtot) + pos;
03518
03519
03520
03521 free(t);
03522 free(q);
03523
03524 Dict retvals;
03525 retvals["qn"] = qn;
03526 retvals["tot"] = tot;
03527 retvals["qm"] = qm;
03528 retvals["tmt"] = tmt;
03529 return retvals;
03530 }
03531
03532 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03533 int nring = numr.size()/3;
03534
03535 int maxrin = numr[numr.size()-1];
03536 double qn; float tot; double qm; float tmt;
03537 float *circ1 = circ1p->get_data();
03538 float *circ2 = circ2p->get_data();
03539
03540
03541
03542
03543
03544
03545
03546
03547
03548
03549
03550
03551 double *t, *q;
03552
03553 int ip, jc, numr3i, numr2i, i, j, jtot = 0;
03554 float t1, t2, t3, t4, c1, c2, d1, d2;
03555
03556 qn = 0.0f;
03557 qm = 0.0f;
03558 tot = 0.0f;
03559 tmt = 0.0f;
03560 #ifdef _WIN32
03561 ip = -(int)(log((float)maxrin)/log(2.0f));
03562 #else
03563 ip = -(int)(log2(maxrin));
03564 #endif //_WIN32
03565
03566
03567
03568
03569
03570 q = (double*)calloc(maxrin,sizeof(double));
03571
03572
03573
03574 t = (double*)calloc(maxrin,sizeof(double));
03575
03576
03577 for (i=1; i<=nring; i++) {
03578
03579 numr3i = numr(3,i);
03580 numr2i = numr(2,i);
03581
03582 t1 = circ1(numr2i) * circ2(numr2i);
03583 q(1) += t1;
03584 t(1) += t1;
03585
03586 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03587 if (numr3i == maxrin) {
03588 q(2) += t1;
03589 t(2) += t1;
03590 } else {
03591 q(numr3i+1) += t1;
03592 t(numr3i+1) += t1;
03593 }
03594
03595 for (j=3; j<=numr3i; j += 2) {
03596 jc = j+numr2i-1;
03597
03598
03599
03600
03601
03602
03603
03604
03605 c1 = circ1(jc);
03606 c2 = circ1(jc+1);
03607 d1 = circ2(jc);
03608 d2 = circ2(jc+1);
03609
03610 t1 = c1 * d1;
03611 t2 = c2 * d2;
03612 t3 = c1 * d2;
03613 t4 = c2 * d1;
03614
03615 q(j) += t1 + t2;
03616 q(j+1) += -t3 + t4;
03617 t(j) += t1 - t2;
03618 t(j+1) += -t3 - t4;
03619 }
03620 }
03621
03622 fftr_d(q,ip);
03623
03624 qn = -1.0e20;
03625
03626 int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03627 int jstep = static_cast<int>(delta/360.0*maxrin);
03628 if (jstep < 1) { jstep = 1; }
03629
03630 for (j=jstart; j<=maxrin; j+=jstep) {
03631 if (q(j) >= qn) {
03632 qn = q(j);
03633 jtot = j;
03634 }
03635 }
03636
03637
03638
03639
03640
03641
03642
03643
03644
03645
03646 tot = (float)(jtot);
03647
03648
03649 fftr_d(t,ip);
03650
03651
03652 qm = -1.0e20;
03653 for (j=jstart; j<=maxrin;j+=jstep) {
03654 if ( t(j) >= qm ) {
03655 qm = t(j);
03656 jtot = j;
03657 }
03658 }
03659
03660
03661
03662
03663
03664
03665
03666
03667
03668
03669
03670 tmt = float(jtot);
03671
03672 free(t);
03673 free(q);
03674
03675 Dict retvals;
03676 retvals["qn"] = qn;
03677 retvals["tot"] = tot;
03678 retvals["qm"] = qm;
03679 retvals["tmt"] = tmt;
03680 return retvals;
03681 }
03682
03683
03684 Dict Util::Crosrng_psi_0_180(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03685 int nring = numr.size()/3;
03686
03687 int maxrin = numr[numr.size()-1];
03688 double qn; float tot; double qm; float tmt;
03689 float *circ1 = circ1p->get_data();
03690 float *circ2 = circ2p->get_data();
03691
03692
03693
03694
03695 double *t, *q, t7[7];
03696
03697 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03698 float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03699
03700 qn = 0.0f;
03701 qm = 0.0f;
03702 tot = 0.0f;
03703 tmt = 0.0f;
03704 #ifdef _WIN32
03705 ip = -(int)(log((float)maxrin)/log(2.0f));
03706 #else
03707 ip = -(int)(log2(maxrin));
03708 #endif //_WIN32
03709
03710
03711
03712
03713
03714 q = (double*)calloc(maxrin,sizeof(double));
03715
03716
03717
03718 t = (double*)calloc(maxrin,sizeof(double));
03719
03720
03721 for (i=1; i<=nring; i++) {
03722
03723 numr3i = numr(3,i);
03724 numr2i = numr(2,i);
03725
03726 t1 = circ1(numr2i) * circ2(numr2i);
03727 q(1) += t1;
03728 t(1) += t1;
03729
03730 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03731 if (numr3i == maxrin) {
03732 q(2) += t1;
03733 t(2) += t1;
03734 } else {
03735 q(numr3i+1) += t1;
03736 t(numr3i+1) += t1;
03737 }
03738
03739 for (j=3; j<=numr3i; j += 2) {
03740 jc = j+numr2i-1;
03741
03742
03743
03744
03745
03746
03747
03748
03749 c1 = circ1(jc);
03750 c2 = circ1(jc+1);
03751 d1 = circ2(jc);
03752 d2 = circ2(jc+1);
03753
03754 t1 = c1 * d1;
03755 t2 = c2 * d2;
03756 t3 = c1 * d2;
03757 t4 = c2 * d1;
03758
03759 q(j) += t1 + t2;
03760 q(j+1) += -t3 + t4;
03761 t(j) += t1 - t2;
03762 t(j+1) += -t3 - t4;
03763 }
03764 }
03765
03766 fftr_d(q,ip);
03767
03768 int psi_range = int(psi_max/360.0*maxrin+0.5);
03769 const int psi_0 = 0;
03770 int psi_180 = int( 180.0/360.0*maxrin+0.5);
03771
03772 qn = -1.0e20;
03773 for (k=-psi_range; k<=psi_range; k++) {
03774 j = (k+psi_0+maxrin-1)%maxrin+1;
03775 if (q(j) >= qn) {
03776 qn = q(j);
03777 jtot = j;
03778 }
03779 }
03780
03781 for (k=-psi_range; k<=psi_range; k++) {
03782 j = (k+psi_180+maxrin-1)%maxrin+1;
03783 if (q(j) >= qn) {
03784 qn = q(j);
03785 jtot = j;
03786 }
03787 }
03788
03789 for (k=-3; k<=3; k++) {
03790 j = ((jtot+k+maxrin-1)%maxrin)+1;
03791 t7(k+4) = q(j);
03792 }
03793
03794
03795 prb1d(t7,7,&pos);
03796 tot = (float)(jtot)+pos;
03797
03798
03799
03800
03801 fftr_d(t,ip);
03802
03803
03804 qm = -1.0e20;
03805 for (k=-psi_range; k<=psi_range; k++) {
03806 j = (k+psi_0+maxrin-1)%maxrin+1;
03807 if (t(j) >= qm) {
03808 qm = t(j);
03809 jtot = j;
03810 }
03811 }
03812
03813 for (k=-psi_range; k<=psi_range; k++) {
03814 j = (k+psi_180+maxrin-1)%maxrin+1;
03815 if (t(j) >= qm) {
03816 qm = t(j);
03817 jtot = j;
03818 }
03819 }
03820
03821 for (k=-3; k<=3; k++) {
03822 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03823 t7(k+4) = t(j);
03824 }
03825
03826
03827
03828 prb1d(t7,7,&pos);
03829 tmt = float(jtot) + pos;
03830
03831
03832
03833 free(t);
03834 free(q);
03835
03836 Dict retvals;
03837 retvals["qn"] = qn;
03838 retvals["tot"] = tot;
03839 retvals["qm"] = qm;
03840 retvals["tmt"] = tmt;
03841 return retvals;
03842 }
03843
03844
03845 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag) {
03846
03847
03848 int nring = numr.size()/3;
03849 int maxrin = numr[numr.size()-1];
03850 double qn; float tot; double qm; float tmt;
03851 float *circ1 = circ1p->get_data();
03852 float *circ2 = circ2p->get_data();
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 qm = 0.0f;
03861 tot = 0.0f;
03862 tmt = 0.0f;
03863 #ifdef _WIN32
03864 ip = -(int)(log((float)maxrin)/log(2.0f));
03865 #else
03866 ip = -(int)(log2(maxrin));
03867 #endif //_WIN32
03868
03869
03870
03871
03872 q = (double*)calloc(maxrin,sizeof(double));
03873
03874
03875 if (flag==0) {
03876 for (i=1; i<=nring; i++) {
03877
03878 numr3i = numr(3,i);
03879 numr2i = numr(2,i);
03880
03881 t1 = circ1(numr2i) * circ2(numr2i);
03882 q(1) += t1;
03883
03884 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03885 if (numr3i == maxrin) {
03886 q(2) += t1;
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 c1 = circ1(jc);
03899 c2 = circ1(jc+1);
03900 d1 = circ2(jc);
03901 d2 = circ2(jc+1);
03902
03903 t1 = c1 * d1;
03904 t3 = c1 * d2;
03905 t2 = c2 * d2;
03906 t4 = c2 * d1;
03907
03908 q(j) += t1 + t2;
03909 q(j+1) += -t3 + t4;
03910 }
03911 }
03912 } else {
03913 for (i=1; i<=nring; i++) {
03914
03915 numr3i = numr(3,i);
03916 numr2i = numr(2,i);
03917
03918 t1 = circ1(numr2i) * circ2(numr2i);
03919 q(1) += t1;
03920
03921 t1 = circ1(numr2i+1) * circ2(numr2i+1);
03922 if (numr3i == maxrin) {
03923 q(2) += t1;
03924 } else {
03925 q(numr3i+1) += t1;
03926 }
03927
03928 for (j=3; j<=numr3i; j += 2) {
03929 jc = j+numr2i-1;
03930
03931
03932
03933
03934
03935 c1 = circ1(jc);
03936 c2 = circ1(jc+1);
03937 d1 = circ2(jc);
03938 d2 = circ2(jc+1);
03939
03940 t1 = c1 * d1;
03941 t3 = c1 * d2;
03942 t2 = c2 * d2;
03943 t4 = c2 * d1;
03944
03945 q(j) += t1 - t2;
03946 q(j+1) += -t3 - t4;
03947 }
03948 }
03949 }
03950 fftr_d(q,ip);
03951
03952 qn = -1.0e20;
03953 int psi_pos = int(psi/360.0*maxrin+0.5);
03954
03955 for (k=-5; k<=5; k++) {
03956 j = (psi_pos+maxrin-1)%maxrin+1;
03957 if (q(j) >= qn) {
03958 qn = q(j);
03959 jtot = j;
03960 }
03961 }
03962
03963 for (k=-3; k<=3; k++) {
03964 j = ((jtot+k+maxrin-1)%maxrin)+1;
03965 t7(k+4) = q(j);
03966 }
03967
03968
03969 prb1d(t7,7,&pos);
03970 tot = (float)(jtot)+pos;
03971 free(q);
03972
03973 Dict retvals;
03974 retvals["qn"] = qn;
03975 retvals["tot"] = tot;
03976 return retvals;
03977 }
03978
03979 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03980 int nring = numr.size()/3;
03981
03982 int maxrin = numr[numr.size()-1];
03983 double qn; float tot;
03984 float *circ1 = circ1p->get_data();
03985 float *circ2 = circ2p->get_data();
03986
03987
03988
03989
03990
03991
03992
03993
03994
03995
03996
03997
03998 double *q, t7[7];
03999
04000 int ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04001 float c1, c2, d1, d2, pos;
04002
04003 qn = 0.0;
04004 tot = 0.0;
04005 #ifdef _WIN32
04006 ip = -(int)(log((float)maxrin)/log(2.0f));
04007 #else
04008 ip = -(int)(log2(maxrin));
04009 #endif //_WIN32
04010
04011
04012
04013
04014
04015 q = (double*)calloc(maxrin,sizeof(double));
04016
04017
04018 for (i=1; i<=nring; i++) {
04019
04020 numr3i = numr(3,i);
04021 numr2i = numr(2,i);
04022
04023 q(1) += circ1(numr2i) * circ2(numr2i);
04024
04025 if (numr3i == maxrin) q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04026 else q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04027
04028 for (j=3; j<=numr3i; j += 2) {
04029 jc = j+numr2i-1;
04030
04031
04032
04033
04034
04035 c1 = circ1(jc);
04036 c2 = circ1(jc+1);
04037 d1 = circ2(jc);
04038 d2 = circ2(jc+1);
04039
04040 q(j) += c1 * d1 + c2 * d2;
04041 q(j+1) += -c1 * d2 + c2 * d1;
04042 }
04043 }
04044
04045 fftr_d(q,ip);
04046
04047 qn = -1.0e20;
04048 for (j=1; j<=maxrin; j++) {
04049 if (q(j) >= qn) {
04050 qn = q(j);
04051 jtot = j;
04052 }
04053 }
04054
04055 for (k=-3; k<=3; k++) {
04056 j = ((jtot+k+maxrin-1)%maxrin)+1;
04057 t7(k+4) = q(j);
04058 }
04059
04060
04061 prb1d(t7,7,&pos);
04062 tot = (float)(jtot)+pos;
04063
04064
04065
04066 free(q);
04067
04068 Dict retvals;
04069 retvals["qn"] = qn;
04070 retvals["tot"] = tot;
04071 return retvals;
04072 }
04073
04074 #define dout(i,j) dout[i+maxrin*j]
04075 #define circ1b(i) circ1b[i-1]
04076 #define circ2b(i) circ2b[i-1]
04077
04078 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04079
04080
04081
04082 int ip, jc, numr3i, numr2i, i, j;
04083 float t1, t2, t3, t4, c1, c2, d1, d2;
04084
04085 int nring = numr.size()/3;
04086
04087 int maxrin = numr[numr.size()-1];
04088
04089 float* circ1b = circ1->get_data();
04090 float* circ2b = circ2->get_data();
04091
04092
04093 double *t, *q;
04094
04095 q = (double*)calloc(maxrin,sizeof(double));
04096 t = (double*)calloc(maxrin,sizeof(double));
04097
04098 #ifdef _WIN32
04099 ip = -(int)(log((float)maxrin)/log(2.0f));
04100 #else
04101 ip = -(int)(log2(maxrin));
04102 #endif //_WIN32
04103
04104
04105
04106
04107
04108
04109
04110 for (i=1; i<=nring; i++) {
04111
04112 numr3i = numr(3,i);
04113 numr2i = numr(2,i);
04114
04115 t1 = circ1b(numr2i) * circ2b(numr2i);
04116 q(1) = q(1)+t1;
04117 t(1) = t(1)+t1;
04118
04119 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04120 if (numr3i == maxrin) {
04121 q(2) += t1;
04122 t(2) += t1;
04123 } else {
04124 q(numr3i+1) += t1;
04125 t(numr3i+1) += t1;
04126 }
04127
04128 for (j=3; j<=numr3i; j=j+2) {
04129 jc = j+numr2i-1;
04130
04131 c1 = circ1b(jc);
04132 c2 = circ1b(jc+1);
04133 d1 = circ2b(jc);
04134 d2 = circ2b(jc+1);
04135
04136 t1 = c1 * d1;
04137 t3 = c1 * d2;
04138 t2 = c2 * d2;
04139 t4 = c2 * d1;
04140
04141 q(j) += t1 + t2;
04142 q(j+1) += - t3 + t4;
04143 t(j) += t1 - t2;
04144 t(j+1) += - t3 - t4;
04145 }
04146 }
04147
04148
04149 fftr_d(q,ip);
04150
04151
04152 fftr_d(t,ip);
04153
04154 EMData* out = new EMData();
04155 out->set_size(maxrin,2,1);
04156 float *dout = out->get_data();
04157 for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04158
04159
04160
04161 free(t);
04162 free(q);
04163 return out;
04164 }
04165
04166
04167 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04168
04169 int maxrin = numr[numr.size()-1];
04170
04171 vector<float> r(2*maxrin);
04172
04173 Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04174
04175 return r;
04176 }
04177
04178 #define dout(i,j) dout[i+maxrin*j]
04179 #define circ1b(i) circ1b[i-1]
04180 #define circ2b(i) circ2b[i-1]
04181
04182 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04183
04184
04185
04186 int ip, jc, numr3i, numr2i, i, j;
04187 float t1, t2, t3, t4, c1, c2, d1, d2;
04188
04189 int nring = numr.size()/3;
04190
04191 int maxrin = numr[numr.size()-1];
04192
04193 float* circ1b = circ1->get_data();
04194 float* circ2b = circ2->get_data();
04195
04196 #ifdef _WIN32
04197 ip = -(int)(log((float)maxrin)/log(2.0f));
04198 #else
04199 ip = -(int)(log2(maxrin));
04200 #endif //_WIN32
04201 for (int i=1; i<=maxrin; i++) {q(i) = 0.0f; t(i) = 0.0f;}
04202
04203
04204
04205
04206
04207 for (i=1; i<=nring; i++) {
04208
04209 numr3i = numr(3,i);
04210 numr2i = numr(2,i);
04211
04212 t1 = circ1b(numr2i) * circ2b(numr2i);
04213 q(1) += t1;
04214 t(1) += t1;
04215
04216 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04217 if (numr3i == maxrin) {
04218 q(2) += t1;
04219 t(2) += t1;
04220 } else {
04221 q(numr3i+1) += t1;
04222 t(numr3i+1) += t1;
04223 }
04224
04225 for (j=3; j<=numr3i; j=j+2) {
04226 jc = j+numr2i-1;
04227
04228 c1 = circ1b(jc);
04229 c2 = circ1b(jc+1);
04230 d1 = circ2b(jc);
04231 d2 = circ2b(jc+1);
04232
04233 t1 = c1 * d1;
04234 t3 = c1 * d2;
04235 t2 = c2 * d2;
04236 t4 = c2 * d1;
04237
04238 q(j) += t1 + t2;
04239 q(j+1) += -t3 + t4;
04240 t(j) += t1 - t2;
04241 t(j+1) += -t3 - t4;
04242 }
04243 }
04244
04245 fftr_q(q,ip);
04246
04247
04248
04249 fftr_q(t,ip);
04250 }
04251
04252
04253
04254 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04255 {
04256
04257 int ip, jc, numr3i, numr2i, i, j;
04258 float t1, t2, t3, t4, c1, c2, d1, d2;
04259
04260 int nring = numr.size()/3;
04261 int maxrin = numr[numr.size()-1];
04262
04263 float* circ1b = circ1->get_data();
04264 float* circ2b = circ2->get_data();
04265
04266 double *q;
04267
04268 q = (double*)calloc(maxrin,sizeof(double));
04269
04270 #ifdef _WIN32
04271 ip = -(int)(log((float)maxrin)/log(2.0f));
04272 #else
04273 ip = -(int)(log2(maxrin));
04274 #endif //_WIN32
04275
04276
04277
04278 for (i=1;i<=nring;i++) {
04279
04280 numr3i = numr(3,i);
04281 numr2i = numr(2,i);
04282
04283 t1 = circ1b(numr2i) * circ2b(numr2i);
04284 q(1) = q(1)+t1;
04285
04286 if (numr3i == maxrin) {
04287 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04288 q(2) = q(2)+t1;
04289 } else {
04290 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04291 q(numr3i+1) = q(numr3i+1)+t1;
04292 }
04293
04294 for (j=3;j<=numr3i;j=j+2) {
04295 jc = j+numr2i-1;
04296
04297 c1 = circ1b(jc);
04298 c2 = circ1b(jc+1);
04299 d1 = circ2b(jc);
04300 d2 = circ2b(jc+1);
04301
04302 t1 = c1 * d1;
04303 t3 = c1 * d2;
04304 t2 = c2 * d2;
04305 t4 = c2 * d1;
04306
04307 q(j) = q(j) + t1 + t2;
04308 q(j+1) = q(j+1) - t3 + t4;
04309 }
04310 }
04311
04312
04313 fftr_d(q,ip);
04314
04315 EMData* out = new EMData();
04316 out->set_size(maxrin,1,1);
04317 float *dout = out->get_data();
04318 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04319 free(q);
04320 return out;
04321
04322 }
04323
04324
04325 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04326 {
04327
04328 int ip, jc, numr3i, numr2i, i, j;
04329 float t1, t2, t3, t4, c1, c2, d1, d2;
04330
04331 int nring = numr.size()/3;
04332 int maxrin = numr[numr.size()-1];
04333
04334 float* circ1b = circ1->get_data();
04335 float* circ2b = circ2->get_data();
04336
04337 double *t;
04338
04339 t = (double*)calloc(maxrin,sizeof(double));
04340
04341 #ifdef _WIN32
04342 ip = -(int)(log((float)maxrin)/log(2.0f));
04343 #else
04344 ip = -(int)(log2(maxrin));
04345 #endif //_WIN32
04346
04347
04348
04349 for (i=1;i<=nring;i++) {
04350
04351 numr3i = numr(3,i);
04352 numr2i = numr(2,i);
04353
04354 t1 = circ1b(numr2i) * circ2b(numr2i);
04355 t(1) = t(1)+t1;
04356
04357 if (numr3i == maxrin) {
04358 t1 = circ1b(numr2i+1) * circ2b(numr2i+1);
04359 t(2) = t(2)+t1;
04360 }
04361
04362 for (j=3;j<=numr3i;j=j+2) {
04363 jc = j+numr2i-1;
04364
04365 c1 = circ1b(jc);
04366 c2 = circ1b(jc+1);
04367 d1 = circ2b(jc);
04368 d2 = circ2b(jc+1);
04369
04370 t1 = c1 * d1;
04371 t3 = c1 * d2;
04372 t2 = c2 * d2;
04373 t4 = c2 * d1;
04374
04375 t(j) = t(j) + t1 - t2;
04376 t(j+1) = t(j+1) - t3 - t4;
04377 }
04378 }
04379
04380
04381 fftr_d(t,ip);
04382
04383 EMData* out = new EMData();
04384 out->set_size(maxrin,1,1);
04385 float *dout = out->get_data();
04386 for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04387 free(t);
04388 return out;
04389
04390 }
04391
04392 #undef circ1b
04393 #undef circ2b
04394 #undef dout
04395
04396 #undef circ1
04397 #undef circ2
04398 #undef t
04399 #undef q
04400 #undef b
04401 #undef t7
04402
04403
04404 #define QUADPI 3.141592653589793238462643383279502884197
04405 #define PI2 2*QUADPI
04406
04407 float Util::ener(EMData* ave, vector<int> numr) {
04408 ENTERFUNC;
04409 long double ener,en;
04410
04411 int nring = numr.size()/3;
04412 float *aveptr = ave->get_data();
04413
04414 ener = 0.0;
04415 for (int i=1; i<=nring; i++) {
04416 int numr3i = numr(3,i);
04417 int np = numr(2,i)-1;
04418 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04419 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04420 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04421 ener += en/numr3i;
04422 }
04423 EXITFUNC;
04424 return static_cast<float>(ener);
04425 }
04426
04427 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04428 ENTERFUNC;
04429 long double ener, en;
04430 float arg, cs, si;
04431
04432 int nima = data.size();
04433 int nring = numr.size()/3;
04434 int maxrin = numr(3,nring);
04435
04436 ener = 0.0;
04437 for (int i=1; i<=nring; i++) {
04438 int numr3i = numr(3,i);
04439 int np = numr(2,i)-1;
04440 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04441 float temp1 = 0.0, temp2 = 0.0;
04442 for (int kk=0; kk<nima; kk++) {
04443 float *ptr = data[kk]->get_data();
04444 temp1 += ptr[np];
04445 temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04446 }
04447 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04448 for (int j=2; j<numr3i; j+=2) {
04449 float tempr = 0.0, tempi = 0.0;
04450 for (int kk=0; kk<nima; kk++) {
04451 float *ptr = data[kk]->get_data();
04452 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04453 cs = cos(arg);
04454 si = sin(arg);
04455 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04456 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04457 }
04458 en += tq*(tempr*tempr+tempi*tempi);
04459 }
04460 ener += en/numr3i;
04461 }
04462 EXITFUNC;
04463 return static_cast<float>(ener);
04464 }
04465
04466 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04467 int nring = numr.size()/3;
04468 float *ave = avep->get_data();
04469 float *dat = datp->get_data();
04470 int i, j, numr3i, np;
04471 float arg, cs, si;
04472 int maxrin = numr(3,nring);
04473 if(mirror == 1) {
04474 for (i=1; i<=nring; i++) {
04475 numr3i = numr(3,i);
04476 np = numr(2,i)-1;
04477 ave[np] += dat[np];
04478 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04479 for (j=2; j<numr3i; j=j+2) {
04480 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04481 cs = cos(arg);
04482 si = sin(arg);
04483
04484 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04485 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04486 }
04487 }
04488 } else {
04489 for (i=1; i<=nring; i++) {
04490 numr3i = numr(3,i);
04491 np = numr(2,i)-1;
04492 ave[np] += dat[np];
04493 ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04494 for (j=2; j<numr3i; j=j+2) {
04495 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04496 cs = cos(arg);
04497 si = sin(arg);
04498
04499 ave[np + j] += dat[np + j]*cs - dat[np + j +1]*si;
04500 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04501 }
04502 }
04503 }
04504 avep->update();
04505 EXITFUNC;
04506 }
04507
04508 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04509 int nring = numr.size()/3;
04510 float *ave = avep->get_data();
04511 float *dat = datp->get_data();
04512 int i, j, numr3i, np;
04513 float arg, cs, si;
04514 int maxrin = numr(3,nring);
04515 if(mirror == 1) {
04516 for (i=1; i<=nring; i++) {
04517 numr3i = numr(3,i);
04518 np = numr(2,i)-1;
04519 ave[np] -= dat[np];
04520 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04521 for (j=2; j<numr3i; j=j+2) {
04522 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04523 cs = cos(arg);
04524 si = sin(arg);
04525
04526 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04527 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04528 }
04529 }
04530 } else {
04531 for (i=1; i<=nring; i++) {
04532 numr3i = numr(3,i);
04533 np = numr(2,i)-1;
04534 ave[np] -= dat[np];
04535 ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04536 for (j=2; j<numr3i; j=j+2) {
04537 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04538 cs = cos(arg);
04539 si = sin(arg);
04540
04541 ave[np + j] -= dat[np + j]*cs - dat[np + j +1]*si;
04542 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04543 }
04544 }
04545 }
04546 avep->update();
04547 EXITFUNC;
04548 }
04549
04550
04551 #undef QUADPI
04552 #undef PI2
04553
04554 #undef numr
04555 #undef circ
04556
04557
04558 #define QUADPI 3.141592653589793238462643383279502884197
04559 #define PI2 QUADPI*2
04560 #define deg_rad QUADPI/180.0
04561 #define rad_deg 180.0/QUADPI
04562
04563 struct ori_t
04564 {
04565 int iphi;
04566 int itht;
04567 int id;
04568 };
04569
04570
04571 struct cmpang
04572 {
04573 bool operator()( const ori_t& a, const ori_t& b )
04574 {
04575 if( a.itht != b.itht )
04576 {
04577 return a.itht < b.itht;
04578 }
04579
04580 return a.iphi < b.iphi;
04581 }
04582 };
04583
04584
04585 vector<double> Util::cml_weights(const vector<float>& cml){
04586 static const int NBIN = 100;
04587 int nline=cml.size()/2;
04588 vector<double> weights(nline);
04589
04590 vector<ori_t> angs(nline);
04591 for( int i=0; i < nline; ++i ) {
04592 angs[i].iphi = int( NBIN*cml[2*i] );
04593 angs[i].itht = int( NBIN*cml[2*i+1] );
04594 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04595 angs[i].id = i;
04596 }
04597
04598
04599
04600 std::sort( angs.begin(), angs.end(), cmpang() );
04601
04602 vector<float> newphi;
04603 vector<float> newtht;
04604 vector< vector<int> > indices;
04605
04606 int curt_iphi = -1;
04607 int curt_itht = -1;
04608 for(unsigned int i=0 ;i < angs.size(); ++i ) {
04609 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04610 Assert( indices.size() > 0 );
04611 indices.back().push_back(angs[i].id);
04612 } else {
04613 curt_iphi = angs[i].iphi;
04614 curt_itht = angs[i].itht;
04615
04616 newphi.push_back( float(curt_iphi)/NBIN );
04617 newtht.push_back( float(curt_itht)/NBIN );
04618 indices.push_back( vector<int>(1,angs[i].id) );
04619 }
04620 }
04621
04622
04623
04624
04625 int num_agl = newphi.size();
04626
04627 if(num_agl>2) {
04628 vector<double> w=Util::vrdg(newphi, newtht);
04629
04630 Assert( w.size()==newphi.size() );
04631 Assert( indices.size()==newphi.size() );
04632
04633 for(unsigned int i=0; i < newphi.size(); ++i ) {
04634
04635
04636
04637
04638
04639
04640
04641
04642 for(unsigned int j=0; j < indices[i].size(); ++j ) {
04643 int id = indices[i][j];
04644 weights[id] = w[i]/indices[i].size();
04645
04646 }
04647
04648
04649
04650 }
04651 } else {
04652 cout<<"warning in Util.cml_weights"<<endl;
04653 double val = PI2/float(nline);
04654 for(int i=0; i<nline; i++) weights[i]=val;
04655 }
04656
04657 return weights;
04658
04659 }
04660
04661
04662
04663
04664
04665 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04666 {
04667 int i;
04668 int nx=img->get_xsize();
04669 float *img_ptr = img->get_data();
04670 float *line_ptr = line->get_data();
04671 for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04672 img->update();
04673 }
04674
04675 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04676 int j;
04677 int nx = sino->get_xsize();
04678 int i = nx * pos_line;
04679 float r1, r2;
04680 float *line_ptr = line->get_data();
04681 float *sino_ptr = sino->get_data();
04682 for (j=ilf;j<=ihf; j += 2) {
04683 r1 = line_ptr[j];
04684 r2 = line_ptr[j + 1];
04685 sino_ptr[i + j - ilf] = r1;
04686 sino_ptr[i + j - ilf + 1] = r2;
04687 sino_ptr[i + nx * nblines + j - ilf] = r1;
04688 sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04689 }
04690 sino->update();
04691 }
04692
04693 vector<double> Util::cml_init_rot(vector<float> Ori){
04694 int nb_ori = Ori.size() / 4;
04695 int i, ind;
04696 float ph, th, ps;
04697 double cph, cth, cps, sph, sth, sps;
04698 vector<double> Rot(nb_ori*9);
04699 for (i=0; i<nb_ori; ++i){
04700 ind = i*4;
04701
04702 ph = Ori[ind+2]-90;
04703 th = Ori[ind+1];
04704 ps = Ori[ind]+90;
04705 ph *= deg_rad;
04706 th *= deg_rad;
04707 ps *= deg_rad;
04708
04709 cph = cos(ph);
04710 cth = cos(th);
04711 cps = cos(ps);
04712 sph = sin(ph);
04713 sth = sin(th);
04714 sps = sin(ps);
04715
04716 ind = i*9;
04717 Rot[ind] = cph*cps-cth*sps*sph;
04718 Rot[ind+1] = cph*sps+cth*cps*sph;
04719 Rot[ind+2] = sth*sph;
04720 Rot[ind+3] = -sph*cps-cth*sps*cph;
04721 Rot[ind+4] = -sph*sps+cth*cps*cph;
04722 Rot[ind+5] = sth*cph;
04723 Rot[ind+6] = sth*sps;
04724 Rot[ind+7] = -sth*cps;
04725 Rot[ind+8] = cth;
04726 }
04727
04728 return Rot;
04729 }
04730
04731 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04732 float ph, ps;
04733 double cph, cth, cps, sph, sth, sps;
04734 int ind = iprj*9;
04735
04736 ph = nps-90;
04737 ps = nph+90;
04738 ph *= deg_rad;
04739 th *= deg_rad;
04740 ps *= deg_rad;
04741
04742 cph = cos(ph);
04743 cth = cos(th);
04744 cps = cos(ps);
04745 sph = sin(ph);
04746 sth = sin(th);
04747 sps = sin(ps);
04748
04749 Rot[ind] = (float)(cph*cps-cth*sps*sph);
04750 Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04751 Rot[ind+2] = (float)(sth*sph);
04752 Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04753 Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04754 Rot[ind+5] = (float)(sth*cph);
04755 Rot[ind+6] = (float)(sth*sps);
04756 Rot[ind+7] = (float)(-sth*cps);
04757 Rot[ind+8] = (float)(cth);
04758
04759 return Rot;
04760 }
04761
04762 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04763 vector<int> com(2*(n_prj - 1));
04764 int a = i_prj*9;
04765 int i, b, c;
04766 int n1=0, n2=0;
04767 float vmax = 1 - 1.0e-6f;
04768 double r11, r12, r13, r23, r31, r32, r33;
04769
04770 c = 0;
04771 for (i=0; i<n_prj; ++i){
04772 if (i!=i_prj){
04773 b = i*9;
04774
04775 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04776 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04777 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04778 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04779 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04780 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04781 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04782 if (r33 > vmax) {
04783 n2 = 270;
04784 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04785 }
04786 else if (r33 < -vmax) {
04787 n2 = 270;
04788 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04789 } else {
04790 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04791 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04792 if (n1 < 0) {n1 += 360;}
04793 if (n2 <= 0) {n2 = abs(n2);}
04794 else {n2 = 360 - n2;}
04795 }
04796
04797 if (n1 >= 360){n1 = n1 % 360;}
04798 if (n2 >= 360){n2 = n2 % 360;}
04799
04800
04801 b = c*2;
04802 com[b] = n1;
04803 com[b+1] = n2;
04804 ++c;
04805 }
04806 }
04807
04808 return com;
04809
04810 }
04811
04812 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int n_prj, int n_lines) {
04813 vector<int> com(2*n_lines);
04814 int a=0, b, c, l;
04815 int n1=0, n2=0, mem=-1;
04816 float vmax = 1 - 1.0e-6f;
04817 double r11, r12, r13, r23, r31, r32, r33;
04818 c = 0;
04819 for (l=0; l<n_lines; ++l){
04820 c = 2*l;
04821 if (seq[c]!=mem){
04822 mem = seq[c];
04823 a = seq[c]*9;
04824 }
04825 b = seq[c+1]*9;
04826
04827
04828 r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04829 r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04830 r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04831 r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04832 r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04833 r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04834 r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04835 if (r33 > vmax) {
04836 n2 = 270;
04837 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04838 }
04839 else if (r33 < -vmax) {
04840 n2 = 270;
04841 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04842 } else {
04843 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04844 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04845 if (n1 < 0) {n1 += 360;}
04846 if (n2 <= 0) {n2 = abs(n2);}
04847 else {n2 = 360 - n2;}
04848 }
04849 if (n1 >= 360){n1 = n1 % 360;}
04850 if (n2 >= 360){n2 = n2 % 360;}
04851
04852
04853 com[c] = n1;
04854 com[c+1] = n2;
04855 }
04856
04857 return com;
04858
04859 }
04860
04861 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int nprj, int nlines){
04862
04863 vector<double> cml(2*nlines);
04864 float ph1, th1;
04865 float ph2, th2;
04866 double nx, ny, nz;
04867 double norm;
04868 double sth1=0, sph1=0, cth1=0, cph1=0;
04869 double sth2, sph2, cth2, cph2;
04870 int l, ind, c;
04871 int mem = -1;
04872 for (l=0; l<nlines; ++l){
04873 c = 2*l;
04874 if (seq[c]!=mem){
04875 mem = seq[c];
04876 ind = 4*seq[c];
04877 ph1 = Ori[ind]*deg_rad;
04878 th1 = Ori[ind+1]*deg_rad;
04879 sth1 = sin(th1);
04880 sph1 = sin(ph1);
04881 cth1 = cos(th1);
04882 cph1 = cos(ph1);
04883 }
04884 ind = 4*seq[c+1];
04885 ph2 = Ori[ind]*deg_rad;
04886 th2 = Ori[ind+1]*deg_rad;
04887 sth2 = sin(th2);
04888 cth2 = cos(th2);
04889 sph2 = sin(ph2);
04890 cph2 = cos(ph2);
04891
04892 nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04893 ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04894 nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04895 norm = sqrt(nx*nx+ny*ny+nz*nz);
04896 nx /= norm;
04897 ny /= norm;
04898 nz /= norm;
04899
04900 if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04901
04902 cml[c+1] = acos(nz);
04903 if (cml[c+1] == 0) {cml[c] = 0;}
04904 else {
04905 cml[c+1] *= rad_deg;
04906 if (cml[c+1] > 89.99) {cml[c+1] = 89.99;}
04907 cml[c] = rad_deg * atan2(nx, ny);
04908 cml[c] = fmod(360 + cml[c], 360);
04909
04910 }
04911 }
04912
04913 return cml;
04914 }
04915
04916 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04917 double res = 0;
04918 double buf = 0;
04919 float* line_1;
04920 float* line_2;
04921 int i, n, ind;
04922 int lnlen = data[0]->get_xsize();
04923 for (n=0; n<n_lines; ++n) {
04924 ind = n*2;
04925 line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04926 line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04927 buf = 0;
04928 for (i=0; i<lnlen; ++i) {
04929 buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04930 }
04931 res += buf * weights[n];
04932 }
04933
04934 return res;
04935
04936 }
04937
04938 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04939 int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04940
04941
04942
04943 vector<double> res(2);
04944 int lnlen = data[0]->get_xsize();
04945 int end = 2*(n_prj-1);
04946 double disc, buf, bdisc, tmp;
04947 int n, i, ipsi, ind, bipsi, c;
04948 float* line_1;
04949 float* line_2;
04950 bdisc = 1.0e6;
04951 bipsi = -1;
04952
04953 for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04954
04955 disc = 0;
04956 c = 0;
04957 for (n=0; n<n_prj; ++n) {
04958 if(n!=iprj) {
04959 ind = 2*c;
04960 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04961 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04962 buf = 0;
04963 for (i=0; i<lnlen; ++i) {
04964 tmp = line_1[i]-line_2[i];
04965 buf += tmp*tmp;
04966 }
04967 disc += buf * weights[iw[c]];
04968 ++c;
04969 }
04970 }
04971
04972 if (disc <= bdisc) {
04973 bdisc = disc;
04974 bipsi = ipsi;
04975 }
04976
04977 for (i=0; i<end; i+=2){
04978 com[i] += d_psi;
04979 if (com[i] >= n_psi) {com[i] = com[i] % n_psi;}
04980 }
04981 }
04982 res[0] = bdisc;
04983 res[1] = float(bipsi);
04984
04985 return res;
04986 }
04987
04988 #undef QUADPI
04989 #undef PI2
04990 #undef deg_rad
04991 #undef rad_deg
04992
04993
04994
04995
04996
04997
04998 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
04999 ENTERFUNC;
05000
05001 int nima = data.size();
05002 vector<float> res(nima);
05003 double result = 0.;
05004 double valmin = 1.0e20;
05005 int valpos = -1;
05006
05007 for (int kk=0; kk<nima; kk++){
05008 result = 0;
05009
05010 float *y_data = data[kk]->get_data();
05011 float *x_data = image->get_data();
05012 long totsize = image->get_xsize()*image->get_ysize();
05013 for (long i = 0; i < totsize; i++) {
05014 double temp = x_data[i]- y_data[i];
05015 result += temp*temp;
05016 }
05017 result /= totsize;
05018 res[kk] = (float)result;
05019
05020 if(result<valmin) {valmin = result; valpos = kk;}
05021
05022 }
05023
05024 Dict retvals;
05025 retvals["dist"] = res;
05026 retvals["pos"] = valpos;
05027
05028 EXITFUNC;
05029 return retvals;
05030
05031 }
05032
05033 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05034 ENTERFUNC;
05035
05036 int nima = data.size();
05037 vector<float> res(nima);
05038 double result = 0.;
05039 double valmin = 1.0e20;
05040 int valpos = -1;
05041
05042 for (int kk=0; kk<nima; kk++){
05043 result = 0;
05044
05045
05046 float *y_data = data[kk]->get_data();
05047 float *x_data = image->get_data();
05048
05049
05050 int nx = data[kk]->get_xsize();
05051 int ny = data[kk]->get_ysize();
05052 nx = (nx - 2 + data[kk]->is_fftodd());
05053 int lsd2 = (nx + 2 - nx%2) ;
05054
05055 int ixb = 2*((nx+1)%2);
05056 int iyb = ny%2;
05057 int iz = 0;
05058
05059 for ( int iy = 0; iy <= ny-1; iy++) {
05060 for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05061 int ii = ix + (iy + iz * ny)* lsd2;
05062 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05063 }
05064 }
05065 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05066 int ii = (iy + iz * ny)* lsd2;
05067 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05068 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05069 }
05070 if(nx%2 == 0) {
05071 for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05072 int ii = lsd2 - 2 + (iy + iz * ny)* lsd2;
05073 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05074 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05075 }
05076
05077 }
05078 result *= 2;
05079 result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05080 if(ny%2 == 0) {
05081 int ii = (ny/2 + iz * ny)* lsd2;
05082 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05083 }
05084 if(nx%2 == 0) {
05085 int ii = lsd2 - 2 + (0 + iz * ny)* lsd2;
05086 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05087 if(ny%2 == 0) {
05088 int ii = lsd2 - 2 +(ny/2 + iz * ny)* lsd2;
05089 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05090 }
05091 }
05092
05093 result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05094 res[kk] = (float)result;
05095
05096 if(result<valmin) {valmin = result; valpos = kk;}
05097
05098 }
05099
05100 Dict retvals;
05101 retvals["dist"] = res;
05102 retvals["pos"] = valpos;
05103
05104 EXITFUNC;
05105 return retvals;
05106 }
05107
05108 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05109 long int d2 = group2[s2 - 1] - group2[0];
05110 long int p2 = 0;
05111 long int i1 = 0;
05112 long int i2 = 0;
05113 long int max = 0;
05114 long int cont = 0;
05115 long int i = 0;
05116 int stop1 = 0;
05117 int stop2 = 0;
05118
05119 for (i=0; i<s1; i++) {
05120 p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05121 if (p2 >= s2) {p2 = s2 - 1;}
05122 i1 = p2;
05123 i2 = p2;
05124 max = s2;
05125 if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05126
05127 stop1 = 0;
05128 stop2 = 0;
05129 while (max--) {
05130 if (group1[i] == group2[i1]) {
05131 if (flag) {stb[cont] = group1[i];}
05132 cont++;
05133 break;
05134 }
05135 if (group2[i1] < group1[i]) {stop1=1;}
05136 if (group1[i] == group2[i2]) {
05137 if (flag) {stb[cont] = group1[i];}
05138 cont++;
05139 break;
05140 }
05141 if (group2[i2] > group1[i]) {stop2=1;}
05142
05143
05144 if (stop1 & stop2) {break;}
05145 i1--;
05146 i2++;
05147 if (i1 < 0) {i1 = 0;}
05148 if (i2 >= s2) {i2 = s2 - 1;}
05149 }
05150
05151 }
05152
05153 return cont;
05154 }
05155
05156
05157
05158 #define old_ptr(i,j,k) old_ptr[i+(j+(k*ny))*nx]
05159 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*new_nx]
05160 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05161 {
05162
05163 if (!img) {
05164 throw NullPointerException("NULL input image");
05165 }
05166
05167
05168
05169 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05170
05171
05172
05173
05174 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)
05175 {
05176 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05177 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05178 }
05179
05180
05181
05182
05183 int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05184
05185
05186
05187
05188 int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05189 int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05190 int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05191 if(r1>1){r1=1;}
05192 if(r2>1){r2=1;}
05193 if(r3>1){r3=1;}
05194 int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05195
05196
05197
05198 EMData* img2 = new EMData();
05199 img2->set_size(new_nx,new_ny,new_nz);
05200 float *new_ptr = img2->get_data();
05201 float *old_ptr = img->get_data();
05202 int iptr, jptr, kptr = 0;
05203 for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05204 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05205 for (int i=new_st_x; i<nx; i+=x_step) {
05206 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05207 iptr++;}
05208 jptr++;}
05209 kptr++;}
05210 img2->update();
05211 return img2;
05212 }
05213 #undef old_ptr
05214 #undef new_ptr
05215
05216 #define inp(i,j,k) inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*nx]
05217 #define outp(i,j,k) outp[i+(j+(k*new_ny))*new_nx]
05218 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05219 {
05220
05221 if (!img) throw NullPointerException("NULL input image");
05222
05223
05224
05225 int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05226
05227
05228
05229 if(new_nx>nx || new_ny>ny || new_nz>nz)
05230 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size.");
05231 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)
05232 throw ImageDimensionException("The offset inconsistent with the input image size.");
05233 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))))
05234 throw ImageDimensionException("The offset inconsistent with the input image size.");
05235
05236
05237
05238 int new_st_x = nx/2-new_nx/2 + x_offset,
05239 new_st_y = ny/2-new_ny/2 + y_offset,
05240 new_st_z = nz/2-new_nz/2 + z_offset;
05241
05242
05243
05244 if (new_st_x<0 || new_st_y<0 || new_st_z<0)
05245 throw ImageDimensionException("The offset inconsistent with the input image size.");
05246
05247
05248 EMData* wind = img->copy_head();
05249 wind->set_size(new_nx, new_ny, new_nz);
05250 float *outp=wind->get_data();
05251 float *inp=img->get_data();
05252
05253 for (int k=0; k<new_nz; k++)
05254 for(int j=0; j<new_ny; j++)
05255 for(int i=0; i<new_nx; i++)
05256 outp(i,j,k) = inp(i,j,k);
05257 wind->update();
05258 return wind;
05259 }
05260 #undef inp
05261 #undef outp
05262
05263 #define inp(i,j,k) inp[i+(j+(k*ny))*nx]
05264 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*new_nx]
05265 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)
05266 {
05267
05268 if (!img) throw NullPointerException("NULL input image");
05269
05270
05271
05272 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05273
05274
05275
05276 if(new_nx<nx || new_ny<ny || new_nz<nz)
05277 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05278 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)
05279 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05280 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))))
05281 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05282
05283
05284 EMData* pading = img->copy_head();
05285 pading->set_size(new_nx, new_ny, new_nz);
05286 float *inp = img->get_data();
05287 float *outp = pading->get_data();
05288
05289
05290
05291
05292 float background;
05293
05294 if (strcmp(params,"average")==0) background = img->get_attr("mean");
05295 else if (strcmp(params,"circumference")==0) {
05296 float sum1=0.0f;
05297 int cnt=0;
05298 for(int i=0;i<nx;i++) {
05299 sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05300 cnt+=2;
05301 }
05302 if(nz-1 == 0) {
05303 for (int j=1;j<ny-1;j++) {
05304 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05305 cnt+=2;
05306 }
05307 } else {
05308 for (int k=1;k<nz-1;k++) {
05309 for (int j=1;j<ny-1;j++) {
05310 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05311 cnt+=2;
05312 }
05313 }
05314 }
05315 background = sum1/cnt;
05316 } else {
05317 background = static_cast<float>( atof( params ) );
05318 }
05319
05320
05321
05322 int new_st_x=0,new_st_y=0,new_st_z=0;
05323 for (int k=0;k<new_nz;k++)
05324 for(int j=0;j<new_ny;j++)
05325 for (int i=0;i<new_nx;i++)
05326 outp(i,j,k)=background;
05327
05328
05329
05330 new_st_x=int((new_nx/2-nx/2) + x_offset);
05331 new_st_y=int((new_ny/2-ny/2) + y_offset);
05332 new_st_z=int((new_nz/2-nz/2) + z_offset);
05333
05334
05335 for (int k=0;k<nz;k++)
05336 for(int j=0;j<ny;j++)
05337 for(int i=0;i<nx;i++)
05338 outp(i,j,k)=inp(i,j,k);
05339 pading->update();
05340 return pading;
05341 }
05342 #undef inp
05343 #undef outp
05344
05345
05346 void Util::colreverse(float* beg, float* end, int nx) {
05347 float* tmp = new float[nx];
05348 int n = (end - beg)/nx;
05349 int nhalf = n/2;
05350 for (int i = 0; i < nhalf; i++) {
05351
05352 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05353 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05354 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05355 }
05356 delete[] tmp;
05357 }
05358
05359 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05360 {
05361 int nxy = nx*ny;
05362 colreverse(beg, end, nxy);
05363 }
05364
05365
05366 void Util::cyclicshift(EMData *image, Dict params) {
05367
05368 if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05369
05370 int dx = params["dx"];
05371 int dy = params["dy"];
05372 int dz = params["dz"];
05373
05374
05375 int nx = image->get_xsize();
05376 dx %= nx;
05377 if (dx < 0) dx += nx;
05378 int ny = image->get_ysize();
05379 dy %= ny;
05380 if (dy < 0) dy += ny;
05381 int nz = image->get_zsize();
05382 dz %= nz;
05383 if (dz < 0) dz += nz;
05384
05385 int mx = -(dx - nx);
05386 int my = -(dy - ny);
05387 int mz = -(dz - nz);
05388
05389 float* data = image->get_data();
05390
05391 if (mx != 0) {
05392 for (int iz = 0; iz < nz; iz++)
05393 for (int iy = 0; iy < ny; iy++) {
05394
05395 int offset = nx*iy + nx*ny*iz;
05396 reverse(&data[offset],&data[offset+mx]);
05397 reverse(&data[offset+mx],&data[offset+nx]);
05398 reverse(&data[offset],&data[offset+nx]);
05399 }
05400 }
05401
05402 if (my != 0) {
05403 for (int iz = 0; iz < nz; iz++) {
05404 int offset = nx*ny*iz;
05405 colreverse(&data[offset], &data[offset + my*nx], nx);
05406 colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05407 colreverse(&data[offset], &data[offset + ny*nx], nx);
05408 }
05409 }
05410 if (mz != 0) {
05411 slicereverse(&data[0], &data[mz*ny*nx], nx, ny);
05412 slicereverse(&data[mz*ny*nx], &data[nz*ny*nx], nx, ny);
05413 slicereverse(&data[0], &data[nz*ny*nx], nx ,ny);
05414 }
05415 image->update();
05416 }
05417
05418
05419
05420
05421 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05422 {
05423 if (image->is_complex())
05424 throw ImageFormatException("Cannot do histogram on Fourier image");
05425
05426 float *imageptr=0, *maskptr=0;
05427 int nx=image->get_xsize();
05428 int ny=image->get_ysize();
05429 int nz=image->get_zsize();
05430
05431 if(mask != NULL){
05432 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05433 throw ImageDimensionException("The size of mask image should be of same size as the input image");
05434 maskptr =mask->get_data();
05435 }
05436 if( nbins == 0) nbins = nx;
05437 vector <float> freq(2*nbins, 0.0);
05438
05439 imageptr=image->get_data();
05440 if( hmin == hmax ) {
05441 if(mask == NULL) {
05442 hmax = image->get_attr("maximum");
05443 hmin = image->get_attr("minimum");
05444 } else {
05445 bool First = true;
05446 for (int i = 0;i < nx*ny*nz; i++) {
05447 if (maskptr[i]>=0.5f) {
05448 if(First) {
05449 hmax = imageptr[i];
05450 hmin = imageptr[i];
05451 First = false;
05452 } else {
05453 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05454 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05455 }
05456 }
05457 }
05458 }
05459 }
05460 float hdiff = hmax - hmin;
05461 float ff = (nbins-1)/hdiff;
05462 for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05463 if(mask == NULL) {
05464 for(int i = 0; i < nx*ny*nz; i++) {
05465 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05466 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05467 }
05468 } else {
05469 for(int i = 0; i < nx*ny*nz; i++) {
05470 if(maskptr[i] >= 0.5) {
05471 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05472 if(jbin >= 1 && jbin <= nbins) freq[jbin-1] += 1.0;
05473 }
05474 }
05475 }
05476 return freq;
05477 }
05478
05479 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05480 {
05481
05482 if (img->is_complex() || ref->is_complex())
05483 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05484
05485 if(mask != NULL){
05486 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05487 throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05488
05489
05490
05491 int size_ref = ((ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05492 int size_img = ((img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05493
05494
05495
05496 float *ref_ptr = ref->get_data();
05497 float ref_h_min = ref->get_attr("minimum");
05498 float ref_h_max = ref->get_attr("maximum");
05499 float ref_h_avg = ref->get_attr("mean");
05500 float ref_h_sig = ref->get_attr("sigma");
05501
05502
05503
05504 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05505
05506 vector<float> img_data = Util::infomask(img, mask);
05507 float img_avg = img_data[0];
05508 float img_sig = img_data[1];
05509
05510
05511 int cnt=0;
05512 for(int i=0;i<size_img;i++)
05513 if (mask_ptr[i]>0.5f)
05514 cnt++;
05515
05516
05517
05518 float ref_h_diff = ref_h_max - ref_h_min;
05519
05520 #ifdef _WIN32
05521 int hist_len = _cpp_min((int)size_ref/16,_cpp_min((int)size_img/16,256));
05522 #else
05523 int hist_len = std::min((int)size_ref/16,std::min((int)size_img/16,256));
05524 #endif //_WIN32
05525
05526 float *ref_freq_bin = new float[3*hist_len];
05527
05528
05529 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05530
05531 for (int i = 0;i < size_ref;i++) {
05532 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05533 ref_freq_bin[L]++;
05534 }
05535 for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05536
05537
05538 float A = ref_h_sig/img_sig;
05539 float B = ref_h_avg - (A*img_avg);
05540
05541 vector<float> args;
05542 args.push_back(A);
05543 args.push_back(B);
05544
05545 vector<float> scale;
05546 scale.push_back(1.e-7f*A);
05547 scale.push_back(-1.e-7f*B);
05548
05549 vector<float> ref_freq_hist;
05550 for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05551
05552 vector<float> data;
05553 data.push_back(ref_h_diff);
05554 data.push_back(ref_h_min);
05555
05556 Dict parameter;
05557
05558
05559 parameter["args"] = args;
05560 parameter["scale"]= scale;
05561 parameter["data"] = data;
05562 parameter["ref_freq_bin"] = ref_freq_hist;
05563 parameter["size_img"]=size_img;
05564 parameter["hist_len"]=hist_len;
05565
05566
05567 return parameter;
05568 }
05569
05570
05571 float Util::hist_comp_freq(float PA,float PB,int size_img, int hist_len, EMData *img, vector<float> ref_freq_hist, EMData *mask, float ref_h_diff, float ref_h_min)
05572 {
05573 float *img_ptr = img->get_data();
05574 float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05575
05576 int *img_freq_bin = new int[3*hist_len];
05577 for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05578 for(int i = 0;i < size_img;i++) {
05579 if(mask_ptr[i] > 0.5f) {
05580 float img_xn = img_ptr[i]*PA + PB;
05581 int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05582 if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05583 }
05584 }
05585 int freq_hist = 0;
05586
05587 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);
05588 freq_hist = (-freq_hist);
05589 return static_cast<float>(freq_hist);
05590 }
05591
05592 #define QUADPI 3.141592653589793238462643383279502884197
05593 #define DGR_TO_RAD QUADPI/180
05594 #define DM(I) DM [I-1]
05595 #define SS(I) SS [I-1]
05596 Dict Util::CANG(float PHI,float THETA,float PSI)
05597 {
05598 double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05599 vector<float> DM,SS;
05600
05601 for(int i =0;i<9;i++) DM.push_back(0);
05602
05603 for(int i =0;i<6;i++) SS.push_back(0);
05604
05605 CPHI = cos(double(PHI)*DGR_TO_RAD);
05606 SPHI = sin(double(PHI)*DGR_TO_RAD);
05607 CTHE = cos(double(THETA)*DGR_TO_RAD);
05608 STHE = sin(double(THETA)*DGR_TO_RAD);
05609 CPSI = cos(double(PSI)*DGR_TO_RAD);
05610 SPSI = sin(double(PSI)*DGR_TO_RAD);
05611
05612 SS(1) = float(CPHI);
05613 SS(2) = float(SPHI);
05614 SS(3) = float(CTHE);
05615 SS(4) = float(STHE);
05616 SS(5) = float(CPSI);
05617 SS(6) = float(SPSI);
05618
05619 DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05620 DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05621 DM(3) = float(-STHE*CPSI);
05622 DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05623 DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05624 DM(6) = float(STHE*SPSI);
05625 DM(7) = float(STHE*CPHI);
05626 DM(8) = float(STHE*SPHI);
05627 DM(9) = float(CTHE);
05628
05629 Dict DMnSS;
05630 DMnSS["DM"] = DM;
05631 DMnSS["SS"] = SS;
05632
05633 return(DMnSS);
05634 }
05635 #undef SS
05636 #undef DM
05637 #undef QUADPI
05638 #undef DGR_TO_RAD
05639
05640 #define DM(I) DM[I-1]
05641 #define B(i,j) Bptr[i-1+((j-1)*NSAM)]
05642 #define CUBE(i,j,k) CUBEptr[(i-1)+((j-1)+((k-1)*NY3D))*NX3D]
05643
05644 void Util::BPCQ(EMData *B,EMData *CUBE, vector<float> DM)
05645 {
05646
05647 float *Bptr = B->get_data();
05648 float *CUBEptr = CUBE->get_data();
05649
05650 int NSAM,NROW,NX3D,NY3D,NZC,KZ,IQX,IQY,LDPX,LDPY,LDPZ,LDPNMX,LDPNMY,NZ1;
05651 float DIPX,DIPY,XB,YB,XBB,YBB;
05652
05653 Transform * t = B->get_attr("xform.projection");
05654 Dict d = t->get_params("spider");
05655 if(t) {delete t; t=0;}
05656
05657 float x_shift = d[ "tx" ];
05658 float y_shift = d[ "ty" ];
05659 x_shift = -x_shift;
05660 y_shift = -y_shift;
05661
05662 NSAM = B->get_xsize();
05663 NROW = B->get_ysize();
05664 NX3D = CUBE->get_xsize();
05665 NY3D = CUBE->get_ysize();
05666 NZC = CUBE->get_zsize();
05667
05668
05669 LDPX = NX3D/2 +1;
05670 LDPY = NY3D/2 +1;
05671 LDPZ = NZC/2 +1;
05672 LDPNMX = NSAM/2 +1;
05673 LDPNMY = NROW/2 +1;
05674 NZ1 = 1;
05675
05676 for(int K=1;K<=NZC;K++) {
05677 KZ=K-1+NZ1;
05678 for(int J=1;J<=NY3D;J++) {
05679 XBB = (1-LDPX)*DM(1)+(J-LDPY)*DM(2)+(KZ-LDPZ)*DM(3);
05680 YBB = (1-LDPX)*DM(4)+(J-LDPY)*DM(5)+(KZ-LDPZ)*DM(6);
05681 for(int I=1;I<=NX3D;I++) {
05682 XB = (I-1)*DM(1)+XBB-x_shift;
05683 IQX = int(XB+float(LDPNMX));
05684 if (IQX <1 || IQX >= NSAM) continue;
05685 YB = (I-1)*DM(4)+YBB-y_shift;
05686 IQY = int(YB+float(LDPNMY));
05687 if (IQY<1 || IQY>=NROW) continue;
05688 DIPX = XB+LDPNMX-IQX;
05689 DIPY = YB+LDPNMY-IQY;
05690
05691 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)));
05692 }
05693 }
05694 }
05695 }
05696
05697 #undef DM
05698 #undef B
05699 #undef CUBE
05700
05701
05702 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
05703 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
05704 #define SS(I,J) SS [I-1 + (J-1)*6]
05705
05706 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05707 {
05708 int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05709 float WW,OX,OY;
05710
05711 NSAM = PROJ->get_xsize();
05712 NROW = PROJ->get_ysize();
05713 int ntotal = NSAM*NROW;
05714 float q = 2.0f;
05715 float qt = 8.0f/q;
05716
05717 int ipad = 1;
05718 NSAM *= ipad;
05719 NROW *= ipad;
05720 NNNN = NSAM+2-(NSAM%2);
05721 int NX2 = NSAM/2;
05722 NR2 = NROW/2;
05723
05724 NANG = int(SS.size())/6;
05725
05726 EMData* W = new EMData();
05727 int Wnx = NNNN/2;
05728 W->set_size(Wnx,NROW,1);
05729 W->to_zero();
05730 float *Wptr = W->get_data();
05731 float *PROJptr = PROJ->get_data();
05732 for (L=1; L<=NANG; L++) {
05733 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);
05734 float tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) );
05735 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05736 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05737 if(OX < 0.0f) {
05738 OX = -OX;
05739 OY = -OY;
05740 }
05741
05742 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f ) {
05743 for(int J=1;J<=NROW;J++) {
05744 JY = (J-1);
05745 if(JY > NR2) JY -= NROW;
05746 #ifdef _WIN32
05747 int xma = _cpp_min(int(0.5f+(q-JY*OY)/OX),NX2);
05748 int xmi = _cpp_max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05749 #else
05750 int xma = std::min(int(0.5f+(q-JY*OY)/OX),NX2);
05751 int xmi = std::max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05752 #endif //_WIN32
05753 if( xmi <= xma) {
05754 for(int I=xmi;I<=xma;I++) {
05755 float Y = fabs(OX*I + OY*JY);
05756 W(I+1,J) += exp(-qt*Y*Y);
05757
05758 }
05759 }
05760 }
05761 } else {
05762 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J) += 1.0f;
05763 }
05764 }
05765 EMData* proj_in = PROJ;
05766
05767 PROJ = PROJ->norm_pad( false, ipad);
05768 PROJ->do_fft_inplace();
05769 PROJ->update();
05770
05771 PROJptr = PROJ->get_data();
05772
05773 float WNRMinv,temp;
05774 float osnr = 1.0f/SNR;
05775 WNRMinv = 1.0f/W(1,1);
05776 for(int J=1;J<=NROW;J++) {
05777 JY = J-1;
05778 if( JY > NR2) JY -= NROW;
05779 float sy = JY;
05780 sy /= NROW;
05781 sy *= sy;
05782 for(int I=1;I<=NNNN;I+=2) {
05783 KX = (I+1)/2;
05784 temp = W(KX,J)*WNRMinv;
05785 WW = temp/(temp*temp + osnr);
05786
05787 float sx = KX-1;
05788 sx /= NSAM;
05789 WW *= exp(qt*(sy + sx*sx));
05790 PROJ(I,J) *= WW;
05791 PROJ(I+1,J) *= WW;
05792 }
05793 }
05794 delete W; W = 0;
05795 PROJ->do_ift_inplace();
05796 PROJ->depad();
05797
05798 float* data_src = PROJ->get_data();
05799 float* data_dst = proj_in->get_data();
05800
05801 for( int i=0; i < ntotal; ++i ) data_dst[i] = data_src[i];
05802
05803 proj_in->update();
05804
05805 delete PROJ;
05806 }
05807
05808
05809
05810
05811
05812
05813
05814
05815
05816
05817
05818
05819
05820
05821
05822
05823
05824
05825
05826
05827
05828
05829
05830
05831
05832
05833
05834
05835
05836
05837
05838
05839
05840
05841
05842
05843
05844
05845
05846
05847
05848
05849
05850
05851
05852
05853
05854
05855
05856
05857
05858
05859
05860
05861
05862
05863
05864
05865
05866
05867
05868
05869
05870
05871
05872
05873
05874
05875
05876
05877
05878
05879
05880
05881
05882
05883
05884
05885
05886 #undef PROJ
05887 #undef W
05888 #undef SS
05889
05890 #define W(i,j) Wptr [i-1+((j-1)*Wnx)]
05891 #define PROJ(i,j) PROJptr [i-1+((j-1)*NNNN)]
05892 #define SS(I,J) SS [I-1 + (J-1)*6]
05893 #define RI(i,j) RI [(i-1) + ((j-1)*3)]
05894 #define CC(i) CC [i-1]
05895 #define CP(i) CP [i-1]
05896 #define VP(i) VP [i-1]
05897 #define VV(i) VV [i-1]
05898 #define AMAX1(i,j) i>j?i:j
05899 #define AMIN1(i,j) i<j?i:j
05900 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
05901 {
05902 float rad2deg =(180.0f/3.1415926f);
05903 float deg2rad = (3.1415926f/180.0f);
05904
05905 int NSAM,NROW,NNNN,NR2,NANG,L,JY;
05906
05907 NSAM = PROJ->get_xsize();
05908 NROW = PROJ->get_ysize();
05909 NNNN = NSAM+2-(NSAM%2);
05910 NR2 = NROW/2;
05911 NANG = int(SS.size())/6;
05912
05913 float RI[9];
05914 RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
05915 RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
05916 RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
05917 RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
05918 RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
05919 RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
05920 RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
05921 RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
05922 RI(3,3)=SS(3,NUMP);
05923
05924 float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
05925
05926 EMData* W = new EMData();
05927 int Wnx = NNNN/2;
05928 W->set_size(NNNN/2,NROW,1);
05929 W->to_one();
05930 float *Wptr = W->get_data();
05931
05932 float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
05933
05934 for (L=1; L<=NANG; L++) {
05935 if (L != NUMP) {
05936 CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
05937 CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
05938 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);
05939
05940 TMP = sqrt(CC(1)*CC(1) + CC(2)*CC(2) + CC(3)*CC(3));
05941 CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
05942 ALPHA=rad2deg*float(asin(CCN));
05943 if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
05944 if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
05945 if(ALPHA<1.0E-6) {
05946 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
05947 } else {
05948 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
05949 CC(1) = CC(1)/CCN;CC(2) = CC(2)/CCN;CC(3) = CC(3)/CCN;
05950 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
05951 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
05952 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
05953 CP(1) = 0.0;CP(2) = 0.0;
05954 VP(1) = 0.0;VP(2) = 0.0;
05955
05956 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
05957 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
05958 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
05959 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
05960
05961 TMP = CP(1)*VP(2)-CP(2)*VP(1);
05962
05963
05964 TMP = AMAX1(1.0E-4f,fabs(TMP));
05965 float tmpinv = 1.0f/TMP;
05966 for(int J=1;J<=NROW;J++) {
05967 JY = (J-1);
05968 if (JY>NR2) JY=JY-NROW;
05969 for(int I=1;I<=NNNN/2;I++) {
05970 FV = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
05971 RT = 1.0f-FV/FM;
05972 W(I,J) += ((RT>0.0f)*RT);
05973 }
05974 }
05975 }
05976 }
05977 }
05978
05979 EMData* proj_in = PROJ;
05980
05981 PROJ = PROJ->norm_pad( false, 1);
05982 PROJ->do_fft_inplace();
05983 PROJ->update();
05984 float *PROJptr = PROJ->get_data();
05985
05986 int KX;
05987 float WW;
05988 for(int J=1; J<=NROW; J++)
05989 for(int I=1; I<=NNNN; I+=2) {
05990 KX = (I+1)/2;
05991 WW = 1.0f/W(KX,J);
05992 PROJ(I,J) = PROJ(I,J)*WW;
05993 PROJ(I+1,J) = PROJ(I+1,J)*WW;
05994 }
05995 delete W; W = 0;
05996 PROJ->do_ift_inplace();
05997 PROJ->depad();
05998
05999 float* data_src = PROJ->get_data();
06000 float* data_dst = proj_in->get_data();
06001
06002 int ntotal = NSAM*NROW;
06003 for( int i=0; i < ntotal; ++i ) data_dst[i] = data_src[i];
06004
06005 proj_in->update();
06006 delete PROJ;
06007 }
06008 #undef AMAX1
06009 #undef AMIN1
06010 #undef RI
06011 #undef CC
06012 #undef CP
06013 #undef VV
06014 #undef VP
06015
06016
06017 #undef W
06018 #undef SS
06019 #undef PROJ
06020
06021 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06022 {
06023 float cst = cs*1.0e7f;
06024
06025 wgh /= 100.0;
06026 float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06027 float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06028 float ak2 = ak*ak;
06029 float g1 = dzz*1.0e4f*lambda*ak2;
06030 float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06031
06032 float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06033 if(b_factor != 0.0f) ctfv *= exp(-b_factor*ak2/4.0f);
06034
06035 return ctfv;
06036 }
06037
06038 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06039 {
06040
06041
06042
06043 int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();
06044
06045
06046
06047 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06048 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06049
06050 int i, size = nx*ny*nz;
06051
06052 float* img_ptr = image->get_data();
06053 float* mask_ptr = mask->get_data();
06054
06055 int ln=0;
06056 for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06057
06058 EMData* new_image = new EMData();
06059 new_image->set_size(ln,1,1);
06060 float *new_ptr = new_image->get_data();
06061
06062 ln=-1;
06063 for(i = 0;i < size;i++){
06064 if(mask_ptr[i] > 0.5f) {
06065 ln++;
06066 new_ptr[ln]=img_ptr[i];
06067 }
06068 }
06069
06070 return new_image;
06071 }
06072
06073 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06074 {
06075
06076
06077
06078 if(mask == NULL)
06079 throw ImageDimensionException("The mask cannot be an null image");
06080
06081
06082
06083
06084 int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06085
06086 int i,size = nx*ny*nz;
06087
06088 EMData *new_image = new EMData();
06089 new_image->set_size(nx,ny,nz);
06090 float *new_ptr = new_image->get_data();
06091 float *mask_ptr = mask->get_data();
06092 float *img_ptr = image->get_data();
06093 int count = 0;
06094 float sum_under_mask = 0.0 ;
06095 for(i = 0;i < size;i++){
06096 if(mask_ptr[i] > 0.5f){
06097 new_ptr[i] = img_ptr[count];
06098 sum_under_mask += img_ptr[count];
06099 count++;
06100 if( count > image->get_xsize() ) {
06101 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06102 }
06103 }
06104 }
06105
06106 if( count > image->get_xsize() ) {
06107 throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06108 }
06109
06110 float avg_under_mask = sum_under_mask / count;
06111 for(i = 0;i < size;i++) {
06112 if(mask_ptr[i] <= 0.5f) new_ptr[i] = avg_under_mask;
06113 }
06114 new_image->update();
06115 return new_image;
06116 }
06117
06118
06119
06120 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06121 {
06122 vector<float>new_peak;
06123 int n1=peak1.size()/3;
06124 float p_size2=p_size*p_size;
06125 for (int i=0;i<n1;++i) {
06126 vector<float>::iterator it2= peak1.begin()+3*i;
06127 bool push_back1=true;
06128 int n2=peak2.size()/3;
06129
06130
06131 if(n2 ==0) {
06132 new_peak.push_back(*it2);
06133 new_peak.push_back(*(it2+1));
06134 new_peak.push_back(*(it2+2));
06135 } else {
06136 int j=0;
06137 while (j< n2-1 ) {
06138 vector<float>::iterator it3= peak2.begin()+3*j;
06139 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06140 if(d2< p_size2 ) {
06141 if( (*it2)<(*it3) ) {
06142 new_peak.push_back(*it3);
06143 new_peak.push_back(*(it3+1));
06144 new_peak.push_back(*(it3+2));
06145 peak2.erase(it3);
06146 peak2.erase(it3);
06147 peak2.erase(it3);
06148 push_back1=false;
06149 } else {
06150 peak2.erase(it3);
06151 peak2.erase(it3);
06152 peak2.erase(it3);
06153 }
06154 } else j=j+1;
06155 n2=peak2.size()/3;
06156 }
06157 if(push_back1) {
06158 new_peak.push_back(*it2);
06159 new_peak.push_back(*(it2+1));
06160 new_peak.push_back(*(it2+2));
06161 }
06162 }
06163 }
06164 return new_peak;
06165 }
06166
06167 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06168 {
06169
06170
06171
06172
06173
06174 ENTERFUNC;
06175
06176 int i;
06177
06178
06179 for ( i = 0 ; i < n * n ; i++ ) eigvec[i] = covmat[i];
06180
06181 char NEEDV = 'V';
06182 char UPLO = 'U';
06183 int lwork = -1;
06184 int info = 0;
06185 float *work, wsize;
06186
06187
06188 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06189 lwork = (int)wsize;
06190
06191 work = (float *)calloc(lwork, sizeof(float));
06192
06193 ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06194 free(work);
06195 EXITFUNC;
06196 return info;
06197 }
06198
06199 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06200 {
06201
06202 ENTERFUNC;
06203 int len = covmatpy.size();
06204 float *eigvec;
06205 float *eigval;
06206 float *covmat;
06207 int status = 0;
06208 eigval = (float*)calloc(ncov,sizeof(float));
06209 eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06210 covmat = (float*)calloc(ncov*ncov, sizeof(float));
06211
06212 const float *covmat_ptr;
06213 covmat_ptr = &covmatpy[0];
06214 for(int i=0;i<len;i++){
06215 covmat[i] = covmat_ptr[i];
06216 }
06217
06218 status = Util::coveig(ncov, covmat, eigval, eigvec);
06219
06220 vector<float> eigval_py(ncov);
06221 const float *eigval_ptr;
06222 eigval_ptr = &eigval[0];
06223 for(int i=0;i<ncov;i++){
06224 eigval_py[i] = eigval_ptr[i];
06225 }
06226
06227 vector<float> eigvec_py(ncov*ncov);
06228 const float *eigvec_ptr;
06229 eigvec_ptr = &eigvec[0];
06230 for(int i=0;i<ncov*ncov;i++){
06231 eigvec_py[i] = eigvec_ptr[i];
06232 }
06233
06234 Dict res;
06235 res["eigval"] = eigval_py;
06236 res["eigvec"] = eigvec_py;
06237
06238 EXITFUNC;
06239 return res;
06240 }
06241
06242 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06243 {
06244 int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06245
06246 k=(int)pw.size();
06247 l=0;
06248 m=k;
06249 n2=n+2;
06250 n1=n+1;
06251 klmd=k+l+m;
06252 klm2d= k+l+m+2;
06253 nklmd=k+l+m+n;
06254 n2d=n+2;
06255
06256 n_larg=klmd*2;
06257 klm2d=n_larg+klm2d;
06258 klmd=n_larg+klmd;
06259 nklmd=n_larg+nklmd;
06260 int size_q=klm2d*n2d;
06261 int size_cu=nklmd*2;
06262 static int i__;
06263
06264 double *q ;
06265 double *x ;
06266 double *res;
06267 double *cu;
06268 float *q2;
06269 float *pw_;
06270 long int *iu;
06271 double *s;
06272 q = (double*)calloc(size_q,sizeof(double));
06273 x = (double*)calloc(n2d,sizeof(double));
06274 res = (double*)calloc(klmd,sizeof(double));
06275 cu =(double*)calloc(size_cu,sizeof(double));
06276 s = (double*)calloc(klmd,sizeof(double));
06277 q2 = (float*)calloc(size_q,sizeof(float));
06278 iu = (long int*)calloc(size_cu,sizeof(long int));
06279 pw_ = (float*)calloc(k,sizeof(float));
06280
06281 for( i__ =0;i__<k;++i__)
06282 {
06283 pw_[i__]=log(pw[i__]); }
06284 long int l_k=k;
06285 long int l_n=n;
06286 long int l_iswi=iswi;
06287 vector<float> cl1_res;
06288 cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06289 free(q);
06290 free(x);
06291 free(res);
06292 free(s);
06293 free(cu);
06294 free(q2);
06295 free(iu);
06296 free(pw_);
06297 return cl1_res;
06298 }
06299 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)
06300 {
06301 long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06302 float r__1;
06303 int tmp__i;
06304 long int i__, j;
06305 --s;
06306 --res;
06307 iu -= 3;
06308 cu -= 3;
06309 --x;
06310 long int klm2d;
06311 klm2d= *k+*k+2;
06312 klm2d=klm2d+klm2d;
06313 q_dim1 = klm2d;
06314 q_offset = 1 + q_dim1;
06315 q -= q_offset;
06316 q2_dim1 = klm2d;
06317 q2_offset = 1 + q2_dim1;
06318 q2 -= q2_offset;
06319 i__2=0;
06320 i__1 = *n - 1;
06321 tmp__i=0;
06322 for (j = 1; j <= i__1; ++j) {
06323 i__2 = *k;
06324 tmp__i+=1;
06325 for (i__ = 1; i__ <= i__2; ++i__) {
06326 r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06327 q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06328 }
06329 }
06330 for (i__ = 1; i__ <= i__2; ++i__)
06331 { q2[i__ + *n * q2_dim1] = 1.f;
06332 q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06333 }
06334 vector<float> fit_res;
06335 fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06336 return fit_res;
06337 }
06338 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)
06339 {
06340
06341 long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06342
06343
06344 long int i__, j, m, n1, ii, jj;
06345 double tmp;
06346 vector<float> p;
06347 --x;
06348 q_dim1 = *klm2d;
06349 q_offset = 1 + q_dim1;
06350 q -= q_offset;
06351 q1_dim1 = *klm2d;
06352 q1_offset = 1 + q1_dim1;
06353 q1 -= q1_offset;
06354 --s;
06355 --res;
06356 iu -= 3;
06357 cu -= 3;
06358
06359
06360 long int l = 0;
06361
06362
06363 m = *ks;
06364 n1 = *n + 1;
06365 if (*iswi == 1) {
06366 i__1 = n1;
06367 for (jj = 1; jj <= i__1; ++jj) {
06368 i__2 = *ks;
06369 for (ii = 1; ii <= i__2; ++ii) {
06370
06371
06372 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06373 ;
06374 }
06375 }
06376 } else if (*iswi == 2) {
06377 i__1 = *ks;
06378 for (ii = 1; ii <= i__1; ++ii) {
06379 i__2 = n1;
06380 for (jj = 1; jj <= i__2; ++jj) {
06381 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06382 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06383 }
06384 }
06385 } else if (*iswi == 3) {
06386 l = 2;
06387 i__1 = n1;
06388 for (jj = 1; jj <= i__1; ++jj) {
06389 i__2 = *ks + 2;
06390 for (ii = 1; ii <= i__2; ++ii) {
06391 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06392 }
06393 i__2 = *ks;
06394 for (ii = 1; ii <= i__2; ++ii) {
06395 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06396 }
06397 }
06398 } else if (*iswi == 4) {
06399 l = 2;
06400 i__1 = n1;
06401 for (jj = 1; jj <= i__1; ++jj) {
06402 i__2 = *ks + 2;
06403 for (ii = 1; ii <= i__2; ++ii) {
06404 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06405 }
06406 i__2 = *ks;
06407 for (ii = 1; ii <= i__2; ++ii) {
06408 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06409 }
06410 }
06411 } else if (*iswi == 5) {
06412 l = 1;
06413 i__1 = n1;
06414 for (jj = 1; jj <= i__1; ++jj) {
06415 i__2 = *ks + 1;
06416 for (ii = 1; ii <= i__2; ++ii) {
06417 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06418 }
06419 i__2 = *ks;
06420 for (ii = 1; ii <= i__2; ++ii) {
06421 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06422 }
06423 }
06424 } else if (*iswi == 6) {
06425 l = 1;
06426 i__1 = n1;
06427 for (jj = 1; jj <= i__1; ++jj) {
06428 i__2 = *ks + 1;
06429 for (ii = 1; ii <= i__2; ++ii) {
06430 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06431 }
06432 i__2 = *ks;
06433 for (ii = 1; ii <= i__2; ++ii) {
06434 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06435 }
06436 }
06437 } else if (*iswi == 7) {
06438 l = 3;
06439 i__1 = n1;
06440 for (jj = 1; jj <= i__1; ++jj) {
06441 i__2 = *ks + 3;
06442 for (ii = 1; ii <= i__2; ++ii) {
06443 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06444 }
06445 i__2 = *ks;
06446 for (ii = 1; ii <= i__2; ++ii) {
06447 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06448 }
06449 }
06450 } else if (*iswi == 8) {
06451 l = 4;
06452 i__1 = n1;
06453 for (jj = 1; jj <= i__1; ++jj) {
06454 i__2 = *ks + 4;
06455 for (ii = 1; ii <= i__2; ++ii) {
06456 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06457 }
06458 i__2 = *ks;
06459 for (ii = 1; ii <= i__2; ++ii) {
06460 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06461 }
06462 }
06463 }
06464
06465 Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06466 i__1 = *ks;
06467 int tmp__j=0;
06468 for (i__ = 1; i__ <= i__1; ++i__) {
06469 tmp = 0.f;
06470 i__2 = *n - 1;
06471 for (j = 1; j <= i__2; ++j) {
06472 tmp__j=j;
06473 tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06474 }
06475 tmp += x[*n];
06476 p.push_back(static_cast<float>(exp(tmp)));
06477 p.push_back(q1[i__ + q1_dim1]);
06478 }
06479 i__2=*n;
06480 for (i__=1;i__<=i__2;++i__)
06481 { p.push_back(static_cast<float>(x[i__]));}
06482 return p;
06483 }
06484 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06485 double *q, double *x, double *res, double *cu, long int *iu, double *s)
06486 {
06487
06488 long int q_dim1, q_offset, i__1, i__2;
06489 double d__1;
06490
06491 static long int i__, j;
06492 static double z__;
06493 static long int n1, n2, ia, ii, kk, in, nk, js;
06494 static double sn, zu, zv;
06495 static long int nk1, klm, nkl, jmn, jpn;
06496 static double cuv;
06497 static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06498 static float xmin;
06499 static double xmax;
06500 static long int iout;
06501 static double xsum;
06502 static long int iineg, maxit;
06503 static double toler;
06504 static float error;
06505 static double pivot;
06506 static long int kforce, iphase;
06507 static double tpivot;
06508
06509 --s;
06510 --res;
06511 iu -= 3;
06512 cu -= 3;
06513 --x;
06514 q_dim1 = *klm2d;
06515 q_offset = 1 + q_dim1;
06516 q -= q_offset;
06517
06518
06519 maxit = 500;
06520 kode = 0;
06521 toler = 1e-4f;
06522 iter = 0;
06523 n1 = *n + 1;
06524 n2 = *n + 2;
06525 nk = *n + *k;
06526 nk1 = nk + 1;
06527 nkl = nk + *l;
06528 nkl1 = nkl + 1;
06529 klm = *k + *l + *m;
06530 klm1 = klm + 1;
06531 klm2 = klm + 2;
06532 nklm = *n + klm;
06533 kforce = 1;
06534 iter = 0;
06535 js = 1;
06536 ia = 0;
06537
06538 i__1 = *n;
06539 for (j = 1; j <= i__1; ++j) {
06540 q[klm2 + j * q_dim1] = (double) j;
06541
06542 }
06543 i__1 = klm;
06544 for (i__ = 1; i__ <= i__1; ++i__) {
06545 q[i__ + n2 * q_dim1] = (double) (*n + i__);
06546 if (q[i__ + n1 * q_dim1] >= 0.f) {
06547 goto L30;
06548 }
06549 i__2 = n2;
06550 for (j = 1; j <= i__2; ++j) {
06551 q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06552
06553 }
06554 L30:
06555 ;
06556 }
06557
06558 iphase = 2;
06559 i__1 = nklm;
06560 for (j = 1; j <= i__1; ++j) {
06561 cu[(j << 1) + 1] = 0.f;
06562 cu[(j << 1) + 2] = 0.f;
06563 iu[(j << 1) + 1] = 0;
06564 iu[(j << 1) + 2] = 0;
06565
06566 }
06567 if (*l == 0) {
06568 goto L60;
06569 }
06570 i__1 = nkl;
06571 for (j = nk1; j <= i__1; ++j) {
06572 cu[(j << 1) + 1] = 1.f;
06573 cu[(j << 1) + 2] = 1.f;
06574 iu[(j << 1) + 1] = 1;
06575 iu[(j << 1) + 2] = 1;
06576
06577 }
06578 iphase = 1;
06579 L60:
06580 if (*m == 0) {
06581 goto L80;
06582 }
06583 i__1 = nklm;
06584 for (j = nkl1; j <= i__1; ++j) {
06585 cu[(j << 1) + 2] = 1.f;
06586 iu[(j << 1) + 2] = 1;
06587 jmn = j - *n;
06588 if (q[jmn + n2 * q_dim1] < 0.f) {
06589 iphase = 1;
06590 }
06591
06592 }
06593 L80:
06594 if (kode == 0) {
06595 goto L150;
06596 }
06597 i__1 = *n;
06598 for (j = 1; j <= i__1; ++j) {
06599 if ((d__1 = x[j]) < 0.) {
06600 goto L90;
06601 } else if (d__1 == 0) {
06602 goto L110;
06603 } else {
06604 goto L100;
06605 }
06606 L90:
06607 cu[(j << 1) + 1] = 1.f;
06608 iu[(j << 1) + 1] = 1;
06609 goto L110;
06610 L100:
06611 cu[(j << 1) + 2] = 1.f;
06612 iu[(j << 1) + 2] = 1;
06613 L110:
06614 ;
06615 }
06616 i__1 = *k;
06617 for (j = 1; j <= i__1; ++j) {
06618 jpn = j + *n;
06619 if ((d__1 = res[j]) < 0.) {
06620 goto L120;
06621 } else if (d__1 == 0) {
06622 goto L140;
06623 } else {
06624 goto L130;
06625 }
06626 L120:
06627 cu[(jpn << 1) + 1] = 1.f;
06628 iu[(jpn << 1) + 1] = 1;
06629 if (q[j + n2 * q_dim1] > 0.f) {
06630 iphase = 1;
06631 }
06632 goto L140;
06633 L130:
06634 cu[(jpn << 1) + 2] = 1.f;
06635 iu[(jpn << 1) + 2] = 1;
06636 if (q[j + n2 * q_dim1] < 0.f) {
06637 iphase = 1;
06638 }
06639 L140:
06640 ;
06641 }
06642 L150:
06643 if (iphase == 2) {
06644 goto L500;
06645 }
06646
06647 L160:
06648 i__1 = n1;
06649 for (j = js; j <= i__1; ++j) {
06650 xsum = 0.;
06651 i__2 = klm;
06652 for (i__ = 1; i__ <= i__2; ++i__) {
06653 ii = (long int) q[i__ + n2 * q_dim1];
06654 if (ii < 0) {
06655 goto L170;
06656 }
06657 z__ = cu[(ii << 1) + 1];
06658 goto L180;
06659 L170:
06660 iineg = -ii;
06661 z__ = cu[(iineg << 1) + 2];
06662 L180:
06663 xsum += q[i__ + j * q_dim1] * z__;
06664
06665
06666 }
06667 q[klm1 + j * q_dim1] = xsum;
06668
06669 }
06670 i__1 = *n;
06671 for (j = js; j <= i__1; ++j) {
06672 ii = (long int) q[klm2 + j * q_dim1];
06673 if (ii < 0) {
06674 goto L210;
06675 }
06676 z__ = cu[(ii << 1) + 1];
06677 goto L220;
06678 L210:
06679 iineg = -ii;
06680 z__ = cu[(iineg << 1) + 2];
06681 L220:
06682 q[klm1 + j * q_dim1] -= z__;
06683
06684 }
06685
06686 L240:
06687 xmax = 0.f;
06688 if (js > *n) {
06689 goto L490;
06690 }
06691 i__1 = *n;
06692 for (j = js; j <= i__1; ++j) {
06693 zu = q[klm1 + j * q_dim1];
06694 ii = (long int) q[klm2 + j * q_dim1];
06695 if (ii > 0) {
06696 goto L250;
06697 }
06698 ii = -ii;
06699 zv = zu;
06700 zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06701 goto L260;
06702 L250:
06703 zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06704 L260:
06705 if (kforce == 1 && ii > *n) {
06706 goto L280;
06707 }
06708 if (iu[(ii << 1) + 1] == 1) {
06709 goto L270;
06710 }
06711 if (zu <= xmax) {
06712 goto L270;
06713 }
06714 xmax = zu;
06715 in = j;
06716 L270:
06717 if (iu[(ii << 1) + 2] == 1) {
06718 goto L280;
06719 }
06720 if (zv <= xmax) {
06721 goto L280;
06722 }
06723 xmax = zv;
06724 in = j;
06725 L280:
06726 ;
06727 }
06728 if (xmax <= toler) {
06729 goto L490;
06730 }
06731 if (q[klm1 + in * q_dim1] == xmax) {
06732 goto L300;
06733 }
06734 i__1 = klm2;
06735 for (i__ = 1; i__ <= i__1; ++i__) {
06736 q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06737
06738 }
06739 q[klm1 + in * q_dim1] = xmax;
06740
06741 L300:
06742 if (iphase == 1 || ia == 0) {
06743 goto L330;
06744 }
06745 xmax = 0.f;
06746 i__1 = ia;
06747 for (i__ = 1; i__ <= i__1; ++i__) {
06748 z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06749 if (z__ <= xmax) {
06750 goto L310;
06751 }
06752 xmax = z__;
06753 iout = i__;
06754 L310:
06755 ;
06756 }
06757 if (xmax <= toler) {
06758 goto L330;
06759 }
06760 i__1 = n2;
06761 for (j = 1; j <= i__1; ++j) {
06762 z__ = q[ia + j * q_dim1];
06763 q[ia + j * q_dim1] = q[iout + j * q_dim1];
06764 q[iout + j * q_dim1] = z__;
06765
06766 }
06767 iout = ia;
06768 --ia;
06769 pivot = q[iout + in * q_dim1];
06770 goto L420;
06771 L330:
06772 kk = 0;
06773 i__1 = klm;
06774 for (i__ = 1; i__ <= i__1; ++i__) {
06775 z__ = q[i__ + in * q_dim1];
06776 if (z__ <= toler) {
06777 goto L340;
06778 }
06779 ++kk;
06780 res[kk] = q[i__ + n1 * q_dim1] / z__;
06781 s[kk] = (double) i__;
06782 L340:
06783 ;
06784 }
06785 L350:
06786 if (kk > 0) {
06787 goto L360;
06788 }
06789 kode = 2;
06790 goto L590;
06791 L360:
06792 xmin = static_cast<float>( res[1] );
06793 iout = (long int) s[1];
06794 j = 1;
06795 if (kk == 1) {
06796 goto L380;
06797 }
06798 i__1 = kk;
06799 for (i__ = 2; i__ <= i__1; ++i__) {
06800 if (res[i__] >= xmin) {
06801 goto L370;
06802 }
06803 j = i__;
06804 xmin = static_cast<float>( res[i__] );
06805 iout = (long int) s[i__];
06806 L370:
06807 ;
06808 }
06809 res[j] = res[kk];
06810 s[j] = s[kk];
06811 L380:
06812 --kk;
06813 pivot = q[iout + in * q_dim1];
06814 ii = (long int) q[iout + n2 * q_dim1];
06815 if (iphase == 1) {
06816 goto L400;
06817 }
06818 if (ii < 0) {
06819 goto L390;
06820 }
06821 if (iu[(ii << 1) + 2] == 1) {
06822 goto L420;
06823 }
06824 goto L400;
06825 L390:
06826 iineg = -ii;
06827 if (iu[(iineg << 1) + 1] == 1) {
06828 goto L420;
06829 }
06830
06831 L400:
06832 ii = abs(ii);
06833 cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06834 if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06835 goto L420;
06836 }
06837
06838 i__1 = n1;
06839 for (j = js; j <= i__1; ++j) {
06840 z__ = q[iout + j * q_dim1];
06841 q[klm1 + j * q_dim1] -= z__ * cuv;
06842 q[iout + j * q_dim1] = -z__;
06843
06844 }
06845 q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06846 goto L350;
06847
06848 L420:
06849 if (iter < maxit) {
06850 goto L430;
06851 }
06852 kode = 3;
06853 goto L590;
06854 L430:
06855 ++iter;
06856 i__1 = n1;
06857 for (j = js; j <= i__1; ++j) {
06858 if (j != in) {
06859 q[iout + j * q_dim1] /= pivot;
06860 }
06861
06862 }
06863
06864
06865
06866
06867
06868
06869
06870
06871 i__1 = n1;
06872 for (j = js; j <= i__1; ++j) {
06873 if (j == in) {
06874 goto L460;
06875 }
06876 z__ = -q[iout + j * q_dim1];
06877 i__2 = klm1;
06878 for (i__ = 1; i__ <= i__2; ++i__) {
06879 if (i__ != iout) {
06880 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06881 }
06882
06883 }
06884 L460:
06885 ;
06886 }
06887 tpivot = -pivot;
06888 i__1 = klm1;
06889 for (i__ = 1; i__ <= i__1; ++i__) {
06890 if (i__ != iout) {
06891 q[i__ + in * q_dim1] /= tpivot;
06892 }
06893
06894 }
06895 q[iout + in * q_dim1] = 1.f / pivot;
06896 z__ = q[iout + n2 * q_dim1];
06897 q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
06898 q[klm2 + in * q_dim1] = z__;
06899 ii = (long int) abs(z__);
06900 if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
06901 goto L240;
06902 }
06903 i__1 = klm2;
06904 for (i__ = 1; i__ <= i__1; ++i__) {
06905 z__ = q[i__ + in * q_dim1];
06906 q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
06907 q[i__ + js * q_dim1] = z__;
06908
06909 }
06910 ++js;
06911 goto L240;
06912
06913 L490:
06914 if (kforce == 0) {
06915 goto L580;
06916 }
06917 if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
06918 goto L500;
06919 }
06920 kforce = 0;
06921 goto L240;
06922
06923 L500:
06924 iphase = 2;
06925 i__1 = nklm;
06926 for (j = 1; j <= i__1; ++j) {
06927 cu[(j << 1) + 1] = 0.f;
06928 cu[(j << 1) + 2] = 0.f;
06929
06930 }
06931 i__1 = nk;
06932 for (j = n1; j <= i__1; ++j) {
06933 cu[(j << 1) + 1] = 1.f;
06934 cu[(j << 1) + 2] = 1.f;
06935
06936 }
06937 i__1 = klm;
06938 for (i__ = 1; i__ <= i__1; ++i__) {
06939 ii = (long int) q[i__ + n2 * q_dim1];
06940 if (ii > 0) {
06941 goto L530;
06942 }
06943 ii = -ii;
06944 if (iu[(ii << 1) + 2] == 0) {
06945 goto L560;
06946 }
06947 cu[(ii << 1) + 2] = 0.f;
06948 goto L540;
06949 L530:
06950 if (iu[(ii << 1) + 1] == 0) {
06951 goto L560;
06952 }
06953 cu[(ii << 1) + 1] = 0.f;
06954 L540:
06955 ++ia;
06956 i__2 = n2;
06957 for (j = 1; j <= i__2; ++j) {
06958 z__ = q[ia + j * q_dim1];
06959 q[ia + j * q_dim1] = q[i__ + j * q_dim1];
06960 q[i__ + j * q_dim1] = z__;
06961
06962 }
06963 L560:
06964 ;
06965 }
06966 goto L160;
06967 L570:
06968 if (q[klm1 + n1 * q_dim1] <= toler) {
06969 goto L500;
06970 }
06971 kode = 1;
06972 goto L590;
06973 L580:
06974 if (iphase == 1) {
06975 goto L570;
06976 }
06977
06978 kode = 0;
06979 L590:
06980 xsum = 0.;
06981 i__1 = *n;
06982 for (j = 1; j <= i__1; ++j) {
06983 x[j] = 0.f;
06984
06985 }
06986 i__1 = klm;
06987 for (i__ = 1; i__ <= i__1; ++i__) {
06988 res[i__] = 0.f;
06989
06990 }
06991 i__1 = klm;
06992 for (i__ = 1; i__ <= i__1; ++i__) {
06993 ii = (long int) q[i__ + n2 * q_dim1];
06994 sn = 1.f;
06995 if (ii > 0) {
06996 goto L620;
06997 }
06998 ii = -ii;
06999 sn = -1.f;
07000 L620:
07001 if (ii > *n) {
07002 goto L630;
07003 }
07004 x[ii] = sn * q[i__ + n1 * q_dim1];
07005 goto L640;
07006 L630:
07007 iimn = ii - *n;
07008 res[iimn] = sn * q[i__ + n1 * q_dim1];
07009 if (ii >= n1 && ii <= nk) {
07010 xsum += q[i__ + n1 * q_dim1];
07011 }
07012 L640:
07013 ;
07014 }
07015 error = (float)xsum;
07016 return;
07017 }
07018
07019 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07020 {
07021 int j,d;
07022 EMData * e = new EMData();
07023 float *eptr, *imgptr;
07024 imgptr = img->get_data();
07025 float SSE = 0.f;
07026 for (j = 0 ; j < N ; j++) {
07027 e->read_image(images,S[j]);
07028 eptr = e->get_data();
07029 for (d = 0; d < size; d++) {
07030 SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07031 }
07032 delete e;
07033 return SSE;
07034 }
07035
07036
07037 #define mymax(x,y) (((x)>(y))?(x):(y))
07038 #define mymin(x,y) (((x)<(y))?(x):(y))
07039 #define sign(x,y) (((((y)>0)?(1):(-1))*(y!=0))*(x))
07040
07041
07042 #define quadpi 3.141592653589793238462643383279502884197
07043 #define dgr_to_rad quadpi/180
07044 #define deg_to_rad quadpi/180
07045 #define rad_to_deg 180/quadpi
07046 #define rad_to_dgr 180/quadpi
07047 #define TRUE 1
07048 #define FALSE 0
07049
07050
07051 #define theta(i) theta [i-1]
07052 #define phi(i) phi [i-1]
07053 #define weight(i) weight [i-1]
07054 #define lband(i) lband [i-1]
07055 #define ts(i) ts [i-1]
07056 #define thetast(i) thetast [i-1]
07057 #define key(i) key [i-1]
07058
07059
07060 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07061 {
07062
07063 ENTERFUNC;
07064
07065 if ( th.size() != ph.size() ) {
07066 LOGERR("images not same size");
07067 throw ImageFormatException( "images not same size");
07068 }
07069
07070
07071 srand(10);
07072
07073 int i,*key;
07074 int len = th.size();
07075 double *theta,*phi,*weight;
07076 theta = (double*) calloc(len,sizeof(double));
07077 phi = (double*) calloc(len,sizeof(double));
07078 weight = (double*) calloc(len,sizeof(double));
07079 key = (int*) calloc(len,sizeof(int));
07080 const float *thptr, *phptr;
07081
07082 thptr = &th[0];
07083 phptr = &ph[0];
07084 for(i=1;i<=len;i++){
07085 key(i) = i;
07086 weight(i) = 0.0;
07087 }
07088
07089 for(i = 0;i<len;i++){
07090 theta[i] = thptr[i];
07091 phi[i] = phptr[i];
07092 }
07093
07094
07095 Util::hsortd(theta, phi, key, len, 1);
07096
07097
07098 Util::voronoi(phi, theta, weight, len);
07099
07100
07101 Util::hsortd(weight, weight, key, len, 2);
07102
07103 free(theta);
07104 free(phi);
07105 free(key);
07106 vector<double> wt;
07107 double count = 0;
07108 for(i=1; i<= len; i++)
07109 {
07110 wt.push_back(weight(i));
07111 count += weight(i);
07112 }
07113
07114
07115
07116
07117
07118
07119 free(weight);
07120
07121 EXITFUNC;
07122 return wt;
07123
07124 }
07125
07126 struct tmpstruct{
07127 double theta1,phi1;
07128 int key1;
07129 };
07130
07131 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07132 {
07133 ENTERFUNC;
07134 vector<tmpstruct> tmp(len);
07135 int i;
07136 for(i = 1;i<=len;i++)
07137 {
07138 tmp[i-1].theta1 = theta(i);
07139 tmp[i-1].phi1 = phi(i);
07140 tmp[i-1].key1 = key(i);
07141 }
07142
07143 if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07144 if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07145
07146 for(i = 1;i<=len;i++)
07147 {
07148 theta(i) = tmp[i-1].theta1;
07149 phi(i) = tmp[i-1].phi1;
07150 key(i) = tmp[i-1].key1;
07151 }
07152 EXITFUNC;
07153 }
07154
07155 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07156 {
07157 return(tmp1.theta1 < tmp2.theta1);
07158 }
07159
07160 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07161 {
07162 return(tmp1.key1 < tmp2.key1);
07163 }
07164
07165
07166
07167
07168
07169
07170
07171
07172
07173
07174
07175
07176
07177
07178
07179
07180
07181
07182
07183
07184
07185
07186
07187
07188
07189
07190
07191
07192
07193
07194
07195
07196
07197
07198
07199
07200
07201
07202
07203
07204
07205
07206
07207
07208
07209
07210
07211
07212
07213
07214
07215
07216
07217
07218
07219
07220
07221
07222
07223
07224
07225
07226
07227
07228
07229
07230
07231
07232
07233
07234
07235
07236
07237
07238
07239
07240
07241
07242
07243
07244
07245
07246
07247
07248
07249
07250
07251
07252
07253
07254
07255
07256
07257
07258
07259
07260
07261
07262
07263
07264
07265
07266
07267
07268
07269
07270
07271
07272
07273
07274
07275
07276
07277
07278
07279
07280
07281
07282
07283
07284
07285
07286
07287
07288
07289
07290
07291
07292
07293
07294
07295
07296
07297
07298
07299
07300
07301
07302
07303
07304
07305
07306
07307
07308
07309
07310
07311
07312
07313
07314
07315
07316
07317
07318
07319
07320
07321
07322
07323
07324
07325
07326
07327
07328
07329
07330
07331
07332
07333
07334
07335
07336
07337
07338
07339
07340
07341
07342
07343
07344
07345
07346
07347
07348
07349
07350
07351
07352
07353
07354
07355
07356
07357
07358
07359
07360
07361
07362
07363
07364
07365
07366
07367
07368
07369
07370
07371
07372
07373
07374
07375
07376
07377
07378
07379
07380
07381
07382
07383
07384
07385
07386
07387
07388
07389
07390
07391
07392
07393
07394
07395
07396
07397
07398
07399
07400
07401
07402
07403
07404
07405
07406
07407
07408
07409
07410
07411
07412
07413
07414
07415
07416
07417
07418
07419
07420
07421
07422
07423
07424
07425
07426
07427
07428
07429
07430
07431
07432
07433
07434
07435
07436
07437
07438
07439
07440
07441
07442
07443
07444
07445
07446
07447
07448
07449
07450
07451
07452
07453
07454
07455
07456
07457
07458
07459
07460
07461
07462
07463
07464
07465
07466
07467
07468
07469
07470
07471
07472
07473
07474
07475
07476
07477
07478
07479
07480
07481
07482
07483 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07484 {
07485
07486 ENTERFUNC;
07487
07488 int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07489 int nt6, n, ier, nout, lnew, mdup, nd;
07490 int i,k,mt,status;
07491
07492
07493 double *ds, *x, *y, *z;
07494 double tol = 1.0e-8;
07495 double dtol = 15;
07496 double a;
07497
07498
07499
07500
07501
07502
07503
07504
07505
07506 n = nt + nt;
07507
07508 nt6 = n*6;
07509
07510 list = (int*)calloc(nt6,sizeof(int));
07511 lptr = (int*)calloc(nt6,sizeof(int));
07512 lend = (int*)calloc(n ,sizeof(int));
07513 iwk = (int*)calloc(n ,sizeof(int));
07514 good = (int*)calloc(n ,sizeof(int));
07515 key = (int*)calloc(n ,sizeof(int));
07516 indx = (int*)calloc(n ,sizeof(int));
07517 lcnt = (int*)calloc(n ,sizeof(int));
07518
07519 ds = (double*) calloc(n,sizeof(double));
07520 x = (double*) calloc(n,sizeof(double));
07521 y = (double*) calloc(n,sizeof(double));
07522 z = (double*) calloc(n,sizeof(double));
07523
07524 if (list == NULL ||
07525 lptr == NULL ||
07526 lend == NULL ||
07527 iwk == NULL ||
07528 good == NULL ||
07529 key == NULL ||
07530 indx == NULL ||
07531 lcnt == NULL ||
07532 x == NULL ||
07533 y == NULL ||
07534 z == NULL ||
07535 ds == NULL) {
07536 printf("memory allocation failure!\n");
07537 exit(1);
07538 }
07539
07540 bool colinear=true;
07541 while(colinear)
07542 {
07543
07544 L1:
07545 for(i = 0; i<nt; i++){
07546 x[i] = theta[i];
07547 y[i] = phi[i];
07548 x[nt+i] = 180.0 - x[i];
07549 y[nt+i] = 180.0 + y[i];
07550 }
07551
07552 Util::disorder2(x, y, key, n);
07553
07554
07555 double val;
07556 for(k=0; k<2; k++){
07557 for(i=k+1; i<3; i++){
07558 val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07559 if( val < dtol) {
07560 goto L1;
07561 }
07562 }
07563 }
07564
07565 Util::ang_to_xyz(x, y, z, n);
07566
07567
07568 bool dupnode=true;
07569 dupnode=true;
07570 while(dupnode)
07571 {
07572 for(k=0; k<2; k++){
07573 for(i=k+1; i<3; i++){
07574 if( x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07575 Util::flip23(x, y, z, key, k, n);
07576 continue;
07577 }
07578 }
07579 }
07580 dupnode = false;
07581 }
07582
07583
07584 ier = 0;
07585
07586 status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07587
07588 if (status != 0) {
07589 printf(" error in trmsh3 \n");
07590 exit(1);
07591 }
07592
07593 if (ier > 0) {
07594 printf("*** Error in TRMESH: duplicate nodes encountered ***\n");
07595 exit(1);
07596 }
07597
07598 mdup=n-nout;
07599 if (ier == -2) {
07600
07601 }
07602 else
07603 {
07604 colinear=false;
07605 }
07606 }
07607
07608
07609 Assert( ier != -2 );
07610
07611
07612 nd=0;
07613 for (k=1; k<=n; k++){
07614 if (indx[k-1]>0) {
07615 nd++;
07616 good[nd-1]=k;
07617 }
07618 }
07619
07620
07621
07622
07623 for(i = 1; i<=nout; i++) {
07624 k=good[i-1];
07625
07626 if (key[k-1] <= nt) {
07627
07628 a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07629 if (ier != 0){
07630
07631
07632 printf(" *** error in areav: ier = %d ***\n", ier);
07633 weight[key[k-1]-1] =-1.0;
07634 } else {
07635
07636 weight[key[k-1]-1]=a/lcnt[i-1];
07637 }
07638 }
07639 }
07640
07641
07642
07643 for(i = 1; i<=n; i++){
07644 mt =- indx[i-1];
07645 if (mt>0){
07646 k = good[mt-1];
07647
07648
07649
07650 if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07651 }
07652 }
07653
07654 free(list);
07655 free(lend);
07656 free(iwk);
07657 free(good);
07658 free(key);
07659 free(lptr);
07660 free(indx);
07661 free(lcnt);
07662 free(ds);
07663 free(x);
07664 free(y);
07665 free(z);
07666
07667
07668 EXITFUNC;
07669 }
07670
07671 void Util::disorder2(double *x,double *y, int *key, int len)
07672 {
07673 ENTERFUNC;
07674 int k, i;
07675 for(i=0; i<len; i++) key[i]=i+1;
07676
07677 for(i = 0; i<len;i++){
07678 k = rand()%len;
07679 std::swap(key[k], key[i]);
07680 std::swap(x[k], x[i]);
07681 std::swap(y[k], y[i]);
07682 }
07683 EXITFUNC;
07684 }
07685
07686 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07687 {
07688 ENTERFUNC;
07689 double costheta,sintheta,cosphi,sinphi;
07690 for(int i = 0; i<len; i++)
07691 {
07692 cosphi = cos(y[i]*dgr_to_rad);
07693 sinphi = sin(y[i]*dgr_to_rad);
07694 if(fabs(x[i]-90.0)< 1.0e-5){
07695 x[i] = cosphi;
07696 y[i] = sinphi;
07697 z[i] = 0.0;
07698 }
07699 else{
07700 costheta = cos(x[i]*dgr_to_rad);
07701 sintheta = sin(x[i]*dgr_to_rad);
07702 x[i] = cosphi*sintheta;
07703 y[i] = sinphi*sintheta;
07704 z[i] = costheta;
07705 }
07706 }
07707 EXITFUNC;
07708 }
07709
07710 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07711 {
07712 ENTERFUNC;
07713 int i = k;
07714 while( i == k ) i = rand()%len;
07715 std::swap(key[i], key[k]);
07716 std::swap(x[i], x[k]);
07717 std::swap(y[i], y[k]);
07718 std::swap(z[i], z[k]);
07719 EXITFUNC;
07720 }
07721
07722
07723 #undef mymax
07724 #undef mymin
07725 #undef sign
07726 #undef quadpi
07727 #undef dgr_to_rad
07728 #undef deg_to_rad
07729 #undef rad_to_deg
07730 #undef rad_to_dgr
07731 #undef TRUE
07732 #undef FALSE
07733 #undef theta
07734 #undef phi
07735 #undef weight
07736 #undef lband
07737 #undef ts
07738 #undef thetast
07739 #undef key
07740
07741
07742
07743
07744
07745
07746
07747
07748
07749
07750
07751 #define TRUE_ (1)
07752 #define FALSE_ (0)
07753 #define abs(x) ((x) >= 0 ? (x) : -(x))
07754
07755 struct stcom_{
07756 double y;
07757 };
07758 stcom_ stcom_1;
07759 #ifdef KR_headers
07760 double floor();
07761 int i_dnnt(x) double *x;
07762 #else
07763 int i_dnnt(double *x)
07764 #endif
07765 {
07766 return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07767 }
07768
07769
07770
07771
07772
07773 int Util::trmsh3_(int *n0, double *tol, double *x,
07774 double *y, double *z__, int *n, int *list, int *
07775 lptr, int *lend, int *lnew, int *indx, int *lcnt,
07776 int *near__, int *next, double *dist, int *ier)
07777 {
07778
07779 int i__1, i__2;
07780
07781
07782 static double d__;
07783 static int i__, j;
07784 static double d1, d2, d3;
07785 static int i0, lp, kt, ku, lpl, nku;
07786 extern long int left_(double *, double *, double *, double
07787 *, double *, double *, double *, double *,
07788 double *);
07789 static int nexti;
07790 extern int addnod_(int *, int *, double *,
07791 double *, double *, int *, int *, int *,
07792 int *, int *);
07793
07794
07795
07796
07797
07798
07799
07800
07801
07802
07803
07804
07805
07806
07807
07808
07809
07810
07811
07812
07813
07814
07815
07816
07817
07818
07819
07820
07821
07822
07823
07824
07825
07826
07827
07828
07829
07830
07831
07832
07833
07834
07835
07836
07837
07838
07839
07840
07841
07842
07843
07844
07845
07846
07847
07848
07849
07850
07851
07852
07853
07854
07855
07856
07857
07858
07859
07860
07861
07862
07863
07864
07865
07866
07867
07868
07869
07870
07871
07872
07873
07874
07875
07876
07877
07878
07879
07880
07881
07882
07883
07884
07885
07886
07887
07888
07889
07890
07891
07892
07893
07894
07895
07896
07897
07898
07899
07900
07901
07902
07903
07904
07905
07906
07907
07908
07909
07910
07911
07912
07913
07914
07915
07916
07917
07918
07919
07920
07921
07922
07923
07924
07925
07926
07927
07928
07929
07930
07931
07932
07933
07934
07935
07936
07937
07938
07939
07940
07941
07942
07943
07944 --dist;
07945 --next;
07946 --near__;
07947 --indx;
07948 --lend;
07949 --z__;
07950 --y;
07951 --x;
07952 --list;
07953 --lptr;
07954 --lcnt;
07955
07956
07957 if (*n0 < 3) {
07958 *n = 0;
07959 *ier = -1;
07960 return 0;
07961 }
07962
07963
07964
07965 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
07966 z__[3])) {
07967
07968
07969
07970 list[1] = 3;
07971 lptr[1] = 2;
07972 list[2] = -2;
07973 lptr[2] = 1;
07974 lend[1] = 2;
07975
07976 list[3] = 1;
07977 lptr[3] = 4;
07978 list[4] = -3;
07979 lptr[4] = 3;
07980 lend[2] = 4;
07981
07982 list[5] = 2;
07983 lptr[5] = 6;
07984 list[6] = -1;
07985 lptr[6] = 5;
07986 lend[3] = 6;
07987
07988 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
07989 y[3], &z__[3])) {
07990
07991
07992
07993
07994
07995 list[1] = 2;
07996 lptr[1] = 2;
07997 list[2] = -3;
07998 lptr[2] = 1;
07999 lend[1] = 2;
08000
08001 list[3] = 3;
08002 lptr[3] = 4;
08003 list[4] = -1;
08004 lptr[4] = 3;
08005 lend[2] = 4;
08006
08007 list[5] = 1;
08008 lptr[5] = 6;
08009 list[6] = -2;
08010 lptr[6] = 5;
08011 lend[3] = 6;
08012
08013
08014 } else {
08015
08016
08017
08018 *n = 0;
08019 *ier = -2;
08020 return 0;
08021 }
08022
08023
08024
08025
08026
08027 *lnew = 7;
08028 indx[1] = 1;
08029 indx[2] = 2;
08030 indx[3] = 3;
08031 lcnt[1] = 1;
08032 lcnt[2] = 1;
08033 lcnt[3] = 1;
08034 if (*n0 == 3) {
08035 *n = 3;
08036 *ier = 0;
08037 return 0;
08038 }
08039
08040
08041
08042
08043
08044
08045
08046
08047
08048
08049
08050
08051
08052
08053
08054
08055
08056
08057
08058
08059
08060
08061
08062
08063
08064
08065
08066 near__[1] = 0;
08067 near__[2] = 0;
08068 near__[3] = 0;
08069 for (ku = *n0; ku >= 4; --ku) {
08070 d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08071 d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08072 d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08073 if (d1 <= d2 && d1 <= d3) {
08074 near__[ku] = 1;
08075 dist[ku] = d1;
08076 next[ku] = near__[1];
08077 near__[1] = ku;
08078 } else if (d2 <= d1 && d2 <= d3) {
08079 near__[ku] = 2;
08080 dist[ku] = d2;
08081 next[ku] = near__[2];
08082 near__[2] = ku;
08083 } else {
08084 near__[ku] = 3;
08085 dist[ku] = d3;
08086 next[ku] = near__[3];
08087 near__[3] = ku;
08088 }
08089
08090 }
08091
08092
08093
08094
08095 kt = 3;
08096 i__1 = *n0;
08097 for (ku = 4; ku <= i__1; ++ku) {
08098 nku = near__[ku];
08099
08100
08101
08102 i__ = nku;
08103 if (near__[i__] == ku) {
08104 near__[i__] = next[ku];
08105 } else {
08106 i__ = near__[i__];
08107 L2:
08108 i0 = i__;
08109 i__ = next[i0];
08110 if (i__ != ku) {
08111 goto L2;
08112 }
08113 next[i0] = next[ku];
08114 }
08115 near__[ku] = 0;
08116
08117
08118
08119 if (dist[ku] <= *tol - 1.) {
08120 indx[ku] = -nku;
08121 ++lcnt[nku];
08122 goto L6;
08123 }
08124
08125
08126
08127 ++kt;
08128 x[kt] = x[ku];
08129 y[kt] = y[ku];
08130 z__[kt] = z__[ku];
08131 indx[ku] = kt;
08132 lcnt[kt] = 1;
08133 addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08134 , lnew, ier);
08135 if (*ier != 0) {
08136 *n = 0;
08137 *ier = -3;
08138 return 0;
08139 }
08140
08141
08142
08143 lpl = lend[kt];
08144 lp = lpl;
08145 L3:
08146 lp = lptr[lp];
08147 j = (i__2 = list[lp], abs(i__2));
08148
08149
08150
08151
08152
08153
08154
08155 i__ = near__[j];
08156 L4:
08157 if (i__ == 0) {
08158 goto L5;
08159 }
08160 nexti = next[i__];
08161
08162
08163
08164
08165 d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08166 if (d__ < dist[i__]) {
08167
08168
08169
08170
08171
08172 near__[i__] = kt;
08173 dist[i__] = d__;
08174 if (i__ == near__[j]) {
08175 near__[j] = nexti;
08176 } else {
08177 next[i0] = nexti;
08178 }
08179 next[i__] = near__[kt];
08180 near__[kt] = i__;
08181 } else {
08182 i0 = i__;
08183 }
08184
08185
08186
08187 i__ = nexti;
08188 goto L4;
08189
08190
08191
08192 L5:
08193 if (lp != lpl) {
08194 goto L3;
08195 }
08196 L6:
08197 ;
08198 }
08199 *n = kt;
08200 *ier = 0;
08201 return 0;
08202 }
08203
08204
08205 int addnod_(int *nst, int *k, double *x,
08206 double *y, double *z__, int *list, int *lptr, int
08207 *lend, int *lnew, int *ier)
08208 {
08209
08210
08211 static double tol = 0.;
08212
08213
08214 int i__1;
08215
08216
08217 static int l;
08218 static double p[3], b1, b2, b3;
08219 static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08220 extern int swap_(int *, int *, int *,
08221 int *, int *, int *, int *, int *);
08222 static int lpo1s;
08223 extern int bdyadd_(int *, int *, int *,
08224 int *, int *, int *, int *), intadd_(int *,
08225 int *, int *, int *, int *, int *, int *,
08226 int *), trfind_(int *, double *, int *,
08227 double *, double *, double *, int *, int *,
08228 int *, double *, double *, double *, int *,
08229 int *, int *), covsph_(int *, int *, int *,
08230 int *, int *, int *);
08231 extern int lstptr_(int *, int *, int *, int *);
08232 extern long int swptst_(int *, int *, int *, int *,
08233 double *, double *, double *);
08234
08235
08236
08237
08238
08239
08240
08241
08242
08243
08244
08245
08246
08247
08248
08249
08250
08251
08252
08253
08254
08255
08256
08257
08258
08259
08260
08261
08262
08263
08264
08265
08266
08267
08268
08269
08270
08271
08272
08273
08274
08275
08276
08277
08278
08279
08280
08281
08282
08283
08284
08285
08286
08287
08288
08289
08290
08291
08292
08293
08294
08295
08296
08297
08298
08299
08300
08301
08302
08303
08304
08305
08306
08307
08308
08309
08310
08311
08312
08313
08314
08315
08316
08317
08318
08319
08320
08321
08322
08323
08324
08325
08326
08327
08328
08329
08330
08331
08332
08333
08334
08335
08336 --lend;
08337 --z__;
08338 --y;
08339 --x;
08340 --list;
08341 --lptr;
08342
08343
08344
08345 kk = *k;
08346 if (kk < 4) {
08347 goto L3;
08348 }
08349
08350
08351 km1 = kk - 1;
08352 ist = *nst;
08353 if (ist < 1) {
08354 ist = km1;
08355 }
08356 p[0] = x[kk];
08357 p[1] = y[kk];
08358 p[2] = z__[kk];
08359
08360
08361
08362
08363 trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08364 , &b1, &b2, &b3, &i1, &i2, &i3);
08365
08366
08367
08368 if (i1 == 0) {
08369 goto L4;
08370 }
08371 l = i1;
08372 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08373 goto L5;
08374 }
08375 l = i2;
08376 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08377 goto L5;
08378 }
08379 if (i3 != 0) {
08380 l = i3;
08381 if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08382 goto L5;
08383 }
08384 intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08385 } else {
08386 if (i1 != i2) {
08387 bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08388 } else {
08389 covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08390 }
08391 }
08392 *ier = 0;
08393
08394
08395
08396 lp = lend[kk];
08397 lpf = lptr[lp];
08398 io2 = list[lpf];
08399 lpo1 = lptr[lpf];
08400 io1 = (i__1 = list[lpo1], abs(i__1));
08401
08402
08403
08404 L1:
08405 lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08406 if (list[lp] < 0) {
08407 goto L2;
08408 }
08409 lp = lptr[lp];
08410 in1 = (i__1 = list[lp], abs(i__1));
08411
08412
08413
08414
08415 lpo1s = lpo1;
08416 if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08417 goto L2;
08418 }
08419 swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08420 if (lpo1 == 0) {
08421
08422
08423
08424
08425
08426
08427 lpo1 = lpo1s;
08428 goto L2;
08429 }
08430 io1 = in1;
08431 goto L1;
08432
08433
08434
08435
08436 L2:
08437 if (lpo1 == lpf || list[lpo1] < 0) {
08438 return 0;
08439 }
08440 io2 = io1;
08441 lpo1 = lptr[lpo1];
08442 io1 = (i__1 = list[lpo1], abs(i__1));
08443 goto L1;
08444
08445
08446
08447 L3:
08448 *ier = -1;
08449 return 0;
08450
08451
08452
08453 L4:
08454 *ier = -2;
08455 return 0;
08456
08457
08458
08459 L5:
08460 *ier = l;
08461 return 0;
08462 }
08463
08464 double angle_(double *v1, double *v2, double *v3)
08465 {
08466
08467 double ret_val;
08468
08469
08470
08471
08472
08473 static double a;
08474 static int i__;
08475 static double ca, s21, s23, u21[3], u23[3];
08476 extern long int left_(double *, double *, double *, double
08477 *, double *, double *, double *, double *,
08478 double *);
08479
08480
08481
08482
08483
08484
08485
08486
08487
08488
08489
08490
08491
08492
08493
08494
08495
08496
08497
08498
08499
08500
08501
08502
08503
08504
08505
08506
08507
08508
08509
08510
08511
08512
08513
08514
08515
08516
08517
08518
08519
08520
08521
08522
08523
08524
08525
08526
08527
08528
08529
08530
08531
08532
08533
08534
08535
08536
08537
08538 --v3;
08539 --v2;
08540 --v1;
08541
08542
08543 u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08544 u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08545 u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08546
08547 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08548 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08549 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08550
08551
08552
08553 s21 = 0.;
08554 s23 = 0.;
08555 for (i__ = 1; i__ <= 3; ++i__) {
08556 s21 += u21[i__ - 1] * u21[i__ - 1];
08557 s23 += u23[i__ - 1] * u23[i__ - 1];
08558
08559 }
08560
08561
08562
08563
08564 if (s21 == 0. || s23 == 0.) {
08565 ret_val = 0.;
08566 return ret_val;
08567 }
08568 s21 = sqrt(s21);
08569 s23 = sqrt(s23);
08570 for (i__ = 1; i__ <= 3; ++i__) {
08571 u21[i__ - 1] /= s21;
08572 u23[i__ - 1] /= s23;
08573
08574 }
08575
08576
08577
08578
08579
08580 ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08581 if (ca < -1.) {
08582 ca = -1.;
08583 }
08584 if (ca > 1.) {
08585 ca = 1.;
08586 }
08587 a = acos(ca);
08588
08589
08590
08591
08592 if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08593 , &v3[3])) {
08594 a = acos(-1.) * 2. - a;
08595 }
08596 ret_val = a;
08597 return ret_val;
08598 }
08599
08600 double areas_(double *v1, double *v2, double *v3)
08601 {
08602
08603 double ret_val;
08604
08605
08606
08607
08608
08609 static int i__;
08610 static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08611 ca2, ca3;
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
08656
08657
08658
08659
08660
08661
08662
08663
08664
08665 --v3;
08666 --v2;
08667 --v1;
08668
08669
08670 u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08671 u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08672 u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08673
08674 u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08675 u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08676 u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08677
08678 u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08679 u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08680 u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08681
08682
08683
08684 s12 = 0.;
08685 s23 = 0.;
08686 s31 = 0.;
08687 for (i__ = 1; i__ <= 3; ++i__) {
08688 s12 += u12[i__ - 1] * u12[i__ - 1];
08689 s23 += u23[i__ - 1] * u23[i__ - 1];
08690 s31 += u31[i__ - 1] * u31[i__ - 1];
08691
08692 }
08693
08694
08695
08696
08697 if (s12 == 0. || s23 == 0. || s31 == 0.) {
08698 ret_val = 0.;
08699 return ret_val;
08700 }
08701 s12 = sqrt(s12);
08702 s23 = sqrt(s23);
08703 s31 = sqrt(s31);
08704 for (i__ = 1; i__ <= 3; ++i__) {
08705 u12[i__ - 1] /= s12;
08706 u23[i__ - 1] /= s23;
08707 u31[i__ - 1] /= s31;
08708
08709 }
08710
08711
08712
08713
08714
08715
08716
08717 ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08718 ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08719 ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08720 if (ca1 < -1.) {
08721 ca1 = -1.;
08722 }
08723 if (ca1 > 1.) {
08724 ca1 = 1.;
08725 }
08726 if (ca2 < -1.) {
08727 ca2 = -1.;
08728 }
08729 if (ca2 > 1.) {
08730 ca2 = 1.;
08731 }
08732 if (ca3 < -1.) {
08733 ca3 = -1.;
08734 }
08735 if (ca3 > 1.) {
08736 ca3 = 1.;
08737 }
08738 a1 = acos(ca1);
08739 a2 = acos(ca2);
08740 a3 = acos(ca3);
08741
08742
08743
08744 ret_val = a1 + a2 + a3 - acos(-1.);
08745 if (ret_val < 0.) {
08746 ret_val = 0.;
08747 }
08748 return ret_val;
08749 }
08750
08751 double Util::areav_(int *k, int *n, double *x, double *y,
08752 double *z__, int *list, int *lptr, int *lend, int
08753 *ier)
08754 {
08755
08756
08757 static double amax = 6.28;
08758
08759
08760 double ret_val;
08761
08762
08763 static double a, c0[3], c2[3], c3[3];
08764 static int n1, n2, n3;
08765 static double v1[3], v2[3], v3[3];
08766 static int lp, lpl, ierr;
08767 static double asum;
08768 extern double areas_(double *, double *, double *);
08769 static long int first;
08770 extern int circum_(double *, double *,
08771 double *, double *, int *);
08772
08773
08774
08775
08776
08777
08778
08779
08780
08781
08782
08783
08784
08785
08786
08787
08788
08789
08790
08791
08792
08793
08794
08795
08796
08797
08798
08799
08800
08801
08802
08803
08804
08805
08806
08807
08808
08809
08810
08811
08812
08813
08814
08815
08816
08817
08818
08819
08820
08821
08822
08823
08824
08825
08826
08827
08828
08829
08830 --lend;
08831 --z__;
08832 --y;
08833 --x;
08834 --list;
08835 --lptr;
08836
08837
08838
08839
08840
08841 if (*k < 1 || *k > *n || *n <= 3) {
08842 goto L11;
08843 }
08844
08845
08846
08847
08848
08849 n1 = *k;
08850 v1[0] = x[n1];
08851 v1[1] = y[n1];
08852 v1[2] = z__[n1];
08853 lpl = lend[n1];
08854 n3 = list[lpl];
08855 if (n3 < 0) {
08856 goto L12;
08857 }
08858 lp = lpl;
08859 first = TRUE_;
08860 asum = 0.;
08861
08862
08863
08864 L1:
08865 n2 = n3;
08866 lp = lptr[lp];
08867 n3 = list[lp];
08868 v2[0] = x[n2];
08869 v2[1] = y[n2];
08870 v2[2] = z__[n2];
08871 v3[0] = x[n3];
08872 v3[1] = y[n3];
08873 v3[2] = z__[n3];
08874 if (first) {
08875
08876
08877
08878
08879 circum_(v1, v2, v3, c3, &ierr);
08880 if (ierr != 0) {
08881 goto L13;
08882 }
08883 c0[0] = c3[0];
08884 c0[1] = c3[1];
08885 c0[2] = c3[2];
08886 first = FALSE_;
08887 } else {
08888
08889
08890
08891
08892 c2[0] = c3[0];
08893 c2[1] = c3[1];
08894 c2[2] = c3[2];
08895 circum_(v1, v2, v3, c3, &ierr);
08896 if (ierr != 0) {
08897 goto L13;
08898 }
08899 a = areas_(v1, c2, c3);
08900 if (a > amax) {
08901 goto L14;
08902 }
08903 asum += a;
08904 }
08905
08906
08907
08908 if (lp != lpl) {
08909 goto L1;
08910 }
08911
08912
08913
08914 a = areas_(v1, c3, c0);
08915 if (a > amax) {
08916 goto L14;
08917 }
08918 asum += a;
08919
08920
08921
08922 *ier = 0;
08923 ret_val = asum;
08924 return ret_val;
08925
08926
08927
08928 L11:
08929 *ier = 1;
08930 ret_val = 0.;
08931 return ret_val;
08932
08933
08934
08935 L12:
08936 *ier = 2;
08937 ret_val = 0.;
08938 return ret_val;
08939
08940
08941
08942 L13:
08943 *ier = 3;
08944 ret_val = 0.;
08945 return ret_val;
08946
08947
08948
08949 L14:
08950 *ier = 4;
08951 ret_val = 0.;
08952 return ret_val;
08953 }
08954
08955 double areav_new__(int *k, int *n, double *x, double *y,
08956 double *z__, int *list, int *lptr, int *lend, int
08957 *ier)
08958 {
08959
08960 double ret_val = 0;
08961
08962
08963
08964
08965
08966 static int m;
08967 static double c1[3], c2[3], c3[3];
08968 static int n1, n2, n3;
08969 static double v1[3], v2[3], v3[3];
08970 static int lp;
08971 static double c1s[3], c2s[3];
08972 static int lpl, ierr;
08973 static double asum;
08974 extern double angle_(double *, double *, double *);
08975 static float areav;
08976 extern int circum_(double *, double *,
08977 double *, double *, int *);
08978
08979
08980
08981
08982
08983
08984
08985
08986
08987
08988
08989
08990
08991
08992
08993
08994
08995
08996
08997
08998
08999
09000
09001
09002
09003
09004
09005
09006
09007
09008
09009
09010
09011
09012
09013
09014
09015
09016
09017
09018
09019
09020
09021
09022
09023
09024
09025
09026
09027
09028
09029
09030
09031
09032
09033
09034
09035
09036
09037
09038
09039 --lend;
09040 --z__;
09041 --y;
09042 --x;
09043 --list;
09044 --lptr;
09045
09046
09047 if (*k < 1 || *k > *n || *n <= 3) {
09048 goto L11;
09049 }
09050
09051
09052
09053
09054
09055 n1 = *k;
09056 v1[0] = x[n1];
09057 v1[1] = y[n1];
09058 v1[2] = z__[n1];
09059 lpl = lend[n1];
09060 n3 = list[lpl];
09061 if (n3 < 0) {
09062 goto L12;
09063 }
09064 lp = lpl;
09065 m = 0;
09066 asum = 0.;
09067
09068
09069
09070 L1:
09071 ++m;
09072 n2 = n3;
09073 lp = lptr[lp];
09074 n3 = list[lp];
09075 v2[0] = x[n2];
09076 v2[1] = y[n2];
09077 v2[2] = z__[n2];
09078 v3[0] = x[n3];
09079 v3[1] = y[n3];
09080 v3[2] = z__[n3];
09081 if (m == 1) {
09082
09083
09084
09085
09086 circum_(v1, v2, v3, c2, &ierr);
09087 if (ierr != 0) {
09088 goto L13;
09089 }
09090 c1s[0] = c2[0];
09091 c1s[1] = c2[1];
09092 c1s[2] = c2[2];
09093 } else if (m == 2) {
09094
09095
09096
09097
09098 circum_(v1, v2, v3, c3, &ierr);
09099 if (ierr != 0) {
09100 goto L13;
09101 }
09102 c2s[0] = c3[0];
09103 c2s[1] = c3[1];
09104 c2s[2] = c3[2];
09105 } else {
09106
09107
09108
09109
09110
09111 c1[0] = c2[0];
09112 c1[1] = c2[1];
09113 c1[2] = c2[2];
09114 c2[0] = c3[0];
09115 c2[1] = c3[1];
09116 c2[2] = c3[2];
09117 circum_(v1, v2, v3, c3, &ierr);
09118 if (ierr != 0) {
09119 goto L13;
09120 }
09121 asum += angle_(c1, c2, c3);
09122 }
09123
09124
09125
09126 if (lp != lpl) {
09127 goto L1;
09128 }
09129
09130
09131
09132
09133 asum += angle_(c2, c3, c1s);
09134
09135
09136
09137
09138 asum += angle_(c3, c1s, c2s);
09139
09140
09141
09142 *ier = 0;
09143 ret_val = asum - (double) (m - 2) * acos(-1.);
09144 return ret_val;
09145
09146
09147
09148 L11:
09149 *ier = 1;
09150 areav = 0.f;
09151 return ret_val;
09152
09153
09154
09155 L12:
09156 *ier = 2;
09157 areav = 0.f;
09158 return ret_val;
09159
09160
09161
09162 L13:
09163 *ier = 3;
09164 areav = 0.f;
09165 return ret_val;
09166 }
09167
09168 int bdyadd_(int *kk, int *i1, int *i2, int *
09169 list, int *lptr, int *lend, int *lnew)
09170 {
09171 static int k, n1, n2, lp, lsav, nsav, next;
09172 extern int insert_(int *, int *, int *,
09173 int *, int *);
09174
09175
09176
09177
09178
09179
09180
09181
09182
09183
09184
09185
09186
09187
09188
09189
09190
09191
09192
09193
09194
09195
09196
09197
09198
09199
09200
09201
09202
09203
09204
09205
09206
09207
09208
09209
09210
09211
09212
09213
09214
09215
09216
09217
09218
09219
09220
09221
09222
09223
09224
09225
09226
09227
09228
09229
09230
09231
09232
09233
09234
09235
09236
09237
09238 --lend;
09239 --lptr;
09240 --list;
09241
09242
09243 k = *kk;
09244 n1 = *i1;
09245 n2 = *i2;
09246
09247
09248
09249 lp = lend[n1];
09250 lsav = lptr[lp];
09251 lptr[lp] = *lnew;
09252 list[*lnew] = -k;
09253 lptr[*lnew] = lsav;
09254 lend[n1] = *lnew;
09255 ++(*lnew);
09256 next = -list[lp];
09257 list[lp] = next;
09258 nsav = next;
09259
09260
09261
09262
09263 L1:
09264 lp = lend[next];
09265 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09266 if (next == n2) {
09267 goto L2;
09268 }
09269 next = -list[lp];
09270 list[lp] = next;
09271 goto L1;
09272
09273
09274
09275
09276 L2:
09277 lsav = *lnew;
09278 list[*lnew] = n1;
09279 lptr[*lnew] = *lnew + 1;
09280 ++(*lnew);
09281 next = nsav;
09282
09283 L3:
09284 if (next == n2) {
09285 goto L4;
09286 }
09287 list[*lnew] = next;
09288 lptr[*lnew] = *lnew + 1;
09289 ++(*lnew);
09290 lp = lend[next];
09291 next = list[lp];
09292 goto L3;
09293
09294 L4:
09295 list[*lnew] = -n2;
09296 lptr[*lnew] = lsav;
09297 lend[k] = *lnew;
09298 ++(*lnew);
09299 return 0;
09300 }
09301
09302 int bnodes_(int *n, int *list, int *lptr,
09303 int *lend, int *nodes, int *nb, int *na, int *nt)
09304 {
09305
09306 int i__1;
09307
09308
09309 static int k, n0, lp, nn, nst;
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
09356
09357
09358
09359
09360
09361
09362
09363
09364
09365
09366
09367
09368
09369
09370 --lend;
09371 --list;
09372 --lptr;
09373 --nodes;
09374
09375
09376 nn = *n;
09377
09378
09379
09380 i__1 = nn;
09381 for (nst = 1; nst <= i__1; ++nst) {
09382 lp = lend[nst];
09383 if (list[lp] < 0) {
09384 goto L2;
09385 }
09386
09387 }
09388
09389
09390
09391 *nb = 0;
09392 *na = (nn - 2) * 3;
09393 *nt = nn - (2<<1);
09394 return 0;
09395
09396
09397
09398
09399 L2:
09400 nodes[1] = nst;
09401 k = 1;
09402 n0 = nst;
09403
09404
09405
09406 L3:
09407 lp = lend[n0];
09408 lp = lptr[lp];
09409 n0 = list[lp];
09410 if (n0 == nst) {
09411 goto L4;
09412 }
09413 ++k;
09414 nodes[k] = n0;
09415 goto L3;
09416
09417
09418
09419 L4:
09420 *nb = k;
09421 *nt = (*n << 1) - *nb - 2;
09422 *na = *nt + *n - 1;
09423 return 0;
09424 }
09425
09426 int circle_(int *k, double *xc, double *yc,
09427 int *ier)
09428 {
09429
09430 int i__1;
09431
09432
09433
09434
09435
09436 static double a, c__;
09437 static int i__;
09438 static double s;
09439 static int k2, k3;
09440 static double x0, y0;
09441 static int kk, np1;
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
09488
09489
09490
09491
09492
09493
09494
09495
09496
09497
09498
09499
09500
09501
09502
09503
09504
09505
09506 --yc;
09507 --xc;
09508
09509
09510 kk = *k;
09511 k2 = kk << 1;
09512 k3 = kk * 3;
09513 np1 = (kk << 2) + 1;
09514
09515
09516
09517
09518 if (kk < 1) {
09519 goto L2;
09520 }
09521 a = atan(1.) * 2. / (double) kk;
09522 c__ = cos(a);
09523 s = sin(a);
09524 x0 = 1.;
09525 y0 = 0.;
09526
09527
09528
09529
09530
09531 i__1 = kk;
09532 for (i__ = 1; i__ <= i__1; ++i__) {
09533 xc[i__] = x0;
09534 yc[i__] = y0;
09535 xc[i__ + kk] = -y0;
09536 yc[i__ + kk] = x0;
09537 xc[i__ + k2] = -x0;
09538 yc[i__ + k2] = -y0;
09539 xc[i__ + k3] = y0;
09540 yc[i__ + k3] = -x0;
09541
09542
09543
09544 x0 = c__ * x0 - s * y0;
09545 y0 = s * x0 + c__ * y0;
09546
09547 }
09548
09549
09550
09551
09552 xc[np1] = xc[1];
09553 yc[np1] = yc[1];
09554 *ier = 0;
09555 return 0;
09556
09557
09558
09559 L2:
09560 *ier = 1;
09561 return 0;
09562 }
09563
09564 int circum_(double *v1, double *v2, double *v3,
09565 double *c__, int *ier)
09566 {
09567
09568
09569
09570
09571 static int i__;
09572 static double e1[3], e2[3], cu[3], cnorm;
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
09624
09625
09626
09627
09628
09629 --c__;
09630 --v3;
09631 --v2;
09632 --v1;
09633
09634
09635 for (i__ = 1; i__ <= 3; ++i__) {
09636 e1[i__ - 1] = v2[i__] - v1[i__];
09637 e2[i__ - 1] = v3[i__] - v1[i__];
09638
09639 }
09640
09641
09642
09643 cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09644 cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09645 cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09646 cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09647
09648
09649
09650
09651 if (cnorm != 0.) {
09652
09653
09654
09655 cnorm = sqrt(cnorm);
09656 for (i__ = 1; i__ <= 3; ++i__) {
09657 c__[i__] = cu[i__ - 1] / cnorm;
09658
09659 }
09660
09661
09662
09663
09664
09665
09666 if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09667 c__[1] = -c__[1];
09668 c__[2] = -c__[2];
09669 c__[3] = -c__[3];
09670 }
09671 *ier = 0;
09672 } else {
09673
09674
09675
09676 *ier = 1;
09677 }
09678 return 0;
09679 }
09680
09681 int covsph_(int *kk, int *n0, int *list, int
09682 *lptr, int *lend, int *lnew)
09683 {
09684 static int k, lp, nst, lsav, next;
09685 extern int insert_(int *, int *, int *,
09686 int *, int *);
09687
09688
09689
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 --lend;
09745 --lptr;
09746 --list;
09747
09748
09749 k = *kk;
09750 nst = *n0;
09751
09752
09753
09754
09755
09756 next = nst;
09757 L1:
09758 lp = lend[next];
09759 insert_(&k, &lp, &list[1], &lptr[1], lnew);
09760 next = -list[lp];
09761 list[lp] = next;
09762 if (next != nst) {
09763 goto L1;
09764 }
09765
09766
09767
09768
09769 lsav = *lnew;
09770 L2:
09771 lp = lend[next];
09772 list[*lnew] = next;
09773 lptr[*lnew] = *lnew + 1;
09774 ++(*lnew);
09775 next = list[lp];
09776 if (next != nst) {
09777 goto L2;
09778 }
09779
09780 lptr[*lnew - 1] = lsav;
09781 lend[k] = *lnew - 1;
09782 return 0;
09783 }
09784
09785 int crlist_(int *n, int *ncol, double *x,
09786 double *y, double *z__, int *list, int *lend, int
09787 *lptr, int *lnew, int *ltri, int *listc, int *nb,
09788 double *xc, double *yc, double *zc, double *rc,
09789 int *ier)
09790 {
09791
09792 int i__1, i__2;
09793
09794
09795
09796
09797
09798 static double c__[3], t;
09799 static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09800 static double v1[3], v2[3], v3[3];
09801 static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09802 lpn;
09803 static long int swp;
09804 static int ierr;
09805 extern int circum_(double *, double *,
09806 double *, double *, int *);
09807 extern int lstptr_(int *, int *, int *, int *);
09808 extern long int swptst_(int *, int *, int *, int *,
09809 double *, double *, double *);
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
09862
09863
09864
09865
09866
09867
09868
09869
09870
09871
09872
09873
09874
09875
09876
09877
09878
09879
09880
09881
09882
09883
09884
09885
09886
09887
09888
09889
09890
09891
09892
09893
09894
09895
09896
09897
09898
09899
09900
09901
09902
09903
09904
09905
09906
09907
09908
09909
09910
09911
09912
09913
09914
09915
09916
09917
09918
09919
09920
09921
09922
09923
09924
09925
09926
09927
09928
09929
09930
09931
09932
09933
09934
09935
09936
09937
09938
09939
09940
09941
09942
09943
09944
09945
09946
09947
09948
09949
09950
09951
09952
09953
09954
09955
09956
09957
09958
09959
09960
09961
09962
09963
09964
09965
09966
09967
09968
09969
09970
09971
09972
09973
09974
09975
09976
09977
09978
09979
09980
09981
09982
09983
09984
09985
09986 --lend;
09987 --z__;
09988 --y;
09989 --x;
09990 ltri -= 7;
09991 --list;
09992 --lptr;
09993 --listc;
09994 --xc;
09995 --yc;
09996 --zc;
09997 --rc;
09998
09999
10000 nn = *n;
10001 *nb = 0;
10002 nt = 0;
10003 if (nn < 3) {
10004 goto L21;
10005 }
10006
10007
10008
10009 i__1 = nn;
10010 for (n1 = 1; n1 <= i__1; ++n1) {
10011 lp = lend[n1];
10012 if (list[lp] < 0) {
10013 goto L2;
10014 }
10015
10016 }
10017
10018
10019
10020 goto L9;
10021
10022
10023
10024
10025
10026
10027
10028
10029 L2:
10030 n2 = -list[lp];
10031 lp = lptr[lp];
10032 n3 = list[lp];
10033
10034
10035
10036
10037
10038
10039 L3:
10040 ++nt;
10041 if (nt <= *ncol) {
10042 ltri[nt * 6 + 1] = n1;
10043 ltri[nt * 6 + 2] = n2;
10044 ltri[nt * 6 + 3] = n3;
10045 ltri[nt * 6 + 4] = nt + 1;
10046 ltri[nt * 6 + 5] = nt - 1;
10047 ltri[nt * 6 + 6] = 0;
10048 }
10049 n1 = n2;
10050 lp = lend[n1];
10051 n2 = -list[lp];
10052 if (n2 != n3) {
10053 goto L3;
10054 }
10055
10056 *nb = nt + 2;
10057 if (*ncol < nt) {
10058 goto L22;
10059 }
10060 ltri[nt * 6 + 4] = 0;
10061 if (nt == 1) {
10062 goto L7;
10063 }
10064
10065
10066
10067
10068
10069
10070
10071 L4:
10072 swp = FALSE_;
10073 i__1 = nt - 1;
10074 for (kt1 = 1; kt1 <= i__1; ++kt1) {
10075 for (i3 = 1; i3 <= 3; ++i3) {
10076 kt2 = ltri[i3 + 3 + kt1 * 6];
10077 if (kt2 <= kt1) {
10078 goto L5;
10079 }
10080
10081
10082
10083
10084 if (i3 == 1) {
10085 i1 = 2;
10086 i2 = 3;
10087 } else if (i3 == 2) {
10088 i1 = 3;
10089 i2 = 1;
10090 } else {
10091 i1 = 1;
10092 i2 = 2;
10093 }
10094 n1 = ltri[i1 + kt1 * 6];
10095 n2 = ltri[i2 + kt1 * 6];
10096 n3 = ltri[i3 + kt1 * 6];
10097
10098
10099
10100
10101 if (ltri[kt2 * 6 + 4] == kt1) {
10102 i4 = 1;
10103 } else if (ltri[kt2 * 6 + 5] == kt1) {
10104 i4 = 2;
10105 } else {
10106 i4 = 3;
10107 }
10108 n4 = ltri[i4 + kt2 * 6];
10109
10110
10111
10112
10113
10114 if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10115 goto L5;
10116 }
10117
10118
10119
10120
10121 swp = TRUE_;
10122 kt11 = ltri[i1 + 3 + kt1 * 6];
10123 kt12 = ltri[i2 + 3 + kt1 * 6];
10124 if (i4 == 1) {
10125 i2 = 2;
10126 i1 = 3;
10127 } else if (i4 == 2) {
10128 i2 = 3;
10129 i1 = 1;
10130 } else {
10131 i2 = 1;
10132 i1 = 2;
10133 }
10134 kt21 = ltri[i1 + 3 + kt2 * 6];
10135 kt22 = ltri[i2 + 3 + kt2 * 6];
10136 ltri[kt1 * 6 + 1] = n4;
10137 ltri[kt1 * 6 + 2] = n3;
10138 ltri[kt1 * 6 + 3] = n1;
10139 ltri[kt1 * 6 + 4] = kt12;
10140 ltri[kt1 * 6 + 5] = kt22;
10141 ltri[kt1 * 6 + 6] = kt2;
10142 ltri[kt2 * 6 + 1] = n3;
10143 ltri[kt2 * 6 + 2] = n4;
10144 ltri[kt2 * 6 + 3] = n2;
10145 ltri[kt2 * 6 + 4] = kt21;
10146 ltri[kt2 * 6 + 5] = kt11;
10147 ltri[kt2 * 6 + 6] = kt1;
10148
10149
10150
10151 if (kt11 != 0) {
10152 i4 = 4;
10153 if (ltri[kt11 * 6 + 4] != kt1) {
10154 i4 = 5;
10155 if (ltri[kt11 * 6 + 5] != kt1) {
10156 i4 = 6;
10157 }
10158 }
10159 ltri[i4 + kt11 * 6] = kt2;
10160 }
10161 if (kt22 != 0) {
10162 i4 = 4;
10163 if (ltri[kt22 * 6 + 4] != kt2) {
10164 i4 = 5;
10165 if (ltri[kt22 * 6 + 5] != kt2) {
10166 i4 = 6;
10167 }
10168 }
10169 ltri[i4 + kt22 * 6] = kt1;
10170 }
10171 L5:
10172 ;
10173 }
10174
10175 }
10176 if (swp) {
10177 goto L4;
10178 }
10179
10180
10181
10182
10183 L7:
10184 i__1 = nt;
10185 for (kt = 1; kt <= i__1; ++kt) {
10186 n1 = ltri[kt * 6 + 1];
10187 n2 = ltri[kt * 6 + 2];
10188 n3 = ltri[kt * 6 + 3];
10189 v1[0] = x[n1];
10190 v1[1] = y[n1];
10191 v1[2] = z__[n1];
10192 v2[0] = x[n2];
10193 v2[1] = y[n2];
10194 v2[2] = z__[n2];
10195 v3[0] = x[n3];
10196 v3[1] = y[n3];
10197 v3[2] = z__[n3];
10198 circum_(v2, v1, v3, c__, &ierr);
10199 if (ierr != 0) {
10200 goto L23;
10201 }
10202
10203
10204
10205
10206 xc[kt] = -c__[0];
10207 yc[kt] = -c__[1];
10208 zc[kt] = -c__[2];
10209 t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10210 if (t < -1.) {
10211 t = -1.;
10212 }
10213 if (t > 1.) {
10214 t = 1.;
10215 }
10216 rc[kt] = acos(t);
10217
10218 }
10219
10220
10221
10222
10223
10224
10225 L9:
10226 kt = nt;
10227
10228
10229
10230 nm2 = nn - 2;
10231 i__1 = nm2;
10232 for (n1 = 1; n1 <= i__1; ++n1) {
10233 lpl = lend[n1];
10234 lp = lpl;
10235 n3 = list[lp];
10236
10237
10238
10239
10240 L10:
10241 lp = lptr[lp];
10242 n2 = n3;
10243 n3 = (i__2 = list[lp], abs(i__2));
10244 if (n2 <= n1 || n3 <= n1) {
10245 goto L11;
10246 }
10247 ++kt;
10248
10249
10250
10251 v1[0] = x[n1];
10252 v1[1] = y[n1];
10253 v1[2] = z__[n1];
10254 v2[0] = x[n2];
10255 v2[1] = y[n2];
10256 v2[2] = z__[n2];
10257 v3[0] = x[n3];
10258 v3[1] = y[n3];
10259 v3[2] = z__[n3];
10260 circum_(v1, v2, v3, c__, &ierr);
10261 if (ierr != 0) {
10262 goto L23;
10263 }
10264
10265
10266
10267 xc[kt] = c__[0];
10268 yc[kt] = c__[1];
10269 zc[kt] = c__[2];
10270 t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10271 if (t < -1.) {
10272 t = -1.;
10273 }
10274 if (t > 1.) {
10275 t = 1.;
10276 }
10277 rc[kt] = acos(t);
10278
10279
10280
10281
10282
10283 lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10284 listc[lpn] = kt;
10285 lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10286 listc[lpn] = kt;
10287 lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10288 listc[lpn] = kt;
10289 L11:
10290 if (lp != lpl) {
10291 goto L10;
10292 }
10293
10294 }
10295 if (nt == 0) {
10296 goto L20;
10297 }
10298
10299
10300
10301
10302
10303
10304 kt1 = 0;
10305 L13:
10306 ++kt1;
10307 if (ltri[kt1 * 6 + 4] == 0) {
10308 i1 = 2;
10309 i2 = 3;
10310 i3 = 1;
10311 goto L14;
10312 } else if (ltri[kt1 * 6 + 5] == 0) {
10313 i1 = 3;
10314 i2 = 1;
10315 i3 = 2;
10316 goto L14;
10317 } else if (ltri[kt1 * 6 + 6] == 0) {
10318 i1 = 1;
10319 i2 = 2;
10320 i3 = 3;
10321 goto L14;
10322 }
10323 goto L13;
10324 L14:
10325 n1 = ltri[i1 + kt1 * 6];
10326 n0 = n1;
10327
10328
10329
10330
10331
10332
10333
10334
10335
10336 L15:
10337 lp = lend[n1];
10338 lpn = lptr[lp];
10339 listc[lp] = kt1;
10340
10341
10342
10343 L16:
10344 kt2 = ltri[i2 + 3 + kt1 * 6];
10345 if (kt2 != 0) {
10346
10347
10348
10349 lptr[lp] = *lnew;
10350 lp = *lnew;
10351 listc[lp] = kt2;
10352 ++(*lnew);
10353
10354
10355
10356
10357 kt1 = kt2;
10358 if (ltri[kt1 * 6 + 1] == n1) {
10359 i1 = 1;
10360 i2 = 2;
10361 i3 = 3;
10362 } else if (ltri[kt1 * 6 + 2] == n1) {
10363 i1 = 2;
10364 i2 = 3;
10365 i3 = 1;
10366 } else {
10367 i1 = 3;
10368 i2 = 1;
10369 i3 = 2;
10370 }
10371 goto L16;
10372 }
10373
10374
10375
10376
10377
10378
10379
10380 lptr[lp] = lpn;
10381 n1 = ltri[i3 + kt1 * 6];
10382 if (n1 != n0) {
10383 i4 = i3;
10384 i3 = i2;
10385 i2 = i1;
10386 i1 = i4;
10387 goto L15;
10388 }
10389
10390
10391
10392 L20:
10393 *ier = 0;
10394 return 0;
10395
10396
10397
10398 L21:
10399 *ier = 1;
10400 return 0;
10401
10402
10403
10404 L22:
10405 *ier = 2;
10406 return 0;
10407
10408
10409
10410 L23:
10411 *ier = 3;
10412 return 0;
10413 }
10414
10415 int delarc_(int *n, int *io1, int *io2, int *
10416 list, int *lptr, int *lend, int *lnew, int *ier)
10417 {
10418
10419 int i__1;
10420
10421
10422 static int n1, n2, n3, lp, lph, lpl;
10423 extern int delnb_(int *, int *, int *,
10424 int *, int *, int *, int *, int *);
10425 extern int lstptr_(int *, int *, int *, int *);
10426
10427
10428
10429
10430
10431
10432
10433
10434
10435
10436
10437
10438
10439
10440
10441
10442
10443
10444
10445
10446
10447
10448
10449
10450
10451
10452
10453
10454
10455
10456
10457
10458
10459
10460
10461
10462
10463
10464
10465
10466
10467
10468
10469
10470
10471
10472
10473
10474
10475
10476
10477
10478
10479
10480
10481
10482
10483
10484
10485
10486
10487
10488
10489
10490
10491
10492
10493
10494
10495
10496
10497
10498
10499 --lend;
10500 --list;
10501 --lptr;
10502
10503
10504 n1 = *io1;
10505 n2 = *io2;
10506
10507
10508
10509
10510
10511 if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10512 *ier = 1;
10513 return 0;
10514 }
10515
10516 lpl = lend[n2];
10517 if (-list[lpl] != n1) {
10518 n1 = n2;
10519 n2 = *io1;
10520 lpl = lend[n2];
10521 if (-list[lpl] != n1) {
10522 *ier = 2;
10523 return 0;
10524 }
10525 }
10526
10527
10528
10529
10530
10531 lpl = lend[n1];
10532 lp = lptr[lpl];
10533 lp = lptr[lp];
10534 n3 = (i__1 = list[lp], abs(i__1));
10535 lpl = lend[n3];
10536 if (list[lpl] <= 0) {
10537 *ier = 3;
10538 return 0;
10539 }
10540
10541
10542
10543
10544
10545
10546 delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10547 if (lph < 0) {
10548 *ier = 4;
10549 return 0;
10550 }
10551
10552
10553
10554
10555 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10556
10557
10558
10559
10560 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10561 lend[n3] = lp;
10562 list[lp] = -n1;
10563
10564
10565
10566 *ier = 0;
10567 return 0;
10568 }
10569
10570 int delnb_(int *n0, int *nb, int *n, int *
10571 list, int *lptr, int *lend, int *lnew, int *lph)
10572 {
10573
10574 int i__1;
10575
10576
10577 static int i__, lp, nn, lpb, lpl, lpp, lnw;
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
10617
10618
10619
10620
10621
10622
10623
10624
10625
10626
10627
10628
10629
10630
10631
10632
10633
10634
10635
10636
10637
10638
10639
10640
10641
10642
10643
10644
10645
10646
10647
10648
10649 --lend;
10650 --list;
10651 --lptr;
10652
10653
10654 nn = *n;
10655
10656
10657
10658 if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10659 *lph = -1;
10660 return 0;
10661 }
10662
10663
10664
10665
10666
10667
10668
10669 lpl = lend[*n0];
10670 lpp = lpl;
10671 lpb = lptr[lpp];
10672 L1:
10673 if (list[lpb] == *nb) {
10674 goto L2;
10675 }
10676 lpp = lpb;
10677 lpb = lptr[lpp];
10678 if (lpb != lpl) {
10679 goto L1;
10680 }
10681
10682
10683
10684 if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10685 *lph = -2;
10686 return 0;
10687 }
10688
10689
10690
10691
10692
10693 lend[*n0] = lpp;
10694 lp = lend[*nb];
10695 if (list[lp] < 0) {
10696 list[lpp] = -list[lpp];
10697 }
10698 goto L3;
10699
10700
10701
10702
10703
10704 L2:
10705 lp = lend[*nb];
10706 if (list[lp] < 0 && list[lpl] > 0) {
10707 lend[*n0] = lpp;
10708 list[lpp] = -list[lpp];
10709 }
10710
10711
10712
10713
10714 L3:
10715 lptr[lpp] = lptr[lpb];
10716 lnw = *lnew - 1;
10717 list[lpb] = list[lnw];
10718 lptr[lpb] = lptr[lnw];
10719 for (i__ = nn; i__ >= 1; --i__) {
10720 if (lend[i__] == lnw) {
10721 lend[i__] = lpb;
10722 goto L5;
10723 }
10724
10725 }
10726
10727 L5:
10728 i__1 = lnw - 1;
10729 for (i__ = 1; i__ <= i__1; ++i__) {
10730 if (lptr[i__] == lnw) {
10731 lptr[i__] = lpb;
10732 }
10733
10734 }
10735
10736
10737
10738 *lnew = lnw;
10739 *lph = lpb;
10740 return 0;
10741 }
10742
10743 int delnod_(int *k, int *n, double *x,
10744 double *y, double *z__, int *list, int *lptr, int
10745 *lend, int *lnew, int *lwk, int *iwk, int *ier)
10746 {
10747
10748 int i__1;
10749
10750
10751 static int i__, j, n1, n2;
10752 static double x1, x2, y1, y2, z1, z2;
10753 static int nl, lp, nn, nr;
10754 static double xl, yl, zl, xr, yr, zr;
10755 static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10756 extern long int left_(double *, double *, double *, double
10757 *, double *, double *, double *, double *,
10758 double *);
10759 static long int bdry;
10760 static int ierr, lwkl;
10761 extern int swap_(int *, int *, int *,
10762 int *, int *, int *, int *, int *), delnb_(
10763 int *, int *, int *, int *, int *, int *,
10764 int *, int *);
10765 extern int nbcnt_(int *, int *);
10766 extern int optim_(double *, double *, double
10767 *, int *, int *, int *, int *, int *, int
10768 *, int *);
10769 static int nfrst;
10770 extern int lstptr_(int *, int *, int *, int *);
10771
10772
10773
10774
10775
10776
10777
10778
10779
10780
10781
10782
10783
10784
10785
10786
10787
10788
10789
10790
10791
10792
10793
10794
10795
10796
10797
10798
10799
10800
10801
10802
10803
10804
10805
10806
10807
10808
10809
10810
10811
10812
10813
10814
10815
10816
10817
10818
10819
10820
10821
10822
10823
10824
10825
10826
10827
10828
10829
10830
10831
10832
10833
10834
10835
10836
10837
10838
10839
10840
10841
10842
10843
10844
10845
10846
10847
10848
10849
10850
10851
10852
10853
10854
10855
10856
10857
10858
10859
10860
10861
10862
10863
10864
10865
10866
10867
10868
10869
10870
10871
10872
10873
10874
10875
10876
10877
10878
10879
10880
10881
10882
10883
10884
10885
10886
10887
10888
10889
10890
10891
10892
10893
10894
10895
10896
10897
10898
10899
10900
10901
10902
10903
10904
10905
10906
10907
10908
10909
10910
10911
10912
10913
10914 iwk -= 3;
10915 --lend;
10916 --lptr;
10917 --list;
10918 --z__;
10919 --y;
10920 --x;
10921
10922
10923 n1 = *k;
10924 nn = *n;
10925 if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
10926 goto L21;
10927 }
10928 lpl = lend[n1];
10929 lpf = lptr[lpl];
10930 nnb = nbcnt_(&lpl, &lptr[1]);
10931 bdry = list[lpl] < 0;
10932 if (bdry) {
10933 ++nnb;
10934 }
10935 if (nnb < 3) {
10936 goto L23;
10937 }
10938 lwkl = *lwk;
10939 *lwk = nnb - 3;
10940 if (lwkl < *lwk) {
10941 goto L22;
10942 }
10943 iwl = 0;
10944 if (nnb == 3) {
10945 goto L3;
10946 }
10947
10948
10949
10950
10951
10952
10953
10954 x1 = x[n1];
10955 y1 = y[n1];
10956 z1 = z__[n1];
10957 nfrst = list[lpf];
10958 nr = nfrst;
10959 xr = x[nr];
10960 yr = y[nr];
10961 zr = z__[nr];
10962 lp = lptr[lpf];
10963 n2 = list[lp];
10964 x2 = x[n2];
10965 y2 = y[n2];
10966 z2 = z__[n2];
10967 lp = lptr[lp];
10968
10969
10970
10971 L1:
10972 nl = (i__1 = list[lp], abs(i__1));
10973 if (nl == nfrst && bdry) {
10974 goto L3;
10975 }
10976 xl = x[nl];
10977 yl = y[nl];
10978 zl = z__[nl];
10979
10980
10981
10982
10983
10984
10985 lpl2 = lend[n2];
10986 if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
10987 list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
10988 z2)))) {
10989
10990
10991
10992 nr = n2;
10993 xr = x2;
10994 yr = y2;
10995 zr = z2;
10996 goto L2;
10997 }
10998
10999
11000
11001
11002
11003
11004
11005
11006 swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11007 if (lp21 == 0) {
11008 nr = n2;
11009 xr = x2;
11010 yr = y2;
11011 zr = z2;
11012 goto L2;
11013 }
11014 ++iwl;
11015 if (nl <= n1) {
11016 iwk[(iwl << 1) + 1] = nl;
11017 } else {
11018 iwk[(iwl << 1) + 1] = nl - 1;
11019 }
11020 if (nr <= n1) {
11021 iwk[(iwl << 1) + 2] = nr;
11022 } else {
11023 iwk[(iwl << 1) + 2] = nr - 1;
11024 }
11025
11026
11027
11028 lpl = lend[n1];
11029 --nnb;
11030 if (nnb == 3) {
11031 goto L3;
11032 }
11033 lpf = lptr[lpl];
11034 nfrst = list[lpf];
11035 lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11036 if (nr == nfrst) {
11037 goto L2;
11038 }
11039
11040
11041
11042
11043
11044
11045
11046 n2 = nr;
11047 x2 = xr;
11048 y2 = yr;
11049 z2 = zr;
11050 lp21 = lptr[lp21];
11051 lp21 = lptr[lp21];
11052 nr = (i__1 = list[lp21], abs(i__1));
11053 xr = x[nr];
11054 yr = y[nr];
11055 zr = z__[nr];
11056 goto L1;
11057
11058
11059
11060 L2:
11061 if (n2 == nfrst) {
11062 goto L3;
11063 }
11064 n2 = nl;
11065 x2 = xl;
11066 y2 = yl;
11067 z2 = zl;
11068 lp = lptr[lp];
11069 goto L1;
11070
11071
11072
11073
11074
11075
11076
11077
11078
11079
11080 L3:
11081 if (! bdry) {
11082 if (nnb > 3) {
11083 bdry = TRUE_;
11084 } else {
11085 lpf = lptr[lpl];
11086 nr = list[lpf];
11087 lp = lptr[lpf];
11088 n2 = list[lp];
11089 nl = list[lpl];
11090 bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11091 x[n2], &y[n2], &z__[n2]);
11092 }
11093 if (bdry) {
11094
11095
11096
11097
11098
11099
11100 i__1 = nn;
11101 for (i__ = 1; i__ <= i__1; ++i__) {
11102 if (list[lend[i__]] < 0) {
11103 bdry = FALSE_;
11104 goto L5;
11105 }
11106
11107 }
11108 list[lpl] = -list[lpl];
11109 ++nnb;
11110 }
11111 }
11112 L5:
11113 if (! bdry && nnb > 3) {
11114 goto L24;
11115 }
11116
11117
11118
11119
11120 lp = lpl;
11121 lnw = *lnew;
11122
11123
11124
11125 L6:
11126 lp = lptr[lp];
11127 n2 = (i__1 = list[lp], abs(i__1));
11128 delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11129 if (lph < 0) {
11130 goto L23;
11131 }
11132
11133
11134
11135 if (lpl == lnw) {
11136 lpl = lph;
11137 }
11138 if (lp == lnw) {
11139 lp = lph;
11140 }
11141 if (lp != lpl) {
11142 goto L6;
11143 }
11144
11145
11146
11147
11148
11149 --nn;
11150 if (n1 > nn) {
11151 goto L9;
11152 }
11153 i__1 = nn;
11154 for (i__ = n1; i__ <= i__1; ++i__) {
11155 x[i__] = x[i__ + 1];
11156 y[i__] = y[i__ + 1];
11157 z__[i__] = z__[i__ + 1];
11158 lend[i__] = lend[i__ + 1];
11159
11160 }
11161
11162 i__1 = lnw - 1;
11163 for (i__ = 1; i__ <= i__1; ++i__) {
11164 if (list[i__] > n1) {
11165 --list[i__];
11166 }
11167 if (list[i__] < -n1) {
11168 ++list[i__];
11169 }
11170
11171 }
11172
11173
11174
11175
11176
11177
11178
11179
11180
11181
11182
11183 L9:
11184 if (bdry) {
11185 --nnb;
11186 }
11187 lpn = lpl;
11188 i__1 = nnb;
11189 for (j = 1; j <= i__1; ++j) {
11190 --lnw;
11191 lp = lpn;
11192 lpn = lptr[lp];
11193 list[lp] = list[lnw];
11194 lptr[lp] = lptr[lnw];
11195 if (lptr[lpn] == lnw) {
11196 lptr[lpn] = lp;
11197 }
11198 if (lpn == lnw) {
11199 lpn = lp;
11200 }
11201 for (i__ = nn; i__ >= 1; --i__) {
11202 if (lend[i__] == lnw) {
11203 lend[i__] = lp;
11204 goto L11;
11205 }
11206
11207 }
11208
11209 L11:
11210 for (i__ = lnw - 1; i__ >= 1; --i__) {
11211 if (lptr[i__] == lnw) {
11212 lptr[i__] = lp;
11213 }
11214
11215 }
11216
11217 }
11218
11219
11220
11221
11222
11223 *n = nn;
11224 *lnew = lnw;
11225 if (iwl > 0) {
11226 nit = iwl << 2;
11227 optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11228 nit, &iwk[3], &ierr);
11229 if (ierr != 0 && ierr != 1) {
11230 goto L25;
11231 }
11232 if (ierr == 1) {
11233 goto L26;
11234 }
11235 }
11236
11237
11238
11239 *ier = 0;
11240 return 0;
11241
11242
11243
11244 L21:
11245 *ier = 1;
11246 return 0;
11247
11248
11249
11250 L22:
11251 *ier = 2;
11252 return 0;
11253
11254
11255
11256
11257 L23:
11258 *ier = 3;
11259 return 0;
11260
11261
11262
11263 L24:
11264 *ier = 4;
11265 return 0;
11266
11267
11268
11269 L25:
11270 *ier = 5;
11271
11272
11273
11274 return 0;
11275
11276
11277
11278 L26:
11279 *ier = 6;
11280 return 0;
11281 }
11282
11283 int drwarc_(int *, double *p, double *q,
11284 double *tol, int *nseg)
11285 {
11286
11287 int i__1;
11288 double d__1;
11289
11290
11291
11292
11293
11294 static int i__, k;
11295 static double s, p1[3], p2[3], u1, u2, v1, v2;
11296 static int na;
11297 static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11298
11299
11300
11301
11302
11303
11304
11305
11306
11307
11308
11309
11310
11311
11312
11313
11314
11315
11316
11317
11318
11319
11320
11321
11322
11323
11324
11325
11326
11327
11328
11329
11330
11331
11332
11333
11334
11335
11336
11337
11338
11339
11340
11341
11342
11343
11344
11345
11346
11347
11348
11349
11350
11351
11352
11353
11354
11355
11356
11357
11358
11359
11360
11361
11362
11363
11364
11365
11366
11367
11368
11369
11370
11371
11372
11373
11374
11375 --q;
11376 --p;
11377
11378
11379 enrm = 0.;
11380 for (i__ = 1; i__ <= 3; ++i__) {
11381 pm[i__ - 1] = p[i__] + q[i__];
11382 enrm += pm[i__ - 1] * pm[i__ - 1];
11383
11384 }
11385 if (enrm == 0.) {
11386 goto L5;
11387 }
11388 enrm = sqrt(enrm);
11389 pm[0] /= enrm;
11390 pm[1] /= enrm;
11391 pm[2] /= enrm;
11392
11393
11394
11395
11396 u1 = p[1];
11397 v1 = p[2];
11398 u2 = q[1];
11399 v2 = q[2];
11400 um = pm[0];
11401 vm = pm[1];
11402
11403
11404
11405
11406
11407
11408 du = u2 - u1;
11409 dv = v2 - v1;
11410 enrm = du * du + dv * dv;
11411 if (enrm == 0.) {
11412 goto L5;
11413 }
11414 err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11415
11416
11417
11418
11419
11420 na = (int) (err / *tol + 1.);
11421
11422
11423
11424
11425
11426 s = 1. / (double) na;
11427 for (i__ = 1; i__ <= 3; ++i__) {
11428 dp[i__ - 1] = s * (q[i__] - p[i__]);
11429 pm[i__ - 1] = p[i__];
11430 p1[i__ - 1] = p[i__];
11431
11432 }
11433
11434
11435
11436
11437 i__1 = na - 1;
11438 for (k = 1; k <= i__1; ++k) {
11439 enrm = 0.;
11440 for (i__ = 1; i__ <= 3; ++i__) {
11441 pm[i__ - 1] += dp[i__ - 1];
11442 enrm += pm[i__ - 1] * pm[i__ - 1];
11443
11444 }
11445 if (enrm == 0.) {
11446 goto L5;
11447 }
11448 enrm = sqrt(enrm);
11449 p2[0] = pm[0] / enrm;
11450 p2[1] = pm[1] / enrm;
11451 p2[2] = pm[2] / enrm;
11452
11453
11454 p1[0] = p2[0];
11455 p1[1] = p2[1];
11456 p1[2] = p2[2];
11457
11458 }
11459
11460
11461
11462
11463 *nseg = na;
11464 return 0;
11465
11466
11467
11468 L5:
11469 *nseg = 0;
11470 return 0;
11471 }
11472
11473 int edge_(int *in1, int *in2, double *x,
11474 double *y, double *z__, int *lwk, int *iwk, int *
11475 list, int *lptr, int *lend, int *ier)
11476 {
11477
11478 int i__1;
11479
11480
11481 static int i__, n0, n1, n2;
11482 static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11483 static int nl, lp, nr;
11484 static double dp12;
11485 static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11486 static double dp1l, dp2l, dp1r, dp2r;
11487 extern long int left_(double *, double *, double *, double
11488 *, double *, double *, double *, double *,
11489 double *);
11490 static int ierr;
11491 extern int swap_(int *, int *, int *,
11492 int *, int *, int *, int *, int *);
11493 static int next, iwcp1, n1lst, iwend;
11494 extern int optim_(double *, double *, double
11495 *, int *, int *, int *, int *, int *, int
11496 *, int *);
11497 static int n1frst;
11498
11499
11500
11501
11502
11503
11504
11505
11506
11507
11508
11509
11510
11511
11512
11513
11514
11515
11516
11517
11518
11519
11520
11521
11522
11523
11524
11525
11526
11527
11528
11529
11530
11531
11532
11533
11534
11535
11536
11537
11538
11539
11540
11541
11542
11543
11544
11545
11546
11547
11548
11549
11550
11551
11552
11553
11554
11555
11556
11557
11558
11559
11560
11561
11562
11563
11564
11565
11566
11567
11568
11569
11570
11571
11572
11573
11574
11575
11576
11577
11578
11579
11580
11581
11582
11583
11584
11585
11586
11587
11588
11589
11590
11591
11592
11593
11594
11595
11596
11597
11598
11599
11600
11601
11602
11603
11604
11605
11606
11607
11608
11609
11610
11611
11612
11613
11614
11615
11616
11617
11618
11619
11620
11621
11622
11623
11624
11625
11626
11627
11628
11629
11630
11631
11632
11633
11634
11635 --lend;
11636 --lptr;
11637 --list;
11638 iwk -= 3;
11639 --z__;
11640 --y;
11641 --x;
11642
11643
11644 n1 = *in1;
11645 n2 = *in2;
11646 iwend = *lwk;
11647 if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11648 goto L31;
11649 }
11650
11651
11652
11653
11654 lpl = lend[n1];
11655 n0 = (i__1 = list[lpl], abs(i__1));
11656 lp = lpl;
11657 L1:
11658 if (n0 == n2) {
11659 goto L30;
11660 }
11661 lp = lptr[lp];
11662 n0 = list[lp];
11663 if (lp != lpl) {
11664 goto L1;
11665 }
11666
11667
11668
11669 iwl = 0;
11670 nit = 0;
11671
11672
11673
11674 L2:
11675 x1 = x[n1];
11676 y1 = y[n1];
11677 z1 = z__[n1];
11678 x2 = x[n2];
11679 y2 = y[n2];
11680 z2 = z__[n2];
11681
11682
11683
11684
11685
11686
11687
11688
11689
11690
11691 lpl = lend[n1];
11692 n1lst = list[lpl];
11693 lp = lptr[lpl];
11694 n1frst = list[lp];
11695 nl = n1frst;
11696 if (n1lst < 0) {
11697 goto L4;
11698 }
11699
11700
11701
11702
11703 L3:
11704 if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11705 goto L4;
11706 }
11707 lp = lptr[lp];
11708 nl = list[lp];
11709 if (nl != n1frst) {
11710 goto L3;
11711 }
11712
11713
11714
11715 goto L5;
11716
11717
11718
11719
11720 L4:
11721 nr = nl;
11722 lp = lptr[lp];
11723 nl = (i__1 = list[lp], abs(i__1));
11724 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11725
11726
11727
11728
11729
11730 dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11731 dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11732 dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11733 dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11734 dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11735 if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11736 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11737 goto L6;
11738 }
11739
11740
11741
11742
11743
11744 if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11745 goto L5;
11746 }
11747 }
11748
11749
11750
11751 if (nl != n1frst) {
11752 goto L4;
11753 }
11754
11755
11756
11757
11758
11759
11760
11761 L5:
11762 if (nit > 0) {
11763 goto L33;
11764 }
11765 nit = 1;
11766 n1 = n2;
11767 n2 = *in1;
11768 goto L2;
11769
11770
11771
11772
11773 L6:
11774 ++iwl;
11775 if (iwl > iwend) {
11776 goto L32;
11777 }
11778 iwk[(iwl << 1) + 1] = nl;
11779 iwk[(iwl << 1) + 2] = nr;
11780
11781
11782
11783 lpl = lend[nl];
11784 lp = lptr[lpl];
11785
11786
11787
11788
11789 L7:
11790 if (list[lp] == nr) {
11791 goto L8;
11792 }
11793 lp = lptr[lp];
11794 if (lp != lpl) {
11795 goto L7;
11796 }
11797
11798
11799
11800
11801 if (list[lp] != nr) {
11802 goto L33;
11803 }
11804
11805
11806
11807
11808 L8:
11809 lp = lptr[lp];
11810 next = (i__1 = list[lp], abs(i__1));
11811 if (next == n2) {
11812 goto L9;
11813 }
11814
11815
11816
11817 if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11818 nl = next;
11819 } else {
11820 nr = next;
11821 }
11822 goto L6;
11823
11824
11825
11826
11827 L9:
11828 *lwk = iwl;
11829 iwend = iwl;
11830
11831
11832
11833
11834
11835
11836
11837
11838 iwf = 1;
11839
11840
11841
11842
11843
11844 L10:
11845 lft = 0;
11846 n0 = n1;
11847 x0 = x1;
11848 y0 = y1;
11849 z0 = z1;
11850 nl = iwk[(iwf << 1) + 1];
11851 nr = iwk[(iwf << 1) + 2];
11852 iwc = iwf;
11853
11854
11855
11856
11857 L11:
11858 if (iwc == iwl) {
11859 goto L21;
11860 }
11861 iwcp1 = iwc + 1;
11862 next = iwk[(iwcp1 << 1) + 1];
11863 if (next != nl) {
11864 goto L16;
11865 }
11866 next = iwk[(iwcp1 << 1) + 2];
11867
11868
11869
11870
11871 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11872 z__[next])) {
11873 goto L14;
11874 }
11875 if (lft >= 0) {
11876 goto L12;
11877 }
11878 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11879 z__[next])) {
11880 goto L14;
11881 }
11882
11883
11884
11885 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11886 iwk[(iwc << 1) + 1] = n0;
11887 iwk[(iwc << 1) + 2] = next;
11888 goto L15;
11889
11890
11891
11892
11893
11894 L12:
11895 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11896 i__1 = iwl;
11897 for (i__ = iwcp1; i__ <= i__1; ++i__) {
11898 iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11899 iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11900
11901 }
11902 iwk[(iwl << 1) + 1] = n0;
11903 iwk[(iwl << 1) + 2] = next;
11904 --iwl;
11905 nr = next;
11906 goto L11;
11907
11908
11909
11910 L14:
11911 n0 = nr;
11912 x0 = x[n0];
11913 y0 = y[n0];
11914 z0 = z__[n0];
11915 lft = 1;
11916
11917
11918
11919 L15:
11920 nr = next;
11921 ++iwc;
11922 goto L11;
11923
11924
11925
11926
11927 L16:
11928 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11929 z__[next])) {
11930 goto L19;
11931 }
11932 if (lft <= 0) {
11933 goto L17;
11934 }
11935 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11936 z__[next])) {
11937 goto L19;
11938 }
11939
11940
11941
11942 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11943 iwk[(iwc << 1) + 1] = next;
11944 iwk[(iwc << 1) + 2] = n0;
11945 goto L20;
11946
11947
11948
11949
11950
11951 L17:
11952 swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11953 i__1 = iwf;
11954 for (i__ = iwc - 1; i__ >= i__1; --i__) {
11955 iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11956 iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11957
11958 }
11959 iwk[(iwf << 1) + 1] = n0;
11960 iwk[(iwf << 1) + 2] = next;
11961 ++iwf;
11962 goto L20;
11963
11964
11965
11966 L19:
11967 n0 = nl;
11968 x0 = x[n0];
11969 y0 = y[n0];
11970 z0 = z__[n0];
11971 lft = -1;
11972
11973
11974
11975 L20:
11976 nl = next;
11977 ++iwc;
11978 goto L11;
11979
11980
11981
11982 L21:
11983 if (n0 == n1) {
11984 goto L24;
11985 }
11986 if (lft < 0) {
11987 goto L22;
11988 }
11989
11990
11991
11992 if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
11993 goto L10;
11994 }
11995
11996
11997
11998
11999 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12000 iwk[(iwl << 1) + 1] = n0;
12001 iwk[(iwl << 1) + 2] = n2;
12002 --iwl;
12003 goto L10;
12004
12005
12006
12007 L22:
12008 if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12009 goto L10;
12010 }
12011
12012
12013
12014
12015 swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12016 i__ = iwl;
12017 L23:
12018 iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12019 iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12020 --i__;
12021 if (i__ > iwf) {
12022 goto L23;
12023 }
12024 iwk[(iwf << 1) + 1] = n0;
12025 iwk[(iwf << 1) + 2] = n2;
12026 ++iwf;
12027 goto L10;
12028
12029
12030
12031
12032 L24:
12033 swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12034 iwk[(iwc << 1) + 1] = 0;
12035 iwk[(iwc << 1) + 2] = 0;
12036
12037
12038
12039 *ier = 0;
12040 if (iwc > 1) {
12041
12042
12043
12044 nit = iwc - (1<<2);
12045 i__1 = iwc - 1;
12046 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12047 nit, &iwk[3], &ierr);
12048 if (ierr != 0 && ierr != 1) {
12049 goto L34;
12050 }
12051 if (ierr == 1) {
12052 *ier = 5;
12053 }
12054 }
12055 if (iwc < iwend) {
12056
12057
12058
12059 nit = iwend - (iwc<<2);
12060 i__1 = iwend - iwc;
12061 optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12062 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12063 if (ierr != 0 && ierr != 1) {
12064 goto L34;
12065 }
12066 if (ierr == 1) {
12067 goto L35;
12068 }
12069 }
12070 if (*ier == 5) {
12071 goto L35;
12072 }
12073
12074
12075
12076 return 0;
12077
12078
12079
12080 L30:
12081 *ier = 0;
12082 return 0;
12083
12084
12085
12086 L31:
12087 *ier = 1;
12088 return 0;
12089
12090
12091
12092 L32:
12093 *ier = 2;
12094 return 0;
12095
12096
12097
12098
12099 L33:
12100 *ier = 3;
12101
12102
12103
12104
12105 return 0;
12106
12107
12108
12109 L34:
12110 *ier = 4;
12111
12112
12113
12114 return 0;
12115
12116
12117
12118 L35:
12119 *ier = 5;
12120 return 0;
12121 }
12122
12123 int getnp_(double *x, double *y, double *z__,
12124 int *list, int *lptr, int *lend, int *l, int *
12125 npts, double *df, int *ier)
12126 {
12127
12128 int i__1, i__2;
12129
12130
12131 static int i__, n1;
12132 static double x1, y1, z1;
12133 static int nb, ni, lp, np, lm1;
12134 static double dnb, dnp;
12135 static int lpl;
12136
12137
12138
12139
12140
12141
12142
12143
12144
12145
12146
12147
12148
12149
12150
12151
12152
12153
12154
12155
12156
12157
12158
12159
12160
12161
12162
12163
12164
12165
12166
12167
12168
12169
12170
12171
12172
12173
12174
12175
12176
12177
12178
12179
12180
12181
12182
12183
12184
12185
12186
12187
12188
12189
12190
12191
12192
12193
12194
12195
12196
12197
12198
12199
12200
12201
12202
12203
12204
12205
12206
12207
12208
12209
12210
12211
12212
12213
12214
12215 --x;
12216 --y;
12217 --z__;
12218 --list;
12219 --lptr;
12220 --lend;
12221 --npts;
12222
12223
12224 lm1 = *l - 1;
12225 if (lm1 < 1) {
12226 goto L6;
12227 }
12228 *ier = 0;
12229
12230
12231
12232 n1 = npts[1];
12233 x1 = x[n1];
12234 y1 = y[n1];
12235 z1 = z__[n1];
12236 i__1 = lm1;
12237 for (i__ = 1; i__ <= i__1; ++i__) {
12238 ni = npts[i__];
12239 lend[ni] = -lend[ni];
12240
12241 }
12242
12243
12244
12245
12246
12247 dnp = 2.;
12248
12249
12250
12251 i__1 = lm1;
12252 for (i__ = 1; i__ <= i__1; ++i__) {
12253 ni = npts[i__];
12254 lpl = -lend[ni];
12255 lp = lpl;
12256
12257
12258
12259 L2:
12260 nb = (i__2 = list[lp], abs(i__2));
12261 if (lend[nb] < 0) {
12262 goto L3;
12263 }
12264
12265
12266
12267
12268 dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12269 if (dnb >= dnp) {
12270 goto L3;
12271 }
12272 np = nb;
12273 dnp = dnb;
12274 L3:
12275 lp = lptr[lp];
12276 if (lp != lpl) {
12277 goto L2;
12278 }
12279
12280 }
12281 npts[*l] = np;
12282 *df = dnp;
12283
12284
12285
12286 i__1 = lm1;
12287 for (i__ = 1; i__ <= i__1; ++i__) {
12288 ni = npts[i__];
12289 lend[ni] = -lend[ni];
12290
12291 }
12292 return 0;
12293
12294
12295
12296 L6:
12297 *ier = 1;
12298 return 0;
12299 }
12300
12301 int insert_(int *k, int *lp, int *list, int *
12302 lptr, int *lnew)
12303 {
12304 static int lsav;
12305
12306
12307
12308
12309
12310
12311
12312
12313
12314
12315
12316
12317
12318
12319
12320
12321
12322
12323
12324
12325
12326
12327
12328
12329
12330
12331
12332
12333
12334
12335
12336
12337
12338
12339
12340
12341
12342
12343
12344
12345
12346
12347
12348 --lptr;
12349 --list;
12350
12351
12352 lsav = lptr[*lp];
12353 lptr[*lp] = *lnew;
12354 list[*lnew] = *k;
12355 lptr[*lnew] = lsav;
12356 ++(*lnew);
12357 return 0;
12358 }
12359
12360 long int inside_(double *p, int *lv, double *xv, double *yv,
12361 double *zv, int *nv, int *listv, int *ier)
12362 {
12363
12364
12365 static double eps = .001;
12366
12367
12368 int i__1;
12369 long int ret_val = 0;
12370
12371
12372
12373
12374
12375 static double b[3], d__;
12376 static int k, n;
12377 static double q[3];
12378 static int i1, i2, k0;
12379 static double v1[3], v2[3], cn[3], bp, bq;
12380 static int ni;
12381 static double pn[3], qn[3], vn[3];
12382 static int imx;
12383 static long int lft1, lft2, even;
12384 static int ierr;
12385 static long int pinr, qinr;
12386 static double qnrm, vnrm;
12387 extern int intrsc_(double *, double *,
12388 double *, double *, int *);
12389
12390
12391
12392
12393
12394
12395
12396
12397
12398
12399
12400
12401
12402
12403
12404
12405
12406
12407
12408
12409
12410
12411
12412
12413
12414
12415
12416
12417
12418
12419
12420
12421
12422
12423
12424
12425
12426
12427
12428
12429
12430
12431
12432
12433
12434
12435
12436
12437
12438
12439
12440
12441
12442
12443
12444
12445
12446
12447
12448
12449
12450
12451
12452
12453
12454
12455
12456
12457
12458
12459
12460
12461
12462
12463
12464
12465
12466
12467
12468
12469
12470
12471
12472
12473
12474
12475
12476
12477
12478
12479
12480
12481
12482
12483
12484
12485
12486
12487
12488
12489
12490
12491
12492
12493
12494
12495
12496
12497
12498
12499
12500
12501
12502
12503
12504
12505
12506
12507
12508
12509
12510
12511
12512
12513
12514
12515
12516
12517
12518
12519
12520
12521
12522
12523
12524
12525
12526
12527
12528
12529
12530
12531
12532
12533
12534
12535
12536
12537
12538
12539
12540
12541 --p;
12542 --zv;
12543 --yv;
12544 --xv;
12545 --listv;
12546
12547
12548
12549
12550
12551
12552 imx = *lv;
12553 n = *nv;
12554 if (n < 3 || n > imx) {
12555 goto L11;
12556 }
12557 k0 = 0;
12558 i1 = listv[1];
12559 if (i1 < 1 || i1 > imx) {
12560 goto L12;
12561 }
12562
12563
12564
12565
12566
12567 L1:
12568 ++k0;
12569 if (k0 > n) {
12570 goto L14;
12571 }
12572 i1 = listv[k0];
12573 if (k0 < n) {
12574 i2 = listv[k0 + 1];
12575 } else {
12576 i2 = listv[1];
12577 }
12578 if (i2 < 1 || i2 > imx) {
12579 goto L12;
12580 }
12581 vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12582 vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12583 vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12584 vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12585 if (vnrm == 0.) {
12586 goto L1;
12587 }
12588 q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12589 q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12590 q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12591 qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12592 q[0] /= qnrm;
12593 q[1] /= qnrm;
12594 q[2] /= qnrm;
12595
12596
12597
12598 cn[0] = q[1] * p[3] - q[2] * p[2];
12599 cn[1] = q[2] * p[1] - q[0] * p[3];
12600 cn[2] = q[0] * p[2] - q[1] * p[1];
12601 if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12602 goto L1;
12603 }
12604 pn[0] = p[2] * cn[2] - p[3] * cn[1];
12605 pn[1] = p[3] * cn[0] - p[1] * cn[2];
12606 pn[2] = p[1] * cn[1] - p[2] * cn[0];
12607 qn[0] = cn[1] * q[2] - cn[2] * q[1];
12608 qn[1] = cn[2] * q[0] - cn[0] * q[2];
12609 qn[2] = cn[0] * q[1] - cn[1] * q[0];
12610
12611
12612
12613 ni = 0;
12614 even = TRUE_;
12615 bp = -2.;
12616 bq = -2.;
12617 pinr = TRUE_;
12618 qinr = TRUE_;
12619 i2 = listv[n];
12620 if (i2 < 1 || i2 > imx) {
12621 goto L12;
12622 }
12623 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12624
12625
12626
12627 i__1 = n;
12628 for (k = 1; k <= i__1; ++k) {
12629 i1 = i2;
12630 lft1 = lft2;
12631 i2 = listv[k];
12632 if (i2 < 1 || i2 > imx) {
12633 goto L12;
12634 }
12635 lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12636 if (lft1 == lft2) {
12637 goto L2;
12638 }
12639
12640
12641
12642
12643 ++ni;
12644 v1[0] = xv[i1];
12645 v1[1] = yv[i1];
12646 v1[2] = zv[i1];
12647 v2[0] = xv[i2];
12648 v2[1] = yv[i2];
12649 v2[2] = zv[i2];
12650 intrsc_(v1, v2, cn, b, &ierr);
12651
12652
12653
12654
12655
12656 if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12657 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12658
12659
12660
12661 even = ! even;
12662 d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12663 if (d__ > bq) {
12664 bq = d__;
12665 qinr = lft2;
12666 }
12667 d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12668 if (d__ > bp) {
12669 bp = d__;
12670 pinr = lft1;
12671 }
12672 }
12673 L2:
12674 ;
12675 }
12676
12677
12678
12679
12680 if (ni != ni / 2 << 1 || ! qinr) {
12681 goto L1;
12682 }
12683
12684
12685
12686 if (pinr != even) {
12687 goto L13;
12688 }
12689
12690
12691
12692 *ier = 0;
12693 ret_val = even;
12694 return ret_val;
12695
12696
12697
12698 L11:
12699 *ier = 1;
12700 return ret_val;
12701
12702
12703
12704 L12:
12705 *ier = 2;
12706 return ret_val;
12707
12708
12709
12710 L13:
12711 *ier = 3;
12712 return ret_val;
12713
12714
12715
12716 L14:
12717 *ier = 4;
12718 return ret_val;
12719 }
12720
12721 int intadd_(int *kk, int *i1, int *i2, int *
12722 i3, int *list, int *lptr, int *lend, int *lnew)
12723 {
12724 static int k, n1, n2, n3, lp;
12725 extern int insert_(int *, int *, int *,
12726 int *, int *);
12727 extern int lstptr_(int *, int *, int *, int *);
12728
12729
12730
12731
12732
12733
12734
12735
12736
12737
12738
12739
12740
12741
12742
12743
12744
12745
12746
12747
12748
12749
12750
12751
12752
12753
12754
12755
12756
12757
12758
12759
12760
12761
12762
12763
12764
12765
12766
12767
12768
12769
12770
12771
12772
12773
12774
12775
12776
12777
12778
12779
12780
12781
12782
12783
12784
12785 --lend;
12786 --lptr;
12787 --list;
12788
12789
12790 k = *kk;
12791
12792
12793
12794 n1 = *i1;
12795 n2 = *i2;
12796 n3 = *i3;
12797
12798
12799
12800 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12801 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12802 lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12803 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12804 lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12805 insert_(&k, &lp, &list[1], &lptr[1], lnew);
12806
12807
12808
12809 list[*lnew] = n1;
12810 list[*lnew + 1] = n2;
12811 list[*lnew + 2] = n3;
12812 lptr[*lnew] = *lnew + 1;
12813 lptr[*lnew + 1] = *lnew + 2;
12814 lptr[*lnew + 2] = *lnew;
12815 lend[k] = *lnew + 2;
12816 *lnew += 3;
12817 return 0;
12818 }
12819
12820 int intrsc_(double *p1, double *p2, double *cn,
12821 double *p, int *ier)
12822 {
12823
12824
12825
12826
12827 static int i__;
12828 static double t, d1, d2, pp[3], ppn;
12829
12830
12831
12832
12833
12834
12835
12836
12837
12838
12839
12840
12841
12842
12843
12844
12845
12846
12847
12848
12849
12850
12851
12852
12853
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 --p;
12899 --cn;
12900 --p2;
12901 --p1;
12902
12903
12904 d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
12905 d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
12906
12907 if (d1 == d2) {
12908 *ier = 1;
12909 return 0;
12910 }
12911
12912
12913
12914 t = d1 / (d1 - d2);
12915 ppn = 0.;
12916 for (i__ = 1; i__ <= 3; ++i__) {
12917 pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
12918 ppn += pp[i__ - 1] * pp[i__ - 1];
12919
12920 }
12921
12922
12923
12924 if (ppn == 0.) {
12925 *ier = 2;
12926 return 0;
12927 }
12928 ppn = sqrt(ppn);
12929
12930
12931
12932 for (i__ = 1; i__ <= 3; ++i__) {
12933 p[i__] = pp[i__ - 1] / ppn;
12934
12935 }
12936 *ier = 0;
12937 return 0;
12938 }
12939
12940 int jrand_(int *n, int *ix, int *iy, int *iz)
12941 {
12942
12943 int ret_val;
12944
12945
12946 static float u, x;
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 *ix = *ix * 171 % 30269;
13001 *iy = *iy * 172 % 30307;
13002 *iz = *iz * 170 % 30323;
13003 x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13004 30323.f;
13005 u = x - (int) x;
13006 ret_val = (int) ((float) (*n) * u + 1.f);
13007 return ret_val;
13008 }
13009
13010 long int left_(double *x1, double *y1, double *z1, double *x2,
13011 double *y2, double *z2, double *x0, double *y0,
13012 double *z0)
13013 {
13014
13015 long int ret_val;
13016
13017
13018
13019
13020
13021
13022
13023
13024
13025
13026
13027
13028
13029
13030
13031
13032
13033
13034
13035
13036
13037
13038
13039
13040
13041
13042
13043
13044
13045
13046
13047
13048
13049
13050
13051
13052
13053
13054 ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13055 *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13056
13057
13058 return ret_val;
13059 }
13060
13061 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13062 {
13063
13064 int ret_val;
13065
13066
13067 static int nd, lp;
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 --lptr;
13116 --list;
13117
13118
13119 lp = lptr[*lpl];
13120 L1:
13121 nd = list[lp];
13122 if (nd == *nb) {
13123 goto L2;
13124 }
13125 lp = lptr[lp];
13126 if (lp != *lpl) {
13127 goto L1;
13128 }
13129
13130 L2:
13131 ret_val = lp;
13132 return ret_val;
13133 }
13134
13135 int nbcnt_(int *lpl, int *lptr)
13136 {
13137
13138 int ret_val;
13139
13140
13141 static int k, lp;
13142
13143
13144
13145
13146
13147
13148
13149
13150
13151
13152
13153
13154
13155
13156
13157
13158
13159
13160
13161
13162
13163
13164
13165
13166
13167
13168
13169
13170
13171
13172
13173
13174
13175
13176
13177
13178
13179
13180
13181
13182
13183
13184 --lptr;
13185
13186
13187 lp = *lpl;
13188 k = 1;
13189
13190 L1:
13191 lp = lptr[lp];
13192 if (lp == *lpl) {
13193 goto L2;
13194 }
13195 ++k;
13196 goto L1;
13197
13198 L2:
13199 ret_val = k;
13200 return ret_val;
13201 }
13202
13203 int nearnd_(double *p, int *ist, int *n, double *x,
13204 double *y, double *z__, int *list, int *lptr, int
13205 *lend, double *al)
13206 {
13207
13208 int ret_val, i__1;
13209
13210
13211
13212
13213
13214 static int l;
13215 static double b1, b2, b3;
13216 static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13217 static double ds1;
13218 static int lp1, lp2;
13219 static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13220 static int lpl;
13221 static double dsr;
13222 static int nst, listp[25], lptrp[25];
13223 extern int trfind_(int *, double *, int *,
13224 double *, double *, double *, int *, int *,
13225 int *, double *, double *, double *, int *,
13226 int *, int *);
13227 extern int lstptr_(int *, int *, int *, int *);
13228
13229
13230
13231
13232
13233
13234
13235
13236
13237
13238
13239
13240
13241
13242
13243
13244
13245
13246
13247
13248
13249
13250
13251
13252
13253
13254
13255
13256
13257
13258
13259
13260
13261
13262
13263
13264
13265
13266
13267
13268
13269
13270
13271
13272
13273
13274
13275
13276
13277
13278
13279
13280
13281
13282
13283
13284
13285
13286
13287
13288
13289
13290
13291
13292
13293
13294
13295
13296
13297
13298
13299
13300
13301
13302
13303
13304
13305
13306
13307
13308
13309
13310
13311
13312
13313
13314
13315
13316
13317
13318
13319
13320
13321
13322
13323
13324 --p;
13325 --lend;
13326 --z__;
13327 --y;
13328 --x;
13329 --list;
13330 --lptr;
13331
13332
13333 nn = *n;
13334 if (nn < 3) {
13335 goto L6;
13336 }
13337 nst = *ist;
13338 if (nst < 1 || nst > nn) {
13339 nst = 1;
13340 }
13341
13342
13343
13344
13345
13346 trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13347 1], &b1, &b2, &b3, &i1, &i2, &i3);
13348
13349
13350
13351 if (i1 == 0) {
13352 goto L6;
13353 }
13354
13355
13356
13357
13358
13359
13360
13361 if (i3 != 0) {
13362 listp[0] = i1;
13363 lptrp[0] = 2;
13364 listp[1] = i2;
13365 lptrp[1] = 3;
13366 listp[2] = i3;
13367 lptrp[2] = 1;
13368 l = 3;
13369 } else {
13370 n1 = i1;
13371 l = 1;
13372 lp1 = 2;
13373 listp[l - 1] = n1;
13374 lptrp[l - 1] = lp1;
13375
13376
13377
13378
13379 L1:
13380 lpl = lend[n1];
13381 n1 = -list[lpl];
13382 l = lp1;
13383 lp1 = l + 1;
13384 listp[l - 1] = n1;
13385 lptrp[l - 1] = lp1;
13386 if (n1 != i2 && lp1 < 25) {
13387 goto L1;
13388 }
13389 l = lp1;
13390 listp[l - 1] = 0;
13391 lptrp[l - 1] = 1;
13392 }
13393
13394
13395
13396
13397
13398
13399 lp2 = 1;
13400 n2 = i1;
13401 lp1 = lptrp[0];
13402 n1 = listp[lp1 - 1];
13403
13404
13405
13406 L2:
13407 lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13408 if (list[lp] < 0) {
13409 goto L3;
13410 }
13411 lp = lptr[lp];
13412 n3 = (i__1 = list[lp], abs(i__1));
13413
13414
13415
13416 if (l == 25) {
13417 goto L4;
13418 }
13419 dx1 = x[n1] - p[1];
13420 dy1 = y[n1] - p[2];
13421 dz1 = z__[n1] - p[3];
13422
13423 dx2 = x[n2] - p[1];
13424 dy2 = y[n2] - p[2];
13425 dz2 = z__[n2] - p[3];
13426
13427 dx3 = x[n3] - p[1];
13428 dy3 = y[n3] - p[2];
13429 dz3 = z__[n3] - p[3];
13430 if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13431 (dx2 * dy1 - dx1 * dy2) <= 0.) {
13432 goto L3;
13433 }
13434
13435
13436
13437
13438 ++l;
13439 lptrp[lp2 - 1] = l;
13440 listp[l - 1] = n3;
13441 lptrp[l - 1] = lp1;
13442 lp1 = l;
13443 n1 = n3;
13444 goto L2;
13445
13446
13447
13448
13449 L3:
13450 if (lp1 == 1) {
13451 goto L4;
13452 }
13453 lp2 = lp1;
13454 n2 = n1;
13455 lp1 = lptrp[lp1 - 1];
13456 n1 = listp[lp1 - 1];
13457 if (n1 == 0) {
13458 goto L4;
13459 }
13460 goto L2;
13461
13462
13463
13464
13465
13466 L4:
13467 nr = i1;
13468 dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13469 i__1 = l;
13470 for (lp = 2; lp <= i__1; ++lp) {
13471 n1 = listp[lp - 1];
13472 if (n1 == 0) {
13473 goto L5;
13474 }
13475 ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13476 if (ds1 < dsr) {
13477 nr = n1;
13478 dsr = ds1;
13479 }
13480 L5:
13481 ;
13482 }
13483 dsr = -dsr;
13484 if (dsr > 1.) {
13485 dsr = 1.;
13486 }
13487 *al = acos(dsr);
13488 ret_val = nr;
13489 return ret_val;
13490
13491
13492
13493 L6:
13494 ret_val = 0;
13495 return ret_val;
13496 }
13497
13498 int optim_(double *x, double *y, double *z__,
13499 int *na, int *list, int *lptr, int *lend, int *
13500 nit, int *iwk, int *ier)
13501 {
13502
13503 int i__1, i__2;
13504
13505
13506 static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13507 static long int swp;
13508 static int iter;
13509 extern int swap_(int *, int *, int *,
13510 int *, int *, int *, int *, int *);
13511 static int maxit;
13512 extern long int swptst_(int *, int *, int *, int *,
13513 double *, double *, double *);
13514
13515
13516
13517
13518
13519
13520
13521
13522
13523
13524
13525
13526
13527
13528
13529
13530
13531
13532
13533
13534
13535
13536
13537
13538
13539
13540
13541
13542
13543
13544
13545
13546
13547
13548
13549
13550
13551
13552
13553
13554
13555
13556
13557
13558
13559
13560
13561
13562
13563
13564
13565
13566
13567
13568
13569
13570
13571
13572
13573
13574
13575
13576
13577
13578
13579
13580
13581
13582
13583
13584
13585
13586
13587
13588
13589
13590
13591
13592
13593
13594
13595
13596
13597
13598
13599
13600
13601
13602
13603
13604
13605
13606
13607
13608
13609
13610 --x;
13611 --y;
13612 --z__;
13613 iwk -= 3;
13614 --list;
13615 --lptr;
13616 --lend;
13617
13618
13619 nna = *na;
13620 maxit = *nit;
13621 if (nna < 0 || maxit < 1) {
13622 goto L7;
13623 }
13624
13625
13626
13627 iter = 0;
13628 if (nna == 0) {
13629 goto L5;
13630 }
13631
13632
13633
13634
13635 L1:
13636 if (iter == maxit) {
13637 goto L6;
13638 }
13639 ++iter;
13640 swp = FALSE_;
13641
13642
13643
13644 i__1 = nna;
13645 for (i__ = 1; i__ <= i__1; ++i__) {
13646 io1 = iwk[(i__ << 1) + 1];
13647 io2 = iwk[(i__ << 1) + 2];
13648
13649
13650
13651
13652
13653
13654
13655
13656 lpl = lend[io1];
13657 lpp = lpl;
13658 lp = lptr[lpp];
13659 L2:
13660 if (list[lp] == io2) {
13661 goto L3;
13662 }
13663 lpp = lp;
13664 lp = lptr[lpp];
13665 if (lp != lpl) {
13666 goto L2;
13667 }
13668
13669
13670
13671
13672
13673 if ((i__2 = list[lp], abs(i__2)) != io2) {
13674 goto L8;
13675 }
13676 if (list[lp] < 0) {
13677 goto L4;
13678 }
13679
13680
13681
13682
13683 L3:
13684 n2 = list[lpp];
13685 if (n2 < 0) {
13686 goto L4;
13687 }
13688 lp = lptr[lp];
13689 n1 = (i__2 = list[lp], abs(i__2));
13690
13691
13692
13693 if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13694 goto L4;
13695 }
13696 swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13697 if (lp21 == 0) {
13698 goto L9;
13699 }
13700 swp = TRUE_;
13701 iwk[(i__ << 1) + 1] = n1;
13702 iwk[(i__ << 1) + 2] = n2;
13703 L4:
13704 ;
13705 }
13706 if (swp) {
13707 goto L1;
13708 }
13709
13710
13711
13712 L5:
13713 *nit = iter;
13714 *ier = 0;
13715 return 0;
13716
13717
13718
13719 L6:
13720 *nit = maxit;
13721 *ier = 1;
13722 return 0;
13723
13724
13725
13726 L7:
13727 *nit = 0;
13728 *ier = 2;
13729 return 0;
13730
13731
13732
13733 L8:
13734 *nit = iter;
13735 *ier = 3;
13736 return 0;
13737
13738
13739
13740 L9:
13741 *nit = iter;
13742 *ier = 4;
13743 return 0;
13744 }
13745
13746 int projct_(double *px, double *py, double *pz,
13747 double *ox, double *oy, double *oz, double *ex,
13748 double *ey, double *ez, double *vx, double *vy,
13749 double *vz, long int *init, double *x, double *y,
13750 double *z__, int *ier)
13751 {
13752
13753
13754
13755
13756 static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13757 oes, xoe, yoe, zoe, xep, yep, zep;
13758
13759
13760
13761
13762
13763
13764
13765
13766
13767
13768
13769
13770
13771
13772
13773
13774
13775
13776
13777
13778
13779
13780
13781
13782
13783
13784
13785
13786
13787
13788
13789
13790
13791
13792
13793
13794
13795
13796
13797
13798
13799
13800
13801
13802
13803
13804
13805
13806
13807
13808
13809
13810
13811
13812
13813
13814
13815
13816
13817
13818
13819
13820
13821
13822
13823
13824
13825
13826
13827
13828
13829
13830
13831
13832
13833
13834
13835
13836
13837
13838
13839
13840
13841
13842
13843
13844
13845
13846
13847
13848
13849
13850
13851
13852
13853
13854
13855
13856
13857
13858
13859
13860
13861
13862
13863
13864
13865
13866
13867
13868
13869
13870
13871
13872 if (*init) {
13873
13874
13875
13876
13877
13878
13879
13880
13881 xe = *ex;
13882 ye = *ey;
13883 ze = *ez;
13884 xoe = xe - *ox;
13885 yoe = ye - *oy;
13886 zoe = ze - *oz;
13887 oes = xoe * xoe + yoe * yoe + zoe * zoe;
13888 if (oes == 0.) {
13889 goto L1;
13890 }
13891
13892
13893
13894 s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
13895 xv = *vx - s * xoe;
13896 yv = *vy - s * yoe;
13897 zv = *vz - s * zoe;
13898
13899
13900
13901 sc = xv * xv + yv * yv + zv * zv;
13902 if (sc == 0.) {
13903 goto L2;
13904 }
13905 sc = 1. / sqrt(sc);
13906 xv = sc * xv;
13907 yv = sc * yv;
13908 zv = sc * zv;
13909
13910
13911
13912 xh = yv * zoe - yoe * zv;
13913 yh = xoe * zv - xv * zoe;
13914 zh = xv * yoe - xoe * yv;
13915 sc = sqrt(xh * xh + yh * yh + zh * zh);
13916 if (sc == 0.) {
13917 goto L2;
13918 }
13919 sc = 1. / sc;
13920 xh = sc * xh;
13921 yh = sc * yh;
13922 zh = sc * zh;
13923 }
13924
13925
13926
13927
13928
13929
13930 xep = *px - xe;
13931 yep = *py - ye;
13932 zep = *pz - ze;
13933 s = xoe * xep + yoe * yep + zoe * zep;
13934 if (s >= 0.) {
13935 goto L1;
13936 }
13937 s = oes / s;
13938 xw = xoe - s * xep;
13939 yw = yoe - s * yep;
13940 zw = zoe - s * zep;
13941
13942
13943
13944
13945 *x = xw * xh + yw * yh + zw * zh;
13946 *y = xw * xv + yw * yv + zw * zv;
13947 *z__ = s + 1.;
13948 *init = FALSE_;
13949 *ier = 0;
13950 return 0;
13951
13952
13953
13954 L1:
13955 *ier = 1;
13956 return 0;
13957
13958
13959
13960 L2:
13961 *ier = 2;
13962 return 0;
13963 }
13964
13965 int scoord_(double *px, double *py, double *pz,
13966 double *plat, double *plon, double *pnrm)
13967 {
13968
13969
13970
13971
13972
13973
13974
13975
13976
13977
13978
13979
13980
13981
13982
13983
13984
13985
13986
13987
13988
13989
13990
13991
13992
13993
13994
13995
13996
13997
13998
13999
14000
14001
14002
14003
14004
14005
14006
14007
14008
14009
14010 *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14011 if (*px != 0. || *py != 0.) {
14012 *plon = atan2(*py, *px);
14013 } else {
14014 *plon = 0.;
14015 }
14016 if (*pnrm != 0.) {
14017 *plat = asin(*pz / *pnrm);
14018 } else {
14019 *plat = 0.;
14020 }
14021 return 0;
14022 }
14023
14024 double store_(double *x)
14025 {
14026
14027 double ret_val;
14028
14029
14030
14031
14032
14033
14034
14035
14036
14037
14038
14039
14040
14041
14042
14043
14044
14045
14046
14047
14048
14049
14050
14051
14052
14053
14054
14055
14056
14057
14058
14059
14060
14061
14062 stcom_1.y = *x;
14063 ret_val = stcom_1.y;
14064 return ret_val;
14065 }
14066
14067 int swap_(int *in1, int *in2, int *io1, int *
14068 io2, int *list, int *lptr, int *lend, int *lp21)
14069 {
14070
14071 int i__1;
14072
14073
14074 static int lp, lph, lpsav;
14075 extern int lstptr_(int *, int *, int *, int *);
14076
14077
14078
14079
14080
14081
14082
14083
14084
14085
14086
14087
14088
14089
14090
14091
14092
14093
14094
14095
14096
14097
14098
14099
14100
14101
14102
14103
14104
14105
14106
14107
14108
14109
14110
14111
14112
14113
14114
14115
14116
14117
14118
14119
14120
14121
14122
14123
14124
14125
14126
14127
14128
14129
14130
14131
14132
14133
14134
14135
14136 --lend;
14137 --lptr;
14138 --list;
14139
14140
14141 lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14142 if ((i__1 = list[lp], abs(i__1)) == *in2) {
14143 *lp21 = 0;
14144 return 0;
14145 }
14146
14147
14148
14149 lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14150 lph = lptr[lp];
14151 lptr[lp] = lptr[lph];
14152
14153
14154
14155
14156 if (lend[*io1] == lph) {
14157 lend[*io1] = lp;
14158 }
14159
14160
14161
14162
14163 lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14164 lpsav = lptr[lp];
14165 lptr[lp] = lph;
14166 list[lph] = *in2;
14167 lptr[lph] = lpsav;
14168
14169
14170
14171 lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14172 lph = lptr[lp];
14173 lptr[lp] = lptr[lph];
14174
14175
14176
14177
14178 if (lend[*io2] == lph) {
14179 lend[*io2] = lp;
14180 }
14181
14182
14183
14184 lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14185 lpsav = lptr[lp];
14186 lptr[lp] = lph;
14187 list[lph] = *in1;
14188 lptr[lph] = lpsav;
14189 *lp21 = lph;
14190 return 0;
14191 }
14192
14193 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14194 double *x, double *y, double *z__)
14195 {
14196
14197 long int ret_val;
14198
14199
14200 static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
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
14254
14255
14256
14257 --z__;
14258 --y;
14259 --x;
14260
14261
14262 x4 = x[*n4];
14263 y4 = y[*n4];
14264 z4 = z__[*n4];
14265 dx1 = x[*n1] - x4;
14266 dx2 = x[*n2] - x4;
14267 dx3 = x[*n3] - x4;
14268 dy1 = y[*n1] - y4;
14269 dy2 = y[*n2] - y4;
14270 dy3 = y[*n3] - y4;
14271 dz1 = z__[*n1] - z4;
14272 dz2 = z__[*n2] - z4;
14273 dz3 = z__[*n3] - z4;
14274
14275
14276
14277
14278
14279 ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14280 dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14281 return ret_val;
14282 }
14283
14284 int trans_(int *n, double *rlat, double *rlon,
14285 double *x, double *y, double *z__)
14286 {
14287
14288 int i__1;
14289
14290
14291
14292
14293
14294 static int i__, nn;
14295 static double phi, theta, cosphi;
14296
14297
14298
14299
14300
14301
14302
14303
14304
14305
14306
14307
14308
14309
14310
14311
14312
14313
14314
14315
14316
14317
14318
14319
14320
14321
14322
14323
14324
14325
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 --z__;
14351 --y;
14352 --x;
14353 --rlon;
14354 --rlat;
14355
14356
14357 nn = *n;
14358 i__1 = nn;
14359 for (i__ = 1; i__ <= i__1; ++i__) {
14360 phi = rlat[i__];
14361 theta = rlon[i__];
14362 cosphi = cos(phi);
14363 x[i__] = cosphi * cos(theta);
14364 y[i__] = cosphi * sin(theta);
14365 z__[i__] = sin(phi);
14366
14367 }
14368 return 0;
14369 }
14370
14371 int trfind_(int *nst, double *p, int *n,
14372 double *x, double *y, double *z__, int *list, int
14373 *lptr, int *lend, double *b1, double *b2, double *b3,
14374 int *i1, int *i2, int *i3)
14375 {
14376
14377
14378 static int ix = 1;
14379 static int iy = 2;
14380 static int iz = 3;
14381
14382
14383 int i__1;
14384 double d__1, d__2;
14385
14386
14387 static double q[3];
14388 static int n0, n1, n2, n3, n4, nf;
14389 static double s12;
14390 static int nl, lp;
14391 static double xp, yp, zp;
14392 static int n1s, n2s;
14393 static double eps, tol, ptn1, ptn2;
14394 static int next;
14395 extern int jrand_(int *, int *, int *, int *);
14396 extern double store_(double *);
14397 extern int lstptr_(int *, int *, int *, int *);
14398
14399
14400
14401
14402
14403
14404
14405
14406
14407
14408
14409
14410
14411
14412
14413
14414
14415
14416
14417
14418
14419
14420
14421
14422
14423
14424
14425
14426
14427
14428
14429
14430
14431
14432
14433
14434
14435
14436
14437
14438
14439
14440
14441
14442
14443
14444
14445
14446
14447
14448
14449
14450
14451
14452
14453
14454
14455
14456
14457
14458
14459
14460
14461
14462
14463
14464
14465
14466
14467 --p;
14468 --lend;
14469 --z__;
14470 --y;
14471 --x;
14472 --list;
14473 --lptr;
14474
14475
14476
14477
14478
14479
14480
14481
14482
14483
14484
14485
14486
14487
14488
14489
14490
14491
14492
14493
14494
14495
14496
14497
14498
14499
14500
14501
14502
14503
14504
14505
14506
14507
14508
14509
14510
14511
14512
14513
14514
14515
14516
14517
14518
14519
14520
14521
14522 xp = p[1];
14523 yp = p[2];
14524 zp = p[3];
14525 n0 = *nst;
14526 if (n0 < 1 || n0 > *n) {
14527 n0 = jrand_(n, &ix, &iy, &iz);
14528 }
14529
14530
14531
14532 eps = 1.;
14533 L1:
14534 eps /= 2.;
14535 d__1 = eps + 1.;
14536 if (store_(&d__1) > 1.) {
14537 goto L1;
14538 }
14539 eps *= 2.;
14540 tol = eps * 4.;
14541
14542
14543
14544
14545 L2:
14546 lp = lend[n0];
14547 nl = list[lp];
14548 lp = lptr[lp];
14549 nf = list[lp];
14550 n1 = nf;
14551
14552
14553
14554
14555 if (nl > 0) {
14556
14557
14558
14559 L3:
14560 if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14561 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14562 -1e-10) {
14563 lp = lptr[lp];
14564 n1 = list[lp];
14565 if (n1 == nl) {
14566 goto L6;
14567 }
14568 goto L3;
14569 }
14570 } else {
14571
14572
14573
14574 nl = -nl;
14575 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14576 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14577 -1e-10) {
14578
14579
14580
14581 n1 = n0;
14582 n2 = nf;
14583 goto L9;
14584 }
14585 if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14586 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14587 -1e-10) {
14588
14589
14590
14591 n1 = nl;
14592 n2 = n0;
14593 goto L9;
14594 }
14595 }
14596
14597
14598
14599
14600 L4:
14601 lp = lptr[lp];
14602 n2 = (i__1 = list[lp], abs(i__1));
14603 if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14604 n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14605 goto L7;
14606 }
14607 n1 = n2;
14608 if (n1 != nl) {
14609 goto L4;
14610 }
14611 if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14612 nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14613 goto L6;
14614 }
14615
14616
14617
14618
14619 d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14620 if (store_(&d__2) < 1. - eps * 4.) {
14621
14622
14623
14624
14625
14626 L5:
14627 if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14628 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14629 -1e-10) {
14630 lp = lptr[lp];
14631 n1 = (i__1 = list[lp], abs(i__1));
14632 if (n1 == nl) {
14633 goto L14;
14634 }
14635 goto L5;
14636 }
14637 }
14638
14639
14640
14641
14642 n0 = n1;
14643 goto L2;
14644
14645
14646
14647 L6:
14648 n2 = nf;
14649
14650
14651
14652
14653
14654 L7:
14655 n3 = n0;
14656 n1s = n1;
14657 n2s = n2;
14658
14659
14660
14661 L8:
14662
14663 *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14664 x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14665 if (*b3 < -1e-10) {
14666
14667
14668
14669
14670 lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14671 if (list[lp] < 0) {
14672 goto L9;
14673 }
14674 lp = lptr[lp];
14675 n4 = (i__1 = list[lp], abs(i__1));
14676
14677
14678
14679 if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14680 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14681 -1e-10) {
14682 n3 = n2;
14683 n2 = n4;
14684 n1s = n1;
14685 if (n2 != n2s && n2 != n0) {
14686 goto L8;
14687 }
14688 } else {
14689 n3 = n1;
14690 n1 = n4;
14691 n2s = n2;
14692 if (n1 != n1s && n1 != n0) {
14693 goto L8;
14694 }
14695 }
14696
14697
14698
14699
14700
14701 n0 = jrand_(n, &ix, &iy, &iz);
14702 goto L2;
14703 }
14704
14705
14706
14707
14708 if (*b3 >= eps) {
14709
14710
14711
14712 *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14713 - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14714 *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14715 - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14716 if (*b1 < -tol || *b2 < -tol) {
14717
14718
14719
14720 n0 = jrand_(n, &ix, &iy, &iz);
14721 goto L2;
14722 }
14723 } else {
14724
14725
14726
14727
14728 *b3 = 0.;
14729 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14730 ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14731 ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14732 *b1 = ptn1 - s12 * ptn2;
14733 *b2 = ptn2 - s12 * ptn1;
14734 if (*b1 < -tol || *b2 < -tol) {
14735
14736
14737
14738 n0 = jrand_(n, &ix, &iy, &iz);
14739 goto L2;
14740 }
14741 }
14742
14743
14744
14745 *i1 = n1;
14746 *i2 = n2;
14747 *i3 = n3;
14748 if (*b1 < 0.f) {
14749 *b1 = 0.f;
14750 }
14751 if (*b2 < 0.f) {
14752 *b2 = 0.f;
14753 }
14754 return 0;
14755
14756
14757
14758
14759
14760 L9:
14761 n1s = n1;
14762 n2s = n2;
14763 nl = 0;
14764
14765
14766
14767 L10:
14768
14769 lp = lend[n2];
14770 lp = lptr[lp];
14771 next = list[lp];
14772 if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14773 - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14774 >= -1e-10) {
14775
14776
14777
14778
14779 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14780 q[0] = x[n1] - s12 * x[n2];
14781 q[1] = y[n1] - s12 * y[n2];
14782 q[2] = z__[n1] - s12 * z__[n2];
14783 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14784 goto L11;
14785 }
14786 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14787 goto L11;
14788 }
14789
14790
14791
14792
14793 nl = n2;
14794 }
14795
14796
14797
14798 n1 = n2;
14799 n2 = next;
14800 if (n2 != n1s) {
14801 goto L10;
14802 }
14803
14804
14805
14806 *i1 = n1s;
14807 *i2 = n1s;
14808 *i3 = 0;
14809 return 0;
14810
14811
14812
14813 L11:
14814 nf = n2;
14815 if (nl == 0) {
14816
14817
14818
14819
14820 n2 = n2s;
14821 n1 = n1s;
14822
14823
14824
14825 L12:
14826 lp = lend[n1];
14827 next = -list[lp];
14828 if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14829 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14830 y[next]) >= -1e-10) {
14831
14832
14833
14834
14835 s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14836 q[0] = x[n2] - s12 * x[n1];
14837 q[1] = y[n2] - s12 * y[n1];
14838 q[2] = z__[n2] - s12 * z__[n1];
14839 if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14840 goto L13;
14841 }
14842 if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14843 goto L13;
14844 }
14845
14846
14847
14848
14849 nf = n1;
14850 }
14851
14852
14853
14854 n2 = n1;
14855 n1 = next;
14856 if (n1 != n1s) {
14857 goto L12;
14858 }
14859
14860
14861
14862 *i1 = n1;
14863 *i2 = n1;
14864 *i3 = 0;
14865 return 0;
14866
14867
14868
14869 L13:
14870 nl = n1;
14871 }
14872
14873
14874
14875 *i1 = nf;
14876 *i2 = nl;
14877 *i3 = 0;
14878 return 0;
14879
14880
14881
14882 L14:
14883 *i1 = 0;
14884 *i2 = 0;
14885 *i3 = 0;
14886 return 0;
14887 }
14888
14889 int trlist_(int *n, int *list, int *lptr,
14890 int *lend, int *nrow, int *nt, int *ltri, int *
14891 ier)
14892 {
14893
14894 int ltri_dim1, ltri_offset, i__1, i__2;
14895
14896
14897 static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
14898 lpl, isv;
14899 static long int arcs;
14900 static int lpln1;
14901
14902
14903
14904
14905
14906
14907
14908
14909
14910
14911
14912
14913
14914
14915
14916
14917
14918
14919
14920
14921
14922
14923
14924
14925
14926
14927
14928
14929
14930
14931
14932
14933
14934
14935
14936
14937
14938
14939
14940
14941
14942
14943
14944
14945
14946
14947
14948
14949
14950
14951
14952
14953
14954
14955
14956
14957
14958
14959
14960
14961
14962
14963
14964
14965
14966
14967
14968
14969
14970
14971
14972
14973
14974
14975
14976
14977
14978
14979
14980
14981
14982
14983
14984
14985
14986
14987
14988
14989
14990
14991
14992
14993
14994
14995
14996
14997
14998
14999
15000
15001
15002
15003
15004
15005 --lend;
15006 --list;
15007 --lptr;
15008 ltri_dim1 = *nrow;
15009 ltri_offset = 1 + ltri_dim1;
15010 ltri -= ltri_offset;
15011
15012
15013 if (*n < 3 || *nrow != 6 && *nrow != 9) {
15014 goto L11;
15015 }
15016
15017
15018
15019
15020
15021
15022
15023
15024 arcs = *nrow == 9;
15025 ka = 0;
15026 kt = 0;
15027 nm2 = *n - 2;
15028
15029
15030
15031 i__1 = nm2;
15032 for (n1 = 1; n1 <= i__1; ++n1) {
15033
15034
15035
15036
15037 lpln1 = lend[n1];
15038 lp2 = lpln1;
15039 L1:
15040 lp2 = lptr[lp2];
15041 n2 = list[lp2];
15042 lp = lptr[lp2];
15043 n3 = (i__2 = list[lp], abs(i__2));
15044 if (n2 < n1 || n3 < n1) {
15045 goto L8;
15046 }
15047
15048
15049
15050 ++kt;
15051 ltri[kt * ltri_dim1 + 1] = n1;
15052 ltri[kt * ltri_dim1 + 2] = n2;
15053 ltri[kt * ltri_dim1 + 3] = n3;
15054
15055
15056
15057
15058 for (i__ = 1; i__ <= 3; ++i__) {
15059 if (i__ == 1) {
15060 i1 = n3;
15061 i2 = n2;
15062 } else if (i__ == 2) {
15063 i1 = n1;
15064 i2 = n3;
15065 } else {
15066 i1 = n2;
15067 i2 = n1;
15068 }
15069
15070
15071
15072
15073 lpl = lend[i1];
15074 lp = lptr[lpl];
15075 L2:
15076 if (list[lp] == i2) {
15077 goto L3;
15078 }
15079 lp = lptr[lp];
15080 if (lp != lpl) {
15081 goto L2;
15082 }
15083
15084
15085
15086
15087
15088 if ((i__2 = list[lp], abs(i__2)) != i2) {
15089 goto L12;
15090 }
15091 kn = 0;
15092 if (list[lp] < 0) {
15093 goto L6;
15094 }
15095
15096
15097
15098
15099 L3:
15100 lp = lptr[lp];
15101 i3 = (i__2 = list[lp], abs(i__2));
15102
15103
15104
15105
15106
15107 if (i1 < i2 && i1 < i3) {
15108 j = 3;
15109 } else if (i2 < i3) {
15110 j = 2;
15111 isv = i1;
15112 i1 = i2;
15113 i2 = i3;
15114 i3 = isv;
15115 } else {
15116 j = 1;
15117 isv = i1;
15118 i1 = i3;
15119 i3 = i2;
15120 i2 = isv;
15121 }
15122
15123
15124
15125 if (i1 > n1) {
15126 goto L7;
15127 }
15128
15129
15130
15131
15132 for (kn = kt - 1; kn >= 1; --kn) {
15133 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15134 == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15135 goto L5;
15136 }
15137
15138 }
15139 goto L7;
15140
15141
15142
15143 L5:
15144 ltri[j + 3 + kn * ltri_dim1] = kt;
15145
15146
15147
15148 L6:
15149 ltri[i__ + 3 + kt * ltri_dim1] = kn;
15150 if (arcs) {
15151 ++ka;
15152 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15153 if (kn != 0) {
15154 ltri[j + 6 + kn * ltri_dim1] = ka;
15155 }
15156 }
15157 L7:
15158 ;
15159 }
15160
15161
15162
15163 L8:
15164 if (lp2 != lpln1) {
15165 goto L1;
15166 }
15167
15168 }
15169
15170
15171
15172 *nt = kt;
15173 *ier = 0;
15174 return 0;
15175
15176
15177
15178 L11:
15179 *nt = 0;
15180 *ier = 1;
15181 return 0;
15182
15183
15184
15185
15186 L12:
15187 *nt = 0;
15188 *ier = 2;
15189 return 0;
15190 }
15191
15192 int trlprt_(int *n, double *x, double *y,
15193 double *z__, int *iflag, int *nrow, int *nt, int *
15194 ltri, int *lout)
15195 {
15196
15197
15198 static int nmax = 9999;
15199 static int nlmax = 58;
15200
15201
15202 int ltri_dim1, ltri_offset, i__1;
15203
15204
15205 static int i__, k, na, nb, nl, lun;
15206
15207
15208
15209
15210
15211
15212
15213
15214
15215
15216
15217
15218
15219
15220
15221
15222
15223
15224
15225
15226
15227
15228
15229
15230
15231
15232
15233
15234
15235
15236
15237
15238
15239
15240
15241
15242
15243
15244
15245
15246
15247
15248
15249
15250
15251
15252
15253
15254
15255
15256
15257
15258
15259
15260
15261
15262
15263
15264
15265
15266
15267
15268
15269
15270
15271
15272
15273
15274
15275
15276
15277
15278
15279 --z__;
15280 --y;
15281 --x;
15282 ltri_dim1 = *nrow;
15283 ltri_offset = 1 + ltri_dim1;
15284 ltri -= ltri_offset;
15285
15286
15287
15288
15289
15290
15291
15292
15293
15294
15295
15296
15297
15298
15299
15300
15301 lun = *lout;
15302 if (lun < 0 || lun > 99) {
15303 lun = 6;
15304 }
15305
15306
15307
15308
15309 nl = 3;
15310 if (*n < 3 || *n > nmax || *nrow != 6 && *nrow != 9 || *nt < 1 || *nt >
15311 nmax) {
15312
15313
15314
15315
15316 return 0;
15317 }
15318 if (*iflag == 0) {
15319
15320
15321
15322
15323 nl = 6;
15324 i__1 = *n;
15325 for (i__ = 1; i__ <= i__1; ++i__) {
15326 if (nl >= nlmax) {
15327
15328 nl = 0;
15329 }
15330
15331 ++nl;
15332
15333 }
15334 } else if (*iflag > 0) {
15335
15336
15337
15338
15339 nl = 6;
15340 i__1 = *n;
15341 for (i__ = 1; i__ <= i__1; ++i__) {
15342 if (nl >= nlmax) {
15343
15344 nl = 0;
15345 }
15346
15347 ++nl;
15348
15349 }
15350 }
15351
15352
15353
15354 if (nl > nlmax / 2) {
15355
15356 nl = 0;
15357 }
15358 if (*nrow == 6) {
15359
15360 } else {
15361
15362 }
15363 nl += 5;
15364 i__1 = *nt;
15365 for (k = 1; k <= i__1; ++k) {
15366 if (nl >= nlmax) {
15367
15368 nl = 0;
15369 }
15370
15371 ++nl;
15372
15373 }
15374
15375
15376
15377
15378 nb = (*n << 1) - *nt - 2;
15379 if (nb < 3) {
15380 nb = 0;
15381 na = *n * 3 - 6;
15382 } else {
15383 na = *nt + *n - 1;
15384 }
15385
15386 return 0;
15387
15388
15389
15390
15391
15392
15393
15394
15395
15396
15397
15398
15399
15400
15401
15402
15403
15404
15405
15406
15407
15408
15409
15410 }
15411
15412 int trmesh_(int *n, double *x, double *y,
15413 double *z__, int *list, int *lptr, int *lend, int
15414 *lnew, int *near__, int *next, double *dist, int *ier)
15415 {
15416
15417 int i__1, i__2;
15418
15419
15420 static double d__;
15421 static int i__, j, k;
15422 static double d1, d2, d3;
15423 static int i0, lp, nn, lpl;
15424 extern long int left_(double *, double *, double *, double
15425 *, double *, double *, double *, double *,
15426 double *);
15427 static int nexti;
15428 extern int addnod_(int *, int *, double *,
15429 double *, double *, int *, int *, int *,
15430 int *, int *);
15431
15432
15433
15434
15435
15436
15437
15438
15439
15440
15441
15442
15443
15444
15445
15446
15447
15448
15449
15450
15451
15452
15453
15454
15455
15456
15457
15458
15459
15460
15461
15462
15463
15464
15465
15466
15467
15468
15469
15470
15471
15472
15473
15474
15475
15476
15477
15478
15479
15480
15481
15482
15483
15484
15485
15486
15487
15488
15489
15490
15491
15492
15493
15494
15495
15496
15497
15498
15499
15500
15501
15502
15503
15504
15505
15506
15507
15508
15509
15510
15511
15512
15513
15514
15515
15516
15517
15518
15519
15520
15521
15522
15523
15524
15525
15526
15527
15528
15529
15530
15531
15532
15533
15534
15535
15536
15537
15538
15539
15540
15541
15542
15543
15544
15545
15546
15547
15548
15549
15550
15551
15552
15553
15554
15555
15556
15557
15558
15559
15560
15561
15562
15563
15564
15565
15566
15567
15568
15569
15570
15571
15572
15573
15574
15575
15576
15577
15578
15579
15580
15581
15582
15583
15584
15585
15586
15587
15588
15589
15590
15591
15592
15593
15594
15595
15596
15597
15598
15599
15600
15601
15602
15603
15604
15605
15606
15607
15608
15609
15610
15611
15612
15613
15614
15615
15616
15617
15618
15619
15620
15621
15622
15623
15624
15625
15626
15627
15628
15629
15630
15631
15632
15633
15634
15635
15636
15637
15638
15639
15640
15641
15642
15643
15644
15645
15646
15647
15648
15649
15650
15651
15652
15653
15654
15655
15656
15657 --dist;
15658 --next;
15659 --near__;
15660 --lend;
15661 --z__;
15662 --y;
15663 --x;
15664 --list;
15665 --lptr;
15666
15667
15668 nn = *n;
15669 if (nn < 3) {
15670 *ier = -1;
15671 return 0;
15672 }
15673
15674
15675
15676 if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15677 z__[3])) {
15678
15679
15680
15681 list[1] = 3;
15682 lptr[1] = 2;
15683 list[2] = -2;
15684 lptr[2] = 1;
15685 lend[1] = 2;
15686
15687 list[3] = 1;
15688 lptr[3] = 4;
15689 list[4] = -3;
15690 lptr[4] = 3;
15691 lend[2] = 4;
15692
15693 list[5] = 2;
15694 lptr[5] = 6;
15695 list[6] = -1;
15696 lptr[6] = 5;
15697 lend[3] = 6;
15698
15699 } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15700 y[3], &z__[3])) {
15701
15702
15703
15704
15705
15706 list[1] = 2;
15707 lptr[1] = 2;
15708 list[2] = -3;
15709 lptr[2] = 1;
15710 lend[1] = 2;
15711
15712 list[3] = 3;
15713 lptr[3] = 4;
15714 list[4] = -1;
15715 lptr[4] = 3;
15716 lend[2] = 4;
15717
15718 list[5] = 1;
15719 lptr[5] = 6;
15720 list[6] = -2;
15721 lptr[6] = 5;
15722 lend[3] = 6;
15723
15724 } else {
15725
15726
15727
15728 *ier = -2;
15729 return 0;
15730 }
15731
15732
15733
15734 *lnew = 7;
15735 if (nn == 3) {
15736 *ier = 0;
15737 return 0;
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 near__[1] = 0;
15767 near__[2] = 0;
15768 near__[3] = 0;
15769 for (k = nn; k >= 4; --k) {
15770 d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15771 d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15772 d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15773 if (d1 <= d2 && d1 <= d3) {
15774 near__[k] = 1;
15775 dist[k] = d1;
15776 next[k] = near__[1];
15777 near__[1] = k;
15778 } else if (d2 <= d1 && d2 <= d3) {
15779 near__[k] = 2;
15780 dist[k] = d2;
15781 next[k] = near__[2];
15782 near__[2] = k;
15783 } else {
15784 near__[k] = 3;
15785 dist[k] = d3;
15786 next[k] = near__[3];
15787 near__[3] = k;
15788 }
15789
15790 }
15791
15792
15793
15794 i__1 = nn;
15795 for (k = 4; k <= i__1; ++k) {
15796 addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15797 lend[1], lnew, ier);
15798 if (*ier != 0) {
15799 return 0;
15800 }
15801
15802
15803
15804
15805 i__ = near__[k];
15806 if (near__[i__] == k) {
15807 near__[i__] = next[k];
15808 } else {
15809 i__ = near__[i__];
15810 L2:
15811 i0 = i__;
15812 i__ = next[i0];
15813 if (i__ != k) {
15814 goto L2;
15815 }
15816 next[i0] = next[k];
15817 }
15818 near__[k] = 0;
15819
15820
15821
15822 lpl = lend[k];
15823 lp = lpl;
15824 L3:
15825 lp = lptr[lp];
15826 j = (i__2 = list[lp], abs(i__2));
15827
15828
15829
15830
15831
15832
15833
15834 i__ = near__[j];
15835 L4:
15836 if (i__ == 0) {
15837 goto L5;
15838 }
15839 nexti = next[i__];
15840
15841
15842
15843
15844 d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15845 if (d__ < dist[i__]) {
15846
15847
15848
15849
15850
15851 near__[i__] = k;
15852 dist[i__] = d__;
15853 if (i__ == near__[j]) {
15854 near__[j] = nexti;
15855 } else {
15856 next[i0] = nexti;
15857 }
15858 next[i__] = near__[k];
15859 near__[k] = i__;
15860 } else {
15861 i0 = i__;
15862 }
15863
15864
15865
15866 i__ = nexti;
15867 goto L4;
15868
15869
15870
15871 L5:
15872 if (lp != lpl) {
15873 goto L3;
15874 }
15875
15876 }
15877 return 0;
15878 }
15879
15880 int trplot_(int *lun, double *pltsiz, double *
15881 elat, double *elon, double *a, int *n, double *x,
15882 double *y, double *z__, int *list, int *lptr, int
15883 *lend, char *, long int *numbr, int *ier, short )
15884 {
15885
15886
15887 static long int annot = TRUE_;
15888 static double fsizn = 10.;
15889 static double fsizt = 16.;
15890 static double tol = .5;
15891
15892
15893 int i__1, i__2;
15894 double d__1;
15895
15896
15897
15898
15899
15900
15901
15902 static double t;
15903 static int n0, n1;
15904 static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
15905 static int ir, lp;
15906 static double ex, ey, ez, wr, tx, ty;
15907 static int lpl;
15908 static double wrs;
15909 static int ipx1, ipx2, ipy1, ipy2, nseg;
15910 extern int drwarc_(int *, double *, double *,
15911 double *, int *);
15912
15913
15914
15915
15916
15917
15918
15919
15920
15921
15922
15923
15924
15925
15926
15927
15928
15929
15930
15931
15932
15933
15934
15935
15936
15937
15938
15939
15940
15941
15942
15943
15944
15945
15946
15947
15948
15949
15950
15951
15952
15953
15954
15955
15956
15957
15958
15959
15960
15961
15962
15963
15964
15965
15966
15967
15968
15969
15970
15971
15972
15973
15974
15975
15976
15977
15978
15979
15980
15981
15982
15983
15984
15985
15986
15987
15988
15989
15990
15991
15992
15993
15994
15995
15996
15997
15998
15999
16000
16001
16002
16003
16004
16005
16006
16007
16008
16009
16010
16011
16012
16013
16014
16015
16016
16017
16018 --lend;
16019 --z__;
16020 --y;
16021 --x;
16022 --list;
16023 --lptr;
16024
16025
16026
16027
16028
16029
16030
16031
16032
16033
16034
16035
16036
16037
16038
16039
16040
16041
16042
16043
16044
16045
16046
16047
16048
16049
16050
16051
16052
16053
16054
16055
16056
16057
16058
16059
16060
16061
16062
16063
16064
16065
16066
16067
16068
16069
16070
16071
16072
16073
16074
16075
16076
16077
16078
16079 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16080 goto L11;
16081 }
16082 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16083 goto L12;
16084 }
16085
16086
16087
16088
16089 cf = atan(1.) / 45.;
16090 wr = sin(cf * *a);
16091 wrs = wr * wr;
16092
16093
16094
16095
16096
16097
16098
16099
16100
16101
16102 d__1 = *pltsiz * 36.;
16103 ir = i_dnnt(&d__1);
16104 ipx1 = 306 - ir;
16105 ipx2 = ir + 306;
16106 ipy1 = 396 - ir;
16107 ipy2 = ir + 396;
16108
16109
16110
16111
16112
16113
16114
16115
16116
16117
16118
16119
16120
16121
16122 d__1 = (double) ir * .88;
16123 ir = i_dnnt(&d__1);
16124 ipx1 = 306 - ir;
16125 ipx2 = ir + 306;
16126 ipy1 = 396 - ir;
16127 ipy2 = ir + 396;
16128
16129
16130
16131
16132 t = 2.;
16133
16134
16135
16136
16137
16138
16139
16140
16141
16142
16143 sf = (double) ir / wr;
16144 tx = ipx1 + sf * wr;
16145 ty = ipy1 + sf * wr;
16146
16147
16148
16149
16150
16151
16152
16153
16154 t = 1. / sf;
16155
16156
16157
16158
16159
16160
16161
16162
16163
16164
16165
16166
16167
16168
16169
16170
16171
16172
16173
16174
16175
16176
16177
16178 t = cf * *elon;
16179 ct = cos(cf * *elat);
16180 ex = ct * cos(t);
16181 ey = ct * sin(t);
16182 ez = sin(cf * *elat);
16183 if (ct != 0.) {
16184 r11 = -ey / ct;
16185 r12 = ex / ct;
16186 } else {
16187 r11 = 0.;
16188 r12 = 1.;
16189 }
16190 r21 = -ez * r12;
16191 r22 = ez * r11;
16192 r23 = ct;
16193
16194
16195
16196
16197 i__1 = *n;
16198 for (n0 = 1; n0 <= i__1; ++n0) {
16199 p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16200 if (p0[2] < 0.) {
16201 goto L3;
16202 }
16203 p0[0] = r11 * x[n0] + r12 * y[n0];
16204 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16205 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16206 goto L3;
16207 }
16208 lpl = lend[n0];
16209 lp = lpl;
16210
16211
16212
16213
16214 L1:
16215 lp = lptr[lp];
16216 n1 = (i__2 = list[lp], abs(i__2));
16217 p1[0] = r11 * x[n1] + r12 * y[n1];
16218 p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16219 p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16220 if (p1[2] < 0.) {
16221
16222
16223
16224
16225
16226 p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16227 p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16228 t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16229 p1[0] /= t;
16230 p1[1] /= t;
16231 }
16232
16233
16234
16235
16236 if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16237 goto L2;
16238 }
16239
16240
16241
16242
16243 if (p1[2] < 0.) {
16244 p1[2] = 0.;
16245 }
16246 d__1 = tol / sf;
16247 drwarc_(lun, p0, p1, &d__1, &nseg);
16248
16249
16250
16251 L2:
16252 if (lp != lpl) {
16253 goto L1;
16254 }
16255 L3:
16256 ;
16257 }
16258
16259
16260
16261
16262
16263
16264
16265 if (*numbr) {
16266
16267
16268
16269
16270
16271 t = fsizn / sf;
16272
16273
16274
16275
16276
16277
16278
16279 i__1 = *n;
16280 for (n0 = 1; n0 <= i__1; ++n0) {
16281 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16282 goto L4;
16283 }
16284 p0[0] = r11 * x[n0] + r12 * y[n0];
16285 p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16286 if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16287 goto L4;
16288 }
16289
16290
16291
16292
16293
16294
16295
16296
16297
16298 L4:
16299 ;
16300 }
16301 }
16302
16303
16304
16305
16306 t = fsizt / sf;
16307
16308
16309
16310
16311 p0[1] = wr + t * 3.;
16312
16313
16314
16315
16316
16317 if (annot) {
16318
16319
16320
16321 p0[0] = -wr;
16322 p0[1] = -wr - 50. / sf;
16323
16324
16325 p0[1] -= t * 2.;
16326
16327
16328
16329
16330
16331 }
16332
16333
16334
16335
16336
16337
16338
16339
16340
16341
16342
16343
16344
16345
16346
16347
16348
16349
16350 *ier = 0;
16351 return 0;
16352
16353
16354
16355 L11:
16356 *ier = 1;
16357 return 0;
16358
16359
16360
16361 L12:
16362 *ier = 2;
16363 return 0;
16364
16365
16366
16367
16368 *ier = 3;
16369 return 0;
16370 }
16371
16372 int trprnt_(int *n, double *x, double *y,
16373 double *z__, int *iflag, int *list, int *lptr,
16374 int *lend, int *lout)
16375 {
16376
16377
16378 static int nmax = 9999;
16379 static int nlmax = 58;
16380
16381
16382 int i__1;
16383
16384
16385 static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16386 400];
16387
16388
16389
16390
16391
16392
16393
16394
16395
16396
16397
16398
16399
16400
16401
16402
16403
16404
16405
16406
16407
16408
16409
16410
16411
16412
16413
16414
16415
16416
16417
16418
16419
16420
16421
16422
16423
16424
16425
16426
16427
16428
16429
16430
16431
16432
16433
16434
16435
16436
16437
16438
16439
16440
16441
16442
16443
16444
16445
16446
16447
16448
16449 --lend;
16450 --z__;
16451 --y;
16452 --x;
16453 --list;
16454 --lptr;
16455
16456
16457
16458
16459
16460
16461
16462
16463
16464
16465
16466
16467
16468
16469
16470
16471
16472
16473
16474
16475
16476
16477
16478
16479
16480
16481
16482 nn = *n;
16483 lun = *lout;
16484 if (lun < 0 || lun > 99) {
16485 lun = 6;
16486 }
16487
16488
16489
16490
16491 if (nn < 3 || nn > nmax) {
16492
16493
16494
16495
16496 return 0;
16497 }
16498
16499
16500
16501
16502 nl = 6;
16503 nb = 0;
16504 if (*iflag < 0) {
16505
16506
16507
16508
16509
16510 i__1 = nn;
16511 for (node = 1; node <= i__1; ++node) {
16512 lpl = lend[node];
16513 lp = lpl;
16514 k = 0;
16515
16516 L1:
16517 ++k;
16518 lp = lptr[lp];
16519 nd = list[lp];
16520 nabor[k - 1] = nd;
16521 if (lp != lpl) {
16522 goto L1;
16523 }
16524 if (nd <= 0) {
16525
16526
16527
16528
16529
16530 nabor[k - 1] = -nd;
16531 ++k;
16532 nabor[k - 1] = 0;
16533 ++nb;
16534 }
16535
16536
16537
16538 inc = (k - 1) / 14 + 2;
16539 nl += inc;
16540 if (nl > nlmax) {
16541
16542 nl = inc;
16543 }
16544
16545
16546
16547
16548 }
16549 } else if (*iflag > 0) {
16550
16551
16552
16553
16554 i__1 = nn;
16555 for (node = 1; node <= i__1; ++node) {
16556 lpl = lend[node];
16557 lp = lpl;
16558 k = 0;
16559
16560 L3:
16561 ++k;
16562 lp = lptr[lp];
16563 nd = list[lp];
16564 nabor[k - 1] = nd;
16565 if (lp != lpl) {
16566 goto L3;
16567 }
16568 if (nd <= 0) {
16569
16570
16571
16572 nabor[k - 1] = -nd;
16573 ++k;
16574 nabor[k - 1] = 0;
16575 ++nb;
16576 }
16577
16578
16579
16580 inc = (k - 1) / 8 + 2;
16581 nl += inc;
16582 if (nl > nlmax) {
16583
16584 nl = inc;
16585 }
16586
16587
16588
16589
16590
16591 }
16592 } else {
16593
16594
16595
16596
16597 i__1 = nn;
16598 for (node = 1; node <= i__1; ++node) {
16599 lpl = lend[node];
16600 lp = lpl;
16601 k = 0;
16602
16603 L5:
16604 ++k;
16605 lp = lptr[lp];
16606 nd = list[lp];
16607 nabor[k - 1] = nd;
16608 if (lp != lpl) {
16609 goto L5;
16610 }
16611 if (nd <= 0) {
16612
16613
16614
16615 nabor[k - 1] = -nd;
16616 ++k;
16617 nabor[k - 1] = 0;
16618 ++nb;
16619 }
16620
16621
16622
16623 inc = (k - 1) / 5 + 2;
16624 nl += inc;
16625 if (nl > nlmax) {
16626
16627 nl = inc;
16628 }
16629
16630
16631
16632
16633
16634 }
16635 }
16636
16637
16638
16639
16640 if (nb != 0) {
16641 na = nn * 3 - nb - 3;
16642 nt = (nn << 1) - nb - 2;
16643 } else {
16644 na = nn * 3 - 6;
16645 nt = (nn << 1) - 4;
16646 }
16647
16648 return 0;
16649
16650
16651
16652
16653
16654
16655
16656
16657
16658
16659
16660
16661
16662
16663
16664
16665
16666
16667
16668
16669 }
16670
16671 int vrplot_(int *lun, double *pltsiz, double *
16672 elat, double *elon, double *a, int *n, double *x,
16673 double *y, double *z__, int *nt, int *listc, int *
16674 lptr, int *lend, double *xc, double *yc, double *zc,
16675 char *, long int *numbr, int *ier, short)
16676 {
16677
16678
16679 static long int annot = TRUE_;
16680 static double fsizn = 10.;
16681 static double fsizt = 16.;
16682 static double tol = .5;
16683
16684
16685 int i__1;
16686 double d__1;
16687
16688
16689
16690
16691
16692
16693
16694 static double t;
16695 static int n0;
16696 static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16697 sf;
16698 static int ir, lp;
16699 static double ex, ey, ez, wr, tx, ty;
16700 static long int in1, in2;
16701 static int kv1, kv2, lpl;
16702 static double wrs;
16703 static int ipx1, ipx2, ipy1, ipy2, nseg;
16704 extern int drwarc_(int *, double *, double *,
16705 double *, int *);
16706
16707
16708
16709
16710
16711
16712
16713
16714
16715
16716
16717
16718
16719
16720
16721
16722
16723
16724
16725
16726
16727
16728
16729
16730
16731
16732
16733
16734
16735
16736
16737
16738
16739
16740
16741
16742
16743
16744
16745
16746
16747
16748
16749
16750
16751
16752
16753
16754
16755
16756
16757
16758
16759
16760
16761
16762
16763
16764
16765
16766
16767
16768
16769
16770
16771
16772
16773
16774
16775
16776
16777
16778
16779
16780
16781
16782
16783
16784
16785
16786
16787
16788
16789
16790
16791
16792
16793
16794
16795
16796
16797
16798
16799
16800
16801
16802
16803
16804
16805
16806
16807
16808
16809
16810
16811
16812
16813
16814
16815
16816
16817
16818
16819
16820
16821
16822
16823
16824
16825
16826
16827
16828
16829
16830
16831
16832
16833
16834
16835
16836
16837
16838
16839
16840
16841
16842
16843
16844
16845
16846
16847
16848 --lend;
16849 --z__;
16850 --y;
16851 --x;
16852 --zc;
16853 --yc;
16854 --xc;
16855 --listc;
16856 --lptr;
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 if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
16916 nt != 2 * *n - 4) {
16917 goto L11;
16918 }
16919 if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16920 goto L12;
16921 }
16922
16923
16924
16925
16926 cf = atan(1.) / 45.;
16927 wr = sin(cf * *a);
16928 wrs = wr * wr;
16929
16930
16931
16932
16933
16934
16935
16936
16937
16938
16939 d__1 = *pltsiz * 36.;
16940 ir = i_dnnt(&d__1);
16941 ipx1 = 306 - ir;
16942 ipx2 = ir + 306;
16943 ipy1 = 396 - ir;
16944 ipy2 = ir + 396;
16945
16946
16947
16948
16949
16950
16951
16952
16953
16954
16955
16956
16957
16958 d__1 = (double) ir * .88;
16959 ir = i_dnnt(&d__1);
16960 ipx1 = 306 - ir;
16961 ipx2 = ir + 306;
16962 ipy1 = 396 - ir;
16963 ipy2 = ir + 396;
16964
16965
16966
16967
16968 t = 2.;
16969
16970
16971
16972
16973
16974
16975
16976
16977
16978
16979 sf = (double) ir / wr;
16980 tx = ipx1 + sf * wr;
16981 ty = ipy1 + sf * wr;
16982
16983
16984
16985
16986
16987
16988
16989
16990 t = 1. / sf;
16991
16992
16993
16994
16995
16996
16997
16998
16999
17000
17001
17002
17003
17004
17005
17006
17007
17008
17009
17010
17011
17012
17013
17014 t = cf * *elon;
17015 ct = cos(cf * *elat);
17016 ex = ct * cos(t);
17017 ey = ct * sin(t);
17018 ez = sin(cf * *elat);
17019 if (ct != 0.) {
17020 r11 = -ey / ct;
17021 r12 = ex / ct;
17022 } else {
17023 r11 = 0.;
17024 r12 = 1.;
17025 }
17026 r21 = -ez * r12;
17027 r22 = ez * r11;
17028 r23 = ct;
17029
17030
17031
17032
17033 i__1 = *n;
17034 for (n0 = 1; n0 <= i__1; ++n0) {
17035 lpl = lend[n0];
17036
17037
17038
17039
17040 kv2 = listc[lpl];
17041 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17042 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17043 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17044
17045
17046
17047 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17048
17049
17050
17051
17052 lp = lpl;
17053 L1:
17054 lp = lptr[lp];
17055 kv1 = kv2;
17056 p1[0] = p2[0];
17057 p1[1] = p2[1];
17058 p1[2] = p2[2];
17059 in1 = in2;
17060 kv2 = listc[lp];
17061
17062
17063
17064 p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17065 p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17066 p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17067 in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17068
17069
17070
17071
17072
17073 if (! in1 || in2 && kv2 <= kv1) {
17074 goto L2;
17075 }
17076 if (p2[2] < 0.) {
17077
17078
17079
17080
17081
17082 p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17083 p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17084 t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17085 p2[0] /= t;
17086 p2[1] /= t;
17087 }
17088
17089
17090
17091
17092 if (p2[2] < 0.) {
17093 p2[2] = 0.f;
17094 }
17095 d__1 = tol / sf;
17096 drwarc_(lun, p1, p2, &d__1, &nseg);
17097
17098
17099
17100 L2:
17101 if (lp != lpl) {
17102 goto L1;
17103 }
17104
17105 }
17106
17107
17108
17109
17110
17111
17112
17113 if (*numbr) {
17114
17115
17116
17117
17118
17119 t = fsizn / sf;
17120
17121
17122
17123
17124
17125
17126
17127 i__1 = *n;
17128 for (n0 = 1; n0 <= i__1; ++n0) {
17129 if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17130 goto L4;
17131 }
17132 x0 = r11 * x[n0] + r12 * y[n0];
17133 y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17134 if (x0 * x0 + y0 * y0 > wrs) {
17135 goto L4;
17136 }
17137
17138
17139
17140
17141
17142
17143
17144
17145 L4:
17146 ;
17147 }
17148 }
17149
17150
17151
17152
17153 t = fsizt / sf;
17154
17155
17156
17157
17158 y0 = wr + t * 3.;
17159
17160
17161
17162
17163
17164 if (annot) {
17165
17166
17167
17168 x0 = -wr;
17169 y0 = -wr - 50. / sf;
17170
17171
17172 y0 -= t * 2.;
17173
17174
17175
17176
17177
17178 }
17179
17180
17181
17182
17183
17184
17185
17186
17187
17188
17189
17190
17191
17192
17193
17194
17195
17196
17197 *ier = 0;
17198 return 0;
17199
17200
17201
17202 L11:
17203 *ier = 1;
17204 return 0;
17205
17206
17207
17208 L12:
17209 *ier = 2;
17210 return 0;
17211
17212
17213
17214
17215 *ier = 3;
17216 return 0;
17217 }
17218
17219 int random_(int *ix, int *iy, int *iz,
17220 double *rannum)
17221 {
17222 static double x;
17223
17224
17225
17226
17227
17228
17229
17230
17231
17232
17233
17234
17235
17236
17237
17238 *ix = *ix * 171 % 30269;
17239 *iy = *iy * 172 % 30307;
17240 *iz = *iz * 170 % 30323;
17241 x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17242 double) (*iz) / 30323.;
17243 *rannum = x - (int) x;
17244 return 0;
17245 }
17246
17247 #undef TRUE_
17248 #undef FALSE_
17249 #undef abs
17250
17251
17252
17253
17254
17255
17256
17257
17258
17259 EMData* Util::mult_scalar(EMData* img, float scalar)
17260 {
17261 ENTERFUNC;
17262
17263 if (!img) {
17264 throw NullPointerException("NULL input image");
17265 }
17266
17267
17268 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17269 int size = nx*ny*nz;
17270 EMData * img2 = img->copy_head();
17271 float *img_ptr =img->get_data();
17272 float *img2_ptr = img2->get_data();
17273 for (int i=0;i<size;i++)img2_ptr[i] = img_ptr[i]*scalar;
17274 img2->update();
17275
17276 if(img->is_complex()) {
17277 img2->set_complex(true);
17278 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17279 }
17280 EXITFUNC;
17281 return img2;
17282 }
17283
17284 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17285 {
17286 ENTERFUNC;
17287
17288 if (!img) {
17289 throw NullPointerException("NULL input image");
17290 }
17291
17292
17293 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17294 int size = nx*ny*nz;
17295 EMData * img2 = img->copy_head();
17296 float *img_ptr =img->get_data();
17297 float *img2_ptr = img2->get_data();
17298 float *img1_ptr = img1->get_data();
17299 for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17300 img2->update();
17301 if(img->is_complex()) {
17302 img2->set_complex(true);
17303 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17304 }
17305
17306 EXITFUNC;
17307 return img2;
17308 }
17309
17310 EMData* Util::addn_img(EMData* img, EMData* img1)
17311 {
17312 ENTERFUNC;
17313
17314 if (!img) {
17315 throw NullPointerException("NULL input image");
17316 }
17317
17318
17319 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17320 int size = nx*ny*nz;
17321 EMData * img2 = img->copy_head();
17322 float *img_ptr =img->get_data();
17323 float *img2_ptr = img2->get_data();
17324 float *img1_ptr = img1->get_data();
17325 for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17326 img2->update();
17327 if(img->is_complex()) {
17328 img2->set_complex(true);
17329 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17330 }
17331
17332 EXITFUNC;
17333 return img2;
17334 }
17335
17336 EMData* Util::subn_img(EMData* img, EMData* img1)
17337 {
17338 ENTERFUNC;
17339
17340 if (!img) {
17341 throw NullPointerException("NULL input image");
17342 }
17343
17344
17345 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17346 int size = nx*ny*nz;
17347 EMData * img2 = img->copy_head();
17348 float *img_ptr =img->get_data();
17349 float *img2_ptr = img2->get_data();
17350 float *img1_ptr = img1->get_data();
17351 for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17352 img2->update();
17353 if(img->is_complex()) {
17354 img2->set_complex(true);
17355 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17356 }
17357
17358 EXITFUNC;
17359 return img2;
17360 }
17361
17362 EMData* Util::muln_img(EMData* img, EMData* img1)
17363 {
17364 ENTERFUNC;
17365
17366 if (!img) {
17367 throw NullPointerException("NULL input image");
17368 }
17369
17370
17371 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17372 int size = nx*ny*nz;
17373 EMData * img2 = img->copy_head();
17374 float *img_ptr =img->get_data();
17375 float *img2_ptr = img2->get_data();
17376 float *img1_ptr = img1->get_data();
17377 if(img->is_complex()) {
17378 for (int i=0; i<size; i+=2) {
17379 img2_ptr[i] = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17380 img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17381 }
17382 img2->set_complex(true);
17383 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17384 } else {
17385 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17386 img2->update();
17387 }
17388
17389 EXITFUNC;
17390 return img2;
17391 }
17392
17393 EMData* Util::divn_img(EMData* img, EMData* img1)
17394 {
17395 ENTERFUNC;
17396
17397 if (!img) {
17398 throw NullPointerException("NULL input image");
17399 }
17400
17401
17402 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17403 int size = nx*ny*nz;
17404 EMData * img2 = img->copy_head();
17405 float *img_ptr =img->get_data();
17406 float *img2_ptr = img2->get_data();
17407 float *img1_ptr = img1->get_data();
17408 if(img->is_complex()) {
17409 float sq2;
17410 for (int i=0; i<size; i+=2) {
17411 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17412 img2_ptr[i] = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17413 img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17414 }
17415 img2->set_complex(true);
17416 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17417 } else {
17418 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17419 img2->update();
17420 }
17421
17422 EXITFUNC;
17423 return img2;
17424 }
17425
17426 EMData* Util::divn_filter(EMData* img, EMData* img1)
17427 {
17428 ENTERFUNC;
17429
17430 if (!img) {
17431 throw NullPointerException("NULL input image");
17432 }
17433
17434
17435 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17436 int size = nx*ny*nz;
17437 EMData * img2 = img->copy_head();
17438 float *img_ptr =img->get_data();
17439 float *img1_ptr = img1->get_data();
17440 float *img2_ptr = img2->get_data();
17441 if(img->is_complex()) {
17442 for (int i=0; i<size; i+=2) {
17443 if(img1_ptr[i] > 1.e-10f) {
17444 img2_ptr[i] = img_ptr[i] /img1_ptr[i];
17445 img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17446 } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17447 }
17448 } else throw ImageFormatException("Only Fourier image allowed");
17449
17450 img->update();
17451
17452 EXITFUNC;
17453 return img2;
17454 }
17455
17456 void Util::mul_scalar(EMData* img, float scalar)
17457 {
17458 ENTERFUNC;
17459
17460 if (!img) {
17461 throw NullPointerException("NULL input image");
17462 }
17463
17464
17465 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17466 int size = nx*ny*nz;
17467 float *img_ptr =img->get_data();
17468 for (int i=0;i<size;i++) img_ptr[i] *= scalar;
17469 img->update();
17470
17471 EXITFUNC;
17472 }
17473
17474 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17475 {
17476 ENTERFUNC;
17477
17478 if (!img) {
17479 throw NullPointerException("NULL input image");
17480 }
17481
17482
17483 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17484 int size = nx*ny*nz;
17485 float *img_ptr =img->get_data();
17486 float *img1_ptr = img1->get_data();
17487 for (int i=0;i<size;i++)img_ptr[i] += img1_ptr[i]*scalar;
17488 img1->update();
17489
17490 EXITFUNC;
17491 }
17492
17493 void Util::add_img(EMData* img, EMData* img1)
17494 {
17495 ENTERFUNC;
17496
17497 if (!img) {
17498 throw NullPointerException("NULL input image");
17499 }
17500
17501
17502 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17503 int size = nx*ny*nz;
17504 float *img_ptr = img->get_data();
17505 float *img1_ptr = img1->get_data();
17506 for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i];
17507 img->update();
17508
17509 EXITFUNC;
17510 }
17511
17512 void Util::add_img_abs(EMData* img, EMData* img1)
17513 {
17514 ENTERFUNC;
17515
17516 if (!img) {
17517 throw NullPointerException("NULL input image");
17518 }
17519
17520
17521 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17522 int size = nx*ny*nz;
17523 float *img_ptr = img->get_data();
17524 float *img1_ptr = img1->get_data();
17525 for (int i=0;i<size;i++) img_ptr[i] += abs(img1_ptr[i]);
17526 img->update();
17527
17528 EXITFUNC;
17529 }
17530
17531 void Util::add_img2(EMData* img, EMData* img1)
17532 {
17533 ENTERFUNC;
17534
17535 if (!img) {
17536 throw NullPointerException("NULL input image");
17537 }
17538
17539
17540 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17541 int size = nx*ny*nz;
17542 float *img_ptr = img->get_data();
17543 float *img1_ptr = img1->get_data();
17544 if(img->is_complex()) {
17545 for (int i=0; i<size; i+=2) img_ptr[i] += img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1] ;
17546 } else {
17547 for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17548 }
17549 img->update();
17550
17551 EXITFUNC;
17552 }
17553
17554 void Util::sub_img(EMData* img, EMData* img1)
17555 {
17556 ENTERFUNC;
17557
17558 if (!img) {
17559 throw NullPointerException("NULL input image");
17560 }
17561
17562
17563 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17564 int size = nx*ny*nz;
17565 float *img_ptr = img->get_data();
17566 float *img1_ptr = img1->get_data();
17567 for (int i=0;i<size;i++) img_ptr[i] -= img1_ptr[i];
17568 img->update();
17569
17570 EXITFUNC;
17571 }
17572
17573 void Util::mul_img(EMData* img, EMData* img1)
17574 {
17575 ENTERFUNC;
17576
17577 if (!img) {
17578 throw NullPointerException("NULL input image");
17579 }
17580
17581
17582 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17583 int size = nx*ny*nz;
17584 float *img_ptr = img->get_data();
17585 float *img1_ptr = img1->get_data();
17586 if(img->is_complex()) {
17587 for (int i=0; i<size; i+=2) {
17588 float tmp = img_ptr[i] * img1_ptr[i] - img_ptr[i+1] * img1_ptr[i+1] ;
17589 img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17590 img_ptr[i] = tmp;
17591
17592 }
17593 } else {
17594 for (int i=0;i<size;i++) img_ptr[i] *= img1_ptr[i];
17595 }
17596 img->update();
17597
17598 EXITFUNC;
17599 }
17600
17601 void Util::div_img(EMData* img, EMData* img1)
17602 {
17603 ENTERFUNC;
17604
17605 if (!img) {
17606 throw NullPointerException("NULL input image");
17607 }
17608
17609
17610 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17611 int size = nx*ny*nz;
17612 float *img_ptr = img->get_data();
17613 float *img1_ptr = img1->get_data();
17614 if(img->is_complex()) {
17615 float sq2;
17616 for (int i=0; i<size; i+=2) {
17617 sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1]);
17618 float tmp = sq2*(img_ptr[i] * img1_ptr[i] + img_ptr[i+1] * img1_ptr[i+1]) ;
17619 img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17620 img_ptr[i] = tmp;
17621 }
17622 } else {
17623 for (int i=0; i<size; i++) img_ptr[i] /= img1_ptr[i];
17624 }
17625 img->update();
17626
17627 EXITFUNC;
17628 }
17629
17630 void Util::div_filter(EMData* img, EMData* img1)
17631 {
17632 ENTERFUNC;
17633
17634 if (!img) {
17635 throw NullPointerException("NULL input image");
17636 }
17637
17638
17639 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17640 int size = nx*ny*nz;
17641 float *img_ptr = img->get_data();
17642 float *img1_ptr = img1->get_data();
17643 if(img->is_complex()) {
17644 for (int i=0; i<size; i+=2) {
17645 if(img1_ptr[i] > 1.e-10f) {
17646 img_ptr[i] /= img1_ptr[i];
17647 img_ptr[i+1] /= img1_ptr[i];
17648 } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17649 }
17650 } else throw ImageFormatException("Only Fourier image allowed");
17651
17652 img->update();
17653
17654 EXITFUNC;
17655 }
17656
17657 #define img_ptr(i,j,k) img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*nxo]
17658
17659 EMData* Util::pack_complex_to_real(EMData* img)
17660 {
17661 ENTERFUNC;
17662
17663 if (!img) {
17664 throw NullPointerException("NULL input image");
17665 }
17666
17667
17668
17669 int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17670 int nx = nxo - 2 + img->is_fftodd();
17671 int lsd2 = (nx + 2 - nx%2) / 2;
17672 int nyt, nzt;
17673 int nx2 = nx/2;
17674 int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17675 int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17676 int nx2p = nx2+nx%2;
17677 int ny2p = ny2+ny%2;
17678 int nz2p = nz2+nz%2;
17679 EMData& power = *(new EMData());
17680 power.set_size(nx, ny, nz);
17681 power.set_array_offsets(-nx2,-ny2,-nz2);
17682
17683 float *img_ptr = img->get_data();
17684 for (int iz = 1; iz <= nz; iz++) {
17685 int jz=iz-1;
17686 if(jz>=nz2p) jz=jz-nzt;
17687 for (int iy = 1; iy <= ny; iy++) {
17688 int jy=iy-1;
17689 if(jy>=ny2p) jy=jy-nyt;
17690 for (int ix = 1; ix <= lsd2; ix++) {
17691 int jx=ix-1;
17692 if(jx>=nx2p) jx=jx-nx;
17693 power(jx,jy,jz) = img_ptr(ix,iy,iz);
17694 }
17695 }
17696 }
17697
17698 int nzb, nze, nyb, nye, nxb, nxe;
17699 nxb =-nx2+(nx+1)%2;
17700 nxe = nx2-(nx+1)%2;
17701 if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17702 if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17703 for (int iz = nzb; iz <= nze; iz++) {
17704 for (int iy = nyb; iy <= nye; iy++) {
17705 for (int ix = 1; ix <= nxe; ix++) {
17706 power(-ix,-iy,-iz) = power(ix,iy,iz);
17707 }
17708 }
17709 }
17710 if(ny2 != 0) {
17711 if(nz2 != 0) {
17712 if(nz%2 == 0) {
17713 for (int iy = nyb; iy <= nye; iy++) {
17714 for (int ix = nxb; ix <= -1; ix++) {
17715 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17716 }
17717 }
17718 if(ny%2 == 0) {
17719 for (int ix = nxb; ix <= -1; ix++) {
17720 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17721 }
17722 }
17723 }
17724 }
17725 if(ny%2 == 0) {
17726 for (int iz = nzb; iz <= nze; iz++) {
17727 for (int ix = nxb; ix <= -1; ix++) {
17728 power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17729 }
17730 }
17731 }
17732
17733 }
17734 power.update();
17735 power.set_array_offsets(0,0,0);
17736 return &power;
17737 }
17738 #undef img_ptr
17739
17740 float Util::ang_n(float peakp, string mode, int maxrin)
17741 {
17742 if (mode == "f" || mode == "F")
17743 return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17744 else
17745 return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17746 }
17747
17748
17749 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17750 {
17751 float* data = ring->get_data();
17752 float av=0.0;
17753 float sq=0.0;
17754 float nn=0.0;
17755 int nring = numr.size()/3;
17756 for( int i=0; i < nring; ++i )
17757 {
17758 int numr3i = numr[3*i+2];
17759 int numr2i = numr[3*i+1]-1;
17760 float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17761 for( int j=0; j < numr3i; ++j )
17762 {
17763 int jc = numr2i+j;
17764 av += data[jc] * w;
17765 sq += data[jc] * data[jc] * w;
17766 nn += w;
17767 }
17768 }
17769
17770 float avg = av/nn;
17771 float sgm = sqrt( (sq-av*av/nn)/nn );
17772 int n = ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17773 for( int i=0; i < n; ++i )
17774 {
17775 data[i] -= avg;
17776 data[i] /= sgm;
17777 }
17778
17779 ring->update();
17780 }
17781
17782 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17783 float xrng, float yrng, float step, string mode,
17784 vector<int>numr, float cnx, float cny) {
17785
17786
17787
17788
17789
17790
17791
17792
17793
17794
17795
17796
17797 size_t crefim_len = crefim.size();
17798
17799 int ky = int(2*yrng/step+0.5)/2;
17800 int kx = int(2*xrng/step+0.5)/2;
17801 int iref, nref=0, mirror=0;
17802 float iy, ix, sx=0, sy=0;
17803 float peak = -1.0E23f;
17804 float ang=0.0f;
17805 for (int i = -ky; i <= ky; i++) {
17806 iy = i * step ;
17807 for (int j = -kx; j <= kx; j++) {
17808 ix = j*step ;
17809 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17810
17811 Normalize_ring( cimage, numr );
17812
17813 Frngs(cimage, numr);
17814
17815
17816 for ( iref = 0; iref < (int)crefim_len; iref++) {
17817 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17818 double qn = retvals["qn"];
17819 double qm = retvals["qm"];
17820 if(qn >= peak || qm >= peak) {
17821 sx = -ix;
17822 sy = -iy;
17823 nref = iref;
17824 if (qn >= qm) {
17825 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17826 peak = static_cast<float>(qn);
17827 mirror = 0;
17828 } else {
17829 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17830 peak = static_cast<float>(qm);
17831 mirror = 1;
17832 }
17833 }
17834 } delete cimage; cimage = 0;
17835 }
17836 }
17837 float co, so, sxs, sys;
17838 co = static_cast<float>( cos(ang*pi/180.0) );
17839 so = static_cast<float>( -sin(ang*pi/180.0) );
17840 sxs = sx*co - sy*so;
17841 sys = sx*so + sy*co;
17842 vector<float> res;
17843 res.push_back(ang);
17844 res.push_back(sxs);
17845 res.push_back(sys);
17846 res.push_back(static_cast<float>(mirror));
17847 res.push_back(static_cast<float>(nref));
17848 res.push_back(peak);
17849 return res;
17850 }
17851
17852 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
17853 float xrng, float yrng, float step, string mode,
17854 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
17855
17856
17857
17858
17859
17860
17861
17862
17863
17864
17865
17866
17867 size_t crefim_len = crefim.size();
17868
17869 int ky = int(2*yrng/step+0.5)/2;
17870 int kx = int(2*xrng/step+0.5)/2;
17871 int iref, nref=0, mirror=0;
17872 float iy, ix, sx=0, sy=0;
17873 float peak = -1.0E23f;
17874 float ang=0.0f;
17875 for (int i = -ky; i <= ky; i++) {
17876 iy = i * step ;
17877 for (int j = -kx; j <= kx; j++) {
17878 ix = j*step ;
17879 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17880
17881 Normalize_ring( cimage, numr );
17882
17883 Frngs(cimage, numr);
17884
17885
17886 for ( iref = 0; iref < (int)crefim_len; iref++) {
17887 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
17888 double qn = retvals["qn"];
17889 double qm = retvals["qm"];
17890 if(qn >= peak || qm >= peak) {
17891 sx = -ix;
17892 sy = -iy;
17893 nref = iref;
17894 if (qn >= qm) {
17895 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17896 peak = static_cast<float>(qn);
17897 mirror = 0;
17898 } else {
17899 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17900 peak = static_cast<float>(qm);
17901 mirror = 1;
17902 }
17903 }
17904 } delete cimage; cimage = 0;
17905 }
17906 }
17907 float co, so, sxs, sys;
17908 co = static_cast<float>( cos(ang*pi/180.0) );
17909 so = static_cast<float>( -sin(ang*pi/180.0) );
17910 sxs = sx*co - sy*so;
17911 sys = sx*so + sy*co;
17912 vector<float> res;
17913 res.push_back(ang);
17914 res.push_back(sxs);
17915 res.push_back(sys);
17916 res.push_back(static_cast<float>(mirror));
17917 res.push_back(static_cast<float>(nref));
17918 res.push_back(peak);
17919 return res;
17920 }
17921
17922 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
17923 float xrng, float yrng, float step, string mode,
17924 vector< int >numr, float cnx, float cny) {
17925
17926
17927
17928
17929
17930
17931
17932
17933
17934
17935
17936 size_t crefim_len = crefim.size();
17937
17938 int ky = int(2*yrng/step+0.5)/2;
17939 int kx = int(2*xrng/step+0.5)/2;
17940 int iref, nref=0;
17941 float iy, ix, sx=0, sy=0;
17942 float peak = -1.0E23f;
17943 float ang=0.0f;
17944 for (int i = -ky; i <= ky; i++) {
17945 iy = i * step ;
17946 for (int j = -kx; j <= kx; j++) {
17947 ix = j*step ;
17948 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17949 Frngs(cimage, numr);
17950
17951
17952 for ( iref = 0; iref < (int)crefim_len; iref++) {
17953 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
17954 double qn = retvals["qn"];
17955 if(qn >= peak) {
17956 sx = -ix;
17957 sy = -iy;
17958 nref = iref;
17959 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17960 peak = static_cast<float>(qn);
17961 }
17962 } delete cimage; cimage = 0;
17963 }
17964 }
17965 float co, so, sxs, sys;
17966 co = static_cast<float>( cos(ang*pi/180.0) );
17967 so = static_cast<float>( -sin(ang*pi/180.0) );
17968 sxs = sx*co - sy*so;
17969 sys = sx*so + sy*co;
17970 vector<float> res;
17971 res.push_back(ang);
17972 res.push_back(sxs);
17973 res.push_back(sys);
17974 res.push_back(static_cast<float>(nref));
17975 res.push_back(peak);
17976 return res;
17977 }
17978
17979 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
17980 float xrng, float yrng, float step, float ant, string mode,
17981 vector<int>numr, float cnx, float cny) {
17982
17983
17984
17985
17986
17987
17988
17989
17990
17991
17992
17993 size_t crefim_len = crefim.size();
17994 const float qv = static_cast<float>( pi/180.0 );
17995
17996 Transform * t = image->get_attr("xform.projection");
17997 Dict d = t->get_params("spider");
17998 if(t) {delete t; t=0;}
17999 float phi = d["phi"];
18000 float theta = d["theta"];
18001 int ky = int(2*yrng/step+0.5)/2;
18002 int kx = int(2*xrng/step+0.5)/2;
18003 int iref, nref=0, mirror=0;
18004 float iy, ix, sx=0, sy=0;
18005 float peak = -1.0E23f;
18006 float ang=0.0f;
18007 float imn1 = sin(theta*qv)*cos(phi*qv);
18008 float imn2 = sin(theta*qv)*sin(phi*qv);
18009 float imn3 = cos(theta*qv);
18010 vector<float> n1(crefim_len);
18011 vector<float> n2(crefim_len);
18012 vector<float> n3(crefim_len);
18013 for ( iref = 0; iref < (int)crefim_len; iref++) {
18014 n1[iref] = crefim[iref]->get_attr("n1");
18015 n2[iref] = crefim[iref]->get_attr("n2");
18016 n3[iref] = crefim[iref]->get_attr("n3");
18017 }
18018 for (int i = -ky; i <= ky; i++) {
18019 iy = i * step ;
18020 for (int j = -kx; j <= kx; j++) {
18021 ix = j*step;
18022 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18023
18024 Normalize_ring( cimage, numr );
18025
18026 Frngs(cimage, numr);
18027
18028
18029 for ( iref = 0; iref < (int)crefim_len; iref++) {
18030 if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18031 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18032 double qn = retvals["qn"];
18033 double qm = retvals["qm"];
18034 if(qn >= peak || qm >= peak) {
18035 sx = -ix;
18036 sy = -iy;
18037 nref = iref;
18038 if (qn >= qm) {
18039 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18040 peak = static_cast<float>( qn );
18041 mirror = 0;
18042 } else {
18043 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18044 peak = static_cast<float>( qm );
18045 mirror = 1;
18046 }
18047 }
18048 }
18049 } delete cimage; cimage = 0;
18050 }
18051 }
18052 float co, so, sxs, sys;
18053 if(peak == -1.0E23) {
18054 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18055 nref = -1;
18056 } else {
18057 co = cos(ang*qv);
18058 so = -sin(ang*qv);
18059 sxs = sx*co - sy*so;
18060 sys = sx*so + sy*co;
18061 }
18062 vector<float> res;
18063 res.push_back(ang);
18064 res.push_back(sxs);
18065 res.push_back(sys);
18066 res.push_back(static_cast<float>(mirror));
18067 res.push_back(static_cast<float>(nref));
18068 res.push_back(peak);
18069 return res;
18070 }
18071
18072 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18073 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18074 vector<int>numr, float cnx, float cny) {
18075
18076
18077
18078
18079
18080
18081
18082
18083
18084
18085
18086 size_t crefim_len = crefim.size();
18087 const float qv = static_cast<float>(pi/180.0);
18088
18089 Transform* t = image->get_attr("xform.projection");
18090 Dict d = t->get_params("spider");
18091 if(t) {delete t; t=0;}
18092 float phi = d["phi"];
18093 float theta = d["theta"];
18094 float psi = d["psi"];
18095 int ky = int(2*yrng/step+0.5)/2;
18096 int kx = int(2*xrng/step+0.5)/2;
18097 int iref, nref = 0, mirror = 0;
18098 float iy, ix, sx = 0, sy = 0;
18099 float peak = -1.0E23f;
18100 float ang = 0.0f;
18101 float imn1 = sin(theta*qv)*cos(phi*qv);
18102 float imn2 = sin(theta*qv)*sin(phi*qv);
18103 float imn3 = cos(theta*qv);
18104 vector<float> n1(crefim_len);
18105 vector<float> n2(crefim_len);
18106 vector<float> n3(crefim_len);
18107 for (iref = 0; iref < (int)crefim_len; iref++) {
18108 n1[iref] = crefim[iref]->get_attr("n1");
18109 n2[iref] = crefim[iref]->get_attr("n2");
18110 n3[iref] = crefim[iref]->get_attr("n3");
18111 }
18112 bool nomirror = (theta<90.0) || (theta==90.0) && (psi<psi_max);
18113 if (!nomirror) {
18114 phi = fmod(phi+540.0f, 360.0f);
18115 theta = 180-theta;
18116 psi = fmod(540.0f-psi, 360.0f);
18117 }
18118 for (int i = -ky; i <= ky; i++) {
18119 iy = i * step ;
18120 for (int j = -kx; j <= kx; j++) {
18121 ix = j*step;
18122 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18123
18124 Normalize_ring(cimage, numr);
18125
18126 Frngs(cimage, numr);
18127
18128
18129 for (iref = 0; iref < (int)crefim_len; iref++) {
18130 if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18131 if (nomirror) {
18132 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 0);
18133 double qn = retvals["qn"];
18134 if (qn >= peak) {
18135 sx = -ix;
18136 sy = -iy;
18137 nref = iref;
18138 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18139 peak = static_cast<float>(qn);
18140 mirror = 0;
18141 }
18142 } else {
18143 Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 1);
18144 double qn = retvals["qn"];
18145 if (qn >= peak) {
18146 sx = -ix;
18147 sy = -iy;
18148 nref = iref;
18149 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18150 peak = static_cast<float>(qn);
18151 mirror = 1;
18152 }
18153 }
18154 }
18155 } delete cimage; cimage = 0;
18156 }
18157 }
18158 float co, so, sxs, sys;
18159 if(peak == -1.0E23) {
18160 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18161 nref = -1;
18162 } else {
18163 co = cos(ang*qv);
18164 so = -sin(ang*qv);
18165 sxs = sx*co - sy*so;
18166 sys = sx*so + sy*co;
18167 }
18168 vector<float> res;
18169 res.push_back(ang);
18170 res.push_back(sxs);
18171 res.push_back(sys);
18172 res.push_back(static_cast<float>(mirror));
18173 res.push_back(static_cast<float>(nref));
18174 res.push_back(peak);
18175 return res;
18176 }
18177
18178
18179 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18180 float xrng, float yrng, float step, float psi_max, string mode,
18181 vector<int>numr, float cnx, float cny) {
18182
18183 size_t crefim_len = crefim.size();
18184
18185 int ky = int(2*yrng/step+0.5)/2;
18186 int kx = int(2*xrng/step+0.5)/2;
18187 int iref, nref=0, mirror=0;
18188 float iy, ix, sx=0, sy=0;
18189 float peak = -1.0E23f;
18190 float ang=0.0f;
18191 for (int i = -ky; i <= ky; i++) {
18192 iy = i * step ;
18193 for (int j = -kx; j <= kx; j++) {
18194 ix = j*step ;
18195 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18196
18197 Normalize_ring( cimage, numr );
18198
18199 Frngs(cimage, numr);
18200
18201
18202 for ( iref = 0; iref < (int)crefim_len; iref++) {
18203 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18204 double qn = retvals["qn"];
18205 double qm = retvals["qm"];
18206 if(qn >= peak || qm >= peak) {
18207 sx = -ix;
18208 sy = -iy;
18209 nref = iref;
18210 if (qn >= qm) {
18211 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18212 peak = static_cast<float>(qn);
18213 mirror = 0;
18214 } else {
18215 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18216 peak = static_cast<float>(qm);
18217 mirror = 1;
18218 }
18219 }
18220 } delete cimage; cimage = 0;
18221 }
18222 }
18223 float co, so, sxs, sys;
18224 co = static_cast<float>( cos(ang*pi/180.0) );
18225 so = static_cast<float>( -sin(ang*pi/180.0) );
18226 sxs = sx*co - sy*so;
18227 sys = sx*so + sy*co;
18228 vector<float> res;
18229 res.push_back(ang);
18230 res.push_back(sxs);
18231 res.push_back(sys);
18232 res.push_back(static_cast<float>(mirror));
18233 res.push_back(static_cast<float>(nref));
18234 res.push_back(peak);
18235 return res;
18236 }
18237
18238 void Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
18239 float xrng, float yrng, float step, string mode,
18240 vector< int >numr, float cnx, float cny,
18241 EMData *peaks, EMData *peakm) {
18242
18243 int maxrin = numr[numr.size()-1];
18244
18245 int ky = int(2*yrng/step+0.5)/2;
18246 int kx = int(2*xrng/step+0.5)/2;
18247
18248 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18249 float *p_ccf1ds = peaks->get_data();
18250
18251 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18252 float *p_ccf1dm = peakm->get_data();
18253
18254 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18255 p_ccf1ds[i] = -1.e20f;
18256 p_ccf1dm[i] = -1.e20f;
18257 }
18258
18259 for (int i = -ky; i <= ky; i++) {
18260 float iy = i * step;
18261 for (int j = -kx; j <= kx; j++) {
18262 float ix = j*step;
18263 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18264 Frngs(cimage, numr);
18265 Crosrng_msg_vec(crefim, cimage, numr,
18266 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18267 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18268 delete cimage; cimage = 0;
18269 }
18270 }
18271 return;
18272 }
18273
18274 void Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
18275 float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
18276 EMData *peaks_compress, EMData *peakm_compress) {
18277
18278 int maxrin = numr[numr.size()-1];
18279
18280 int ky = int(2*yrng/step+0.5)/2;
18281 int kx = int(2*xrng/step+0.5)/2;
18282
18283 peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18284 float *p_ccf1ds = peaks->get_data();
18285
18286 peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18287 float *p_ccf1dm = peakm->get_data();
18288
18289 peaks_compress->set_size(maxrin, 1, 1);
18290 float *p_ccf1ds_compress = peaks_compress->get_data();
18291
18292 peakm_compress->set_size(maxrin, 1, 1);
18293 float *p_ccf1dm_compress = peakm_compress->get_data();
18294
18295 for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18296 p_ccf1ds[i] = -1.e20f;
18297 p_ccf1dm[i] = -1.e20f;
18298 }
18299
18300 for (int i = -ky; i <= ky; i++) {
18301 float iy = i * step;
18302 for (int j = -kx; j <= kx; j++) {
18303 float ix = j*step;
18304 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18305 Frngs(cimage, numr);
18306 Crosrng_msg_vec(crefim, cimage, numr,
18307 p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18308 p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18309 delete cimage; cimage = 0;
18310 }
18311 }
18312 for (int x=0; x<maxrin; x++) {
18313 float maxs = -1.0e22f;
18314 float maxm = -1.0e22f;
18315 for (int i=1; i<=2*ky+1; i++) {
18316 for (int j=1; j<=2*kx+1; j++) {
18317 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
18318 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
18319 }
18320 }
18321 p_ccf1ds_compress[x] = maxs;
18322 p_ccf1dm_compress[x] = maxm;
18323 }
18324 return;
18325 }
18326
18327 struct ccf_point
18328 {
18329 float value;
18330 int i;
18331 int j;
18332 int k;
18333 int mirror;
18334 };
18335
18336
18337 struct ccf_value
18338 {
18339 bool operator()( const ccf_point& a, const ccf_point& b )
18340 {
18341 return a.value > b.value;
18342 }
18343 };
18344
18345
18346 vector<float> Util::ali2d_ccf_list(EMData* image, EMData* crefim,
18347 float xrng, float yrng, float step, string mode,
18348 vector< int >numr, float cnx, float cny, double T) {
18349
18350 int maxrin = numr[numr.size()-1];
18351
18352 int ky = int(2*yrng/step+0.5)/2;
18353 int kx = int(2*xrng/step+0.5)/2;
18354
18355 float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
18356 float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
18357 int vol = maxrin*(2*kx+1)*(2*ky+1);
18358 vector<ccf_point> ccf(2*vol);
18359 ccf_point temp;
18360
18361 int index = 0;
18362 for (int i = -ky; i <= ky; i++) {
18363 float iy = i * step;
18364 for (int j = -kx; j <= kx; j++) {
18365 float ix = j*step;
18366 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18367 Frngs(cimage, numr);
18368 Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
18369 for (int k=0; k<maxrin; k++) {
18370 temp.value = p_ccf1ds[k];
18371 temp.i = k;
18372 temp.j = j;
18373 temp.k = i;
18374 temp.mirror = 0;
18375 ccf[index] = temp;
18376 index++;
18377 temp.value = p_ccf1dm[k];
18378 temp.mirror = 1;
18379 ccf[index] = temp;
18380 index++;
18381 }
18382 delete cimage; cimage = 0;
18383 }
18384 }
18385
18386 delete p_ccf1ds;
18387 delete p_ccf1dm;
18388 std::sort(ccf.begin(), ccf.end(), ccf_value());
18389
18390 double qt = (double)ccf[0].value;
18391 vector <double> p(2*vol), cp(2*vol);
18392
18393 double sump = 0.0;
18394 for (int i=0; i<2*vol; i++) {
18395 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
18396 sump += p[i];
18397 }
18398 for (int i=0; i<2*vol; i++) {
18399 p[i] /= sump;
18400 }
18401 for (int i=1; i<2*vol; i++) {
18402 p[i] += p[i-1];
18403 }
18404 p[2*vol-1] = 2.0;
18405
18406 float t = get_frand(0.0f, 1.0f);
18407 int select = 0;
18408 while (p[select] < t) select += 1;
18409
18410 vector<float> a(6);
18411 a[0] = ccf[select].value;
18412 a[1] = (float)ccf[select].i;
18413 a[2] = (float)ccf[select].j;
18414 a[3] = (float)ccf[select].k;
18415 a[4] = (float)ccf[select].mirror;
18416 a[5] = (float)select;
18417 return a;
18418 }
18419
18420
18421
18422
18423
18424
18425
18426
18427
18428
18429
18430
18431
18432
18433
18434
18435
18436
18437
18438
18439
18440
18441
18442
18443
18444
18445
18446
18447
18448
18449
18450
18451
18452
18453
18454
18455
18456
18457
18458
18459
18460
18461
18462
18463
18464
18465
18466
18467
18468
18469
18470
18471
18472
18473
18474
18475
18476
18477
18478
18479
18480
18481
18482
18483
18484
18485
18486
18487 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18488
18489 EMData *rot;
18490
18491 const int nmax=3, mmax=3;
18492 char task[60], csave[60];
18493 long int lsave[4];
18494 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18495 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];
18496 long int SIXTY=60;
18497
18498
18499 iprint = -1;
18500
18501
18502 factr=1.0e1;
18503 pgtol=1.0e-5;
18504
18505
18506
18507
18508 n=3;
18509 m=3;
18510
18511
18512
18513
18514
18515 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18516 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18517 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18518
18519
18520
18521
18522 strcpy(task,"START");
18523 for (int i=5;i<60;i++) task[i]=' ';
18524
18525
18526
18527 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18528
18529
18530
18531 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18532
18533 if (strncmp(task,"FG",2)==0) {
18534
18535
18536
18537
18538 rot = new EMData();
18539 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
18540 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18541
18542 delete rot;
18543
18544
18545 float dt = 1.0e-3f;
18546 rot = new EMData();
18547 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
18548 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18549
18550 g[0] = (f1-f)/dt;
18551 delete rot;
18552
18553 dt = 1.0e-2f;
18554 rot = new EMData();
18555 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
18556 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18557
18558 g[1] = (f2-f)/dt;
18559 delete rot;
18560
18561 rot = new EMData();
18562 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
18563 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18564
18565 g[2] = (f3-f)/dt;
18566 delete rot;
18567 }
18568
18569
18570 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18571
18572 }
18573
18574
18575 vector<float> res;
18576 res.push_back(static_cast<float>(x[0]));
18577 res.push_back(static_cast<float>(x[1]));
18578 res.push_back(static_cast<float>(x[2]));
18579
18580 return res;
18581 }
18582
18583 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
18584
18585 EMData *rot;
18586
18587 const int nmax=3, mmax=3;
18588 char task[60], csave[60];
18589 long int lsave[4];
18590 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18591 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];
18592 long int SIXTY=60;
18593
18594
18595 iprint = -1;
18596
18597
18598 factr=1.0e1;
18599 pgtol=1.0e-5;
18600
18601
18602
18603
18604 n=3;
18605 m=3;
18606
18607
18608
18609
18610
18611 x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18612 x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18613 x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18614
18615
18616
18617
18618 strcpy(task,"START");
18619 for (int i=5;i<60;i++) task[i]=' ';
18620
18621
18622
18623 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18624
18625
18626
18627 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18628
18629 if (strncmp(task,"FG",2)==0) {
18630
18631
18632
18633
18634 rot = new EMData();
18635 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18636 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18637
18638 delete rot;
18639
18640
18641 float dt = 1.0e-3f;
18642 rot = new EMData();
18643 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18644 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18645
18646 g[0] = (f1-f)/dt;
18647 delete rot;
18648
18649 rot = new EMData();
18650 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
18651 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18652
18653 g[1] = (f2-f)/dt;
18654 delete rot;
18655
18656 rot = new EMData();
18657 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
18658 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18659
18660 g[2] = (f3-f)/dt;
18661 delete rot;
18662 }
18663
18664
18665 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18666
18667 }
18668
18669
18670 vector<float> res;
18671 res.push_back(static_cast<float>(x[0]));
18672 res.push_back(static_cast<float>(x[1]));
18673 res.push_back(static_cast<float>(x[2]));
18674
18675 return res;
18676 }
18677
18678 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) {
18679
18680 EMData *proj, *proj2;
18681
18682 const int nmax=5, mmax=5;
18683 char task[60], csave[60];
18684 long int lsave[4];
18685 long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18686 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];
18687 long int SIXTY=60;
18688
18689
18690 iprint = -1;
18691
18692
18693 factr=1.0e1;
18694 pgtol=1.0e-5;
18695
18696
18697
18698
18699 n=5;
18700 m=5;
18701
18702
18703
18704
18705
18706 x[0] = phi; nbd[0] = 2; l[0] = phi-2.0; u[0] = phi+2.0;
18707 x[1] = theta; nbd[1] = 2; l[1] = theta-2.0; u[1] = theta+2.0;
18708 x[2] = psi; nbd[2] = 2; l[2] = psi-2.0; u[2] = psi+2.0;
18709 x[3] = sxs; nbd[3] = 2; l[3] = sxs-2.0; u[3] = sxs+2.0;
18710 x[4] = sys; nbd[4] = 2; l[4] = sys-2.0; u[4] = sys+2.0;
18711
18712
18713
18714
18715 strcpy(task,"START");
18716 for (int i=5;i<60;i++) task[i]=' ';
18717
18718
18719
18720 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18721 int step = 1;
18722
18723
18724 while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18725
18726 if (strncmp(task,"FG",2)==0) {
18727
18728
18729
18730
18731 proj = new EMData();
18732 proj2 = new EMData();
18733 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18734 proj->fft_shuffle();
18735 proj->center_origin_fft();
18736 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18737 proj->do_ift_inplace();
18738 int M = proj->get_ysize()/2;
18739 proj2 = proj->window_center(M);
18740 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18741
18742 delete proj;
18743 delete proj2;
18744
18745
18746 float dt = 1.0e-3f;
18747 proj = new EMData();
18748 proj2 = new EMData();
18749 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
18750 proj->fft_shuffle();
18751 proj->center_origin_fft();
18752 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18753 proj->do_ift_inplace();
18754 proj2 = proj->window_center(M);
18755 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18756
18757 delete proj;
18758 delete proj2;
18759 g[0] = (ft-f)/dt;
18760
18761 proj = new EMData();
18762 proj2 = new EMData();
18763 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
18764 proj->fft_shuffle();
18765 proj->center_origin_fft();
18766 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18767 proj->do_ift_inplace();
18768 proj2 = proj->window_center(M);
18769 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18770
18771 delete proj;
18772 delete proj2;
18773 g[1] = (ft-f)/dt;
18774
18775 proj = new EMData();
18776 proj2 = new EMData();
18777 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
18778 proj->fft_shuffle();
18779 proj->center_origin_fft();
18780 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18781 proj->do_ift_inplace();
18782 proj2 = proj->window_center(M);
18783 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18784
18785 delete proj;
18786 delete proj2;
18787 g[2] = (ft-f)/dt;
18788
18789 proj = new EMData();
18790 proj2 = new EMData();
18791 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18792 proj->fft_shuffle();
18793 proj->center_origin_fft();
18794 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
18795 proj->do_ift_inplace();
18796 proj2 = proj->window_center(M);
18797 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18798
18799 delete proj;
18800 delete proj2;
18801 g[3] = (ft-f)/dt;
18802
18803 proj = new EMData();
18804 proj2 = new EMData();
18805 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18806 proj->fft_shuffle();
18807 proj->center_origin_fft();
18808 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
18809 proj->do_ift_inplace();
18810 proj2 = proj->window_center(M);
18811 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18812
18813 delete proj;
18814 delete proj2;
18815 g[4] = (ft-f)/dt;
18816 }
18817
18818
18819 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18820 step++;
18821 }
18822
18823
18824 vector<float> res;
18825 res.push_back(static_cast<float>(x[0]));
18826 res.push_back(static_cast<float>(x[1]));
18827 res.push_back(static_cast<float>(x[2]));
18828 res.push_back(static_cast<float>(x[3]));
18829 res.push_back(static_cast<float>(x[4]));
18830
18831 return res;
18832 }
18833
18834
18835 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18836
18837 double x[4];
18838 int n;
18839 int l = 3;
18840 int m = 200;
18841 double e = 1e-9;
18842 double step = 0.01;
18843 float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
18844
18845 x[1] = ang;
18846 x[2] = sxs;
18847 x[3] = sys;
18848
18849 Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);
18850
18851
18852 vector<float> res;
18853 res.push_back(static_cast<float>(x[1]));
18854 res.push_back(static_cast<float>(x[2]));
18855 res.push_back(static_cast<float>(x[3]));
18856 res.push_back(static_cast<float>(n));
18857 return res;
18858 }
18859
18860
18861 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
18862
18863 EMData *rot= new EMData();
18864 float ccc;
18865
18866 rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
18867 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18868 delete rot;
18869 return ccc;
18870 }
18871
18872 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
18873
18874 double x[4];
18875 int n;
18876 int l = 3;
18877 int m = 200;
18878 double e = 1e-9;
18879 double step = 0.001;
18880 float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
18881
18882 x[1] = ang;
18883 x[2] = sxs;
18884 x[3] = sys;
18885
18886 Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);
18887
18888
18889 vector<float> res;
18890 res.push_back(static_cast<float>(x[1]));
18891 res.push_back(static_cast<float>(x[2]));
18892 res.push_back(static_cast<float>(x[3]));
18893 res.push_back(static_cast<float>(n));
18894 return res;
18895 }
18896
18897
18898 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
18899
18900 EMData *rot= new EMData();
18901 float ccc;
18902
18903 rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
18904 ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18905 delete rot;
18906 return ccc;
18907 }
18908
18909 #define img_ptr(i,j,k) img_ptr[i+(j+(k*ny))*nx]
18910 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*nx]
18911 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
18912 {
18913 ENTERFUNC;
18914
18915 if (!img) {
18916 throw NullPointerException("NULL input image");
18917 }
18918
18919 int newx, newy, newz;
18920 bool keep_going;
18921 cout << " entered " <<endl;
18922 int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
18923
18924 EMData * img2 = new EMData();
18925 img2->set_size(nx,ny,nz);
18926 img2->to_zero();
18927 float *img_ptr =img->get_data();
18928 float *img2_ptr = img2->get_data();
18929 int r2 = ro*ro;
18930 int r3 = r2*ro;
18931 int ri2 = ri*ri;
18932 int ri3 = ri2*ri;
18933
18934 int n2 = nx/2;
18935
18936 for (int k=-n2; k<=n2; k++) {
18937 float z2 = static_cast<float>(k*k);
18938 for (int j=-n2; j<=n2; j++) {
18939 float y2 = z2 + j*j;
18940 if(y2 <= r2) {
18941
18942
18943 for (int i=-n2; i<=n2; i++) {
18944 float x2 = y2 + i*i;
18945 if(x2 <= r3) {
18946
18947 int ib = i+n2; int jb = j+n2; int kb = k+n2;
18948 if(x2 >= ri3) {
18949
18950 if(img_ptr(ib,jb,kb) == 1.0f) {
18951
18952 if(Util::get_frand(0.0f, 1.0f) > qprob){
18953 img2_ptr(ib,jb,kb) = 0.0f;
18954 keep_going = true;
18955
18956 while(keep_going) {
18957 newx = Util::get_irand(-ro,ro);
18958 newy = Util::get_irand(-ro,ro);
18959 newz = Util::get_irand(-ro,ro);
18960 if(newx*newx+newy*newy+newz*newz <= r3) {
18961 newx += n2; newy += n2; newz += n2;
18962 if( img_ptr(newx,newy,newz) == 0.0f) {
18963 img2_ptr(newx,newy,newz) = 1.0f;
18964 keep_going = false; }
18965 }
18966 }
18967 } else img2_ptr(ib,jb,kb) = 1.0f;
18968 }
18969 } else {
18970
18971 if(img_ptr(ib,jb,kb) == 1.0) {
18972 if(Util::get_frand(0.0f,1.0f) > qprob) {
18973
18974 float numn = -1.0f;
18975 for (newz = -1; newz <= 1; newz++)
18976 for (newy = -1; newy <= 1; newy++)
18977 for (newx = -1; newx <= 1; newx++)
18978 numn += img_ptr(ib+newx,jb+newy,kb+newz);
18979 img2_ptr(ib,jb,kb) = 0.0;
18980 if(numn == 26.0f) {
18981
18982 keep_going = true;
18983 while(keep_going) {
18984 newx = Util::get_irand(-ro,ro);
18985 newy = Util::get_irand(-ro,ro);
18986 newz = Util::get_irand(-ro,ro);
18987 if(newx*newx+newy*newy+newz*newz < r3) {
18988 newx += n2; newy += n2; newz += n2;
18989 if( img_ptr(newx,newy,newz) == 0.0f) {
18990 if(newx*newx+newy*newy+newz*newz < r3) {
18991 if(newx*newx+newy*newy+newz*newz < r3) {
18992 newx += n2; newy += n2; newz += n2;
18993 if( img_ptr(newx,newy,newz) == 0.0f) {
18994 img2_ptr(newx,newy,newz) = 1.0f;
18995 keep_going = false; }
18996 }
18997 }
18998 }
18999 }
19000 }
19001 } else if(numn == 25.0f) {
19002
19003 for (newz = -1; newz <= 1; newz++) {
19004 for (newy = -1; newy <= 1; newy++) {
19005 for (newx = -1; newx <= 1; newx++) {
19006 if( newx != 0 && newy != 0 && newz != 0) {
19007 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
19008 img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
19009 }
19010 }
19011 }
19012 }
19013 }
19014 } else {
19015
19016 keep_going = true;
19017 while(keep_going) {
19018 newx = Util::get_irand(-1,1);
19019 newy = Util::get_irand(-1,1);
19020 newz = Util::get_irand(-1,1);
19021 if(newx != 0 && newy != 0 && newz != 0) {
19022 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
19023 img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;
19024 keep_going = false;
19025 }
19026 }
19027 }
19028 }
19029 } else img2_ptr(ib,jb,kb) = 1.0f;
19030 }
19031 }
19032 }
19033 }
19034 }
19035 }
19036 }
19037
19038 img2->update();
19039
19040 EXITFUNC;
19041 return img2;
19042 }
19043 #undef img_ptr
19044 #undef img2_ptr
19045
19046 struct point3d_t
19047 {
19048 point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
19049
19050 int x;
19051 int y;
19052 int z;
19053 };
19054
19055
19056 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
19057 {
19058 int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
19059 int noff = 6;
19060
19061 int nx = visited->get_xsize();
19062 int ny = visited->get_ysize();
19063 int nz = visited->get_zsize();
19064
19065 vector< point3d_t > pts;
19066 pts.push_back( point3d_t(ix, iy, iz) );
19067 visited->set_value_at( ix, iy, iz, (float)grpid );
19068
19069 int start = 0;
19070 int end = pts.size();
19071
19072 while( end > start ) {
19073 for(int i=start; i < end; ++i ) {
19074 int ix = pts[i].x;
19075 int iy = pts[i].y;
19076 int iz = pts[i].z;
19077
19078 for( int j=0; j < noff; ++j ) {
19079 int jx = ix + offs[j][0];
19080 int jy = iy + offs[j][1];
19081 int jz = iz + offs[j][2];
19082
19083 if( jx < 0 || jx >= nx ) continue;
19084 if( jy < 0 || jy >= ny ) continue;
19085 if( jz < 0 || jz >= nz ) continue;
19086
19087
19088 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
19089 pts.push_back( point3d_t(jx, jy, jz) );
19090 visited->set_value_at( jx, jy, jz, (float)grpid );
19091 }
19092
19093 }
19094 }
19095
19096 start = end;
19097 end = pts.size();
19098 }
19099 return pts.size();
19100 }
19101
19102
19103 EMData* Util::get_biggest_cluster( EMData* mg )
19104 {
19105 int nx = mg->get_xsize();
19106 int ny = mg->get_ysize();
19107 int nz = mg->get_zsize();
19108
19109 EMData* visited = new EMData();
19110 visited->set_size( nx, ny, nz );
19111 visited->to_zero();
19112 int grpid = 0;
19113 int maxgrp = 0;
19114 int maxsize = 0;
19115 for( int iz=0; iz < nz; ++iz ) {
19116 for( int iy=0; iy < ny; ++iy ) {
19117 for( int ix=0; ix < nx; ++ix ) {
19118 if( (*mg)(ix, iy, iz)==0.0 ) continue;
19119
19120 if( (*visited)(ix, iy, iz) > 0.0 ) {
19121
19122 continue;
19123 }
19124
19125 grpid++;
19126 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
19127 if( grpsize > maxsize ) {
19128 maxsize = grpsize;
19129 maxgrp = grpid;
19130 }
19131 }
19132 }
19133 }
19134
19135 Assert( maxgrp > 0 );
19136
19137 int npoint = 0;
19138 EMData* result = new EMData();
19139 result->set_size( nx, ny, nz );
19140 result->to_zero();
19141
19142 for( int iz=0; iz < nz; ++iz ) {
19143 for( int iy=0; iy < ny; ++iy ) {
19144 for( int ix=0; ix < nx; ++ix ) {
19145 if( (*visited)(ix, iy, iz)==maxgrp ) {
19146 (*result)(ix,iy,iz) = 1.0;
19147 npoint++;
19148 }
19149 }
19150 }
19151 }
19152
19153 Assert( npoint==maxsize );
19154 delete visited;
19155 return result;
19156
19157 }
19158
19159 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)
19160 {
19161 int ix, iy, iz;
19162 int i, j, k;
19163 int nr2, nl2;
19164 float dzz, az, ak;
19165 float scx, scy, scz;
19166 int offset = 2 - nx%2;
19167 int lsm = nx + offset;
19168 EMData* ctf_img1 = new EMData();
19169 ctf_img1->set_size(lsm, ny, nz);
19170 float freq = 1.0f/(2.0f*ps);
19171 scx = 2.0f/float(nx);
19172 if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
19173 if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
19174 nr2 = ny/2 ;
19175 nl2 = nz/2 ;
19176 for ( k=0; k<nz;k++) {
19177 iz = k; if(k>nl2) iz=k-nz;
19178 for ( j=0; j<ny;j++) {
19179 iy = j; if(j>nr2) iy=j - ny;
19180 for ( i=0; i<lsm/2; i++) {
19181 ix=i;
19182 ak=pow(ix*ix*scx*scx+iy*scy*iy*scy+iz*scz*iz*scz, 0.5f)*freq;
19183 if(ak!=0) az=0.0; else az=M_PI;
19184 dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f));
19185 (*ctf_img1) (i*2,j,k) = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
19186 (*ctf_img1) (i*2+1,j,k) = 0.0f;
19187 }
19188 }
19189 }
19190 ctf_img1->update();
19191 ctf_img1->set_complex(true);
19192 ctf_img1->set_ri(true);
19193
19194
19195 if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
19196 return ctf_img1;
19197 }
19198
19199
19200
19201
19202
19203
19204
19205
19206
19207
19208
19209
19210
19211
19212
19213
19214
19215
19216
19217
19218
19219
19220
19221
19222
19223
19224
19225
19226
19227
19228
19229
19230
19231
19232
19233
19234
19235
19236
19237
19238
19239
19240
19241
19242
19243
19244
19245
19246
19247
19248
19249
19250
19251
19252
19253
19254
19255
19256
19257
19258
19259
19260
19261
19262
19263
19264
19265
19266
19267
19268
19269
19270
19271
19272
19273
19274
19275
19276
19277
19278
19279
19280
19281
19282
19283
19284
19285 #define cent(i) out[i+N]
19286 #define assign(i) out[i]
19287 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
19288 int nx = d->get_xsize();
19289 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19290 vector<float> out(N+K+2);
19291 if(N*(N-1)/2 != nx) {
19292
19293 return out;}
19294
19295 for(int i=0; i<N; i++) assign(i) = float(i);
19296
19297 for(int i=0; i<N; i++) {
19298 int j = Util::get_irand(0,N-1);
19299 float temp = assign(i);
19300 assign(i) = assign(j);
19301 assign(j) = temp;
19302 }
19303 for(int k=0; k<K; k++) cent(k) = float(assign(k));
19304
19305
19306 for(int i=0; i<N; i++) assign(i) = 0.0f;
19307 float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
19308 bool change = true;
19309 int it = -1;
19310 int ct = -1;
19311 while(change && disp < dispold || ct > 0) {
19312
19313 change = false;
19314 dispold = disp;
19315 it++;
19316
19317
19318 disp = 0.0f;
19319 ct = 0;
19320 for(int i=0; i<N; i++) {
19321 qm = 1.0e23f;
19322 for(int k=0; k<K; k++) {
19323 if(float(i) == cent(k)) {
19324 qm = 0.0f;
19325 na = (float)k;
19326 } else {
19327 float dt = (*d)(mono(i,int(cent(k))));
19328 if(dt < qm) {
19329 qm = dt;
19330 na = (float)k;
19331 }
19332 }
19333 }
19334
19335
19336
19337 if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
19338 na = (float)(Util::get_irand(0, K));
19339 qm = (*d)(mono(i,int(na)));
19340 ct++;
19341 }
19342
19343 disp += qm;
19344
19345 if(na != assign(i)) {
19346 assign(i) = na;
19347 change = true;
19348 }
19349 }
19350
19351
19352 T = T*F;
19353
19354
19355
19356
19357
19358 for(int k=0; k<K; k++) {
19359 qm = 1.0e23f;
19360 for(int i=0; i<N; i++) {
19361 if(assign(i) == float(k)) {
19362 float q = 0.0;
19363 for(int j=0; j<N; j++) {
19364 if(assign(j) == float(k)) {
19365
19366 if(i != j) q += (*d)(mono(i,j));
19367
19368 }
19369 }
19370 if(q < qm) {
19371
19372 qm = q;
19373 cent(k) = float(i);
19374 }
19375 }
19376 }
19377 }
19378
19379 }
19380 out[N+K] = disp;
19381 out[N+K+1] = float(it);
19382 return out;
19383 }
19384 #undef cent
19385 #undef assign
19386
19387
19388
19389
19390
19391
19392
19393
19394
19395
19396
19397
19398
19399
19400
19401
19402
19403
19404
19405
19406
19407
19408
19409
19410
19411
19412
19413
19414
19415
19416
19417
19418
19419
19420
19421
19422
19423
19424
19425
19426
19427
19428
19429
19430
19431
19432
19433
19434
19435
19436
19437
19438
19439
19440
19441
19442
19443
19444
19445
19446
19447
19448
19449
19450
19451
19452
19453
19454
19455
19456
19457
19458
19459
19460
19461
19462
19463
19464
19465
19466
19467
19468
19469
19470
19471
19472
19473
19474
19475
19476
19477
19478
19479
19480
19481
19482
19483
19484
19485
19486
19487
19488
19489
19490
19491
19492
19493
19494
19495 vector<float> Util::cluster_equalsize(EMData* d) {
19496
19497 int nx = d->get_xsize();
19498 int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19499 int K = N/2;
19500 vector<float> group(N);
19501 if(N*(N-1)/2 != nx) {
19502
19503 return group;}
19504
19505 bool * active = new bool[N];
19506 for(int i=0; i<N; i++) active[i] = true;
19507
19508 float dm, qd;
19509 int ppi = 0, ppj = 0;
19510 for(int k=0; k<K; k++) {
19511
19512
19513 dm = 1.0e23f;
19514 for(int i=1; i<N; i++) {
19515 if(active[i]) {
19516 for(int j=0; j<i; j++) {
19517 if(active[j]) {
19518 qd = (*d)(i*(i - 1)/2 + j);
19519 if(qd < dm) {
19520 dm = qd;
19521 ppi = i;
19522 ppj = j;
19523 }
19524 }
19525 }
19526 }
19527 }
19528 group[2*k] = float(ppi);
19529 group[1+2*k] = float(ppj);
19530 active[ppi] = false;
19531 active[ppj] = false;
19532 }
19533
19534 delete [] active;
19535 active = NULL;
19536 return group;
19537 }
19538
19539
19540
19541
19542
19543
19544
19545
19546
19547
19548
19549
19550
19551
19552
19553
19554
19555
19556
19557
19558
19559
19560
19561
19562
19563
19564
19565
19566
19567
19568
19569
19570
19571
19572
19573
19574
19575
19576
19577
19578
19579
19580
19581
19582
19583
19584
19585
19586
19587
19588
19589
19590
19591
19592 #define data(i,j) group[i*ny+j]
19593 vector<float> Util::vareas(EMData* d) {
19594 const float step=0.001f;
19595 int ny = d->get_ysize();
19596
19597
19598
19599 vector<float> group(2*ny);
19600 for(int i=0; i<2*ny; i++) group[i] = 0.0f;
19601 int K = int(1.0f/step) +1;
19602 int hit = 0;
19603 for(int kx=0; kx<=K; kx++) {
19604 float tx = kx*step;
19605 for(int ky=0; ky<=K; ky++) {
19606 float ty = ky*step;
19607 float dm = 1.0e23f;
19608 for(int i=0; i<ny; i++) {
19609 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
19610 if( qd < dm) {
19611 dm = qd;
19612 hit = i;
19613 }
19614 }
19615 data(0,hit) += 1.0f;
19616 if(kx == 0 || ky == 0 || kx == K || ky == K) data(1,hit) = 1.0f;
19617 }
19618 }
19619 return group;
19620 }
19621 #undef data
19622
19623 EMData* Util::get_slice(EMData *vol, int dim, int index) {
19624
19625 int nx = vol->get_xsize();
19626 int ny = vol->get_ysize();
19627 int nz = vol->get_zsize();
19628 float *vol_data = vol->get_data();
19629 int new_nx, new_ny;
19630
19631 if (nz == 1)
19632 throw ImageDimensionException("Error: Input must be a 3-D object");
19633 if ((dim < 1) || (dim > 3))
19634 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
19635 if (((dim == 1) && (index < 0 || index > nx-1)) ||
19636 ((dim == 1) && (index < 0 || index > nx-1)) ||
19637 ((dim == 1) && (index < 0 || index > nx-1)))
19638 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
19639
19640 if (dim == 1) {
19641 new_nx = ny;
19642 new_ny = nz;
19643 } else if (dim == 2) {
19644 new_nx = nx;
19645 new_ny = nz;
19646 } else {
19647 new_nx = nx;
19648 new_ny = ny;
19649 }
19650
19651 EMData *slice = new EMData();
19652 slice->set_size(new_nx, new_ny, 1);
19653 float *slice_data = slice->get_data();
19654
19655 if (dim == 1) {
19656 for (int x=0; x<new_nx; x++)
19657 for (int y=0; y<new_ny; y++)
19658 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
19659 } else if (dim == 2) {
19660 for (int x=0; x<new_nx; x++)
19661 for (int y=0; y<new_ny; y++)
19662 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
19663 } else {
19664 for (int x=0; x<new_nx; x++)
19665 for (int y=0; y<new_ny; y++)
19666 slice_data[y*new_nx+x] = vol_data[(index*ny+y)*nx+x];
19667 }
19668
19669 return slice;
19670 }
19671
19672 void Util::image_mutation(EMData *img, float mutation_rate) {
19673 int nx = img->get_xsize();
19674 float min = img->get_attr("minimum");
19675 float max = img->get_attr("maximum");
19676 float* img_data = img->get_data();
19677 array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
19678 return;
19679 }
19680
19681
19682 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
19683
19684 if (is_mirror != 0) {
19685 for (int i=0; i<len_list; i++) {
19686 int r = rand()%10000;
19687 float f = r/10000.0f;
19688 if (f < mutation_rate) list[i] = 1-list[i];
19689 }
19690 } else {
19691 map<int, vector<int> > graycode;
19692 map<vector<int>, int> rev_graycode;
19693 vector <int> gray;
19694
19695 int K=1;
19696 for (int i=0; i<L; i++) K*=2;
19697
19698 for (int k=0; k<K; k++) {
19699 int shift = 0;
19700 vector <int> gray;
19701 for (int i=L-1; i>-1; i--) {
19702 int t = ((k>>i)%2-shift)%2;
19703 gray.push_back(t);
19704 shift += t-2;
19705 }
19706 graycode[k] = gray;
19707 rev_graycode[gray] = k;
19708 }
19709
19710 float gap = (K-1)/(max_val-min_val);
19711 for (int i=0; i<len_list; i++) {
19712 float val = list[i];
19713 if (val < min_val) { val = min_val; }
19714 else if (val > max_val) { val = max_val; }
19715 int k = int((val-min_val)*gap+0.5);
19716 vector<int> gray = graycode[k];
19717 bool changed = false;
19718 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
19719 int r = rand()%10000;
19720 float f = r/10000.0f;
19721 if (f < mutation_rate) {
19722 *p = 1-*p;
19723 changed = true;
19724 }
19725 }
19726 if (changed) {
19727 k = rev_graycode[gray];
19728 list[i] = k/gap+min_val;
19729 }
19730 }
19731 }
19732
19733 }
19734
19735 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
19736
19737 if (is_mirror != 0) {
19738 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
19739 int r = rand()%10000;
19740 float f = r/10000.0f;
19741 if (f < mutation_rate) *q = 1-*q;
19742 }
19743 } else {
19744 map<int, vector<int> > graycode;
19745 map<vector<int>, int> rev_graycode;
19746 vector <int> gray;
19747
19748 int K=1;
19749 for (int i=0; i<L; i++) K*=2;
19750
19751 for (int k=0; k<K; k++) {
19752 int shift = 0;
19753 vector <int> gray;
19754 for (int i=L-1; i>-1; i--) {
19755 int t = ((k>>i)%2-shift)%2;
19756 gray.push_back(t);
19757 shift += t-2;
19758 }
19759 graycode[k] = gray;
19760 rev_graycode[gray] = k;
19761 }
19762
19763 float gap = (K-1)/(max_val-min_val);
19764 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
19765 float val = *q;
19766 if (val < min_val) { val = min_val; }
19767 else if (val > max_val) { val = max_val; }
19768 int k = int((val-min_val)*gap+0.5);
19769 vector<int> gray = graycode[k];
19770 bool changed = false;
19771 for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
19772 int r = rand()%10000;
19773 float f = r/10000.0f;
19774 if (f < mutation_rate) {
19775 *p = 1-*p;
19776 changed = true;
19777 }
19778 }
19779 if (changed) {
19780 k = rev_graycode[gray];
19781 *q = k/gap+min_val;
19782 }
19783 }
19784 }
19785 return list;
19786 }
19787
19788
19789 void Util::bb_enumerate_(int* argParts, int* dimClasses, int nParts, int K, int T,int n_guesses, int* Levels) {
19790
19791 int* Indices = new int[nParts*K];
19792
19793 vector <vector <int*> > Parts(nParts,vector<int*>(K));
19794 int ind_c = 0;
19795
19796 for (int i=0; i < nParts; i++){
19797 for(int j = 0; j < K; j++){
19798 Parts[i][j]=argParts + ind_c;
19799 Indices[i*K + j] = ind_c;
19800 ind_c = ind_c + *(dimClasses+i*K + j);
19801
19802 }
19803 }
19804
19805
19806
19807 vector <vector <int*> > oldParts = Parts;
19808
19809
19810
19811
19812 Util::initial_prune(Parts, dimClasses, nParts, K,T);
19813
19814
19815
19816 unsigned int numLevels = Parts[0].size();
19817 for (int i=1; i < nParts; i++){
19818 if (Parts[i].size() < numLevels) numLevels = Parts[i].size();
19819 }
19820
19821
19822
19823
19824
19825
19826
19827
19828
19829
19830
19831
19832
19833
19834
19835 for(int i = 0; i < nParts; i++){
19836 for(int j=0; j < K; j++){
19837 *(argParts + Indices[i*K + j]+1) = -1;
19838 }
19839 }
19840
19841 int num_classes;
19842 int old_index;
19843 for(int i=0; i<nParts; i++){
19844 num_classes = Parts[i].size();
19845
19846 for (int j=0; j < num_classes; j++){
19847 old_index = *(Parts[i][j]);
19848
19849 *(argParts + Indices[i*K + old_index]+1) = 1;
19850 }
19851 }
19852
19853
19854
19855
19856 cout <<"begin partition matching\n";
19857 int* output = Util::branch(argParts, Indices,dimClasses, nParts, K, T,Levels, numLevels,0,n_guesses);
19858 cout <<"done with partition matching \n";
19859 cout<<"total cost: "<<*output<<"\n";
19860 cout<<"number of matches: "<<*(output+1)<<"\n";
19861
19862
19863 }
19864
19865 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
19866
19867 int total_cost = *output;
19868 int num_matches = *(output+1);
19869
19870 int cost=0;
19871 int* intx;
19872 int intx_size;
19873 int* intx_next(0);
19874 int intx_next_size = 0;
19875 int curclass;
19876 int curclass_size;
19877
19878 for(int i = 0; i < num_matches; i++){
19879 curclass = *(output+2+ i*nParts);
19880
19881 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
19882 *(argParts + Indices[curclass]+1) = -5;
19883
19884 curclass_size = *(dimClasses+curclass)-2;
19885 intx = new int[curclass_size];
19886 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
19887 intx_size = curclass_size;
19888
19889 for (int j=1; j < nParts; j++){
19890 curclass = *(output+2+ i*nParts+j);
19891 if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
19892 *(argParts + Indices[j*K+curclass]+1)=-5;
19893
19894 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);
19895 intx_next = new int[intx_next_size];
19896 Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
19897 delete[] intx;
19898 intx=intx_next;
19899 intx_size= intx_next_size;
19900 if (j==nParts-1) delete[] intx_next;
19901 }
19902
19903 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
19904
19905 cost = cost + intx_next_size;
19906 }
19907
19908 if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
19909
19910 return 1;
19911
19912 }
19913
19914 int* Util::branch(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int curlevel,int n_guesses) {
19915
19916
19917 if (curlevel > nLevels-1){
19918 int* res = new int[2];
19919 *res = 0;
19920 *(res+1)=0;
19921 return res;
19922 }
19923
19924
19925 int nBranches = *(Levels + curlevel);
19926
19927
19928
19929
19930 int* matchlist = new int[nBranches*nParts];
19931 int* costlist = new int[nBranches];
19932
19933
19934
19935 for (int i=0; i < nBranches; i++) *(costlist+i)=0;
19936
19937
19938
19939
19940 Util::findTopLargest(argParts,Indices, dimClasses, nParts, K, T, matchlist, nBranches,costlist,n_guesses);
19941
19942
19943 if (costlist[0]<= T){
19944 int* res = new int[2];
19945 *res = 0;
19946 *(res+1)=0;
19947 return res;
19948 }
19949
19950 int* maxreturn = new int[2];
19951 *maxreturn=0;
19952 *(maxreturn+1)=0;
19953
19954
19955 int old_index;
19956 int totalcost;
19957 int nmatches;
19958
19959
19960 for(int i=0; i < nBranches ; i++){
19961
19962
19963
19964 if (costlist[i] <= T) break;
19965
19966
19967
19968
19969 for(int j=0; j < nParts; j++){
19970
19971 old_index=matchlist[i*nParts + j];
19972 *(argParts + Indices[j*K+old_index] + 1) = -2;
19973 }
19974
19975
19976 int* ret = Util::branch(argParts, Indices, dimClasses, nParts, K, T, Levels, nLevels, curlevel+1,n_guesses);
19977
19978
19979 totalcost = costlist[i] + *ret;
19980
19981
19982
19983
19984
19985 bool debug1 = 0;
19986 if (debug1){
19987 int maxLevel = 2;
19988 if (curlevel < maxLevel) cout<<"total cost level" << curlevel<<": "<<totalcost<<"\n";
19989 }
19990
19991
19992 bool debug2 = 0;
19993 if (debug2){
19994 int skip1 = 5;
19995 int max1=20;
19996 if ((curlevel <= max1) && (curlevel%skip1 == 0)) cout << "total cost level "<< curlevel<<": "<<totalcost<<"\n";
19997
19998 int skip2 = 10;
19999 int max2 = 70;
20000 if ((curlevel > max1 )&&(curlevel <= max2) && (curlevel%skip2 == 0)) cout << "total cost level "<< curlevel<<": "<<totalcost<<"\n";
20001 }
20002
20003
20004
20005
20006
20007
20008
20009
20010
20011
20012
20013 if (totalcost > *maxreturn)
20014
20015
20016
20017 {
20018 nmatches = 1 + *(ret+1);
20019 delete[] maxreturn;
20020 maxreturn = new int[2+nmatches*nParts];
20021 *maxreturn = totalcost;
20022 *(maxreturn + 1)= nmatches;
20023 int nret = 2+(nmatches-1)*nParts;
20024 for(int iret=2; iret <nret;iret++) *(maxreturn+iret)=*(ret+iret);
20025 for(int imax=0; imax<nParts;imax++) *(maxreturn+nret+imax)=matchlist[i*nParts + imax];
20026 }
20027
20028
20029 delete[] ret;
20030
20031
20032
20033 for(int j=0; j < nParts; j++){
20034
20035 old_index=matchlist[i*nParts + j];
20036 *(argParts + Indices[j*K+old_index] + 1) = 1;
20037 }
20038
20039 }
20040
20041 delete[] matchlist;
20042 delete[] costlist;
20043 return maxreturn;
20044
20045 }
20046
20047 int Util::findTopLargest(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int max_num_matches, int* costlist, int n_guesses){
20048 int guess;
20049 int* curmax = new int[nParts+1];
20050 int newT=T;
20051 int num_found=0;
20052
20053 for(int i=0; i < max_num_matches; i++){
20054 guess = Util::generatesubmax(argParts, Indices,dimClasses,nParts, K, T, n_guesses);
20055
20056 if (T < guess) newT = guess -1;
20057
20058 Util::search2(argParts, Indices,dimClasses,nParts, K, newT,curmax);
20059 if (*curmax <= T){
20060 max_num_matches=i;
20061 break;
20062 }
20063 else {
20064 *(costlist+i) = *curmax;
20065
20066 for (int j=0; j<nParts; j++){
20067 *(matchlist+i*nParts+j) = *(curmax+1+j);
20068 *(argParts + Indices[j*K+*(curmax+1+j)] + 1) = -3;
20069
20070 }
20071 num_found = num_found+1;
20072 }
20073
20074 }
20075
20076
20077 delete[] curmax;
20078
20079
20080 for (int i=0 ; i < max_num_matches; i++){
20081 for (int j = 0; j < nParts; j++){
20082 *(argParts + Indices[j*K+*(matchlist+i*nParts +j)] + 1) = 1;
20083 }
20084
20085 }
20086
20087
20088 return num_found;
20089 }
20090
20091
20092 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int newT, int* curmax){
20093
20094 *curmax= 0;
20095
20096 bool flag = 0;
20097 int nintx;
20098 int* dummy(0);
20099 int* ret;
20100
20101 for(int a=0; a<K; a++)
20102 {
20103
20104 if (*(argParts + Indices[a] + 1) < 1) continue;
20105 if (*(dimClasses + a)-2 <= newT) continue;
20106
20107
20108
20109 for( int i=1; i < nParts; i++){
20110 flag = 0;
20111 for(int j=0; j < K; j++){
20112 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
20113 if (*(dimClasses + i*K+j)-2 <= newT) {*(argParts + Indices[i*K+j] + 1) =-4; continue;}
20114 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);
20115 if (nintx > newT) flag=1;
20116 else *(argParts + Indices[i*K+j] + 1) =-4;
20117 }
20118 if (flag==0) {break;}
20119 }
20120
20121
20122 if (flag > 0){
20123 ret=Util::explore2(argParts, Indices, dimClasses, nParts, K, newT, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2, *(dimClasses+a)-2,0);
20124
20125 if (*ret > *curmax){
20126 *curmax = *ret;
20127 *(curmax+1)=a;
20128 for (int cp =0; cp < nParts-1; cp++) *(curmax+2+cp) = *(ret+1+cp);
20129
20130 }
20131 delete[] ret;
20132 }
20133
20134 for( int i=1; i < nParts; i++){
20135 for(int j=0; j < K; j++){
20136 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
20137
20138 }
20139 }
20140
20141
20142
20143 }
20144
20145
20146 }
20147
20148
20149 int* Util::explore2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int newT, int* curintx, int size_curintx, int* next, int size_next, int depth){
20150
20151 int* curintx2(0);
20152
20153 int nintx = size_curintx;
20154
20155
20156 if (depth >0){
20157 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
20158 if (nintx <= newT) {curintx2 = new int[1]; *curintx2=0;return curintx2;}
20159 }
20160
20161
20162 if (depth == (nParts-1)) { curintx2 = new int[1]; *curintx2 = nintx; return curintx2;}
20163
20164
20165
20166
20167 if (depth > 0){
20168 curintx2 = new int[nintx];
20169 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
20170 }
20171
20172 if (depth == 0){
20173
20174 curintx2 = new int[size_curintx];
20175 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
20176 }
20177
20178
20179
20180 depth=depth+1;
20181 int* curmax = new int[nParts-depth+1];
20182 *curmax=0;
20183 int* ret;
20184
20185 for (int i=0; i < K; i++){
20186
20187 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue;
20188 size_next = (*(dimClasses + depth*K+i ))-2;
20189 if (size_next <= newT) continue;
20190 ret = Util::explore2(argParts,Indices, dimClasses, nParts, K, newT, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth);
20191 if (*ret > *curmax && *ret > newT){
20192 *curmax = *ret;
20193 *(curmax+1)=i;
20194 for (int j=0; j<nParts-depth-1; j++) { *(curmax+2 + j) = *(ret+1+j);}
20195 }
20196 delete[] ret;
20197 }
20198
20199 delete[] curintx2;
20200 return curmax;
20201 }
20202
20203
20204 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
20205
20206
20207
20208
20209
20210
20211
20212
20213
20214
20215 int* dummy(0);
20216 int* cref;
20217 int cref_size;
20218 int* ccomp;
20219 int ccomp_size;
20220 int nintx;
20221 for (int i=0; i < nParts; i++){
20222 for (int j =0; j < K; j++){
20223
20224
20225 cref = Parts[i][j];
20226 cref_size = (*(dimClasses+i*K+(*cref)))-2;
20227
20228
20229 if (cref_size <= T){
20230
20231 *cref = -1;
20232 continue;
20233 }
20234 bool done = 0;
20235 for (int a = 0; a < nParts; a++){
20236 if (a == i) continue;
20237 bool hasActive=0;
20238 for (unsigned int b=0; b < Parts[a].size(); b++){
20239
20240
20241 ccomp = Parts[a][b];
20242 ccomp_size= (*(dimClasses+a*K+(*ccomp)))-2;
20243 nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
20244
20245
20246 if (nintx <= T)
20247 *(ccomp+1) = 0;
20248 else{
20249 *(ccomp+1)=1;
20250 hasActive=1;
20251 }
20252 }
20253
20254 if (hasActive < 1){
20255 done=1;
20256 break;
20257 }
20258
20259 }
20260
20261 if (done > 0){
20262
20263
20264 *cref = -1;
20265 continue;
20266 }
20267
20268
20269
20270
20271
20272
20273
20274
20275
20276
20277 bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
20278
20279 if (found<1){
20280
20281 *cref = -1;
20282 }
20283 }
20284
20285
20286
20287 for (int d = K-1; d > -1; d--){
20288 if (*(Parts[i][d]) < 0) Parts[i].erase(Parts[i].begin()+d);
20289 }
20290
20291 }
20292
20293
20294
20295
20296
20297 }
20298
20299
20300 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) {
20301
20302
20303 if (size_next <= T) return 0;
20304
20305
20306 int* curintx2(0);
20307 int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
20308 if (nintx <= T) return 0;
20309
20310 int old_depth=depth;
20311 if (depth == partref) depth = depth + 1;
20312 if (depth == (nParts)) { if (old_depth>0) return 1;}
20313
20314
20315
20316 curintx2 = new int[nintx];
20317 Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
20318
20319
20320
20321
20322 bool gt_thresh;
20323 int num_classes = (Parts[depth]).size();
20324
20325 for (int i=0; i < num_classes; i++){
20326 if (*(Parts[depth][i]+1) < 1) continue;
20327 size_next = (*(dimClasses + depth*K+(*(Parts[depth][i])) ))-2;
20328 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
20329 if (gt_thresh) return 1;
20330 }
20331 delete[] curintx2;
20332 return 0;
20333 }
20334
20335
20336 int Util::generatesubmax(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int n_guesses){
20337 int guess=0;
20338
20339 int* perm = new int[nParts];
20340 for(int i=0; i<nParts; i++) perm[i]=i;
20341
20342
20343 int* intx(0);
20344 int* intx_next(0);
20345 int nintx;
20346 int nintxmax=0;
20347 int class_max = 0, class_max_next = 0;
20348 int intx_size = 0, part, part_next;
20349 int ipold,indsw;
20350
20351 for(int i=0; i< n_guesses; i++){
20352
20353 for(int ip = 0; ip<nParts; ip++){
20354 indsw = (rand())%nParts;
20355
20356 ipold = perm[ip];
20357 perm[ip]=perm[indsw];
20358 perm[indsw]=ipold;
20359 }
20360
20361
20362
20363 part=*perm;
20364 part_next=*(perm+1);
20365 for (int a=0; a < K; a++)
20366 {
20367 if (*(argParts + Indices[part*K+a] + 1) < 1) continue;
20368 for (int b=0; b < K; b++)
20369 {
20370 if (*(argParts + Indices[part_next*K + b] + 1) < 1) continue;
20371 nintx = Util::k_means_cont_table_(argParts + Indices[part*K+a]+2,argParts + Indices[part_next*K + b]+2, intx, *(dimClasses + part*K+a)-2, *(dimClasses + part_next*K + b)-2,0);
20372 if (nintx <= nintxmax) continue;
20373 nintxmax = nintx;
20374 class_max = a;
20375 class_max_next = b;
20376 }
20377 }
20378
20379
20380 if (nintxmax < 1) {continue;}
20381
20382 if (nParts > 2){
20383 intx = new int[nintxmax];
20384 intx_size = nintxmax;
20385 Util::k_means_cont_table_(argParts + Indices[part*K+class_max]+2,argParts + Indices[part_next*K + class_max_next]+2, intx, *(dimClasses + part*K+class_max)-2, *(dimClasses+part_next*K+class_max_next)-2,1);
20386 }
20387
20388
20389 for(int j = 2; j < nParts; j++){
20390 part = *(perm+j);
20391 nintxmax=0;
20392 for(int a = 0; a < K; a++){
20393 if (*(argParts + Indices[part*K+a] + 1) < 1) continue;
20394 nintx = Util::k_means_cont_table_(intx, argParts + Indices[part*K + a]+2, intx_next, intx_size, (*(dimClasses + part*K+a))-2,0);
20395 if (nintx <= nintxmax) continue;
20396 nintxmax = nintx;
20397 class_max = a;
20398 }
20399
20400
20401 if (nintxmax < 1) {
20402
20403 delete[] intx;
20404 break;
20405 }
20406
20407
20408 intx_next = new int[nintxmax];
20409 Util::k_means_cont_table_(intx, argParts + Indices[part*K + class_max]+2, intx_next, intx_size, *(dimClasses + part*K+class_max)-2,1);
20410 delete[] intx;
20411 intx = intx_next;
20412 intx_size = nintxmax;
20413 if (j==nParts - 1) delete[] intx_next;
20414
20415 }
20416
20417 if (nintxmax > guess) guess = nintxmax;
20418
20419 }
20420 delete[] perm;
20421 return guess;
20422 }
20423
20424
20425 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int nTop, int n_guesses, bool doMPI, int* Levels) {
20426
20427
20428
20429
20430 int* Indices = new int[nParts*K];
20431 int ind_c = 0;
20432 for (int i=0; i < nParts; i++){
20433 for(int j = 0; j < K; j++){
20434 Indices[i*K + j] = ind_c;
20435 ind_c = ind_c + *(dimClasses+i*K + j);
20436
20437 }
20438 }
20439
20440
20441 if (nTop > 0 && doMPI > 0){
20442
20443 int* matchlist = new int[nTop*nParts];
20444 int* costlist=new int[nTop];
20445 for (int i=0; i< nTop; i++) {*(costlist+i) = 0;}
20446 int matchesFound = Util::findTopLargest(argParts,Indices, dimClasses, nParts, K, T, matchlist, nTop,costlist,n_guesses);
20447 vector<int> ret(nTop*(nParts+1) + 1);
20448 ret[0] = matchesFound;
20449 int m = nParts + 1;
20450
20451 for(int i=0; i < nTop; i++){
20452 ret[1+i*m] = *(costlist+i);
20453 for (int j=0; j < nParts; j++){
20454 ret[1+i*m + 1 + j] = matchlist[i*nParts + j];
20455 }
20456 }
20457
20458 return ret;
20459
20460 }
20461
20462
20463
20464
20465 vector <vector <int*> > Parts(nParts,vector<int*>(K));
20466 ind_c = 0;
20467 int argParts_size=0;
20468 for (int i=0; i < nParts; i++){
20469 for(int j = 0; j < K; j++){
20470 Parts[i][j]=argParts + ind_c;
20471 ind_c = ind_c + *(dimClasses+i*K + j);
20472 argParts_size = argParts_size + *(dimClasses+i*K + j);
20473
20474 }
20475 }
20476
20477
20478
20479
20480
20481 Util::initial_prune(Parts, dimClasses, nParts, K,T);
20482 for(int i = 0; i < nParts; i++){
20483 for(int j=0; j < K; j++){
20484 *(argParts + Indices[i*K + j]+1) = -1;
20485 }
20486 }
20487
20488 int num_classes;
20489 int old_index;
20490 for(int i=0; i<nParts; i++){
20491 num_classes = Parts[i].size();
20492 for (int j=0; j < num_classes; j++){
20493 old_index = *(Parts[i][j]);
20494
20495 *(argParts + Indices[i*K + old_index]+1) = 1;
20496 }
20497 }
20498
20499
20500 if (doMPI > 0){
20501
20502 vector<int> ret(argParts_size);
20503 for(int i=0; i < argParts_size; i++)
20504 ret[i]= (*(argParts+i));
20505
20506 return ret;
20507 }
20508
20509
20510
20511 int* dummy(0);
20512 int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T,Levels, K,0,n_guesses,-1, dummy);
20513
20514
20515
20516
20517 bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
20518
20519
20520 if (correct < 1){
20521 cout << "something is wrong with output of branchMPI!\n";
20522 vector<int> ret(1);
20523 ret[0]=-1;
20524 return ret;
20525 }
20526
20527
20528
20529
20530
20531 int output_size = 2+ *(output+1) * nParts;
20532 vector<int> ret(output_size);
20533 for (int i = 0; i < output_size; i++){
20534 ret[i]=*(output+i);
20535 }
20536 return ret;
20537
20538 }
20539
20540 vector<int> Util::branchMPIpy_(int* argParts, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int n_guesses, int nFirst, int* firstmatches){
20541
20542
20543
20544 int num_active;
20545 int* Indices = new int[nParts*K];
20546
20547 int ind_c = 0;
20548 for (int i=0; i < nParts; i++){
20549 num_active = 0;
20550 for(int j = 0; j < K; j++){
20551 Indices[i*K + j] = ind_c;
20552 if (*(argParts+ind_c + 1) == 1) num_active = num_active + 1;
20553 ind_c = ind_c + *(dimClasses+i*K + j);
20554 }
20555
20556 if (num_active < nLevels) {nLevels = num_active;}
20557 }
20558
20559
20560
20561
20562
20563
20564
20565 int* output = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, Levels, nLevels, 0,n_guesses, nFirst,firstmatches);
20566
20567
20568
20569
20570
20571 bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
20572
20573
20574 if (correct < 1){
20575 cout << "something is wrong with output of branchMPI!\n";
20576 vector<int> ret(1);
20577 ret[0]=-1;
20578 return ret;
20579 }
20580
20581
20582
20583
20584
20585 int output_size = 2+ *(output+1) * nParts;
20586 vector<int> ret(output_size);
20587 for (int i = 0; i < output_size; i++){
20588 ret[i]=*(output+i);
20589 }
20590 return ret;
20591 }
20592
20593
20594 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int curlevel,int n_guesses, int nFirst, int* firstmatches) {
20595
20596
20597 if (curlevel > nLevels-1){
20598 int* res = new int[2];
20599 *res = 0;
20600 *(res+1)=0;
20601 return res;
20602 }
20603
20604
20605
20606 int nBranches = *(Levels + curlevel);
20607
20608
20609 if (curlevel==0 && nFirst > 0)
20610 {
20611 nBranches = nFirst;
20612 }
20613
20614
20615
20616 int* matchlist = new int[nBranches*nParts];
20617 int* costlist = new int[nBranches];
20618
20619
20620 for (int i=0; i < nBranches; i++)
20621 *(costlist+i)=0;
20622
20623
20624
20625
20626
20627 if (curlevel == 0 && nFirst > 0){
20628 for(int i = 0; i < nBranches; i++){
20629 *(costlist+i) = *(firstmatches +i*(nParts+1));
20630 for (int j=0; j< nParts; j++)
20631 *(matchlist + i*nParts +j) = *(firstmatches +i*(nParts+1) + 1 + j);
20632 }
20633 }
20634 else
20635 Util::findTopLargest(argParts,Indices, dimClasses, nParts, K, T, matchlist, nBranches,costlist,n_guesses);
20636
20637
20638 if (costlist[0]<= T){
20639 int* res = new int[2];
20640 *res = 0;
20641 *(res+1)=0;
20642 return res;
20643 }
20644
20645 int* maxreturn = new int[2];
20646 *maxreturn=0;
20647 *(maxreturn+1)=0;
20648
20649
20650 int old_index;
20651 int totalcost;
20652 int nmatches;
20653
20654
20655 for(int i=0; i < nBranches ; i++){
20656
20657
20658
20659 if (costlist[i] <= T) break;
20660
20661
20662
20663
20664 for(int j=0; j < nParts; j++){
20665
20666 old_index=matchlist[i*nParts + j];
20667 *(argParts + Indices[j*K+old_index] + 1) = -2;
20668 }
20669
20670
20671 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, Levels, nLevels, curlevel+1,n_guesses, nFirst, firstmatches);
20672
20673
20674 totalcost = costlist[i] + *ret;
20675
20676
20677 if (totalcost > *maxreturn)
20678 {
20679 nmatches = 1 + *(ret+1);
20680 delete[] maxreturn;
20681 maxreturn = new int[2+nmatches*nParts];
20682 *maxreturn = totalcost;
20683 *(maxreturn + 1)= nmatches;
20684 int nret = 2+(nmatches-1)*nParts;
20685 for(int iret=2; iret <nret;iret++) *(maxreturn+iret)=*(ret+iret);
20686 for(int imax=0; imax<nParts;imax++) *(maxreturn+nret+imax)=matchlist[i*nParts + imax];
20687 }
20688
20689
20690 delete[] ret;
20691
20692
20693
20694 for(int j=0; j < nParts; j++){
20695
20696 old_index=matchlist[i*nParts + j];
20697 *(argParts + Indices[j*K+old_index] + 1) = 1;
20698 }
20699
20700 }
20701
20702 delete[] matchlist;
20703 delete[] costlist;
20704
20705 return maxreturn;
20706
20707 }