Main Page | Modules | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members

util_sparx.cpp

Go to the documentation of this file.
00001 
00005 /*
00006  * Author: Pawel A.Penczek, 09/09/2006 (Pawel.A.Penczek@uth.tmc.edu)
00007  * Copyright (c) 2000-2006 The University of Texas - Houston Medical School
00008  *
00009  * This software is issued under a joint BSD/GNU license. You may use the
00010  * source code in this file under either license. However, note that the
00011  * complete EMAN2 and SPARX software packages have some GPL dependencies,
00012  * so you are responsible for compliance with the licenses of these packages
00013  * if you opt to use BSD licensing. The warranty disclaimer below holds
00014  * in either instance.
00015  *
00016  * This complete copyright notice must be included in any revised version of the
00017  * source code. Additional authorship citations may be added, but existing
00018  * author citations must be preserved.
00019  *
00020  * This program is free software; you can redistribute it and/or modify
00021  * it under the terms of the GNU General Public License as published by
00022  * the Free Software Foundation; either version 2 of the License, or
00023  * (at your option) any later version.
00024  *
00025  * This program is distributed in the hope that it will be useful,
00026  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00028  * GNU General Public License for more details.
00029  *
00030  * You should have received a copy of the GNU General Public License
00031  * along with this program; if not, write to the Free Software
00032  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
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 //  flip true:  find statistics under the mask (mask >0.5)
00064 //  flip false: find statistics ourside the mask (mask <0.5)
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                 //Vol->update_stat();
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         /* Check if the sizes of the mask and image are same */
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  /*       if (nx != mask_nx ||
00101             ny != mask_ny ||
00102             nz != mask_nz  ) {
00103            // should throw an exception here!!! (will clean it up later CY)
00104            fprintf(stderr, "The dimension of the image does not match the dimension of the mask!\n");
00105            fprintf(stderr, " nx = %d, mask_nx = %d\n", nx, mask_nx);
00106            fprintf(stderr, " ny = %d, mask_ny = %d\n", ny, mask_ny);
00107            fprintf(stderr, " nz = %d, mask_nz = %d\n", nz, mask_nz);
00108            exit(1);
00109         }
00110  */
00111         Volptr = Vol->get_data();
00112         maskptr = mask->get_data();
00113 
00114         for (size_t i = 0; i < (size_t)nx*ny*nz; ++i) {
00115                 if ((maskptr[i]>0.5f) == flip) {
00116                         Sum1 += Volptr[i];
00117                         Sum2 += Volptr[i]*double(Volptr[i]);
00118                         MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00119                         MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00120                         count++;
00121                 }
00122         }
00123 
00124         if (count == 0) {
00125                 LOGERR("Invalid mask");
00126                 throw ImageFormatException( "Invalid mask");
00127         }
00128 
00129         float avg = static_cast<float>(Sum1/count);
00130         float sig = static_cast<float>(sqrt((Sum2 - Sum1*Sum1/count)/(count-1)));
00131 
00132         stats.push_back(avg);
00133         stats.push_back(sig);
00134         stats.push_back(MIN);
00135         stats.push_back(MAX);
00136 
00137         return stats;
00138 }
00139 
00140 
00141 //----------------------------------------------------------------------------------------------------------
00142 
00143 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00144 {
00145         ENTERFUNC;
00146 
00147         if (!EMUtil::is_same_size(V1, V2)) {
00148                 LOGERR("images not same size");
00149                 throw ImageFormatException( "images not same size");
00150         }
00151 
00152         size_t nx = V1->get_xsize();
00153         size_t ny = V1->get_ysize();
00154         size_t nz = V1->get_zsize();
00155         size_t size = (size_t)nx*ny*nz;
00156 
00157         EMData *BD = new EMData();
00158         BD->set_size(nx, ny, nz);
00159 
00160         float *params = new float[2];
00161 
00162         float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00163         long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00164         int nvox = 0L;
00165 
00166         V1ptr = V1->get_data();
00167         V2ptr = V2->get_data();
00168         BDptr = BD->get_data();
00169 
00170 
00171         if(!mask){
00172                 EMData * Mask = new EMData();
00173                 Mask->set_size(nx,ny,nz);
00174                 Mask->to_one();
00175                 MASKptr = Mask->get_data();
00176         } else {
00177                 if (!EMUtil::is_same_size(V1, mask)) {
00178                         LOGERR("mask not same size");
00179                         throw ImageFormatException( "mask not same size");
00180                 }
00181 
00182                 MASKptr = mask->get_data();
00183         }
00184 
00185 
00186 
00187 //       calculation of S1,S2,S3,S3,nvox
00188 
00189         for (size_t i = 0L;i < size; i++) {
00190               if (MASKptr[i]>0.5f) {
00191                S1 += V1ptr[i]*V2ptr[i];
00192                S2 += V1ptr[i]*V1ptr[i];
00193                S3 += V2ptr[i];
00194                S4 += V1ptr[i];
00195                nvox ++;
00196               }
00197         }
00198 
00199         if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00200                 A =1.0f ;
00201         } else {
00202                 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00203         }
00204         B = static_cast<float> (A*S4  -  S3)/nvox;
00205 
00206         // calculation of the difference image
00207 
00208         for (size_t i = 0L;i < size; i++) {
00209              if (MASKptr[i]>0.5f) {
00210                BDptr[i] = A*V1ptr[i] -  B  - V2ptr[i];
00211              }  else  {
00212                BDptr[i] = 0.f;
00213              }
00214         }
00215 
00216         BD->update();
00217 
00218         params[0] = A;
00219         params[1] = B;
00220 
00221         Dict BDnParams;
00222         BDnParams["imdiff"] = BD;
00223         BDnParams["A"] = params[0];
00224         BDnParams["B"] = params[1];
00225 
00226         EXITFUNC;
00227         return BDnParams;
00228  }
00229 
00230 //----------------------------------------------------------------------------------------------------------
00231 
00232 
00233 
00234 EMData *Util::TwoDTestFunc(int Size, float p, float q,  float a, float b, int flag, float alphaDeg) //PRB
00235 {
00236         ENTERFUNC;
00237         int Mid= (Size+1)/2;
00238 
00239         if (flag==0) { // This is the real function
00240                 EMData* ImBW = new EMData();
00241                 ImBW->set_size(Size,Size,1);
00242                 ImBW->to_zero();
00243 
00244                 float tempIm;
00245                 float x,y;
00246 
00247                 for (int ix=(1-Mid);  ix<Mid; ix++){
00248                         for (int iy=(1-Mid);  iy<Mid; iy++){
00249                                 x = (float)ix;
00250                                 y = (float)iy;
00251                         tempIm= static_cast<float>( (1/(2*M_PI)) * cos(p*x)* cos(q*y) * exp(-.5*x*x/(a*a))* exp(-.5*y*y/(b*b)) );
00252                                 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00253                         }
00254                 }
00255                 ImBW->update();
00256                 ImBW->set_complex(false);
00257                 ImBW->set_ri(true);
00258 
00259                 return ImBW;
00260         }
00261         else if (flag==1) {  // This is the Fourier Transform
00262                 EMData* ImBWFFT = new EMData();
00263                 ImBWFFT ->set_size(2*Size,Size,1);
00264                 ImBWFFT ->to_zero();
00265 
00266                 float r,s;
00267 
00268                 for (int ir=(1-Mid);  ir<Mid; ir++){
00269                         for (int is=(1-Mid);  is<Mid; is++){
00270                                 r = (float)ir;
00271                                 s = (float)is;
00272                         (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00273                                 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00274                         }
00275                 }
00276                 ImBWFFT->update();
00277                 ImBWFFT->set_complex(true);
00278                 ImBWFFT->set_ri(true);
00279                 ImBWFFT->set_shuffled(true);
00280                 ImBWFFT->set_fftodd(true);
00281 
00282                 return ImBWFFT;
00283         }
00284         else if (flag==2 || flag==3) { //   This is the projection in Real Space
00285                 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00286                 float C=cos(alpha);
00287                 float S=sin(alpha);
00288                 float D= sqrt(S*S*b*b + C*C*a*a);
00289                 //float D2 = D*D;   PAP - to get rid of warning
00290 
00291                 float P = p * C *a*a/D ;
00292                 float Q = q * S *b*b/D ;
00293 
00294                 if (flag==2) {
00295                         EMData* pofalpha = new EMData();
00296                         pofalpha ->set_size(Size,1,1);
00297                         pofalpha ->to_zero();
00298 
00299                         float Norm0 =  D*(float)sqrt(2*pi);
00300                         float Norm1 =  exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00301                         float Norm2 =  exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00302                         float sD;
00303 
00304                         for (int is=(1-Mid);  is<Mid; is++){
00305                                 sD = is/D ;
00306                                 (*pofalpha)(is+Mid-1) =  Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00307                          + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00308                         }
00309                         pofalpha-> update();
00310                         pofalpha-> set_complex(false);
00311                         pofalpha-> set_ri(true);
00312 
00313                         return pofalpha;
00314                 }
00315                 if (flag==3) { // This is the projection in Fourier Space
00316                         float vD;
00317 
00318                         EMData* pofalphak = new EMData();
00319                         pofalphak ->set_size(2*Size,1,1);
00320                         pofalphak ->to_zero();
00321 
00322                         for (int iv=(1-Mid);  iv<Mid; iv++){
00323                                 vD = iv*D ;
00324                                 (*pofalphak)(2*(iv+Mid-1)) =  exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00325                         }
00326                         pofalphak-> update();
00327                         pofalphak-> set_complex(false);
00328                         pofalphak-> set_ri(true);
00329 
00330                         return pofalphak;
00331                 }
00332         }
00333         else if (flag==4) {
00334                 cout <<" FH under construction";
00335                 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00336                 EMData* TryFH= OutFT -> real2FH(4.0);
00337                 return TryFH;
00338         } else {
00339                 cout <<" flag must be 0,1,2,3, or 4";
00340         }
00341 
00342         EXITFUNC;
00343         return 0;
00344 }
00345 
00346 
00347 void Util::spline_mat(float *x, float *y, int n,  float *xq, float *yq, int m) //PRB
00348 {
00349 
00350         float x0= x[0];
00351         float x1= x[1];
00352         float x2= x[2];
00353         float y0= y[0];
00354         float y1= y[1];
00355         float y2= y[2];
00356         float yp1 =  (y1-y0)/(x1-x0) +  (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1)  ;
00357         float xn  = x[n];
00358         float xnm1= x[n-1];
00359         float xnm2= x[n-2];
00360         float yn  = y[n];
00361         float ynm1= y[n-1];
00362         float ynm2= y[n-2];
00363         float ypn=  (yn-ynm1)/(xn-xnm1) +  (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00364         float *y2d = new float[n];
00365         Util::spline(x,y,n,yp1,ypn,y2d);
00366         Util::splint(x,y,y2d,n,xq,yq,m); //PRB
00367         delete [] y2d;
00368         return;
00369 }
00370 
00371 
00372 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2) //PRB
00373 {
00374         int i,k;
00375         float p, qn, sig, un, *u;
00376         u = new float[n-1];
00377 
00378         if (yp1 > .99e30){
00379                 y2[0]=u[0]=0.0;
00380         } else {
00381                 y2[0]=-.5f;
00382                 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00383         }
00384 
00385         for (i=1; i < n-1; i++) {
00386                 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00387                 p = sig*y2[i-1] + 2.0f;
00388                 y2[i]  = (sig-1.0f)/p;
00389                 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) -  (y[i] - y[i-1] )/(x[i] -x[i-1]);
00390                 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00391         }
00392 
00393         if (ypn>.99e30){
00394                 qn=0; un=0;
00395         } else {
00396                 qn= .5f;
00397                 un= (3.0f/(x[n-1] -x[n-2])) * (ypn -  (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00398         }
00399         y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00400         for (k=n-2; k>=0; k--){
00401                 y2[k]=y2[k]*y2[k+1]+u[k];
00402         }
00403         delete [] u;
00404 }
00405 
00406 
00407 void Util::splint( float *xa, float *ya, float *y2a, int n,  float *xq, float *yq, int m) //PRB
00408 {
00409         int klo, khi, k;
00410         float h, b, a;
00411 
00412 //      klo=0; // can try to put here
00413         for (int j=0; j<m;j++){
00414                 klo=0;
00415                 khi=n-1;
00416                 while (khi-klo >1) {
00417                         k=(khi+klo) >>1;
00418                         if  (xa[k]>xq[j]){ khi=k;}
00419                         else { klo=k;}
00420                 }
00421                 h=xa[khi]- xa[klo];
00422                 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00423                 a =(xa[khi]-xq[j])/h;
00424                 b=(xq[j]-xa[klo])/h;
00425                 yq[j]=a*ya[klo] + b*ya[khi]
00426                         + ((a*a*a-a)*y2a[klo]
00427                              +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00428         }
00429 //      printf("h=%f, a = %f, b=%f, ya[klo]=%f, ya[khi]=%f , yq=%f\n",h, a, b, ya[klo], ya[khi],yq[0]);
00430 }
00431 
00432 
00433 void Util::Radialize(int *PermMatTr, float *kValsSorted,   // PRB
00434                float *weightofkValsSorted, int Size, int *SizeReturned)
00435 {
00436         int iMax = (int) floor( (Size-1.0)/2 +.01);
00437         int CountMax = (iMax+2)*(iMax+1)/2;
00438         int Count=-1;
00439         float *kVals     = new float[CountMax];
00440         float *weightMat = new float[CountMax];
00441         int *PermMat     = new   int[CountMax];
00442         SizeReturned[0] = CountMax;
00443 
00444 //      printf("Aa \n");        fflush(stdout);
00445         for (int jkx=0; jkx< iMax+1; jkx++) {
00446                 for (int jky=0; jky< jkx+1; jky++) {
00447                         Count++;
00448                         kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00449                         weightMat[Count]=  1.0;
00450                         if (jkx!=0)  { weightMat[Count] *=2;}
00451                         if (jky!=0)  { weightMat[Count] *=2;}
00452                         if (jkx!=jky){ weightMat[Count] *=2;}
00453                         PermMat[Count]=Count+1;
00454                 }
00455         }
00456 
00457         int lkVals = Count+1;
00458 //      printf("Cc \n");fflush(stdout);
00459 
00460         sort_mat(&kVals[0],&kVals[Count],
00461              &PermMat[0],  &PermMat[Count]);  //PermMat is
00462                                 //also returned as well as kValsSorted
00463         fflush(stdout);
00464 
00465         int newInd;
00466 
00467         for (int iP=0; iP < lkVals ; iP++ ) {
00468                 newInd =  PermMat[iP];
00469                 PermMatTr[newInd-1] = iP+1;
00470         }
00471 
00472 //      printf("Ee \n"); fflush(stdout);
00473 
00474         int CountA=-1;
00475         int CountB=-1;
00476 
00477         while (CountB< (CountMax-1)) {
00478                 CountA++;
00479                 CountB++;
00480 //              printf("CountA=%d ; CountB=%d \n", CountA,CountB);fflush(stdout);
00481                 kValsSorted[CountA] = kVals[CountB] ;
00482                 if (CountB<(CountMax-1) ) {
00483                         while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001  ) {
00484                                 SizeReturned[0]--;
00485                                 for (int iP=0; iP < lkVals; iP++){
00486 //                                      printf("iP=%d \n", iP);fflush(stdout);
00487                                         if  (PermMatTr[iP]>CountA+1) {
00488                                                 PermMatTr[iP]--;
00489                                         }
00490                                 }
00491                                 CountB++;
00492                         }
00493                 }
00494         }
00495 
00496 
00497         for (int CountD=0; CountD < CountMax; CountD++) {
00498             newInd = PermMatTr[CountD];
00499             weightofkValsSorted[newInd-1] += weightMat[CountD];
00500         }
00501 
00502 }
00503 
00504 
00505 vector<float>
00506 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00507 {
00508         vector<float> angles;
00509         float psi = 0.0;
00510         if ((0.0 == t1 && 0.0 == t2)||(t1 >= t2)) {
00511                 t1 = 0.0f;
00512                 t2 = 90.0f;
00513         }
00514         if ((0.0 == p1 && 0.0 == p2)||(p1 >= p2)) {
00515                 p1 = 0.0f;
00516                 p2 = 359.9f;
00517         }
00518         bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00519         for (float theta = t1; theta <= t2; theta += delta) {
00520                 float detphi;
00521                 int lt;
00522                 if ((0.0 == theta)||(180.0 == theta)) {
00523                         detphi = 360.0f;
00524                         lt = 1;
00525                 } else {
00526                         detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00527                         lt = int((p2 - p1)/detphi)-1;
00528                         if (lt < 1) lt = 1;
00529                         detphi = (p2 - p1)/lt;
00530                 }
00531                 for (int i = 0; i < lt; i++) {
00532                         float phi = p1 + i*detphi;
00533                         if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00534                         angles.push_back(phi);
00535                         angles.push_back(theta);
00536                         angles.push_back(psi);
00537                 }
00538         }
00539         return angles;
00540 }
00541 
00542 
00543 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00544 /*float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00545 {
00546 
00547 //  purpose: quadratic interpolation
00548 //
00549 //  parameters:       xx,yy treated as circularly closed.
00550 //                    fdata - image 1..nxdata, 1..nydata
00551 //
00552 //                    f3    fc       f0, f1, f2, f3 are the values
00553 //                     +             at the grid points.  x is the
00554 //                     + x           point at which the function
00555 //              f2++++f0++++f1       is to be estimated. (it need
00556 //                     +             not be in the first quadrant).
00557 //                     +             fc - the outer corner point
00558 //                    f4             nearest x.
00559 c
00560 //                                   f0 is the value of the fdata at
00561 //                                   fdata(i,j), it is the interior mesh
00562 //                                   point nearest  x.
00563 //                                   the coordinates of f0 are (x0,y0),
00564 //                                   the coordinates of f1 are (xb,y0),
00565 //                                   the coordinates of f2 are (xa,y0),
00566 //                                   the coordinates of f3 are (x0,yb),
00567 //                                   the coordinates of f4 are (x0,ya),
00568 //                                   the coordinates of fc are (xc,yc),
00569 c
00570 //                   o               hxa, hxb are the mesh spacings
00571 //                   +               in the x-direction to the left
00572 //                  hyb              and right of the center point.
00573 //                   +
00574 //            ++hxa++o++hxb++o       hyb, hya are the mesh spacings
00575 //                   +               in the y-direction.
00576 //                  hya
00577 //                   +               hxc equals either  hxb  or  hxa
00578 //                   o               depending on where the corner
00579 //                                   point is located.
00580 c
00581 //                                   construct the interpolant
00582 //                                   f = f0 + c1*(x-x0) +
00583 //                                       c2*(x-x0)*(x-x1) +
00584 //                                       c3*(y-y0) + c4*(y-y0)*(y-y1)
00585 //                                       + c5*(x-x0)*(y-y0)
00586 //
00587 //
00588 
00589     float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00590     float quadri;
00591     int   i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00592 
00593     x = xx;
00594     y = yy;
00595 
00596     // circular closure
00597         while ( x < 1.0 ) x += nxdata;
00598         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00599         while ( y < 1.0 ) y += nydata;
00600         while ( y >= (float)(nydata+1) )  y -= nydata;
00601 
00602 
00603     i   = (int) x;
00604     j   = (int) y;
00605 
00606     dx0 = x - i;
00607     dy0 = y - j;
00608 
00609     ip1 = i + 1;
00610     im1 = i - 1;
00611     jp1 = j + 1;
00612     jm1 = j - 1;
00613 
00614     if (ip1 > nxdata) ip1 = ip1 - nxdata;
00615     if (im1 < 1)      im1 = im1 + nxdata;
00616     if (jp1 > nydata) jp1 = jp1 - nydata;
00617     if (jm1 < 1)      jm1 = jm1 + nydata;
00618 
00619     f0  = fdata(i,j);
00620     c1  = fdata(ip1,j) - f0;
00621     c2  = (c1 - f0 + fdata(im1,j)) * 0.5;
00622     c3  = fdata(i,jp1) - f0;
00623     c4  = (c3 - f0 + fdata(i,jm1)) * 0.5;
00624 
00625     dxb = dx0 - 1;
00626     dyb = dy0 - 1;
00627 
00628     // hxc & hyc are either 1 or -1
00629     if (dx0 >= 0) { hxc = 1; } else { hxc = -1; }
00630     if (dy0 >= 0) { hyc = 1; } else { hyc = -1; }
00631 
00632     ic  = i + hxc;
00633     jc  = j + hyc;
00634 
00635     if (ic > nxdata) { ic = ic - nxdata; }  else if (ic < 1) { ic = ic + nxdata; }
00636     if (jc > nydata) { jc = jc - nydata; } else if (jc < 1) { jc = jc + nydata; }
00637 
00638     c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0)) * c2
00639             - hyc * c3 - (hyc * (hyc - 1.0)) * c4) * (hxc * hyc));
00640 
00641     quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00642 
00643     return quadri;
00644 }*/
00645 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00646 {
00647 //  purpose: quadratic interpolation
00648 //  Optimized for speed, circular closer removed, checking of ranges removed
00649         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00650         float  quadri;
00651         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00652 
00653         x = xx;
00654         y = yy;
00655 
00656         //     any xx and yy
00657         while ( x < 1.0 )                 x += nxdata;
00658         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00659         while ( y < 1.0 )                 y += nydata;
00660         while ( y >= (float)(nydata+1) )  y -= nydata;
00661 
00662         i   = (int) x;
00663         j   = (int) y;
00664 
00665         dx0 = x - i;
00666         dy0 = y - j;
00667 
00668         ip1 = i + 1;
00669         im1 = i - 1;
00670         jp1 = j + 1;
00671         jm1 = j - 1;
00672 
00673         if (ip1 > nxdata) ip1 -= nxdata;
00674         if (im1 < 1)      im1 += nxdata;
00675         if (jp1 > nydata) jp1 -= nydata;
00676         if (jm1 < 1)      jm1 += nydata;
00677 
00678         f0  = fdata(i,j);
00679         c1  = fdata(ip1,j) - f0;
00680         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00681         c3  = fdata(i,jp1) - f0;
00682         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00683 
00684         dxb = dx0 - 1;
00685         dyb = dy0 - 1;
00686 
00687         // hxc & hyc are either 1 or -1
00688         if (dx0 >= 0) hxc = 1; else hxc = -1;
00689         if (dy0 >= 0) hyc = 1; else hyc = -1;
00690 
00691         ic  = i + hxc;
00692         jc  = j + hyc;
00693 
00694         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00695         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00696 
00697         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00698                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00699 
00700 
00701         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00702 
00703         return quadri;
00704 }
00705 
00706 #undef fdata
00707 
00708 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00709 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00710 {
00711 //  purpose: quadratic interpolation
00712 //  Optimized for speed, circular closer removed, checking of ranges removed
00713         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00714         float  quadri;
00715         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00716 
00717         x = xx;
00718         y = yy;
00719 
00720         // wrap around is not done circulantly; if (x,y) is not in the image, then x = xnew and y = ynew
00721         if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00722               x = (float)xnew;
00723                   y = (float)ynew;
00724      }
00725 
00726 
00727         i   = (int) x;
00728         j   = (int) y;
00729 
00730         dx0 = x - i;
00731         dy0 = y - j;
00732 
00733         ip1 = i + 1;
00734         im1 = i - 1;
00735         jp1 = j + 1;
00736         jm1 = j - 1;
00737 
00738         if (ip1 > nxdata) ip1 -= nxdata;
00739         if (im1 < 1)      im1 += nxdata;
00740         if (jp1 > nydata) jp1 -= nydata;
00741         if (jm1 < 1)      jm1 += nydata;
00742 
00743         f0  = fdata(i,j);
00744         c1  = fdata(ip1,j) - f0;
00745         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00746         c3  = fdata(i,jp1) - f0;
00747         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00748 
00749         dxb = dx0 - 1;
00750         dyb = dy0 - 1;
00751 
00752         // hxc & hyc are either 1 or -1
00753         if (dx0 >= 0) hxc = 1; else hxc = -1;
00754         if (dy0 >= 0) hyc = 1; else hyc = -1;
00755 
00756         ic  = i + hxc;
00757         jc  = j + hyc;
00758 
00759         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00760         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00761 
00762         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00763                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00764 
00765 
00766         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00767 
00768         return quadri;
00769 }
00770 
00771 #undef fdata
00772 
00773 
00774 float  Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00775         int K = kb.get_window_size();
00776         int kbmin = -K/2;
00777         int kbmax = -kbmin;
00778         int kbc = kbmax+1;
00779 
00780         float pixel =0.0f;
00781         float w=0.0f;
00782 
00783         delx = restrict1(delx, nx);
00784         int inxold = int(round(delx));
00785         if ( ny < 2 ) {  //1D
00786                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00787                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00788                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00789                 float tablex4 = kb.i0win_tab(delx-inxold);
00790                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00791                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00792                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00793 
00794                 int x1, x2, x3, x4, x5, x6, x7;
00795 
00796                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
00797                         x1 = (inxold-3+nx)%nx;
00798                         x2 = (inxold-2+nx)%nx;
00799                         x3 = (inxold-1+nx)%nx;
00800                         x4 = (inxold  +nx)%nx;
00801                         x5 = (inxold+1+nx)%nx;
00802                         x6 = (inxold+2+nx)%nx;
00803                         x7 = (inxold+3+nx)%nx;
00804                 } else {
00805                         x1 = inxold-3;
00806                         x2 = inxold-2;
00807                         x3 = inxold-1;
00808                         x4 = inxold;
00809                         x5 = inxold+1;
00810                         x6 = inxold+2;
00811                         x7 = inxold+3;
00812                 }
00813 
00814                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00815                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00816                         data[x7]*tablex7 ;
00817 
00818                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00819         } else if ( nz < 2 ) {  // 2D
00820                 dely = restrict1(dely, ny);
00821                 int inyold = int(round(dely));
00822                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00823                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00824                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00825                 float tablex4 = kb.i0win_tab(delx-inxold);
00826                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00827                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00828                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00829 
00830                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00831                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00832                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00833                 float tabley4 = kb.i0win_tab(dely-inyold);
00834                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00835                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00836                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00837 
00838                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00839 
00840                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
00841                         x1 = (inxold-3+nx)%nx;
00842                         x2 = (inxold-2+nx)%nx;
00843                         x3 = (inxold-1+nx)%nx;
00844                         x4 = (inxold  +nx)%nx;
00845                         x5 = (inxold+1+nx)%nx;
00846                         x6 = (inxold+2+nx)%nx;
00847                         x7 = (inxold+3+nx)%nx;
00848 
00849                         y1 = ((inyold-3+ny)%ny)*nx;
00850                         y2 = ((inyold-2+ny)%ny)*nx;
00851                         y3 = ((inyold-1+ny)%ny)*nx;
00852                         y4 = ((inyold  +ny)%ny)*nx;
00853                         y5 = ((inyold+1+ny)%ny)*nx;
00854                         y6 = ((inyold+2+ny)%ny)*nx;
00855                         y7 = ((inyold+3+ny)%ny)*nx;
00856                 } else {
00857                         x1 = inxold-3;
00858                         x2 = inxold-2;
00859                         x3 = inxold-1;
00860                         x4 = inxold;
00861                         x5 = inxold+1;
00862                         x6 = inxold+2;
00863                         x7 = inxold+3;
00864 
00865                         y1 = (inyold-3)*nx;
00866                         y2 = (inyold-2)*nx;
00867                         y3 = (inyold-1)*nx;
00868                         y4 = inyold*nx;
00869                         y5 = (inyold+1)*nx;
00870                         y6 = (inyold+2)*nx;
00871                         y7 = (inyold+3)*nx;
00872                 }
00873 
00874                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00875                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00876                              data[x7+y1]*tablex7 ) * tabley1 +
00877                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00878                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00879                              data[x7+y2]*tablex7 ) * tabley2 +
00880                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00881                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00882                              data[x7+y3]*tablex7 ) * tabley3 +
00883                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00884                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00885                              data[x7+y4]*tablex7 ) * tabley4 +
00886                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00887                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00888                              data[x7+y5]*tablex7 ) * tabley5 +
00889                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00890                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00891                              data[x7+y6]*tablex7 ) * tabley6 +
00892                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00893                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00894                              data[x7+y7]*tablex7 ) * tabley7;
00895 
00896                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00897                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00898         } else {  //  3D
00899                 dely = restrict1(dely, ny);
00900                 int inyold = int(Util::round(dely));
00901                 delz = restrict1(delz, nz);
00902                 int inzold = int(Util::round(delz));
00903 
00904                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00905                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00906                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00907                 float tablex4 = kb.i0win_tab(delx-inxold);
00908                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00909                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00910                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00911 
00912                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00913                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00914                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00915                 float tabley4 = kb.i0win_tab(dely-inyold);
00916                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00917                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00918                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00919 
00920                 float tablez1 = kb.i0win_tab(delz-inzold+3);
00921                 float tablez2 = kb.i0win_tab(delz-inzold+2);
00922                 float tablez3 = kb.i0win_tab(delz-inzold+1);
00923                 float tablez4 = kb.i0win_tab(delz-inzold);
00924                 float tablez5 = kb.i0win_tab(delz-inzold-1);
00925                 float tablez6 = kb.i0win_tab(delz-inzold-2);
00926                 float tablez7 = kb.i0win_tab(delz-inzold-3);
00927 
00928                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00929 
00930                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
00931                         x1 = (inxold-3+nx)%nx;
00932                         x2 = (inxold-2+nx)%nx;
00933                         x3 = (inxold-1+nx)%nx;
00934                         x4 = (inxold  +nx)%nx;
00935                         x5 = (inxold+1+nx)%nx;
00936                         x6 = (inxold+2+nx)%nx;
00937                         x7 = (inxold+3+nx)%nx;
00938 
00939                         y1 = ((inyold-3+ny)%ny)*nx;
00940                         y2 = ((inyold-2+ny)%ny)*nx;
00941                         y3 = ((inyold-1+ny)%ny)*nx;
00942                         y4 = ((inyold  +ny)%ny)*nx;
00943                         y5 = ((inyold+1+ny)%ny)*nx;
00944                         y6 = ((inyold+2+ny)%ny)*nx;
00945                         y7 = ((inyold+3+ny)%ny)*nx;
00946 
00947                         z1 = ((inzold-3+nz)%nz)*nx*ny;
00948                         z2 = ((inzold-2+nz)%nz)*nx*ny;
00949                         z3 = ((inzold-1+nz)%nz)*nx*ny;
00950                         z4 = ((inzold  +nz)%nz)*nx*ny;
00951                         z5 = ((inzold+1+nz)%nz)*nx*ny;
00952                         z6 = ((inzold+2+nz)%nz)*nx*ny;
00953                         z7 = ((inzold+3+nz)%nz)*nx*ny;
00954                 } else {
00955                         x1 = inxold-3;
00956                         x2 = inxold-2;
00957                         x3 = inxold-1;
00958                         x4 = inxold;
00959                         x5 = inxold+1;
00960                         x6 = inxold+2;
00961                         x7 = inxold+3;
00962 
00963                         y1 = (inyold-3)*nx;
00964                         y2 = (inyold-2)*nx;
00965                         y3 = (inyold-1)*nx;
00966                         y4 = inyold*nx;
00967                         y5 = (inyold+1)*nx;
00968                         y6 = (inyold+2)*nx;
00969                         y7 = (inyold+3)*nx;
00970 
00971                         z1 = (inzold-3)*nx*ny;
00972                         z2 = (inzold-2)*nx*ny;
00973                         z3 = (inzold-1)*nx*ny;
00974                         z4 = inzold*nx*ny;
00975                         z5 = (inzold+1)*nx*ny;
00976                         z6 = (inzold+2)*nx*ny;
00977                         z7 = (inzold+3)*nx*ny;
00978                 }
00979 
00980                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00981                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00982                              data[x7+y1+z1]*tablex7 ) * tabley1 +
00983                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00984                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00985                              data[x7+y2+z1]*tablex7 ) * tabley2 +
00986                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00987                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00988                              data[x7+y3+z1]*tablex7 ) * tabley3 +
00989                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00990                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00991                              data[x7+y4+z1]*tablex7 ) * tabley4 +
00992                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
00993                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
00994                              data[x7+y5+z1]*tablex7 ) * tabley5 +
00995                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
00996                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
00997                              data[x7+y6+z1]*tablex7 ) * tabley6 +
00998                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
00999                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01000                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01001                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01002                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01003                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01004                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01005                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01006                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01007                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01008                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01009                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01010                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01011                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01012                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01013                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01014                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01015                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01016                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01017                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01018                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01019                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01020                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01021                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01022                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01023                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01024                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01025                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01026                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01027                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01028                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01029                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01030                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01031                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01032                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01033                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01034                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01035                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01036                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01037                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01038                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01039                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01040                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01041                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01042                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01043                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01044                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01045                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01046                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01047                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01048                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01049                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01050                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01051                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01052                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01053                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01054                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01055                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01056                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01057                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01058                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01059                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01060                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01061                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01062                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01063                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01064                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01065                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01066                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01067                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01068                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01069                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01070                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01071                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01072                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01073                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01074                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01075                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01076                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01077                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01078                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01079                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01080                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01081                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01082                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01083                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01084                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01085                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01086                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01087                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01088                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01089                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01090                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01091                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01092                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01093                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01094                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01095                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01096                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01097                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01098                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01099                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01100                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01101                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01102                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01103                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01104                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01105                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01106                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01107                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01108                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01109                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01110                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01111                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01112                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01113                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01114                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01115                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01116                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01117                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01118                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01119                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01120                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01121                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01122                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01123                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01124                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01125                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01126                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01127 
01128                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01129                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01130                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01131         }
01132         return pixel/w;
01133 }
01134 
01135 float  Util::get_pixel_conv_new_background(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb, int xnew, int ynew) {
01136         int K = kb.get_window_size();
01137         int kbmin = -K/2;
01138         int kbmax = -kbmin;
01139         int kbc = kbmax+1;
01140 
01141         float pixel =0.0f;
01142         float w=0.0f;
01143 
01144     float argdelx = delx; // adding this for 2D case where the wrap around is not done circulantly using restrict1.
01145         delx = restrict1(delx, nx);
01146         int inxold = int(round(delx));
01147         if ( ny < 2 ) {  //1D
01148                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01149                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01150                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01151                 float tablex4 = kb.i0win_tab(delx-inxold);
01152                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01153                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01154                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01155 
01156                 int x1, x2, x3, x4, x5, x6, x7;
01157 
01158                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
01159                         x1 = (inxold-3+nx)%nx;
01160                         x2 = (inxold-2+nx)%nx;
01161                         x3 = (inxold-1+nx)%nx;
01162                         x4 = (inxold  +nx)%nx;
01163                         x5 = (inxold+1+nx)%nx;
01164                         x6 = (inxold+2+nx)%nx;
01165                         x7 = (inxold+3+nx)%nx;
01166                 } else {
01167                         x1 = inxold-3;
01168                         x2 = inxold-2;
01169                         x3 = inxold-1;
01170                         x4 = inxold;
01171                         x5 = inxold+1;
01172                         x6 = inxold+2;
01173                         x7 = inxold+3;
01174                 }
01175 
01176                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01177                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01178                         data[x7]*tablex7 ;
01179 
01180                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01181         } else if ( nz < 2 ) {  // 2D
01182 
01183                 delx = argdelx;
01184                 // the wrap around is not done circulantly for 2D case; if (argdelx, argdely) is not in the image, then make them (xnew, ynew) which is definitely in the image
01185                 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01186                 delx = (float)xnew*2.0f;
01187                 dely = (float)ynew*2.0f;
01188                 }
01189 
01190                 int inxold = int(round(delx));
01191                 int inyold = int(round(dely));
01192 
01193                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01194                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01195                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01196                 float tablex4 = kb.i0win_tab(delx-inxold);
01197                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01198                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01199                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01200 
01201                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01202                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01203                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01204                 float tabley4 = kb.i0win_tab(dely-inyold);
01205                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01206                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01207                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01208 
01209                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01210 
01211                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
01212                         x1 = (inxold-3+nx)%nx;
01213                         x2 = (inxold-2+nx)%nx;
01214                         x3 = (inxold-1+nx)%nx;
01215                         x4 = (inxold  +nx)%nx;
01216                         x5 = (inxold+1+nx)%nx;
01217                         x6 = (inxold+2+nx)%nx;
01218                         x7 = (inxold+3+nx)%nx;
01219 
01220                         y1 = ((inyold-3+ny)%ny)*nx;
01221                         y2 = ((inyold-2+ny)%ny)*nx;
01222                         y3 = ((inyold-1+ny)%ny)*nx;
01223                         y4 = ((inyold  +ny)%ny)*nx;
01224                         y5 = ((inyold+1+ny)%ny)*nx;
01225                         y6 = ((inyold+2+ny)%ny)*nx;
01226                         y7 = ((inyold+3+ny)%ny)*nx;
01227                 } else {
01228                         x1 = inxold-3;
01229                         x2 = inxold-2;
01230                         x3 = inxold-1;
01231                         x4 = inxold;
01232                         x5 = inxold+1;
01233                         x6 = inxold+2;
01234                         x7 = inxold+3;
01235 
01236                         y1 = (inyold-3)*nx;
01237                         y2 = (inyold-2)*nx;
01238                         y3 = (inyold-1)*nx;
01239                         y4 = inyold*nx;
01240                         y5 = (inyold+1)*nx;
01241                         y6 = (inyold+2)*nx;
01242                         y7 = (inyold+3)*nx;
01243                 }
01244 
01245                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01246                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01247                              data[x7+y1]*tablex7 ) * tabley1 +
01248                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01249                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01250                              data[x7+y2]*tablex7 ) * tabley2 +
01251                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01252                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01253                              data[x7+y3]*tablex7 ) * tabley3 +
01254                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01255                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01256                              data[x7+y4]*tablex7 ) * tabley4 +
01257                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01258                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01259                              data[x7+y5]*tablex7 ) * tabley5 +
01260                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01261                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01262                              data[x7+y6]*tablex7 ) * tabley6 +
01263                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01264                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01265                              data[x7+y7]*tablex7 ) * tabley7;
01266 
01267                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01268                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01269         } else {  //  3D
01270                 dely = restrict1(dely, ny);
01271                 int inyold = int(Util::round(dely));
01272                 delz = restrict1(delz, nz);
01273                 int inzold = int(Util::round(delz));
01274 
01275                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01276                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01277                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01278                 float tablex4 = kb.i0win_tab(delx-inxold);
01279                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01280                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01281                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01282 
01283                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01284                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01285                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01286                 float tabley4 = kb.i0win_tab(dely-inyold);
01287                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01288                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01289                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01290 
01291                 float tablez1 = kb.i0win_tab(delz-inzold+3);
01292                 float tablez2 = kb.i0win_tab(delz-inzold+2);
01293                 float tablez3 = kb.i0win_tab(delz-inzold+1);
01294                 float tablez4 = kb.i0win_tab(delz-inzold);
01295                 float tablez5 = kb.i0win_tab(delz-inzold-1);
01296                 float tablez6 = kb.i0win_tab(delz-inzold-2);
01297                 float tablez7 = kb.i0win_tab(delz-inzold-3);
01298 
01299                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01300 
01301                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
01302                         x1 = (inxold-3+nx)%nx;
01303                         x2 = (inxold-2+nx)%nx;
01304                         x3 = (inxold-1+nx)%nx;
01305                         x4 = (inxold  +nx)%nx;
01306                         x5 = (inxold+1+nx)%nx;
01307                         x6 = (inxold+2+nx)%nx;
01308                         x7 = (inxold+3+nx)%nx;
01309 
01310                         y1 = ((inyold-3+ny)%ny)*nx;
01311                         y2 = ((inyold-2+ny)%ny)*nx;
01312                         y3 = ((inyold-1+ny)%ny)*nx;
01313                         y4 = ((inyold  +ny)%ny)*nx;
01314                         y5 = ((inyold+1+ny)%ny)*nx;
01315                         y6 = ((inyold+2+ny)%ny)*nx;
01316                         y7 = ((inyold+3+ny)%ny)*nx;
01317 
01318                         z1 = ((inzold-3+nz)%nz)*nx*ny;
01319                         z2 = ((inzold-2+nz)%nz)*nx*ny;
01320                         z3 = ((inzold-1+nz)%nz)*nx*ny;
01321                         z4 = ((inzold  +nz)%nz)*nx*ny;
01322                         z5 = ((inzold+1+nz)%nz)*nx*ny;
01323                         z6 = ((inzold+2+nz)%nz)*nx*ny;
01324                         z7 = ((inzold+3+nz)%nz)*nx*ny;
01325                 } else {
01326                         x1 = inxold-3;
01327                         x2 = inxold-2;
01328                         x3 = inxold-1;
01329                         x4 = inxold;
01330                         x5 = inxold+1;
01331                         x6 = inxold+2;
01332                         x7 = inxold+3;
01333 
01334                         y1 = (inyold-3)*nx;
01335                         y2 = (inyold-2)*nx;
01336                         y3 = (inyold-1)*nx;
01337                         y4 = inyold*nx;
01338                         y5 = (inyold+1)*nx;
01339                         y6 = (inyold+2)*nx;
01340                         y7 = (inyold+3)*nx;
01341 
01342                         z1 = (inzold-3)*nx*ny;
01343                         z2 = (inzold-2)*nx*ny;
01344                         z3 = (inzold-1)*nx*ny;
01345                         z4 = inzold*nx*ny;
01346                         z5 = (inzold+1)*nx*ny;
01347                         z6 = (inzold+2)*nx*ny;
01348                         z7 = (inzold+3)*nx*ny;
01349                 }
01350 
01351                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01352                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01353                              data[x7+y1+z1]*tablex7 ) * tabley1 +
01354                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01355                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01356                              data[x7+y2+z1]*tablex7 ) * tabley2 +
01357                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01358                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01359                              data[x7+y3+z1]*tablex7 ) * tabley3 +
01360                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01361                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01362                              data[x7+y4+z1]*tablex7 ) * tabley4 +
01363                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01364                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01365                              data[x7+y5+z1]*tablex7 ) * tabley5 +
01366                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01367                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01368                              data[x7+y6+z1]*tablex7 ) * tabley6 +
01369                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01370                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01371                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01372                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01373                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01374                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01375                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01376                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01377                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01378                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01379                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01380                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01381                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01382                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01383                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01384                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01385                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01386                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01387                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01388                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01389                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01390                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01391                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01392                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01393                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01394                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01395                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01396                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01397                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01398                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01399                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01400                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01401                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01402                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01403                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01404                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01405                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01406                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01407                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01408                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01409                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01410                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01411                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01412                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01413                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01414                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01415                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01416                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01417                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01418                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01419                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01420                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01421                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01422                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01423                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01424                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01425                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01426                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01427                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01428                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01429                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01430                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01431                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01432                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01433                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01434                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01435                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01436                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01437                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01438                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01439                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01440                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01441                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01442                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01443                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01444                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01445                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01446                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01447                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01448                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01449                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01450                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01451                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01452                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01453                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01454                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01455                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01456                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01457                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01458                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01459                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01460                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01461                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01462                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01463                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01464                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01465                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01466                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01467                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01468                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01469                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01470                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01471                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01472                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01473                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01474                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01475                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01476                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01477                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01478                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01479                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01480                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01481                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01482                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01483                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01484                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01485                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01486                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01487                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01488                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01489                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01490                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01491                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01492                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01493                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01494                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01495                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01496                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01497                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01498 
01499                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01500                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01501                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01502         }
01503         return pixel/w;
01504 }
01505 
01506 /*
01507 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01508 
01509         int nxreal = nx - 2;
01510         if (nxreal != ny)
01511                 throw ImageDimensionException("extractpoint requires ny == nx");
01512         int nhalf = nxreal/2;
01513         int kbsize = kb.get_window_size();
01514         int kbmin = -kbsize/2;
01515         int kbmax = -kbmin;
01516         bool flip = (nuxnew < 0.f);
01517         if (flip) {
01518                 nuxnew *= -1;
01519                 nuynew *= -1;
01520         }
01521         // put (xnew,ynew) on a grid.  The indices will be wrong for
01522         // the Fourier elements in the image, but the grid sizing will
01523         // be correct.
01524         int ixn = int(Util::round(nuxnew));
01525         int iyn = int(Util::round(nuynew));
01526         // set up some temporary weighting arrays
01527         float* wy0 = new float[kbmax - kbmin + 1];
01528         float* wy = wy0 - kbmin; // wy[kbmin:kbmax]
01529         float* wx0 = new float[kbmax - kbmin + 1];
01530         float* wx = wx0 - kbmin;
01531         for (int i = kbmin; i <= kbmax; i++) {
01532                         int iyp = iyn + i;
01533                         wy[i] = kb.i0win_tab(nuynew - iyp);
01534                         int ixp = ixn + i;
01535                         wx[i] = kb.i0win_tab(nuxnew - ixp);
01536         }
01537         // restrict loops to non-zero elements
01538         int iymin = 0;
01539         for (int iy = kbmin; iy <= -1; iy++) {
01540                 if (wy[iy] != 0.f) {
01541                         iymin = iy;
01542                         break;
01543                 }
01544         }
01545         int iymax = 0;
01546         for (int iy = kbmax; iy >= 1; iy--) {
01547                 if (wy[iy] != 0.f) {
01548                         iymax = iy;
01549                         break;
01550                 }
01551         }
01552         int ixmin = 0;
01553         for (int ix = kbmin; ix <= -1; ix++) {
01554                 if (wx[ix] != 0.f) {
01555                         ixmin = ix;
01556                         break;
01557                 }
01558         }
01559         int ixmax = 0;
01560         for (int ix = kbmax; ix >= 1; ix--) {
01561                 if (wx[ix] != 0.f) {
01562                         ixmax = ix;
01563                         break;
01564                 }
01565         }
01566         float wsum = 0.0f;
01567         for (int iy = iymin; iy <= iymax; iy++)
01568                 for (int ix = ixmin; ix <= ixmax; ix++)
01569                         wsum += wx[ix]*wy[iy];
01570 
01571         complex<float> result(0.f,0.f);
01572         if ((ixn >= -kbmin) && (ixn <= nhalf-1-kbmax) && (iyn >= -nhalf-kbmin) && (iyn <= nhalf-1-kbmax)) {
01573                 // (xin,yin) not within window border from the edge
01574                 for (int iy = iymin; iy <= iymax; iy++) {
01575                         int iyp = iyn + iy;
01576                         for (int ix = ixmin; ix <= ixmax; ix++) {
01577                                 int ixp = ixn + ix;
01578                                 float w = wx[ix]*wy[iy];
01579                                 complex<float> val = fimage->cmplx(ixp,iyp);
01580                                 result += val*w;
01581                         }
01582                 }
01583         } else {
01584                 // points that "stick out"
01585                 for (int iy = iymin; iy <= iymax; iy++) {
01586                         int iyp = iyn + iy;
01587                         for (int ix = ixmin; ix <= ixmax; ix++) {
01588                                 int ixp = ixn + ix;
01589                                 bool mirror = false;
01590                                 int ixt= ixp, iyt= iyp;
01591                                 if (ixt < 0) {
01592                                         ixt = -ixt;
01593                                         iyt = -iyt;
01594                                         mirror = !mirror;
01595                                 }
01596                                 if (ixt > nhalf) {
01597                                         ixt = nxreal - ixt;
01598                                         iyt = -iyt;
01599                                         mirror = !mirror;
01600                                 }
01601                                 if (iyt > nhalf-1)  iyt -= nxreal;
01602                                 if (iyt < -nhalf)   iyt += nxreal;
01603                                 float w = wx[ix]*wy[iy];
01604                                 complex<float> val = fimage->cmplx(ixt,iyt);
01605                                 if (mirror)  result += conj(val)*w;
01606                                 else         result += val*w;
01607                         }
01608                 }
01609         }
01610         if (flip)  result = conj(result)/wsum;
01611         else result /= wsum;
01612         delete [] wx0;
01613         delete [] wy0;
01614         return result;
01615 }*/
01616 
01617 /*
01618 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01619 
01620         int nxreal = nx - 2;
01621         if (nxreal != ny)
01622                 throw ImageDimensionException("extractpoint requires ny == nx");
01623         int nhalf = nxreal/2;
01624         bool flip = false;
01625         if (nuxnew < 0.f) {
01626                 nuxnew *= -1;
01627                 nuynew *= -1;
01628                 flip = true;
01629         }
01630         if (nuynew >= nhalf-0.5)  {
01631                 nuynew -= nxreal;
01632         } else if (nuynew < -nhalf-0.5) {
01633                 nuynew += nxreal;
01634         }
01635 
01636         // put (xnew,ynew) on a grid.  The indices will be wrong for
01637         // the Fourier elements in the image, but the grid sizing will
01638         // be correct.
01639         int ixn = int(Util::round(nuxnew));
01640         int iyn = int(Util::round(nuynew));
01641 
01642         // set up some temporary weighting arrays
01643         static float wy[7];
01644         static float wx[7];
01645 
01646         float iynn = nuynew - iyn;
01647         wy[0] = kb.i0win_tab(iynn+3);
01648         wy[1] = kb.i0win_tab(iynn+2);
01649         wy[2] = kb.i0win_tab(iynn+1);
01650         wy[3] = kb.i0win_tab(iynn);
01651         wy[4] = kb.i0win_tab(iynn-1);
01652         wy[5] = kb.i0win_tab(iynn-2);
01653         wy[6] = kb.i0win_tab(iynn-3);
01654 
01655         float ixnn = nuxnew - ixn;
01656         wx[0] = kb.i0win_tab(ixnn+3);
01657         wx[1] = kb.i0win_tab(ixnn+2);
01658         wx[2] = kb.i0win_tab(ixnn+1);
01659         wx[3] = kb.i0win_tab(ixnn);
01660         wx[4] = kb.i0win_tab(ixnn-1);
01661         wx[5] = kb.i0win_tab(ixnn-2);
01662         wx[6] = kb.i0win_tab(ixnn-3);
01663 
01664         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]);
01665 
01666         complex<float> result(0.f,0.f);
01667         for (int iy = 0; iy < 7; iy++) {
01668                 int iyp = iyn + iy - 3 ;
01669                 for (int ix = 0; ix < 7; ix++) {
01670                         int ixp = ixn + ix - 3;
01671                         float w = wx[ix]*wy[iy];
01672                         complex<float> val = fimage->cmplx(ixp,iyp);
01673                         result += val*w;
01674                 }
01675         }
01676 
01677         if (flip)  result = conj(result)/wsum;
01678         else result /= wsum;
01679 
01680         return result;
01681 }*/
01682 
01683 
01684 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01685 
01686         int nxreal = nx - 2;
01687         if (nxreal != ny)
01688                 throw ImageDimensionException("extractpoint requires ny == nx");
01689         int nhalf = nxreal/2;
01690         bool flip = (nuxnew < 0.f);
01691         if (flip) {
01692                 nuxnew *= -1;
01693                 nuynew *= -1;
01694         }
01695         if (nuynew >= nhalf-0.5)  {
01696                 nuynew -= nxreal;
01697         } else if (nuynew < -nhalf-0.5) {
01698                 nuynew += nxreal;
01699         }
01700 
01701         // put (xnew,ynew) on a grid.  The indices will be wrong for
01702         // the Fourier elements in the image, but the grid sizing will
01703         // be correct.
01704         int ixn = int(Util::round(nuxnew));
01705         int iyn = int(Util::round(nuynew));
01706 
01707         // set up some temporary weighting arrays
01708         static float wy[7];
01709         static float wx[7];
01710 
01711         float iynn = nuynew - iyn;
01712         wy[0] = kb.i0win_tab(iynn+3);
01713         wy[1] = kb.i0win_tab(iynn+2);
01714         wy[2] = kb.i0win_tab(iynn+1);
01715         wy[3] = kb.i0win_tab(iynn);
01716         wy[4] = kb.i0win_tab(iynn-1);
01717         wy[5] = kb.i0win_tab(iynn-2);
01718         wy[6] = kb.i0win_tab(iynn-3);
01719 
01720         float ixnn = nuxnew - ixn;
01721         wx[0] = kb.i0win_tab(ixnn+3);
01722         wx[1] = kb.i0win_tab(ixnn+2);
01723         wx[2] = kb.i0win_tab(ixnn+1);
01724         wx[3] = kb.i0win_tab(ixnn);
01725         wx[4] = kb.i0win_tab(ixnn-1);
01726         wx[5] = kb.i0win_tab(ixnn-2);
01727         wx[6] = kb.i0win_tab(ixnn-3);
01728 
01729         float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01730 
01731         complex<float> result(0.f,0.f);
01732         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01733                 // (xin,yin) not within window border from the edge
01734                 for (int iy = 0; iy < 7; iy++) {
01735                         int iyp = iyn + iy - 3 ;
01736                         for (int ix = 0; ix < 7; ix++) {
01737                                 int ixp = ixn + ix - 3;
01738                                 float w = wx[ix]*wy[iy];
01739                                 complex<float> val = fimage->cmplx(ixp,iyp);
01740                                 result += val*w;
01741                         }
01742                 }
01743         } else {
01744                 // points that "stick out"
01745                 for (int iy = 0; iy < 7; iy++) {
01746                         int iyp = iyn + iy - 3;
01747                         for (int ix = 0; ix < 7; ix++) {
01748                                 int ixp = ixn + ix - 3;
01749                                 bool mirror = false;
01750                                 int ixt = ixp, iyt = iyp;
01751                                 if (ixt < 0) {
01752                                         ixt = -ixt;
01753                                         iyt = -iyt;
01754                                         mirror = !mirror;
01755                                 }
01756                                 if (ixt > nhalf) {
01757                                         ixt = nxreal - ixt;
01758                                         iyt = -iyt;
01759                                         mirror = !mirror;
01760                                 }
01761                                 if (iyt > nhalf-1)  iyt -= nxreal;
01762                                 if (iyt < -nhalf)   iyt += nxreal;
01763                                 float w = wx[ix]*wy[iy];
01764                                 complex<float> val = fimage->cmplx(ixt,iyt);
01765                                 if (mirror)  result += conj(val)*w;
01766                                 else         result += val*w;
01767                         }
01768                 }
01769         }
01770         if (flip)  result = conj(result)/wsum;
01771         else result /= wsum;
01772         return result;
01773 }
01774 
01775 /*
01776 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01777 
01778         int nxreal = nx - 2;
01779         if (nxreal != ny)
01780                 throw ImageDimensionException("extractpoint requires ny == nx");
01781         int nhalf = nxreal/2;
01782         bool flip = (nuxnew < 0.f);
01783         if (flip) {
01784                 nuxnew *= -1;
01785                 nuynew *= -1;
01786         }
01787         // put (xnew,ynew) on a grid.  The indices will be wrong for
01788         // the Fourier elements in the image, but the grid sizing will
01789         // be correct.
01790         int ixn = int(Util::round(nuxnew));
01791         int iyn = int(Util::round(nuynew));
01792         // set up some temporary weighting arrays
01793         static float wy[7];
01794         static float wx[7];
01795 
01796         float iynn = nuynew - iyn;
01797         wy[0] = kb.i0win_tab(iynn+3);
01798         wy[1] = kb.i0win_tab(iynn+2);
01799         wy[2] = kb.i0win_tab(iynn+1);
01800         wy[3] = kb.i0win_tab(iynn);
01801         wy[4] = kb.i0win_tab(iynn-1);
01802         wy[5] = kb.i0win_tab(iynn-2);
01803         wy[6] = kb.i0win_tab(iynn-3);
01804 
01805         float ixnn = nuxnew - ixn;
01806         wx[0] = kb.i0win_tab(ixnn+3);
01807         wx[1] = kb.i0win_tab(ixnn+2);
01808         wx[2] = kb.i0win_tab(ixnn+1);
01809         wx[3] = kb.i0win_tab(ixnn);
01810         wx[4] = kb.i0win_tab(ixnn-1);
01811         wx[5] = kb.i0win_tab(ixnn-2);
01812         wx[6] = kb.i0win_tab(ixnn-3);
01813 
01814         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]);
01815 
01816         complex<float> result(0.f,0.f);
01817 
01818         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01819                 // (xin,yin) not within window border from the edge
01820                 result = ( fimage->cmplx(ixn-3,iyn-3)*wx[0] +
01821                            fimage->cmplx(ixn-2,iyn-3)*wx[1] +
01822                            fimage->cmplx(ixn-1,iyn-3)*wx[2] +
01823                            fimage->cmplx(ixn+0,iyn-3)*wx[3] +
01824                            fimage->cmplx(ixn+1,iyn-3)*wx[4] +
01825                            fimage->cmplx(ixn+2,iyn-3)*wx[5] +
01826                            fimage->cmplx(ixn+3,iyn-3)*wx[6] )*wy[0] +
01827                            ( fimage->cmplx(ixn-3,iyn-2)*wx[0] +
01828                            fimage->cmplx(ixn-2,iyn-2)*wx[1] +
01829                            fimage->cmplx(ixn-1,iyn-2)*wx[2] +
01830                            fimage->cmplx(ixn+0,iyn-2)*wx[3] +
01831                            fimage->cmplx(ixn+1,iyn-2)*wx[4] +
01832                            fimage->cmplx(ixn+2,iyn-2)*wx[5] +
01833                            fimage->cmplx(ixn+3,iyn-2)*wx[6] )*wy[1] +
01834                            ( fimage->cmplx(ixn-3,iyn-1)*wx[0] +
01835                            fimage->cmplx(ixn-2,iyn-1)*wx[1] +
01836                            fimage->cmplx(ixn-1,iyn-1)*wx[2] +
01837                            fimage->cmplx(ixn+0,iyn-1)*wx[3] +
01838                            fimage->cmplx(ixn+1,iyn-1)*wx[4] +
01839                            fimage->cmplx(ixn+2,iyn-1)*wx[5] +
01840                            fimage->cmplx(ixn+3,iyn-1)*wx[6] )*wy[2] +
01841                            ( fimage->cmplx(ixn-3,iyn+0)*wx[0] +
01842                            fimage->cmplx(ixn-2,iyn+0)*wx[1] +
01843                            fimage->cmplx(ixn-1,iyn+0)*wx[2] +
01844                            fimage->cmplx(ixn+0,iyn+0)*wx[3] +
01845                            fimage->cmplx(ixn+1,iyn+0)*wx[4] +
01846                            fimage->cmplx(ixn+2,iyn+0)*wx[5] +
01847                            fimage->cmplx(ixn+3,iyn+0)*wx[6] )*wy[3] +
01848                            ( fimage->cmplx(ixn-3,iyn+1)*wx[0] +
01849                            fimage->cmplx(ixn-2,iyn+1)*wx[1] +
01850                            fimage->cmplx(ixn-1,iyn+1)*wx[2] +
01851                            fimage->cmplx(ixn+0,iyn+1)*wx[3] +
01852                            fimage->cmplx(ixn+1,iyn+1)*wx[4] +
01853                            fimage->cmplx(ixn+2,iyn+1)*wx[5] +
01854                            fimage->cmplx(ixn+3,iyn+1)*wx[6] )*wy[4] +
01855                            ( fimage->cmplx(ixn-3,iyn+2)*wx[0] +
01856                            fimage->cmplx(ixn-2,iyn+2)*wx[1] +
01857                            fimage->cmplx(ixn-1,iyn+2)*wx[2] +
01858                            fimage->cmplx(ixn+0,iyn+2)*wx[3] +
01859                            fimage->cmplx(ixn+1,iyn+2)*wx[4] +
01860                            fimage->cmplx(ixn+2,iyn+2)*wx[5] +
01861                            fimage->cmplx(ixn+3,iyn+2)*wx[6] )*wy[5] +
01862                            ( fimage->cmplx(ixn-3,iyn+3)*wx[0] +
01863                            fimage->cmplx(ixn-2,iyn+3)*wx[1] +
01864                            fimage->cmplx(ixn-1,iyn+3)*wx[2] +
01865                            fimage->cmplx(ixn+0,iyn+3)*wx[3] +
01866                            fimage->cmplx(ixn+1,iyn+3)*wx[4] +
01867                            fimage->cmplx(ixn+2,iyn+3)*wx[5] +
01868                            fimage->cmplx(ixn+3,iyn+3)*wx[6] )*wy[6];
01869 
01870         } else {
01871                 // points that "stick out"
01872                 for (int iy = 0; iy < 7; iy++) {
01873                         int iyp = iyn + iy - 3;
01874                         for (int ix = 0; ix < 7; ix++) {
01875                                 int ixp = ixn + ix - 3;
01876                                 bool mirror = false;
01877                                 int ixt= ixp, iyt= iyp;
01878                                 if (ixt < 0) {
01879                                         ixt = -ixt;
01880                                         iyt = -iyt;
01881                                         mirror = !mirror;
01882                                 }
01883                                 if (ixt > nhalf) {
01884                                         ixt = nxreal - ixt;
01885                                         iyt = -iyt;
01886                                         mirror = !mirror;
01887                                 }
01888                                 if (iyt > nhalf-1)  iyt -= nxreal;
01889                                 if (iyt < -nhalf)   iyt += nxreal;
01890                                 float w = wx[ix]*wy[iy];
01891                                 complex<float> val = fimage->cmplx(ixt,iyt);
01892                                 if (mirror)  result += conj(val)*w;
01893                                 else         result += val*w;
01894                         }
01895                 }
01896         }
01897         if (flip)  result = conj(result)/wsum;
01898         else result /= wsum;
01899         return result;
01900 }*/
01901 
01902 
01903 float Util::triquad(float R, float S, float T, float* fdata)
01904 {
01905 
01906     const float C2 = 0.5f;    //1.0 / 2.0;
01907     const float C4 = 0.25f;   //1.0 / 4.0;
01908     const float C8 = 0.125f;  //1.0 / 8.0;
01909 
01910     float  RS   = R * S;
01911     float  ST   = S * T;
01912     float  RT   = R * T;
01913     float  RST  = R * ST;
01914 
01915     float  RSQ  = 1-R*R;
01916     float  SSQ  = 1-S*S;
01917     float  TSQ  = 1-T*T;
01918 
01919     float  RM1  = (1-R);
01920     float  SM1  = (1-S);
01921     float  TM1  = (1-T);
01922 
01923     float  RP1  = (1+R);
01924     float  SP1  = (1+S);
01925     float  TP1  = (1+T);
01926 
01927     float triquad =
01928     (-C8) * RST * RM1  * SM1  * TM1 * fdata[0] +
01929         ( C4) * ST  * RSQ  * SM1  * TM1 * fdata[1] +
01930         ( C8) * RST * RP1  * SM1  * TM1 * fdata[2] +
01931         ( C4) * RT  * RM1  * SSQ  * TM1 * fdata[3] +
01932         (-C2) * T   * RSQ  * SSQ  * TM1 * fdata[4] +
01933         (-C4) * RT  * RP1  * SSQ  * TM1 * fdata[5] +
01934         ( C8) * RST * RM1  * SP1  * TM1 * fdata[6] +
01935         (-C4) * ST  * RSQ  * SP1  * TM1 * fdata[7] +
01936         (-C8) * RST * RP1  * SP1  * TM1 * fdata[8] +
01937 //
01938         ( C4) * RS  * RM1  * SM1  * TSQ * fdata[9]  +
01939         (-C2) * S   * RSQ  * SM1  * TSQ * fdata[10] +
01940         (-C4) * RS  * RP1  * SM1  * TSQ * fdata[11] +
01941         (-C2) * R   * RM1  * SSQ  * TSQ * fdata[12] +
01942                       RSQ  * SSQ  * TSQ * fdata[13] +
01943         ( C2) * R   * RP1  * SSQ  * TSQ * fdata[14] +
01944         (-C4) * RS  * RM1  * SP1  * TSQ * fdata[15] +
01945         ( C2) * S   * RSQ  * SP1  * TSQ * fdata[16] +
01946         ( C4) * RS  * RP1  * SP1  * TSQ * fdata[17] +
01947  //
01948         ( C8) * RST * RM1  * SM1  * TP1 * fdata[18] +
01949         (-C4) * ST  * RSQ  * SM1  * TP1 * fdata[19] +
01950         (-C8) * RST * RP1  * SM1  * TP1 * fdata[20] +
01951         (-C4) * RT  * RM1  * SSQ  * TP1 * fdata[21] +
01952         ( C2) * T   * RSQ  * SSQ  * TP1 * fdata[22] +
01953         ( C4) * RT  * RP1  * SSQ  * TP1 * fdata[23] +
01954         (-C8) * RST * RM1  * SP1  * TP1 * fdata[24] +
01955         ( C4) * ST  * RSQ  * SP1  * TP1 * fdata[25] +
01956         ( C8) * RST * RP1  * SP1  * TP1 * fdata[26]   ;
01957      return triquad;
01958 }
01959 
01960 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01961                 : M(M_), fc(fc_), ntable(ntable_) {
01962         // Sinc-Blackman kernel
01963         build_sBtable();
01964 }
01965 
01966 void Util::sincBlackman::build_sBtable() {
01967         sBtable.resize(ntable+1);
01968         int ltab = int(round(float(ntable)/1.25f));
01969         int M2 = M/2;
01970         fltb = float(ltab)/M2;
01971         for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01972         float x = 1.0e-7f;
01973         sBtable[0] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01974         for (int i=1; i <= ltab; i++) {
01975                 x = float(i)/fltb;
01976                 sBtable[i] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01977                 //cout << "  "<<x<<"  "<<sBtable[i] <<endl;
01978         }
01979 }
01980 
01981 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01982                                          int N_, float vtable_, int ntable_)
01983                 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01984                   ntable(ntable_) {
01985         // Default values are alpha=1.25, K=6, r=0.5, v = K/2
01986         if (0.f == v) v = float(K)/2;
01987         if (0.f == vtable) vtable = v;
01988         alphar = alpha*r;
01989         fac = static_cast<float>(twopi)*alphar*v;
01990         vadjust = 1.0f*v;
01991         facadj = static_cast<float>(twopi)*alphar*vadjust;
01992         build_I0table();
01993 }
01994 
01995 float Util::KaiserBessel::i0win(float x) const {
01996         float val0 = float(gsl_sf_bessel_I0(facadj));
01997         float absx = fabs(x);
01998         if (absx > vadjust) return 0.f;
01999         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02000         float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02001         return res;
02002 }
02003 
02004 void Util::KaiserBessel::build_I0table() {
02005         i0table.resize(ntable+1); // i0table[0:ntable]
02006         int ltab = int(round(float(ntable)/1.25f));
02007         fltb = float(ltab)/(K/2);
02008         float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02009         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02010         for (int i=0; i <= ltab; i++) {
02011                 float s = float(i)/fltb/N;
02012                 if (s < vadjust) {
02013                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02014                         i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02015                 } else {
02016                         i0table[i] = 0.f;
02017                 }
02018 //              cout << "  "<<s*N<<"  "<<i0table[i] <<endl;
02019         }
02020 }
02021 
02022 float Util::KaiserBessel::I0table_maxerror() {
02023         float maxdiff = 0.f;
02024         for (int i = 1; i <= ntable; i++) {
02025                 float diff = fabs(i0table[i] - i0table[i-1]);
02026                 if (diff > maxdiff) maxdiff = diff;
02027         }
02028         return maxdiff;
02029 }
02030 
02031 float Util::KaiserBessel::sinhwin(float x) const {
02032         float val0 = sinh(fac)/fac;
02033         float absx = fabs(x);
02034         if (0.0 == x) {
02035                 float res = 1.0f;
02036                 return res;
02037         } else if (absx == alphar) {
02038                 return 1.0f/val0;
02039         } else if (absx < alphar) {
02040                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02041                 float facrt = fac*rt;
02042                 float res = (sinh(facrt)/facrt)/val0;
02043                 return res;
02044         } else {
02045                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02046                 float facrt = fac*rt;
02047                 float res = (sin(facrt)/facrt)/val0;
02048                 return res;
02049         }
02050 }
02051 
02052 float Util::FakeKaiserBessel::i0win(float x) const {
02053         float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02054         float absx = fabs(x);
02055         if (absx > vadjust) return 0.f;
02056         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02057         float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02058         return res;
02059 }
02060 
02061 void Util::FakeKaiserBessel::build_I0table() {
02062         i0table.resize(ntable+1); // i0table[0:ntable]
02063         int ltab = int(round(float(ntable)/1.1f));
02064         fltb = float(ltab)/(K/2);
02065         float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02066         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02067         for (int i=0; i <= ltab; i++) {
02068                 float s = float(i)/fltb/N;
02069                 if (s < vadjust) {
02070                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02071                         i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02072                 } else {
02073                         i0table[i] = 0.f;
02074                 }
02075         }
02076 }
02077 
02078 float Util::FakeKaiserBessel::sinhwin(float x) const {
02079         float val0 = sinh(fac)/fac;
02080         float absx = fabs(x);
02081         if (0.0 == x) {
02082                 float res = 1.0f;
02083                 return res;
02084         } else if (absx == alphar) {
02085                 return 1.0f/val0;
02086         } else if (absx < alphar) {
02087                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02088                 float facrt = fac*rt;
02089                 float res = (sinh(facrt)/facrt)/val0;
02090                 return res;
02091         } else {
02092                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02093                 float facrt = fac*rt;
02094                 float res = (sin(facrt)/facrt)/val0;
02095                 return res;
02096         }
02097 }
02098 
02099 #if 0 // 1-st order KB window
02100 float Util::FakeKaiserBessel::sinhwin(float x) const {
02101         //float val0 = sinh(fac)/fac;
02102         float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02103         float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02104         float absx = fabs(x);
02105         if (0.0 == x) {
02106                 //float res = 1.0f;
02107                 float res = val0;
02108                 return res;
02109         } else if (absx == alphar) {
02110                 //return 1.0f/val0;
02111                 return prefix;
02112         } else if (absx < alphar) {
02113                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02114                 //float facrt = fac*rt;
02115                 float facrt = facadj*rt;
02116                 //float res = (sinh(facrt)/facrt)/val0;
02117                 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02118                 return res;
02119         } else {
02120                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02121                 //float facrt = fac*rt;
02122                 float facrt = facadj*rt;
02123                 //float res = (sin(facrt)/facrt)/val0;
02124                 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02125                 return res;
02126         }
02127 }
02128 #endif // 0
02129 
02130 
02131 
02132 #define  circ(i)         circ[i-1]
02133 #define  numr(i,j)       numr[(j-1)*3 + i-1]
02134 #define  xim(i,j)        xim[(j-1)*nsam + i-1]
02135 
02136 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02137         int nsam = image->get_xsize();
02138         int nrow = image->get_ysize();
02139         int nring = numr.size()/3;
02140         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02141         EMData* out = new EMData();
02142         out->set_size(lcirc,1,1);
02143         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02144         float *xim  = image->get_data();
02145         float *circ = out->get_data();
02146 /*   alrq(image->get_data(), nsam, nrow, &numr[0], out->get_data(), lcirc, nring, cmode);
02147    return out;
02148 }
02149 void Util::alrq(float *xim,  int nsam , int nrow , int *numr,
02150           float *circ, int lcirc, int nring, char mode)
02151 {*/
02152 /*
02153 c
02154 c  purpose:
02155 c
02156 c  resmaple to polar coordinates
02157 c
02158 */
02159         //  dimension         xim(nsam,nrow),circ(lcirc)
02160         //  integer           numr(3,nring)
02161 
02162         double dfi, dpi;
02163         int    ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02164         float  yq, xold, yold, fi, x, y;
02165 
02166         ns2 = nsam/2+1;
02167         nr2 = nrow/2+1;
02168         dpi = 2.0*atan(1.0);
02169 
02170         for (i=1;i<=nring;i++) {
02171                 // radius of the ring
02172                 inr = numr(1,i);
02173                 yq  = static_cast<float>(inr);
02174                 l   = numr(3,i);
02175                 if (mode == 'h' || mode == 'H')  lt = l/2;
02176                 else                             lt = l/4;
02177 
02178                 nsim           = lt-1;
02179                 dfi            = dpi/(nsim+1);
02180                 kcirc          = numr(2,i);
02181                 xold           = 0.0f;
02182                 yold           = static_cast<float>(inr);
02183                 circ(kcirc)    = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02184                 xold           = static_cast<float>(inr);
02185                 yold           = 0.0f;
02186                 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02187 
02188                 if (mode == 'f' || mode == 'F') {
02189                         xold              = 0.0f;
02190                         yold              = static_cast<float>(-inr);
02191                         circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02192                         xold              = static_cast<float>(-inr);
02193                         yold              = 0.0f;
02194                         circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02195                 }
02196 
02197                 for (j=1;j<=nsim;j++) {
02198                         fi               = static_cast<float>(dfi*j);
02199                         x                = sin(fi)*yq;
02200                         y                = cos(fi)*yq;
02201                         xold             = x;
02202                         yold             = y;
02203                         circ(j+kcirc)    = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02204                         xold             =  y;
02205                         yold             = -x;
02206                         circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02207 
02208                         if (mode == 'f' || mode == 'F')  {
02209                                 xold                = -x;
02210                                 yold                = -y;
02211                                 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02212                                 xold                = -y;
02213                                 yold                =  x;
02214                                 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02215                         }
02216                 }
02217         }
02218         return  out;
02219 }
02220 
02221 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02222         int nsam = image->get_xsize();
02223         int nrow = image->get_ysize();
02224         int nring = numr.size()/3;
02225         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02226         EMData* out = new EMData();
02227         out->set_size(lcirc,1,1);
02228         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02229         float *xim  = image->get_data();
02230         float *circ = out->get_data();
02231         double dpi, dfi;
02232         int    it, jt, inr, l, nsim, kcirc, lt;
02233         float  xold, yold, fi, x, y;
02234 
02235         //     cns2 and cnr2 are predefined centers
02236         //     no need to set to zero, all elements are defined
02237         dpi = 2*atan(1.0);
02238         for (it=1; it<=nring; it++) {
02239                 // radius of the ring
02240                 inr = numr(1,it);
02241 
02242                 // "F" means a full circle interpolation
02243                 // "H" means a half circle interpolation
02244 
02245                 l = numr(3,it);
02246                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02247                 else                              lt = l / 4;
02248 
02249                 nsim  = lt - 1;
02250                 dfi   = dpi / (nsim+1);
02251                 kcirc = numr(2,it);
02252                 xold  = 0.0f+cns2;
02253                 yold  = inr+cnr2;
02254 
02255                 Assert( kcirc <= lcirc );
02256                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on 90 degree
02257 
02258                 xold  = inr+cns2;
02259                 yold  = 0.0f+cnr2;
02260                 Assert( lt+kcirc <= lcirc );
02261                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 0 degree
02262 
02263                 if ( mode == 'f' || mode == 'F' ) {
02264                         xold = 0.0f+cns2;
02265                         yold = -inr+cnr2;
02266                         Assert( lt+lt+kcirc <= lcirc );
02267                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 270 degree
02268 
02269                         xold = -inr+cns2;
02270                         yold = 0.0f+cnr2;
02271                         Assert(lt+lt+lt+kcirc <= lcirc );
02272                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on 180 degree
02273                 }
02274 
02275                 for (jt=1; jt<=nsim; jt++) {
02276                         fi   = static_cast<float>(dfi * jt);
02277                         x    = sin(fi) * inr;
02278                         y    = cos(fi) * inr;
02279 
02280                         xold = x+cns2;
02281                         yold = y+cnr2;
02282 
02283                         Assert( jt+kcirc <= lcirc );
02284                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);      // Sampling on the first quadrant
02285 
02286                         xold = y+cns2;
02287                         yold = -x+cnr2;
02288 
02289                         Assert( jt+lt+kcirc <= lcirc );
02290                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on the fourth quadrant
02291 
02292                         if ( mode == 'f' || mode == 'F' ) {
02293                                 xold = -x+cns2;
02294                                 yold = -y+cnr2;
02295 
02296                                 Assert( jt+lt+lt+kcirc <= lcirc );
02297                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on the third quadrant
02298 
02299                                 xold = -y+cns2;
02300                                 yold = x+cnr2;
02301 
02302                                 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02303                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on the second quadrant
02304                         }
02305                 } // end for jt
02306         } //end for it
02307         return out;
02308 }
02309 
02310 float Util::bilinear(float xold, float yold, int nsam, int, float* xim)
02311 {
02312 /*
02313 c  purpose: linear interpolation
02314   Optimized for speed, circular closer removed, checking of ranges removed
02315 */
02316     float bilinear;
02317     int   ixold, iyold;
02318 
02319 /*
02320         float xdif, ydif, xrem, yrem;
02321         ixold   = (int) floor(xold);
02322         iyold   = (int) floor(yold);
02323         ydif = yold - iyold;
02324         yrem = 1.0f - ydif;
02325 
02326         //  May want to insert if?
02327 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02328 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02329 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02330         xdif = xold - ixold;
02331         xrem = 1.0f- xdif;
02332 //                 RBUF(K) = YDIF*(BUF(NADDR+NSAM)*XREM
02333 //     &                    +BUF(NADDR+NSAM+1)*XDIF)
02334 //     &                    +YREM*(BUF(NADDR)*XREM + BUF(NADDR+1)*XDIF)
02335         bilinear = ydif*(xim(ixold,iyold+1)*xrem + xim(ixold+1,iyold+1)*xdif) +
02336                                         yrem*(xim(ixold,iyold)*xrem+xim(ixold+1,iyold)*xdif);
02337 
02338     return bilinear;
02339 }
02340 */
02341         float xdif, ydif;
02342 
02343         ixold   = (int) xold;
02344         iyold   = (int) yold;
02345         ydif = yold - iyold;
02346 
02347         //  May want to insert it?
02348 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02349 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02350 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02351         xdif = xold - ixold;
02352         bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02353                    xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02354                            ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02355 
02356         return bilinear;
02357 }
02358 
02359 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02360              int  *numr, float *circ, int , int  nring, char  mode) {
02361         double dpi, dfi;
02362         int    it, jt, inr, l, nsim, kcirc, lt;
02363         float   xold, yold, fi, x, y;
02364 
02365         //     cns2 and cnr2 are predefined centers
02366         //     no need to set to zero, all elements are defined
02367 
02368         dpi = 2*atan(1.0);
02369         for (it=1; it<=nring; it++) {
02370                 // radius of the ring
02371                 inr = numr(1,it);
02372 
02373                 l = numr(3,it);
02374                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02375                 else                              lt = l / 4;
02376 
02377                 nsim  = lt - 1;
02378                 dfi   = dpi / (nsim+1);
02379                 kcirc = numr(2,it);
02380 
02381 
02382                 xold  = 0.0f+cns2;
02383                 yold  = inr+cnr2;
02384 
02385                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02386 
02387                 xold  = inr+cns2;
02388                 yold  = 0.0f+cnr2;
02389                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02390 
02391                 if ( mode == 'f' || mode == 'F' ) {
02392                         xold = 0.0f+cns2;
02393                         yold = -inr+cnr2;
02394                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02395 
02396                         xold = -inr+cns2;
02397                         yold = 0.0f+cnr2;
02398                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02399                 }
02400 
02401                 for (jt=1; jt<=nsim; jt++) {
02402                         fi   = static_cast<float>(dfi * jt);
02403                         x    = sin(fi) * inr;
02404                         y    = cos(fi) * inr;
02405 
02406                         xold = x+cns2;
02407                         yold = y+cnr2;
02408                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02409 
02410                         xold = y+cns2;
02411                         yold = -x+cnr2;
02412                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02413 
02414                         if ( mode == 'f' || mode == 'F' ) {
02415                                 xold = -x+cns2;
02416                                 yold = -y+cnr2;
02417                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02418 
02419                                 xold = -y+cns2;
02420                                 yold = x+cnr2;
02421                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02422                         }
02423                 } // end for jt
02424         } //end for it
02425 }
02426 /*
02427 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02428              int  *numr, float *circ, int lcirc, int  nring, char  mode)
02429 {
02430    double dpi, dfi;
02431    int    it, jt, inr, l, nsim, kcirc, lt, xold, yold;
02432    float  yq, fi, x, y;
02433 
02434    //     cns2 and cnr2 are predefined centers
02435    //     no need to set to zero, all elements are defined
02436 
02437    dpi = 2*atan(1.0);
02438    for (it=1; it<=nring; it++) {
02439       // radius of the ring
02440       inr = numr(1,it);
02441       yq  = inr;
02442 
02443       l = numr(3,it);
02444       if ( mode == 'h' || mode == 'H' ) {
02445          lt = l / 2;
02446       }
02447       else { // if ( mode == 'f' || mode == 'F' )
02448          lt = l / 4;
02449       }
02450 
02451       nsim  = lt - 1;
02452       dfi   = dpi / (nsim+1);
02453       kcirc = numr(2,it);
02454 
02455 
02456         xold = (int) (0.0+cns2);
02457         yold = (int) (inr+cnr2);
02458 
02459         circ(kcirc) = xim(xold, yold);
02460 
02461       xold = (int) (inr+cns2);
02462       yold = (int) (0.0+cnr2);
02463       circ(lt+kcirc) = xim(xold, yold);
02464 
02465       if ( mode == 'f' || mode == 'F' ) {
02466          xold  = (int) (0.0+cns2);
02467          yold = (int) (-inr+cnr2);
02468          circ(lt+lt+kcirc) = xim(xold, yold);
02469 
02470          xold  = (int) (-inr+cns2);
02471          yold = (int) (0.0+cnr2);
02472          circ(lt+lt+lt+kcirc) = xim(xold, yold);
02473       }
02474 
02475       for (jt=1; jt<=nsim; jt++) {
02476          fi   = dfi * jt;
02477          x    = sin(fi) * yq;
02478          y    = cos(fi) * yq;
02479 
02480          xold  = (int) (x+cns2);
02481          yold = (int) (y+cnr2);
02482          circ(jt+kcirc) = xim(xold, yold);
02483 
02484          xold  = (int) (y+cns2);
02485          yold = (int) (-x+cnr2);
02486          circ(jt+lt+kcirc) = xim(xold, yold);
02487 
02488          if ( mode == 'f' || mode == 'F' ) {
02489             xold  = (int) (-x+cns2);
02490             yold = (int) (-y+cnr2);
02491             circ(jt+lt+lt+kcirc) = xim(xold, yold);
02492 
02493             xold  = (int) (-y+cns2);
02494             yold = (int) (x+cnr2);
02495             circ(jt+lt+lt+lt+kcirc) = xim(xold, yold);
02496          }
02497       } // end for jt
02498    } //end for it
02499 }
02500 */
02501 //xim((int) floor(xold), (int) floor(yold))
02502 #undef  xim
02503 
02504 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02505 // input image is twice the size of the original image
02506         int nring = numr.size()/3;
02507         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02508         EMData* out = new EMData();
02509         out->set_size(lcirc,1,1);
02510         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02511         float *circ = out->get_data();
02512         float *fimage = image->get_data();
02513         int nx = image->get_xsize();
02514         int ny = image->get_ysize();
02515         int nz = image->get_zsize();
02516         double dpi, dfi;
02517         int    it, jt, inr, l, nsim, kcirc, lt;
02518         float  yq, xold, yold, fi, x, y;
02519 
02520         //     cns2 and cnr2 are predefined centers
02521         //     no need to set to zero, all elements are defined
02522 
02523         dpi = 2*atan(1.0);
02524         for (it=1;it<=nring;it++) {
02525                 // radius of the ring
02526                 inr = numr(1,it);
02527                 yq  = static_cast<float>(inr);
02528 
02529                 l = numr(3,it);
02530                 if ( mode == 'h' || mode == 'H' )  lt = l / 2;
02531                 else                               lt = l / 4;
02532 
02533                 nsim  = lt - 1;
02534                 dfi   = dpi / (nsim+1);
02535                 kcirc = numr(2,it);
02536                 xold  = 0.0f;
02537                 yold  = static_cast<float>(inr);
02538                 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02539 //      circ(kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02540 
02541                 xold  = static_cast<float>(inr);
02542                 yold  = 0.0f;
02543                 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02544 //      circ(lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02545 
02546         if ( mode == 'f' || mode == 'F' ) {
02547                 xold = 0.0f;
02548                 yold = static_cast<float>(-inr);
02549                 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02550 //         circ(lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02551 
02552                 xold = static_cast<float>(-inr);
02553                 yold = 0.0f;
02554                 circ(lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02555 //         circ(lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02556         }
02557 
02558         for (jt=1;jt<=nsim;jt++) {
02559                 fi   = static_cast<float>(dfi * jt);
02560                 x    = sin(fi) * yq;
02561                 y    = cos(fi) * yq;
02562 
02563                 xold = x;
02564                 yold = y;
02565                 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02566 //         circ(jt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02567 
02568                 xold = y;
02569                 yold = -x;
02570                 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02571 //         circ(jt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02572 
02573         if ( mode == 'f' || mode == 'F' ) {
02574                 xold = -x;
02575                 yold = -y;
02576                 circ(jt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02577 //            circ(jt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02578 
02579                 xold = -y;
02580                 yold = x;
02581                 circ(jt+lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02582 //            circ(jt+lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02583         }
02584         } // end for jt
02585         } //end for it
02586         return  out;
02587 }
02588 
02589 /*
02590 
02591         A set of 1-D power-of-two FFTs
02592         Pawel & Chao 01/20/06
02593 
02594 fftr_q(xcmplx,nv)
02595   single precision
02596 
02597  dimension xcmplx(2,iabs(nv)/2);
02598  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02599  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02600 
02601 
02602 fftr_d(xcmplx,nv)
02603   double precision
02604 
02605  dimension xcmplx(2,iabs(nv)/2);
02606  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02607  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02608 
02609 
02610 
02611 */
02612 #define  tab1(i)      tab1[i-1]
02613 #define  xcmplx(i,j)  xcmplx [(j-1)*2 + i-1]
02614 #define  br(i)        br[i-1]
02615 #define  bi(i)        bi[i-1]
02616 //-----------------------------------------
02617 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02618 {
02619         double rni,sgn,tr1,tr2,ti1,ti2;
02620         double cc,c,ss,s,t,x2,x3,x4,x5;
02621         int    b3,b4,b5,b6,b7,b56;
02622         int    n, k, l, j, i, ix0, ix1, status=0;
02623 
02624         const double tab1[] = {
02625                 9.58737990959775e-5,
02626                 1.91747597310703e-4,
02627                 3.83495187571395e-4,
02628                 7.66990318742704e-4,
02629                 1.53398018628476e-3,
02630                 3.06795676296598e-3,
02631                 6.13588464915449e-3,
02632                 1.22715382857199e-2,
02633                 2.45412285229123e-2,
02634                 4.90676743274181e-2,
02635                 9.80171403295604e-2,
02636                 1.95090322016128e-1,
02637                 3.82683432365090e-1,
02638                 7.07106781186546e-1,
02639                 1.00000000000000,
02640         };
02641 
02642         n=(int)pow(2.0f,ln);
02643 
02644         k=abs(ks);
02645         l=16-ln;
02646         b3=n*k;
02647         b6=b3;
02648         b7=k;
02649         if (ks > 0) {
02650                 sgn=1.0f;
02651         } else {
02652                 sgn=-1.0f;
02653                 rni=1.0f/(float)(n);
02654                 j=1;
02655                 for (i=1; i<=n; i++) {
02656                         br(j)=br(j)*rni;
02657                         bi(j)=bi(j)*rni;
02658                         j=j+k;
02659                 }
02660         }
02661 
02662 L12:
02663    b6=b6/2;
02664    b5=b6;
02665    b4=2*b6;
02666    b56=b5-b6;
02667 
02668 L14:
02669    tr1=br(b5+1);
02670    ti1=bi(b5+1);
02671    tr2=br(b56+1);
02672    ti2=bi(b56+1);
02673 
02674    br(b5+1)=tr2-tr1;
02675    bi(b5+1)=ti2-ti1;
02676    br(b56+1)=tr1+tr2;
02677    bi(b56+1)=ti1+ti2;
02678 
02679    b5=b5+b4;
02680    b56=b5-b6;
02681    if ( b5 <= b3 )  goto  L14;
02682    if ( b6 == b7 )  goto  L20;
02683 
02684    b4=b7;
02685    cc=2.0f*pow(tab1(l),2);
02686    c=1.0f-cc;
02687    l++;
02688    ss=sgn*tab1(l);
02689    s=ss;
02690 
02691 L16:
02692    b5=b6+b4;
02693    b4=2*b6;
02694    b56=b5-b6;
02695 
02696 L18:
02697    tr1=br(b5+1);
02698    ti1=bi(b5+1);
02699    tr2=br(b56+1);
02700    ti2=bi(b56+1);
02701    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02702    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02703    br(b56+1)=tr1+tr2;
02704    bi(b56+1)=ti1+ti2;
02705 
02706    b5=b5+b4;
02707    b56=b5-b6;
02708    if ( b5 <= b3 )  goto  L18;
02709    b4=b5-b6;
02710    b5=b4-b3;
02711    c=-c;
02712    b4=b6-b5;
02713    if ( b5 < b4 )  goto  L16;
02714    b4=b4+b7;
02715    if ( b4 >= b5 ) goto  L12;
02716 
02717    t=c-cc*c-ss*s;
02718    s=s+ss*c-cc*s;
02719    c=t;
02720    goto  L16;
02721 
02722 L20:
02723    ix0=b3/2;
02724    b3=b3-b7;
02725    b4=0;
02726    b5=0;
02727    b6=ix0;
02728    ix1=0;
02729    if (b6 == b7) goto EXIT;
02730 
02731 L22:
02732    b4=b3-b4;
02733    b5=b3-b5;
02734    x2=br(b4+1);
02735    x3=br(b5+1);
02736    x4=bi(b4+1);
02737    x5=bi(b5+1);
02738    br(b4+1)=x3;
02739    br(b5+1)=x2;
02740    bi(b4+1)=x5;
02741    bi(b5+1)=x4;
02742    if(b6 < b4)  goto  L22;
02743 
02744 L24:
02745    b4=b4+b7;
02746    b5=b6+b5;
02747    x2=br(b4+1);
02748    x3=br(b5+1);
02749    x4=bi(b4+1);
02750    x5=bi(b5+1);
02751    br(b4+1)=x3;
02752    br(b5+1)=x2;
02753    bi(b4+1)=x5;
02754    bi(b5+1)=x4;
02755    ix0=b6;
02756 
02757 L26:
02758    ix0=ix0/2;
02759    ix1=ix1-ix0;
02760    if( ix1 >= 0)  goto L26;
02761 
02762    ix0=2*ix0;
02763    b4=b4+b7;
02764    ix1=ix1+ix0;
02765    b5=ix1;
02766    if ( b5 >= b4)  goto  L22;
02767    if ( b4 < b6)   goto  L24;
02768 
02769 EXIT:
02770    status = 0;
02771 }
02772 
02773 // -----------------------------------------------------------------
02774 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02775 {
02776         //  dimension  br(1),bi(1)
02777 
02778         int b3,b4,b5,b6,b7,b56;
02779         int n, k, l, j, i, ix0, ix1;
02780         float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02781         int status=0;
02782 
02783         const float tab1[] = {
02784                 9.58737990959775e-5f,
02785                 1.91747597310703e-4f,
02786                 3.83495187571395e-4f,
02787                 7.66990318742704e-4f,
02788                 1.53398018628476e-3f,
02789                 3.06795676296598e-3f,
02790                 6.13588464915449e-3f,
02791                 1.22715382857199e-2f,
02792                 2.45412285229123e-2f,
02793                 4.90676743274181e-2f,
02794                 9.80171403295604e-2f,
02795                 1.95090322016128e-1f,
02796                 3.82683432365090e-1f,
02797                 7.07106781186546e-1f,
02798                 1.00000000000000f,
02799         };
02800 
02801         n=(int)pow(2.0f,ln);
02802 
02803         k=abs(ks);
02804         l=16-ln;
02805         b3=n*k;
02806         b6=b3;
02807         b7=k;
02808         if( ks > 0 ) {
02809                 sgn=1.0f;
02810         } else {
02811                 sgn=-1.0f;
02812                 rni=1.0f/(float)n;
02813                 j=1;
02814                 for (i=1; i<=n; i++) {
02815                         br(j)=br(j)*rni;
02816                         bi(j)=bi(j)*rni;
02817                         j=j+k;
02818                 }
02819         }
02820 L12:
02821    b6=b6/2;
02822    b5=b6;
02823    b4=2*b6;
02824    b56=b5-b6;
02825 L14:
02826    tr1=br(b5+1);
02827    ti1=bi(b5+1);
02828 
02829    tr2=br(b56+1);
02830    ti2=bi(b56+1);
02831 
02832    br(b5+1)=tr2-tr1;
02833    bi(b5+1)=ti2-ti1;
02834    br(b56+1)=tr1+tr2;
02835    bi(b56+1)=ti1+ti2;
02836 
02837    b5=b5+b4;
02838    b56=b5-b6;
02839    if ( b5 <= b3 )  goto  L14;
02840    if ( b6 == b7 )  goto  L20;
02841 
02842    b4=b7;
02843    cc=2.0f*pow(tab1(l),2);
02844    c=1.0f-cc;
02845    l++;
02846    ss=sgn*tab1(l);
02847    s=ss;
02848 L16:
02849    b5=b6+b4;
02850    b4=2*b6;
02851    b56=b5-b6;
02852 L18:
02853    tr1=br(b5+1);
02854    ti1=bi(b5+1);
02855    tr2=br(b56+1);
02856    ti2=bi(b56+1);
02857    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02858    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02859    br(b56+1)=tr1+tr2;
02860    bi(b56+1)=ti1+ti2;
02861 
02862    b5=b5+b4;
02863    b56=b5-b6;
02864    if(b5 <= b3)  goto L18;
02865    b4=b5-b6;
02866    b5=b4-b3;
02867    c=-c;
02868    b4=b6-b5;
02869    if(b5 < b4)  goto  L16;
02870    b4=b4+b7;
02871    if(b4 >= b5) goto  L12;
02872 
02873    t=c-cc*c-ss*s;
02874    s=s+ss*c-cc*s;
02875    c=t;
02876    goto  L16;
02877 L20:
02878    ix0=b3/2;
02879    b3=b3-b7;
02880    b4=0;
02881    b5=0;
02882    b6=ix0;
02883    ix1=0;
02884    if ( b6 == b7) goto EXIT;
02885 L22:
02886    b4=b3-b4;
02887    b5=b3-b5;
02888    x2=br(b4+1);
02889    x3=br(b5+1);
02890    x4=bi(b4+1);
02891    x5=bi(b5+1);
02892    br(b4+1)=x3;
02893    br(b5+1)=x2;
02894    bi(b4+1)=x5;
02895    bi(b5+1)=x4;
02896    if (b6 < b4) goto  L22;
02897 L24:
02898    b4=b4+b7;
02899    b5=b6+b5;
02900    x2=br(b4+1);
02901    x3=br(b5+1);
02902    x4=bi(b4+1);
02903    x5=bi(b5+1);
02904    br(b4+1)=x3;
02905    br(b5+1)=x2;
02906    bi(b4+1)=x5;
02907    bi(b5+1)=x4;
02908    ix0=b6;
02909 L26:
02910    ix0=ix0/2;
02911    ix1=ix1-ix0;
02912    if(ix1 >= 0)  goto  L26;
02913 
02914    ix0=2*ix0;
02915    b4=b4+b7;
02916    ix1=ix1+ix0;
02917    b5=ix1;
02918    if (b5 >= b4)  goto  L22;
02919    if (b4 < b6)   goto  L24;
02920 EXIT:
02921    status = 0;
02922 }
02923 
02924 void  Util::fftr_q(float *xcmplx, int nv)
02925 {
02926    // dimension xcmplx(2,1); xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02927 
02928         int nu, inv, nu1, n, isub, n2, i1, i2, i;
02929         float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02930 
02931         const float tab1[] = {
02932                 9.58737990959775e-5f,
02933                 1.91747597310703e-4f,
02934                 3.83495187571395e-4f,
02935                 7.66990318742704e-4f,
02936                 1.53398018628476e-3f,
02937                 3.06795676296598e-3f,
02938                 6.13588464915449e-3f,
02939                 1.22715382857199e-2f,
02940                 2.45412285229123e-2f,
02941                 4.90676743274181e-2f,
02942                 9.80171403295604e-2f,
02943                 1.95090322016128e-1f,
02944                 3.82683432365090e-1f,
02945                 7.07106781186546e-1f,
02946                 1.00000000000000f,
02947         };
02948 
02949         nu=abs(nv);
02950         inv=nv/nu;
02951         nu1=nu-1;
02952         n=(int)pow(2.f,nu1);
02953         isub=16-nu1;
02954 
02955         ss=-tab1(isub);
02956         cc=-2.0f*pow(tab1(isub-1),2.f);
02957         c=1.0f;
02958         s=0.0f;
02959         n2=n/2;
02960         if ( inv > 0) {
02961                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02962                 tr=xcmplx(1,1);
02963                 ti=xcmplx(2,1);
02964                 xcmplx(1,1)=tr+ti;
02965                 xcmplx(2,1)=tr-ti;
02966                 for (i=1;i<=n2;i++) {
02967                         i1=i+1;
02968                         i2=n-i+1;
02969                         tr1=xcmplx(1,i1);
02970                         tr2=xcmplx(1,i2);
02971                         ti1=xcmplx(2,i1);
02972                         ti2=xcmplx(2,i2);
02973                         t=(cc*c-ss*s)+c;
02974                         s=(cc*s+ss*c)+s;
02975                         c=t;
02976                         xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02977                         xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02978                         xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02979                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02980                 }
02981         } else {
02982                 tr=xcmplx(1,1);
02983                 ti=xcmplx(2,1);
02984                 xcmplx(1,1)=0.5f*(tr+ti);
02985                 xcmplx(2,1)=0.5f*(tr-ti);
02986                 for (i=1; i<=n2; i++) {
02987                         i1=i+1;
02988                         i2=n-i+1;
02989                         tr1=xcmplx(1,i1);
02990                         tr2=xcmplx(1,i2);
02991                         ti1=xcmplx(2,i1);
02992                         ti2=xcmplx(2,i2);
02993                         t=(cc*c-ss*s)+c;
02994                         s=(cc*s+ss*c)+s;
02995                         c=t;
02996                         xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
02997                         xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
02998                         xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
02999                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03000                 }
03001                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03002         }
03003 }
03004 
03005 // -------------------------------------------
03006 void  Util::fftr_d(double *xcmplx, int nv)
03007 {
03008         // double precision  x(2,1)
03009         int    i1, i2,  nu, inv, nu1, n, isub, n2, i;
03010         double tr1,tr2,ti1,ti2,tr,ti;
03011         double cc,c,ss,s,t;
03012         const double tab1[] = {
03013                 9.58737990959775e-5,
03014                 1.91747597310703e-4,
03015                 3.83495187571395e-4,
03016                 7.66990318742704e-4,
03017                 1.53398018628476e-3,
03018                 3.06795676296598e-3,
03019                 6.13588464915449e-3,
03020                 1.22715382857199e-2,
03021                 2.45412285229123e-2,
03022                 4.90676743274181e-2,
03023                 9.80171403295604e-2,
03024                 1.95090322016128e-1,
03025                 3.82683432365090e-1,
03026                 7.07106781186546e-1,
03027                 1.00000000000000,
03028         };
03029 
03030         nu=abs(nv);
03031         inv=nv/nu;
03032         nu1=nu-1;
03033         n=(int)pow(2.0f,nu1);
03034         isub=16-nu1;
03035         ss=-tab1(isub);
03036         cc=-2.0*pow(tab1(isub-1),2);
03037         c=1.0f;
03038         s=0.0f;
03039         n2=n/2;
03040 
03041         if ( inv > 0 ) {
03042                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03043                 tr=xcmplx(1,1);
03044                 ti=xcmplx(2,1);
03045                 xcmplx(1,1)=tr+ti;
03046                 xcmplx(2,1)=tr-ti;
03047                 for (i=1;i<=n2;i++) {
03048                         i1=i+1;
03049                         i2=n-i+1;
03050                         tr1=xcmplx(1,i1);
03051                         tr2=xcmplx(1,i2);
03052                         ti1=xcmplx(2,i1);
03053                         ti2=xcmplx(2,i2);
03054                         t=(cc*c-ss*s)+c;
03055                         s=(cc*s+ss*c)+s;
03056                         c=t;
03057                         xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03058                         xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03059                         xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03060                         xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03061                 }
03062         } else {
03063                 tr=xcmplx(1,1);
03064                 ti=xcmplx(2,1);
03065                 xcmplx(1,1)=0.5*(tr+ti);
03066                 xcmplx(2,1)=0.5*(tr-ti);
03067                 for (i=1; i<=n2; i++) {
03068                         i1=i+1;
03069                         i2=n-i+1;
03070                         tr1=xcmplx(1,i1);
03071                         tr2=xcmplx(1,i2);
03072                         ti1=xcmplx(2,i1);
03073                         ti2=xcmplx(2,i2);
03074                         t=(cc*c-ss*s)+c;
03075                         s=(cc*s+ss*c)+s;
03076                         c=t;
03077                         xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03078                         xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03079                         xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03080                         xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03081                 }
03082                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03083         }
03084 }
03085 #undef  tab1
03086 #undef  xcmplx
03087 #undef  br
03088 #undef  bi
03089 
03090 
03091 void Util::Frngs(EMData* circp, vector<int> numr){
03092         int nring = numr.size()/3;
03093         float *circ = circp->get_data();
03094         int i, l;
03095         for (i=1; i<=nring;i++) {
03096 
03097 #ifdef _WIN32
03098                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03099 #else
03100                 l=(int)(log2(numr(3,i)));
03101 #endif  //_WIN32
03102 
03103                 fftr_q(&circ(numr(2,i)),l);
03104         }
03105 }
03106 
03107 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03108         int nring = numr.size()/3;
03109         float *circ = circp->get_data();
03110         int i, l;
03111         for (i=1; i<=nring;i++) {
03112 
03113 #ifdef _WIN32
03114                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03115 #else
03116                 l=(int)(log2(numr(3,i)));
03117 #endif  //_WIN32
03118 
03119                 fftr_q(&circ(numr(2,i)),-l);
03120         }
03121 }
03122 #undef  circ
03123 
03124 #define  b(i)            b[i-1]
03125 void Util::prb1d(double *b, int npoint, float *pos) {
03126         double  c2,c3;
03127         int     nhalf;
03128 
03129         nhalf = npoint/2 + 1;
03130         *pos  = 0.0;
03131 
03132         if (npoint == 7) {
03133                 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03134                      - 6.*b(6) + 31.*b(7);
03135                 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03136         }
03137         else if (npoint == 5) {
03138                 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03139                    + 46.*b(5) ) / (-70.);
03140                 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03141         }
03142         else if (npoint == 3) {
03143                 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03144                 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03145         }
03146         //else if (npoint == 9) {
03147         else  { // at least one has to be true!!
03148                 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03149                      - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03150                      + 1092.*b(9) ) / (-4620.);
03151                 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03152                      - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03153         }
03154         if (c3 != 0.0)  *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03155 }
03156 #undef  b
03157 
03158 #define  circ1(i)        circ1[i-1]
03159 #define  circ2(i)        circ2[i-1]
03160 #define  t(i)            t[i-1]
03161 #define  q(i)            q[i-1]
03162 #define  b(i)            b[i-1]
03163 #define  t7(i)           t7[i-1]
03164 Dict Util::Crosrng_e(EMData*  circ1p, EMData* circ2p, vector<int> numr, int neg) {
03165         //  neg = 0 straight,  neg = 1 mirrored
03166         int nring = numr.size()/3;
03167         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03168         int maxrin = numr[numr.size()-1];
03169         double qn;   float  tot;
03170         float *circ1 = circ1p->get_data();
03171         float *circ2 = circ2p->get_data();
03172 /*
03173 c checks single position, neg is flag for checking mirrored position
03174 c
03175 c  input - fourier transforms of rings!
03176 c  first set is conjugated (mirrored) if neg
03177 c  circ1 already multiplied by weights!
03178 c       automatic arrays
03179         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03180         double precision  q(maxrin)
03181         double precision  t7(-3:3)
03182 */
03183         float *t;
03184         double t7[7], *q;
03185         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03186         float  pos;
03187 
03188 #ifdef _WIN32
03189         ip = -(int)(log((float)maxrin)/log(2.0f));
03190 #else
03191         ip = -(int) (log2(maxrin));
03192 #endif  //_WIN32
03193 
03194         q = (double*)calloc(maxrin, sizeof(double));
03195         t = (float*)calloc(maxrin, sizeof(float));
03196 
03197 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03198         for (i=1; i<=nring; i++) {
03199                 numr3i = numr(3,i);
03200                 numr2i = numr(2,i);
03201 
03202                 t(1) = (circ1(numr2i)) * circ2(numr2i);
03203 
03204                 if (numr3i != maxrin) {
03205                          // test .ne. first for speed on some compilers
03206                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03207                         t(2)            = 0.0;
03208 
03209                         if (neg) {
03210                                 // first set is conjugated (mirrored)
03211                                 for (j=3;j<=numr3i;j=j+2) {
03212                                         jc = j+numr2i-1;
03213                                         t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03214                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03215                                 }
03216                         } else {
03217                                 for (j=3;j<=numr3i;j=j+2) {
03218                                         jc = j+numr2i-1;
03219                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03220                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03221                                 }
03222                         }
03223                         for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03224                 } else {
03225                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03226                         if (neg) {
03227                                 // first set is conjugated (mirrored)
03228                                 for (j=3;j<=maxrin;j=j+2) {
03229                                         jc = j+numr2i-1;
03230                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03231                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03232                                 }
03233                         } else {
03234                                 for (j=3;j<=maxrin;j=j+2) {
03235                                         jc = j+numr2i-1;
03236                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03237                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03238                                 }
03239                         }
03240                         for (j = 1; j <= maxrin; j++) q(j) += t(j);
03241                 }
03242         }
03243 
03244         fftr_d(q,ip);
03245 
03246         qn = -1.0e20;
03247         for (j=1;j<=maxrin;j++) {
03248            if (q(j) >= qn) {
03249                   qn = q(j); jtot = j;
03250            }
03251         }
03252 
03253         for (k=-3; k<=3; k++) {
03254                 j = (jtot+k+maxrin-1)%maxrin + 1;
03255                 t7(k+4) = q(j);
03256         }
03257 
03258         prb1d(t7,7,&pos);
03259 
03260         tot = (float)jtot + pos;
03261 
03262         if (q) free(q);
03263         if (t) free(t);
03264 
03265         Dict retvals;
03266         retvals["qn"] = qn;
03267         retvals["tot"] = tot;
03268         return  retvals;
03269 }
03270 
03271 Dict Util::Crosrng_ew(EMData*  circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03272    //  neg = 0 straight,  neg = 1 mirrored
03273         int nring = numr.size()/3;
03274         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03275         int maxrin = numr[numr.size()-1];
03276         double qn;   float  tot;
03277         float *circ1 = circ1p->get_data();
03278         float *circ2 = circ2p->get_data();
03279 /*
03280 c checks single position, neg is flag for checking mirrored position
03281 c
03282 c  input - fourier transforms of rings!
03283 c  first set is conjugated (mirrored) if neg
03284 c  multiplication by weights!
03285 c       automatic arrays
03286         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03287         double precision  q(maxrin)
03288         double precision  t7(-3:3)
03289 */
03290         float *t;
03291         double t7[7], *q;
03292         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03293         float  pos;
03294 
03295 #ifdef _WIN32
03296         ip = -(int)(log((float)maxrin)/log(2.0f));
03297 #else
03298         ip = -(int) (log2(maxrin));
03299 #endif  //_WIN32
03300 
03301         q = (double*)calloc(maxrin, sizeof(double));
03302         t = (float*)calloc(maxrin, sizeof(float));
03303 
03304 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03305         for (i=1;i<=nring;i++) {
03306                 numr3i = numr(3,i);
03307                 numr2i = numr(2,i);
03308 
03309                 t(1) = circ1(numr2i) * circ2(numr2i);
03310 
03311                 if (numr3i != maxrin) {
03312                         // test .ne. first for speed on some compilers
03313                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03314                         t(2)      = 0.0;
03315 
03316                         if (neg) {
03317                                 // first set is conjugated (mirrored)
03318                                 for (j=3; j<=numr3i; j=j+2) {
03319                                         jc = j+numr2i-1;
03320                                         t(j)   =  (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03321                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03322                                 }
03323                         } else {
03324                                 for (j=3; j<=numr3i; j=j+2) {
03325                                         jc = j+numr2i-1;
03326                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03327                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03328                                 }
03329                         }
03330                         for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03331                 } else {
03332                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03333                         if (neg) {
03334                                 // first set is conjugated (mirrored)
03335                                 for (j=3; j<=maxrin; j=j+2) {
03336                                         jc = j+numr2i-1;
03337                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03338                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03339                                 }
03340                         } else {
03341                                 for (j=3; j<=maxrin; j=j+2) {
03342                                 jc = j+numr2i-1;
03343                                 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03344                                 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03345                                 }
03346                         }
03347                         for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03348                 }
03349         }
03350 
03351         fftr_d(q,ip);
03352 
03353         qn = -1.0e20;
03354         for (j=1;j<=maxrin;j++) {
03355                 //cout << j << "  " << q(j) << endl;
03356                 if (q(j) >= qn) {
03357                         qn = q(j);
03358                         jtot = j;
03359                 }
03360         }
03361 
03362         for (k=-3; k<=3; k++) {
03363                 j = (jtot+k+maxrin-1)%maxrin + 1;
03364                 t7(k+4) = q(j);
03365         }
03366 
03367         prb1d(t7,7,&pos);
03368 
03369         tot = (float)jtot + pos;
03370 
03371         //if (q) free(q);
03372         if (t) free(t);
03373 
03374         Dict retvals;
03375         //tot = 1;
03376         //qn = q(1);
03377         retvals["qn"] = qn;
03378         retvals["tot"] = tot;
03379 
03380         if (q) free(q);
03381 
03382         return  retvals;
03383 }
03384 
03385 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03386         int nring = numr.size()/3;
03387         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03388         int maxrin = numr[numr.size()-1];
03389         double qn; float tot; double qm; float tmt;
03390         float *circ1 = circ1p->get_data();
03391         float *circ2 = circ2p->get_data();
03392 /*
03393 c
03394 c  checks both straight & mirrored positions
03395 c
03396 c  input - fourier transforms of rings!!
03397 c  circ1 already multiplied by weights!
03398 c
03399 */
03400 
03401         // dimension             circ1(lcirc),circ2(lcirc)
03402 
03403         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03404         double *t, *q, t7[7];
03405 
03406         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03407         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03408 
03409         qn  = 0.0f;
03410         qm  = 0.0f;
03411         tot = 0.0f;
03412         tmt = 0.0f;
03413 #ifdef _WIN32
03414         ip = -(int)(log((float)maxrin)/log(2.0f));
03415 #else
03416         ip = -(int)(log2(maxrin));
03417 #endif  //_WIN32
03418   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03419 
03420         //  c - straight  = circ1 * conjg(circ2)
03421         //  zero q array
03422 
03423         q = (double*)calloc(maxrin,sizeof(double));
03424 
03425         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03426         //   zero t array
03427         t = (double*)calloc(maxrin,sizeof(double));
03428 
03429    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03430         for (i=1; i<=nring; i++) {
03431 
03432                 numr3i = numr(3,i);   // Number of samples of this ring
03433                 numr2i = numr(2,i);   // The beginning point of this ring
03434 
03435                 t1   = circ1(numr2i) * circ2(numr2i);
03436                 q(1) += t1;
03437                 t(1) += t1;
03438 
03439                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03440                 if (numr3i == maxrin)  {
03441                         q(2) += t1;
03442                         t(2) += t1;
03443                 } else {
03444                         q(numr3i+1) += t1;
03445                         t(numr3i+1) += t1;
03446                 }
03447 
03448                 for (j=3; j<=numr3i; j += 2) {
03449                         jc     = j+numr2i-1;
03450 
03451 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03452 //                                ----- -----    ----- -----
03453 //                                 t1     t2      t3    t4
03454 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03455 //                                    ----- -----    ----- -----
03456 //                                     t1    t2       t3    t4
03457 
03458                         c1     = circ1(jc);
03459                         c2     = circ1(jc+1);
03460                         d1     = circ2(jc);
03461                         d2     = circ2(jc+1);
03462 
03463                         t1     = c1 * d1;
03464                         t2     = c2 * d2;
03465                         t3     = c1 * d2;
03466                         t4     = c2 * d1;
03467 
03468                         q(j)   += t1 + t2;
03469                         q(j+1) += -t3 + t4;
03470                         t(j)   += t1 - t2;
03471                         t(j+1) += -t3 - t4;
03472                 }
03473         }
03474         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03475         fftr_d(q,ip);
03476 
03477         qn  = -1.0e20;
03478         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03479                 if (q(j) >= qn) {
03480                         qn  = q(j);
03481                         jtot = j;
03482                 }
03483         }
03484 
03485         for (k=-3; k<=3; k++) {
03486                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03487                 t7(k+4) = q(j);
03488         }
03489 
03490         // interpolate
03491         prb1d(t7,7,&pos);
03492         tot = (float)(jtot)+pos;
03493         // Do not interpolate
03494         //tot = (float)(jtot);
03495 
03496         // mirrored
03497         fftr_d(t,ip);
03498 
03499         // find angle
03500         qm = -1.0e20;
03501         for (j=1; j<=maxrin;j++) {//cout <<"  "<<j<<"   "<<t(j) <<endl;
03502                 if ( t(j) >= qm ) {
03503                         qm   = t(j);
03504                         jtot = j;
03505                 }
03506         }
03507 
03508         for (k=-3; k<=3; k++)  {
03509                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03510                 t7(k+4) = t(j);
03511         }
03512 
03513         // interpolate
03514 
03515         prb1d(t7,7,&pos);
03516         tmt = float(jtot) + pos;
03517         // Do not interpolate
03518         //tmt = float(jtot);
03519 
03520         free(t);
03521         free(q);
03522 
03523         Dict retvals;
03524         retvals["qn"] = qn;
03525         retvals["tot"] = tot;
03526         retvals["qm"] = qm;
03527         retvals["tmt"] = tmt;
03528         return retvals;
03529 }
03530 
03531 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03532         int nring = numr.size()/3;
03533         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03534         int maxrin = numr[numr.size()-1];
03535         double qn; float tot; double qm; float tmt;
03536         float *circ1 = circ1p->get_data();
03537         float *circ2 = circ2p->get_data();
03538 /*
03539 c
03540 c  checks both straight & mirrored positions
03541 c
03542 c  input - fourier transforms of rings!!
03543 c  circ1 already multiplied by weights!
03544 c
03545 */
03546 
03547         // dimension             circ1(lcirc),circ2(lcirc)
03548 
03549         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03550         double *t, *q;
03551 
03552         int   ip, jc, numr3i, numr2i, i, j, jtot = 0;
03553         float t1, t2, t3, t4, c1, c2, d1, d2;
03554 
03555         qn  = 0.0f;
03556         qm  = 0.0f;
03557         tot = 0.0f;
03558         tmt = 0.0f;
03559 #ifdef _WIN32
03560         ip = -(int)(log((float)maxrin)/log(2.0f));
03561 #else
03562         ip = -(int)(log2(maxrin));
03563 #endif  //_WIN32
03564   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03565 
03566         //  c - straight  = circ1 * conjg(circ2)
03567         //  zero q array
03568 
03569         q = (double*)calloc(maxrin,sizeof(double));
03570 
03571         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03572         //   zero t array
03573         t = (double*)calloc(maxrin,sizeof(double));
03574 
03575    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03576         for (i=1; i<=nring; i++) {
03577 
03578                 numr3i = numr(3,i);   // Number of samples of this ring
03579                 numr2i = numr(2,i);   // The beginning point of this ring
03580 
03581                 t1   = circ1(numr2i) * circ2(numr2i);
03582                 q(1) += t1;
03583                 t(1) += t1;
03584 
03585                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03586                 if (numr3i == maxrin)  {
03587                         q(2) += t1;
03588                         t(2) += t1;
03589                 } else {
03590                         q(numr3i+1) += t1;
03591                         t(numr3i+1) += t1;
03592                 }
03593 
03594                 for (j=3; j<=numr3i; j += 2) {
03595                         jc     = j+numr2i-1;
03596 
03597 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03598 //                                ----- -----    ----- -----
03599 //                                 t1     t2      t3    t4
03600 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03601 //                                    ----- -----    ----- -----
03602 //                                     t1    t2       t3    t4
03603 
03604                         c1     = circ1(jc);
03605                         c2     = circ1(jc+1);
03606                         d1     = circ2(jc);
03607                         d2     = circ2(jc+1);
03608 
03609                         t1     = c1 * d1;
03610                         t2     = c2 * d2;
03611                         t3     = c1 * d2;
03612                         t4     = c2 * d1;
03613 
03614                         q(j)   += t1 + t2;
03615                         q(j+1) += -t3 + t4;
03616                         t(j)   += t1 - t2;
03617                         t(j+1) += -t3 - t4;
03618                 }
03619         }
03620         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03621         fftr_d(q,ip);
03622 
03623         qn  = -1.0e20;
03624 
03625         int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03626         int jstep = static_cast<int>(delta/360.0*maxrin);
03627         if (jstep < 1) { jstep = 1; }
03628 
03629         for (j=jstart; j<=maxrin; j+=jstep) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03630                 if (q(j) >= qn) {
03631                         qn  = q(j);
03632                         jtot = j;
03633                 }
03634         }
03635 
03636         //for (k=-3; k<=3; k++) {
03637         //      j = ((jtot+k+maxrin-1)%maxrin)+1;
03638         //      t7(k+4) = q(j);
03639         //}
03640 
03641         // interpolate
03642         //prb1d(t7,7,&pos);
03643         //tot = (float)(jtot)+pos;
03644         // Do not interpolate
03645         tot = (float)(jtot);
03646 
03647         // mirrored
03648         fftr_d(t,ip);
03649 
03650         // find angle
03651         qm = -1.0e20;
03652         for (j=jstart; j<=maxrin;j+=jstep) {//cout <<"  "<<j<<" "<<t(j) <<endl;
03653                 if ( t(j) >= qm ) {
03654                         qm   = t(j);
03655                         jtot = j;
03656                 }
03657         }
03658 
03659         //for (k=-3; k<=3; k++)  {
03660         //      j = ((jtot+k+maxrin-1)%maxrin) + 1;
03661         //      t7(k+4) = t(j);
03662         //}
03663 
03664         // interpolate
03665 
03666         //prb1d(t7,7,&pos);
03667         //tmt = float(jtot) + pos;
03668         // Do not interpolate
03669         tmt = float(jtot);
03670 
03671         free(t);
03672         free(q);
03673 
03674         Dict retvals;
03675         retvals["qn"] = qn;
03676         retvals["tot"] = tot;
03677         retvals["qm"] = qm;
03678         retvals["tmt"] = tmt;
03679         return retvals;
03680 }
03681 
03682 
03683 Dict Util::Crosrng_psi_0_180(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03684         int nring = numr.size()/3;
03685         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03686         int maxrin = numr[numr.size()-1];
03687         double qn; float tot; double qm; float tmt;
03688         float *circ1 = circ1p->get_data();
03689         float *circ2 = circ2p->get_data();
03690 
03691         // dimension             circ1(lcirc),circ2(lcirc)
03692 
03693         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03694         double *t, *q, t7[7];
03695 
03696         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03697         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03698 
03699         qn  = 0.0f;
03700         qm  = 0.0f;
03701         tot = 0.0f;
03702         tmt = 0.0f;
03703 #ifdef _WIN32
03704         ip = -(int)(log((float)maxrin)/log(2.0f));
03705 #else
03706         ip = -(int)(log2(maxrin));
03707 #endif  //_WIN32
03708   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03709 
03710         //  c - straight  = circ1 * conjg(circ2)
03711         //  zero q array
03712 
03713         q = (double*)calloc(maxrin,sizeof(double));
03714 
03715         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03716         //   zero t array
03717         t = (double*)calloc(maxrin,sizeof(double));
03718 
03719    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03720         for (i=1; i<=nring; i++) {
03721 
03722                 numr3i = numr(3,i);   // Number of samples of this ring
03723                 numr2i = numr(2,i);   // The beginning point of this ring
03724 
03725                 t1   = circ1(numr2i) * circ2(numr2i);
03726                 q(1) += t1;
03727                 t(1) += t1;
03728 
03729                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03730                 if (numr3i == maxrin)  {
03731                         q(2) += t1;
03732                         t(2) += t1;
03733                 } else {
03734                         q(numr3i+1) += t1;
03735                         t(numr3i+1) += t1;
03736                 }
03737 
03738                 for (j=3; j<=numr3i; j += 2) {
03739                         jc     = j+numr2i-1;
03740 
03741 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03742 //                                ----- -----    ----- -----
03743 //                                 t1     t2      t3    t4
03744 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03745 //                                    ----- -----    ----- -----
03746 //                                     t1    t2       t3    t4
03747 
03748                         c1     = circ1(jc);
03749                         c2     = circ1(jc+1);
03750                         d1     = circ2(jc);
03751                         d2     = circ2(jc+1);
03752 
03753                         t1     = c1 * d1;
03754                         t2     = c2 * d2;
03755                         t3     = c1 * d2;
03756                         t4     = c2 * d1;
03757 
03758                         q(j)   += t1 + t2;
03759                         q(j+1) += -t3 + t4;
03760                         t(j)   += t1 - t2;
03761                         t(j+1) += -t3 - t4;
03762                 }
03763         }
03764         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03765         fftr_d(q,ip);
03766 
03767         int psi_range  = int(psi_max/360.0*maxrin+0.5);
03768         const int psi_0 = 0;
03769         int psi_180    = int(  180.0/360.0*maxrin+0.5);
03770 
03771         qn  = -1.0e20;
03772         for (k=-psi_range; k<=psi_range; k++) {
03773                 j = (k+psi_0+maxrin-1)%maxrin+1;//string modemo = "f";cout <<" 90   "<<j<<"   "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03774                 if (q(j) >= qn) {
03775                         qn  = q(j);
03776                         jtot = j;
03777                 }
03778         }
03779 
03780         for (k=-psi_range; k<=psi_range; k++) {
03781                 j = (k+psi_180+maxrin-1)%maxrin+1; //cout <<" 270  "<<j<<"  "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03782                 if (q(j) >= qn) {
03783                         qn  = q(j);
03784                         jtot = j;
03785                 }
03786         }
03787 
03788         for (k=-3; k<=3; k++) {
03789                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03790                 t7(k+4) = q(j);
03791         }
03792 
03793         // interpolate
03794         prb1d(t7,7,&pos);
03795         tot = (float)(jtot)+pos;
03796         // Do not interpolate
03797         //tot = (float)(jtot);
03798 
03799         // mirrored
03800         fftr_d(t,ip);
03801 
03802         // find angle
03803         qm = -1.0e20;
03804         for (k=-psi_range; k<=psi_range; k++) {
03805                 j = (k+psi_0+maxrin-1)%maxrin+1;//string modemo = "f";cout <<" 90   "<<j<<"   "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03806                 if (t(j) >= qm) {
03807                         qm  = t(j);
03808                         jtot = j;
03809                 }
03810         }
03811 
03812         for (k=-psi_range; k<=psi_range; k++) {
03813                 j = (k+psi_180+maxrin-1)%maxrin+1; //cout <<" 270  "<<j<<"  "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03814                 if (t(j) >= qm) {
03815                         qm  = t(j);
03816                         jtot = j;
03817                 }
03818         }
03819 
03820         for (k=-3; k<=3; k++)  {
03821                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03822                 t7(k+4) = t(j);
03823         }
03824 
03825         // interpolate
03826 
03827         prb1d(t7,7,&pos);
03828         tmt = float(jtot) + pos;
03829         // Do not interpolate
03830         //tmt = float(jtot);
03831 
03832         free(t);
03833         free(q);
03834 
03835         Dict retvals;
03836         retvals["qn"] = qn;
03837         retvals["tot"] = tot;
03838         retvals["qm"] = qm;
03839         retvals["tmt"] = tmt;
03840         return retvals;
03841 }
03842 
03843 
03844 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag) {
03845 // flag 0 - straignt, 1 - mirror
03846 
03847         int nring = numr.size()/3;
03848         int maxrin = numr[numr.size()-1];
03849         double qn; float tot; double qm; float tmt;
03850         float *circ1 = circ1p->get_data();
03851         float *circ2 = circ2p->get_data();
03852 
03853         double *q, t7[7];
03854 
03855         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03856         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03857 
03858         qn  = 0.0f;
03859         qm  = 0.0f;
03860         tot = 0.0f;
03861         tmt = 0.0f;
03862 #ifdef _WIN32
03863         ip = -(int)(log((float)maxrin)/log(2.0f));
03864 #else
03865         ip = -(int)(log2(maxrin));
03866 #endif  //_WIN32
03867 
03868         //  c - straight  = circ1 * conjg(circ2)
03869         //  zero q array
03870 
03871         q = (double*)calloc(maxrin,sizeof(double));
03872 
03873    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03874         if (flag==0) {
03875                 for (i=1; i<=nring; i++) {
03876 
03877                         numr3i = numr(3,i);   // Number of samples of this ring
03878                         numr2i = numr(2,i);   // The beginning point of this ring
03879 
03880                         t1   = circ1(numr2i) * circ2(numr2i);
03881                         q(1) += t1;
03882 
03883                         t1   = circ1(numr2i+1) * circ2(numr2i+1);
03884                         if (numr3i == maxrin)  {
03885                                 q(2) += t1;
03886                         } else {
03887                                 q(numr3i+1) += t1;
03888                         }
03889 
03890                         for (j=3; j<=numr3i; j += 2) {
03891                                 jc     = j+numr2i-1;
03892 
03893         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03894         //                                ----- -----    ----- -----
03895         //                                 t1     t2      t3    t4
03896 
03897                                 c1     = circ1(jc);
03898                                 c2     = circ1(jc+1);
03899                                 d1     = circ2(jc);
03900                                 d2     = circ2(jc+1);
03901 
03902                                 t1     = c1 * d1;
03903                                 t3     = c1 * d2;
03904                                 t2     = c2 * d2;
03905                                 t4     = c2 * d1;
03906 
03907                                 q(j)   += t1 + t2;
03908                                 q(j+1) += -t3 + t4;
03909                         }
03910                 }
03911         } else {
03912                 for (i=1; i<=nring; i++) {
03913 
03914                         numr3i = numr(3,i);   // Number of samples of this ring
03915                         numr2i = numr(2,i);   // The beginning point of this ring
03916 
03917                         t1   = circ1(numr2i) * circ2(numr2i);
03918                         q(1) += t1;
03919 
03920                         t1   = circ1(numr2i+1) * circ2(numr2i+1);
03921                         if (numr3i == maxrin)  {
03922                                 q(2) += t1;
03923                         } else {
03924                                 q(numr3i+1) += t1;
03925                         }
03926 
03927                         for (j=3; j<=numr3i; j += 2) {
03928                                 jc     = j+numr2i-1;
03929 
03930         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03931         //                                ----- -----    ----- -----
03932         //                                 t1     t2      t3    t4
03933 
03934                                 c1     = circ1(jc);
03935                                 c2     = circ1(jc+1);
03936                                 d1     = circ2(jc);
03937                                 d2     = circ2(jc+1);
03938 
03939                                 t1     = c1 * d1;
03940                                 t3     = c1 * d2;
03941                                 t2     = c2 * d2;
03942                                 t4     = c2 * d1;
03943 
03944                                 q(j)   += t1 - t2;
03945                                 q(j+1) += -t3 - t4;
03946                         }
03947                 }
03948         }
03949         fftr_d(q,ip);
03950 
03951         qn  = -1.0e20;
03952         int psi_pos = int(psi/360.0*maxrin+0.5);
03953 
03954         for (k=-5; k<=5; k++) {
03955                 j = (psi_pos+maxrin-1)%maxrin+1;
03956                 if (q(j) >= qn) {
03957                         qn  = q(j);
03958                         jtot = j;
03959                 }
03960         }
03961 
03962         for (k=-3; k<=3; k++) {
03963                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03964                 t7(k+4) = q(j);
03965         }
03966 
03967         // interpolate
03968         prb1d(t7,7,&pos);
03969         tot = (float)(jtot)+pos;
03970         free(q);
03971 
03972         Dict retvals;
03973         retvals["qn"] = qn;
03974         retvals["tot"] = tot;
03975         return retvals;
03976 }
03977 
03978 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03979         int nring = numr.size()/3;
03980         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03981         int maxrin = numr[numr.size()-1];
03982         double qn; float tot;
03983         float *circ1 = circ1p->get_data();
03984         float *circ2 = circ2p->get_data();
03985 /*
03986 c
03987 c  checks only straight position
03988 c
03989 c  input - fourier transforms of rings!!
03990 c  circ1 already multiplied by weights!
03991 c
03992 */
03993 
03994         // dimension             circ1(lcirc),circ2(lcirc)
03995 
03996         // q(maxrin), t7(-3:3)  //maxrin+2 removed
03997         double *q, t7[7];
03998 
03999         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04000         float c1, c2, d1, d2, pos;
04001 
04002         qn  = 0.0;
04003         tot = 0.0;
04004 #ifdef _WIN32
04005         ip = -(int)(log((float)maxrin)/log(2.0f));
04006 #else
04007    ip = -(int)(log2(maxrin));
04008 #endif  //_WIN32
04009         //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
04010 
04011         //  c - straight  = circ1 * conjg(circ2)
04012         //  zero q array
04013 
04014         q = (double*)calloc(maxrin,sizeof(double));
04015 
04016                         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04017         for (i=1; i<=nring; i++) {
04018 
04019                 numr3i = numr(3,i);   // Number of samples of this ring
04020                 numr2i = numr(2,i);   // The beginning point of this ring
04021 
04022                 q(1) += circ1(numr2i) * circ2(numr2i);
04023 
04024                 if (numr3i == maxrin)   q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04025                 else  q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04026 
04027                 for (j=3; j<=numr3i; j += 2) {
04028                         jc     = j+numr2i-1;
04029 
04030 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
04031 //                                ----- -----    ----- -----
04032 //                                 t1     t2      t3    t4
04033 
04034                         c1     = circ1(jc);
04035                         c2     = circ1(jc+1);
04036                         d1     = circ2(jc);
04037                         d2     = circ2(jc+1);
04038 
04039                         q(j)   += c1 * d1 + c2 * d2;
04040                         q(j+1) += -c1 * d2 + c2 * d1;
04041                 }
04042         }
04043 //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<endl;
04044         fftr_d(q,ip);
04045 
04046         qn  = -1.0e20;
04047         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
04048                 if (q(j) >= qn) {
04049                         qn  = q(j);
04050                         jtot = j;
04051                 }
04052         }
04053 
04054         for (k=-3; k<=3; k++)  {
04055                 j = ((jtot+k+maxrin-1)%maxrin)+1;
04056                 t7(k+4) = q(j);
04057         }
04058 
04059         // interpolate
04060         prb1d(t7,7,&pos);
04061         tot = (float)(jtot)+pos;
04062         // Do not interpolate
04063         //*tot = (float)(jtot);
04064 
04065         free(q);
04066 
04067         Dict retvals;
04068         retvals["qn"] = qn;
04069         retvals["tot"] = tot;
04070         return retvals;
04071 }
04072 
04073 #define  dout(i,j)        dout[i+maxrin*j]
04074 #define  circ1b(i)        circ1b[i-1]
04075 #define  circ2b(i)        circ2b[i-1]
04076 
04077 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04078 
04079    // dimension         circ1(lcirc),circ2(lcirc)
04080 
04081         int   ip, jc, numr3i, numr2i, i, j;
04082         float t1, t2, t3, t4, c1, c2, d1, d2;
04083 
04084         int nring = numr.size()/3;
04085         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04086         int maxrin = numr[numr.size()-1];
04087 
04088         float* circ1b = circ1->get_data();
04089         float* circ2b = circ2->get_data();
04090 
04091         // t(maxrin), q(maxrin)  // removed +2
04092         double *t, *q;
04093 
04094         q = (double*)calloc(maxrin,sizeof(double));
04095         t = (double*)calloc(maxrin,sizeof(double));
04096 
04097 #ifdef _WIN32
04098         ip = -(int)(log((float)maxrin)/log(2.0f));
04099 #else
04100         ip = -(int)(log2(maxrin));
04101 #endif  //_WIN32
04102 
04103         //  q - straight  = circ1 * conjg(circ2)
04104 
04105         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04106 
04107         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04108 
04109         for (i=1; i<=nring; i++) {
04110 
04111                 numr3i = numr(3,i);
04112                 numr2i = numr(2,i);
04113 
04114                 t1   = circ1b(numr2i) * circ2b(numr2i);
04115                 q(1) = q(1)+t1;
04116                 t(1) = t(1)+t1;
04117 
04118                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04119                 if (numr3i == maxrin)  {
04120                         q(2) += t1;
04121                         t(2) += t1;
04122                 } else {
04123                         q(numr3i+1) += t1;
04124                         t(numr3i+1) += t1;
04125                 }
04126 
04127                 for (j=3; j<=numr3i; j=j+2) {
04128                         jc     = j+numr2i-1;
04129 
04130                         c1     = circ1b(jc);
04131                         c2     = circ1b(jc+1);
04132                         d1     = circ2b(jc);
04133                         d2     = circ2b(jc+1);
04134 
04135                         t1     = c1 * d1;
04136                         t3     = c1 * d2;
04137                         t2     = c2 * d2;
04138                         t4     = c2 * d1;
04139 
04140                         q(j)   += t1 + t2;
04141                         q(j+1) += - t3 + t4;
04142                         t(j)   += t1 - t2;
04143                         t(j+1) += - t3 - t4;
04144                 }
04145         }
04146 
04147         // straight
04148         fftr_d(q,ip);
04149 
04150         // mirrored
04151         fftr_d(t,ip);
04152 
04153         EMData* out = new EMData();
04154         out->set_size(maxrin,2,1);
04155         float *dout = out->get_data();
04156         for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04157         //out->set_size(maxrin,1,1);
04158         //float *dout = out->get_data();
04159         //for (int i=0; i<maxrin; i++) {dout(i,0)=q[i];}
04160         free(t);
04161         free(q);
04162         return out;
04163 }
04164 
04165 
04166 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04167 
04168         int maxrin = numr[numr.size()-1];
04169 
04170         vector<float> r(2*maxrin);
04171 
04172         Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04173 
04174         return r;
04175 }
04176 
04177 #define  dout(i,j)        dout[i+maxrin*j]
04178 #define  circ1b(i)        circ1b[i-1]
04179 #define  circ2b(i)        circ2b[i-1]
04180 
04181 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04182 
04183    // dimension         circ1(lcirc),circ2(lcirc)
04184 
04185         int   ip, jc, numr3i, numr2i, i, j;
04186         float t1, t2, t3, t4, c1, c2, d1, d2;
04187 
04188         int nring = numr.size()/3;
04189         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04190         int maxrin = numr[numr.size()-1];
04191 
04192         float* circ1b = circ1->get_data();
04193         float* circ2b = circ2->get_data();
04194 
04195 #ifdef _WIN32
04196         ip = -(int)(log((float)maxrin)/log(2.0f));
04197 #else
04198         ip = -(int)(log2(maxrin));
04199 #endif  //_WIN32
04200         for (int i=1; i<=maxrin; i++)  {q(i) = 0.0f; t(i) = 0.0f;}
04201 
04202         //  q - straight  = circ1 * conjg(circ2)
04203 
04204         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04205 
04206         for (i=1; i<=nring; i++) {
04207 
04208                 numr3i = numr(3,i);
04209                 numr2i = numr(2,i);
04210 
04211                 t1   = circ1b(numr2i) * circ2b(numr2i);
04212                 q(1) += t1;
04213                 t(1) += t1;
04214 
04215                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04216                 if (numr3i == maxrin)  {
04217                         q(2) += t1;
04218                         t(2) += t1;
04219                 } else {
04220                         q(numr3i+1) += t1;
04221                         t(numr3i+1) += t1;
04222                 }
04223 
04224                 for (j=3; j<=numr3i; j=j+2) {
04225                         jc     = j+numr2i-1;
04226 
04227                         c1     = circ1b(jc);
04228                         c2     = circ1b(jc+1);
04229                         d1     = circ2b(jc);
04230                         d2     = circ2b(jc+1);
04231 
04232                         t1     = c1 * d1;
04233                         t3     = c1 * d2;
04234                         t2     = c2 * d2;
04235                         t4     = c2 * d1;
04236 
04237                         q(j)   += t1 + t2;
04238                         q(j+1) += -t3 + t4;
04239                         t(j)   += t1 - t2;
04240                         t(j+1) += -t3 - t4;
04241                 }
04242         }
04243         // straight
04244         fftr_q(q,ip);
04245         //for (int i=0; i<maxrin; i++) cout<<i<<"  B    "<<q[i]<<"       "<<t[i]<<endl;
04246 
04247         // mirrored
04248         fftr_q(t,ip);
04249 }
04250 
04251 
04252 
04253 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04254 {
04255 
04256         int   ip, jc, numr3i, numr2i, i, j;
04257         float t1, t2, t3, t4, c1, c2, d1, d2;
04258 
04259         int nring = numr.size()/3;
04260         int maxrin = numr[numr.size()-1];
04261 
04262         float* circ1b = circ1->get_data();
04263         float* circ2b = circ2->get_data();
04264 
04265         double *q;
04266 
04267         q = (double*)calloc(maxrin,sizeof(double));
04268 
04269 #ifdef _WIN32
04270         ip = -(int)(log((float)maxrin)/log(2.0f));
04271 #else
04272         ip = -(int)(log2(maxrin));
04273 #endif  //_WIN32
04274 
04275          //  q - straight  = circ1 * conjg(circ2)
04276 
04277         for (i=1;i<=nring;i++) {
04278 
04279                 numr3i = numr(3,i);
04280                 numr2i = numr(2,i);
04281 
04282                 t1   = circ1b(numr2i) * circ2b(numr2i);
04283                 q(1) = q(1)+t1;
04284 
04285                 if (numr3i == maxrin)  {
04286                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04287                         q(2) = q(2)+t1;
04288                 } else {
04289                         t1              = circ1b(numr2i+1) * circ2b(numr2i+1);
04290                         q(numr3i+1) = q(numr3i+1)+t1;
04291                 }
04292 
04293                 for (j=3;j<=numr3i;j=j+2) {
04294                         jc     = j+numr2i-1;
04295 
04296                         c1     = circ1b(jc);
04297                         c2     = circ1b(jc+1);
04298                         d1     = circ2b(jc);
04299                         d2     = circ2b(jc+1);
04300 
04301                         t1     = c1 * d1;
04302                         t3     = c1 * d2;
04303                         t2     = c2 * d2;
04304                         t4     = c2 * d1;
04305 
04306                         q(j)   = q(j)   + t1 + t2;
04307                         q(j+1) = q(j+1) - t3 + t4;
04308                 }
04309         }
04310 
04311         // straight
04312         fftr_d(q,ip);
04313 
04314         EMData* out = new EMData();
04315         out->set_size(maxrin,1,1);
04316         float *dout = out->get_data();
04317         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04318         free(q);
04319         return out;
04320 
04321 }
04322 
04323 
04324 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04325 {
04326 
04327         int   ip, jc, numr3i, numr2i, i, j;
04328         float t1, t2, t3, t4, c1, c2, d1, d2;
04329 
04330         int nring = numr.size()/3;
04331         int maxrin = numr[numr.size()-1];
04332 
04333         float* circ1b = circ1->get_data();
04334         float* circ2b = circ2->get_data();
04335 
04336         double *t;
04337 
04338         t = (double*)calloc(maxrin,sizeof(double));
04339 
04340 #ifdef _WIN32
04341         ip = -(int)(log((float)maxrin)/log(2.0f));
04342 #else
04343         ip = -(int)(log2(maxrin));
04344 #endif  //_WIN32
04345 
04346          //   t - mirrored  = conjg(circ1) * conjg(circ2)
04347 
04348         for (i=1;i<=nring;i++) {
04349 
04350                 numr3i = numr(3,i);
04351                 numr2i = numr(2,i);
04352 
04353                 t1   = circ1b(numr2i) * circ2b(numr2i);
04354                 t(1) = t(1)+t1;
04355 
04356                 if (numr3i == maxrin)  {
04357                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04358                         t(2) = t(2)+t1;
04359                 }
04360 
04361                 for (j=3;j<=numr3i;j=j+2) {
04362                         jc     = j+numr2i-1;
04363 
04364                         c1     = circ1b(jc);
04365                         c2     = circ1b(jc+1);
04366                         d1     = circ2b(jc);
04367                         d2     = circ2b(jc+1);
04368 
04369                         t1     = c1 * d1;
04370                         t3     = c1 * d2;
04371                         t2     = c2 * d2;
04372                         t4     = c2 * d1;
04373 
04374                         t(j)   = t(j)   + t1 - t2;
04375                         t(j+1) = t(j+1) - t3 - t4;
04376                 }
04377         }
04378 
04379         // mirrored
04380         fftr_d(t,ip);
04381 
04382         EMData* out = new EMData();
04383         out->set_size(maxrin,1,1);
04384         float *dout = out->get_data();
04385         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04386         free(t);
04387         return out;
04388 
04389 }
04390 
04391 #undef circ1b
04392 #undef circ2b
04393 #undef dout
04394 
04395 #undef  circ1
04396 #undef  circ2
04397 #undef  t
04398 #undef  q
04399 #undef  b
04400 #undef  t7
04401 
04402 
04403 #define    QUADPI                   3.141592653589793238462643383279502884197
04404 #define    PI2                      2*QUADPI
04405 
04406 float Util::ener(EMData* ave, vector<int> numr) {
04407         ENTERFUNC;
04408         long double ener,en;
04409 
04410         int nring = numr.size()/3;
04411         float *aveptr = ave->get_data();
04412 
04413         ener = 0.0;
04414         for (int i=1; i<=nring; i++) {
04415                 int numr3i = numr(3,i);
04416                 int np     = numr(2,i)-1;
04417                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04418                 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04419                 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04420                 ener += en/numr3i;
04421         }
04422         EXITFUNC;
04423         return static_cast<float>(ener);
04424 }
04425 
04426 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04427         ENTERFUNC;
04428         long double ener, en;
04429         float arg, cs, si;
04430 
04431         int nima = data.size();
04432         int nring = numr.size()/3;
04433         int maxrin = numr(3,nring);
04434 
04435         ener = 0.0;
04436         for (int i=1; i<=nring; i++) {
04437                 int numr3i = numr(3,i);
04438                 int np     = numr(2,i)-1;
04439                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04440                 float temp1 = 0.0, temp2 = 0.0;
04441                 for (int kk=0; kk<nima; kk++) {
04442                         float *ptr = data[kk]->get_data();
04443                         temp1 += ptr[np];
04444                         temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04445                 }
04446                 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04447                 for (int j=2; j<numr3i; j+=2) {
04448                         float tempr = 0.0, tempi = 0.0;
04449                         for (int kk=0; kk<nima; kk++) {
04450                                 float *ptr = data[kk]->get_data();
04451                                 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04452                                 cs = cos(arg);
04453                                 si = sin(arg);
04454                                 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04455                                 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04456                         }
04457                         en += tq*(tempr*tempr+tempi*tempi);
04458                 }
04459                 ener += en/numr3i;
04460         }
04461         EXITFUNC;
04462         return static_cast<float>(ener);
04463 }
04464 
04465 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04466         int nring = numr.size()/3;
04467         float *ave = avep->get_data();
04468         float *dat = datp->get_data();
04469         int i, j, numr3i, np;
04470         float  arg, cs, si;
04471         int maxrin = numr(3,nring);
04472         if(mirror == 1) { //for mirrored data has to be conjugated
04473                 for (i=1; i<=nring; i++) {
04474                         numr3i = numr(3,i);
04475                         np     = numr(2,i)-1;
04476                         ave[np]   += dat[np];
04477                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04478                         for (j=2; j<numr3i; j=j+2) {
04479                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04480                                 cs = cos(arg);
04481                                 si = sin(arg);
04482                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04483                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04484                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04485                         }
04486                 }
04487         } else {
04488                 for (i=1; i<=nring; i++) {
04489                         numr3i = numr(3,i);
04490                         np     = numr(2,i)-1;
04491                         ave[np]   += dat[np];
04492                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04493                         for (j=2; j<numr3i; j=j+2) {
04494                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04495                                 cs = cos(arg);
04496                                 si = sin(arg);
04497                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04498                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04499                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04500                         }
04501                 }
04502         }
04503         avep->update();
04504         EXITFUNC;
04505 }
04506 
04507 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04508         int nring = numr.size()/3;
04509         float *ave = avep->get_data();
04510         float *dat = datp->get_data();
04511         int i, j, numr3i, np;
04512         float  arg, cs, si;
04513         int maxrin = numr(3,nring);
04514         if(mirror == 1) { //for mirrored data has to be conjugated
04515                 for (i=1; i<=nring; i++) {
04516                         numr3i = numr(3,i);
04517                         np     = numr(2,i)-1;
04518                         ave[np]   -= dat[np];
04519                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04520                         for (j=2; j<numr3i; j=j+2) {
04521                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04522                                 cs = cos(arg);
04523                                 si = sin(arg);
04524                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04525                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04526                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04527                         }
04528                 }
04529         } else {
04530                 for (i=1; i<=nring; i++) {
04531                         numr3i = numr(3,i);
04532                         np     = numr(2,i)-1;
04533                         ave[np]   -= dat[np];
04534                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04535                         for (j=2; j<numr3i; j=j+2) {
04536                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04537                                 cs = cos(arg);
04538                                 si = sin(arg);
04539                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04540                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04541                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04542                         }
04543                 }
04544         }
04545         avep->update();
04546         EXITFUNC;
04547 }
04548 
04549 
04550 #undef    QUADPI
04551 #undef    PI2
04552 
04553 #undef  numr
04554 #undef  circ
04555 
04556 
04557 #define QUADPI   3.141592653589793238462643383279502884197
04558 #define PI2      QUADPI*2
04559 #define deg_rad  QUADPI/180.0
04560 #define rad_deg  180.0/QUADPI
04561 
04562 struct ori_t
04563 {
04564     int iphi;
04565     int itht;
04566     int id;
04567 };
04568 
04569 
04570 struct cmpang
04571 {
04572     bool operator()( const ori_t& a, const ori_t& b )
04573     {
04574         if( a.itht != b.itht )
04575         {
04576             return a.itht < b.itht;
04577         }
04578 
04579         return a.iphi < b.iphi;
04580     }
04581 };
04582 
04583 
04584 vector<double> Util::cml_weights(const vector<float>& cml){
04585         static const int NBIN = 100;
04586         int nline=cml.size()/2;
04587         vector<double> weights(nline);
04588 
04589         vector<ori_t> angs(nline);
04590         for( int i=0; i < nline; ++i ) {
04591                 angs[i].iphi = int( NBIN*cml[2*i] );
04592                 angs[i].itht = int( NBIN*cml[2*i+1] );
04593                 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04594                 angs[i].id = i;
04595         }
04596 
04597         //std::cout << "# of angs: " << angs.size() << std::endl;
04598 
04599         std::sort( angs.begin(), angs.end(), cmpang() );
04600 
04601         vector<float> newphi;
04602         vector<float> newtht;
04603         vector< vector<int> > indices;
04604 
04605         int curt_iphi = -1;
04606         int curt_itht = -1;
04607         for(unsigned int i=0 ;i < angs.size(); ++i ) {
04608                 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04609                         Assert( indices.size() > 0 );
04610                         indices.back().push_back(angs[i].id);
04611                 } else {
04612                         curt_iphi = angs[i].iphi;
04613                         curt_itht = angs[i].itht;
04614 
04615                         newphi.push_back( float(curt_iphi)/NBIN );
04616                         newtht.push_back( float(curt_itht)/NBIN );
04617                         indices.push_back( vector<int>(1,angs[i].id) );
04618                 }
04619         }
04620 
04621         //std::cout << "# of indpendent ang: " << newphi.size() << std::endl;
04622 
04623 
04624         int num_agl = newphi.size();
04625 
04626         if(num_agl>2) {
04627                 vector<double> w=Util::vrdg(newphi, newtht);
04628 
04629                 Assert( w.size()==newphi.size() );
04630                 Assert( indices.size()==newphi.size() );
04631 
04632                 for(unsigned int i=0; i < newphi.size(); ++i ) {
04633                     /*
04634                     std::cout << "phi,tht,w,n: ";
04635                     std::cout << boost::format( "%10.3f" ) % newphi[i] << " ";
04636                     std::cout << boost::format( "%10.3f" ) % newtht[i] << " ";
04637                     std::cout << boost::format( "%8.6f"  ) % w[i] << " ";
04638                     std::cout << indices[i].size() << "(";
04639                     */
04640 
04641                     for(unsigned int j=0; j < indices[i].size(); ++j ) {
04642                             int id = indices[i][j];
04643                             weights[id] = w[i]/indices[i].size();
04644                             //std::cout << id << " ";
04645                     }
04646 
04647                     //std::cout << ")" << std::endl;
04648 
04649                 }
04650         } else {
04651                 cout<<"warning in Util.cml_weights"<<endl;
04652                 double val = PI2/float(nline);
04653                 for(int i=0; i<nline; i++)  weights[i]=val;
04654         }
04655 
04656         return weights;
04657 
04658 }
04659 
04660 /****************************************************
04661  * New code for common-lines
04662  ****************************************************/
04663 
04664 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04665 {
04666         int i;
04667         int nx=img->get_xsize();
04668         float *img_ptr  = img->get_data();
04669         float *line_ptr = line->get_data();
04670         for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04671         img->update();
04672 }
04673 
04674 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04675     int j;
04676     int nx = sino->get_xsize();
04677     int i = nx * pos_line;
04678     float r1, r2;
04679     float *line_ptr = line->get_data();
04680     float *sino_ptr = sino->get_data();
04681     for (j=ilf;j<=ihf; j += 2) {
04682         r1 = line_ptr[j];
04683         r2 = line_ptr[j + 1];
04684         sino_ptr[i + j - ilf] = r1;
04685         sino_ptr[i + j - ilf + 1] = r2;
04686         sino_ptr[i + nx * nblines + j - ilf] = r1;
04687         sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04688     }
04689     sino->update();
04690 }
04691 
04692 vector<double> Util::cml_init_rot(vector<float> Ori){
04693     int nb_ori = Ori.size() / 4;
04694     int i, ind;
04695     float ph, th, ps;
04696     double cph, cth, cps, sph, sth, sps;
04697     vector<double> Rot(nb_ori*9);
04698     for (i=0; i<nb_ori; ++i){
04699         ind = i*4;
04700         // spider convention phi=psi-90, psi=phi+90
04701         ph = Ori[ind+2]-90;
04702         th = Ori[ind+1];
04703         ps = Ori[ind]+90;
04704         ph *= deg_rad;
04705         th *= deg_rad;
04706         ps *= deg_rad;
04707         // pre-calculate some trigo stuffs
04708         cph = cos(ph);
04709         cth = cos(th);
04710         cps = cos(ps);
04711         sph = sin(ph);
04712         sth = sin(th);
04713         sps = sin(ps);
04714         // fill rotation matrix
04715         ind = i*9;
04716         Rot[ind] = cph*cps-cth*sps*sph;
04717         Rot[ind+1] = cph*sps+cth*cps*sph;
04718         Rot[ind+2] = sth*sph;
04719         Rot[ind+3] = -sph*cps-cth*sps*cph;
04720         Rot[ind+4] = -sph*sps+cth*cps*cph;
04721         Rot[ind+5] = sth*cph;
04722         Rot[ind+6] = sth*sps;
04723         Rot[ind+7] = -sth*cps;
04724         Rot[ind+8] = cth;
04725     }
04726 
04727     return Rot;
04728 }
04729 
04730 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04731     float ph, ps;
04732     double cph, cth, cps, sph, sth, sps;
04733     int ind = iprj*9;
04734     // spider convention phi=psi-90, psi=phi+90
04735     ph = nps-90;
04736     ps = nph+90;
04737     ph *= deg_rad;
04738     th *= deg_rad;
04739     ps *= deg_rad;
04740     // pre-calculate some trigo stuffs
04741     cph = cos(ph);
04742     cth = cos(th);
04743     cps = cos(ps);
04744     sph = sin(ph);
04745     sth = sin(th);
04746     sps = sin(ps);
04747     // fill rotation matrix
04748     Rot[ind] = (float)(cph*cps-cth*sps*sph);
04749     Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04750     Rot[ind+2] = (float)(sth*sph);
04751     Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04752     Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04753     Rot[ind+5] = (float)(sth*cph);
04754     Rot[ind+6] = (float)(sth*sps);
04755     Rot[ind+7] = (float)(-sth*cps);
04756     Rot[ind+8] = (float)(cth);
04757 
04758     return Rot;
04759 }
04760 
04761 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04762     vector<int> com(2*(n_prj - 1));
04763     int a = i_prj*9;
04764     int i, b, c;
04765     int n1=0, n2=0;
04766     float vmax = 1 - 1.0e-6f;
04767     double r11, r12, r13, r23, r31, r32, r33;
04768 
04769     c = 0;
04770     for (i=0; i<n_prj; ++i){
04771         if (i!=i_prj){
04772             b = i*9;
04773             // this is equivalent to R = A*B'
04774             r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04775             r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04776             r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04777             r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04778             r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04779             r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04780             r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04781             if (r33 > vmax) {
04782                 n2 = 270;
04783                 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04784             }
04785             else if (r33 < -vmax) {
04786                 n2 = 270;
04787                 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04788             } else {
04789                 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04790                 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04791                 if (n1 < 0) {n1 += 360;}
04792                 if (n2 <= 0) {n2 = abs(n2);}
04793                 else {n2 = 360 - n2;}
04794             }
04795 
04796             if (n1 >= 360){n1 = n1 % 360;}
04797             if (n2 >= 360){n2 = n2 % 360;}
04798 
04799             // store common-lines
04800             b = c*2;
04801             com[b] = n1;
04802             com[b+1] = n2;
04803             ++c;
04804         }
04805     }
04806 
04807     return com;
04808 
04809 }
04810 
04811 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int, int n_lines) {
04812     vector<int> com(2*n_lines);
04813     int a=0, b, c, l;
04814     int n1=0, n2=0, mem=-1;
04815     float vmax = 1 - 1.0e-6f;
04816     double r11, r12, r13, r23, r31, r32, r33;
04817     c = 0;
04818     for (l=0; l<n_lines; ++l){
04819         c = 2*l;
04820         if (seq[c]!=mem){
04821             mem = seq[c];
04822             a = seq[c]*9;
04823         }
04824         b = seq[c+1]*9;
04825 
04826         // this is equivalent to R = A*B'
04827         r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04828         r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04829         r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04830         r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04831         r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04832         r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04833         r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04834         if (r33 > vmax) {
04835             n2 = 270;
04836             n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04837         }
04838         else if (r33 < -vmax) {
04839             n2 = 270;
04840             n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04841         } else {
04842             n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04843             n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04844             if (n1 < 0) {n1 += 360;}
04845             if (n2 <= 0) {n2 = abs(n2);}
04846             else {n2 = 360 - n2;}
04847         }
04848         if (n1 >= 360){n1 = n1 % 360;}
04849         if (n2 >= 360){n2 = n2 % 360;}
04850 
04851         // store common-lines
04852         com[c] = n1;
04853         com[c+1] = n2;
04854     }
04855 
04856     return com;
04857 
04858 }
04859 
04860 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int, int nlines){
04861     // seq is the pairwise index ij: 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04862     vector<double> cml(2*nlines); // [phi, theta] / line
04863     float ph1, th1;
04864     float ph2, th2;
04865     double nx, ny, nz;
04866     double norm;
04867     double sth1=0, sph1=0, cth1=0, cph1=0;
04868     double sth2, sph2, cth2, cph2;
04869     int l, ind, c;
04870     int mem = -1;
04871     for (l=0; l<nlines; ++l){
04872         c = 2*l;
04873         if (seq[c]!=mem){
04874             mem = seq[c];
04875             ind = 4*seq[c];
04876             ph1 = Ori[ind]*deg_rad;
04877             th1 = Ori[ind+1]*deg_rad;
04878             sth1 = sin(th1);
04879             sph1 = sin(ph1);
04880             cth1 = cos(th1);
04881             cph1 = cos(ph1);
04882         }
04883         ind = 4*seq[c+1];
04884         ph2 = Ori[ind]*deg_rad;
04885         th2 = Ori[ind+1]*deg_rad;
04886         sth2 = sin(th2);
04887         cth2 = cos(th2);
04888         sph2 = sin(ph2);
04889         cph2 = cos(ph2);
04890         // cross product
04891         nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04892         ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04893         nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04894         norm = sqrt(nx*nx+ny*ny+nz*nz);
04895         nx /= norm;
04896         ny /= norm;
04897         nz /= norm;
04898         // apply mirror if need
04899         if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04900         // compute theta and phi
04901         cml[c+1] = acos(nz);
04902         if (cml[c+1] == 0) {cml[c] = 0;}
04903         else {
04904             cml[c+1] *= rad_deg;
04905             if (cml[c+1] > 89.99) {cml[c+1] = 89.99;} // this fix some pb in Voronoi
04906             cml[c] = rad_deg * atan2(nx, ny);
04907             cml[c] = fmod(360 + cml[c], 360);
04908 
04909         }
04910     }
04911 
04912     return cml;
04913 }
04914 
04915 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04916     double res = 0;
04917     double buf = 0;
04918     float* line_1;
04919     float* line_2;
04920     int i, n, ind;
04921     int lnlen = data[0]->get_xsize();
04922     for (n=0; n<n_lines; ++n) {
04923         ind = n*2;
04924         line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04925         line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04926         buf = 0;
04927         for (i=0; i<lnlen; ++i) {
04928             buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04929         }
04930         res += buf * weights[n];
04931     }
04932 
04933     return res;
04934 
04935 }
04936 
04937 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04938                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04939     // res: [best_disc, best_ipsi]
04940     // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04941     // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
04942     vector<double> res(2);
04943     int lnlen = data[0]->get_xsize();
04944     int end = 2*(n_prj-1);
04945     double disc, buf, bdisc, tmp;
04946     int n, i, ipsi, ind, bipsi, c;
04947     float* line_1;
04948     float* line_2;
04949     bdisc = 1.0e6;
04950     bipsi = -1;
04951     // loop psi
04952     for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04953         // discrepancy
04954         disc = 0;
04955         c = 0;
04956         for (n=0; n<n_prj; ++n) {
04957             if(n!=iprj) {
04958                 ind = 2*c;
04959                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04960                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04961                 buf = 0;
04962                 for (i=0; i<lnlen; ++i) {
04963                     tmp = line_1[i]-line_2[i];
04964                     buf += tmp*tmp;
04965                 }
04966                 disc += buf * weights[iw[c]];
04967                 ++c;
04968             }
04969         }
04970         // select the best value
04971         if (disc <= bdisc) {
04972             bdisc = disc;
04973             bipsi = ipsi;
04974         }
04975         // update common-lines
04976         for (i=0; i<end; i+=2){
04977             com[i] += d_psi;
04978             if (com[i] >= n_psi) {com[i] = com[i] % n_psi;}
04979         }
04980     }
04981     res[0] = bdisc;
04982     res[1] = float(bipsi);
04983 
04984     return res;
04985 }
04986 
04987 #undef  QUADPI
04988 #undef  PI2
04989 #undef  deg_rad
04990 #undef  rad_deg
04991 
04992 /****************************************************
04993  * END OF NEW CODE FOR COMMON-LINES
04994  ****************************************************/
04995 
04996 // helper function for k-means
04997 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
04998         ENTERFUNC;
04999 
05000         int nima = data.size();
05001         vector<float> res(nima);
05002         double result = 0.;
05003         double valmin = 1.0e20;
05004         int valpos = -1;
05005 
05006         for (int kk=0; kk<nima; kk++){
05007         result = 0;
05008 
05009         float *y_data = data[kk]->get_data();
05010         float *x_data = image->get_data();
05011         long totsize = image->get_xsize()*image->get_ysize();
05012         for (long i = 0; i < totsize; i++) {
05013             double temp = x_data[i]- y_data[i];
05014             result += temp*temp;
05015         }
05016         result /= totsize;
05017         res[kk] = (float)result;
05018 
05019         if(result<valmin) {valmin = result; valpos = kk;}
05020 
05021         }
05022 
05023         Dict retvals;
05024         retvals["dist"] = res;
05025         retvals["pos"]  = valpos;
05026 
05027         EXITFUNC;
05028         return retvals;
05029 
05030 }
05031 
05032 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05033         ENTERFUNC;
05034 
05035         int nima = data.size();
05036         vector<float> res(nima);
05037         double result = 0.;
05038         double valmin = 1.0e20;
05039         int valpos = -1;
05040 
05041         for (int kk=0; kk<nima; kk++){
05042         result = 0;
05043         //validate_input_args(image, data[kk]);
05044 
05045         float *y_data = data[kk]->get_data();
05046         float *x_data = image->get_data();
05047 
05048         // Implemented by PAP  01/09/06 - please do not change.  If in doubts, write/call me.
05049         int nx  = data[kk]->get_xsize();
05050         int ny  = data[kk]->get_ysize();
05051         nx = (nx - 2 + data[kk]->is_fftodd()); // nx is the real-space size of the input image
05052         int lsd2 = (nx + 2 - nx%2) ; // Extended x-dimension of the complex image
05053 
05054         int ixb = 2*((nx+1)%2);
05055         int iyb = ny%2;
05056         int iz = 0;
05057 
05058         for ( int iy = 0; iy <= ny-1; iy++) {
05059             for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05060                 int ii = ix + (iy  + iz * ny)* lsd2;
05061                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05062             }
05063         }
05064         for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05065             int ii = (iy  + iz * ny)* lsd2;
05066             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05067             result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05068         }
05069         if(nx%2 == 0) {
05070             for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05071                 int ii = lsd2 - 2 + (iy  + iz * ny)* lsd2;
05072                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05073                 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05074             }
05075 
05076         }
05077         result *= 2;
05078         result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05079         if(ny%2 == 0) {
05080             int ii = (ny/2  + iz * ny)* lsd2;
05081             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05082         }
05083         if(nx%2 == 0) {
05084             int ii = lsd2 - 2 + (0  + iz * ny)* lsd2;
05085             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05086             if(ny%2 == 0) {
05087                 int ii = lsd2 - 2 +(ny/2  + iz * ny)* lsd2;
05088                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05089             }
05090         }
05091 
05092         result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05093         res[kk] = (float)result;
05094 
05095         if(result<valmin) {valmin = result; valpos = kk;}
05096 
05097         }
05098 
05099         Dict retvals;
05100         retvals["dist"] = res;
05101         retvals["pos"]  = valpos;
05102 
05103         EXITFUNC;
05104         return retvals;
05105 }
05106 
05107 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05108     long int d2 = group2[s2 - 1] - group2[0];
05109     long int p2 = 0;
05110     long int i1 = 0;
05111     long int i2 = 0;
05112     long int max = 0;
05113     long int cont = 0;
05114     long int i = 0;
05115     int stop1 = 0;
05116     int stop2 = 0;
05117 
05118     for (i=0; i<s1; i++) {
05119         p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05120         if (p2 >= s2) {p2 = s2 - 1;}
05121         i1 = p2;
05122         i2 = p2;
05123         max = s2;
05124         if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05125 
05126         stop1 = 0;
05127         stop2 = 0;
05128         while (max--) {
05129             if (group1[i] == group2[i1]) {
05130                 if (flag) {stb[cont] = group1[i];}
05131                 cont++;
05132                 break;
05133             }
05134             if (group2[i1] < group1[i]) {stop1=1;}
05135             if (group1[i] == group2[i2]) {
05136                 if (flag) {stb[cont] = group1[i];}
05137                 cont++;
05138                 break;
05139             }
05140             if (group2[i2] > group1[i]) {stop2=1;}
05141             //printf("i1 %li i2 %li    v2 %i v2 %i   stop1 %i stop2 %i\n", i1, i2, group2[i1], group2[i2], stop1, stop2);
05142 
05143             if (stop1 & stop2) {break;}
05144             i1--;
05145             i2++;
05146             if (i1 < 0) {i1 = 0;}
05147             if (i2 >= s2) {i2 = s2 - 1;}
05148         }
05149         //printf("v1: %i    ite: %li   cont: %li\n", group1[i], s2-max, cont);
05150     }
05151 
05152     return cont;
05153 }
05154 
05155 
05156 
05157 #define old_ptr(i,j,k)          old_ptr[i+(j+(k*ny))*(size_t)nx]
05158 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*(size_t)new_nx]
05159 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05160 {
05161         /* Exception Handle */
05162         if (!img) {
05163                 throw NullPointerException("NULL input image");
05164         }
05165         /* ============================== */
05166 
05167         // Get the size of the input image
05168         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05169         /* ============================== */
05170 
05171 
05172         /* Exception Handle */
05173         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)
05174         {
05175                 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05176                 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05177         }
05178         /* ============================== */
05179 
05180 
05181         /*    Calculation of the start point */
05182         int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05183         /* ============================*/
05184 
05185 
05186         /* Calculation of the size of the decimated image */
05187         int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05188         int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05189         int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05190         if(r1>1){r1=1;}
05191         if(r2>1){r2=1;}
05192         if(r3>1){r3=1;}
05193         int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05194         /* ===========================================*/
05195 
05196 
05197         EMData* img2 = new EMData();
05198         img2->set_size(new_nx,new_ny,new_nz);
05199         float *new_ptr = img2->get_data();
05200         float *old_ptr = img->get_data();
05201         int iptr, jptr, kptr = 0;
05202         for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05203                 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05204                         for (int i=new_st_x; i<nx; i+=x_step) {
05205                                 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05206                         iptr++;}
05207                 jptr++;}
05208         kptr++;}
05209         img2->update();
05210         return img2;
05211 }
05212 #undef old_ptr
05213 #undef new_ptr
05214 
05215 #define inp(i,j,k)  inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*(size_t)nx]
05216 #define outp(i,j,k) outp[i+(j+(k*new_ny))*(size_t)new_nx]
05217 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05218 {
05219         /* Exception Handle */
05220         if (!img) throw NullPointerException("NULL input image");
05221         /* ============================== */
05222 
05223         // Get the size of the input image
05224         int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05225         /* ============================== */
05226 
05227         /* Exception Handle */
05228         if(new_nx>nx || new_ny>ny || new_nz>nz)
05229                 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size.");
05230         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)
05231                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05232         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))))
05233                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05234         /* ============================== */
05235 
05236         /*    Calculation of the start point */
05237         int  new_st_x = nx/2-new_nx/2 + x_offset,
05238              new_st_y = ny/2-new_ny/2 + y_offset,
05239              new_st_z = nz/2-new_nz/2 + z_offset;
05240         /* ============================== */
05241 
05242         /* Exception Handle */
05243         if (new_st_x<0 || new_st_y<0 || new_st_z<0)   //  WHAT HAPPENS WITH THE END POINT CHECK??  PAP
05244                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05245         /* ============================== */
05246 
05247         EMData* wind = img->copy_empty_head();
05248         wind->set_size(new_nx, new_ny, new_nz);
05249         float *outp=wind->get_data();
05250         float *inp=img->get_data();
05251 
05252         for (int k=0; k<new_nz; k++)
05253                 for(int j=0; j<new_ny; j++)
05254                         for(int i=0; i<new_nx; i++)
05255                                 outp(i,j,k) = inp(i,j,k);
05256         wind->update();
05257         return wind;
05258 }
05259 #undef inp
05260 #undef outp
05261 
05262 #define inp(i,j,k) inp[i+(j+(k*ny))*(size_t)nx]
05263 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*(size_t)new_nx]
05264 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)
05265 {
05266         /* Exception Handle */
05267         if (!img)  throw NullPointerException("NULL input image");
05268         /* ============================== */
05269 
05270         // Get the size of the input image
05271         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05272         /* ============================== */
05273 
05274         /* Exception Handle */
05275         if(new_nx<nx || new_ny<ny || new_nz<nz)
05276                 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05277         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)
05278                 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05279         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))))
05280                 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05281         /* ============================== */
05282 
05283         EMData* pading = img->copy_head();
05284         pading->set_size(new_nx, new_ny, new_nz);
05285         float *inp  = img->get_data();
05286         float *outp = pading->get_data();
05287 
05288 
05289         /* Calculation of the average and the circumference values for background substitution
05290         =======================================================================================*/
05291         float background;
05292 
05293         if (strcmp(params,"average")==0) background = img->get_attr("mean");
05294         else if (strcmp(params,"circumference")==0) {
05295                 float sum1=0.0f;
05296                 size_t cnt=0;
05297                 for(int i=0;i<nx;i++) {
05298                         sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05299                         cnt+=2;
05300                 }
05301                 if(nz-1 == 0) {
05302                         for (int j=1;j<ny-1;j++) {
05303                                 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05304                                 cnt+=2;
05305                         }
05306                 } else {
05307                         for (int k=1;k<nz-1;k++) {
05308                                 for (int j=1;j<ny-1;j++) {
05309                                         sum1 += inp(1,j,0) + inp(nx-1,j,0);
05310                                         cnt+=2;
05311                                 }
05312                         }
05313                 }
05314                 background = sum1/cnt;
05315         } else {
05316                 background = static_cast<float>( atof( params ) );
05317         }
05318         /*=====================================================================================*/
05319 
05320          /*Initial Padding */
05321         int new_st_x=0,new_st_y=0,new_st_z=0;
05322         for (int k=0;k<new_nz;k++)
05323                 for(int j=0;j<new_ny;j++)
05324                         for (int i=0;i<new_nx;i++)
05325                                 outp(i,j,k)=background;
05326         /*============================== */
05327 
05328         /*    Calculation of the start point */
05329         new_st_x=int((new_nx/2-nx/2)  + x_offset);
05330         new_st_y=int((new_ny/2-ny/2)  + y_offset);
05331         new_st_z=int((new_nz/2-nz/2)  + z_offset);
05332         /* ============================== */
05333 
05334         for (int k=0;k<nz;k++)
05335                 for(int j=0;j<ny;j++)
05336                         for(int i=0;i<nx;i++)
05337                                 outp(i,j,k)=inp(i,j,k);
05338         pading->update();
05339         return pading;
05340 }
05341 #undef inp
05342 #undef outp
05343 //-------------------------------------------------------------------------------------------------------------------------------------------------------------
05344 
05345 void Util::colreverse(float* beg, float* end, int nx) {
05346         float* tmp = new float[nx];
05347         int n = (end - beg)/nx;
05348         int nhalf = n/2;
05349         for (int i = 0; i < nhalf; i++) {
05350                 // swap col i and col n-1-i
05351                 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05352                 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05353                 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05354         }
05355         delete[] tmp;
05356 }
05357 
05358 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05359 {
05360         int nxy = nx*ny;
05361         colreverse(beg, end, nxy);
05362 }
05363 
05364 
05365 void Util::cyclicshift(EMData *image, Dict params) {
05366 
05367         if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05368 
05369         int dx = params["dx"];
05370         int dy = params["dy"];
05371         int dz = params["dz"];
05372 
05373         // The reverse trick we're using shifts to the left (a negative shift)
05374         int nx = image->get_xsize();
05375         dx %= nx;
05376         if (dx < 0) dx += nx;
05377         int ny = image->get_ysize();
05378         dy %= ny;
05379         if (dy < 0) dy += ny;
05380         int nz = image->get_zsize();
05381         dz %= nz;
05382         if (dz < 0) dz += nz;
05383 
05384         int mx = -(dx - nx);
05385         int my = -(dy - ny);
05386         int mz = -(dz - nz);
05387 
05388         float* data = image->get_data();
05389         // x-reverses
05390         if (mx != 0) {
05391                 for (int iz = 0; iz < nz; iz++)
05392                        for (int iy = 0; iy < ny; iy++) {
05393                                 // reverses for column iy
05394                                 size_t offset = nx*iy + (size_t)nx*ny*iz; // starting location for column iy in slice iz
05395                                 reverse(&data[offset],&data[offset+mx]);
05396                                 reverse(&data[offset+mx],&data[offset+nx]);
05397                                 reverse(&data[offset],&data[offset+nx]);
05398                         }
05399         }
05400         // y-reverses
05401         if (my != 0) {
05402                 for (int iz = 0; iz < nz; iz++) {
05403                         size_t offset = (size_t)nx*ny*iz;
05404                         colreverse(&data[offset], &data[offset + my*nx], nx);
05405                         colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05406                         colreverse(&data[offset], &data[offset + ny*nx], nx);
05407                 }
05408         }
05409         if (mz != 0) {
05410                 slicereverse(&data[0], &data[(size_t)mz*ny*nx], nx, ny);
05411                 slicereverse(&data[mz*ny*nx], &data[(size_t)nz*ny*nx], nx, ny);
05412                 slicereverse(&data[0], &data[(size_t)nz*ny*nx], nx ,ny);
05413         }
05414         image->update();
05415 }
05416 
05417 //-----------------------------------------------------------------------------------------------------------------------
05418 
05419 
05420 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05421 {
05422         if (image->is_complex())
05423                 throw ImageFormatException("Cannot do histogram on Fourier image");
05424         //float hmax, hmin;
05425         float *imageptr=0, *maskptr=0;
05426         int nx=image->get_xsize();
05427         int ny=image->get_ysize();
05428         int nz=image->get_zsize();
05429 
05430         if(mask != NULL){
05431                 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05432                         throw ImageDimensionException("The size of mask image should be of same size as the input image");
05433                 maskptr =mask->get_data();
05434         }
05435         if( nbins == 0) nbins = nx;
05436         vector <float> freq(2*nbins, 0.0);
05437 
05438         imageptr=image->get_data();
05439         if( hmin == hmax ) {
05440                 if(mask == NULL) {
05441                         hmax = image->get_attr("maximum");
05442                         hmin = image->get_attr("minimum");
05443                 } else {
05444                         bool  First = true;
05445                         for (size_t i = 0;i < (size_t)nx*ny*nz; i++) {
05446                         if (maskptr[i]>=0.5f) {
05447                                         if(First) {
05448                                                 hmax = imageptr[i];
05449                                                 hmin = imageptr[i];
05450                                                 First = false;
05451                                         } else {
05452                                                 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05453                                                 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05454                                         }
05455                                 }
05456                         }
05457                 }
05458         }
05459         float hdiff = hmax - hmin;
05460         float ff = (nbins-1)/hdiff;
05461         for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05462         if(mask == NULL) {
05463                 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05464                         int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05465                         if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05466                 }
05467         } else {
05468                 for(size_t i = 0; i < (size_t)nx*ny*nz; i++) {
05469                         if(maskptr[i] >= 0.5) {
05470                                 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05471                                 if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05472                         }
05473                 }
05474         }
05475         return freq;
05476 }
05477 
05478 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05479 {
05480         /* Exception Handle */
05481         if (img->is_complex() || ref->is_complex())
05482                 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05483 
05484         if(mask != NULL){
05485                 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05486                         throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05487         /* ===================================================== */
05488 
05489         /* Image size calculation */
05490         size_t size_ref = ((size_t)(ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05491         size_t size_img = ((size_t)(img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05492         /* ===================================================== */
05493 
05494         /* The reference image attributes */
05495         float *ref_ptr = ref->get_data();
05496         float ref_h_min = ref->get_attr("minimum");
05497         float ref_h_max = ref->get_attr("maximum");
05498         float ref_h_avg = ref->get_attr("mean");
05499         float ref_h_sig = ref->get_attr("sigma");
05500         /* ===================================================== */
05501 
05502         /* Input image under mask attributes */
05503         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05504 
05505         vector<float> img_data = Util::infomask(img, mask);
05506         float img_avg = img_data[0];
05507         float img_sig = img_data[1];
05508 
05509         /* The image under mask -- size calculation */
05510         int cnt=0;
05511         for(size_t i=0;i<size_img;++i)
05512                 if (mask_ptr[i]>0.5f)
05513                                 cnt++;
05514         /* ===================================================== */
05515 
05516         /* Histogram of reference image calculation */
05517         float ref_h_diff = ref_h_max - ref_h_min;
05518 
05519         #ifdef _WIN32
05520                 int hist_len = _cpp_min((unsigned long)size_ref/16,_cpp_min((unsigned long)size_img/16,256lu));
05521         #else
05522                 int hist_len = std::min((unsigned long)size_ref/16,std::min((unsigned long)size_img/16,256lu));
05523         #endif  //_WIN32
05524 
05525         float *ref_freq_bin = new float[3*hist_len];
05526 
05527         //initialize value in each bin to zero
05528         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05529 
05530         for (size_t i = 0;i < size_ref;++i) {
05531                 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05532                 ref_freq_bin[L]++;
05533         }
05534         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05535 
05536         //Parameters Calculation (i.e) 'A' x + 'B'
05537         float A = ref_h_sig/img_sig;
05538         float B = ref_h_avg - (A*img_avg);
05539 
05540         vector<float> args;
05541         args.push_back(A);
05542         args.push_back(B);
05543 
05544         vector<float> scale;
05545         scale.push_back(1.e-7f*A);
05546         scale.push_back(-1.e-7f*B);
05547 
05548         vector<float> ref_freq_hist;
05549         for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05550 
05551         vector<float> data;
05552         data.push_back(ref_h_diff);
05553         data.push_back(ref_h_min);
05554 
05555         Dict parameter;
05556 
05557         /* Parameters displaying the arguments A & B, and the scaling function and the data's */
05558         parameter["args"] = args;
05559         parameter["scale"]= scale;
05560         parameter["data"] = data;
05561         parameter["ref_freq_bin"] = ref_freq_hist;
05562         parameter["size_img"]=(double)size_img;
05563         parameter["hist_len"]=hist_len;
05564         /* ===================================================== */
05565 
05566         return parameter;
05567 }
05568 
05569 
05570 float Util::hist_comp_freq(float PA,float PB,size_t size_img, int hist_len, EMData *img, vector<float> ref_freq_hist, EMData *mask, float ref_h_diff, float ref_h_min)
05571 {
05572         float *img_ptr = img->get_data();
05573         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05574 
05575         int *img_freq_bin = new int[3*hist_len];
05576         for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05577         for(size_t i = 0;i < size_img;++i) {
05578                 if(mask_ptr[i] > 0.5f) {
05579                         float img_xn = img_ptr[i]*PA + PB;
05580                         int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05581                         if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05582                 }
05583         }
05584         int freq_hist = 0;
05585 
05586         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);
05587         freq_hist = (-freq_hist);
05588         return static_cast<float>(freq_hist);
05589 }
05590 //------------------------------------------------------------------------------------------------------------------------------------------------------------------
05591 #define    QUADPI                       3.141592653589793238462643383279502884197
05592 #define    DGR_TO_RAD                   QUADPI/180
05593 #define    DM(I)                        DM          [I-1]
05594 #define    SS(I)                        SS          [I-1]
05595 Dict Util::CANG(float PHI,float THETA,float PSI)
05596 {
05597         double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05598         vector<float>   DM,SS;
05599 
05600         for(int i =0;i<9;i++) DM.push_back(0);
05601 
05602         for(int i =0;i<6;i++) SS.push_back(0);
05603 
05604         CPHI = cos(double(PHI)*DGR_TO_RAD);
05605         SPHI = sin(double(PHI)*DGR_TO_RAD);
05606         CTHE = cos(double(THETA)*DGR_TO_RAD);
05607         STHE = sin(double(THETA)*DGR_TO_RAD);
05608         CPSI = cos(double(PSI)*DGR_TO_RAD);
05609         SPSI = sin(double(PSI)*DGR_TO_RAD);
05610 
05611         SS(1) = float(CPHI);
05612         SS(2) = float(SPHI);
05613         SS(3) = float(CTHE);
05614         SS(4) = float(STHE);
05615         SS(5) = float(CPSI);
05616         SS(6) = float(SPSI);
05617 
05618         DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05619         DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05620         DM(3) = float(-STHE*CPSI);
05621         DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05622         DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05623         DM(6) = float(STHE*SPSI);
05624         DM(7) = float(STHE*CPHI);
05625         DM(8) = float(STHE*SPHI);
05626         DM(9) = float(CTHE);
05627 
05628         Dict DMnSS;
05629         DMnSS["DM"] = DM;
05630         DMnSS["SS"] = SS;
05631 
05632         return(DMnSS);
05633 }
05634 #undef SS
05635 #undef DM
05636 #undef QUADPI
05637 #undef DGR_TO_RAD
05638 //-----------------------------------------------------------------------------------------------------------------------
05639 #define    DM(I)                        DM[I-1]
05640 #define    B(i,j)                       Bptr[i-1+((j-1)*NSAM)]
05641 #define    CUBE(i,j,k)                  CUBEptr[(i-1)+((j-1)+((k-1)*NY3D))*(size_t)NX3D]
05642 
05643 void Util::BPCQ(EMData *B,EMData *CUBE, vector<float> DM)
05644 {
05645 
05646         float  *Bptr = B->get_data();
05647         float  *CUBEptr = CUBE->get_data();
05648 
05649         int NSAM,NROW,NX3D,NY3D,NZC,KZ,IQX,IQY,LDPX,LDPY,LDPZ,LDPNMX,LDPNMY,NZ1;
05650         float DIPX,DIPY,XB,YB,XBB,YBB;
05651 
05652         Transform * t = B->get_attr("xform.projection");
05653         Dict d = t->get_params("spider");
05654         if(t) {delete t; t=0;}
05655         //  Unsure about sign of shifts, check later PAP 06/28/09
05656         float x_shift = d[ "tx" ];
05657         float y_shift = d[ "ty" ];
05658         x_shift = -x_shift;
05659         y_shift = -y_shift;
05660 
05661         NSAM = B->get_xsize();
05662         NROW = B->get_ysize();
05663         NX3D = CUBE->get_xsize();
05664         NY3D = CUBE->get_ysize();
05665         NZC  = CUBE->get_zsize();
05666 
05667 
05668         LDPX   = NX3D/2 +1;
05669         LDPY   = NY3D/2 +1;
05670         LDPZ   = NZC/2 +1;
05671         LDPNMX = NSAM/2 +1;
05672         LDPNMY = NROW/2 +1;
05673         NZ1    = 1;
05674 
05675         for(int K=1;K<=NZC;K++) {
05676                 KZ=K-1+NZ1;
05677                 for(int J=1;J<=NY3D;J++) {
05678                         XBB = (1-LDPX)*DM(1)+(J-LDPY)*DM(2)+(KZ-LDPZ)*DM(3);
05679                         YBB = (1-LDPX)*DM(4)+(J-LDPY)*DM(5)+(KZ-LDPZ)*DM(6);
05680                         for(int I=1;I<=NX3D;I++) {
05681                                 XB  = (I-1)*DM(1)+XBB-x_shift;
05682                                 IQX = int(XB+float(LDPNMX));
05683                                 if (IQX <1 || IQX >= NSAM) continue;
05684                                 YB  = (I-1)*DM(4)+YBB-y_shift;
05685                                 IQY = int(YB+float(LDPNMY));
05686                                 if (IQY<1 || IQY>=NROW)  continue;
05687                                 DIPX = XB+LDPNMX-IQX;
05688                                 DIPY = YB+LDPNMY-IQY;
05689 
05690                                 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)));
05691                         }
05692                 }
05693         }
05694 }
05695 
05696 #undef DM
05697 #undef B
05698 #undef CUBE
05699 
05700 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05701 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
05702 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
05703 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
05704 
05705 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05706 {
05707         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05708         float WW,OX,OY;
05709 
05710         NSAM = PROJ->get_xsize();
05711         NROW = PROJ->get_ysize();
05712         int ntotal = NSAM*NROW;
05713         float q = 2.0f;
05714         float qt = 8.0f/q;
05715         //  Fix for padding 2x
05716         int ipad = 1;
05717         NSAM *= ipad;
05718         NROW *= ipad;
05719         NNNN = NSAM+2-(NSAM%2);
05720         int NX2 = NSAM/2;
05721         NR2  = NROW/2;
05722 
05723         NANG = int(SS.size())/6;
05724 
05725         EMData* W = new EMData();
05726         int Wnx = NNNN/2;
05727         W->set_size(Wnx,NROW,1);
05728         W->to_zero();
05729         float *Wptr = W->get_data();
05730         float *PROJptr = PROJ->get_data();
05731         for (L=1; L<=NANG; L++) {
05732                 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);
05733                 float  tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) ); 
05734                 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05735                 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05736                 if(OX < 0.0f) {
05737                         OX = -OX;
05738                         OY = -OY;
05739                 }
05740 
05741                 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f ) {
05742                         for(int J=1;J<=NROW;J++) {
05743                                 JY = (J-1);
05744                                 if(JY > NR2) JY -= NROW;
05745 #ifdef _WIN32
05746                                 int xma = _cpp_min(int(0.5f+(q-JY*OY)/OX),NX2);
05747                                 int xmi = _cpp_max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05748 #else
05749                                 int xma = std::min(int(0.5f+(q-JY*OY)/OX),NX2);
05750                                 int xmi = std::max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05751 #endif  //_WIN32
05752                                 if( xmi <= xma) {
05753                                         for(int I=xmi;I<=xma;I++) {
05754                                                 float Y = fabs(OX*I + OY*JY);
05755                                                 W(I+1,J) += exp(-qt*Y*Y);
05756         //cout << " L   "<<L << " I   "<<I << " JY   "<<JY << " ARG   "<<qt*Y*Y <<endl;
05757                                         }
05758                                 }
05759                         }
05760                 } else {
05761                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
05762                 }
05763         }
05764         EMData* proj_in = PROJ;
05765 
05766         PROJ = PROJ->norm_pad( false, ipad);
05767         PROJ->do_fft_inplace();
05768         PROJ->update();
05769         //cout << " x   "<<PROJ->get_xsize() << " y   "<<PROJ->get_ysize() <<endl;
05770         PROJptr = PROJ->get_data();
05771 
05772         float WNRMinv,temp;
05773         float osnr = 1.0f/SNR;
05774         WNRMinv = 1.0f/W(1,1);
05775         for(int J=1;J<=NROW;J++)  {
05776                 JY = J-1;
05777                 if( JY > NR2)  JY -= NROW;
05778                 float sy = JY;
05779                 sy /= NROW;
05780                 sy *= sy;
05781                 for(int I=1;I<=NNNN;I+=2) {
05782                         KX           = (I+1)/2;
05783                         temp         = W(KX,J)*WNRMinv;
05784                         WW           = temp/(temp*temp + osnr);
05785                         // This is supposed to fix fall-off due to Gaussian function in the weighting function
05786                         float sx = KX-1;
05787                         sx /= NSAM;
05788                         WW *= exp(qt*(sy + sx*sx));
05789                         PROJ(I,J)   *= WW;
05790                         PROJ(I+1,J) *= WW;
05791                 }
05792         }
05793         delete W; W = 0;
05794         PROJ->do_ift_inplace();
05795         PROJ->depad();
05796 
05797         float* data_src = PROJ->get_data();
05798         float* data_dst = proj_in->get_data();
05799 
05800         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
05801 
05802         proj_in->update();
05803 
05804         delete PROJ;
05805 }
05806 /*
05807 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05808 {
05809         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05810         float WW,OX,OY,Y;
05811 
05812         NSAM = PROJ->get_xsize();
05813         NROW = PROJ->get_ysize();
05814         //  Fix for padding 2x
05815         int ntotal = NSAM*NROW;
05816         int ipad = 1;
05817         NSAM *= ipad;
05818         NROW *= ipad;
05819         NNNN = NSAM+2-(NSAM%2);
05820         NR2  = NROW/2;
05821 
05822         NANG = int(SS.size())/6;
05823 
05824         EMData* W = new EMData();
05825         int Wnx = NNNN/2;
05826         W->set_size(Wnx,NROW,1);
05827         W->to_zero();
05828         float *Wptr = W->get_data();
05829         float *PROJptr = PROJ->get_data();
05830         for (L=1; L<=NANG; L++) {
05831                 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);
05832                 float  tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) ); 
05833                 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05834                 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05835         //cout << " OX   "<<OX << " OY   "<<OY <<endl;
05836 
05837                 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f) {
05838                         for(int J=1;J<=NROW;J++) {
05839                                 JY = (J-1);
05840                                 if(JY > NR2) JY=JY-NROW;
05841                                 for(int I=1;I<=NNNN/2;I++) {
05842                                         Y =  fabs(OX * (I-1) + OY * JY);
05843                                         if(Y < 2.0f) {
05844                                         W(I,J) += exp(-4*Y*Y);
05845         cout << " L   "<<L << " I   "<<I-1 << " JY   "<<JY << " ARG   "<<4*Y*Y<<endl;}
05846                                 }
05847                         }
05848                 } else {
05849                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
05850                 }
05851         }
05852         EMData* proj_in = PROJ;
05853 
05854         PROJ = PROJ->norm_pad( false, ipad);
05855         PROJ->do_fft_inplace();
05856         PROJ->update();
05857         //cout << " x   "<<PROJ->get_xsize() << " y   "<<PROJ->get_ysize() <<endl;
05858         PROJptr = PROJ->get_data();
05859 
05860         float WNRMinv,temp;
05861         float osnr = 1.0f/SNR;
05862         WNRMinv = 1.0f/W(1,1);
05863         for(int J=1;J<=NROW;J++)
05864                 for(int I=1;I<=NNNN;I+=2) {
05865                         KX           = (I+1)/2;
05866                         temp         = W(KX,J)*WNRMinv;
05867                         WW           = temp/(temp*temp + osnr);
05868                         PROJ(I,J)   *= WW;
05869                         PROJ(I+1,J) *= WW;
05870                 }
05871         delete W; W = 0;
05872         PROJ->do_ift_inplace();
05873         PROJ->depad();
05874 
05875         float* data_src = PROJ->get_data();
05876         float* data_dst = proj_in->get_data();
05877 
05878         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
05879 
05880         proj_in->update();
05881 
05882         delete PROJ;
05883 }
05884 */
05885 #undef PROJ
05886 #undef W
05887 #undef SS
05888 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05889 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
05890 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
05891 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
05892 #define    RI(i,j)                      RI          [(i-1) + ((j-1)*3)]
05893 #define    CC(i)                        CC          [i-1]
05894 #define    CP(i)                        CP          [i-1]
05895 #define    VP(i)                        VP          [i-1]
05896 #define    VV(i)                        VV          [i-1]
05897 #define    AMAX1(i,j)                   i>j?i:j
05898 #define    AMIN1(i,j)                   i<j?i:j
05899 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
05900 {
05901         float rad2deg =(180.0f/3.1415926f);
05902         float deg2rad = (3.1415926f/180.0f);
05903 
05904         int NSAM,NROW,NNNN,NR2,NANG,L,JY;
05905 
05906         NSAM = PROJ->get_xsize();
05907         NROW = PROJ->get_ysize();
05908         NNNN = NSAM+2-(NSAM%2);
05909         NR2  = NROW/2;
05910         NANG = int(SS.size())/6;
05911 
05912         float RI[9];
05913         RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
05914         RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
05915         RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
05916         RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
05917         RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
05918         RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
05919         RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
05920         RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
05921         RI(3,3)=SS(3,NUMP);
05922 
05923         float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
05924 
05925         EMData* W = new EMData();
05926         int Wnx = NNNN/2;
05927         W->set_size(NNNN/2,NROW,1);
05928         W->to_one();
05929         float *Wptr = W->get_data();
05930 
05931         float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
05932 
05933         for (L=1; L<=NANG; L++) {
05934                 if (L != NUMP) {
05935                         CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
05936                         CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
05937                         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);
05938 
05939                         TMP = sqrt(CC(1)*CC(1) +  CC(2)*CC(2) + CC(3)*CC(3));
05940                         CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
05941                         ALPHA=rad2deg*float(asin(CCN));
05942                         if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
05943                         if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
05944                         if(ALPHA<1.0E-6) {
05945                                 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
05946                         } else {
05947                                 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
05948                                 CC(1)   = CC(1)/CCN;CC(2)   = CC(2)/CCN;CC(3)   = CC(3)/CCN;
05949                                 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
05950                                 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
05951                                 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
05952                                 CP(1)   = 0.0;CP(2) = 0.0;
05953                                 VP(1)   = 0.0;VP(2) = 0.0;
05954 
05955                                 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
05956                                 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
05957                                 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
05958                                 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
05959 
05960                                 TMP = CP(1)*VP(2)-CP(2)*VP(1);
05961 
05962                                 //     PREVENT TMP TO BE TOO SMALL, SIGN IS IRRELEVANT
05963                                 TMP = AMAX1(1.0E-4f,fabs(TMP));
05964                                 float tmpinv = 1.0f/TMP;
05965                                 for(int J=1;J<=NROW;J++) {
05966                                         JY = (J-1);
05967                                         if (JY>NR2)  JY=JY-NROW;
05968                                         for(int I=1;I<=NNNN/2;I++) {
05969                                                 FV     = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
05970                                                 RT     = 1.0f-FV/FM;
05971                                                 W(I,J) += ((RT>0.0f)*RT);
05972                                         }
05973                                 }
05974                         }
05975                 }
05976         }
05977 
05978         EMData* proj_in = PROJ;
05979 
05980         PROJ = PROJ->norm_pad( false, 1);
05981         PROJ->do_fft_inplace();
05982         PROJ->update();
05983         float *PROJptr = PROJ->get_data();
05984 
05985         int KX;
05986         float WW;
05987         for(int J=1; J<=NROW; J++)
05988                 for(int I=1; I<=NNNN; I+=2) {
05989                         KX          =  (I+1)/2;
05990                         WW          =  1.0f/W(KX,J);
05991                         PROJ(I,J)   = PROJ(I,J)*WW;
05992                         PROJ(I+1,J) = PROJ(I+1,J)*WW;
05993                 }
05994         delete W; W = 0;
05995         PROJ->do_ift_inplace();
05996         PROJ->depad();
05997 
05998         float* data_src = PROJ->get_data();
05999         float* data_dst = proj_in->get_data();
06000 
06001         int ntotal = NSAM*NROW;
06002         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
06003 
06004         proj_in->update();
06005         delete PROJ;
06006 }
06007 #undef   AMAX1
06008 #undef   AMIN1
06009 #undef   RI
06010 #undef   CC
06011 #undef   CP
06012 #undef   VV
06013 #undef   VP
06014 
06015 
06016 #undef   W
06017 #undef   SS
06018 #undef   PROJ
06019 
06020 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06021 {
06022         float cst  = cs*1.0e7f;
06023 
06024         wgh /= 100.0;
06025         float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06026         float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06027         float ak2 = ak*ak;
06028         float g1 = dzz*1.0e4f*lambda*ak2;
06029         float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06030 
06031         float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06032         if(b_factor != 0.0f)  ctfv *= exp(-b_factor*ak2/4.0f);
06033 
06034         return ctfv;
06035 }
06036 
06037 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06038 {
06039         /***********
06040         ***get the size of the image for validation purpose
06041         **************/
06042         int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();  //Aren't  these  implied?  Please check and let me know, PAP.
06043         /********
06044         ***Exception Handle
06045         *************/
06046         if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06047                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06048 
06049         size_t i, size = (size_t)nx*ny*nz;
06050 
06051         float* img_ptr = image->get_data();
06052         float* mask_ptr = mask->get_data();
06053 
06054         int ln=0;  //length of the output image = number of points under the mask.
06055         for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06056 
06057         EMData* new_image = new EMData();
06058         new_image->set_size(ln,1,1); /* set size of the new image */
06059         float *new_ptr    = new_image->get_data();
06060 
06061         ln=-1;
06062         for(i = 0;i < size;i++){
06063                 if(mask_ptr[i] > 0.5f) {
06064                         ln++;
06065                         new_ptr[ln]=img_ptr[i];
06066                 }
06067         }
06068 
06069         return new_image;
06070 }
06071 
06072 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06073 {
06074         /********
06075         ***Exception Handle
06076         *************/
06077         if(mask == NULL)
06078                 throw ImageDimensionException("The mask cannot be an null image");
06079 
06080         /***********
06081         ***get the size of the mask
06082         **************/
06083         int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06084 
06085         size_t i,size = (size_t)nx*ny*nz;                        /* loop counters */
06086         /* new image declaration */
06087         EMData *new_image = new EMData();
06088         new_image->set_size(nx,ny,nz);           /* set the size of new image */
06089         float *new_ptr  = new_image->get_data(); /* set size of the new image */
06090         float *mask_ptr = mask->get_data();      /* assign a pointer to the mask image */
06091         float *img_ptr  = image->get_data();     /* assign a pointer to the 1D image */
06092         int count = 0;
06093         float sum_under_mask = 0.0 ;
06094         for(i = 0;i < size;i++){
06095                         if(mask_ptr[i] > 0.5f){
06096                                 new_ptr[i] = img_ptr[count];
06097                                 sum_under_mask += img_ptr[count];
06098                                 count++;
06099                                 if( count > image->get_xsize() ) {
06100                                     throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06101                                 }
06102                         }
06103         }
06104 
06105         if( count > image->get_xsize() ) {
06106             throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06107         }
06108 
06109         float avg_under_mask = sum_under_mask / count;
06110         for(i = 0;i < size;i++) {
06111                 if(mask_ptr[i] <= 0.5f)  new_ptr[i] = avg_under_mask;
06112         }
06113         new_image->update();
06114         return new_image;
06115 }
06116 
06117 
06118 
06119 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06120 {
06121         vector<float>new_peak;
06122         int n1=peak1.size()/3;
06123         float p_size2=p_size*p_size;
06124         for (int i=0;i<n1;++i) {
06125                 vector<float>::iterator it2= peak1.begin()+3*i;
06126                 bool push_back1=true;
06127                 int n2=peak2.size()/3;
06128                 /*cout<<"peak2 size==="<<n2<<"i====="<<i<<endl;
06129                        cout<<"new peak size==="<<new_peak.size()/3<<endl;*/
06130                 if(n2 ==0) {
06131                         new_peak.push_back(*it2);
06132                         new_peak.push_back(*(it2+1));
06133                         new_peak.push_back(*(it2+2));
06134                 } else  {
06135                         int j=0;
06136                         while (j< n2-1 ) {
06137                                 vector<float>::iterator it3= peak2.begin()+3*j;
06138                                 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06139                                 if(d2< p_size2 ) {
06140                                         if( (*it2)<(*it3) ) {
06141                                                 new_peak.push_back(*it3);
06142                                                 new_peak.push_back(*(it3+1));
06143                                                 new_peak.push_back(*(it3+2));
06144                                                 peak2.erase(it3);
06145                                                 peak2.erase(it3);
06146                                                 peak2.erase(it3);
06147                                                 push_back1=false;
06148                                         } else {
06149                                                 peak2.erase(it3);
06150                                                 peak2.erase(it3);
06151                                                 peak2.erase(it3);
06152                                         }
06153                                 } else  j=j+1;
06154                                 n2=peak2.size()/3;
06155                         }
06156                         if(push_back1) {
06157                                 new_peak.push_back(*it2);
06158                                 new_peak.push_back(*(it2+1));
06159                                 new_peak.push_back(*(it2+2));
06160                         }
06161                 }
06162         }
06163         return new_peak;
06164 }
06165 
06166 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06167 {
06168         // n size of the covariance/correlation matrix
06169         // covmat --- covariance/correlation matrix (n by n)
06170         // eigval --- returns eigenvalues
06171         // eigvec --- returns eigenvectors
06172 
06173         ENTERFUNC;
06174 
06175         int i;
06176 
06177         // make a copy of covmat so that it will not be overwritten
06178         for ( i = 0 ; i < n * n ; i++ )   eigvec[i] = covmat[i];
06179 
06180         char NEEDV = 'V';
06181         char UPLO = 'U';
06182         int lwork = -1;
06183         int info = 0;
06184         float *work, wsize;
06185 
06186         //  query to get optimal workspace
06187         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06188         lwork = (int)wsize;
06189 
06190         work = (float *)calloc(lwork, sizeof(float));
06191         //  calculate eigs
06192         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06193         free(work);
06194         EXITFUNC;
06195         return info;
06196 }
06197 
06198 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06199 {
06200 
06201         ENTERFUNC;
06202         int len = covmatpy.size();
06203         float *eigvec;
06204         float *eigval;
06205         float *covmat;
06206         int status = 0;
06207         eigval = (float*)calloc(ncov,sizeof(float));
06208         eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06209         covmat = (float*)calloc(ncov*ncov, sizeof(float));
06210 
06211         const float *covmat_ptr;
06212         covmat_ptr = &covmatpy[0];
06213         for(int i=0;i<len;i++){
06214             covmat[i] = covmat_ptr[i];
06215         }
06216 
06217         status = Util::coveig(ncov, covmat, eigval, eigvec);
06218 
06219         vector<float> eigval_py(ncov);
06220         const float *eigval_ptr;
06221         eigval_ptr = &eigval[0];
06222         for(int i=0;i<ncov;i++){
06223             eigval_py[i] = eigval_ptr[i];
06224         }
06225 
06226         vector<float> eigvec_py(ncov*ncov);
06227         const float *eigvec_ptr;
06228         eigvec_ptr = &eigvec[0];
06229         for(int i=0;i<ncov*ncov;i++){
06230             eigvec_py[i] = eigvec_ptr[i];
06231         }
06232 
06233         Dict res;
06234         res["eigval"] = eigval_py;
06235         res["eigvec"] = eigvec_py;
06236 
06237         EXITFUNC;
06238         return res;
06239 }
06240 
06241 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06242 {
06243         int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06244 
06245         k=(int)pw.size();
06246         l=0;
06247         m=k;
06248         n2=n+2;
06249         n1=n+1;
06250         klmd=k+l+m;
06251         klm2d= k+l+m+2;
06252         nklmd=k+l+m+n;
06253         n2d=n+2;
06254         /*size has to be increased when N is large*/
06255         n_larg=klmd*2;
06256         klm2d=n_larg+klm2d;
06257         klmd=n_larg+klmd;
06258         nklmd=n_larg+nklmd;
06259         int size_q=klm2d*n2d;
06260         int size_cu=nklmd*2;
06261         static int i__;
06262 
06263          double *q ;
06264          double *x ;
06265          double *res;
06266          double *cu;
06267          float *q2;
06268          float *pw_;
06269          long int *iu;
06270          double *s;
06271          q = (double*)calloc(size_q,sizeof(double));
06272          x = (double*)calloc(n2d,sizeof(double));
06273          res = (double*)calloc(klmd,sizeof(double));
06274          cu =(double*)calloc(size_cu,sizeof(double));
06275          s = (double*)calloc(klmd,sizeof(double));
06276          q2 = (float*)calloc(size_q,sizeof(float));
06277          iu = (long int*)calloc(size_cu,sizeof(long int));
06278          pw_ = (float*)calloc(k,sizeof(float));
06279 
06280         for( i__ =0;i__<k;++i__)
06281                 {
06282                 pw_[i__]=log(pw[i__]); }
06283         long int l_k=k;
06284         long int l_n=n;
06285         long int l_iswi=iswi;
06286         vector<float> cl1_res;
06287         cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06288         free(q);
06289         free(x);
06290         free(res);
06291         free(s);
06292         free(cu);
06293         free(q2);
06294         free(iu);
06295         free(pw_);
06296         return cl1_res;
06297 }
06298 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)
06299 {
06300     long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06301     float r__1;
06302     int tmp__i;
06303     long int i__, j;
06304     --s;
06305     --res;
06306     iu -= 3;
06307     cu -= 3;
06308     --x;
06309     long int klm2d;
06310     klm2d= *k+*k+2;
06311     klm2d=klm2d+klm2d;
06312     q_dim1 = klm2d;
06313     q_offset = 1 + q_dim1;
06314     q -= q_offset;
06315     q2_dim1 = klm2d;
06316     q2_offset = 1 + q2_dim1;
06317     q2 -= q2_offset;
06318     i__2=0;
06319     i__1 = *n - 1;
06320     tmp__i=0;
06321     for (j = 1; j <= i__1; ++j) {
06322         i__2 = *k;
06323         tmp__i+=1;
06324         for (i__ = 1; i__ <= i__2; ++i__) {
06325             r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06326             q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06327         }
06328     }
06329     for  (i__ = 1; i__ <= i__2; ++i__)
06330       { q2[i__ + *n * q2_dim1] = 1.f;
06331             q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06332         }
06333    vector<float> fit_res;
06334    fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06335    return fit_res;
06336 }
06337 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)
06338 {
06339     /* System generated locals */
06340     long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06341 
06342     /* Local variables */
06343     long int i__, j, m, n1, ii, jj;
06344     double tmp;
06345     vector<float> p;
06346     --x;
06347     q_dim1 = *klm2d;
06348     q_offset = 1 + q_dim1;
06349     q -= q_offset;
06350     q1_dim1 = *klm2d;
06351     q1_offset = 1 + q1_dim1;
06352     q1 -= q1_offset;
06353     --s;
06354     --res;
06355     iu -= 3;
06356     cu -= 3;
06357 
06358     /* Function Body */
06359     long int l = 0;
06360 
06361 /* C==ZHONG HUANG,JULY,12,02;L=0,1,2,3,4,5,6 correspond to different equality constraints */
06362     m = *ks;
06363     n1 = *n + 1;
06364     if (*iswi == 1) {
06365         i__1 = n1;
06366         for (jj = 1; jj <= i__1; ++jj) {
06367             i__2 = *ks;
06368             for (ii = 1; ii <= i__2; ++ii) {
06369         /*      q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];*/
06370 
06371                 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06372                         ;
06373             }
06374         }
06375     } else if (*iswi == 2) {
06376         i__1 = *ks;
06377         for (ii = 1; ii <= i__1; ++ii) {
06378             i__2 = n1;
06379             for (jj = 1; jj <= i__2; ++jj) {
06380                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06381                 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06382             }
06383         }
06384     } else if (*iswi == 3) {
06385         l = 2;
06386         i__1 = n1;
06387         for (jj = 1; jj <= i__1; ++jj) {
06388             i__2 = *ks + 2;
06389             for (ii = 1; ii <= i__2; ++ii) {
06390                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06391             }
06392             i__2 = *ks;
06393             for (ii = 1; ii <= i__2; ++ii) {
06394                 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06395             }
06396         }
06397     } else if (*iswi == 4) {
06398         l = 2;
06399         i__1 = n1;
06400         for (jj = 1; jj <= i__1; ++jj) {
06401             i__2 = *ks + 2;
06402             for (ii = 1; ii <= i__2; ++ii) {
06403                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06404             }
06405             i__2 = *ks;
06406             for (ii = 1; ii <= i__2; ++ii) {
06407                 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06408             }
06409         }
06410     } else if (*iswi == 5) {
06411         l = 1;
06412         i__1 = n1;
06413         for (jj = 1; jj <= i__1; ++jj) {
06414             i__2 = *ks + 1;
06415             for (ii = 1; ii <= i__2; ++ii) {
06416                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06417             }
06418             i__2 = *ks;
06419             for (ii = 1; ii <= i__2; ++ii) {
06420                 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06421             }
06422         }
06423     } else if (*iswi == 6) {
06424         l = 1;
06425         i__1 = n1;
06426         for (jj = 1; jj <= i__1; ++jj) {
06427             i__2 = *ks + 1;
06428             for (ii = 1; ii <= i__2; ++ii) {
06429                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06430             }
06431             i__2 = *ks;
06432             for (ii = 1; ii <= i__2; ++ii) {
06433                 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06434             }
06435         }
06436     } else if (*iswi == 7) {
06437         l = 3;
06438         i__1 = n1;
06439         for (jj = 1; jj <= i__1; ++jj) {
06440             i__2 = *ks + 3;
06441             for (ii = 1; ii <= i__2; ++ii) {
06442                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06443             }
06444             i__2 = *ks;
06445             for (ii = 1; ii <= i__2; ++ii) {
06446                 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06447             }
06448         }
06449     } else if (*iswi == 8) {
06450         l = 4;
06451         i__1 = n1;
06452         for (jj = 1; jj <= i__1; ++jj) {
06453             i__2 = *ks + 4;
06454             for (ii = 1; ii <= i__2; ++ii) {
06455                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06456             }
06457             i__2 = *ks;
06458             for (ii = 1; ii <= i__2; ++ii) {
06459                 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06460             }
06461         }
06462     }
06463 
06464     Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06465     i__1 = *ks;
06466     int tmp__j=0;
06467     for (i__ = 1; i__ <= i__1; ++i__) {
06468         tmp = 0.f;
06469         i__2 = *n - 1;
06470         for (j = 1; j <= i__2; ++j) {
06471         tmp__j=j;
06472             tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06473         }
06474         tmp += x[*n];
06475         p.push_back(static_cast<float>(exp(tmp)));
06476         p.push_back(q1[i__ + q1_dim1]);
06477     }
06478     i__2=*n;
06479     for (i__=1;i__<=i__2;++i__)
06480         { p.push_back(static_cast<float>(x[i__]));}
06481     return p;
06482 }
06483 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06484         double *q, double *x, double *res, double *cu, long int *iu, double *s)
06485 {
06486 
06487     long int q_dim1, q_offset, i__1, i__2;
06488     double d__1;
06489 
06490     static long int i__, j;
06491     static double z__;
06492     static long int n1, n2, ia, ii, kk, in, nk, js;
06493     static double sn, zu, zv;
06494     static long int nk1, klm, nkl, jmn, jpn;
06495     static double cuv;
06496     static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06497     static float xmin;
06498     static double xmax;
06499     static long int iout;
06500     static double xsum;
06501     static long int iineg, maxit;
06502     static double toler;
06503     static float error;
06504     static double pivot;
06505     static long int kforce, iphase;
06506     static double tpivot;
06507 
06508     --s;
06509     --res;
06510     iu -= 3;
06511     cu -= 3;
06512     --x;
06513     q_dim1 = *klm2d;
06514     q_offset = 1 + q_dim1;
06515     q -= q_offset;
06516 
06517     /* Function Body */
06518     maxit = 500;
06519     kode = 0;
06520     toler = 1e-4f;
06521     iter = 0;
06522     n1 = *n + 1;
06523     n2 = *n + 2;
06524     nk = *n + *k;
06525     nk1 = nk + 1;
06526     nkl = nk + *l;
06527     nkl1 = nkl + 1;
06528     klm = *k + *l + *m;
06529     klm1 = klm + 1;
06530     klm2 = klm + 2;
06531     nklm = *n + klm;
06532     kforce = 1;
06533     iter = 0;
06534     js = 1;
06535     ia = 0;
06536 /* SET UP LABELS IN Q. */
06537     i__1 = *n;
06538     for (j = 1; j <= i__1; ++j) {
06539         q[klm2 + j * q_dim1] = (double) j;
06540 /* L10: */
06541     }
06542     i__1 = klm;
06543     for (i__ = 1; i__ <= i__1; ++i__) {
06544         q[i__ + n2 * q_dim1] = (double) (*n + i__);
06545         if (q[i__ + n1 * q_dim1] >= 0.f) {
06546             goto L30;
06547         }
06548         i__2 = n2;
06549         for (j = 1; j <= i__2; ++j) {
06550             q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06551 /* L20: */
06552         }
06553 L30:
06554         ;
06555     }
06556 /* SET UP PHASE 1 COSTS. */
06557     iphase = 2;
06558     i__1 = nklm;
06559     for (j = 1; j <= i__1; ++j) {
06560         cu[(j << 1) + 1] = 0.f;
06561         cu[(j << 1) + 2] = 0.f;
06562         iu[(j << 1) + 1] = 0;
06563         iu[(j << 1) + 2] = 0;
06564 /* L40: */
06565     }
06566     if (*l == 0) {
06567         goto L60;
06568     }
06569     i__1 = nkl;
06570     for (j = nk1; j <= i__1; ++j) {
06571         cu[(j << 1) + 1] = 1.f;
06572         cu[(j << 1) + 2] = 1.f;
06573         iu[(j << 1) + 1] = 1;
06574         iu[(j << 1) + 2] = 1;
06575 /* L50: */
06576     }
06577     iphase = 1;
06578 L60:
06579     if (*m == 0) {
06580         goto L80;
06581     }
06582     i__1 = nklm;
06583     for (j = nkl1; j <= i__1; ++j) {
06584         cu[(j << 1) + 2] = 1.f;
06585         iu[(j << 1) + 2] = 1;
06586         jmn = j - *n;
06587         if (q[jmn + n2 * q_dim1] < 0.f) {
06588             iphase = 1;
06589         }
06590 /* L70: */
06591     }
06592 L80:
06593     if (kode == 0) {
06594         goto L150;
06595     }
06596     i__1 = *n;
06597     for (j = 1; j <= i__1; ++j) {
06598         if ((d__1 = x[j]) < 0.) {
06599             goto L90;
06600         } else if (d__1 == 0) {
06601             goto L110;
06602         } else {
06603             goto L100;
06604         }
06605 L90:
06606         cu[(j << 1) + 1] = 1.f;
06607         iu[(j << 1) + 1] = 1;
06608         goto L110;
06609 L100:
06610         cu[(j << 1) + 2] = 1.f;
06611         iu[(j << 1) + 2] = 1;
06612 L110:
06613         ;
06614     }
06615     i__1 = *k;
06616     for (j = 1; j <= i__1; ++j) {
06617         jpn = j + *n;
06618         if ((d__1 = res[j]) < 0.) {
06619             goto L120;
06620         } else if (d__1 == 0) {
06621             goto L140;
06622         } else {
06623             goto L130;
06624         }
06625 L120:
06626         cu[(jpn << 1) + 1] = 1.f;
06627         iu[(jpn << 1) + 1] = 1;
06628         if (q[j + n2 * q_dim1] > 0.f) {
06629             iphase = 1;
06630         }
06631         goto L140;
06632 L130:
06633         cu[(jpn << 1) + 2] = 1.f;
06634         iu[(jpn << 1) + 2] = 1;
06635         if (q[j + n2 * q_dim1] < 0.f) {
06636             iphase = 1;
06637         }
06638 L140:
06639         ;
06640     }
06641 L150:
06642     if (iphase == 2) {
06643         goto L500;
06644     }
06645 /* COMPUTE THE MARGINAL COSTS. */
06646 L160:
06647     i__1 = n1;
06648     for (j = js; j <= i__1; ++j) {
06649         xsum = 0.;
06650         i__2 = klm;
06651         for (i__ = 1; i__ <= i__2; ++i__) {
06652             ii = (long int) q[i__ + n2 * q_dim1];
06653             if (ii < 0) {
06654                 goto L170;
06655             }
06656             z__ = cu[(ii << 1) + 1];
06657             goto L180;
06658 L170:
06659             iineg = -ii;
06660             z__ = cu[(iineg << 1) + 2];
06661 L180:
06662             xsum += q[i__ + j * q_dim1] * z__;
06663 /*  180       XSUM = XSUM + Q(I,J)*Z */
06664 /* L190: */
06665         }
06666         q[klm1 + j * q_dim1] = xsum;
06667 /* L200: */
06668     }
06669     i__1 = *n;
06670     for (j = js; j <= i__1; ++j) {
06671         ii = (long int) q[klm2 + j * q_dim1];
06672         if (ii < 0) {
06673             goto L210;
06674         }
06675         z__ = cu[(ii << 1) + 1];
06676         goto L220;
06677 L210:
06678         iineg = -ii;
06679         z__ = cu[(iineg << 1) + 2];
06680 L220:
06681         q[klm1 + j * q_dim1] -= z__;
06682 /* L230: */
06683     }
06684 /* DETERMINE THE VECTOR TO ENTER THE BASIS. */
06685 L240:
06686     xmax = 0.f;
06687     if (js > *n) {
06688         goto L490;
06689     }
06690     i__1 = *n;
06691     for (j = js; j <= i__1; ++j) {
06692         zu = q[klm1 + j * q_dim1];
06693         ii = (long int) q[klm2 + j * q_dim1];
06694         if (ii > 0) {
06695             goto L250;
06696         }
06697         ii = -ii;
06698         zv = zu;
06699         zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06700         goto L260;
06701 L250:
06702         zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06703 L260:
06704         if (kforce == 1 && ii > *n) {
06705             goto L280;
06706         }
06707         if (iu[(ii << 1) + 1] == 1) {
06708             goto L270;
06709         }
06710         if (zu <= xmax) {
06711             goto L270;
06712         }
06713         xmax = zu;
06714         in = j;
06715 L270:
06716         if (iu[(ii << 1) + 2] == 1) {
06717             goto L280;
06718         }
06719         if (zv <= xmax) {
06720             goto L280;
06721         }
06722         xmax = zv;
06723         in = j;
06724 L280:
06725         ;
06726     }
06727     if (xmax <= toler) {
06728         goto L490;
06729     }
06730     if (q[klm1 + in * q_dim1] == xmax) {
06731         goto L300;
06732     }
06733     i__1 = klm2;
06734     for (i__ = 1; i__ <= i__1; ++i__) {
06735         q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06736 /* L290: */
06737     }
06738     q[klm1 + in * q_dim1] = xmax;
06739 /* DETERMINE THE VECTOR TO LEAVE THE BASIS. */
06740 L300:
06741     if (iphase == 1 || ia == 0) {
06742         goto L330;
06743     }
06744     xmax = 0.f;
06745     i__1 = ia;
06746     for (i__ = 1; i__ <= i__1; ++i__) {
06747         z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06748         if (z__ <= xmax) {
06749             goto L310;
06750         }
06751         xmax = z__;
06752         iout = i__;
06753 L310:
06754         ;
06755     }
06756     if (xmax <= toler) {
06757         goto L330;
06758     }
06759     i__1 = n2;
06760     for (j = 1; j <= i__1; ++j) {
06761         z__ = q[ia + j * q_dim1];
06762         q[ia + j * q_dim1] = q[iout + j * q_dim1];
06763         q[iout + j * q_dim1] = z__;
06764 /* L320: */
06765     }
06766     iout = ia;
06767     --ia;
06768     pivot = q[iout + in * q_dim1];
06769     goto L420;
06770 L330:
06771     kk = 0;
06772     i__1 = klm;
06773     for (i__ = 1; i__ <= i__1; ++i__) {
06774         z__ = q[i__ + in * q_dim1];
06775         if (z__ <= toler) {
06776             goto L340;
06777         }
06778         ++kk;
06779         res[kk] = q[i__ + n1 * q_dim1] / z__;
06780         s[kk] = (double) i__;
06781 L340:
06782         ;
06783     }
06784 L350:
06785     if (kk > 0) {
06786         goto L360;
06787     }
06788     kode = 2;
06789     goto L590;
06790 L360:
06791     xmin = static_cast<float>( res[1] );
06792     iout = (long int) s[1];
06793     j = 1;
06794     if (kk == 1) {
06795         goto L380;
06796     }
06797     i__1 = kk;
06798     for (i__ = 2; i__ <= i__1; ++i__) {
06799         if (res[i__] >= xmin) {
06800             goto L370;
06801         }
06802         j = i__;
06803         xmin = static_cast<float>( res[i__] );
06804         iout = (long int) s[i__];
06805 L370:
06806         ;
06807     }
06808     res[j] = res[kk];
06809     s[j] = s[kk];
06810 L380:
06811     --kk;
06812     pivot = q[iout + in * q_dim1];
06813     ii = (long int) q[iout + n2 * q_dim1];
06814     if (iphase == 1) {
06815         goto L400;
06816     }
06817     if (ii < 0) {
06818         goto L390;
06819     }
06820     if (iu[(ii << 1) + 2] == 1) {
06821         goto L420;
06822     }
06823     goto L400;
06824 L390:
06825     iineg = -ii;
06826     if (iu[(iineg << 1) + 1] == 1) {
06827         goto L420;
06828     }
06829 /* 400 II = IABS(II) */
06830 L400:
06831     ii = abs(ii);
06832     cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06833     if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06834         goto L420;
06835     }
06836 /* BYPASS INTERMEDIATE VERTICES. */
06837     i__1 = n1;
06838     for (j = js; j <= i__1; ++j) {
06839         z__ = q[iout + j * q_dim1];
06840         q[klm1 + j * q_dim1] -= z__ * cuv;
06841         q[iout + j * q_dim1] = -z__;
06842 /* L410: */
06843     }
06844     q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06845     goto L350;
06846 /* GAUSS-JORDAN ELIMINATION. */
06847 L420:
06848     if (iter < maxit) {
06849         goto L430;
06850     }
06851     kode = 3;
06852     goto L590;
06853 L430:
06854     ++iter;
06855     i__1 = n1;
06856     for (j = js; j <= i__1; ++j) {
06857         if (j != in) {
06858             q[iout + j * q_dim1] /= pivot;
06859         }
06860 /* L440: */
06861     }
06862 /* IF PERMITTED, USE SUBROUTINE COL OF THE DESCRIPTION */
06863 /* SECTION AND REPLACE THE FOLLOWING SEVEN STATEMENTS DOWN */
06864 /* TO AND INCLUDING STATEMENT NUMBER 460 BY.. */
06865 /*     DO 460 J=JS,N1 */
06866 /*        IF(J .EQ. IN) GO TO 460 */
06867 /*        Z = -Q(IOUT,J) */
06868 /*        CALL COL(Q(1,J), Q(1,IN), Z, IOUT, KLM1) */
06869 /* 460 CONTINUE */
06870     i__1 = n1;
06871     for (j = js; j <= i__1; ++j) {
06872         if (j == in) {
06873             goto L460;
06874         }
06875         z__ = -q[iout + j * q_dim1];
06876         i__2 = klm1;
06877         for (i__ = 1; i__ <= i__2; ++i__) {
06878             if (i__ != iout) {
06879                 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06880             }
06881 /* L450: */
06882         }
06883 L460:
06884         ;
06885     }
06886     tpivot = -pivot;
06887     i__1 = klm1;
06888     for (i__ = 1; i__ <= i__1; ++i__) {
06889         if (i__ != iout) {
06890             q[i__ + in * q_dim1] /= tpivot;
06891         }
06892 /* L470: */
06893     }
06894     q[iout + in * q_dim1] = 1.f / pivot;
06895     z__ = q[iout + n2 * q_dim1];
06896     q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
06897     q[klm2 + in * q_dim1] = z__;
06898     ii = (long int) abs(z__);
06899     if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
06900         goto L240;
06901     }
06902     i__1 = klm2;
06903     for (i__ = 1; i__ <= i__1; ++i__) {
06904         z__ = q[i__ + in * q_dim1];
06905         q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
06906         q[i__ + js * q_dim1] = z__;
06907 /* L480: */
06908     }
06909     ++js;
06910     goto L240;
06911 /* TEST FOR OPTIMALITY. */
06912 L490:
06913     if (kforce == 0) {
06914         goto L580;
06915     }
06916     if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
06917         goto L500;
06918     }
06919     kforce = 0;
06920     goto L240;
06921 /* SET UP PHASE 2 COSTS. */
06922 L500:
06923     iphase = 2;
06924     i__1 = nklm;
06925     for (j = 1; j <= i__1; ++j) {
06926         cu[(j << 1) + 1] = 0.f;
06927         cu[(j << 1) + 2] = 0.f;
06928 /* L510: */
06929     }
06930     i__1 = nk;
06931     for (j = n1; j <= i__1; ++j) {
06932         cu[(j << 1) + 1] = 1.f;
06933         cu[(j << 1) + 2] = 1.f;
06934 /* L520: */
06935     }
06936     i__1 = klm;
06937     for (i__ = 1; i__ <= i__1; ++i__) {
06938         ii = (long int) q[i__ + n2 * q_dim1];
06939         if (ii > 0) {
06940             goto L530;
06941         }
06942         ii = -ii;
06943         if (iu[(ii << 1) + 2] == 0) {
06944             goto L560;
06945         }
06946         cu[(ii << 1) + 2] = 0.f;
06947         goto L540;
06948 L530:
06949         if (iu[(ii << 1) + 1] == 0) {
06950             goto L560;
06951         }
06952         cu[(ii << 1) + 1] = 0.f;
06953 L540:
06954         ++ia;
06955         i__2 = n2;
06956         for (j = 1; j <= i__2; ++j) {
06957             z__ = q[ia + j * q_dim1];
06958             q[ia + j * q_dim1] = q[i__ + j * q_dim1];
06959             q[i__ + j * q_dim1] = z__;
06960 /* L550: */
06961         }
06962 L560:
06963         ;
06964     }
06965     goto L160;
06966 L570:
06967     if (q[klm1 + n1 * q_dim1] <= toler) {
06968         goto L500;
06969     }
06970     kode = 1;
06971     goto L590;
06972 L580:
06973     if (iphase == 1) {
06974         goto L570;
06975     }
06976 /* PREPARE OUTPUT. */
06977     kode = 0;
06978 L590:
06979     xsum = 0.;
06980     i__1 = *n;
06981     for (j = 1; j <= i__1; ++j) {
06982         x[j] = 0.f;
06983 /* L600: */
06984     }
06985     i__1 = klm;
06986     for (i__ = 1; i__ <= i__1; ++i__) {
06987         res[i__] = 0.f;
06988 /* L610: */
06989     }
06990     i__1 = klm;
06991     for (i__ = 1; i__ <= i__1; ++i__) {
06992         ii = (long int) q[i__ + n2 * q_dim1];
06993         sn = 1.f;
06994         if (ii > 0) {
06995             goto L620;
06996         }
06997         ii = -ii;
06998         sn = -1.f;
06999 L620:
07000         if (ii > *n) {
07001             goto L630;
07002         }
07003         x[ii] = sn * q[i__ + n1 * q_dim1];
07004         goto L640;
07005 L630:
07006         iimn = ii - *n;
07007         res[iimn] = sn * q[i__ + n1 * q_dim1];
07008         if (ii >= n1 && ii <= nk) {
07009             xsum += q[i__ + n1 * q_dim1];
07010         }
07011 L640:
07012         ;
07013     }
07014     error = (float)xsum;
07015     return;
07016 }
07017 
07018 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07019 {
07020         int j,d;
07021         EMData * e = new EMData();
07022         float *eptr, *imgptr;
07023         imgptr = img->get_data();
07024         float SSE = 0.f;
07025         for (j = 0 ; j < N ; j++) {
07026                 e->read_image(images,S[j]);
07027                 eptr = e->get_data();
07028                 for (d = 0; d < size; d++) {
07029                         SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07030                 }
07031         delete e;
07032         return SSE;
07033 }
07034 
07035 
07036 #define         mymax(x,y)              (((x)>(y))?(x):(y))
07037 #define         mymin(x,y)              (((x)<(y))?(x):(y))
07038 #define         sign(x,y)               (((((y)>0)?(1):(-1))*(y!=0))*(x))
07039 
07040 
07041 #define         quadpi                  3.141592653589793238462643383279502884197
07042 #define         dgr_to_rad              quadpi/180
07043 #define         deg_to_rad              quadpi/180
07044 #define         rad_to_deg              180/quadpi
07045 #define         rad_to_dgr              180/quadpi
07046 #define         TRUE                    1
07047 #define         FALSE                   0
07048 
07049 
07050 #define theta(i)                theta   [i-1]
07051 #define phi(i)                  phi     [i-1]
07052 #define weight(i)               weight  [i-1]
07053 #define lband(i)                lband   [i-1]
07054 #define ts(i)                   ts      [i-1]
07055 #define thetast(i)              thetast [i-1]
07056 #define key(i)                  key     [i-1]
07057 
07058 
07059 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07060 {
07061 
07062         ENTERFUNC;
07063 
07064         if ( th.size() != ph.size() ) {
07065                 LOGERR("images not same size");
07066                 throw ImageFormatException( "images not same size");
07067         }
07068 
07069         // rand_seed
07070         srand(10);
07071 
07072         int i,*key;
07073         int len = th.size();
07074         double *theta,*phi,*weight;
07075         theta   =       (double*) calloc(len,sizeof(double));
07076         phi     =       (double*) calloc(len,sizeof(double));
07077         weight  =       (double*) calloc(len,sizeof(double));
07078         key     =       (int*) calloc(len,sizeof(int));
07079         const float *thptr, *phptr;
07080 
07081         thptr = &th[0];
07082         phptr = &ph[0];
07083         for(i=1;i<=len;i++){
07084                 key(i) = i;
07085                 weight(i) = 0.0;
07086         }
07087 
07088         for(i = 0;i<len;i++){
07089                 theta[i] = thptr[i];
07090                 phi[i]   = phptr[i];
07091         }
07092 
07093         //  sort by theta
07094         Util::hsortd(theta, phi, key, len, 1);
07095 
07096         //Util::voronoidiag(theta,phi, weight, len);
07097         Util::voronoi(phi, theta, weight, len);
07098 
07099         //sort by key
07100         Util::hsortd(weight, weight, key, len, 2);
07101 
07102         free(theta);
07103         free(phi);
07104         free(key);
07105         vector<double> wt;
07106         double count = 0;
07107         for(i=1; i<= len; i++)
07108         {
07109                 wt.push_back(weight(i));
07110                 count += weight(i);
07111         }
07112 
07113         //if( abs(count-6.28) > 0.1 )
07114         //{
07115         //    printf("Warning: SUM OF VORONOI CELLS AREAS IS %lf, should 2*PI\n", count);
07116         //}
07117 
07118         free(weight);
07119 
07120         EXITFUNC;
07121         return wt;
07122 
07123 }
07124 
07125 struct  tmpstruct{
07126         double theta1,phi1;
07127         int key1;
07128         };
07129 
07130 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07131 {
07132         ENTERFUNC;
07133         vector<tmpstruct> tmp(len);
07134         int i;
07135         for(i = 1;i<=len;i++)
07136         {
07137                 tmp[i-1].theta1 = theta(i);
07138                 tmp[i-1].phi1 = phi(i);
07139                 tmp[i-1].key1 = key(i);
07140         }
07141 
07142         if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07143         if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07144 
07145         for(i = 1;i<=len;i++)
07146         {
07147                 theta(i) = tmp[i-1].theta1;
07148                 phi(i)   = tmp[i-1].phi1;
07149                 key(i)   = tmp[i-1].key1;
07150         }
07151         EXITFUNC;
07152 }
07153 
07154 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07155 {
07156         return(tmp1.theta1 < tmp2.theta1);
07157 }
07158 
07159 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07160 {
07161         return(tmp1.key1 < tmp2.key1);
07162 }
07163 
07164 /******************  VORONOI DIAGRAM **********************************/
07165 /*
07166 void Util::voronoidiag(double *theta,double *phi,double* weight,int n)
07167 {
07168         ENTERFUNC;
07169 
07170         int     *lband;
07171         double  aat=0.0f,*ts;
07172         double  aa,acum,area;
07173         int     last;
07174         int numth       =       1;
07175         int nbt         =       1;//mymax((int)(sqrt((n/500.0))) , 3);
07176 
07177         int i,it,l,k;
07178         int nband,lb,low,medium,lhigh,lbw,lenw;
07179 
07180 
07181         lband   =       (int*)calloc(nbt,sizeof(int));
07182         ts      =       (double*)calloc(nbt,sizeof(double));
07183 
07184         if(lband == NULL || ts == NULL ){
07185                 fprintf(stderr,"memory allocation failure!\n");
07186                 exit(1);
07187         }
07188 
07189         nband=nbt;
07190 
07191         while(nband>0){
07192                 Util::angstep(ts,nband);
07193 
07194                 l=1;
07195                 for(i=1;i<=n;i++){
07196                         if(theta(i)>ts(l)){
07197                                 lband(l)=i;
07198                                 l=l+1;
07199                                 if(l>nband)  exit(1);
07200                         }
07201                 }
07202 
07203                 l=1;
07204                 for(i=1;i<=n;i++){
07205                         if(theta(i)>ts(l)){
07206                                 lband(l)=i;
07207                                 l=l+1;
07208                                 if(l>nband)  exit(1);
07209                         }
07210                 }
07211 
07212                 lband(l)=n+1;
07213                 acum=0.0;
07214                 for(it=l;it>=1;it-=numth){
07215                         for(i=it;i>=mymax(1,it-numth+1);i--){
07216                         if(i==l) last   =        TRUE;
07217                         else     last   =        FALSE;
07218 
07219                         if(l==1){
07220                                 lb=1;
07221                                 low=1;
07222                                 medium=n+1;
07223                                 lhigh=n-lb+1;
07224                                 lbw=1;
07225                         }
07226                         else if(i==1){
07227                                 lb=1;
07228                                 low=1;
07229                                 medium=lband(1);
07230                                 lhigh=lband(2)-1;
07231                                 lbw=1;
07232                         }
07233                         else if(i==l){
07234                                 if(l==2)        lb=1;
07235                                 else            lb=lband(l-2);
07236                                 low=lband(l-1)-lb+1;
07237                                 medium=lband(l)-lb+1;
07238                                 lhigh=n-lb+1;
07239                                 lbw=lband(i-1);
07240                         }
07241                         else{
07242                                 if(i==2)        lb=1;
07243                                 else            lb=lband(i-2);
07244                                 low=lband(i-1)-lb+1;
07245                                 medium=lband(i)-lb+1;
07246                                 lhigh=lband(i+1)-1-lb+1;
07247                                 lbw=lband(i-1);
07248                         }
07249                         lenw=medium-low;
07250 
07251 
07252                         Util::voronoi(&phi(lb),&theta(lb),&weight(lbw),lenw,low,medium,lhigh,last);
07253 
07254 
07255                         if(nband>1){
07256                                 if(i==1)        area=quadpi*2.0*(1.0-cos(ts(1)*dgr_to_rad));
07257                                 else            area=quadpi*2.0*(cos(ts(i-1)*dgr_to_rad)-cos(ts(i)*dgr_to_rad));
07258 
07259                                 aa = 0.0;
07260                                 for(k = lbw;k<=lbw+lenw-1;k++)
07261                                         aa = aa+weight(k);
07262 
07263                                 acum=acum+aa;
07264                                 aat=aa/area;
07265                                 }
07266 
07267                         }
07268                         for(i=it;mymax(1,it-numth+1);i--){
07269                         if(fabs(aat-1.0)>0.02){
07270                                 nband=mymax(0,mymin( (int)(((float)nband) * 0.75) ,nband-1) );
07271                                 goto  label2;
07272                                 }
07273                         }
07274                 acum=acum/quadpi/2.0;
07275                 exit(1);
07276 label2:
07277 
07278                 continue;
07279                 }
07280 
07281         free(ts);
07282         free(lband);
07283 
07284         }
07285 
07286         EXITFUNC;
07287 }
07288 
07289 
07290 void Util::angstep(double* thetast,int len){
07291 
07292         ENTERFUNC;
07293 
07294         double t1,t2,tmp;
07295         int i;
07296         if(len>1){
07297                 t1=0;
07298                 for(i=1;i<=len-1;i++){
07299                         tmp=cos(t1)-1.0/((float)len);
07300                         t2=acos(sign(mymin(1.0,fabs(tmp)),tmp));
07301                         thetast(i)=t2 * rad_to_deg;
07302                         t1=t2;
07303                 }
07304         }
07305         thetast(len)=90.0;
07306 
07307         EXITFUNC;
07308 }
07309 */
07310 /*
07311 void Util::voronoi(double *phi, double *theta, double *weight, int lenw, int low, int medium, int nt, int last)
07312 {
07313 
07314         ENTERFUNC;
07315         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07316         int nt6, n, ier,nout,lnew,mdup,nd;
07317         int i,k,mt,status;
07318 
07319 
07320         double *ds, *x, *y, *z;
07321         double tol=1.0e-8;
07322         double a;
07323 
07324         if(last){
07325                 if(medium>nt)  n = nt+nt;
07326                 else           n = nt+nt-medium+1;
07327         }
07328         else{
07329                 n=nt;
07330         }
07331 
07332         nt6 = n*6;
07333 
07334         list = (int*)calloc(nt6,sizeof(int));
07335         lptr = (int*)calloc(nt6,sizeof(int));
07336         lend = (int*)calloc(n  ,sizeof(int));
07337         iwk  = (int*)calloc(n  ,sizeof(int));
07338         good = (int*)calloc(n  ,sizeof(int));
07339         key  = (int*)calloc(n  ,sizeof(int));
07340         indx = (int*)calloc(n  ,sizeof(int));
07341         lcnt = (int*)calloc(n  ,sizeof(int));
07342 
07343         ds      =       (double*) calloc(n,sizeof(double));
07344         x       =       (double*) calloc(n,sizeof(double));
07345         y       =       (double*) calloc(n,sizeof(double));
07346         z       =       (double*) calloc(n,sizeof(double));
07347 
07348         if (list == NULL ||
07349         lptr == NULL ||
07350         lend == NULL ||
07351         iwk  == NULL ||
07352         good == NULL ||
07353         key  == NULL ||
07354         indx == NULL ||
07355         lcnt == NULL ||
07356         x    == NULL ||
07357         y    == NULL ||
07358         z    == NULL ||
07359         ds   == NULL) {
07360                 printf("memory allocation failure!\n");
07361                 exit(1);
07362         }
07363 
07364 
07365 
07366         for(i = 1;i<=nt;i++){
07367                 x[i-1] = theta(i);
07368                 y[i-1] = phi(i);
07369         }
07370 
07371 
07372 
07373         if (last) {
07374                 for(i=nt+1;i<=n;i++){
07375                         x[i-1]=180.0-x[2*nt-i];
07376                         y[i-1]=180.0+y[2*nt-i];
07377                 }
07378         }
07379 
07380 
07381         Util::disorder2(x,y,key,n);
07382 
07383         Util::ang_to_xyz(x,y,z,n);
07384 
07385 
07386         //  Make sure that first three are no colinear
07387         label1:
07388         for(k=0; k<2; k++){
07389                 for(i=k+1; i<3; i++){
07390                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol){
07391                                 Util::flip23(x, y, z, key, k, n);
07392                                 goto label1;
07393                         }
07394                 }
07395         }
07396 
07397 
07398         status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew,indx,lcnt, iwk, good, ds, &ier);
07399 
07400 
07401         if (status != 0) {
07402                 printf(" error in trmsh3 \n");
07403                 exit(1);
07404         }
07405 
07406 
07407         mdup=n-nout;
07408         if (ier == -2) {
07409                 printf("*** Error in TRMESH:the first three nodes are collinear***\n");
07410                 exit(1);
07411         }
07412         else if (ier > 0) {
07413                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07414                 exit(1);
07415         }
07416 
07417         nd=0;
07418         for (k=1;k<=n;k++){
07419                 if (indx[k-1]>0){
07420                         nd++;
07421                         good[nd-1]=k;
07422                 }
07423         }
07424 
07425 
07426         for(i = 1;i<=nout;i++) {
07427                 k=good[i-1];
07428                 if (key[k-1] >= low && key[k-1]<medium){
07429                         a = Util::areav_(&i,&nout,x,y,z,list,lptr,lend,&ier);
07430                         if (ier != 0){
07431                                 weight[key[k-1]-low] =-1.0;
07432                         }
07433                         else {
07434                                 weight[key[k-1]-low]=a/lcnt[i-1];
07435                         }
07436                 }
07437         }
07438 
07439 // Fill out the duplicated weights
07440         for(i = 1;i<=n;i++){
07441                 mt=-indx[i-1];
07442                 if (mt>0){
07443                         k=good[mt-1];
07444 //  This is a duplicated entry, get the already calculated
07445 //   weight and assign it.
07446                         if (key[i-1]>=low && key[i-1]<medium){
07447 //  Is it already calculated weight??
07448                                 if(key[k-1]>=low && key[k-1]<medium){
07449                                         weight[key[i-1]-low]=weight[key[k-1]-low];
07450                                 }
07451                                 else{
07452 //  No, the weight is from the outside of valid region, calculate it anyway
07453                                         a = Util::areav_(&mt, &nout, x, y, z, list, lptr, lend, &ier);
07454                                         if (ier != 0){
07455                                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07456                                                 weight[key[i-1]-low] =-1.0;
07457                                         }
07458                                         else {
07459                                                 weight[key[i-1]-low] = a/lcnt[mt-1];
07460                                         }
07461                                 }
07462                         }
07463                 }
07464         }
07465 
07466 
07467         free(list);
07468         free(lend);
07469         free(iwk);
07470         free(good);
07471         free(key);
07472 
07473         free(indx);
07474         free(lcnt);
07475         free(ds);
07476         free(x);
07477         free(y);
07478         free(z);
07479         EXITFUNC;
07480 }
07481 */
07482 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07483 {
07484 
07485         ENTERFUNC;
07486 
07487         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07488         int nt6, n, ier, nout, lnew, mdup, nd;
07489         int i,k,mt,status;
07490 
07491 
07492         double *ds, *x, *y, *z;
07493         double tol  = 1.0e-8;
07494         double dtol = 15;
07495         double a;
07496 
07497         /*if(last){
07498                 if(medium>nt)  n = nt+nt;
07499                 else           n = nt+nt-medium+1;
07500         }
07501         else{
07502                 n=nt;
07503         }*/
07504 
07505         n = nt + nt;
07506 
07507         nt6 = n*6;
07508 
07509         list = (int*)calloc(nt6,sizeof(int));
07510         lptr = (int*)calloc(nt6,sizeof(int));
07511         lend = (int*)calloc(n  ,sizeof(int));
07512         iwk  = (int*)calloc(n  ,sizeof(int));
07513         good = (int*)calloc(n  ,sizeof(int));
07514         key  = (int*)calloc(n  ,sizeof(int));
07515         indx = (int*)calloc(n  ,sizeof(int));
07516         lcnt = (int*)calloc(n  ,sizeof(int));
07517 
07518         ds      =       (double*) calloc(n,sizeof(double));
07519         x       =       (double*) calloc(n,sizeof(double));
07520         y       =       (double*) calloc(n,sizeof(double));
07521         z       =       (double*) calloc(n,sizeof(double));
07522 
07523         if (list == NULL ||
07524         lptr == NULL ||
07525         lend == NULL ||
07526         iwk  == NULL ||
07527         good == NULL ||
07528         key  == NULL ||
07529         indx == NULL ||
07530         lcnt == NULL ||
07531         x    == NULL ||
07532         y    == NULL ||
07533         z    == NULL ||
07534         ds   == NULL) {
07535                 printf("memory allocation failure!\n");
07536                 exit(1);
07537         }
07538 
07539         bool colinear=true;
07540         while(colinear)
07541         {
07542 
07543         L1:
07544             for(i = 0; i<nt; i++){
07545                 x[i] = theta[i];
07546                 y[i] = phi[i];
07547                 x[nt+i] = 180.0 - x[i];
07548                 y[nt+i] = 180.0 + y[i];
07549             }
07550 
07551             Util::disorder2(x, y, key, n);
07552 
07553             // check if the first three angles are not close, else shuffle
07554             double val;
07555             for(k=0; k<2; k++){
07556                 for(i=k+1; i<3; i++){
07557                     val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07558                     if( val  < dtol) {
07559                         goto L1;
07560                     }
07561                 }
07562             }
07563 
07564             Util::ang_to_xyz(x, y, z, n);
07565 
07566             //  Make sure that first three has no duplication
07567             bool dupnode=true;
07568             dupnode=true;
07569             while(dupnode)
07570             {
07571                 for(k=0; k<2; k++){
07572                     for(i=k+1; i<3; i++){
07573                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07574                                 Util::flip23(x, y, z, key, k, n);
07575                                 continue;
07576                         }
07577                     }
07578                 }
07579                 dupnode = false;
07580             }
07581 
07582 
07583             ier = 0;
07584 
07585             status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07586 
07587             if (status != 0) {
07588                 printf(" error in trmsh3 \n");
07589                 exit(1);
07590             }
07591 
07592             if (ier > 0) {
07593                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07594                 exit(1);
07595             }
07596 
07597             mdup=n-nout;
07598             if (ier == -2) {
07599                 //printf("in TRMESH:the first three nodes are colinear*** disorder again\n");
07600             }
07601             else
07602             {
07603                 colinear=false;
07604             }
07605         }
07606 
07607 
07608         Assert( ier != -2 );
07609 //  Create a list of unique nodes GOOD, the numbers refer to locations on the full list
07610 //  INDX contains node numbers from the squeezed list
07611         nd=0;
07612         for (k=1; k<=n; k++){
07613                 if (indx[k-1]>0) {
07614                         nd++;
07615                         good[nd-1]=k;
07616                 }
07617         }
07618 
07619 //
07620 // *** Compute the Voronoi region areas.
07621 //
07622         for(i = 1; i<=nout; i++) {
07623                 k=good[i-1];
07624                 //  We only need n weights from hemisphere
07625                 if (key[k-1] <= nt) {
07626 //  CALCULATE THE AREA
07627                         a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07628                         if (ier != 0){
07629 //  We set the weight to -1, this will signal the error in the calling
07630 //   program, as the area will turn out incorrect
07631                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07632                                 weight[key[k-1]-1] =-1.0;
07633                         } else {
07634 //  Assign the weight
07635                                 weight[key[k-1]-1]=a/lcnt[i-1];
07636                         }
07637                 }
07638         }
07639 
07640 
07641 // Fill out the duplicated weights
07642         for(i = 1; i<=n; i++){
07643                 mt =- indx[i-1];
07644                 if (mt>0){
07645                         k = good[mt-1];
07646 //  This is a duplicated entry, get the already calculated
07647 //   weight and assign it.
07648                 //  We only need n weights from hemisphere
07649                         if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07650                         }
07651         }
07652 
07653         free(list);
07654         free(lend);
07655         free(iwk);
07656         free(good);
07657         free(key);
07658         free(lptr);
07659         free(indx);
07660         free(lcnt);
07661         free(ds);
07662         free(x);
07663         free(y);
07664         free(z);
07665 
07666 
07667         EXITFUNC;
07668 }
07669 
07670 void Util::disorder2(double *x,double *y, int *key, int len)
07671 {
07672         ENTERFUNC;
07673         int k, i;
07674         for(i=0; i<len; i++) key[i]=i+1;
07675 
07676         for(i = 0; i<len;i++){
07677                 k = rand()%len;
07678                 std::swap(key[k], key[i]);
07679                 std::swap(x[k], x[i]);
07680                 std::swap(y[k], y[i]);
07681         }
07682         EXITFUNC;
07683 }
07684 
07685 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07686 {
07687         ENTERFUNC;
07688         double costheta,sintheta,cosphi,sinphi;
07689         for(int i = 0;  i<len;  i++)
07690         {
07691                 cosphi = cos(y[i]*dgr_to_rad);
07692                 sinphi = sin(y[i]*dgr_to_rad);
07693                 if(fabs(x[i]-90.0)< 1.0e-5){
07694                         x[i] = cosphi;
07695                         y[i] = sinphi;
07696                         z[i] = 0.0;
07697                 }
07698                 else{
07699                         costheta = cos(x[i]*dgr_to_rad);
07700                         sintheta = sin(x[i]*dgr_to_rad);
07701                         x[i] = cosphi*sintheta;
07702                         y[i] = sinphi*sintheta;
07703                         z[i] = costheta;
07704                 }
07705         }
07706         EXITFUNC;
07707 }
07708 
07709 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07710 {
07711         ENTERFUNC;
07712         int i = k;
07713         while( i == k )  i = rand()%len;
07714         std::swap(key[i], key[k]);
07715         std::swap(x[i], x[k]);
07716         std::swap(y[i], y[k]);
07717         std::swap(z[i], z[k]);
07718         EXITFUNC;
07719 }
07720 
07721 
07722 #undef  mymax
07723 #undef  mymin
07724 #undef  sign
07725 #undef  quadpi
07726 #undef  dgr_to_rad
07727 #undef  deg_to_rad
07728 #undef  rad_to_deg
07729 #undef  rad_to_dgr
07730 #undef  TRUE
07731 #undef  FALSE
07732 #undef  theta
07733 #undef  phi
07734 #undef  weight
07735 #undef  lband
07736 #undef  ts
07737 #undef  thetast
07738 #undef  key
07739 
07740 
07741 /*################################################################################################
07742 ##########  strid.f -- translated by f2c (version 20030320). ###################################
07743 ######   You must link the resulting object file with the libraries: #############################
07744 ####################    -lf2c -lm   (in that order)   ############################################
07745 ################################################################################################*/
07746 
07747 /* Common Block Declarations */
07748 
07749 
07750 #define TRUE_ (1)
07751 #define FALSE_ (0)
07752 #define abs(x) ((x) >= 0 ? (x) : -(x))
07753 
07754 struct stcom_{
07755     double y;
07756 };
07757 stcom_ stcom_1;
07758 #ifdef KR_headers
07759 double floor();
07760 int i_dnnt(x) double *x;
07761 #else
07762 int i_dnnt(double *x)
07763 #endif
07764 {
07765         return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07766 }
07767 
07768 
07769 
07770 
07771 /* ____________________STRID______________________________________ */
07772 /* Subroutine */ int Util::trmsh3_(int *n0, double *tol, double *x,
07773         double *y, double *z__, int *n, int *list, int *
07774         lptr, int *lend, int *lnew, int *indx, int *lcnt,
07775         int *near__, int *next, double *dist, int *ier)
07776 {
07777     /* System generated locals */
07778     int i__1, i__2;
07779 
07780     /* Local variables */
07781     static double d__;
07782     static int i__, j;
07783     static double d1, d2, d3;
07784     static int i0, lp, kt, ku, lpl, nku;
07785     extern long int left_(double *, double *, double *, double
07786             *, double *, double *, double *, double *,
07787             double *);
07788     static int nexti;
07789     extern /* Subroutine */ int addnod_(int *, int *, double *,
07790             double *, double *, int *, int *, int *,
07791             int *, int *);
07792 
07793 
07794 /* *********************************************************** */
07795 
07796 /*                                              From STRIPACK */
07797 /*                                            Robert J. Renka */
07798 /*                                  Dept. of Computer Science */
07799 /*                                       Univ. of North Texas */
07800 /*                                           renka@cs.unt.edu */
07801 /*                                                   01/20/03 */
07802 
07803 /*   This is an alternative to TRMESH with the inclusion of */
07804 /* an efficient means of removing duplicate or nearly dupli- */
07805 /* cate nodes. */
07806 
07807 /*   This subroutine creates a Delaunay triangulation of a */
07808 /* set of N arbitrarily distributed points, referred to as */
07809 /* nodes, on the surface of the unit sphere.  Refer to Sub- */
07810 /* routine TRMESH for definitions and a list of additional */
07811 /* subroutines.  This routine is an alternative to TRMESH */
07812 /* with the inclusion of an efficient means of removing dup- */
07813 /* licate or nearly duplicate nodes. */
07814 
07815 /*   The algorithm has expected time complexity O(N*log(N)) */
07816 /* for random nodal distributions. */
07817 
07818 
07819 /* On input: */
07820 
07821 /*       N0 = Number of nodes, possibly including duplicates. */
07822 /*            N0 .GE. 3. */
07823 
07824 /*       TOL = Tolerance defining a pair of duplicate nodes: */
07825 /*             bound on the deviation from 1 of the cosine of */
07826 /*             the angle between the nodes.  Note that */
07827 /*             |1-cos(A)| is approximately A*A/2. */
07828 
07829 /* The above parameters are not altered by this routine. */
07830 
07831 /*       X,Y,Z = Arrays of length at least N0 containing the */
07832 /*               Cartesian coordinates of nodes.  (X(K),Y(K), */
07833 /*               Z(K)) is referred to as node K, and K is re- */
07834 /*               ferred to as a nodal index.  It is required */
07835 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
07836 /*               K.  The first three nodes must not be col- */
07837 /*               linear (lie on a common great circle). */
07838 
07839 /*       LIST,LPTR = Arrays of length at least 6*N0-12. */
07840 
07841 /*       LEND = Array of length at least N0. */
07842 
07843 /*       INDX = Array of length at least N0. */
07844 
07845 /*       LCNT = Array of length at least N0 (length N is */
07846 /*              sufficient). */
07847 
07848 /*       NEAR,NEXT,DIST = Work space arrays of length at */
07849 /*                        least N0.  The space is used to */
07850 /*                        efficiently determine the nearest */
07851 /*                        triangulation node to each un- */
07852 /*                        processed node for use by ADDNOD. */
07853 
07854 /* On output: */
07855 
07856 /*       N = Number of nodes in the triangulation.  3 .LE. N */
07857 /*           .LE. N0, or N = 0 if IER < 0. */
07858 
07859 /*       X,Y,Z = Arrays containing the Cartesian coordinates */
07860 /*               of the triangulation nodes in the first N */
07861 /*               locations.  The original array elements are */
07862 /*               shifted down as necessary to eliminate dup- */
07863 /*               licate nodes. */
07864 
07865 /*       LIST = Set of nodal indexes which, along with LPTR, */
07866 /*              LEND, and LNEW, define the triangulation as a */
07867 /*              set of N adjacency lists -- counterclockwise- */
07868 /*              ordered sequences of neighboring nodes such */
07869 /*              that the first and last neighbors of a bound- */
07870 /*              ary node are boundary nodes (the first neigh- */
07871 /*              bor of an interior node is arbitrary).  In */
07872 /*              order to distinguish between interior and */
07873 /*              boundary nodes, the last neighbor of each */
07874 /*              boundary node is represented by the negative */
07875 /*              of its index. */
07876 
07877 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
07878 /*              correspondence with the elements of LIST. */
07879 /*              LIST(LPTR(I)) indexes the node which follows */
07880 /*              LIST(I) in cyclical counterclockwise order */
07881 /*              (the first neighbor follows the last neigh- */
07882 /*              bor). */
07883 
07884 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
07885 /*              points to the last neighbor of node K for */
07886 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
07887 /*              only if K is a boundary node. */
07888 
07889 /*       LNEW = Pointer to the first empty location in LIST */
07890 /*              and LPTR (list length plus one).  LIST, LPTR, */
07891 /*              LEND, and LNEW are not altered if IER < 0, */
07892 /*              and are incomplete if IER > 0. */
07893 
07894 /*       INDX = Array of output (triangulation) nodal indexes */
07895 /*              associated with input nodes.  For I = 1 to */
07896 /*              N0, INDX(I) is the index (for X, Y, and Z) of */
07897 /*              the triangulation node with the same (or */
07898 /*              nearly the same) coordinates as input node I. */
07899 
07900 /*       LCNT = Array of int weights (counts) associated */
07901 /*              with the triangulation nodes.  For I = 1 to */
07902 /*              N, LCNT(I) is the number of occurrences of */
07903 /*              node I in the input node set, and thus the */
07904 /*              number of duplicates is LCNT(I)-1. */
07905 
07906 /*       NEAR,NEXT,DIST = Garbage. */
07907 
07908 /*       IER = Error indicator: */
07909 /*             IER =  0 if no errors were encountered. */
07910 /*             IER = -1 if N0 < 3 on input. */
07911 /*             IER = -2 if the first three nodes are */
07912 /*                      collinear. */
07913 /*             IER = -3 if Subroutine ADDNOD returns an error */
07914 /*                      flag.  This should not occur. */
07915 
07916 /* Modules required by TRMSH3:  ADDNOD, BDYADD, COVSPH, */
07917 /*                                INSERT, INTADD, JRAND, */
07918 /*                                LEFT, LSTPTR, STORE, SWAP, */
07919 /*                                SWPTST, TRFIND */
07920 
07921 /* Intrinsic function called by TRMSH3:  ABS */
07922 
07923 /* *********************************************************** */
07924 
07925 
07926 /* Local parameters: */
07927 
07928 /* D =        (Negative cosine of) distance from node KT to */
07929 /*              node I */
07930 /* D1,D2,D3 = Distances from node KU to nodes 1, 2, and 3, */
07931 /*              respectively */
07932 /* I,J =      Nodal indexes */
07933 /* I0 =       Index of the node preceding I in a sequence of */
07934 /*              unprocessed nodes:  I = NEXT(I0) */
07935 /* KT =       Index of a triangulation node */
07936 /* KU =       Index of an unprocessed node and DO-loop index */
07937 /* LP =       LIST index (pointer) of a neighbor of KT */
07938 /* LPL =      Pointer to the last neighbor of KT */
07939 /* NEXTI =    NEXT(I) */
07940 /* NKU =      NEAR(KU) */
07941 
07942     /* Parameter adjustments */
07943     --dist;
07944     --next;
07945     --near__;
07946     --indx;
07947     --lend;
07948     --z__;
07949     --y;
07950     --x;
07951     --list;
07952     --lptr;
07953     --lcnt;
07954 
07955     /* Function Body */
07956     if (*n0 < 3) {
07957         *n = 0;
07958         *ier = -1;
07959         return 0;
07960     }
07961 
07962 /* Store the first triangle in the linked list. */
07963 
07964     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
07965             z__[3])) {
07966 
07967 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
07968 
07969         list[1] = 3;
07970         lptr[1] = 2;
07971         list[2] = -2;
07972         lptr[2] = 1;
07973         lend[1] = 2;
07974 
07975         list[3] = 1;
07976         lptr[3] = 4;
07977         list[4] = -3;
07978         lptr[4] = 3;
07979         lend[2] = 4;
07980 
07981         list[5] = 2;
07982         lptr[5] = 6;
07983         list[6] = -1;
07984         lptr[6] = 5;
07985         lend[3] = 6;
07986 
07987     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
07988             y[3], &z__[3])) {
07989 
07990 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
07991 /*     i.e., node 3 lies in the left hemisphere defined by */
07992 /*     arc 1->2. */
07993 
07994         list[1] = 2;
07995         lptr[1] = 2;
07996         list[2] = -3;
07997         lptr[2] = 1;
07998         lend[1] = 2;
07999 
08000         list[3] = 3;
08001         lptr[3] = 4;
08002         list[4] = -1;
08003         lptr[4] = 3;
08004         lend[2] = 4;
08005 
08006         list[5] = 1;
08007         lptr[5] = 6;
08008         list[6] = -2;
08009         lptr[6] = 5;
08010         lend[3] = 6;
08011 
08012 
08013     } else {
08014 
08015 /*   The first three nodes are collinear. */
08016 
08017         *n = 0;
08018         *ier = -2;
08019         return 0;
08020     }
08021 
08022     //printf("pass check colinear\n");
08023 
08024 /* Initialize LNEW, INDX, and LCNT, and test for N = 3. */
08025 
08026     *lnew = 7;
08027     indx[1] = 1;
08028     indx[2] = 2;
08029     indx[3] = 3;
08030     lcnt[1] = 1;
08031     lcnt[2] = 1;
08032     lcnt[3] = 1;
08033     if (*n0 == 3) {
08034         *n = 3;
08035         *ier = 0;
08036         return 0;
08037     }
08038 
08039 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
08040 /*   used to obtain an expected-time (N*log(N)) incremental */
08041 /*   algorithm by enabling constant search time for locating */
08042 /*   each new node in the triangulation. */
08043 
08044 /* For each unprocessed node KU, NEAR(KU) is the index of the */
08045 /*   triangulation node closest to KU (used as the starting */
08046 /*   point for the search in Subroutine TRFIND) and DIST(KU) */
08047 /*   is an increasing function of the arc length (angular */
08048 /*   distance) between nodes KU and NEAR(KU):  -Cos(a) for */
08049 /*   arc length a. */
08050 
08051 /* Since it is necessary to efficiently find the subset of */
08052 /*   unprocessed nodes associated with each triangulation */
08053 /*   node J (those that have J as their NEAR entries), the */
08054 /*   subsets are stored in NEAR and NEXT as follows:  for */
08055 /*   each node J in the triangulation, I = NEAR(J) is the */
08056 /*   first unprocessed node in J's set (with I = 0 if the */
08057 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
08058 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
08059 /*   set are initially ordered by increasing indexes (which */
08060 /*   maximizes efficiency) but that ordering is not main- */
08061 /*   tained as the data structure is updated. */
08062 
08063 /* Initialize the data structure for the single triangle. */
08064 
08065     near__[1] = 0;
08066     near__[2] = 0;
08067     near__[3] = 0;
08068     for (ku = *n0; ku >= 4; --ku) {
08069         d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08070         d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08071         d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08072         if (d1 <= d2 && d1 <= d3) {
08073             near__[ku] = 1;
08074             dist[ku] = d1;
08075             next[ku] = near__[1];
08076             near__[1] = ku;
08077         } else if (d2 <= d1 && d2 <= d3) {
08078             near__[ku] = 2;
08079             dist[ku] = d2;
08080             next[ku] = near__[2];
08081             near__[2] = ku;
08082         } else {
08083             near__[ku] = 3;
08084             dist[ku] = d3;
08085             next[ku] = near__[3];
08086             near__[3] = ku;
08087         }
08088 /* L1: */
08089     }
08090 
08091 /* Loop on unprocessed nodes KU.  KT is the number of nodes */
08092 /*   in the triangulation, and NKU = NEAR(KU). */
08093 
08094     kt = 3;
08095     i__1 = *n0;
08096     for (ku = 4; ku <= i__1; ++ku) {
08097         nku = near__[ku];
08098 
08099 /* Remove KU from the set of unprocessed nodes associated */
08100 /*   with NEAR(KU). */
08101         i__ = nku;
08102         if (near__[i__] == ku) {
08103             near__[i__] = next[ku];
08104         } else {
08105             i__ = near__[i__];
08106 L2:
08107             i0 = i__;
08108             i__ = next[i0];
08109             if (i__ != ku) {
08110                 goto L2;
08111             }
08112             next[i0] = next[ku];
08113         }
08114         near__[ku] = 0;
08115 
08116 /* Bypass duplicate nodes. */
08117 
08118         if (dist[ku] <= *tol - 1.) {
08119             indx[ku] = -nku;
08120             ++lcnt[nku];
08121             goto L6;
08122         }
08123 
08124 
08125 /* Add a new triangulation node KT with LCNT(KT) = 1. */
08126         ++kt;
08127         x[kt] = x[ku];
08128         y[kt] = y[ku];
08129         z__[kt] = z__[ku];
08130         indx[ku] = kt;
08131         lcnt[kt] = 1;
08132         addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08133                 , lnew, ier);
08134         if (*ier != 0) {
08135             *n = 0;
08136             *ier = -3;
08137             return 0;
08138         }
08139 
08140 /* Loop on neighbors J of node KT. */
08141 
08142         lpl = lend[kt];
08143         lp = lpl;
08144 L3:
08145         lp = lptr[lp];
08146         j = (i__2 = list[lp], abs(i__2));
08147 
08148 /* Loop on elements I in the sequence of unprocessed nodes */
08149 /*   associated with J:  KT is a candidate for replacing J */
08150 /*   as the nearest triangulation node to I.  The next value */
08151 /*   of I in the sequence, NEXT(I), must be saved before I */
08152 /*   is moved because it is altered by adding I to KT's set. */
08153 
08154         i__ = near__[j];
08155 L4:
08156         if (i__ == 0) {
08157             goto L5;
08158         }
08159         nexti = next[i__];
08160 
08161 /* Test for the distance from I to KT less than the distance */
08162 /*   from I to J. */
08163 
08164         d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08165         if (d__ < dist[i__]) {
08166 
08167 /* Replace J by KT as the nearest triangulation node to I: */
08168 /*   update NEAR(I) and DIST(I), and remove I from J's set */
08169 /*   of unprocessed nodes and add it to KT's set. */
08170 
08171             near__[i__] = kt;
08172             dist[i__] = d__;
08173             if (i__ == near__[j]) {
08174                 near__[j] = nexti;
08175             } else {
08176                 next[i0] = nexti;
08177             }
08178             next[i__] = near__[kt];
08179             near__[kt] = i__;
08180         } else {
08181             i0 = i__;
08182         }
08183 
08184 /* Bottom of loop on I. */
08185 
08186         i__ = nexti;
08187         goto L4;
08188 
08189 /* Bottom of loop on neighbors J. */
08190 
08191 L5:
08192         if (lp != lpl) {
08193             goto L3;
08194         }
08195 L6:
08196         ;
08197     }
08198     *n = kt;
08199     *ier = 0;
08200     return 0;
08201 } /* trmsh3_ */
08202 
08203 /* stripack.dbl sent by Robert on 06/03/03 */
08204 /* Subroutine */ int addnod_(int *nst, int *k, double *x,
08205         double *y, double *z__, int *list, int *lptr, int
08206         *lend, int *lnew, int *ier)
08207 {
08208     /* Initialized data */
08209 
08210     static double tol = 0.;
08211 
08212     /* System generated locals */
08213     int i__1;
08214 
08215     /* Local variables */
08216     static int l;
08217     static double p[3], b1, b2, b3;
08218     static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08219     extern /* Subroutine */ int swap_(int *, int *, int *,
08220             int *, int *, int *, int *, int *);
08221     static int lpo1s;
08222     extern /* Subroutine */ int bdyadd_(int *, int *, int *,
08223             int *, int *, int *, int *), intadd_(int *,
08224             int *, int *, int *, int *, int *, int *,
08225             int *), trfind_(int *, double *, int *,
08226             double *, double *, double *, int *, int *,
08227             int *, double *, double *, double *, int *,
08228             int *, int *), covsph_(int *, int *, int *,
08229             int *, int *, int *);
08230     extern int lstptr_(int *, int *, int *, int *);
08231     extern long int swptst_(int *, int *, int *, int *,
08232             double *, double *, double *);
08233 
08234 
08235 /* *********************************************************** */
08236 
08237 /*                                              From STRIPACK */
08238 /*                                            Robert J. Renka */
08239 /*                                  Dept. of Computer Science */
08240 /*                                       Univ. of North Texas */
08241 /*                                           renka@cs.unt.edu */
08242 /*                                                   01/08/03 */
08243 
08244 /*   This subroutine adds node K to a triangulation of the */
08245 /* convex hull of nodes 1,...,K-1, producing a triangulation */
08246 /* of the convex hull of nodes 1,...,K. */
08247 
08248 /*   The algorithm consists of the following steps:  node K */
08249 /* is located relative to the triangulation (TRFIND), its */
08250 /* index is added to the data structure (INTADD or BDYADD), */
08251 /* and a sequence of swaps (SWPTST and SWAP) are applied to */
08252 /* the arcs opposite K so that all arcs incident on node K */
08253 /* and opposite node K are locally optimal (satisfy the cir- */
08254 /* cumcircle test).  Thus, if a Delaunay triangulation is */
08255 /* input, a Delaunay triangulation will result. */
08256 
08257 
08258 /* On input: */
08259 
08260 /*       NST = Index of a node at which TRFIND begins its */
08261 /*             search.  Search time depends on the proximity */
08262 /*             of this node to K.  If NST < 1, the search is */
08263 /*             begun at node K-1. */
08264 
08265 /*       K = Nodal index (index for X, Y, Z, and LEND) of the */
08266 /*           new node to be added.  K .GE. 4. */
08267 
08268 /*       X,Y,Z = Arrays of length .GE. K containing Car- */
08269 /*               tesian coordinates of the nodes. */
08270 /*               (X(I),Y(I),Z(I)) defines node I for */
08271 /*               I = 1,...,K. */
08272 
08273 /* The above parameters are not altered by this routine. */
08274 
08275 /*       LIST,LPTR,LEND,LNEW = Data structure associated with */
08276 /*                             the triangulation of nodes 1 */
08277 /*                             to K-1.  The array lengths are */
08278 /*                             assumed to be large enough to */
08279 /*                             add node K.  Refer to Subrou- */
08280 /*                             tine TRMESH. */
08281 
08282 /* On output: */
08283 
08284 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
08285 /*                             the addition of node K as the */
08286 /*                             last entry unless IER .NE. 0 */
08287 /*                             and IER .NE. -3, in which case */
08288 /*                             the arrays are not altered. */
08289 
08290 /*       IER = Error indicator: */
08291 /*             IER =  0 if no errors were encountered. */
08292 /*             IER = -1 if K is outside its valid range */
08293 /*                      on input. */
08294 /*             IER = -2 if all nodes (including K) are col- */
08295 /*                      linear (lie on a common geodesic). */
08296 /*             IER =  L if nodes L and K coincide for some */
08297 /*                      L < K.  Refer to TOL below. */
08298 
08299 /* Modules required by ADDNOD:  BDYADD, COVSPH, INSERT, */
08300 /*                                INTADD, JRAND, LSTPTR, */
08301 /*                                STORE, SWAP, SWPTST, */
08302 /*                                TRFIND */
08303 
08304 /* Intrinsic function called by ADDNOD:  ABS */
08305 
08306 /* *********************************************************** */
08307 
08308 
08309 /* Local parameters: */
08310 
08311 /* B1,B2,B3 = Unnormalized barycentric coordinates returned */
08312 /*              by TRFIND. */
08313 /* I1,I2,I3 = Vertex indexes of a triangle containing K */
08314 /* IN1 =      Vertex opposite K:  first neighbor of IO2 */
08315 /*              that precedes IO1.  IN1,IO1,IO2 are in */
08316 /*              counterclockwise order. */
08317 /* IO1,IO2 =  Adjacent neighbors of K defining an arc to */
08318 /*              be tested for a swap */
08319 /* IST =      Index of node at which TRFIND begins its search */
08320 /* KK =       Local copy of K */
08321 /* KM1 =      K-1 */
08322 /* L =        Vertex index (I1, I2, or I3) returned in IER */
08323 /*              if node K coincides with a vertex */
08324 /* LP =       LIST pointer */
08325 /* LPF =      LIST pointer to the first neighbor of K */
08326 /* LPO1 =     LIST pointer to IO1 */
08327 /* LPO1S =    Saved value of LPO1 */
08328 /* P =        Cartesian coordinates of node K */
08329 /* TOL =      Tolerance defining coincident nodes:  bound on */
08330 /*              the deviation from 1 of the cosine of the */
08331 /*              angle between the nodes. */
08332 /*              Note that |1-cos(A)| is approximately A*A/2. */
08333 
08334     /* Parameter adjustments */
08335     --lend;
08336     --z__;
08337     --y;
08338     --x;
08339     --list;
08340     --lptr;
08341 
08342     /* Function Body */
08343 
08344     kk = *k;
08345     if (kk < 4) {
08346         goto L3;
08347     }
08348 
08349 /* Initialization: */
08350     km1 = kk - 1;
08351     ist = *nst;
08352     if (ist < 1) {
08353         ist = km1;
08354     }
08355     p[0] = x[kk];
08356     p[1] = y[kk];
08357     p[2] = z__[kk];
08358 
08359 /* Find a triangle (I1,I2,I3) containing K or the rightmost */
08360 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
08361 /*   from node K. */
08362     trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08363             , &b1, &b2, &b3, &i1, &i2, &i3);
08364 
08365 /*   Test for collinear or (nearly) duplicate nodes. */
08366 
08367     if (i1 == 0) {
08368         goto L4;
08369     }
08370     l = i1;
08371     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08372         goto L5;
08373     }
08374     l = i2;
08375     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08376         goto L5;
08377     }
08378     if (i3 != 0) {
08379         l = i3;
08380         if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08381             goto L5;
08382         }
08383         intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08384     } else {
08385         if (i1 != i2) {
08386             bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08387         } else {
08388             covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08389         }
08390     }
08391     *ier = 0;
08392 
08393 /* Initialize variables for optimization of the */
08394 /*   triangulation. */
08395     lp = lend[kk];
08396     lpf = lptr[lp];
08397     io2 = list[lpf];
08398     lpo1 = lptr[lpf];
08399     io1 = (i__1 = list[lpo1], abs(i__1));
08400 
08401 /* Begin loop:  find the node opposite K. */
08402 
08403 L1:
08404     lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08405     if (list[lp] < 0) {
08406         goto L2;
08407     }
08408     lp = lptr[lp];
08409     in1 = (i__1 = list[lp], abs(i__1));
08410 
08411 /* Swap test:  if a swap occurs, two new arcs are */
08412 /*             opposite K and must be tested. */
08413 
08414     lpo1s = lpo1;
08415     if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08416         goto L2;
08417     }
08418     swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08419     if (lpo1 == 0) {
08420 
08421 /*   A swap is not possible because KK and IN1 are already */
08422 /*     adjacent.  This error in SWPTST only occurs in the */
08423 /*     neutral case and when there are nearly duplicate */
08424 /*     nodes. */
08425 
08426         lpo1 = lpo1s;
08427         goto L2;
08428     }
08429     io1 = in1;
08430     goto L1;
08431 
08432 /* No swap occurred.  Test for termination and reset */
08433 /*   IO2 and IO1. */
08434 
08435 L2:
08436     if (lpo1 == lpf || list[lpo1] < 0) {
08437         return 0;
08438     }
08439     io2 = io1;
08440     lpo1 = lptr[lpo1];
08441     io1 = (i__1 = list[lpo1], abs(i__1));
08442     goto L1;
08443 
08444 /* KK < 4. */
08445 
08446 L3:
08447     *ier = -1;
08448     return 0;
08449 
08450 /* All nodes are collinear. */
08451 
08452 L4:
08453     *ier = -2;
08454     return 0;
08455 
08456 /* Nodes L and K coincide. */
08457 
08458 L5:
08459     *ier = l;
08460     return 0;
08461 } /* addnod_ */
08462 
08463 double angle_(double *v1, double *v2, double *v3)
08464 {
08465     /* System generated locals */
08466     double ret_val;
08467 
08468     /* Builtin functions */
08469     //double sqrt(double), acos(double);
08470 
08471     /* Local variables */
08472     static double a;
08473     static int i__;
08474     static double ca, s21, s23, u21[3], u23[3];
08475     extern long int left_(double *, double *, double *, double
08476             *, double *, double *, double *, double *,
08477             double *);
08478 
08479 
08480 /* *********************************************************** */
08481 
08482 /*                                              From STRIPACK */
08483 /*                                            Robert J. Renka */
08484 /*                                  Dept. of Computer Science */
08485 /*                                       Univ. of North Texas */
08486 /*                                           renka@cs.unt.edu */
08487 /*                                                   06/03/03 */
08488 
08489 /*   Given a sequence of three nodes (V1,V2,V3) on the sur- */
08490 /* face of the unit sphere, this function returns the */
08491 /* interior angle at V2 -- the dihedral angle between the */
08492 /* plane defined by V2 and V3 (and the origin) and the plane */
08493 /* defined by V2 and V1 or, equivalently, the angle between */
08494 /* the normals V2 X V3 and V2 X V1.  Note that the angle is */
08495 /* in the range 0 to Pi if V3 Left V1->V2, Pi to 2*Pi other- */
08496 /* wise.  The surface area of a spherical polygon with CCW- */
08497 /* ordered vertices V1, V2, ..., Vm is Asum - (m-2)*Pi, where */
08498 /* Asum is the sum of the m interior angles computed from the */
08499 /* sequences (Vm,V1,V2), (V1,V2,V3), (V2,V3,V4), ..., */
08500 /* (Vm-1,Vm,V1). */
08501 
08502 
08503 /* On input: */
08504 
08505 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08506 /*                  sian coordinates of unit vectors.  These */
08507 /*                  vectors, if nonzero, are implicitly */
08508 /*                  scaled to have length 1. */
08509 
08510 /* Input parameters are not altered by this function. */
08511 
08512 /* On output: */
08513 
08514 /*       ANGLE = Angle defined above, or 0 if V2 X V1 = 0 or */
08515 /*               V2 X V3 = 0. */
08516 
08517 /* Module required by ANGLE:  LEFT */
08518 
08519 /* Intrinsic functions called by ANGLE:  ACOS, SQRT */
08520 
08521 /* *********************************************************** */
08522 
08523 
08524 /* Local parameters: */
08525 
08526 /* A =       Interior angle at V2 */
08527 /* CA =      cos(A) */
08528 /* I =       DO-loop index and index for U21 and U23 */
08529 /* S21,S23 = Sum of squared components of U21 and U23 */
08530 /* U21,U23 = Unit normal vectors to the planes defined by */
08531 /*             pairs of triangle vertices */
08532 
08533 
08534 /* Compute cross products U21 = V2 X V1 and U23 = V2 X V3. */
08535 
08536     /* Parameter adjustments */
08537     --v3;
08538     --v2;
08539     --v1;
08540 
08541     /* Function Body */
08542     u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08543     u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08544     u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08545 
08546     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08547     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08548     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08549 
08550 /* Normalize U21 and U23 to unit vectors. */
08551 
08552     s21 = 0.;
08553     s23 = 0.;
08554     for (i__ = 1; i__ <= 3; ++i__) {
08555         s21 += u21[i__ - 1] * u21[i__ - 1];
08556         s23 += u23[i__ - 1] * u23[i__ - 1];
08557 /* L1: */
08558     }
08559 
08560 /* Test for a degenerate triangle associated with collinear */
08561 /*   vertices. */
08562 
08563     if (s21 == 0. || s23 == 0.) {
08564         ret_val = 0.;
08565         return ret_val;
08566     }
08567     s21 = sqrt(s21);
08568     s23 = sqrt(s23);
08569     for (i__ = 1; i__ <= 3; ++i__) {
08570         u21[i__ - 1] /= s21;
08571         u23[i__ - 1] /= s23;
08572 /* L2: */
08573     }
08574 
08575 /* Compute the angle A between normals: */
08576 
08577 /*   CA = cos(A) = <U21,U23> */
08578 
08579     ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08580     if (ca < -1.) {
08581         ca = -1.;
08582     }
08583     if (ca > 1.) {
08584         ca = 1.;
08585     }
08586     a = acos(ca);
08587 
08588 /* Adjust A to the interior angle:  A > Pi iff */
08589 /*   V3 Right V1->V2. */
08590 
08591     if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08592             , &v3[3])) {
08593         a = acos(-1.) * 2. - a;
08594     }
08595     ret_val = a;
08596     return ret_val;
08597 } /* angle_ */
08598 
08599 double areas_(double *v1, double *v2, double *v3)
08600 {
08601     /* System generated locals */
08602     double ret_val;
08603 
08604     /* Builtin functions */
08605     //double sqrt(double), acos(double);
08606 
08607     /* Local variables */
08608     static int i__;
08609     static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08610             ca2, ca3;
08611 
08612 
08613 /* *********************************************************** */
08614 
08615 /*                                              From STRIPACK */
08616 /*                                            Robert J. Renka */
08617 /*                                  Dept. of Computer Science */
08618 /*                                       Univ. of North Texas */
08619 /*                                           renka@cs.unt.edu */
08620 /*                                                   06/22/98 */
08621 
08622 /*   This function returns the area of a spherical triangle */
08623 /* on the unit sphere. */
08624 
08625 
08626 /* On input: */
08627 
08628 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08629 /*                  sian coordinates of unit vectors (the */
08630 /*                  three triangle vertices in any order). */
08631 /*                  These vectors, if nonzero, are implicitly */
08632 /*                  scaled to have length 1. */
08633 
08634 /* Input parameters are not altered by this function. */
08635 
08636 /* On output: */
08637 
08638 /*       AREAS = Area of the spherical triangle defined by */
08639 /*               V1, V2, and V3 in the range 0 to 2*PI (the */
08640 /*               area of a hemisphere).  AREAS = 0 (or 2*PI) */
08641 /*               if and only if V1, V2, and V3 lie in (or */
08642 /*               close to) a plane containing the origin. */
08643 
08644 /* Modules required by AREAS:  None */
08645 
08646 /* Intrinsic functions called by AREAS:  ACOS, SQRT */
08647 
08648 /* *********************************************************** */
08649 
08650 
08651 /* Local parameters: */
08652 
08653 /* A1,A2,A3 =    Interior angles of the spherical triangle */
08654 /* CA1,CA2,CA3 = cos(A1), cos(A2), and cos(A3), respectively */
08655 /* I =           DO-loop index and index for Uij */
08656 /* S12,S23,S31 = Sum of squared components of U12, U23, U31 */
08657 /* U12,U23,U31 = Unit normal vectors to the planes defined by */
08658 /*                 pairs of triangle vertices */
08659 
08660 
08661 /* Compute cross products Uij = Vi X Vj. */
08662 
08663     /* Parameter adjustments */
08664     --v3;
08665     --v2;
08666     --v1;
08667 
08668     /* Function Body */
08669     u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08670     u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08671     u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08672 
08673     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08674     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08675     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08676 
08677     u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08678     u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08679     u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08680 
08681 /* Normalize Uij to unit vectors. */
08682 
08683     s12 = 0.;
08684     s23 = 0.;
08685     s31 = 0.;
08686     for (i__ = 1; i__ <= 3; ++i__) {
08687         s12 += u12[i__ - 1] * u12[i__ - 1];
08688         s23 += u23[i__ - 1] * u23[i__ - 1];
08689         s31 += u31[i__ - 1] * u31[i__ - 1];
08690 /* L2: */
08691     }
08692 
08693 /* Test for a degenerate triangle associated with collinear */
08694 /*   vertices. */
08695 
08696     if (s12 == 0. || s23 == 0. || s31 == 0.) {
08697         ret_val = 0.;
08698         return ret_val;
08699     }
08700     s12 = sqrt(s12);
08701     s23 = sqrt(s23);
08702     s31 = sqrt(s31);
08703     for (i__ = 1; i__ <= 3; ++i__) {
08704         u12[i__ - 1] /= s12;
08705         u23[i__ - 1] /= s23;
08706         u31[i__ - 1] /= s31;
08707 /* L3: */
08708     }
08709 
08710 /* Compute interior angles Ai as the dihedral angles between */
08711 /*   planes: */
08712 /*           CA1 = cos(A1) = -<U12,U31> */
08713 /*           CA2 = cos(A2) = -<U23,U12> */
08714 /*           CA3 = cos(A3) = -<U31,U23> */
08715 
08716     ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08717     ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08718     ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08719     if (ca1 < -1.) {
08720         ca1 = -1.;
08721     }
08722     if (ca1 > 1.) {
08723         ca1 = 1.;
08724     }
08725     if (ca2 < -1.) {
08726         ca2 = -1.;
08727     }
08728     if (ca2 > 1.) {
08729         ca2 = 1.;
08730     }
08731     if (ca3 < -1.) {
08732         ca3 = -1.;
08733     }
08734     if (ca3 > 1.) {
08735         ca3 = 1.;
08736     }
08737     a1 = acos(ca1);
08738     a2 = acos(ca2);
08739     a3 = acos(ca3);
08740 
08741 /* Compute AREAS = A1 + A2 + A3 - PI. */
08742 
08743     ret_val = a1 + a2 + a3 - acos(-1.);
08744     if (ret_val < 0.) {
08745         ret_val = 0.;
08746     }
08747     return ret_val;
08748 } /* areas_ */
08749 
08750 double Util::areav_(int *k, int *n, double *x, double *y,
08751         double *z__, int *list, int *lptr, int *lend, int
08752         *ier)
08753 {
08754     /* Initialized data */
08755 
08756     static double amax = 6.28;
08757 
08758     /* System generated locals */
08759     double ret_val;
08760 
08761     /* Local variables */
08762     static double a, c0[3], c2[3], c3[3];
08763     static int n1, n2, n3;
08764     static double v1[3], v2[3], v3[3];
08765     static int lp, lpl, ierr;
08766     static double asum;
08767     extern double areas_(double *, double *, double *);
08768     static long int first;
08769     extern /* Subroutine */ int circum_(double *, double *,
08770             double *, double *, int *);
08771 
08772 
08773 /* *********************************************************** */
08774 
08775 /*                                            Robert J. Renka */
08776 /*                                  Dept. of Computer Science */
08777 /*                                       Univ. of North Texas */
08778 /*                                           renka@cs.unt.edu */
08779 /*                                                   10/25/02 */
08780 
08781 /*   Given a Delaunay triangulation and the index K of an */
08782 /* interior node, this subroutine returns the (surface) area */
08783 /* of the Voronoi region associated with node K.  The Voronoi */
08784 /* region is the polygon whose vertices are the circumcenters */
08785 /* of the triangles that contain node K, where a triangle */
08786 /* circumcenter is the point (unit vector) lying at the same */
08787 /* angular distance from the three vertices and contained in */
08788 /* the same hemisphere as the vertices. */
08789 
08790 
08791 /* On input: */
08792 
08793 /*       K = Nodal index in the range 1 to N. */
08794 
08795 /*       N = Number of nodes in the triangulation.  N > 3. */
08796 
08797 /*       X,Y,Z = Arrays of length N containing the Cartesian */
08798 /*               coordinates of the nodes (unit vectors). */
08799 
08800 /*       LIST,LPTR,LEND = Data structure defining the trian- */
08801 /*                        gulation.  Refer to Subroutine */
08802 /*                        TRMESH. */
08803 
08804 /* Input parameters are not altered by this function. */
08805 
08806 /* On output: */
08807 
08808 /*       AREAV = Area of Voronoi region K unless IER > 0, */
08809 /*               in which case AREAV = 0. */
08810 
08811 /*       IER = Error indicator: */
08812 /*             IER = 0 if no errors were encountered. */
08813 /*             IER = 1 if K or N is outside its valid range */
08814 /*                     on input. */
08815 /*             IER = 2 if K indexes a boundary node. */
08816 /*             IER = 3 if an error flag is returned by CIRCUM */
08817 /*                     (null triangle). */
08818 /*             IER = 4 if AREAS returns a value greater than */
08819 /*                     AMAX (defined below). */
08820 
08821 /* Modules required by AREAV:  AREAS, CIRCUM */
08822 
08823 /* *********************************************************** */
08824 
08825 
08826 /* Maximum valid triangle area is less than 2*Pi: */
08827 
08828     /* Parameter adjustments */
08829     --lend;
08830     --z__;
08831     --y;
08832     --x;
08833     --list;
08834     --lptr;
08835 
08836     /* Function Body */
08837 
08838 /* Test for invalid input. */
08839 
08840     if (*k < 1 || *k > *n || *n <= 3) {
08841         goto L11;
08842     }
08843 
08844 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
08845 /*   FIRST = TRUE only for the first triangle. */
08846 /*   The Voronoi region area is accumulated in ASUM. */
08847 
08848     n1 = *k;
08849     v1[0] = x[n1];
08850     v1[1] = y[n1];
08851     v1[2] = z__[n1];
08852     lpl = lend[n1];
08853     n3 = list[lpl];
08854     if (n3 < 0) {
08855         goto L12;
08856     }
08857     lp = lpl;
08858     first = TRUE_;
08859     asum = 0.;
08860 
08861 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
08862 
08863 L1:
08864     n2 = n3;
08865     lp = lptr[lp];
08866     n3 = list[lp];
08867     v2[0] = x[n2];
08868     v2[1] = y[n2];
08869     v2[2] = z__[n2];
08870     v3[0] = x[n3];
08871     v3[1] = y[n3];
08872     v3[2] = z__[n3];
08873     if (first) {
08874 
08875 /* First triangle:  compute the circumcenter C3 and save a */
08876 /*   copy in C0. */
08877 
08878         circum_(v1, v2, v3, c3, &ierr);
08879         if (ierr != 0) {
08880             goto L13;
08881         }
08882         c0[0] = c3[0];
08883         c0[1] = c3[1];
08884         c0[2] = c3[2];
08885         first = FALSE_;
08886     } else {
08887 
08888 /* Set C2 to C3, compute the new circumcenter C3, and compute */
08889 /*   the area A of triangle (V1,C2,C3). */
08890 
08891         c2[0] = c3[0];
08892         c2[1] = c3[1];
08893         c2[2] = c3[2];
08894         circum_(v1, v2, v3, c3, &ierr);
08895         if (ierr != 0) {
08896             goto L13;
08897         }
08898         a = areas_(v1, c2, c3);
08899         if (a > amax) {
08900             goto L14;
08901         }
08902         asum += a;
08903     }
08904 
08905 /* Bottom on loop on neighbors of K. */
08906 
08907     if (lp != lpl) {
08908         goto L1;
08909     }
08910 
08911 /* Compute the area of triangle (V1,C3,C0). */
08912 
08913     a = areas_(v1, c3, c0);
08914     if (a > amax) {
08915         goto L14;
08916     }
08917     asum += a;
08918 
08919 /* No error encountered. */
08920 
08921     *ier = 0;
08922     ret_val = asum;
08923     return ret_val;
08924 
08925 /* Invalid input. */
08926 
08927 L11:
08928     *ier = 1;
08929     ret_val = 0.;
08930     return ret_val;
08931 
08932 /* K indexes a boundary node. */
08933 
08934 L12:
08935     *ier = 2;
08936     ret_val = 0.;
08937     return ret_val;
08938 
08939 /* Error in CIRCUM. */
08940 
08941 L13:
08942     *ier = 3;
08943     ret_val = 0.;
08944     return ret_val;
08945 
08946 /* AREAS value larger than AMAX. */
08947 
08948 L14:
08949     *ier = 4;
08950     ret_val = 0.;
08951     return ret_val;
08952 } /* areav_ */
08953 
08954 double areav_new__(int *k, int *n, double *x, double *y,
08955         double *z__, int *list, int *lptr, int *lend, int
08956         *ier)
08957 {
08958     /* System generated locals */
08959     double ret_val = 0;
08960 
08961     /* Builtin functions */
08962     //double acos(double);
08963 
08964     /* Local variables */
08965     static int m;
08966     static double c1[3], c2[3], c3[3];
08967     static int n1, n2, n3;
08968     static double v1[3], v2[3], v3[3];
08969     static int lp;
08970     static double c1s[3], c2s[3];
08971     static int lpl, ierr;
08972     static double asum;
08973     extern double angle_(double *, double *, double *);
08974     static float areav;
08975     extern /* Subroutine */ int circum_(double *, double *,
08976             double *, double *, int *);
08977 
08978 
08979 /* *********************************************************** */
08980 
08981 /*                                            Robert J. Renka */
08982 /*                                  Dept. of Computer Science */
08983 /*                                       Univ. of North Texas */
08984 /*                                           renka@cs.unt.edu */
08985 /*                                                   06/03/03 */
08986 
08987 /*   Given a Delaunay triangulation and the index K of an */
08988 /* interior node, this subroutine returns the (surface) area */
08989 /* of the Voronoi region associated with node K.  The Voronoi */
08990 /* region is the polygon whose vertices are the circumcenters */
08991 /* of the triangles that contain node K, where a triangle */
08992 /* circumcenter is the point (unit vector) lying at the same */
08993 /* angular distance from the three vertices and contained in */
08994 /* the same hemisphere as the vertices.  The Voronoi region */
08995 /* area is computed as Asum-(m-2)*Pi, where m is the number */
08996 /* of Voronoi vertices (neighbors of K) and Asum is the sum */
08997 /* of interior angles at the vertices. */
08998 
08999 
09000 /* On input: */
09001 
09002 /*       K = Nodal index in the range 1 to N. */
09003 
09004 /*       N = Number of nodes in the triangulation.  N > 3. */
09005 
09006 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09007 /*               coordinates of the nodes (unit vectors). */
09008 
09009 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09010 /*                        gulation.  Refer to Subroutine */
09011 /*                        TRMESH. */
09012 
09013 /* Input parameters are not altered by this function. */
09014 
09015 /* On output: */
09016 
09017 /*       AREAV = Area of Voronoi region K unless IER > 0, */
09018 /*               in which case AREAV = 0. */
09019 
09020 /*       IER = Error indicator: */
09021 /*             IER = 0 if no errors were encountered. */
09022 /*             IER = 1 if K or N is outside its valid range */
09023 /*                     on input. */
09024 /*             IER = 2 if K indexes a boundary node. */
09025 /*             IER = 3 if an error flag is returned by CIRCUM */
09026 /*                     (null triangle). */
09027 
09028 /* Modules required by AREAV:  ANGLE, CIRCUM */
09029 
09030 /* Intrinsic functions called by AREAV:  ACOS, DBLE */
09031 
09032 /* *********************************************************** */
09033 
09034 
09035 /* Test for invalid input. */
09036 
09037     /* Parameter adjustments */
09038     --lend;
09039     --z__;
09040     --y;
09041     --x;
09042     --list;
09043     --lptr;
09044 
09045     /* Function Body */
09046     if (*k < 1 || *k > *n || *n <= 3) {
09047         goto L11;
09048     }
09049 
09050 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
09051 /*   The number of neighbors and the sum of interior angles */
09052 /*   are accumulated in M and ASUM, respectively. */
09053 
09054     n1 = *k;
09055     v1[0] = x[n1];
09056     v1[1] = y[n1];
09057     v1[2] = z__[n1];
09058     lpl = lend[n1];
09059     n3 = list[lpl];
09060     if (n3 < 0) {
09061         goto L12;
09062     }
09063     lp = lpl;
09064     m = 0;
09065     asum = 0.;
09066 
09067 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
09068 
09069 L1:
09070     ++m;
09071     n2 = n3;
09072     lp = lptr[lp];
09073     n3 = list[lp];
09074     v2[0] = x[n2];
09075     v2[1] = y[n2];
09076     v2[2] = z__[n2];
09077     v3[0] = x[n3];
09078     v3[1] = y[n3];
09079     v3[2] = z__[n3];
09080     if (m == 1) {
09081 
09082 /* First triangle:  compute the circumcenter C2 and save a */
09083 /*   copy in C1S. */
09084 
09085         circum_(v1, v2, v3, c2, &ierr);
09086         if (ierr != 0) {
09087             goto L13;
09088         }
09089         c1s[0] = c2[0];
09090         c1s[1] = c2[1];
09091         c1s[2] = c2[2];
09092     } else if (m == 2) {
09093 
09094 /* Second triangle:  compute the circumcenter C3 and save a */
09095 /*   copy in C2S. */
09096 
09097         circum_(v1, v2, v3, c3, &ierr);
09098         if (ierr != 0) {
09099             goto L13;
09100         }
09101         c2s[0] = c3[0];
09102         c2s[1] = c3[1];
09103         c2s[2] = c3[2];
09104     } else {
09105 
09106 /* Set C1 to C2, set C2 to C3, compute the new circumcenter */
09107 /*   C3, and compute the interior angle at C2 from the */
09108 /*   sequence of vertices (C1,C2,C3). */
09109 
09110         c1[0] = c2[0];
09111         c1[1] = c2[1];
09112         c1[2] = c2[2];
09113         c2[0] = c3[0];
09114         c2[1] = c3[1];
09115         c2[2] = c3[2];
09116         circum_(v1, v2, v3, c3, &ierr);
09117         if (ierr != 0) {
09118             goto L13;
09119         }
09120         asum += angle_(c1, c2, c3);
09121     }
09122 
09123 /* Bottom on loop on neighbors of K. */
09124 
09125     if (lp != lpl) {
09126         goto L1;
09127     }
09128 
09129 /* C3 is the last vertex.  Compute its interior angle from */
09130 /*   the sequence (C2,C3,C1S). */
09131 
09132     asum += angle_(c2, c3, c1s);
09133 
09134 /* Compute the interior angle at C1S from */
09135 /*   the sequence (C3,C1S,C2S). */
09136 
09137     asum += angle_(c3, c1s, c2s);
09138 
09139 /* No error encountered. */
09140 
09141     *ier = 0;
09142     ret_val = asum - (double) (m - 2) * acos(-1.);
09143     return ret_val;
09144 
09145 /* Invalid input. */
09146 
09147 L11:
09148     *ier = 1;
09149     areav = 0.f;
09150     return ret_val;
09151 
09152 /* K indexes a boundary node. */
09153 
09154 L12:
09155     *ier = 2;
09156     areav = 0.f;
09157     return ret_val;
09158 
09159 /* Error in CIRCUM. */
09160 
09161 L13:
09162     *ier = 3;
09163     areav = 0.f;
09164     return ret_val;
09165 } /* areav_new__ */
09166 
09167 /* Subroutine */ int bdyadd_(int *kk, int *i1, int *i2, int *
09168         list, int *lptr, int *lend, int *lnew)
09169 {
09170     static int k, n1, n2, lp, lsav, nsav, next;
09171     extern /* Subroutine */ int insert_(int *, int *, int *,
09172             int *, int *);
09173 
09174 
09175 /* *********************************************************** */
09176 
09177 /*                                              From STRIPACK */
09178 /*                                            Robert J. Renka */
09179 /*                                  Dept. of Computer Science */
09180 /*                                       Univ. of North Texas */
09181 /*                                           renka@cs.unt.edu */
09182 /*                                                   07/11/96 */
09183 
09184 /*   This subroutine adds a boundary node to a triangulation */
09185 /* of a set of KK-1 points on the unit sphere.  The data */
09186 /* structure is updated with the insertion of node KK, but no */
09187 /* optimization is performed. */
09188 
09189 /*   This routine is identical to the similarly named routine */
09190 /* in TRIPACK. */
09191 
09192 
09193 /* On input: */
09194 
09195 /*       KK = Index of a node to be connected to the sequence */
09196 /*            of all visible boundary nodes.  KK .GE. 1 and */
09197 /*            KK must not be equal to I1 or I2. */
09198 
09199 /*       I1 = First (rightmost as viewed from KK) boundary */
09200 /*            node in the triangulation that is visible from */
09201 /*            node KK (the line segment KK-I1 intersects no */
09202 /*            arcs. */
09203 
09204 /*       I2 = Last (leftmost) boundary node that is visible */
09205 /*            from node KK.  I1 and I2 may be determined by */
09206 /*            Subroutine TRFIND. */
09207 
09208 /* The above parameters are not altered by this routine. */
09209 
09210 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09211 /*                             created by Subroutine TRMESH. */
09212 /*                             Nodes I1 and I2 must be in- */
09213 /*                             cluded in the triangulation. */
09214 
09215 /* On output: */
09216 
09217 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09218 /*                             the addition of node KK.  Node */
09219 /*                             KK is connected to I1, I2, and */
09220 /*                             all boundary nodes in between. */
09221 
09222 /* Module required by BDYADD:  INSERT */
09223 
09224 /* *********************************************************** */
09225 
09226 
09227 /* Local parameters: */
09228 
09229 /* K =     Local copy of KK */
09230 /* LP =    LIST pointer */
09231 /* LSAV =  LIST pointer */
09232 /* N1,N2 = Local copies of I1 and I2, respectively */
09233 /* NEXT =  Boundary node visible from K */
09234 /* NSAV =  Boundary node visible from K */
09235 
09236     /* Parameter adjustments */
09237     --lend;
09238     --lptr;
09239     --list;
09240 
09241     /* Function Body */
09242     k = *kk;
09243     n1 = *i1;
09244     n2 = *i2;
09245 
09246 /* Add K as the last neighbor of N1. */
09247 
09248     lp = lend[n1];
09249     lsav = lptr[lp];
09250     lptr[lp] = *lnew;
09251     list[*lnew] = -k;
09252     lptr[*lnew] = lsav;
09253     lend[n1] = *lnew;
09254     ++(*lnew);
09255     next = -list[lp];
09256     list[lp] = next;
09257     nsav = next;
09258 
09259 /* Loop on the remaining boundary nodes between N1 and N2, */
09260 /*   adding K as the first neighbor. */
09261 
09262 L1:
09263     lp = lend[next];
09264     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09265     if (next == n2) {
09266         goto L2;
09267     }
09268     next = -list[lp];
09269     list[lp] = next;
09270     goto L1;
09271 
09272 /* Add the boundary nodes between N1 and N2 as neighbors */
09273 /*   of node K. */
09274 
09275 L2:
09276     lsav = *lnew;
09277     list[*lnew] = n1;
09278     lptr[*lnew] = *lnew + 1;
09279     ++(*lnew);
09280     next = nsav;
09281 
09282 L3:
09283     if (next == n2) {
09284         goto L4;
09285     }
09286     list[*lnew] = next;
09287     lptr[*lnew] = *lnew + 1;
09288     ++(*lnew);
09289     lp = lend[next];
09290     next = list[lp];
09291     goto L3;
09292 
09293 L4:
09294     list[*lnew] = -n2;
09295     lptr[*lnew] = lsav;
09296     lend[k] = *lnew;
09297     ++(*lnew);
09298     return 0;
09299 } /* bdyadd_ */
09300 
09301 /* Subroutine */ int bnodes_(int *n, int *list, int *lptr,
09302         int *lend, int *nodes, int *nb, int *na, int *nt)
09303 {
09304     /* System generated locals */
09305     int i__1;
09306 
09307     /* Local variables */
09308     static int k, n0, lp, nn, nst;
09309 
09310 
09311 /* *********************************************************** */
09312 
09313 /*                                              From STRIPACK */
09314 /*                                            Robert J. Renka */
09315 /*                                  Dept. of Computer Science */
09316 /*                                       Univ. of North Texas */
09317 /*                                           renka@cs.unt.edu */
09318 /*                                                   06/26/96 */
09319 
09320 /*   Given a triangulation of N nodes on the unit sphere */
09321 /* created by Subroutine TRMESH, this subroutine returns an */
09322 /* array containing the indexes (if any) of the counterclock- */
09323 /* wise-ordered sequence of boundary nodes -- the nodes on */
09324 /* the boundary of the convex hull of the set of nodes.  (The */
09325 /* boundary is empty if the nodes do not lie in a single */
09326 /* hemisphere.)  The numbers of boundary nodes, arcs, and */
09327 /* triangles are also returned. */
09328 
09329 
09330 /* On input: */
09331 
09332 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09333 
09334 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09335 /*                        gulation.  Refer to Subroutine */
09336 /*                        TRMESH. */
09337 
09338 /* The above parameters are not altered by this routine. */
09339 
09340 /*       NODES = int array of length at least NB */
09341 /*               (NB .LE. N). */
09342 
09343 /* On output: */
09344 
09345 /*       NODES = Ordered sequence of boundary node indexes */
09346 /*               in the range 1 to N (in the first NB loca- */
09347 /*               tions). */
09348 
09349 /*       NB = Number of boundary nodes. */
09350 
09351 /*       NA,NT = Number of arcs and triangles, respectively, */
09352 /*               in the triangulation. */
09353 
09354 /* Modules required by BNODES:  None */
09355 
09356 /* *********************************************************** */
09357 
09358 
09359 /* Local parameters: */
09360 
09361 /* K =   NODES index */
09362 /* LP =  LIST pointer */
09363 /* N0 =  Boundary node to be added to NODES */
09364 /* NN =  Local copy of N */
09365 /* NST = First element of nodes (arbitrarily chosen to be */
09366 /*         the one with smallest index) */
09367 
09368     /* Parameter adjustments */
09369     --lend;
09370     --list;
09371     --lptr;
09372     --nodes;
09373 
09374     /* Function Body */
09375     nn = *n;
09376 
09377 /* Search for a boundary node. */
09378 
09379     i__1 = nn;
09380     for (nst = 1; nst <= i__1; ++nst) {
09381         lp = lend[nst];
09382         if (list[lp] < 0) {
09383             goto L2;
09384         }
09385 /* L1: */
09386     }
09387 
09388 /* The triangulation contains no boundary nodes. */
09389 
09390     *nb = 0;
09391     *na = (nn - 2) * 3;
09392     *nt = nn - (2<<1);
09393     return 0;
09394 
09395 /* NST is the first boundary node encountered.  Initialize */
09396 /*   for traversal of the boundary. */
09397 
09398 L2:
09399     nodes[1] = nst;
09400     k = 1;
09401     n0 = nst;
09402 
09403 /* Traverse the boundary in counterclockwise order. */
09404 
09405 L3:
09406     lp = lend[n0];
09407     lp = lptr[lp];
09408     n0 = list[lp];
09409     if (n0 == nst) {
09410         goto L4;
09411     }
09412     ++k;
09413     nodes[k] = n0;
09414     goto L3;
09415 
09416 /* Store the counts. */
09417 
09418 L4:
09419     *nb = k;
09420     *nt = (*n << 1) - *nb - 2;
09421     *na = *nt + *n - 1;
09422     return 0;
09423 } /* bnodes_ */
09424 
09425 /* Subroutine */ int circle_(int *k, double *xc, double *yc,
09426         int *ier)
09427 {
09428     /* System generated locals */
09429     int i__1;
09430 
09431     /* Builtin functions */
09432     //double atan(double), cos(double), sin(double);
09433 
09434     /* Local variables */
09435     static double a, c__;
09436     static int i__;
09437     static double s;
09438     static int k2, k3;
09439     static double x0, y0;
09440     static int kk, np1;
09441 
09442 
09443 /* *********************************************************** */
09444 
09445 /*                                              From STRIPACK */
09446 /*                                            Robert J. Renka */
09447 /*                                  Dept. of Computer Science */
09448 /*                                       Univ. of North Texas */
09449 /*                                           renka@cs.unt.edu */
09450 /*                                                   04/06/90 */
09451 
09452 /*   This subroutine computes the coordinates of a sequence */
09453 /* of N equally spaced points on the unit circle centered at */
09454 /* (0,0).  An N-sided polygonal approximation to the circle */
09455 /* may be plotted by connecting (XC(I),YC(I)) to (XC(I+1), */
09456 /* YC(I+1)) for I = 1,...,N, where XC(N+1) = XC(1) and */
09457 /* YC(N+1) = YC(1).  A reasonable value for N in this case */
09458 /* is 2*PI*R, where R is the radius of the circle in device */
09459 /* coordinates. */
09460 
09461 
09462 /* On input: */
09463 
09464 /*       K = Number of points in each quadrant, defining N as */
09465 /*           4K.  K .GE. 1. */
09466 
09467 /*       XC,YC = Arrays of length at least N+1 = 4K+1. */
09468 
09469 /* K is not altered by this routine. */
09470 
09471 /* On output: */
09472 
09473 /*       XC,YC = Cartesian coordinates of the points on the */
09474 /*               unit circle in the first N+1 locations. */
09475 /*               XC(I) = cos(A*(I-1)), YC(I) = sin(A*(I-1)), */
09476 /*               where A = 2*PI/N.  Note that XC(N+1) = XC(1) */
09477 /*               and YC(N+1) = YC(1). */
09478 
09479 /*       IER = Error indicator: */
09480 /*             IER = 0 if no errors were encountered. */
09481 /*             IER = 1 if K < 1 on input. */
09482 
09483 /* Modules required by CIRCLE:  None */
09484 
09485 /* Intrinsic functions called by CIRCLE:  ATAN, COS, DBLE, */
09486 /*                                          SIN */
09487 
09488 /* *********************************************************** */
09489 
09490 
09491 /* Local parameters: */
09492 
09493 /* I =     DO-loop index and index for XC and YC */
09494 /* KK =    Local copy of K */
09495 /* K2 =    K*2 */
09496 /* K3 =    K*3 */
09497 /* NP1 =   N+1 = 4*K + 1 */
09498 /* A =     Angular separation between adjacent points */
09499 /* C,S =   Cos(A) and sin(A), respectively, defining a */
09500 /*           rotation through angle A */
09501 /* X0,Y0 = Cartesian coordinates of a point on the unit */
09502 /*           circle in the first quadrant */
09503 
09504     /* Parameter adjustments */
09505     --yc;
09506     --xc;
09507 
09508     /* Function Body */
09509     kk = *k;
09510     k2 = kk << 1;
09511     k3 = kk * 3;
09512     np1 = (kk << 2) + 1;
09513 
09514 /* Test for invalid input, compute A, C, and S, and */
09515 /*   initialize (X0,Y0) to (1,0). */
09516 
09517     if (kk < 1) {
09518         goto L2;
09519     }
09520     a = atan(1.) * 2. / (double) kk;
09521     c__ = cos(a);
09522     s = sin(a);
09523     x0 = 1.;
09524     y0 = 0.;
09525 
09526 /* Loop on points (X0,Y0) in the first quadrant, storing */
09527 /*   the point and its reflections about the x axis, the */
09528 /*   y axis, and the line y = -x. */
09529 
09530     i__1 = kk;
09531     for (i__ = 1; i__ <= i__1; ++i__) {
09532         xc[i__] = x0;
09533         yc[i__] = y0;
09534         xc[i__ + kk] = -y0;
09535         yc[i__ + kk] = x0;
09536         xc[i__ + k2] = -x0;
09537         yc[i__ + k2] = -y0;
09538         xc[i__ + k3] = y0;
09539         yc[i__ + k3] = -x0;
09540 
09541 /*   Rotate (X0,Y0) counterclockwise through angle A. */
09542 
09543         x0 = c__ * x0 - s * y0;
09544         y0 = s * x0 + c__ * y0;
09545 /* L1: */
09546     }
09547 
09548 /* Store the coordinates of the first point as the last */
09549 /*   point. */
09550 
09551     xc[np1] = xc[1];
09552     yc[np1] = yc[1];
09553     *ier = 0;
09554     return 0;
09555 
09556 /* K < 1. */
09557 
09558 L2:
09559     *ier = 1;
09560     return 0;
09561 } /* circle_ */
09562 
09563 /* Subroutine */ int circum_(double *v1, double *v2, double *v3,
09564         double *c__, int *ier)
09565 {
09566     /* Builtin functions */
09567     //double sqrt(double);
09568 
09569     /* Local variables */
09570     static int i__;
09571     static double e1[3], e2[3], cu[3], cnorm;
09572 
09573 
09574 /* *********************************************************** */
09575 
09576 /*                                              From STRIPACK */
09577 /*                                            Robert J. Renka */
09578 /*                                  Dept. of Computer Science */
09579 /*                                       Univ. of North Texas */
09580 /*                                           renka@cs.unt.edu */
09581 /*                                                   10/27/02 */
09582 
09583 /*   This subroutine returns the circumcenter of a spherical */
09584 /* triangle on the unit sphere:  the point on the sphere sur- */
09585 /* face that is equally distant from the three triangle */
09586 /* vertices and lies in the same hemisphere, where distance */
09587 /* is taken to be arc-length on the sphere surface. */
09588 
09589 
09590 /* On input: */
09591 
09592 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
09593 /*                  sian coordinates of the three triangle */
09594 /*                  vertices (unit vectors) in CCW order. */
09595 
09596 /* The above parameters are not altered by this routine. */
09597 
09598 /*       C = Array of length 3. */
09599 
09600 /* On output: */
09601 
09602 /*       C = Cartesian coordinates of the circumcenter unless */
09603 /*           IER > 0, in which case C is not defined.  C = */
09604 /*           (V2-V1) X (V3-V1) normalized to a unit vector. */
09605 
09606 /*       IER = Error indicator: */
09607 /*             IER = 0 if no errors were encountered. */
09608 /*             IER = 1 if V1, V2, and V3 lie on a common */
09609 /*                     line:  (V2-V1) X (V3-V1) = 0. */
09610 /*             (The vertices are not tested for validity.) */
09611 
09612 /* Modules required by CIRCUM:  None */
09613 
09614 /* Intrinsic function called by CIRCUM:  SQRT */
09615 
09616 /* *********************************************************** */
09617 
09618 
09619 /* Local parameters: */
09620 
09621 /* CNORM = Norm of CU:  used to compute C */
09622 /* CU =    Scalar multiple of C:  E1 X E2 */
09623 /* E1,E2 = Edges of the underlying planar triangle: */
09624 /*           V2-V1 and V3-V1, respectively */
09625 /* I =     DO-loop index */
09626 
09627     /* Parameter adjustments */
09628     --c__;
09629     --v3;
09630     --v2;
09631     --v1;
09632 
09633     /* Function Body */
09634     for (i__ = 1; i__ <= 3; ++i__) {
09635         e1[i__ - 1] = v2[i__] - v1[i__];
09636         e2[i__ - 1] = v3[i__] - v1[i__];
09637 /* L1: */
09638     }
09639 
09640 /* Compute CU = E1 X E2 and CNORM**2. */
09641 
09642     cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09643     cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09644     cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09645     cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09646 
09647 /* The vertices lie on a common line if and only if CU is */
09648 /*   the zero vector. */
09649 
09650     if (cnorm != 0.) {
09651 
09652 /*   No error:  compute C. */
09653 
09654         cnorm = sqrt(cnorm);
09655         for (i__ = 1; i__ <= 3; ++i__) {
09656             c__[i__] = cu[i__ - 1] / cnorm;
09657 /* L2: */
09658         }
09659 
09660 /* If the vertices are nearly identical, the problem is */
09661 /*   ill-conditioned and it is possible for the computed */
09662 /*   value of C to be 180 degrees off:  <C,V1> near -1 */
09663 /*   when it should be positive. */
09664 
09665         if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09666             c__[1] = -c__[1];
09667             c__[2] = -c__[2];
09668             c__[3] = -c__[3];
09669         }
09670         *ier = 0;
09671     } else {
09672 
09673 /*   CU = 0. */
09674 
09675         *ier = 1;
09676     }
09677     return 0;
09678 } /* circum_ */
09679 
09680 /* Subroutine */ int covsph_(int *kk, int *n0, int *list, int
09681         *lptr, int *lend, int *lnew)
09682 {
09683     static int k, lp, nst, lsav, next;
09684     extern /* Subroutine */ int insert_(int *, int *, int *,
09685             int *, int *);
09686 
09687 
09688 /* *********************************************************** */
09689 
09690 /*                                              From STRIPACK */
09691 /*                                            Robert J. Renka */
09692 /*                                  Dept. of Computer Science */
09693 /*                                       Univ. of North Texas */
09694 /*                                           renka@cs.unt.edu */
09695 /*                                                   07/17/96 */
09696 
09697 /*   This subroutine connects an exterior node KK to all */
09698 /* boundary nodes of a triangulation of KK-1 points on the */
09699 /* unit sphere, producing a triangulation that covers the */
09700 /* sphere.  The data structure is updated with the addition */
09701 /* of node KK, but no optimization is performed.  All boun- */
09702 /* dary nodes must be visible from node KK. */
09703 
09704 
09705 /* On input: */
09706 
09707 /*       KK = Index of the node to be connected to the set of */
09708 /*            all boundary nodes.  KK .GE. 4. */
09709 
09710 /*       N0 = Index of a boundary node (in the range 1 to */
09711 /*            KK-1).  N0 may be determined by Subroutine */
09712 /*            TRFIND. */
09713 
09714 /* The above parameters are not altered by this routine. */
09715 
09716 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09717 /*                             created by Subroutine TRMESH. */
09718 /*                             Node N0 must be included in */
09719 /*                             the triangulation. */
09720 
09721 /* On output: */
09722 
09723 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09724 /*                             the addition of node KK as the */
09725 /*                             last entry.  The updated */
09726 /*                             triangulation contains no */
09727 /*                             boundary nodes. */
09728 
09729 /* Module required by COVSPH:  INSERT */
09730 
09731 /* *********************************************************** */
09732 
09733 
09734 /* Local parameters: */
09735 
09736 /* K =     Local copy of KK */
09737 /* LP =    LIST pointer */
09738 /* LSAV =  LIST pointer */
09739 /* NEXT =  Boundary node visible from K */
09740 /* NST =   Local copy of N0 */
09741 
09742     /* Parameter adjustments */
09743     --lend;
09744     --lptr;
09745     --list;
09746 
09747     /* Function Body */
09748     k = *kk;
09749     nst = *n0;
09750 
09751 /* Traverse the boundary in clockwise order, inserting K as */
09752 /*   the first neighbor of each boundary node, and converting */
09753 /*   the boundary node to an interior node. */
09754 
09755     next = nst;
09756 L1:
09757     lp = lend[next];
09758     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09759     next = -list[lp];
09760     list[lp] = next;
09761     if (next != nst) {
09762         goto L1;
09763     }
09764 
09765 /* Traverse the boundary again, adding each node to K's */
09766 /*   adjacency list. */
09767 
09768     lsav = *lnew;
09769 L2:
09770     lp = lend[next];
09771     list[*lnew] = next;
09772     lptr[*lnew] = *lnew + 1;
09773     ++(*lnew);
09774     next = list[lp];
09775     if (next != nst) {
09776         goto L2;
09777     }
09778 
09779     lptr[*lnew - 1] = lsav;
09780     lend[k] = *lnew - 1;
09781     return 0;
09782 } /* covsph_ */
09783 
09784 /* Subroutine */ int crlist_(int *n, int *ncol, double *x,
09785         double *y, double *z__, int *list, int *lend, int
09786         *lptr, int *lnew, int *ltri, int *listc, int *nb,
09787         double *xc, double *yc, double *zc, double *rc,
09788         int *ier)
09789 {
09790     /* System generated locals */
09791     int i__1, i__2;
09792 
09793     /* Builtin functions */
09794     //double acos(double);
09795 
09796     /* Local variables */
09797     static double c__[3], t;
09798     static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09799     static double v1[3], v2[3], v3[3];
09800     static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09801              lpn;
09802     static long int swp;
09803     static int ierr;
09804     extern /* Subroutine */ int circum_(double *, double *,
09805             double *, double *, int *);
09806     extern int lstptr_(int *, int *, int *, int *);
09807     extern long int swptst_(int *, int *, int *, int *,
09808             double *, double *, double *);
09809 
09810 
09811 /* *********************************************************** */
09812 
09813 /*                                              From STRIPACK */
09814 /*                                            Robert J. Renka */
09815 /*                                  Dept. of Computer Science */
09816 /*                                       Univ. of North Texas */
09817 /*                                           renka@cs.unt.edu */
09818 /*                                                   03/05/03 */
09819 
09820 /*   Given a Delaunay triangulation of nodes on the surface */
09821 /* of the unit sphere, this subroutine returns the set of */
09822 /* triangle circumcenters corresponding to Voronoi vertices, */
09823 /* along with the circumradii and a list of triangle indexes */
09824 /* LISTC stored in one-to-one correspondence with LIST/LPTR */
09825 /* entries. */
09826 
09827 /*   A triangle circumcenter is the point (unit vector) lying */
09828 /* at the same angular distance from the three vertices and */
09829 /* contained in the same hemisphere as the vertices.  (Note */
09830 /* that the negative of a circumcenter is also equidistant */
09831 /* from the vertices.)  If the triangulation covers the sur- */
09832 /* face, the Voronoi vertices are the circumcenters of the */
09833 /* triangles in the Delaunay triangulation.  LPTR, LEND, and */
09834 /* LNEW are not altered in this case. */
09835 
09836 /*   On the other hand, if the nodes are contained in a sin- */
09837 /* gle hemisphere, the triangulation is implicitly extended */
09838 /* to the entire surface by adding pseudo-arcs (of length */
09839 /* greater than 180 degrees) between boundary nodes forming */
09840 /* pseudo-triangles whose 'circumcenters' are included in the */
09841 /* list.  This extension to the triangulation actually con- */
09842 /* sists of a triangulation of the set of boundary nodes in */
09843 /* which the swap test is reversed (a non-empty circumcircle */
09844 /* test).  The negative circumcenters are stored as the */
09845 /* pseudo-triangle 'circumcenters'.  LISTC, LPTR, LEND, and */
09846 /* LNEW contain a data structure corresponding to the ex- */
09847 /* tended triangulation (Voronoi diagram), but LIST is not */
09848 /* altered in this case.  Thus, if it is necessary to retain */
09849 /* the original (unextended) triangulation data structure, */
09850 /* copies of LPTR and LNEW must be saved before calling this */
09851 /* routine. */
09852 
09853 
09854 /* On input: */
09855 
09856 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09857 /*           Note that, if N = 3, there are only two Voronoi */
09858 /*           vertices separated by 180 degrees, and the */
09859 /*           Voronoi regions are not well defined. */
09860 
09861 /*       NCOL = Number of columns reserved for LTRI.  This */
09862 /*              must be at least NB-2, where NB is the number */
09863 /*              of boundary nodes. */
09864 
09865 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09866 /*               coordinates of the nodes (unit vectors). */
09867 
09868 /*       LIST = int array containing the set of adjacency */
09869 /*              lists.  Refer to Subroutine TRMESH. */
09870 
09871 /*       LEND = Set of pointers to ends of adjacency lists. */
09872 /*              Refer to Subroutine TRMESH. */
09873 
09874 /* The above parameters are not altered by this routine. */
09875 
09876 /*       LPTR = Array of pointers associated with LIST.  Re- */
09877 /*              fer to Subroutine TRMESH. */
09878 
09879 /*       LNEW = Pointer to the first empty location in LIST */
09880 /*              and LPTR (list length plus one). */
09881 
09882 /*       LTRI = int work space array dimensioned 6 by */
09883 /*              NCOL, or unused dummy parameter if NB = 0. */
09884 
09885 /*       LISTC = int array of length at least 3*NT, where */
09886 /*               NT = 2*N-4 is the number of triangles in the */
09887 /*               triangulation (after extending it to cover */
09888 /*               the entire surface if necessary). */
09889 
09890 /*       XC,YC,ZC,RC = Arrays of length NT = 2*N-4. */
09891 
09892 /* On output: */
09893 
09894 /*       LPTR = Array of pointers associated with LISTC: */
09895 /*              updated for the addition of pseudo-triangles */
09896 /*              if the original triangulation contains */
09897 /*              boundary nodes (NB > 0). */
09898 
09899 /*       LNEW = Pointer to the first empty location in LISTC */
09900 /*              and LPTR (list length plus one).  LNEW is not */
09901 /*              altered if NB = 0. */
09902 
09903 /*       LTRI = Triangle list whose first NB-2 columns con- */
09904 /*              tain the indexes of a clockwise-ordered */
09905 /*              sequence of vertices (first three rows) */
09906 /*              followed by the LTRI column indexes of the */
09907 /*              triangles opposite the vertices (or 0 */
09908 /*              denoting the exterior region) in the last */
09909 /*              three rows.  This array is not generally of */
09910 /*              any use. */
09911 
09912 /*       LISTC = Array containing triangle indexes (indexes */
09913 /*               to XC, YC, ZC, and RC) stored in 1-1 corres- */
09914 /*               pondence with LIST/LPTR entries (or entries */
09915 /*               that would be stored in LIST for the */
09916 /*               extended triangulation):  the index of tri- */
09917 /*               angle (N1,N2,N3) is stored in LISTC(K), */
09918 /*               LISTC(L), and LISTC(M), where LIST(K), */
09919 /*               LIST(L), and LIST(M) are the indexes of N2 */
09920 /*               as a neighbor of N1, N3 as a neighbor of N2, */
09921 /*               and N1 as a neighbor of N3.  The Voronoi */
09922 /*               region associated with a node is defined by */
09923 /*               the CCW-ordered sequence of circumcenters in */
09924 /*               one-to-one correspondence with its adjacency */
09925 /*               list (in the extended triangulation). */
09926 
09927 /*       NB = Number of boundary nodes unless IER = 1. */
09928 
09929 /*       XC,YC,ZC = Arrays containing the Cartesian coordi- */
09930 /*                  nates of the triangle circumcenters */
09931 /*                  (Voronoi vertices).  XC(I)**2 + YC(I)**2 */
09932 /*                  + ZC(I)**2 = 1.  The first NB-2 entries */
09933 /*                  correspond to pseudo-triangles if NB > 0. */
09934 
09935 /*       RC = Array containing circumradii (the arc lengths */
09936 /*            or angles between the circumcenters and associ- */
09937 /*            ated triangle vertices) in 1-1 correspondence */
09938 /*            with circumcenters. */
09939 
09940 /*       IER = Error indicator: */
09941 /*             IER = 0 if no errors were encountered. */
09942 /*             IER = 1 if N < 3. */
09943 /*             IER = 2 if NCOL < NB-2. */
09944 /*             IER = 3 if a triangle is degenerate (has ver- */
09945 /*                     tices lying on a common geodesic). */
09946 
09947 /* Modules required by CRLIST:  CIRCUM, LSTPTR, SWPTST */
09948 
09949 /* Intrinsic functions called by CRLIST:  ABS, ACOS */
09950 
09951 /* *********************************************************** */
09952 
09953 
09954 /* Local parameters: */
09955 
09956 /* C =         Circumcenter returned by Subroutine CIRCUM */
09957 /* I1,I2,I3 =  Permutation of (1,2,3):  LTRI row indexes */
09958 /* I4 =        LTRI row index in the range 1 to 3 */
09959 /* IERR =      Error flag for calls to CIRCUM */
09960 /* KT =        Triangle index */
09961 /* KT1,KT2 =   Indexes of a pair of adjacent pseudo-triangles */
09962 /* KT11,KT12 = Indexes of the pseudo-triangles opposite N1 */
09963 /*               and N2 as vertices of KT1 */
09964 /* KT21,KT22 = Indexes of the pseudo-triangles opposite N1 */
09965 /*               and N2 as vertices of KT2 */
09966 /* LP,LPN =    LIST pointers */
09967 /* LPL =       LIST pointer of the last neighbor of N1 */
09968 /* N0 =        Index of the first boundary node (initial */
09969 /*               value of N1) in the loop on boundary nodes */
09970 /*               used to store the pseudo-triangle indexes */
09971 /*               in LISTC */
09972 /* N1,N2,N3 =  Nodal indexes defining a triangle (CCW order) */
09973 /*               or pseudo-triangle (clockwise order) */
09974 /* N4 =        Index of the node opposite N2 -> N1 */
09975 /* NM2 =       N-2 */
09976 /* NN =        Local copy of N */
09977 /* NT =        Number of pseudo-triangles:  NB-2 */
09978 /* SWP =       long int variable set to TRUE in each optimiza- */
09979 /*               tion loop (loop on pseudo-arcs) iff a swap */
09980 /*               is performed */
09981 /* V1,V2,V3 =  Vertices of triangle KT = (N1,N2,N3) sent to */
09982 /*               Subroutine CIRCUM */
09983 
09984     /* Parameter adjustments */
09985     --lend;
09986     --z__;
09987     --y;
09988     --x;
09989     ltri -= 7;
09990     --list;
09991     --lptr;
09992     --listc;
09993     --xc;
09994     --yc;
09995     --zc;
09996     --rc;
09997 
09998     /* Function Body */
09999     nn = *n;
10000     *nb = 0;
10001     nt = 0;
10002     if (nn < 3) {
10003         goto L21;
10004     }
10005 
10006 /* Search for a boundary node N1. */
10007 
10008     i__1 = nn;
10009     for (n1 = 1; n1 <= i__1; ++n1) {
10010         lp = lend[n1];
10011         if (list[lp] < 0) {
10012             goto L2;
10013         }
10014 /* L1: */
10015     }
10016 
10017 /* The triangulation already covers the sphere. */
10018 
10019     goto L9;
10020 
10021 /* There are NB .GE. 3 boundary nodes.  Add NB-2 pseudo- */
10022 /*   triangles (N1,N2,N3) by connecting N3 to the NB-3 */
10023 /*   boundary nodes to which it is not already adjacent. */
10024 
10025 /*   Set N3 and N2 to the first and last neighbors, */
10026 /*     respectively, of N1. */
10027 
10028 L2:
10029     n2 = -list[lp];
10030     lp = lptr[lp];
10031     n3 = list[lp];
10032 
10033 /*   Loop on boundary arcs N1 -> N2 in clockwise order, */
10034 /*     storing triangles (N1,N2,N3) in column NT of LTRI */
10035 /*     along with the indexes of the triangles opposite */
10036 /*     the vertices. */
10037 
10038 L3:
10039     ++nt;
10040     if (nt <= *ncol) {
10041         ltri[nt * 6 + 1] = n1;
10042         ltri[nt * 6 + 2] = n2;
10043         ltri[nt * 6 + 3] = n3;
10044         ltri[nt * 6 + 4] = nt + 1;
10045         ltri[nt * 6 + 5] = nt - 1;
10046         ltri[nt * 6 + 6] = 0;
10047     }
10048     n1 = n2;
10049     lp = lend[n1];
10050     n2 = -list[lp];
10051     if (n2 != n3) {
10052         goto L3;
10053     }
10054 
10055     *nb = nt + 2;
10056     if (*ncol < nt) {
10057         goto L22;
10058     }
10059     ltri[nt * 6 + 4] = 0;
10060     if (nt == 1) {
10061         goto L7;
10062     }
10063 
10064 /* Optimize the exterior triangulation (set of pseudo- */
10065 /*   triangles) by applying swaps to the pseudo-arcs N1-N2 */
10066 /*   (pairs of adjacent pseudo-triangles KT1 and KT2 > KT1). */
10067 /*   The loop on pseudo-arcs is repeated until no swaps are */
10068 /*   performed. */
10069 
10070 L4:
10071     swp = FALSE_;
10072     i__1 = nt - 1;
10073     for (kt1 = 1; kt1 <= i__1; ++kt1) {
10074         for (i3 = 1; i3 <= 3; ++i3) {
10075             kt2 = ltri[i3 + 3 + kt1 * 6];
10076             if (kt2 <= kt1) {
10077                 goto L5;
10078             }
10079 
10080 /*   The LTRI row indexes (I1,I2,I3) of triangle KT1 = */
10081 /*     (N1,N2,N3) are a cyclical permutation of (1,2,3). */
10082 
10083             if (i3 == 1) {
10084                 i1 = 2;
10085                 i2 = 3;
10086             } else if (i3 == 2) {
10087                 i1 = 3;
10088                 i2 = 1;
10089             } else {
10090                 i1 = 1;
10091                 i2 = 2;
10092             }
10093             n1 = ltri[i1 + kt1 * 6];
10094             n2 = ltri[i2 + kt1 * 6];
10095             n3 = ltri[i3 + kt1 * 6];
10096 
10097 /*   KT2 = (N2,N1,N4) for N4 = LTRI(I,KT2), where */
10098 /*     LTRI(I+3,KT2) = KT1. */
10099 
10100             if (ltri[kt2 * 6 + 4] == kt1) {
10101                 i4 = 1;
10102             } else if (ltri[kt2 * 6 + 5] == kt1) {
10103                 i4 = 2;
10104             } else {
10105                 i4 = 3;
10106             }
10107             n4 = ltri[i4 + kt2 * 6];
10108 
10109 /*   The empty circumcircle test is reversed for the pseudo- */
10110 /*     triangles.  The reversal is implicit in the clockwise */
10111 /*     ordering of the vertices. */
10112 
10113             if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10114                 goto L5;
10115             }
10116 
10117 /*   Swap arc N1-N2 for N3-N4.  KTij is the triangle opposite */
10118 /*     Nj as a vertex of KTi. */
10119 
10120             swp = TRUE_;
10121             kt11 = ltri[i1 + 3 + kt1 * 6];
10122             kt12 = ltri[i2 + 3 + kt1 * 6];
10123             if (i4 == 1) {
10124                 i2 = 2;
10125                 i1 = 3;
10126             } else if (i4 == 2) {
10127                 i2 = 3;
10128                 i1 = 1;
10129             } else {
10130                 i2 = 1;
10131                 i1 = 2;
10132             }
10133             kt21 = ltri[i1 + 3 + kt2 * 6];
10134             kt22 = ltri[i2 + 3 + kt2 * 6];
10135             ltri[kt1 * 6 + 1] = n4;
10136             ltri[kt1 * 6 + 2] = n3;
10137             ltri[kt1 * 6 + 3] = n1;
10138             ltri[kt1 * 6 + 4] = kt12;
10139             ltri[kt1 * 6 + 5] = kt22;
10140             ltri[kt1 * 6 + 6] = kt2;
10141             ltri[kt2 * 6 + 1] = n3;
10142             ltri[kt2 * 6 + 2] = n4;
10143             ltri[kt2 * 6 + 3] = n2;
10144             ltri[kt2 * 6 + 4] = kt21;
10145             ltri[kt2 * 6 + 5] = kt11;
10146             ltri[kt2 * 6 + 6] = kt1;
10147 
10148 /*   Correct the KT11 and KT22 entries that changed. */
10149 
10150             if (kt11 != 0) {
10151                 i4 = 4;
10152                 if (ltri[kt11 * 6 + 4] != kt1) {
10153                     i4 = 5;
10154                     if (ltri[kt11 * 6 + 5] != kt1) {
10155                         i4 = 6;
10156                     }
10157                 }
10158                 ltri[i4 + kt11 * 6] = kt2;
10159             }
10160             if (kt22 != 0) {
10161                 i4 = 4;
10162                 if (ltri[kt22 * 6 + 4] != kt2) {
10163                     i4 = 5;
10164                     if (ltri[kt22 * 6 + 5] != kt2) {
10165                         i4 = 6;
10166                     }
10167                 }
10168                 ltri[i4 + kt22 * 6] = kt1;
10169             }
10170 L5:
10171             ;
10172         }
10173 /* L6: */
10174     }
10175     if (swp) {
10176         goto L4;
10177     }
10178 
10179 /* Compute and store the negative circumcenters and radii of */
10180 /*   the pseudo-triangles in the first NT positions. */
10181 
10182 L7:
10183     i__1 = nt;
10184     for (kt = 1; kt <= i__1; ++kt) {
10185         n1 = ltri[kt * 6 + 1];
10186         n2 = ltri[kt * 6 + 2];
10187         n3 = ltri[kt * 6 + 3];
10188         v1[0] = x[n1];
10189         v1[1] = y[n1];
10190         v1[2] = z__[n1];
10191         v2[0] = x[n2];
10192         v2[1] = y[n2];
10193         v2[2] = z__[n2];
10194         v3[0] = x[n3];
10195         v3[1] = y[n3];
10196         v3[2] = z__[n3];
10197         circum_(v2, v1, v3, c__, &ierr);
10198         if (ierr != 0) {
10199             goto L23;
10200         }
10201 
10202 /*   Store the negative circumcenter and radius (computed */
10203 /*     from <V1,C>). */
10204 
10205         xc[kt] = -c__[0];
10206         yc[kt] = -c__[1];
10207         zc[kt] = -c__[2];
10208         t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10209         if (t < -1.) {
10210             t = -1.;
10211         }
10212         if (t > 1.) {
10213             t = 1.;
10214         }
10215         rc[kt] = acos(t);
10216 /* L8: */
10217     }
10218 
10219 /* Compute and store the circumcenters and radii of the */
10220 /*   actual triangles in positions KT = NT+1, NT+2, ... */
10221 /*   Also, store the triangle indexes KT in the appropriate */
10222 /*   LISTC positions. */
10223 
10224 L9:
10225     kt = nt;
10226 
10227 /*   Loop on nodes N1. */
10228 
10229     nm2 = nn - 2;
10230     i__1 = nm2;
10231     for (n1 = 1; n1 <= i__1; ++n1) {
10232         lpl = lend[n1];
10233         lp = lpl;
10234         n3 = list[lp];
10235 
10236 /*   Loop on adjacent neighbors N2,N3 of N1 for which N2 > N1 */
10237 /*     and N3 > N1. */
10238 
10239 L10:
10240         lp = lptr[lp];
10241         n2 = n3;
10242         n3 = (i__2 = list[lp], abs(i__2));
10243         if (n2 <= n1 || n3 <= n1) {
10244             goto L11;
10245         }
10246         ++kt;
10247 
10248 /*   Compute the circumcenter C of triangle KT = (N1,N2,N3). */
10249 
10250         v1[0] = x[n1];
10251         v1[1] = y[n1];
10252         v1[2] = z__[n1];
10253         v2[0] = x[n2];
10254         v2[1] = y[n2];
10255         v2[2] = z__[n2];
10256         v3[0] = x[n3];
10257         v3[1] = y[n3];
10258         v3[2] = z__[n3];
10259         circum_(v1, v2, v3, c__, &ierr);
10260         if (ierr != 0) {
10261             goto L23;
10262         }
10263 
10264 /*   Store the circumcenter, radius and triangle index. */
10265 
10266         xc[kt] = c__[0];
10267         yc[kt] = c__[1];
10268         zc[kt] = c__[2];
10269         t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10270         if (t < -1.) {
10271             t = -1.;
10272         }
10273         if (t > 1.) {
10274             t = 1.;
10275         }
10276         rc[kt] = acos(t);
10277 
10278 /*   Store KT in LISTC(LPN), where Abs(LIST(LPN)) is the */
10279 /*     index of N2 as a neighbor of N1, N3 as a neighbor */
10280 /*     of N2, and N1 as a neighbor of N3. */
10281 
10282         lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10283         listc[lpn] = kt;
10284         lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10285         listc[lpn] = kt;
10286         lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10287         listc[lpn] = kt;
10288 L11:
10289         if (lp != lpl) {
10290             goto L10;
10291         }
10292 /* L12: */
10293     }
10294     if (nt == 0) {
10295         goto L20;
10296     }
10297 
10298 /* Store the first NT triangle indexes in LISTC. */
10299 
10300 /*   Find a boundary triangle KT1 = (N1,N2,N3) with a */
10301 /*     boundary arc opposite N3. */
10302 
10303     kt1 = 0;
10304 L13:
10305     ++kt1;
10306     if (ltri[kt1 * 6 + 4] == 0) {
10307         i1 = 2;
10308         i2 = 3;
10309         i3 = 1;
10310         goto L14;
10311     } else if (ltri[kt1 * 6 + 5] == 0) {
10312         i1 = 3;
10313         i2 = 1;
10314         i3 = 2;
10315         goto L14;
10316     } else if (ltri[kt1 * 6 + 6] == 0) {
10317         i1 = 1;
10318         i2 = 2;
10319         i3 = 3;
10320         goto L14;
10321     }
10322     goto L13;
10323 L14:
10324     n1 = ltri[i1 + kt1 * 6];
10325     n0 = n1;
10326 
10327 /*   Loop on boundary nodes N1 in CCW order, storing the */
10328 /*     indexes of the clockwise-ordered sequence of triangles */
10329 /*     that contain N1.  The first triangle overwrites the */
10330 /*     last neighbor position, and the remaining triangles, */
10331 /*     if any, are appended to N1's adjacency list. */
10332 
10333 /*   A pointer to the first neighbor of N1 is saved in LPN. */
10334 
10335 L15:
10336     lp = lend[n1];
10337     lpn = lptr[lp];
10338     listc[lp] = kt1;
10339 
10340 /*   Loop on triangles KT2 containing N1. */
10341 
10342 L16:
10343     kt2 = ltri[i2 + 3 + kt1 * 6];
10344     if (kt2 != 0) {
10345 
10346 /*   Append KT2 to N1's triangle list. */
10347 
10348         lptr[lp] = *lnew;
10349         lp = *lnew;
10350         listc[lp] = kt2;
10351         ++(*lnew);
10352 
10353 /*   Set KT1 to KT2 and update (I1,I2,I3) such that */
10354 /*     LTRI(I1,KT1) = N1. */
10355 
10356         kt1 = kt2;
10357         if (ltri[kt1 * 6 + 1] == n1) {
10358             i1 = 1;
10359             i2 = 2;
10360             i3 = 3;
10361         } else if (ltri[kt1 * 6 + 2] == n1) {
10362             i1 = 2;
10363             i2 = 3;
10364             i3 = 1;
10365         } else {
10366             i1 = 3;
10367             i2 = 1;
10368             i3 = 2;
10369         }
10370         goto L16;
10371     }
10372 
10373 /*   Store the saved first-triangle pointer in LPTR(LP), set */
10374 /*     N1 to the next boundary node, test for termination, */
10375 /*     and permute the indexes:  the last triangle containing */
10376 /*     a boundary node is the first triangle containing the */
10377 /*     next boundary node. */
10378 
10379     lptr[lp] = lpn;
10380     n1 = ltri[i3 + kt1 * 6];
10381     if (n1 != n0) {
10382         i4 = i3;
10383         i3 = i2;
10384         i2 = i1;
10385         i1 = i4;
10386         goto L15;
10387     }
10388 
10389 /* No errors encountered. */
10390 
10391 L20:
10392     *ier = 0;
10393     return 0;
10394 
10395 /* N < 3. */
10396 
10397 L21:
10398     *ier = 1;
10399     return 0;
10400 
10401 /* Insufficient space reserved for LTRI. */
10402 
10403 L22:
10404     *ier = 2;
10405     return 0;
10406 
10407 /* Error flag returned by CIRCUM: KT indexes a null triangle. */
10408 
10409 L23:
10410     *ier = 3;
10411     return 0;
10412 } /* crlist_ */
10413 
10414 /* Subroutine */ int delarc_(int *n, int *io1, int *io2, int *
10415         list, int *lptr, int *lend, int *lnew, int *ier)
10416 {
10417     /* System generated locals */
10418     int i__1;
10419 
10420     /* Local variables */
10421     static int n1, n2, n3, lp, lph, lpl;
10422     extern /* Subroutine */ int delnb_(int *, int *, int *,
10423             int *, int *, int *, int *, int *);
10424     extern int lstptr_(int *, int *, int *, int *);
10425 
10426 
10427 /* *********************************************************** */
10428 
10429 /*                                              From STRIPACK */
10430 /*                                            Robert J. Renka */
10431 /*                                  Dept. of Computer Science */
10432 /*                                       Univ. of North Texas */
10433 /*                                           renka@cs.unt.edu */
10434 /*                                                   07/17/96 */
10435 
10436 /*   This subroutine deletes a boundary arc from a triangula- */
10437 /* tion.  It may be used to remove a null triangle from the */
10438 /* convex hull boundary.  Note, however, that if the union of */
10439 /* triangles is rendered nonconvex, Subroutines DELNOD, EDGE, */
10440 /* and TRFIND (and hence ADDNOD) may fail.  Also, Function */
10441 /* NEARND should not be called following an arc deletion. */
10442 
10443 /*   This routine is identical to the similarly named routine */
10444 /* in TRIPACK. */
10445 
10446 
10447 /* On input: */
10448 
10449 /*       N = Number of nodes in the triangulation.  N .GE. 4. */
10450 
10451 /*       IO1,IO2 = Indexes (in the range 1 to N) of a pair of */
10452 /*                 adjacent boundary nodes defining the arc */
10453 /*                 to be removed. */
10454 
10455 /* The above parameters are not altered by this routine. */
10456 
10457 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
10458 /*                             created by Subroutine TRMESH. */
10459 
10460 /* On output: */
10461 
10462 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10463 /*                             the removal of arc IO1-IO2 */
10464 /*                             unless IER > 0. */
10465 
10466 /*       IER = Error indicator: */
10467 /*             IER = 0 if no errors were encountered. */
10468 /*             IER = 1 if N, IO1, or IO2 is outside its valid */
10469 /*                     range, or IO1 = IO2. */
10470 /*             IER = 2 if IO1-IO2 is not a boundary arc. */
10471 /*             IER = 3 if the node opposite IO1-IO2 is al- */
10472 /*                     ready a boundary node, and thus IO1 */
10473 /*                     or IO2 has only two neighbors or a */
10474 /*                     deletion would result in two triangu- */
10475 /*                     lations sharing a single node. */
10476 /*             IER = 4 if one of the nodes is a neighbor of */
10477 /*                     the other, but not vice versa, imply- */
10478 /*                     ing an invalid triangulation data */
10479 /*                     structure. */
10480 
10481 /* Module required by DELARC:  DELNB, LSTPTR */
10482 
10483 /* Intrinsic function called by DELARC:  ABS */
10484 
10485 /* *********************************************************** */
10486 
10487 
10488 /* Local parameters: */
10489 
10490 /* LP =       LIST pointer */
10491 /* LPH =      LIST pointer or flag returned by DELNB */
10492 /* LPL =      Pointer to the last neighbor of N1, N2, or N3 */
10493 /* N1,N2,N3 = Nodal indexes of a triangle such that N1->N2 */
10494 /*              is the directed boundary edge associated */
10495 /*              with IO1-IO2 */
10496 
10497     /* Parameter adjustments */
10498     --lend;
10499     --list;
10500     --lptr;
10501 
10502     /* Function Body */
10503     n1 = *io1;
10504     n2 = *io2;
10505 
10506 /* Test for errors, and set N1->N2 to the directed boundary */
10507 /*   edge associated with IO1-IO2:  (N1,N2,N3) is a triangle */
10508 /*   for some N3. */
10509 
10510     if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10511         *ier = 1;
10512         return 0;
10513     }
10514 
10515     lpl = lend[n2];
10516     if (-list[lpl] != n1) {
10517         n1 = n2;
10518         n2 = *io1;
10519         lpl = lend[n2];
10520         if (-list[lpl] != n1) {
10521             *ier = 2;
10522             return 0;
10523         }
10524     }
10525 
10526 /* Set N3 to the node opposite N1->N2 (the second neighbor */
10527 /*   of N1), and test for error 3 (N3 already a boundary */
10528 /*   node). */
10529 
10530     lpl = lend[n1];
10531     lp = lptr[lpl];
10532     lp = lptr[lp];
10533     n3 = (i__1 = list[lp], abs(i__1));
10534     lpl = lend[n3];
10535     if (list[lpl] <= 0) {
10536         *ier = 3;
10537         return 0;
10538     }
10539 
10540 /* Delete N2 as a neighbor of N1, making N3 the first */
10541 /*   neighbor, and test for error 4 (N2 not a neighbor */
10542 /*   of N1).  Note that previously computed pointers may */
10543 /*   no longer be valid following the call to DELNB. */
10544 
10545     delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10546     if (lph < 0) {
10547         *ier = 4;
10548         return 0;
10549     }
10550 
10551 /* Delete N1 as a neighbor of N2, making N3 the new last */
10552 /*   neighbor. */
10553 
10554     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10555 
10556 /* Make N3 a boundary node with first neighbor N2 and last */
10557 /*   neighbor N1. */
10558 
10559     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10560     lend[n3] = lp;
10561     list[lp] = -n1;
10562 
10563 /* No errors encountered. */
10564 
10565     *ier = 0;
10566     return 0;
10567 } /* delarc_ */
10568 
10569 /* Subroutine */ int delnb_(int *n0, int *nb, int *n, int *
10570         list, int *lptr, int *lend, int *lnew, int *lph)
10571 {
10572     /* System generated locals */
10573     int i__1;
10574 
10575     /* Local variables */
10576     static int i__, lp, nn, lpb, lpl, lpp, lnw;
10577 
10578 
10579 /* *********************************************************** */
10580 
10581 /*                                              From STRIPACK */
10582 /*                                            Robert J. Renka */
10583 /*                                  Dept. of Computer Science */
10584 /*                                       Univ. of North Texas */
10585 /*                                           renka@cs.unt.edu */
10586 /*                                                   07/29/98 */
10587 
10588 /*   This subroutine deletes a neighbor NB from the adjacency */
10589 /* list of node N0 (but N0 is not deleted from the adjacency */
10590 /* list of NB) and, if NB is a boundary node, makes N0 a */
10591 /* boundary node.  For pointer (LIST index) LPH to NB as a */
10592 /* neighbor of N0, the empty LIST,LPTR location LPH is filled */
10593 /* in with the values at LNEW-1, pointer LNEW-1 (in LPTR and */
10594 /* possibly in LEND) is changed to LPH, and LNEW is decremen- */
10595 /* ted.  This requires a search of LEND and LPTR entailing an */
10596 /* expected operation count of O(N). */
10597 
10598 /*   This routine is identical to the similarly named routine */
10599 /* in TRIPACK. */
10600 
10601 
10602 /* On input: */
10603 
10604 /*       N0,NB = Indexes, in the range 1 to N, of a pair of */
10605 /*               nodes such that NB is a neighbor of N0. */
10606 /*               (N0 need not be a neighbor of NB.) */
10607 
10608 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
10609 
10610 /* The above parameters are not altered by this routine. */
10611 
10612 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10613 /*                             triangulation. */
10614 
10615 /* On output: */
10616 
10617 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10618 /*                             the removal of NB from the ad- */
10619 /*                             jacency list of N0 unless */
10620 /*                             LPH < 0. */
10621 
10622 /*       LPH = List pointer to the hole (NB as a neighbor of */
10623 /*             N0) filled in by the values at LNEW-1 or error */
10624 /*             indicator: */
10625 /*             LPH > 0 if no errors were encountered. */
10626 /*             LPH = -1 if N0, NB, or N is outside its valid */
10627 /*                      range. */
10628 /*             LPH = -2 if NB is not a neighbor of N0. */
10629 
10630 /* Modules required by DELNB:  None */
10631 
10632 /* Intrinsic function called by DELNB:  ABS */
10633 
10634 /* *********************************************************** */
10635 
10636 
10637 /* Local parameters: */
10638 
10639 /* I =   DO-loop index */
10640 /* LNW = LNEW-1 (output value of LNEW) */
10641 /* LP =  LIST pointer of the last neighbor of NB */
10642 /* LPB = Pointer to NB as a neighbor of N0 */
10643 /* LPL = Pointer to the last neighbor of N0 */
10644 /* LPP = Pointer to the neighbor of N0 that precedes NB */
10645 /* NN =  Local copy of N */
10646 
10647     /* Parameter adjustments */
10648     --lend;
10649     --list;
10650     --lptr;
10651 
10652     /* Function Body */
10653     nn = *n;
10654 
10655 /* Test for error 1. */
10656 
10657     if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10658         *lph = -1;
10659         return 0;
10660     }
10661 
10662 /*   Find pointers to neighbors of N0: */
10663 
10664 /*     LPL points to the last neighbor, */
10665 /*     LPP points to the neighbor NP preceding NB, and */
10666 /*     LPB points to NB. */
10667 
10668     lpl = lend[*n0];
10669     lpp = lpl;
10670     lpb = lptr[lpp];
10671 L1:
10672     if (list[lpb] == *nb) {
10673         goto L2;
10674     }
10675     lpp = lpb;
10676     lpb = lptr[lpp];
10677     if (lpb != lpl) {
10678         goto L1;
10679     }
10680 
10681 /*   Test for error 2 (NB not found). */
10682 
10683     if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10684         *lph = -2;
10685         return 0;
10686     }
10687 
10688 /*   NB is the last neighbor of N0.  Make NP the new last */
10689 /*     neighbor and, if NB is a boundary node, then make N0 */
10690 /*     a boundary node. */
10691 
10692     lend[*n0] = lpp;
10693     lp = lend[*nb];
10694     if (list[lp] < 0) {
10695         list[lpp] = -list[lpp];
10696     }
10697     goto L3;
10698 
10699 /*   NB is not the last neighbor of N0.  If NB is a boundary */
10700 /*     node and N0 is not, then make N0 a boundary node with */
10701 /*     last neighbor NP. */
10702 
10703 L2:
10704     lp = lend[*nb];
10705     if (list[lp] < 0 && list[lpl] > 0) {
10706         lend[*n0] = lpp;
10707         list[lpp] = -list[lpp];
10708     }
10709 
10710 /*   Update LPTR so that the neighbor following NB now fol- */
10711 /*     lows NP, and fill in the hole at location LPB. */
10712 
10713 L3:
10714     lptr[lpp] = lptr[lpb];
10715     lnw = *lnew - 1;
10716     list[lpb] = list[lnw];
10717     lptr[lpb] = lptr[lnw];
10718     for (i__ = nn; i__ >= 1; --i__) {
10719         if (lend[i__] == lnw) {
10720             lend[i__] = lpb;
10721             goto L5;
10722         }
10723 /* L4: */
10724     }
10725 
10726 L5:
10727     i__1 = lnw - 1;
10728     for (i__ = 1; i__ <= i__1; ++i__) {
10729         if (lptr[i__] == lnw) {
10730             lptr[i__] = lpb;
10731         }
10732 /* L6: */
10733     }
10734 
10735 /* No errors encountered. */
10736 
10737     *lnew = lnw;
10738     *lph = lpb;
10739     return 0;
10740 } /* delnb_ */
10741 
10742 /* Subroutine */ int delnod_(int *k, int *n, double *x,
10743         double *y, double *z__, int *list, int *lptr, int
10744         *lend, int *lnew, int *lwk, int *iwk, int *ier)
10745 {
10746     /* System generated locals */
10747     int i__1;
10748 
10749     /* Local variables */
10750     static int i__, j, n1, n2;
10751     static double x1, x2, y1, y2, z1, z2;
10752     static int nl, lp, nn, nr;
10753     static double xl, yl, zl, xr, yr, zr;
10754     static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10755     extern long int left_(double *, double *, double *, double
10756             *, double *, double *, double *, double *,
10757             double *);
10758     static long int bdry;
10759     static int ierr, lwkl;
10760     extern /* Subroutine */ int swap_(int *, int *, int *,
10761             int *, int *, int *, int *, int *), delnb_(
10762             int *, int *, int *, int *, int *, int *,
10763             int *, int *);
10764     extern int nbcnt_(int *, int *);
10765     extern /* Subroutine */ int optim_(double *, double *, double
10766             *, int *, int *, int *, int *, int *, int
10767             *, int *);
10768     static int nfrst;
10769     extern int lstptr_(int *, int *, int *, int *);
10770 
10771 
10772 /* *********************************************************** */
10773 
10774 /*                                              From STRIPACK */
10775 /*                                            Robert J. Renka */
10776 /*                                  Dept. of Computer Science */
10777 /*                                       Univ. of North Texas */
10778 /*                                           renka@cs.unt.edu */
10779 /*                                                   11/30/99 */
10780 
10781 /*   This subroutine deletes node K (along with all arcs */
10782 /* incident on node K) from a triangulation of N nodes on the */
10783 /* unit sphere, and inserts arcs as necessary to produce a */
10784 /* triangulation of the remaining N-1 nodes.  If a Delaunay */
10785 /* triangulation is input, a Delaunay triangulation will */
10786 /* result, and thus, DELNOD reverses the effect of a call to */
10787 /* Subroutine ADDNOD. */
10788 
10789 
10790 /* On input: */
10791 
10792 /*       K = Index (for X, Y, and Z) of the node to be */
10793 /*           deleted.  1 .LE. K .LE. N. */
10794 
10795 /* K is not altered by this routine. */
10796 
10797 /*       N = Number of nodes in the triangulation on input. */
10798 /*           N .GE. 4.  Note that N will be decremented */
10799 /*           following the deletion. */
10800 
10801 /*       X,Y,Z = Arrays of length N containing the Cartesian */
10802 /*               coordinates of the nodes in the triangula- */
10803 /*               tion. */
10804 
10805 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10806 /*                             triangulation.  Refer to Sub- */
10807 /*                             routine TRMESH. */
10808 
10809 /*       LWK = Number of columns reserved for IWK.  LWK must */
10810 /*             be at least NNB-3, where NNB is the number of */
10811 /*             neighbors of node K, including an extra */
10812 /*             pseudo-node if K is a boundary node. */
10813 
10814 /*       IWK = int work array dimensioned 2 by LWK (or */
10815 /*             array of length .GE. 2*LWK). */
10816 
10817 /* On output: */
10818 
10819 /*       N = Number of nodes in the triangulation on output. */
10820 /*           The input value is decremented unless 1 .LE. IER */
10821 /*           .LE. 4. */
10822 
10823 /*       X,Y,Z = Updated arrays containing nodal coordinates */
10824 /*               (with elements K+1,...,N+1 shifted up one */
10825 /*               position, thus overwriting element K) unless */
10826 /*               1 .LE. IER .LE. 4. */
10827 
10828 /*       LIST,LPTR,LEND,LNEW = Updated triangulation data */
10829 /*                             structure reflecting the dele- */
10830 /*                             tion unless 1 .LE. IER .LE. 4. */
10831 /*                             Note that the data structure */
10832 /*                             may have been altered if IER > */
10833 /*                             3. */
10834 
10835 /*       LWK = Number of IWK columns required unless IER = 1 */
10836 /*             or IER = 3. */
10837 
10838 /*       IWK = Indexes of the endpoints of the new arcs added */
10839 /*             unless LWK = 0 or 1 .LE. IER .LE. 4.  (Arcs */
10840 /*             are associated with columns, or pairs of */
10841 /*             adjacent elements if IWK is declared as a */
10842 /*             singly-subscripted array.) */
10843 
10844 /*       IER = Error indicator: */
10845 /*             IER = 0 if no errors were encountered. */
10846 /*             IER = 1 if K or N is outside its valid range */
10847 /*                     or LWK < 0 on input. */
10848 /*             IER = 2 if more space is required in IWK. */
10849 /*                     Refer to LWK. */
10850 /*             IER = 3 if the triangulation data structure is */
10851 /*                     invalid on input. */
10852 /*             IER = 4 if K indexes an interior node with */
10853 /*                     four or more neighbors, none of which */
10854 /*                     can be swapped out due to collineari- */
10855 /*                     ty, and K cannot therefore be deleted. */
10856 /*             IER = 5 if an error flag (other than IER = 1) */
10857 /*                     was returned by OPTIM.  An error */
10858 /*                     message is written to the standard */
10859 /*                     output unit in this case. */
10860 /*             IER = 6 if error flag 1 was returned by OPTIM. */
10861 /*                     This is not necessarily an error, but */
10862 /*                     the arcs may not be optimal. */
10863 
10864 /*   Note that the deletion may result in all remaining nodes */
10865 /* being collinear.  This situation is not flagged. */
10866 
10867 /* Modules required by DELNOD:  DELNB, LEFT, LSTPTR, NBCNT, */
10868 /*                                OPTIM, SWAP, SWPTST */
10869 
10870 /* Intrinsic function called by DELNOD:  ABS */
10871 
10872 /* *********************************************************** */
10873 
10874 
10875 /* Local parameters: */
10876 
10877 /* BDRY =    long int variable with value TRUE iff N1 is a */
10878 /*             boundary node */
10879 /* I,J =     DO-loop indexes */
10880 /* IERR =    Error flag returned by OPTIM */
10881 /* IWL =     Number of IWK columns containing arcs */
10882 /* LNW =     Local copy of LNEW */
10883 /* LP =      LIST pointer */
10884 /* LP21 =    LIST pointer returned by SWAP */
10885 /* LPF,LPL = Pointers to the first and last neighbors of N1 */
10886 /* LPH =     Pointer (or flag) returned by DELNB */
10887 /* LPL2 =    Pointer to the last neighbor of N2 */
10888 /* LPN =     Pointer to a neighbor of N1 */
10889 /* LWKL =    Input value of LWK */
10890 /* N1 =      Local copy of K */
10891 /* N2 =      Neighbor of N1 */
10892 /* NFRST =   First neighbor of N1:  LIST(LPF) */
10893 /* NIT =     Number of iterations in OPTIM */
10894 /* NR,NL =   Neighbors of N1 preceding (to the right of) and */
10895 /*             following (to the left of) N2, respectively */
10896 /* NN =      Number of nodes in the triangulation */
10897 /* NNB =     Number of neighbors of N1 (including a pseudo- */
10898 /*             node representing the boundary if N1 is a */
10899 /*             boundary node) */
10900 /* X1,Y1,Z1 = Coordinates of N1 */
10901 /* X2,Y2,Z2 = Coordinates of N2 */
10902 /* XL,YL,ZL = Coordinates of NL */
10903 /* XR,YR,ZR = Coordinates of NR */
10904 
10905 
10906 /* Set N1 to K and NNB to the number of neighbors of N1 (plus */
10907 /*   one if N1 is a boundary node), and test for errors.  LPF */
10908 /*   and LPL are LIST indexes of the first and last neighbors */
10909 /*   of N1, IWL is the number of IWK columns containing arcs, */
10910 /*   and BDRY is TRUE iff N1 is a boundary node. */
10911 
10912     /* Parameter adjustments */
10913     iwk -= 3;
10914     --lend;
10915     --lptr;
10916     --list;
10917     --z__;
10918     --y;
10919     --x;
10920 
10921     /* Function Body */
10922     n1 = *k;
10923     nn = *n;
10924     if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
10925         goto L21;
10926     }
10927     lpl = lend[n1];
10928     lpf = lptr[lpl];
10929     nnb = nbcnt_(&lpl, &lptr[1]);
10930     bdry = list[lpl] < 0;
10931     if (bdry) {
10932         ++nnb;
10933     }
10934     if (nnb < 3) {
10935         goto L23;
10936     }
10937     lwkl = *lwk;
10938     *lwk = nnb - 3;
10939     if (lwkl < *lwk) {
10940         goto L22;
10941     }
10942     iwl = 0;
10943     if (nnb == 3) {
10944         goto L3;
10945     }
10946 
10947 /* Initialize for loop on arcs N1-N2 for neighbors N2 of N1, */
10948 /*   beginning with the second neighbor.  NR and NL are the */
10949 /*   neighbors preceding and following N2, respectively, and */
10950 /*   LP indexes NL.  The loop is exited when all possible */
10951 /*   swaps have been applied to arcs incident on N1. */
10952 
10953     x1 = x[n1];
10954     y1 = y[n1];
10955     z1 = z__[n1];
10956     nfrst = list[lpf];
10957     nr = nfrst;
10958     xr = x[nr];
10959     yr = y[nr];
10960     zr = z__[nr];
10961     lp = lptr[lpf];
10962     n2 = list[lp];
10963     x2 = x[n2];
10964     y2 = y[n2];
10965     z2 = z__[n2];
10966     lp = lptr[lp];
10967 
10968 /* Top of loop:  set NL to the neighbor following N2. */
10969 
10970 L1:
10971     nl = (i__1 = list[lp], abs(i__1));
10972     if (nl == nfrst && bdry) {
10973         goto L3;
10974     }
10975     xl = x[nl];
10976     yl = y[nl];
10977     zl = z__[nl];
10978 
10979 /*   Test for a convex quadrilateral.  To avoid an incorrect */
10980 /*     test caused by collinearity, use the fact that if N1 */
10981 /*     is a boundary node, then N1 LEFT NR->NL and if N2 is */
10982 /*     a boundary node, then N2 LEFT NL->NR. */
10983 
10984     lpl2 = lend[n2];
10985     if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
10986             list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
10987             z2)))) {
10988 
10989 /*   Nonconvex quadrilateral -- no swap is possible. */
10990 
10991         nr = n2;
10992         xr = x2;
10993         yr = y2;
10994         zr = z2;
10995         goto L2;
10996     }
10997 
10998 /*   The quadrilateral defined by adjacent triangles */
10999 /*     (N1,N2,NL) and (N2,N1,NR) is convex.  Swap in */
11000 /*     NL-NR and store it in IWK unless NL and NR are */
11001 /*     already adjacent, in which case the swap is not */
11002 /*     possible.  Indexes larger than N1 must be decremented */
11003 /*     since N1 will be deleted from X, Y, and Z. */
11004 
11005     swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11006     if (lp21 == 0) {
11007         nr = n2;
11008         xr = x2;
11009         yr = y2;
11010         zr = z2;
11011         goto L2;
11012     }
11013     ++iwl;
11014     if (nl <= n1) {
11015         iwk[(iwl << 1) + 1] = nl;
11016     } else {
11017         iwk[(iwl << 1) + 1] = nl - 1;
11018     }
11019     if (nr <= n1) {
11020         iwk[(iwl << 1) + 2] = nr;
11021     } else {
11022         iwk[(iwl << 1) + 2] = nr - 1;
11023     }
11024 
11025 /*   Recompute the LIST indexes and NFRST, and decrement NNB. */
11026 
11027     lpl = lend[n1];
11028     --nnb;
11029     if (nnb == 3) {
11030         goto L3;
11031     }
11032     lpf = lptr[lpl];
11033     nfrst = list[lpf];
11034     lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11035     if (nr == nfrst) {
11036         goto L2;
11037     }
11038 
11039 /*   NR is not the first neighbor of N1. */
11040 /*     Back up and test N1-NR for a swap again:  Set N2 to */
11041 /*     NR and NR to the previous neighbor of N1 -- the */
11042 /*     neighbor of NR which follows N1.  LP21 points to NL */
11043 /*     as a neighbor of NR. */
11044 
11045     n2 = nr;
11046     x2 = xr;
11047     y2 = yr;
11048     z2 = zr;
11049     lp21 = lptr[lp21];
11050     lp21 = lptr[lp21];
11051     nr = (i__1 = list[lp21], abs(i__1));
11052     xr = x[nr];
11053     yr = y[nr];
11054     zr = z__[nr];
11055     goto L1;
11056 
11057 /*   Bottom of loop -- test for termination of loop. */
11058 
11059 L2:
11060     if (n2 == nfrst) {
11061         goto L3;
11062     }
11063     n2 = nl;
11064     x2 = xl;
11065     y2 = yl;
11066     z2 = zl;
11067     lp = lptr[lp];
11068     goto L1;
11069 
11070 /* Delete N1 and all its incident arcs.  If N1 is an interior */
11071 /*   node and either NNB > 3 or NNB = 3 and N2 LEFT NR->NL, */
11072 /*   then N1 must be separated from its neighbors by a plane */
11073 /*   containing the origin -- its removal reverses the effect */
11074 /*   of a call to COVSPH, and all its neighbors become */
11075 /*   boundary nodes.  This is achieved by treating it as if */
11076 /*   it were a boundary node (setting BDRY to TRUE, changing */
11077 /*   a sign in LIST, and incrementing NNB). */
11078 
11079 L3:
11080     if (! bdry) {
11081         if (nnb > 3) {
11082             bdry = TRUE_;
11083         } else {
11084             lpf = lptr[lpl];
11085             nr = list[lpf];
11086             lp = lptr[lpf];
11087             n2 = list[lp];
11088             nl = list[lpl];
11089             bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11090                     x[n2], &y[n2], &z__[n2]);
11091         }
11092         if (bdry) {
11093 
11094 /*   IF a boundary node already exists, then N1 and its */
11095 /*     neighbors cannot be converted to boundary nodes. */
11096 /*     (They must be collinear.)  This is a problem if */
11097 /*     NNB > 3. */
11098 
11099             i__1 = nn;
11100             for (i__ = 1; i__ <= i__1; ++i__) {
11101                 if (list[lend[i__]] < 0) {
11102                     bdry = FALSE_;
11103                     goto L5;
11104                 }
11105 /* L4: */
11106             }
11107             list[lpl] = -list[lpl];
11108             ++nnb;
11109         }
11110     }
11111 L5:
11112     if (! bdry && nnb > 3) {
11113         goto L24;
11114     }
11115 
11116 /* Initialize for loop on neighbors.  LPL points to the last */
11117 /*   neighbor of N1.  LNEW is stored in local variable LNW. */
11118 
11119     lp = lpl;
11120     lnw = *lnew;
11121 
11122 /* Loop on neighbors N2 of N1, beginning with the first. */
11123 
11124 L6:
11125     lp = lptr[lp];
11126     n2 = (i__1 = list[lp], abs(i__1));
11127     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11128     if (lph < 0) {
11129         goto L23;
11130     }
11131 
11132 /*   LP and LPL may require alteration. */
11133 
11134     if (lpl == lnw) {
11135         lpl = lph;
11136     }
11137     if (lp == lnw) {
11138         lp = lph;
11139     }
11140     if (lp != lpl) {
11141         goto L6;
11142     }
11143 
11144 /* Delete N1 from X, Y, Z, and LEND, and remove its adjacency */
11145 /*   list from LIST and LPTR.  LIST entries (nodal indexes) */
11146 /*   which are larger than N1 must be decremented. */
11147 
11148     --nn;
11149     if (n1 > nn) {
11150         goto L9;
11151     }
11152     i__1 = nn;
11153     for (i__ = n1; i__ <= i__1; ++i__) {
11154         x[i__] = x[i__ + 1];
11155         y[i__] = y[i__ + 1];
11156         z__[i__] = z__[i__ + 1];
11157         lend[i__] = lend[i__ + 1];
11158 /* L7: */
11159     }
11160 
11161     i__1 = lnw - 1;
11162     for (i__ = 1; i__ <= i__1; ++i__) {
11163         if (list[i__] > n1) {
11164             --list[i__];
11165         }
11166         if (list[i__] < -n1) {
11167             ++list[i__];
11168         }
11169 /* L8: */
11170     }
11171 
11172 /*   For LPN = first to last neighbors of N1, delete the */
11173 /*     preceding neighbor (indexed by LP). */
11174 
11175 /*   Each empty LIST,LPTR location LP is filled in with the */
11176 /*     values at LNW-1, and LNW is decremented.  All pointers */
11177 /*     (including those in LPTR and LEND) with value LNW-1 */
11178 /*     must be changed to LP. */
11179 
11180 /*  LPL points to the last neighbor of N1. */
11181 
11182 L9:
11183     if (bdry) {
11184         --nnb;
11185     }
11186     lpn = lpl;
11187     i__1 = nnb;
11188     for (j = 1; j <= i__1; ++j) {
11189         --lnw;
11190         lp = lpn;
11191         lpn = lptr[lp];
11192         list[lp] = list[lnw];
11193         lptr[lp] = lptr[lnw];
11194         if (lptr[lpn] == lnw) {
11195             lptr[lpn] = lp;
11196         }
11197         if (lpn == lnw) {
11198             lpn = lp;
11199         }
11200         for (i__ = nn; i__ >= 1; --i__) {
11201             if (lend[i__] == lnw) {
11202                 lend[i__] = lp;
11203                 goto L11;
11204             }
11205 /* L10: */
11206         }
11207 
11208 L11:
11209         for (i__ = lnw - 1; i__ >= 1; --i__) {
11210             if (lptr[i__] == lnw) {
11211                 lptr[i__] = lp;
11212             }
11213 /* L12: */
11214         }
11215 /* L13: */
11216     }
11217 
11218 /* Update N and LNEW, and optimize the patch of triangles */
11219 /*   containing K (on input) by applying swaps to the arcs */
11220 /*   in IWK. */
11221 
11222     *n = nn;
11223     *lnew = lnw;
11224     if (iwl > 0) {
11225         nit = iwl << 2;
11226         optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11227                 nit, &iwk[3], &ierr);
11228         if (ierr != 0 && ierr != 1) {
11229             goto L25;
11230         }
11231         if (ierr == 1) {
11232             goto L26;
11233         }
11234     }
11235 
11236 /* Successful termination. */
11237 
11238     *ier = 0;
11239     return 0;
11240 
11241 /* Invalid input parameter. */
11242 
11243 L21:
11244     *ier = 1;
11245     return 0;
11246 
11247 /* Insufficient space reserved for IWK. */
11248 
11249 L22:
11250     *ier = 2;
11251     return 0;
11252 
11253 /* Invalid triangulation data structure.  NNB < 3 on input or */
11254 /*   N2 is a neighbor of N1 but N1 is not a neighbor of N2. */
11255 
11256 L23:
11257     *ier = 3;
11258     return 0;
11259 
11260 /* N1 is interior but NNB could not be reduced to 3. */
11261 
11262 L24:
11263     *ier = 4;
11264     return 0;
11265 
11266 /* Error flag (other than 1) returned by OPTIM. */
11267 
11268 L25:
11269     *ier = 5;
11270 /*      WRITE (*,100) NIT, IERR */
11271 /*  100 FORMAT (//5X,'*** Error in OPTIM (called from ', */
11272 /*     .        'DELNOD):  NIT = ',I4,', IER = ',I1,' ***'/) */
11273     return 0;
11274 
11275 /* Error flag 1 returned by OPTIM. */
11276 
11277 L26:
11278     *ier = 6;
11279     return 0;
11280 } /* delnod_ */
11281 
11282 /* Subroutine */ int drwarc_(int *, double *p, double *q,
11283         double *tol, int *nseg)
11284 {
11285     /* System generated locals */
11286     int i__1;
11287     double d__1;
11288 
11289     /* Builtin functions */
11290     //double sqrt(double);
11291 
11292     /* Local variables */
11293     static int i__, k;
11294     static double s, p1[3], p2[3], u1, u2, v1, v2;
11295     static int na;
11296     static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11297 
11298 
11299 /* *********************************************************** */
11300 
11301 /*                                              From STRIPACK */
11302 /*                                            Robert J. Renka */
11303 /*                                  Dept. of Computer Science */
11304 /*                                       Univ. of North Texas */
11305 /*                                           renka@cs.unt.edu */
11306 /*                                                   03/04/03 */
11307 
11308 /*   Given unit vectors P and Q corresponding to northern */
11309 /* hemisphere points (with positive third components), this */
11310 /* subroutine draws a polygonal line which approximates the */
11311 /* projection of arc P-Q onto the plane containing the */
11312 /* equator. */
11313 
11314 /*   The line segment is drawn by writing a sequence of */
11315 /* 'moveto' and 'lineto' Postscript commands to unit LUN.  It */
11316 /* is assumed that an open file is attached to the unit, */
11317 /* header comments have been written to the file, a window- */
11318 /* to-viewport mapping has been established, etc. */
11319 
11320 /* On input: */
11321 
11322 /*       LUN = long int unit number in the range 0 to 99. */
11323 
11324 /*       P,Q = Arrays of length 3 containing the endpoints of */
11325 /*             the arc to be drawn. */
11326 
11327 /*       TOL = Maximum distance in world coordinates between */
11328 /*             the projected arc and polygonal line. */
11329 
11330 /* Input parameters are not altered by this routine. */
11331 
11332 /* On output: */
11333 
11334 /*       NSEG = Number of line segments in the polygonal */
11335 /*              approximation to the projected arc.  This is */
11336 /*              a decreasing function of TOL.  NSEG = 0 and */
11337 /*              no drawing is performed if P = Q or P = -Q */
11338 /*              or an error is encountered in writing to unit */
11339 /*              LUN. */
11340 
11341 /* STRIPACK modules required by DRWARC:  None */
11342 
11343 /* Intrinsic functions called by DRWARC:  ABS, DBLE, SQRT */
11344 
11345 /* *********************************************************** */
11346 
11347 
11348 /* Local parameters: */
11349 
11350 /* DP =    (Q-P)/NSEG */
11351 /* DU,DV = Components of the projection Q'-P' of arc P->Q */
11352 /*           onto the projection plane */
11353 /* ENRM =  Euclidean norm (or squared norm) of Q'-P' or PM */
11354 /* ERR =   Orthogonal distance from the projected midpoint */
11355 /*           PM' to the line defined by P' and Q': */
11356 /*           |Q'-P' X PM'-P'|/|Q'-P'| */
11357 /* I,K =   DO-loop indexes */
11358 /* NA =    Number of arcs (segments) in the partition of P-Q */
11359 /* P1,P2 = Pairs of adjacent points in a uniform partition of */
11360 /*           arc P-Q into NSEG segments; obtained by normal- */
11361 /*           izing PM values */
11362 /* PM =    Midpoint of arc P-Q or a point P + k*DP in a */
11363 /*           uniform partition of the line segment P-Q into */
11364 /*           NSEG segments */
11365 /* S =     Scale factor 1/NA */
11366 /* U1,V1 = Components of P' */
11367 /* U2,V2 = Components of Q' */
11368 /* UM,VM = Components of the midpoint PM' */
11369 
11370 
11371 /* Compute the midpoint PM of arc P-Q. */
11372 
11373     /* Parameter adjustments */
11374     --q;
11375     --p;
11376 
11377     /* Function Body */
11378     enrm = 0.;
11379     for (i__ = 1; i__ <= 3; ++i__) {
11380         pm[i__ - 1] = p[i__] + q[i__];
11381         enrm += pm[i__ - 1] * pm[i__ - 1];
11382 /* L1: */
11383     }
11384     if (enrm == 0.) {
11385         goto L5;
11386     }
11387     enrm = sqrt(enrm);
11388     pm[0] /= enrm;
11389     pm[1] /= enrm;
11390     pm[2] /= enrm;
11391 
11392 /* Project P, Q, and PM to P' = (U1,V1), Q' = (U2,V2), and */
11393 /*   PM' = (UM,VM), respectively. */
11394 
11395     u1 = p[1];
11396     v1 = p[2];
11397     u2 = q[1];
11398     v2 = q[2];
11399     um = pm[0];
11400     vm = pm[1];
11401 
11402 /* Compute the orthogonal distance ERR from PM' to the line */
11403 /*   defined by P' and Q'.  This is the maximum deviation */
11404 /*   between the projected arc and the line segment.  It is */
11405 /*   undefined if P' = Q'. */
11406 
11407     du = u2 - u1;
11408     dv = v2 - v1;
11409     enrm = du * du + dv * dv;
11410     if (enrm == 0.) {
11411         goto L5;
11412     }
11413     err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11414 
11415 /* Compute the number of arcs into which P-Q will be parti- */
11416 /*   tioned (the number of line segments to be drawn): */
11417 /*   NA = ERR/TOL. */
11418 
11419     na = (int) (err / *tol + 1.);
11420 
11421 /* Initialize for loop on arcs P1-P2, where the intermediate */
11422 /*   points are obtained by normalizing PM = P + k*DP for */
11423 /*   DP = (Q-P)/NA */
11424 
11425     s = 1. / (double) na;
11426     for (i__ = 1; i__ <= 3; ++i__) {
11427         dp[i__ - 1] = s * (q[i__] - p[i__]);
11428         pm[i__ - 1] = p[i__];
11429         p1[i__ - 1] = p[i__];
11430 /* L2: */
11431     }
11432 
11433 /* Loop on arcs P1-P2, drawing the line segments associated */
11434 /*   with the projected endpoints. */
11435 
11436     i__1 = na - 1;
11437     for (k = 1; k <= i__1; ++k) {
11438         enrm = 0.;
11439         for (i__ = 1; i__ <= 3; ++i__) {
11440             pm[i__ - 1] += dp[i__ - 1];
11441             enrm += pm[i__ - 1] * pm[i__ - 1];
11442 /* L3: */
11443         }
11444         if (enrm == 0.) {
11445             goto L5;
11446         }
11447         enrm = sqrt(enrm);
11448         p2[0] = pm[0] / enrm;
11449         p2[1] = pm[1] / enrm;
11450         p2[2] = pm[2] / enrm;
11451 /*        WRITE (LUN,100,ERR=5) P1(1), P1(2), P2(1), P2(2) */
11452 /*  100   FORMAT (2F12.6,' moveto',2F12.6,' lineto') */
11453         p1[0] = p2[0];
11454         p1[1] = p2[1];
11455         p1[2] = p2[2];
11456 /* L4: */
11457     }
11458 /*      WRITE (LUN,100,ERR=5) P1(1), P1(2), Q(1), Q(2) */
11459 
11460 /* No error encountered. */
11461 
11462     *nseg = na;
11463     return 0;
11464 
11465 /* Invalid input value of P or Q. */
11466 
11467 L5:
11468     *nseg = 0;
11469     return 0;
11470 } /* drwarc_ */
11471 
11472 /* Subroutine */ int edge_(int *in1, int *in2, double *x,
11473         double *y, double *z__, int *lwk, int *iwk, int *
11474         list, int *lptr, int *lend, int *ier)
11475 {
11476     /* System generated locals */
11477     int i__1;
11478 
11479     /* Local variables */
11480     static int i__, n0, n1, n2;
11481     static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11482     static int nl, lp, nr;
11483     static double dp12;
11484     static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11485     static double dp1l, dp2l, dp1r, dp2r;
11486     extern long int left_(double *, double *, double *, double
11487             *, double *, double *, double *, double *,
11488             double *);
11489     static int ierr;
11490     extern /* Subroutine */ int swap_(int *, int *, int *,
11491             int *, int *, int *, int *, int *);
11492     static int next, iwcp1, n1lst, iwend;
11493     extern /* Subroutine */ int optim_(double *, double *, double
11494             *, int *, int *, int *, int *, int *, int
11495             *, int *);
11496     static int n1frst;
11497 
11498 
11499 /* *********************************************************** */
11500 
11501 /*                                              From STRIPACK */
11502 /*                                            Robert J. Renka */
11503 /*                                  Dept. of Computer Science */
11504 /*                                       Univ. of North Texas */
11505 /*                                           renka@cs.unt.edu */
11506 /*                                                   07/30/98 */
11507 
11508 /*   Given a triangulation of N nodes and a pair of nodal */
11509 /* indexes IN1 and IN2, this routine swaps arcs as necessary */
11510 /* to force IN1 and IN2 to be adjacent.  Only arcs which */
11511 /* intersect IN1-IN2 are swapped out.  If a Delaunay triangu- */
11512 /* lation is input, the resulting triangulation is as close */
11513 /* as possible to a Delaunay triangulation in the sense that */
11514 /* all arcs other than IN1-IN2 are locally optimal. */
11515 
11516 /*   A sequence of calls to EDGE may be used to force the */
11517 /* presence of a set of edges defining the boundary of a non- */
11518 /* convex and/or multiply connected region, or to introduce */
11519 /* barriers into the triangulation.  Note that Subroutine */
11520 /* GETNP will not necessarily return closest nodes if the */
11521 /* triangulation has been constrained by a call to EDGE. */
11522 /* However, this is appropriate in some applications, such */
11523 /* as triangle-based interpolation on a nonconvex domain. */
11524 
11525 
11526 /* On input: */
11527 
11528 /*       IN1,IN2 = Indexes (of X, Y, and Z) in the range 1 to */
11529 /*                 N defining a pair of nodes to be connected */
11530 /*                 by an arc. */
11531 
11532 /*       X,Y,Z = Arrays of length N containing the Cartesian */
11533 /*               coordinates of the nodes. */
11534 
11535 /* The above parameters are not altered by this routine. */
11536 
11537 /*       LWK = Number of columns reserved for IWK.  This must */
11538 /*             be at least NI -- the number of arcs that */
11539 /*             intersect IN1-IN2.  (NI is bounded by N-3.) */
11540 
11541 /*       IWK = int work array of length at least 2*LWK. */
11542 
11543 /*       LIST,LPTR,LEND = Data structure defining the trian- */
11544 /*                        gulation.  Refer to Subroutine */
11545 /*                        TRMESH. */
11546 
11547 /* On output: */
11548 
11549 /*       LWK = Number of arcs which intersect IN1-IN2 (but */
11550 /*             not more than the input value of LWK) unless */
11551 /*             IER = 1 or IER = 3.  LWK = 0 if and only if */
11552 /*             IN1 and IN2 were adjacent (or LWK=0) on input. */
11553 
11554 /*       IWK = Array containing the indexes of the endpoints */
11555 /*             of the new arcs other than IN1-IN2 unless */
11556 /*             IER > 0 or LWK = 0.  New arcs to the left of */
11557 /*             IN1->IN2 are stored in the first K-1 columns */
11558 /*             (left portion of IWK), column K contains */
11559 /*             zeros, and new arcs to the right of IN1->IN2 */
11560 /*             occupy columns K+1,...,LWK.  (K can be deter- */
11561 /*             mined by searching IWK for the zeros.) */
11562 
11563 /*       LIST,LPTR,LEND = Data structure updated if necessary */
11564 /*                        to reflect the presence of an arc */
11565 /*                        connecting IN1 and IN2 unless IER > */
11566 /*                        0.  The data structure has been */
11567 /*                        altered if IER >= 4. */
11568 
11569 /*       IER = Error indicator: */
11570 /*             IER = 0 if no errors were encountered. */
11571 /*             IER = 1 if IN1 < 1, IN2 < 1, IN1 = IN2, */
11572 /*                     or LWK < 0 on input. */
11573 /*             IER = 2 if more space is required in IWK. */
11574 /*                     Refer to LWK. */
11575 /*             IER = 3 if IN1 and IN2 could not be connected */
11576 /*                     due to either an invalid data struc- */
11577 /*                     ture or collinear nodes (and floating */
11578 /*                     point error). */
11579 /*             IER = 4 if an error flag other than IER = 1 */
11580 /*                     was returned by OPTIM. */
11581 /*             IER = 5 if error flag 1 was returned by OPTIM. */
11582 /*                     This is not necessarily an error, but */
11583 /*                     the arcs other than IN1-IN2 may not */
11584 /*                     be optimal. */
11585 
11586 /*   An error message is written to the standard output unit */
11587 /* in the case of IER = 3 or IER = 4. */
11588 
11589 /* Modules required by EDGE:  LEFT, LSTPTR, OPTIM, SWAP, */
11590 /*                              SWPTST */
11591 
11592 /* Intrinsic function called by EDGE:  ABS */
11593 
11594 /* *********************************************************** */
11595 
11596 
11597 /* Local parameters: */
11598 
11599 /* DPij =     Dot product <Ni,Nj> */
11600 /* I =        DO-loop index and column index for IWK */
11601 /* IERR =     Error flag returned by Subroutine OPTIM */
11602 /* IWC =      IWK index between IWF and IWL -- NL->NR is */
11603 /*              stored in IWK(1,IWC)->IWK(2,IWC) */
11604 /* IWCP1 =    IWC + 1 */
11605 /* IWEND =    Input or output value of LWK */
11606 /* IWF =      IWK (column) index of the first (leftmost) arc */
11607 /*              which intersects IN1->IN2 */
11608 /* IWL =      IWK (column) index of the last (rightmost) are */
11609 /*              which intersects IN1->IN2 */
11610 /* LFT =      Flag used to determine if a swap results in the */
11611 /*              new arc intersecting IN1-IN2 -- LFT = 0 iff */
11612 /*              N0 = IN1, LFT = -1 implies N0 LEFT IN1->IN2, */
11613 /*              and LFT = 1 implies N0 LEFT IN2->IN1 */
11614 /* LP =       List pointer (index for LIST and LPTR) */
11615 /* LP21 =     Unused parameter returned by SWAP */
11616 /* LPL =      Pointer to the last neighbor of IN1 or NL */
11617 /* N0 =       Neighbor of N1 or node opposite NR->NL */
11618 /* N1,N2 =    Local copies of IN1 and IN2 */
11619 /* N1FRST =   First neighbor of IN1 */
11620 /* N1LST =    (Signed) last neighbor of IN1 */
11621 /* NEXT =     Node opposite NL->NR */
11622 /* NIT =      Flag or number of iterations employed by OPTIM */
11623 /* NL,NR =    Endpoints of an arc which intersects IN1-IN2 */
11624 /*              with NL LEFT IN1->IN2 */
11625 /* X0,Y0,Z0 = Coordinates of N0 */
11626 /* X1,Y1,Z1 = Coordinates of IN1 */
11627 /* X2,Y2,Z2 = Coordinates of IN2 */
11628 
11629 
11630 /* Store IN1, IN2, and LWK in local variables and test for */
11631 /*   errors. */
11632 
11633     /* Parameter adjustments */
11634     --lend;
11635     --lptr;
11636     --list;
11637     iwk -= 3;
11638     --z__;
11639     --y;
11640     --x;
11641 
11642     /* Function Body */
11643     n1 = *in1;
11644     n2 = *in2;
11645     iwend = *lwk;
11646     if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11647         goto L31;
11648     }
11649 
11650 /* Test for N2 as a neighbor of N1.  LPL points to the last */
11651 /*   neighbor of N1. */
11652 
11653     lpl = lend[n1];
11654     n0 = (i__1 = list[lpl], abs(i__1));
11655     lp = lpl;
11656 L1:
11657     if (n0 == n2) {
11658         goto L30;
11659     }
11660     lp = lptr[lp];
11661     n0 = list[lp];
11662     if (lp != lpl) {
11663         goto L1;
11664     }
11665 
11666 /* Initialize parameters. */
11667 
11668     iwl = 0;
11669     nit = 0;
11670 
11671 /* Store the coordinates of N1 and N2. */
11672 
11673 L2:
11674     x1 = x[n1];
11675     y1 = y[n1];
11676     z1 = z__[n1];
11677     x2 = x[n2];
11678     y2 = y[n2];
11679     z2 = z__[n2];
11680 
11681 /* Set NR and NL to adjacent neighbors of N1 such that */
11682 /*   NR LEFT N2->N1 and NL LEFT N1->N2, */
11683 /*   (NR Forward N1->N2 or NL Forward N1->N2), and */
11684 /*   (NR Forward N2->N1 or NL Forward N2->N1). */
11685 
11686 /*   Initialization:  Set N1FRST and N1LST to the first and */
11687 /*     (signed) last neighbors of N1, respectively, and */
11688 /*     initialize NL to N1FRST. */
11689 
11690     lpl = lend[n1];
11691     n1lst = list[lpl];
11692     lp = lptr[lpl];
11693     n1frst = list[lp];
11694     nl = n1frst;
11695     if (n1lst < 0) {
11696         goto L4;
11697     }
11698 
11699 /*   N1 is an interior node.  Set NL to the first candidate */
11700 /*     for NR (NL LEFT N2->N1). */
11701 
11702 L3:
11703     if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11704         goto L4;
11705     }
11706     lp = lptr[lp];
11707     nl = list[lp];
11708     if (nl != n1frst) {
11709         goto L3;
11710     }
11711 
11712 /*   All neighbors of N1 are strictly left of N1->N2. */
11713 
11714     goto L5;
11715 
11716 /*   NL = LIST(LP) LEFT N2->N1.  Set NR to NL and NL to the */
11717 /*     following neighbor of N1. */
11718 
11719 L4:
11720     nr = nl;
11721     lp = lptr[lp];
11722     nl = (i__1 = list[lp], abs(i__1));
11723     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11724 
11725 /*   NL LEFT N1->N2 and NR LEFT N2->N1.  The Forward tests */
11726 /*     are employed to avoid an error associated with */
11727 /*     collinear nodes. */
11728 
11729         dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11730         dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11731         dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11732         dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11733         dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11734         if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11735                 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11736             goto L6;
11737         }
11738 
11739 /*   NL-NR does not intersect N1-N2.  However, there is */
11740 /*     another candidate for the first arc if NL lies on */
11741 /*     the line N1-N2. */
11742 
11743         if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11744             goto L5;
11745         }
11746     }
11747 
11748 /*   Bottom of loop. */
11749 
11750     if (nl != n1frst) {
11751         goto L4;
11752     }
11753 
11754 /* Either the triangulation is invalid or N1-N2 lies on the */
11755 /*   convex hull boundary and an edge NR->NL (opposite N1 and */
11756 /*   intersecting N1-N2) was not found due to floating point */
11757 /*   error.  Try interchanging N1 and N2 -- NIT > 0 iff this */
11758 /*   has already been done. */
11759 
11760 L5:
11761     if (nit > 0) {
11762         goto L33;
11763     }
11764     nit = 1;
11765     n1 = n2;
11766     n2 = *in1;
11767     goto L2;
11768 
11769 /* Store the ordered sequence of intersecting edges NL->NR in */
11770 /*   IWK(1,IWL)->IWK(2,IWL). */
11771 
11772 L6:
11773     ++iwl;
11774     if (iwl > iwend) {
11775         goto L32;
11776     }
11777     iwk[(iwl << 1) + 1] = nl;
11778     iwk[(iwl << 1) + 2] = nr;
11779 
11780 /*   Set NEXT to the neighbor of NL which follows NR. */
11781 
11782     lpl = lend[nl];
11783     lp = lptr[lpl];
11784 
11785 /*   Find NR as a neighbor of NL.  The search begins with */
11786 /*     the first neighbor. */
11787 
11788 L7:
11789     if (list[lp] == nr) {
11790         goto L8;
11791     }
11792     lp = lptr[lp];
11793     if (lp != lpl) {
11794         goto L7;
11795     }
11796 
11797 /*   NR must be the last neighbor, and NL->NR cannot be a */
11798 /*     boundary edge. */
11799 
11800     if (list[lp] != nr) {
11801         goto L33;
11802     }
11803 
11804 /*   Set NEXT to the neighbor following NR, and test for */
11805 /*     termination of the store loop. */
11806 
11807 L8:
11808     lp = lptr[lp];
11809     next = (i__1 = list[lp], abs(i__1));
11810     if (next == n2) {
11811         goto L9;
11812     }
11813 
11814 /*   Set NL or NR to NEXT. */
11815 
11816     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11817         nl = next;
11818     } else {
11819         nr = next;
11820     }
11821     goto L6;
11822 
11823 /* IWL is the number of arcs which intersect N1-N2. */
11824 /*   Store LWK. */
11825 
11826 L9:
11827     *lwk = iwl;
11828     iwend = iwl;
11829 
11830 /* Initialize for edge swapping loop -- all possible swaps */
11831 /*   are applied (even if the new arc again intersects */
11832 /*   N1-N2), arcs to the left of N1->N2 are stored in the */
11833 /*   left portion of IWK, and arcs to the right are stored in */
11834 /*   the right portion.  IWF and IWL index the first and last */
11835 /*   intersecting arcs. */
11836 
11837     iwf = 1;
11838 
11839 /* Top of loop -- set N0 to N1 and NL->NR to the first edge. */
11840 /*   IWC points to the arc currently being processed.  LFT */
11841 /*   .LE. 0 iff N0 LEFT N1->N2. */
11842 
11843 L10:
11844     lft = 0;
11845     n0 = n1;
11846     x0 = x1;
11847     y0 = y1;
11848     z0 = z1;
11849     nl = iwk[(iwf << 1) + 1];
11850     nr = iwk[(iwf << 1) + 2];
11851     iwc = iwf;
11852 
11853 /*   Set NEXT to the node opposite NL->NR unless IWC is the */
11854 /*     last arc. */
11855 
11856 L11:
11857     if (iwc == iwl) {
11858         goto L21;
11859     }
11860     iwcp1 = iwc + 1;
11861     next = iwk[(iwcp1 << 1) + 1];
11862     if (next != nl) {
11863         goto L16;
11864     }
11865     next = iwk[(iwcp1 << 1) + 2];
11866 
11867 /*   NEXT RIGHT N1->N2 and IWC .LT. IWL.  Test for a possible */
11868 /*     swap. */
11869 
11870     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11871             z__[next])) {
11872         goto L14;
11873     }
11874     if (lft >= 0) {
11875         goto L12;
11876     }
11877     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11878             z__[next])) {
11879         goto L14;
11880     }
11881 
11882 /*   Replace NL->NR with N0->NEXT. */
11883 
11884     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11885     iwk[(iwc << 1) + 1] = n0;
11886     iwk[(iwc << 1) + 2] = next;
11887     goto L15;
11888 
11889 /*   Swap NL-NR for N0-NEXT, shift columns IWC+1,...,IWL to */
11890 /*     the left, and store N0-NEXT in the right portion of */
11891 /*     IWK. */
11892 
11893 L12:
11894     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11895     i__1 = iwl;
11896     for (i__ = iwcp1; i__ <= i__1; ++i__) {
11897         iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11898         iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11899 /* L13: */
11900     }
11901     iwk[(iwl << 1) + 1] = n0;
11902     iwk[(iwl << 1) + 2] = next;
11903     --iwl;
11904     nr = next;
11905     goto L11;
11906 
11907 /*   A swap is not possible.  Set N0 to NR. */
11908 
11909 L14:
11910     n0 = nr;
11911     x0 = x[n0];
11912     y0 = y[n0];
11913     z0 = z__[n0];
11914     lft = 1;
11915 
11916 /*   Advance to the next arc. */
11917 
11918 L15:
11919     nr = next;
11920     ++iwc;
11921     goto L11;
11922 
11923 /*   NEXT LEFT N1->N2, NEXT .NE. N2, and IWC .LT. IWL. */
11924 /*     Test for a possible swap. */
11925 
11926 L16:
11927     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11928             z__[next])) {
11929         goto L19;
11930     }
11931     if (lft <= 0) {
11932         goto L17;
11933     }
11934     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11935             z__[next])) {
11936         goto L19;
11937     }
11938 
11939 /*   Replace NL->NR with NEXT->N0. */
11940 
11941     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11942     iwk[(iwc << 1) + 1] = next;
11943     iwk[(iwc << 1) + 2] = n0;
11944     goto L20;
11945 
11946 /*   Swap NL-NR for N0-NEXT, shift columns IWF,...,IWC-1 to */
11947 /*     the right, and store N0-NEXT in the left portion of */
11948 /*     IWK. */
11949 
11950 L17:
11951     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11952     i__1 = iwf;
11953     for (i__ = iwc - 1; i__ >= i__1; --i__) {
11954         iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11955         iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11956 /* L18: */
11957     }
11958     iwk[(iwf << 1) + 1] = n0;
11959     iwk[(iwf << 1) + 2] = next;
11960     ++iwf;
11961     goto L20;
11962 
11963 /*   A swap is not possible.  Set N0 to NL. */
11964 
11965 L19:
11966     n0 = nl;
11967     x0 = x[n0];
11968     y0 = y[n0];
11969     z0 = z__[n0];
11970     lft = -1;
11971 
11972 /*   Advance to the next arc. */
11973 
11974 L20:
11975     nl = next;
11976     ++iwc;
11977     goto L11;
11978 
11979 /*   N2 is opposite NL->NR (IWC = IWL). */
11980 
11981 L21:
11982     if (n0 == n1) {
11983         goto L24;
11984     }
11985     if (lft < 0) {
11986         goto L22;
11987     }
11988 
11989 /*   N0 RIGHT N1->N2.  Test for a possible swap. */
11990 
11991     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
11992         goto L10;
11993     }
11994 
11995 /*   Swap NL-NR for N0-N2 and store N0-N2 in the right */
11996 /*     portion of IWK. */
11997 
11998     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11999     iwk[(iwl << 1) + 1] = n0;
12000     iwk[(iwl << 1) + 2] = n2;
12001     --iwl;
12002     goto L10;
12003 
12004 /*   N0 LEFT N1->N2.  Test for a possible swap. */
12005 
12006 L22:
12007     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12008         goto L10;
12009     }
12010 
12011 /*   Swap NL-NR for N0-N2, shift columns IWF,...,IWL-1 to the */
12012 /*     right, and store N0-N2 in the left portion of IWK. */
12013 
12014     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12015     i__ = iwl;
12016 L23:
12017     iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12018     iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12019     --i__;
12020     if (i__ > iwf) {
12021         goto L23;
12022     }
12023     iwk[(iwf << 1) + 1] = n0;
12024     iwk[(iwf << 1) + 2] = n2;
12025     ++iwf;
12026     goto L10;
12027 
12028 /* IWF = IWC = IWL.  Swap out the last arc for N1-N2 and */
12029 /*   store zeros in IWK. */
12030 
12031 L24:
12032     swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12033     iwk[(iwc << 1) + 1] = 0;
12034     iwk[(iwc << 1) + 2] = 0;
12035 
12036 /* Optimization procedure -- */
12037 
12038     *ier = 0;
12039     if (iwc > 1) {
12040 
12041 /*   Optimize the set of new arcs to the left of IN1->IN2. */
12042 
12043         nit = iwc - (1<<2);
12044         i__1 = iwc - 1;
12045         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12046                 nit, &iwk[3], &ierr);
12047         if (ierr != 0 && ierr != 1) {
12048             goto L34;
12049         }
12050         if (ierr == 1) {
12051             *ier = 5;
12052         }
12053     }
12054     if (iwc < iwend) {
12055 
12056 /*   Optimize the set of new arcs to the right of IN1->IN2. */
12057 
12058         nit = iwend - (iwc<<2);
12059         i__1 = iwend - iwc;
12060         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12061                 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12062         if (ierr != 0 && ierr != 1) {
12063             goto L34;
12064         }
12065         if (ierr == 1) {
12066             goto L35;
12067         }
12068     }
12069     if (*ier == 5) {
12070         goto L35;
12071     }
12072 
12073 /* Successful termination (IER = 0). */
12074 
12075     return 0;
12076 
12077 /* IN1 and IN2 were adjacent on input. */
12078 
12079 L30:
12080     *ier = 0;
12081     return 0;
12082 
12083 /* Invalid input parameter. */
12084 
12085 L31:
12086     *ier = 1;
12087     return 0;
12088 
12089 /* Insufficient space reserved for IWK. */
12090 
12091 L32:
12092     *ier = 2;
12093     return 0;
12094 
12095 /* Invalid triangulation data structure or collinear nodes */
12096 /*   on convex hull boundary. */
12097 
12098 L33:
12099     *ier = 3;
12100 /*      WRITE (*,130) IN1, IN2 */
12101 /*  130 FORMAT (//5X,'*** Error in EDGE:  Invalid triangula', */
12102 /*     .        'tion or null triangles on boundary'/ */
12103 /*     .        9X,'IN1 =',I4,', IN2=',I4/) */
12104     return 0;
12105 
12106 /* Error flag (other than 1) returned by OPTIM. */
12107 
12108 L34:
12109     *ier = 4;
12110 /*      WRITE (*,140) NIT, IERR */
12111 /*  140 FORMAT (//5X,'*** Error in OPTIM (called from EDGE):', */
12112 /*     .        '  NIT = ',I4,', IER = ',I1,' ***'/) */
12113     return 0;
12114 
12115 /* Error flag 1 returned by OPTIM. */
12116 
12117 L35:
12118     *ier = 5;
12119     return 0;
12120 } /* edge_ */
12121 
12122 /* Subroutine */ int getnp_(double *x, double *y, double *z__,
12123         int *list, int *lptr, int *lend, int *l, int *
12124         npts, double *df, int *ier)
12125 {
12126     /* System generated locals */
12127     int i__1, i__2;
12128 
12129     /* Local variables */
12130     static int i__, n1;
12131     static double x1, y1, z1;
12132     static int nb, ni, lp, np, lm1;
12133     static double dnb, dnp;
12134     static int lpl;
12135 
12136 
12137 /* *********************************************************** */
12138 
12139 /*                                              From STRIPACK */
12140 /*                                            Robert J. Renka */
12141 /*                                  Dept. of Computer Science */
12142 /*                                       Univ. of North Texas */
12143 /*                                           renka@cs.unt.edu */
12144 /*                                                   07/28/98 */
12145 
12146 /*   Given a Delaunay triangulation of N nodes on the unit */
12147 /* sphere and an array NPTS containing the indexes of L-1 */
12148 /* nodes ordered by angular distance from NPTS(1), this sub- */
12149 /* routine sets NPTS(L) to the index of the next node in the */
12150 /* sequence -- the node, other than NPTS(1),...,NPTS(L-1), */
12151 /* that is closest to NPTS(1).  Thus, the ordered sequence */
12152 /* of K closest nodes to N1 (including N1) may be determined */
12153 /* by K-1 calls to GETNP with NPTS(1) = N1 and L = 2,3,...,K */
12154 /* for K .GE. 2. */
12155 
12156 /*   The algorithm uses the property of a Delaunay triangula- */
12157 /* tion that the K-th closest node to N1 is a neighbor of one */
12158 /* of the K-1 closest nodes to N1. */
12159 
12160 
12161 /* On input: */
12162 
12163 /*       X,Y,Z = Arrays of length N containing the Cartesian */
12164 /*               coordinates of the nodes. */
12165 
12166 /*       LIST,LPTR,LEND = Triangulation data structure.  Re- */
12167 /*                        fer to Subroutine TRMESH. */
12168 
12169 /*       L = Number of nodes in the sequence on output.  2 */
12170 /*           .LE. L .LE. N. */
12171 
12172 /* The above parameters are not altered by this routine. */
12173 
12174 /*       NPTS = Array of length .GE. L containing the indexes */
12175 /*              of the L-1 closest nodes to NPTS(1) in the */
12176 /*              first L-1 locations. */
12177 
12178 /* On output: */
12179 
12180 /*       NPTS = Array updated with the index of the L-th */
12181 /*              closest node to NPTS(1) in position L unless */
12182 /*              IER = 1. */
12183 
12184 /*       DF = Value of an increasing function (negative cos- */
12185 /*            ine) of the angular distance between NPTS(1) */
12186 /*            and NPTS(L) unless IER = 1. */
12187 
12188 /*       IER = Error indicator: */
12189 /*             IER = 0 if no errors were encountered. */
12190 /*             IER = 1 if L < 2. */
12191 
12192 /* Modules required by GETNP:  None */
12193 
12194 /* Intrinsic function called by GETNP:  ABS */
12195 
12196 /* *********************************************************** */
12197 
12198 
12199 /* Local parameters: */
12200 
12201 /* DNB,DNP =  Negative cosines of the angular distances from */
12202 /*              N1 to NB and to NP, respectively */
12203 /* I =        NPTS index and DO-loop index */
12204 /* LM1 =      L-1 */
12205 /* LP =       LIST pointer of a neighbor of NI */
12206 /* LPL =      Pointer to the last neighbor of NI */
12207 /* N1 =       NPTS(1) */
12208 /* NB =       Neighbor of NI and candidate for NP */
12209 /* NI =       NPTS(I) */
12210 /* NP =       Candidate for NPTS(L) */
12211 /* X1,Y1,Z1 = Coordinates of N1 */
12212 
12213     /* Parameter adjustments */
12214     --x;
12215     --y;
12216     --z__;
12217     --list;
12218     --lptr;
12219     --lend;
12220     --npts;
12221 
12222     /* Function Body */
12223     lm1 = *l - 1;
12224     if (lm1 < 1) {
12225         goto L6;
12226     }
12227     *ier = 0;
12228 
12229 /* Store N1 = NPTS(1) and mark the elements of NPTS. */
12230 
12231     n1 = npts[1];
12232     x1 = x[n1];
12233     y1 = y[n1];
12234     z1 = z__[n1];
12235     i__1 = lm1;
12236     for (i__ = 1; i__ <= i__1; ++i__) {
12237         ni = npts[i__];
12238         lend[ni] = -lend[ni];
12239 /* L1: */
12240     }
12241 
12242 /* Candidates for NP = NPTS(L) are the unmarked neighbors */
12243 /*   of nodes in NPTS.  DNP is initially greater than -cos(PI) */
12244 /*   (the maximum distance). */
12245 
12246     dnp = 2.;
12247 
12248 /* Loop on nodes NI in NPTS. */
12249 
12250     i__1 = lm1;
12251     for (i__ = 1; i__ <= i__1; ++i__) {
12252         ni = npts[i__];
12253         lpl = -lend[ni];
12254         lp = lpl;
12255 
12256 /* Loop on neighbors NB of NI. */
12257 
12258 L2:
12259         nb = (i__2 = list[lp], abs(i__2));
12260         if (lend[nb] < 0) {
12261             goto L3;
12262         }
12263 
12264 /* NB is an unmarked neighbor of NI.  Replace NP if NB is */
12265 /*   closer to N1. */
12266 
12267         dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12268         if (dnb >= dnp) {
12269             goto L3;
12270         }
12271         np = nb;
12272         dnp = dnb;
12273 L3:
12274         lp = lptr[lp];
12275         if (lp != lpl) {
12276             goto L2;
12277         }
12278 /* L4: */
12279     }
12280     npts[*l] = np;
12281     *df = dnp;
12282 
12283 /* Unmark the elements of NPTS. */
12284 
12285     i__1 = lm1;
12286     for (i__ = 1; i__ <= i__1; ++i__) {
12287         ni = npts[i__];
12288         lend[ni] = -lend[ni];
12289 /* L5: */
12290     }
12291     return 0;
12292 
12293 /* L is outside its valid range. */
12294 
12295 L6:
12296     *ier = 1;
12297     return 0;
12298 } /* getnp_ */
12299 
12300 /* Subroutine */ int insert_(int *k, int *lp, int *list, int *
12301         lptr, int *lnew)
12302 {
12303     static int lsav;
12304 
12305 
12306 /* *********************************************************** */
12307 
12308 /*                                              From STRIPACK */
12309 /*                                            Robert J. Renka */
12310 /*                                  Dept. of Computer Science */
12311 /*                                       Univ. of North Texas */
12312 /*                                           renka@cs.unt.edu */
12313 /*                                                   07/17/96 */
12314 
12315 /*   This subroutine inserts K as a neighbor of N1 following */
12316 /* N2, where LP is the LIST pointer of N2 as a neighbor of */
12317 /* N1.  Note that, if N2 is the last neighbor of N1, K will */
12318 /* become the first neighbor (even if N1 is a boundary node). */
12319 
12320 /*   This routine is identical to the similarly named routine */
12321 /* in TRIPACK. */
12322 
12323 
12324 /* On input: */
12325 
12326 /*       K = Index of the node to be inserted. */
12327 
12328 /*       LP = LIST pointer of N2 as a neighbor of N1. */
12329 
12330 /* The above parameters are not altered by this routine. */
12331 
12332 /*       LIST,LPTR,LNEW = Data structure defining the trian- */
12333 /*                        gulation.  Refer to Subroutine */
12334 /*                        TRMESH. */
12335 
12336 /* On output: */
12337 
12338 /*       LIST,LPTR,LNEW = Data structure updated with the */
12339 /*                        addition of node K. */
12340 
12341 /* Modules required by INSERT:  None */
12342 
12343 /* *********************************************************** */
12344 
12345 
12346     /* Parameter adjustments */
12347     --lptr;
12348     --list;
12349 
12350     /* Function Body */
12351     lsav = lptr[*lp];
12352     lptr[*lp] = *lnew;
12353     list[*lnew] = *k;
12354     lptr[*lnew] = lsav;
12355     ++(*lnew);
12356     return 0;
12357 } /* insert_ */
12358 
12359 long int inside_(double *p, int *lv, double *xv, double *yv,
12360         double *zv, int *nv, int *listv, int *ier)
12361 {
12362     /* Initialized data */
12363 
12364     static double eps = .001;
12365 
12366     /* System generated locals */
12367     int i__1;
12368     long int ret_val = 0;
12369 
12370     /* Builtin functions */
12371     //double sqrt(double);
12372 
12373     /* Local variables */
12374     static double b[3], d__;
12375     static int k, n;
12376     static double q[3];
12377     static int i1, i2, k0;
12378     static double v1[3], v2[3], cn[3], bp, bq;
12379     static int ni;
12380     static double pn[3], qn[3], vn[3];
12381     static int imx;
12382     static long int lft1, lft2, even;
12383     static int ierr;
12384     static long int pinr, qinr;
12385     static double qnrm, vnrm;
12386     extern /* Subroutine */ int intrsc_(double *, double *,
12387             double *, double *, int *);
12388 
12389 
12390 /* *********************************************************** */
12391 
12392 /*                                              From STRIPACK */
12393 /*                                            Robert J. Renka */
12394 /*                                  Dept. of Computer Science */
12395 /*                                       Univ. of North Texas */
12396 /*                                           renka@cs.unt.edu */
12397 /*                                                   12/27/93 */
12398 
12399 /*   This function locates a point P relative to a polygonal */
12400 /* region R on the surface of the unit sphere, returning */
12401 /* INSIDE = TRUE if and only if P is contained in R.  R is */
12402 /* defined by a cyclically ordered sequence of vertices which */
12403 /* form a positively-oriented simple closed curve.  Adjacent */
12404 /* vertices need not be distinct but the curve must not be */
12405 /* self-intersecting.  Also, while polygon edges are by defi- */
12406 /* nition restricted to a single hemisphere, R is not so */
12407 /* restricted.  Its interior is the region to the left as the */
12408 /* vertices are traversed in order. */
12409 
12410 /*   The algorithm consists of selecting a point Q in R and */
12411 /* then finding all points at which the great circle defined */
12412 /* by P and Q intersects the boundary of R.  P lies inside R */
12413 /* if and only if there is an even number of intersection */
12414 /* points between Q and P.  Q is taken to be a point immedi- */
12415 /* ately to the left of a directed boundary edge -- the first */
12416 /* one that results in no consistency-check failures. */
12417 
12418 /*   If P is close to the polygon boundary, the problem is */
12419 /* ill-conditioned and the decision may be incorrect.  Also, */
12420 /* an incorrect decision may result from a poor choice of Q */
12421 /* (if, for example, a boundary edge lies on the great cir- */
12422 /* cle defined by P and Q).  A more reliable result could be */
12423 /* obtained by a sequence of calls to INSIDE with the ver- */
12424 /* tices cyclically permuted before each call (to alter the */
12425 /* choice of Q). */
12426 
12427 
12428 /* On input: */
12429 
12430 /*       P = Array of length 3 containing the Cartesian */
12431 /*           coordinates of the point (unit vector) to be */
12432 /*           located. */
12433 
12434 /*       LV = Length of arrays XV, YV, and ZV. */
12435 
12436 /*       XV,YV,ZV = Arrays of length LV containing the Carte- */
12437 /*                  sian coordinates of unit vectors (points */
12438 /*                  on the unit sphere).  These values are */
12439 /*                  not tested for validity. */
12440 
12441 /*       NV = Number of vertices in the polygon.  3 .LE. NV */
12442 /*            .LE. LV. */
12443 
12444 /*       LISTV = Array of length NV containing the indexes */
12445 /*               (for XV, YV, and ZV) of a cyclically-ordered */
12446 /*               (and CCW-ordered) sequence of vertices that */
12447 /*               define R.  The last vertex (indexed by */
12448 /*               LISTV(NV)) is followed by the first (indexed */
12449 /*               by LISTV(1)).  LISTV entries must be in the */
12450 /*               range 1 to LV. */
12451 
12452 /* Input parameters are not altered by this function. */
12453 
12454 /* On output: */
12455 
12456 /*       INSIDE = TRUE if and only if P lies inside R unless */
12457 /*                IER .NE. 0, in which case the value is not */
12458 /*                altered. */
12459 
12460 /*       IER = Error indicator: */
12461 /*             IER = 0 if no errors were encountered. */
12462 /*             IER = 1 if LV or NV is outside its valid */
12463 /*                     range. */
12464 /*             IER = 2 if a LISTV entry is outside its valid */
12465 /*                     range. */
12466 /*             IER = 3 if the polygon boundary was found to */
12467 /*                     be self-intersecting.  This error will */
12468 /*                     not necessarily be detected. */
12469 /*             IER = 4 if every choice of Q (one for each */
12470 /*                     boundary edge) led to failure of some */
12471 /*                     internal consistency check.  The most */
12472 /*                     likely cause of this error is invalid */
12473 /*                     input:  P = (0,0,0), a null or self- */
12474 /*                     intersecting polygon, etc. */
12475 
12476 /* Module required by INSIDE:  INTRSC */
12477 
12478 /* Intrinsic function called by INSIDE:  SQRT */
12479 
12480 /* *********************************************************** */
12481 
12482 
12483 /* Local parameters: */
12484 
12485 /* B =         Intersection point between the boundary and */
12486 /*               the great circle defined by P and Q */
12487 /* BP,BQ =     <B,P> and <B,Q>, respectively, maximized over */
12488 /*               intersection points B that lie between P and */
12489 /*               Q (on the shorter arc) -- used to find the */
12490 /*               closest intersection points to P and Q */
12491 /* CN =        Q X P = normal to the plane of P and Q */
12492 /* D =         Dot product <B,P> or <B,Q> */
12493 /* EPS =       Parameter used to define Q as the point whose */
12494 /*               orthogonal distance to (the midpoint of) */
12495 /*               boundary edge V1->V2 is approximately EPS/ */
12496 /*               (2*Cos(A/2)), where <V1,V2> = Cos(A). */
12497 /* EVEN =      TRUE iff an even number of intersection points */
12498 /*               lie between P and Q (on the shorter arc) */
12499 /* I1,I2 =     Indexes (LISTV elements) of a pair of adjacent */
12500 /*               boundary vertices (endpoints of a boundary */
12501 /*               edge) */
12502 /* IERR =      Error flag for calls to INTRSC (not tested) */
12503 /* IMX =       Local copy of LV and maximum value of I1 and */
12504 /*               I2 */
12505 /* K =         DO-loop index and LISTV index */
12506 /* K0 =        LISTV index of the first endpoint of the */
12507 /*               boundary edge used to compute Q */
12508 /* LFT1,LFT2 = long int variables associated with I1 and I2 in */
12509 /*               the boundary traversal:  TRUE iff the vertex */
12510 /*               is strictly to the left of Q->P (<V,CN> > 0) */
12511 /* N =         Local copy of NV */
12512 /* NI =        Number of intersections (between the boundary */
12513 /*               curve and the great circle P-Q) encountered */
12514 /* PINR =      TRUE iff P is to the left of the directed */
12515 /*               boundary edge associated with the closest */
12516 /*               intersection point to P that lies between P */
12517 /*               and Q (a left-to-right intersection as */
12518 /*               viewed from Q), or there is no intersection */
12519 /*               between P and Q (on the shorter arc) */
12520 /* PN,QN =     P X CN and CN X Q, respectively:  used to */
12521 /*               locate intersections B relative to arc Q->P */
12522 /* Q =         (V1 + V2 + EPS*VN/VNRM)/QNRM, where V1->V2 is */
12523 /*               the boundary edge indexed by LISTV(K0) -> */
12524 /*               LISTV(K0+1) */
12525 /* QINR =      TRUE iff Q is to the left of the directed */
12526 /*               boundary edge associated with the closest */
12527 /*               intersection point to Q that lies between P */
12528 /*               and Q (a right-to-left intersection as */
12529 /*               viewed from Q), or there is no intersection */
12530 /*               between P and Q (on the shorter arc) */
12531 /* QNRM =      Euclidean norm of V1+V2+EPS*VN/VNRM used to */
12532 /*               compute (normalize) Q */
12533 /* V1,V2 =     Vertices indexed by I1 and I2 in the boundary */
12534 /*               traversal */
12535 /* VN =        V1 X V2, where V1->V2 is the boundary edge */
12536 /*               indexed by LISTV(K0) -> LISTV(K0+1) */
12537 /* VNRM =      Euclidean norm of VN */
12538 
12539     /* Parameter adjustments */
12540     --p;
12541     --zv;
12542     --yv;
12543     --xv;
12544     --listv;
12545 
12546     /* Function Body */
12547 
12548 /* Store local parameters, test for error 1, and initialize */
12549 /*   K0. */
12550 
12551     imx = *lv;
12552     n = *nv;
12553     if (n < 3 || n > imx) {
12554         goto L11;
12555     }
12556     k0 = 0;
12557     i1 = listv[1];
12558     if (i1 < 1 || i1 > imx) {
12559         goto L12;
12560     }
12561 
12562 /* Increment K0 and set Q to a point immediately to the left */
12563 /*   of the midpoint of edge V1->V2 = LISTV(K0)->LISTV(K0+1): */
12564 /*   Q = (V1 + V2 + EPS*VN/VNRM)/QNRM, where VN = V1 X V2. */
12565 
12566 L1:
12567     ++k0;
12568     if (k0 > n) {
12569         goto L14;
12570     }
12571     i1 = listv[k0];
12572     if (k0 < n) {
12573         i2 = listv[k0 + 1];
12574     } else {
12575         i2 = listv[1];
12576     }
12577     if (i2 < 1 || i2 > imx) {
12578         goto L12;
12579     }
12580     vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12581     vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12582     vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12583     vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12584     if (vnrm == 0.) {
12585         goto L1;
12586     }
12587     q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12588     q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12589     q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12590     qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12591     q[0] /= qnrm;
12592     q[1] /= qnrm;
12593     q[2] /= qnrm;
12594 
12595 /* Compute CN = Q X P, PN = P X CN, and QN = CN X Q. */
12596 
12597     cn[0] = q[1] * p[3] - q[2] * p[2];
12598     cn[1] = q[2] * p[1] - q[0] * p[3];
12599     cn[2] = q[0] * p[2] - q[1] * p[1];
12600     if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12601         goto L1;
12602     }
12603     pn[0] = p[2] * cn[2] - p[3] * cn[1];
12604     pn[1] = p[3] * cn[0] - p[1] * cn[2];
12605     pn[2] = p[1] * cn[1] - p[2] * cn[0];
12606     qn[0] = cn[1] * q[2] - cn[2] * q[1];
12607     qn[1] = cn[2] * q[0] - cn[0] * q[2];
12608     qn[2] = cn[0] * q[1] - cn[1] * q[0];
12609 
12610 /* Initialize parameters for the boundary traversal. */
12611 
12612     ni = 0;
12613     even = TRUE_;
12614     bp = -2.;
12615     bq = -2.;
12616     pinr = TRUE_;
12617     qinr = TRUE_;
12618     i2 = listv[n];
12619     if (i2 < 1 || i2 > imx) {
12620         goto L12;
12621     }
12622     lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12623 
12624 /* Loop on boundary arcs I1->I2. */
12625 
12626     i__1 = n;
12627     for (k = 1; k <= i__1; ++k) {
12628         i1 = i2;
12629         lft1 = lft2;
12630         i2 = listv[k];
12631         if (i2 < 1 || i2 > imx) {
12632             goto L12;
12633         }
12634         lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12635         if (lft1 == lft2) {
12636             goto L2;
12637         }
12638 
12639 /*   I1 and I2 are on opposite sides of Q->P.  Compute the */
12640 /*     point of intersection B. */
12641 
12642         ++ni;
12643         v1[0] = xv[i1];
12644         v1[1] = yv[i1];
12645         v1[2] = zv[i1];
12646         v2[0] = xv[i2];
12647         v2[1] = yv[i2];
12648         v2[2] = zv[i2];
12649         intrsc_(v1, v2, cn, b, &ierr);
12650 
12651 /*   B is between Q and P (on the shorter arc) iff */
12652 /*     B Forward Q->P and B Forward P->Q       iff */
12653 /*     <B,QN> > 0 and <B,PN> > 0. */
12654 
12655         if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12656                 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12657 
12658 /*   Update EVEN, BQ, QINR, BP, and PINR. */
12659 
12660             even = ! even;
12661             d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12662             if (d__ > bq) {
12663                 bq = d__;
12664                 qinr = lft2;
12665             }
12666             d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12667             if (d__ > bp) {
12668                 bp = d__;
12669                 pinr = lft1;
12670             }
12671         }
12672 L2:
12673         ;
12674     }
12675 
12676 /* Test for consistency:  NI must be even and QINR must be */
12677 /*   TRUE. */
12678 
12679     if (ni != ni / 2 << 1 || ! qinr) {
12680         goto L1;
12681     }
12682 
12683 /* Test for error 3:  different values of PINR and EVEN. */
12684 
12685     if (pinr != even) {
12686         goto L13;
12687     }
12688 
12689 /* No error encountered. */
12690 
12691     *ier = 0;
12692     ret_val = even;
12693     return ret_val;
12694 
12695 /* LV or NV is outside its valid range. */
12696 
12697 L11:
12698     *ier = 1;
12699     return ret_val;
12700 
12701 /* A LISTV entry is outside its valid range. */
12702 
12703 L12:
12704     *ier = 2;
12705     return ret_val;
12706 
12707 /* The polygon boundary is self-intersecting. */
12708 
12709 L13:
12710     *ier = 3;
12711     return ret_val;
12712 
12713 /* Consistency tests failed for all values of Q. */
12714 
12715 L14:
12716     *ier = 4;
12717     return ret_val;
12718 } /* inside_ */
12719 
12720 /* Subroutine */ int intadd_(int *kk, int *i1, int *i2, int *
12721         i3, int *list, int *lptr, int *lend, int *lnew)
12722 {
12723     static int k, n1, n2, n3, lp;
12724     extern /* Subroutine */ int insert_(int *, int *, int *,
12725             int *, int *);
12726     extern int lstptr_(int *, int *, int *, int *);
12727 
12728 
12729 /* *********************************************************** */
12730 
12731 /*                                              From STRIPACK */
12732 /*                                            Robert J. Renka */
12733 /*                                  Dept. of Computer Science */
12734 /*                                       Univ. of North Texas */
12735 /*                                           renka@cs.unt.edu */
12736 /*                                                   07/17/96 */
12737 
12738 /*   This subroutine adds an interior node to a triangulation */
12739 /* of a set of points on the unit sphere.  The data structure */
12740 /* is updated with the insertion of node KK into the triangle */
12741 /* whose vertices are I1, I2, and I3.  No optimization of the */
12742 /* triangulation is performed. */
12743 
12744 /*   This routine is identical to the similarly named routine */
12745 /* in TRIPACK. */
12746 
12747 
12748 /* On input: */
12749 
12750 /*       KK = Index of the node to be inserted.  KK .GE. 1 */
12751 /*            and KK must not be equal to I1, I2, or I3. */
12752 
12753 /*       I1,I2,I3 = Indexes of the counterclockwise-ordered */
12754 /*                  sequence of vertices of a triangle which */
12755 /*                  contains node KK. */
12756 
12757 /* The above parameters are not altered by this routine. */
12758 
12759 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
12760 /*                             triangulation.  Refer to Sub- */
12761 /*                             routine TRMESH.  Triangle */
12762 /*                             (I1,I2,I3) must be included */
12763 /*                             in the triangulation. */
12764 
12765 /* On output: */
12766 
12767 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
12768 /*                             the addition of node KK.  KK */
12769 /*                             will be connected to nodes I1, */
12770 /*                             I2, and I3. */
12771 
12772 /* Modules required by INTADD:  INSERT, LSTPTR */
12773 
12774 /* *********************************************************** */
12775 
12776 
12777 /* Local parameters: */
12778 
12779 /* K =        Local copy of KK */
12780 /* LP =       LIST pointer */
12781 /* N1,N2,N3 = Local copies of I1, I2, and I3 */
12782 
12783     /* Parameter adjustments */
12784     --lend;
12785     --lptr;
12786     --list;
12787 
12788     /* Function Body */
12789     k = *kk;
12790 
12791 /* Initialization. */
12792 
12793     n1 = *i1;
12794     n2 = *i2;
12795     n3 = *i3;
12796 
12797 /* Add K as a neighbor of I1, I2, and I3. */
12798 
12799     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12800     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12801     lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12802     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12803     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12804     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12805 
12806 /* Add I1, I2, and I3 as neighbors of K. */
12807 
12808     list[*lnew] = n1;
12809     list[*lnew + 1] = n2;
12810     list[*lnew + 2] = n3;
12811     lptr[*lnew] = *lnew + 1;
12812     lptr[*lnew + 1] = *lnew + 2;
12813     lptr[*lnew + 2] = *lnew;
12814     lend[k] = *lnew + 2;
12815     *lnew += 3;
12816     return 0;
12817 } /* intadd_ */
12818 
12819 /* Subroutine */ int intrsc_(double *p1, double *p2, double *cn,
12820         double *p, int *ier)
12821 {
12822     /* Builtin functions */
12823     //double sqrt(double);
12824 
12825     /* Local variables */
12826     static int i__;
12827     static double t, d1, d2, pp[3], ppn;
12828 
12829 
12830 /* *********************************************************** */
12831 
12832 /*                                              From STRIPACK */
12833 /*                                            Robert J. Renka */
12834 /*                                  Dept. of Computer Science */
12835 /*                                       Univ. of North Texas */
12836 /*                                           renka@cs.unt.edu */
12837 /*                                                   07/19/90 */
12838 
12839 /*   Given a great circle C and points P1 and P2 defining an */
12840 /* arc A on the surface of the unit sphere, where A is the */
12841 /* shorter of the two portions of the great circle C12 assoc- */
12842 /* iated with P1 and P2, this subroutine returns the point */
12843 /* of intersection P between C and C12 that is closer to A. */
12844 /* Thus, if P1 and P2 lie in opposite hemispheres defined by */
12845 /* C, P is the point of intersection of C with A. */
12846 
12847 
12848 /* On input: */
12849 
12850 /*       P1,P2 = Arrays of length 3 containing the Cartesian */
12851 /*               coordinates of unit vectors. */
12852 
12853 /*       CN = Array of length 3 containing the Cartesian */
12854 /*            coordinates of a nonzero vector which defines C */
12855 /*            as the intersection of the plane whose normal */
12856 /*            is CN with the unit sphere.  Thus, if C is to */
12857 /*            be the great circle defined by P and Q, CN */
12858 /*            should be P X Q. */
12859 
12860 /* The above parameters are not altered by this routine. */
12861 
12862 /*       P = Array of length 3. */
12863 
12864 /* On output: */
12865 
12866 /*       P = Point of intersection defined above unless IER */
12867 /*           .NE. 0, in which case P is not altered. */
12868 
12869 /*       IER = Error indicator. */
12870 /*             IER = 0 if no errors were encountered. */
12871 /*             IER = 1 if <CN,P1> = <CN,P2>.  This occurs */
12872 /*                     iff P1 = P2 or CN = 0 or there are */
12873 /*                     two intersection points at the same */
12874 /*                     distance from A. */
12875 /*             IER = 2 if P2 = -P1 and the definition of A is */
12876 /*                     therefore ambiguous. */
12877 
12878 /* Modules required by INTRSC:  None */
12879 
12880 /* Intrinsic function called by INTRSC:  SQRT */
12881 
12882 /* *********************************************************** */
12883 
12884 
12885 /* Local parameters: */
12886 
12887 /* D1 =  <CN,P1> */
12888 /* D2 =  <CN,P2> */
12889 /* I =   DO-loop index */
12890 /* PP =  P1 + T*(P2-P1) = Parametric representation of the */
12891 /*         line defined by P1 and P2 */
12892 /* PPN = Norm of PP */
12893 /* T =   D1/(D1-D2) = Parameter value chosen so that PP lies */
12894 /*         in the plane of C */
12895 
12896     /* Parameter adjustments */
12897     --p;
12898     --cn;
12899     --p2;
12900     --p1;
12901 
12902     /* Function Body */
12903     d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
12904     d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
12905 
12906     if (d1 == d2) {
12907         *ier = 1;
12908         return 0;
12909     }
12910 
12911 /* Solve for T such that <PP,CN> = 0 and compute PP and PPN. */
12912 
12913     t = d1 / (d1 - d2);
12914     ppn = 0.;
12915     for (i__ = 1; i__ <= 3; ++i__) {
12916         pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
12917         ppn += pp[i__ - 1] * pp[i__ - 1];
12918 /* L1: */
12919     }
12920 
12921 /* PPN = 0 iff PP = 0 iff P2 = -P1 (and T = .5). */
12922 
12923     if (ppn == 0.) {
12924         *ier = 2;
12925         return 0;
12926     }
12927     ppn = sqrt(ppn);
12928 
12929 /* Compute P = PP/PPN. */
12930 
12931     for (i__ = 1; i__ <= 3; ++i__) {
12932         p[i__] = pp[i__ - 1] / ppn;
12933 /* L2: */
12934     }
12935     *ier = 0;
12936     return 0;
12937 } /* intrsc_ */
12938 
12939 int jrand_(int *n, int *ix, int *iy, int *iz)
12940 {
12941     /* System generated locals */
12942     int ret_val;
12943 
12944     /* Local variables */
12945     static float u, x;
12946 
12947 
12948 /* *********************************************************** */
12949 
12950 /*                                              From STRIPACK */
12951 /*                                            Robert J. Renka */
12952 /*                                  Dept. of Computer Science */
12953 /*                                       Univ. of North Texas */
12954 /*                                           renka@cs.unt.edu */
12955 /*                                                   07/28/98 */
12956 
12957 /*   This function returns a uniformly distributed pseudo- */
12958 /* random int in the range 1 to N. */
12959 
12960 
12961 /* On input: */
12962 
12963 /*       N = Maximum value to be returned. */
12964 
12965 /* N is not altered by this function. */
12966 
12967 /*       IX,IY,IZ = int seeds initialized to values in */
12968 /*                  the range 1 to 30,000 before the first */
12969 /*                  call to JRAND, and not altered between */
12970 /*                  subsequent calls (unless a sequence of */
12971 /*                  random numbers is to be repeated by */
12972 /*                  reinitializing the seeds). */
12973 
12974 /* On output: */
12975 
12976 /*       IX,IY,IZ = Updated int seeds. */
12977 
12978 /*       JRAND = Random int in the range 1 to N. */
12979 
12980 /* Reference:  B. A. Wichmann and I. D. Hill, "An Efficient */
12981 /*             and Portable Pseudo-random Number Generator", */
12982 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
12983 /*             pp. 188-190. */
12984 
12985 /* Modules required by JRAND:  None */
12986 
12987 /* Intrinsic functions called by JRAND:  INT, MOD, float */
12988 
12989 /* *********************************************************** */
12990 
12991 
12992 /* Local parameters: */
12993 
12994 /* U = Pseudo-random number uniformly distributed in the */
12995 /*     interval (0,1). */
12996 /* X = Pseudo-random number in the range 0 to 3 whose frac- */
12997 /*       tional part is U. */
12998 
12999     *ix = *ix * 171 % 30269;
13000     *iy = *iy * 172 % 30307;
13001     *iz = *iz * 170 % 30323;
13002     x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13003             30323.f;
13004     u = x - (int) x;
13005     ret_val = (int) ((float) (*n) * u + 1.f);
13006     return ret_val;
13007 } /* jrand_ */
13008 
13009 long int left_(double *x1, double *y1, double *z1, double *x2,
13010         double *y2, double *z2, double *x0, double *y0,
13011         double *z0)
13012 {
13013     /* System generated locals */
13014     long int ret_val;
13015 
13016 
13017 /* *********************************************************** */
13018 
13019 /*                                              From STRIPACK */
13020 /*                                            Robert J. Renka */
13021 /*                                  Dept. of Computer Science */
13022 /*                                       Univ. of North Texas */
13023 /*                                           renka@cs.unt.edu */
13024 /*                                                   07/15/96 */
13025 
13026 /*   This function determines whether node N0 is in the */
13027 /* (closed) left hemisphere defined by the plane containing */
13028 /* N1, N2, and the origin, where left is defined relative to */
13029 /* an observer at N1 facing N2. */
13030 
13031 
13032 /* On input: */
13033 
13034 /*       X1,Y1,Z1 = Coordinates of N1. */
13035 
13036 /*       X2,Y2,Z2 = Coordinates of N2. */
13037 
13038 /*       X0,Y0,Z0 = Coordinates of N0. */
13039 
13040 /* Input parameters are not altered by this function. */
13041 
13042 /* On output: */
13043 
13044 /*       LEFT = TRUE if and only if N0 is in the closed */
13045 /*              left hemisphere. */
13046 
13047 /* Modules required by LEFT:  None */
13048 
13049 /* *********************************************************** */
13050 
13051 /* LEFT = TRUE iff <N0,N1 X N2> = det(N0,N1,N2) .GE. 0. */
13052 
13053     ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13054             *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13055 
13056 
13057     return ret_val;
13058 } /* left_ */
13059 
13060 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13061 {
13062     /* System generated locals */
13063     int ret_val;
13064 
13065     /* Local variables */
13066     static int nd, lp;
13067 
13068 
13069 /* *********************************************************** */
13070 
13071 /*                                              From STRIPACK */
13072 /*                                            Robert J. Renka */
13073 /*                                  Dept. of Computer Science */
13074 /*                                       Univ. of North Texas */
13075 /*                                           renka@cs.unt.edu */
13076 /*                                                   07/15/96 */
13077 
13078 /*   This function returns the index (LIST pointer) of NB in */
13079 /* the adjacency list for N0, where LPL = LEND(N0). */
13080 
13081 /*   This function is identical to the similarly named */
13082 /* function in TRIPACK. */
13083 
13084 
13085 /* On input: */
13086 
13087 /*       LPL = LEND(N0) */
13088 
13089 /*       NB = Index of the node whose pointer is to be re- */
13090 /*            turned.  NB must be connected to N0. */
13091 
13092 /*       LIST,LPTR = Data structure defining the triangula- */
13093 /*                   tion.  Refer to Subroutine TRMESH. */
13094 
13095 /* Input parameters are not altered by this function. */
13096 
13097 /* On output: */
13098 
13099 /*       LSTPTR = Pointer such that LIST(LSTPTR) = NB or */
13100 /*                LIST(LSTPTR) = -NB, unless NB is not a */
13101 /*                neighbor of N0, in which case LSTPTR = LPL. */
13102 
13103 /* Modules required by LSTPTR:  None */
13104 
13105 /* *********************************************************** */
13106 
13107 
13108 /* Local parameters: */
13109 
13110 /* LP = LIST pointer */
13111 /* ND = Nodal index */
13112 
13113     /* Parameter adjustments */
13114     --lptr;
13115     --list;
13116 
13117     /* Function Body */
13118     lp = lptr[*lpl];
13119 L1:
13120     nd = list[lp];
13121     if (nd == *nb) {
13122         goto L2;
13123     }
13124     lp = lptr[lp];
13125     if (lp != *lpl) {
13126         goto L1;
13127     }
13128 
13129 L2:
13130     ret_val = lp;
13131     return ret_val;
13132 } /* lstptr_ */
13133 
13134 int nbcnt_(int *lpl, int *lptr)
13135 {
13136     /* System generated locals */
13137     int ret_val;
13138 
13139     /* Local variables */
13140     static int k, lp;
13141 
13142 
13143 /* *********************************************************** */
13144 
13145 /*                                              From STRIPACK */
13146 /*                                            Robert J. Renka */
13147 /*                                  Dept. of Computer Science */
13148 /*                                       Univ. of North Texas */
13149 /*                                           renka@cs.unt.edu */
13150 /*                                                   07/15/96 */
13151 
13152 /*   This function returns the number of neighbors of a node */
13153 /* N0 in a triangulation created by Subroutine TRMESH. */
13154 
13155 /*   This function is identical to the similarly named */
13156 /* function in TRIPACK. */
13157 
13158 
13159 /* On input: */
13160 
13161 /*       LPL = LIST pointer to the last neighbor of N0 -- */
13162 /*             LPL = LEND(N0). */
13163 
13164 /*       LPTR = Array of pointers associated with LIST. */
13165 
13166 /* Input parameters are not altered by this function. */
13167 
13168 /* On output: */
13169 
13170 /*       NBCNT = Number of neighbors of N0. */
13171 
13172 /* Modules required by NBCNT:  None */
13173 
13174 /* *********************************************************** */
13175 
13176 
13177 /* Local parameters: */
13178 
13179 /* K =  Counter for computing the number of neighbors */
13180 /* LP = LIST pointer */
13181 
13182     /* Parameter adjustments */
13183     --lptr;
13184 
13185     /* Function Body */
13186     lp = *lpl;
13187     k = 1;
13188 
13189 L1:
13190     lp = lptr[lp];
13191     if (lp == *lpl) {
13192         goto L2;
13193     }
13194     ++k;
13195     goto L1;
13196 
13197 L2:
13198     ret_val = k;
13199     return ret_val;
13200 } /* nbcnt_ */
13201 
13202 int nearnd_(double *p, int *ist, int *n, double *x,
13203         double *y, double *z__, int *list, int *lptr, int
13204         *lend, double *al)
13205 {
13206     /* System generated locals */
13207     int ret_val, i__1;
13208 
13209     /* Builtin functions */
13210     //double acos(double);
13211 
13212     /* Local variables */
13213     static int l;
13214     static double b1, b2, b3;
13215     static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13216     static double ds1;
13217     static int lp1, lp2;
13218     static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13219     static int lpl;
13220     static double dsr;
13221     static int nst, listp[25], lptrp[25];
13222     extern /* Subroutine */ int trfind_(int *, double *, int *,
13223             double *, double *, double *, int *, int *,
13224             int *, double *, double *, double *, int *,
13225             int *, int *);
13226     extern int lstptr_(int *, int *, int *, int *);
13227 
13228 
13229 /* *********************************************************** */
13230 
13231 /*                                              From STRIPACK */
13232 /*                                            Robert J. Renka */
13233 /*                                  Dept. of Computer Science */
13234 /*                                       Univ. of North Texas */
13235 /*                                           renka@cs.unt.edu */
13236 /*                                                   07/28/98 */
13237 
13238 /*   Given a point P on the surface of the unit sphere and a */
13239 /* Delaunay triangulation created by Subroutine TRMESH, this */
13240 /* function returns the index of the nearest triangulation */
13241 /* node to P. */
13242 
13243 /*   The algorithm consists of implicitly adding P to the */
13244 /* triangulation, finding the nearest neighbor to P, and */
13245 /* implicitly deleting P from the triangulation.  Thus, it */
13246 /* is based on the fact that, if P is a node in a Delaunay */
13247 /* triangulation, the nearest node to P is a neighbor of P. */
13248 
13249 
13250 /* On input: */
13251 
13252 /*       P = Array of length 3 containing the Cartesian coor- */
13253 /*           dinates of the point P to be located relative to */
13254 /*           the triangulation.  It is assumed without a test */
13255 /*           that P(1)**2 + P(2)**2 + P(3)**2 = 1. */
13256 
13257 /*       IST = Index of a node at which TRFIND begins the */
13258 /*             search.  Search time depends on the proximity */
13259 /*             of this node to P. */
13260 
13261 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
13262 
13263 /*       X,Y,Z = Arrays of length N containing the Cartesian */
13264 /*               coordinates of the nodes. */
13265 
13266 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13267 /*                        gulation.  Refer to TRMESH. */
13268 
13269 /* Input parameters are not altered by this function. */
13270 
13271 /* On output: */
13272 
13273 /*       NEARND = Nodal index of the nearest node to P, or 0 */
13274 /*                if N < 3 or the triangulation data struc- */
13275 /*                ture is invalid. */
13276 
13277 /*       AL = Arc length (angular distance in radians) be- */
13278 /*            tween P and NEARND unless NEARND = 0. */
13279 
13280 /*       Note that the number of candidates for NEARND */
13281 /*       (neighbors of P) is limited to LMAX defined in */
13282 /*       the PARAMETER statement below. */
13283 
13284 /* Modules required by NEARND:  JRAND, LSTPTR, TRFIND, STORE */
13285 
13286 /* Intrinsic functions called by NEARND:  ABS, ACOS */
13287 
13288 /* *********************************************************** */
13289 
13290 
13291 /* Local parameters: */
13292 
13293 /* B1,B2,B3 =  Unnormalized barycentric coordinates returned */
13294 /*               by TRFIND */
13295 /* DS1 =       (Negative cosine of the) distance from P to N1 */
13296 /* DSR =       (Negative cosine of the) distance from P to NR */
13297 /* DX1,..DZ3 = Components of vectors used by the swap test */
13298 /* I1,I2,I3 =  Nodal indexes of a triangle containing P, or */
13299 /*               the rightmost (I1) and leftmost (I2) visible */
13300 /*               boundary nodes as viewed from P */
13301 /* L =         Length of LISTP/LPTRP and number of neighbors */
13302 /*               of P */
13303 /* LMAX =      Maximum value of L */
13304 /* LISTP =     Indexes of the neighbors of P */
13305 /* LPTRP =     Array of pointers in 1-1 correspondence with */
13306 /*               LISTP elements */
13307 /* LP =        LIST pointer to a neighbor of N1 and LISTP */
13308 /*               pointer */
13309 /* LP1,LP2 =   LISTP indexes (pointers) */
13310 /* LPL =       Pointer to the last neighbor of N1 */
13311 /* N1 =        Index of a node visible from P */
13312 /* N2 =        Index of an endpoint of an arc opposite P */
13313 /* N3 =        Index of the node opposite N1->N2 */
13314 /* NN =        Local copy of N */
13315 /* NR =        Index of a candidate for the nearest node to P */
13316 /* NST =       Index of the node at which TRFIND begins the */
13317 /*               search */
13318 
13319 
13320 /* Store local parameters and test for N invalid. */
13321 
13322     /* Parameter adjustments */
13323     --p;
13324     --lend;
13325     --z__;
13326     --y;
13327     --x;
13328     --list;
13329     --lptr;
13330 
13331     /* Function Body */
13332     nn = *n;
13333     if (nn < 3) {
13334         goto L6;
13335     }
13336     nst = *ist;
13337     if (nst < 1 || nst > nn) {
13338         nst = 1;
13339     }
13340 
13341 /* Find a triangle (I1,I2,I3) containing P, or the rightmost */
13342 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
13343 /*   from P. */
13344 
13345     trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13346             1], &b1, &b2, &b3, &i1, &i2, &i3);
13347 
13348 /* Test for collinear nodes. */
13349 
13350     if (i1 == 0) {
13351         goto L6;
13352     }
13353 
13354 /* Store the linked list of 'neighbors' of P in LISTP and */
13355 /*   LPTRP.  I1 is the first neighbor, and 0 is stored as */
13356 /*   the last neighbor if P is not contained in a triangle. */
13357 /*   L is the length of LISTP and LPTRP, and is limited to */
13358 /*   LMAX. */
13359 
13360     if (i3 != 0) {
13361         listp[0] = i1;
13362         lptrp[0] = 2;
13363         listp[1] = i2;
13364         lptrp[1] = 3;
13365         listp[2] = i3;
13366         lptrp[2] = 1;
13367         l = 3;
13368     } else {
13369         n1 = i1;
13370         l = 1;
13371         lp1 = 2;
13372         listp[l - 1] = n1;
13373         lptrp[l - 1] = lp1;
13374 
13375 /*   Loop on the ordered sequence of visible boundary nodes */
13376 /*     N1 from I1 to I2. */
13377 
13378 L1:
13379         lpl = lend[n1];
13380         n1 = -list[lpl];
13381         l = lp1;
13382         lp1 = l + 1;
13383         listp[l - 1] = n1;
13384         lptrp[l - 1] = lp1;
13385         if (n1 != i2 && lp1 < 25) {
13386             goto L1;
13387         }
13388         l = lp1;
13389         listp[l - 1] = 0;
13390         lptrp[l - 1] = 1;
13391     }
13392 
13393 /* Initialize variables for a loop on arcs N1-N2 opposite P */
13394 /*   in which new 'neighbors' are 'swapped' in.  N1 follows */
13395 /*   N2 as a neighbor of P, and LP1 and LP2 are the LISTP */
13396 /*   indexes of N1 and N2. */
13397 
13398     lp2 = 1;
13399     n2 = i1;
13400     lp1 = lptrp[0];
13401     n1 = listp[lp1 - 1];
13402 
13403 /* Begin loop:  find the node N3 opposite N1->N2. */
13404 
13405 L2:
13406     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13407     if (list[lp] < 0) {
13408         goto L3;
13409     }
13410     lp = lptr[lp];
13411     n3 = (i__1 = list[lp], abs(i__1));
13412 
13413 /* Swap test:  Exit the loop if L = LMAX. */
13414 
13415     if (l == 25) {
13416         goto L4;
13417     }
13418     dx1 = x[n1] - p[1];
13419     dy1 = y[n1] - p[2];
13420     dz1 = z__[n1] - p[3];
13421 
13422     dx2 = x[n2] - p[1];
13423     dy2 = y[n2] - p[2];
13424     dz2 = z__[n2] - p[3];
13425 
13426     dx3 = x[n3] - p[1];
13427     dy3 = y[n3] - p[2];
13428     dz3 = z__[n3] - p[3];
13429     if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13430             (dx2 * dy1 - dx1 * dy2) <= 0.) {
13431         goto L3;
13432     }
13433 
13434 /* Swap:  Insert N3 following N2 in the adjacency list for P. */
13435 /*        The two new arcs opposite P must be tested. */
13436 
13437     ++l;
13438     lptrp[lp2 - 1] = l;
13439     listp[l - 1] = n3;
13440     lptrp[l - 1] = lp1;
13441     lp1 = l;
13442     n1 = n3;
13443     goto L2;
13444 
13445 /* No swap:  Advance to the next arc and test for termination */
13446 /*           on N1 = I1 (LP1 = 1) or N1 followed by 0. */
13447 
13448 L3:
13449     if (lp1 == 1) {
13450         goto L4;
13451     }
13452     lp2 = lp1;
13453     n2 = n1;
13454     lp1 = lptrp[lp1 - 1];
13455     n1 = listp[lp1 - 1];
13456     if (n1 == 0) {
13457         goto L4;
13458     }
13459     goto L2;
13460 
13461 /* Set NR and DSR to the index of the nearest node to P and */
13462 /*   an increasing function (negative cosine) of its distance */
13463 /*   from P, respectively. */
13464 
13465 L4:
13466     nr = i1;
13467     dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13468     i__1 = l;
13469     for (lp = 2; lp <= i__1; ++lp) {
13470         n1 = listp[lp - 1];
13471         if (n1 == 0) {
13472             goto L5;
13473         }
13474         ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13475         if (ds1 < dsr) {
13476             nr = n1;
13477             dsr = ds1;
13478         }
13479 L5:
13480         ;
13481     }
13482     dsr = -dsr;
13483     if (dsr > 1.) {
13484         dsr = 1.;
13485     }
13486     *al = acos(dsr);
13487     ret_val = nr;
13488     return ret_val;
13489 
13490 /* Invalid input. */
13491 
13492 L6:
13493     ret_val = 0;
13494     return ret_val;
13495 } /* nearnd_ */
13496 
13497 /* Subroutine */ int optim_(double *x, double *y, double *z__,
13498         int *na, int *list, int *lptr, int *lend, int *
13499         nit, int *iwk, int *ier)
13500 {
13501     /* System generated locals */
13502     int i__1, i__2;
13503 
13504     /* Local variables */
13505     static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13506     static long int swp;
13507     static int iter;
13508     extern /* Subroutine */ int swap_(int *, int *, int *,
13509             int *, int *, int *, int *, int *);
13510     static int maxit;
13511     extern long int swptst_(int *, int *, int *, int *,
13512             double *, double *, double *);
13513 
13514 
13515 /* *********************************************************** */
13516 
13517 /*                                              From STRIPACK */
13518 /*                                            Robert J. Renka */
13519 /*                                  Dept. of Computer Science */
13520 /*                                       Univ. of North Texas */
13521 /*                                           renka@cs.unt.edu */
13522 /*                                                   07/30/98 */
13523 
13524 /*   Given a set of NA triangulation arcs, this subroutine */
13525 /* optimizes the portion of the triangulation consisting of */
13526 /* the quadrilaterals (pairs of adjacent triangles) which */
13527 /* have the arcs as diagonals by applying the circumcircle */
13528 /* test and appropriate swaps to the arcs. */
13529 
13530 /*   An iteration consists of applying the swap test and */
13531 /* swaps to all NA arcs in the order in which they are */
13532 /* stored.  The iteration is repeated until no swap occurs */
13533 /* or NIT iterations have been performed.  The bound on the */
13534 /* number of iterations may be necessary to prevent an */
13535 /* infinite loop caused by cycling (reversing the effect of a */
13536 /* previous swap) due to floating point inaccuracy when four */
13537 /* or more nodes are nearly cocircular. */
13538 
13539 
13540 /* On input: */
13541 
13542 /*       X,Y,Z = Arrays containing the nodal coordinates. */
13543 
13544 /*       NA = Number of arcs in the set.  NA .GE. 0. */
13545 
13546 /* The above parameters are not altered by this routine. */
13547 
13548 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13549 /*                        gulation.  Refer to Subroutine */
13550 /*                        TRMESH. */
13551 
13552 /*       NIT = Maximum number of iterations to be performed. */
13553 /*             NIT = 4*NA should be sufficient.  NIT .GE. 1. */
13554 
13555 /*       IWK = int array dimensioned 2 by NA containing */
13556 /*             the nodal indexes of the arc endpoints (pairs */
13557 /*             of endpoints are stored in columns). */
13558 
13559 /* On output: */
13560 
13561 /*       LIST,LPTR,LEND = Updated triangulation data struc- */
13562 /*                        ture reflecting the swaps. */
13563 
13564 /*       NIT = Number of iterations performed. */
13565 
13566 /*       IWK = Endpoint indexes of the new set of arcs */
13567 /*             reflecting the swaps. */
13568 
13569 /*       IER = Error indicator: */
13570 /*             IER = 0 if no errors were encountered. */
13571 /*             IER = 1 if a swap occurred on the last of */
13572 /*                     MAXIT iterations, where MAXIT is the */
13573 /*                     value of NIT on input.  The new set */
13574 /*                     of arcs is not necessarily optimal */
13575 /*                     in this case. */
13576 /*             IER = 2 if NA < 0 or NIT < 1 on input. */
13577 /*             IER = 3 if IWK(2,I) is not a neighbor of */
13578 /*                     IWK(1,I) for some I in the range 1 */
13579 /*                     to NA.  A swap may have occurred in */
13580 /*                     this case. */
13581 /*             IER = 4 if a zero pointer was returned by */
13582 /*                     Subroutine SWAP. */
13583 
13584 /* Modules required by OPTIM:  LSTPTR, SWAP, SWPTST */
13585 
13586 /* Intrinsic function called by OPTIM:  ABS */
13587 
13588 /* *********************************************************** */
13589 
13590 
13591 /* Local parameters: */
13592 
13593 /* I =       Column index for IWK */
13594 /* IO1,IO2 = Nodal indexes of the endpoints of an arc in IWK */
13595 /* ITER =    Iteration count */
13596 /* LP =      LIST pointer */
13597 /* LP21 =    Parameter returned by SWAP (not used) */
13598 /* LPL =     Pointer to the last neighbor of IO1 */
13599 /* LPP =     Pointer to the node preceding IO2 as a neighbor */
13600 /*             of IO1 */
13601 /* MAXIT =   Input value of NIT */
13602 /* N1,N2 =   Nodes opposite IO1->IO2 and IO2->IO1, */
13603 /*             respectively */
13604 /* NNA =     Local copy of NA */
13605 /* SWP =     Flag set to TRUE iff a swap occurs in the */
13606 /*             optimization loop */
13607 
13608     /* Parameter adjustments */
13609     --x;
13610     --y;
13611     --z__;
13612     iwk -= 3;
13613     --list;
13614     --lptr;
13615     --lend;
13616 
13617     /* Function Body */
13618     nna = *na;
13619     maxit = *nit;
13620     if (nna < 0 || maxit < 1) {
13621         goto L7;
13622     }
13623 
13624 /* Initialize iteration count ITER and test for NA = 0. */
13625 
13626     iter = 0;
13627     if (nna == 0) {
13628         goto L5;
13629     }
13630 
13631 /* Top of loop -- */
13632 /*   SWP = TRUE iff a swap occurred in the current iteration. */
13633 
13634 L1:
13635     if (iter == maxit) {
13636         goto L6;
13637     }
13638     ++iter;
13639     swp = FALSE_;
13640 
13641 /*   Inner loop on arcs IO1-IO2 -- */
13642 
13643     i__1 = nna;
13644     for (i__ = 1; i__ <= i__1; ++i__) {
13645         io1 = iwk[(i__ << 1) + 1];
13646         io2 = iwk[(i__ << 1) + 2];
13647 
13648 /*   Set N1 and N2 to the nodes opposite IO1->IO2 and */
13649 /*     IO2->IO1, respectively.  Determine the following: */
13650 
13651 /*     LPL = pointer to the last neighbor of IO1, */
13652 /*     LP = pointer to IO2 as a neighbor of IO1, and */
13653 /*     LPP = pointer to the node N2 preceding IO2. */
13654 
13655         lpl = lend[io1];
13656         lpp = lpl;
13657         lp = lptr[lpp];
13658 L2:
13659         if (list[lp] == io2) {
13660             goto L3;
13661         }
13662         lpp = lp;
13663         lp = lptr[lpp];
13664         if (lp != lpl) {
13665             goto L2;
13666         }
13667 
13668 /*   IO2 should be the last neighbor of IO1.  Test for no */
13669 /*     arc and bypass the swap test if IO1 is a boundary */
13670 /*     node. */
13671 
13672         if ((i__2 = list[lp], abs(i__2)) != io2) {
13673             goto L8;
13674         }
13675         if (list[lp] < 0) {
13676             goto L4;
13677         }
13678 
13679 /*   Store N1 and N2, or bypass the swap test if IO1 is a */
13680 /*     boundary node and IO2 is its first neighbor. */
13681 
13682 L3:
13683         n2 = list[lpp];
13684         if (n2 < 0) {
13685             goto L4;
13686         }
13687         lp = lptr[lp];
13688         n1 = (i__2 = list[lp], abs(i__2));
13689 
13690 /*   Test IO1-IO2 for a swap, and update IWK if necessary. */
13691 
13692         if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13693             goto L4;
13694         }
13695         swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13696         if (lp21 == 0) {
13697             goto L9;
13698         }
13699         swp = TRUE_;
13700         iwk[(i__ << 1) + 1] = n1;
13701         iwk[(i__ << 1) + 2] = n2;
13702 L4:
13703         ;
13704     }
13705     if (swp) {
13706         goto L1;
13707     }
13708 
13709 /* Successful termination. */
13710 
13711 L5:
13712     *nit = iter;
13713     *ier = 0;
13714     return 0;
13715 
13716 /* MAXIT iterations performed without convergence. */
13717 
13718 L6:
13719     *nit = maxit;
13720     *ier = 1;
13721     return 0;
13722 
13723 /* Invalid input parameter. */
13724 
13725 L7:
13726     *nit = 0;
13727     *ier = 2;
13728     return 0;
13729 
13730 /* IO2 is not a neighbor of IO1. */
13731 
13732 L8:
13733     *nit = iter;
13734     *ier = 3;
13735     return 0;
13736 
13737 /* Zero pointer returned by SWAP. */
13738 
13739 L9:
13740     *nit = iter;
13741     *ier = 4;
13742     return 0;
13743 } /* optim_ */
13744 
13745 /* Subroutine */ int projct_(double *px, double *py, double *pz,
13746         double *ox, double *oy, double *oz, double *ex,
13747         double *ey, double *ez, double *vx, double *vy,
13748         double *vz, long int *init, double *x, double *y,
13749         double *z__, int *ier)
13750 {
13751     /* Builtin functions */
13752     //double sqrt(double);
13753 
13754     /* Local variables */
13755     static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13756             oes, xoe, yoe, zoe, xep, yep, zep;
13757 
13758 
13759 /* *********************************************************** */
13760 
13761 /*                        From PLTPACK, SCRPLOT, and STRIPACK */
13762 /*                                            Robert J. Renka */
13763 /*                                  Dept. of Computer Science */
13764 /*                                       Univ. of North Texas */
13765 /*                                           renka@cs.unt.edu */
13766 /*                                                   07/18/90 */
13767 
13768 /*   Given a projection plane and associated coordinate sys- */
13769 /* tem defined by an origin O, eye position E, and up-vector */
13770 /* V, this subroutine applies a perspective depth transform- */
13771 /* ation T to a point P = (PX,PY,PZ), returning the point */
13772 /* T(P) = (X,Y,Z), where X and Y are the projection plane */
13773 /* coordinates of the point that lies in the projection */
13774 /* plane and on the line defined by P and E, and Z is the */
13775 /* depth associated with P. */
13776 
13777 /*   The projection plane is defined to be the plane that */
13778 /* contains O and has normal defined by O and E. */
13779 
13780 /*   The depth Z is defined in such a way that Z < 1, T maps */
13781 /* lines to lines (and planes to planes), and if two distinct */
13782 /* points have the same projection plane coordinates, then */
13783 /* the one closer to E has a smaller depth.  (Z increases */
13784 /* monotonically with orthogonal distance from P to the plane */
13785 /* that is parallel to the projection plane and contains E.) */
13786 /* This depth value facilitates depth sorting and depth buf- */
13787 /* fer methods. */
13788 
13789 
13790 /* On input: */
13791 
13792 /*       PX,PY,PZ = Cartesian coordinates of the point P to */
13793 /*                  be mapped onto the projection plane.  The */
13794 /*                  half line that contains P and has end- */
13795 /*                  point at E must intersect the plane. */
13796 
13797 /*       OX,OY,OZ = Coordinates of O (the origin of a coordi- */
13798 /*                  nate system in the projection plane).  A */
13799 /*                  reasonable value for O is a point near */
13800 /*                  the center of an object or scene to be */
13801 /*                  viewed. */
13802 
13803 /*       EX,EY,EZ = Coordinates of the eye-position E defin- */
13804 /*                  ing the normal to the plane and the line */
13805 /*                  of sight for the projection.  E must not */
13806 /*                  coincide with O or P, and the angle be- */
13807 /*                  tween the vectors O-E and P-E must be */
13808 /*                  less than 90 degrees.  Note that E and P */
13809 /*                  may lie on opposite sides of the projec- */
13810 /*                  tion plane. */
13811 
13812 /*       VX,VY,VZ = Coordinates of a point V which defines */
13813 /*                  the positive Y axis of an X-Y coordinate */
13814 /*                  system in the projection plane as the */
13815 /*                  half-line containing O and the projection */
13816 /*                  of O+V onto the plane.  The positive X */
13817 /*                  axis has direction defined by the cross */
13818 /*                  product V X (E-O). */
13819 
13820 /* The above parameters are not altered by this routine. */
13821 
13822 /*       INIT = long int switch which must be set to TRUE on */
13823 /*              the first call and when the values of O, E, */
13824 /*              or V have been altered since a previous call. */
13825 /*              If INIT = FALSE, it is assumed that only the */
13826 /*              coordinates of P have changed since a previ- */
13827 /*              ous call.  Previously stored quantities are */
13828 /*              used for increased efficiency in this case. */
13829 
13830 /* On output: */
13831 
13832 /*       INIT = Switch with value reset to FALSE if IER = 0. */
13833 
13834 /*       X,Y = Projection plane coordinates of the point */
13835 /*             that lies in the projection plane and on the */
13836 /*             line defined by E and P.  X and Y are not */
13837 /*             altered if IER .NE. 0. */
13838 
13839 /*       Z = Depth value defined above unless IER .NE. 0. */
13840 
13841 /*       IER = Error indicator. */
13842 /*             IER = 0 if no errors were encountered. */
13843 /*             IER = 1 if the inner product of O-E with P-E */
13844 /*                     is not positive, implying that E is */
13845 /*                     too close to the plane. */
13846 /*             IER = 2 if O, E, and O+V are collinear.  See */
13847 /*                     the description of VX,VY,VZ. */
13848 
13849 /* Modules required by PROJCT:  None */
13850 
13851 /* Intrinsic function called by PROJCT:  SQRT */
13852 
13853 /* *********************************************************** */
13854 
13855 
13856 /* Local parameters: */
13857 
13858 /* OES =         Norm squared of OE -- inner product (OE,OE) */
13859 /* S =           Scale factor for computing projections */
13860 /* SC =          Scale factor for normalizing VN and HN */
13861 /* XE,YE,ZE =    Local copies of EX, EY, EZ */
13862 /* XEP,YEP,ZEP = Components of the vector EP from E to P */
13863 /* XH,YH,ZH =    Components of a unit vector HN defining the */
13864 /*                 positive X-axis in the plane */
13865 /* XOE,YOE,ZOE = Components of the vector OE from O to E */
13866 /* XV,YV,ZV =    Components of a unit vector VN defining the */
13867 /*                 positive Y-axis in the plane */
13868 /* XW,YW,ZW =    Components of the vector W from O to the */
13869 /*                 projection of P onto the plane */
13870 
13871     if (*init) {
13872 
13873 /* Compute parameters defining the transformation: */
13874 /*   17 adds, 27 multiplies, 3 divides, 2 compares, and */
13875 /*   2 square roots. */
13876 
13877 /* Set the coordinates of E to local variables, compute */
13878 /*   OE = E-O and OES, and test for OE = 0. */
13879 
13880         xe = *ex;
13881         ye = *ey;
13882         ze = *ez;
13883         xoe = xe - *ox;
13884         yoe = ye - *oy;
13885         zoe = ze - *oz;
13886         oes = xoe * xoe + yoe * yoe + zoe * zoe;
13887         if (oes == 0.) {
13888             goto L1;
13889         }
13890 
13891 /* Compute S = (OE,V)/OES and VN = V - S*OE. */
13892 
13893         s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
13894         xv = *vx - s * xoe;
13895         yv = *vy - s * yoe;
13896         zv = *vz - s * zoe;
13897 
13898 /* Normalize VN to a unit vector. */
13899 
13900         sc = xv * xv + yv * yv + zv * zv;
13901         if (sc == 0.) {
13902             goto L2;
13903         }
13904         sc = 1. / sqrt(sc);
13905         xv = sc * xv;
13906         yv = sc * yv;
13907         zv = sc * zv;
13908 
13909 /* Compute HN = VN X OE (normalized). */
13910 
13911         xh = yv * zoe - yoe * zv;
13912         yh = xoe * zv - xv * zoe;
13913         zh = xv * yoe - xoe * yv;
13914         sc = sqrt(xh * xh + yh * yh + zh * zh);
13915         if (sc == 0.) {
13916             goto L2;
13917         }
13918         sc = 1. / sc;
13919         xh = sc * xh;
13920         yh = sc * yh;
13921         zh = sc * zh;
13922     }
13923 
13924 /* Apply the transformation:  13 adds, 12 multiplies, */
13925 /*                            1 divide, and 1 compare. */
13926 
13927 /* Compute EP = P-E, S = OES/(OE,EP), and W = OE - S*EP. */
13928 
13929     xep = *px - xe;
13930     yep = *py - ye;
13931     zep = *pz - ze;
13932     s = xoe * xep + yoe * yep + zoe * zep;
13933     if (s >= 0.) {
13934         goto L1;
13935     }
13936     s = oes / s;
13937     xw = xoe - s * xep;
13938     yw = yoe - s * yep;
13939     zw = zoe - s * zep;
13940 
13941 /* Map W into X = (W,HN), Y = (W,VN), compute Z = 1+S, and */
13942 /*   reset INIT. */
13943 
13944     *x = xw * xh + yw * yh + zw * zh;
13945     *y = xw * xv + yw * yv + zw * zv;
13946     *z__ = s + 1.;
13947     *init = FALSE_;
13948     *ier = 0;
13949     return 0;
13950 
13951 /* (OE,EP) .GE. 0. */
13952 
13953 L1:
13954     *ier = 1;
13955     return 0;
13956 
13957 /* O, E, and O+V are collinear. */
13958 
13959 L2:
13960     *ier = 2;
13961     return 0;
13962 } /* projct_ */
13963 
13964 /* Subroutine */ int scoord_(double *px, double *py, double *pz,
13965         double *plat, double *plon, double *pnrm)
13966 {
13967     /* Builtin functions */
13968     //double sqrt(double), atan2(double, double), asin(double);
13969 
13970 
13971 /* *********************************************************** */
13972 
13973 /*                                              From STRIPACK */
13974 /*                                            Robert J. Renka */
13975 /*                                  Dept. of Computer Science */
13976 /*                                       Univ. of North Texas */
13977 /*                                           renka@cs.unt.edu */
13978 /*                                                   08/27/90 */
13979 
13980 /*   This subroutine converts a point P from Cartesian coor- */
13981 /* dinates to spherical coordinates. */
13982 
13983 
13984 /* On input: */
13985 
13986 /*       PX,PY,PZ = Cartesian coordinates of P. */
13987 
13988 /* Input parameters are not altered by this routine. */
13989 
13990 /* On output: */
13991 
13992 /*       PLAT = Latitude of P in the range -PI/2 to PI/2, or */
13993 /*              0 if PNRM = 0.  PLAT should be scaled by */
13994 /*              180/PI to obtain the value in degrees. */
13995 
13996 /*       PLON = Longitude of P in the range -PI to PI, or 0 */
13997 /*              if P lies on the Z-axis.  PLON should be */
13998 /*              scaled by 180/PI to obtain the value in */
13999 /*              degrees. */
14000 
14001 /*       PNRM = Magnitude (Euclidean norm) of P. */
14002 
14003 /* Modules required by SCOORD:  None */
14004 
14005 /* Intrinsic functions called by SCOORD:  ASIN, ATAN2, SQRT */
14006 
14007 /* *********************************************************** */
14008 
14009     *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14010     if (*px != 0. || *py != 0.) {
14011         *plon = atan2(*py, *px);
14012     } else {
14013         *plon = 0.;
14014     }
14015     if (*pnrm != 0.) {
14016         *plat = asin(*pz / *pnrm);
14017     } else {
14018         *plat = 0.;
14019     }
14020     return 0;
14021 } /* scoord_ */
14022 
14023 double store_(double *x)
14024 {
14025     /* System generated locals */
14026     double ret_val;
14027 
14028 
14029 /* *********************************************************** */
14030 
14031 /*                                              From STRIPACK */
14032 /*                                            Robert J. Renka */
14033 /*                                  Dept. of Computer Science */
14034 /*                                       Univ. of North Texas */
14035 /*                                           renka@cs.unt.edu */
14036 /*                                                   05/09/92 */
14037 
14038 /*   This function forces its argument X to be stored in a */
14039 /* memory location, thus providing a means of determining */
14040 /* floating point number characteristics (such as the machine */
14041 /* precision) when it is necessary to avoid computation in */
14042 /* high precision registers. */
14043 
14044 
14045 /* On input: */
14046 
14047 /*       X = Value to be stored. */
14048 
14049 /* X is not altered by this function. */
14050 
14051 /* On output: */
14052 
14053 /*       STORE = Value of X after it has been stored and */
14054 /*               possibly truncated or rounded to the single */
14055 /*               precision word length. */
14056 
14057 /* Modules required by STORE:  None */
14058 
14059 /* *********************************************************** */
14060 
14061     stcom_1.y = *x;
14062     ret_val = stcom_1.y;
14063     return ret_val;
14064 } /* store_ */
14065 
14066 /* Subroutine */ int swap_(int *in1, int *in2, int *io1, int *
14067         io2, int *list, int *lptr, int *lend, int *lp21)
14068 {
14069     /* System generated locals */
14070     int i__1;
14071 
14072     /* Local variables */
14073     static int lp, lph, lpsav;
14074     extern int lstptr_(int *, int *, int *, int *);
14075 
14076 
14077 /* *********************************************************** */
14078 
14079 /*                                              From STRIPACK */
14080 /*                                            Robert J. Renka */
14081 /*                                  Dept. of Computer Science */
14082 /*                                       Univ. of North Texas */
14083 /*                                           renka@cs.unt.edu */
14084 /*                                                   06/22/98 */
14085 
14086 /*   Given a triangulation of a set of points on the unit */
14087 /* sphere, this subroutine replaces a diagonal arc in a */
14088 /* strictly convex quadrilateral (defined by a pair of adja- */
14089 /* cent triangles) with the other diagonal.  Equivalently, a */
14090 /* pair of adjacent triangles is replaced by another pair */
14091 /* having the same union. */
14092 
14093 
14094 /* On input: */
14095 
14096 /*       IN1,IN2,IO1,IO2 = Nodal indexes of the vertices of */
14097 /*                         the quadrilateral.  IO1-IO2 is re- */
14098 /*                         placed by IN1-IN2.  (IO1,IO2,IN1) */
14099 /*                         and (IO2,IO1,IN2) must be trian- */
14100 /*                         gles on input. */
14101 
14102 /* The above parameters are not altered by this routine. */
14103 
14104 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14105 /*                        gulation.  Refer to Subroutine */
14106 /*                        TRMESH. */
14107 
14108 /* On output: */
14109 
14110 /*       LIST,LPTR,LEND = Data structure updated with the */
14111 /*                        swap -- triangles (IO1,IO2,IN1) and */
14112 /*                        (IO2,IO1,IN2) are replaced by */
14113 /*                        (IN1,IN2,IO2) and (IN2,IN1,IO1) */
14114 /*                        unless LP21 = 0. */
14115 
14116 /*       LP21 = Index of IN1 as a neighbor of IN2 after the */
14117 /*              swap is performed unless IN1 and IN2 are */
14118 /*              adjacent on input, in which case LP21 = 0. */
14119 
14120 /* Module required by SWAP:  LSTPTR */
14121 
14122 /* Intrinsic function called by SWAP:  ABS */
14123 
14124 /* *********************************************************** */
14125 
14126 
14127 /* Local parameters: */
14128 
14129 /* LP,LPH,LPSAV = LIST pointers */
14130 
14131 
14132 /* Test for IN1 and IN2 adjacent. */
14133 
14134     /* Parameter adjustments */
14135     --lend;
14136     --lptr;
14137     --list;
14138 
14139     /* Function Body */
14140     lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14141     if ((i__1 = list[lp], abs(i__1)) == *in2) {
14142         *lp21 = 0;
14143         return 0;
14144     }
14145 
14146 /* Delete IO2 as a neighbor of IO1. */
14147 
14148     lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14149     lph = lptr[lp];
14150     lptr[lp] = lptr[lph];
14151 
14152 /* If IO2 is the last neighbor of IO1, make IN2 the */
14153 /*   last neighbor. */
14154 
14155     if (lend[*io1] == lph) {
14156         lend[*io1] = lp;
14157     }
14158 
14159 /* Insert IN2 as a neighbor of IN1 following IO1 */
14160 /*   using the hole created above. */
14161 
14162     lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14163     lpsav = lptr[lp];
14164     lptr[lp] = lph;
14165     list[lph] = *in2;
14166     lptr[lph] = lpsav;
14167 
14168 /* Delete IO1 as a neighbor of IO2. */
14169 
14170     lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14171     lph = lptr[lp];
14172     lptr[lp] = lptr[lph];
14173 
14174 /* If IO1 is the last neighbor of IO2, make IN1 the */
14175 /*   last neighbor. */
14176 
14177     if (lend[*io2] == lph) {
14178         lend[*io2] = lp;
14179     }
14180 
14181 /* Insert IN1 as a neighbor of IN2 following IO2. */
14182 
14183     lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14184     lpsav = lptr[lp];
14185     lptr[lp] = lph;
14186     list[lph] = *in1;
14187     lptr[lph] = lpsav;
14188     *lp21 = lph;
14189     return 0;
14190 } /* swap_ */
14191 
14192 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14193         double *x, double *y, double *z__)
14194 {
14195     /* System generated locals */
14196     long int ret_val;
14197 
14198     /* Local variables */
14199     static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14200 
14201 
14202 /* *********************************************************** */
14203 
14204 /*                                              From STRIPACK */
14205 /*                                            Robert J. Renka */
14206 /*                                  Dept. of Computer Science */
14207 /*                                       Univ. of North Texas */
14208 /*                                           renka@cs.unt.edu */
14209 /*                                                   03/29/91 */
14210 
14211 /*   This function decides whether or not to replace a */
14212 /* diagonal arc in a quadrilateral with the other diagonal. */
14213 /* The decision will be to swap (SWPTST = TRUE) if and only */
14214 /* if N4 lies above the plane (in the half-space not contain- */
14215 /* ing the origin) defined by (N1,N2,N3), or equivalently, if */
14216 /* the projection of N4 onto this plane is interior to the */
14217 /* circumcircle of (N1,N2,N3).  The decision will be for no */
14218 /* swap if the quadrilateral is not strictly convex. */
14219 
14220 
14221 /* On input: */
14222 
14223 /*       N1,N2,N3,N4 = Indexes of the four nodes defining the */
14224 /*                     quadrilateral with N1 adjacent to N2, */
14225 /*                     and (N1,N2,N3) in counterclockwise */
14226 /*                     order.  The arc connecting N1 to N2 */
14227 /*                     should be replaced by an arc connec- */
14228 /*                     ting N3 to N4 if SWPTST = TRUE.  Refer */
14229 /*                     to Subroutine SWAP. */
14230 
14231 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14232 /*               coordinates of the nodes.  (X(I),Y(I),Z(I)) */
14233 /*               define node I for I = N1, N2, N3, and N4. */
14234 
14235 /* Input parameters are not altered by this routine. */
14236 
14237 /* On output: */
14238 
14239 /*       SWPTST = TRUE if and only if the arc connecting N1 */
14240 /*                and N2 should be swapped for an arc con- */
14241 /*                necting N3 and N4. */
14242 
14243 /* Modules required by SWPTST:  None */
14244 
14245 /* *********************************************************** */
14246 
14247 
14248 /* Local parameters: */
14249 
14250 /* DX1,DY1,DZ1 = Coordinates of N4->N1 */
14251 /* DX2,DY2,DZ2 = Coordinates of N4->N2 */
14252 /* DX3,DY3,DZ3 = Coordinates of N4->N3 */
14253 /* X4,Y4,Z4 =    Coordinates of N4 */
14254 
14255     /* Parameter adjustments */
14256     --z__;
14257     --y;
14258     --x;
14259 
14260     /* Function Body */
14261     x4 = x[*n4];
14262     y4 = y[*n4];
14263     z4 = z__[*n4];
14264     dx1 = x[*n1] - x4;
14265     dx2 = x[*n2] - x4;
14266     dx3 = x[*n3] - x4;
14267     dy1 = y[*n1] - y4;
14268     dy2 = y[*n2] - y4;
14269     dy3 = y[*n3] - y4;
14270     dz1 = z__[*n1] - z4;
14271     dz2 = z__[*n2] - z4;
14272     dz3 = z__[*n3] - z4;
14273 
14274 /* N4 lies above the plane of (N1,N2,N3) iff N3 lies above */
14275 /*   the plane of (N2,N1,N4) iff Det(N3-N4,N2-N4,N1-N4) = */
14276 /*   (N3-N4,N2-N4 X N1-N4) > 0. */
14277 
14278     ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14279             dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14280     return ret_val;
14281 } /* swptst_ */
14282 
14283 /* Subroutine */ int trans_(int *n, double *rlat, double *rlon,
14284         double *x, double *y, double *z__)
14285 {
14286     /* System generated locals */
14287     int i__1;
14288 
14289     /* Builtin functions */
14290     //double cos(double), sin(double);
14291 
14292     /* Local variables */
14293     static int i__, nn;
14294     static double phi, theta, cosphi;
14295 
14296 
14297 /* *********************************************************** */
14298 
14299 /*                                              From STRIPACK */
14300 /*                                            Robert J. Renka */
14301 /*                                  Dept. of Computer Science */
14302 /*                                       Univ. of North Texas */
14303 /*                                           renka@cs.unt.edu */
14304 /*                                                   04/08/90 */
14305 
14306 /*   This subroutine transforms spherical coordinates into */
14307 /* Cartesian coordinates on the unit sphere for input to */
14308 /* Subroutine TRMESH.  Storage for X and Y may coincide with */
14309 /* storage for RLAT and RLON if the latter need not be saved. */
14310 
14311 
14312 /* On input: */
14313 
14314 /*       N = Number of nodes (points on the unit sphere) */
14315 /*           whose coordinates are to be transformed. */
14316 
14317 /*       RLAT = Array of length N containing latitudinal */
14318 /*              coordinates of the nodes in radians. */
14319 
14320 /*       RLON = Array of length N containing longitudinal */
14321 /*              coordinates of the nodes in radians. */
14322 
14323 /* The above parameters are not altered by this routine. */
14324 
14325 /*       X,Y,Z = Arrays of length at least N. */
14326 
14327 /* On output: */
14328 
14329 /*       X,Y,Z = Cartesian coordinates in the range -1 to 1. */
14330 /*               X(I)**2 + Y(I)**2 + Z(I)**2 = 1 for I = 1 */
14331 /*               to N. */
14332 
14333 /* Modules required by TRANS:  None */
14334 
14335 /* Intrinsic functions called by TRANS:  COS, SIN */
14336 
14337 /* *********************************************************** */
14338 
14339 
14340 /* Local parameters: */
14341 
14342 /* COSPHI = cos(PHI) */
14343 /* I =      DO-loop index */
14344 /* NN =     Local copy of N */
14345 /* PHI =    Latitude */
14346 /* THETA =  Longitude */
14347 
14348     /* Parameter adjustments */
14349     --z__;
14350     --y;
14351     --x;
14352     --rlon;
14353     --rlat;
14354 
14355     /* Function Body */
14356     nn = *n;
14357     i__1 = nn;
14358     for (i__ = 1; i__ <= i__1; ++i__) {
14359         phi = rlat[i__];
14360         theta = rlon[i__];
14361         cosphi = cos(phi);
14362         x[i__] = cosphi * cos(theta);
14363         y[i__] = cosphi * sin(theta);
14364         z__[i__] = sin(phi);
14365 /* L1: */
14366     }
14367     return 0;
14368 } /* trans_ */
14369 
14370 /* Subroutine */ int trfind_(int *nst, double *p, int *n,
14371         double *x, double *y, double *z__, int *list, int
14372         *lptr, int *lend, double *b1, double *b2, double *b3,
14373         int *i1, int *i2, int *i3)
14374 {
14375     /* Initialized data */
14376 
14377     static int ix = 1;
14378     static int iy = 2;
14379     static int iz = 3;
14380 
14381     /* System generated locals */
14382     int i__1;
14383     double d__1, d__2;
14384 
14385     /* Local variables */
14386     static double q[3];
14387     static int n0, n1, n2, n3, n4, nf;
14388     static double s12;
14389     static int nl, lp;
14390     static double xp, yp, zp;
14391     static int n1s, n2s;
14392     static double eps, tol, ptn1, ptn2;
14393     static int next;
14394     extern int jrand_(int *, int *, int *, int *);
14395     extern double store_(double *);
14396     extern int lstptr_(int *, int *, int *, int *);
14397 
14398 
14399 /* *********************************************************** */
14400 
14401 /*                                              From STRIPACK */
14402 /*                                            Robert J. Renka */
14403 /*                                  Dept. of Computer Science */
14404 /*                                       Univ. of North Texas */
14405 /*                                           renka@cs.unt.edu */
14406 /*                                                   11/30/99 */
14407 
14408 /*   This subroutine locates a point P relative to a triangu- */
14409 /* lation created by Subroutine TRMESH.  If P is contained in */
14410 /* a triangle, the three vertex indexes and barycentric coor- */
14411 /* dinates are returned.  Otherwise, the indexes of the */
14412 /* visible boundary nodes are returned. */
14413 
14414 
14415 /* On input: */
14416 
14417 /*       NST = Index of a node at which TRFIND begins its */
14418 /*             search.  Search time depends on the proximity */
14419 /*             of this node to P. */
14420 
14421 /*       P = Array of length 3 containing the x, y, and z */
14422 /*           coordinates (in that order) of the point P to be */
14423 /*           located. */
14424 
14425 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14426 
14427 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14428 /*               coordinates of the triangulation nodes (unit */
14429 /*               vectors).  (X(I),Y(I),Z(I)) defines node I */
14430 /*               for I = 1 to N. */
14431 
14432 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14433 /*                        gulation.  Refer to Subroutine */
14434 /*                        TRMESH. */
14435 
14436 /* Input parameters are not altered by this routine. */
14437 
14438 /* On output: */
14439 
14440 /*       B1,B2,B3 = Unnormalized barycentric coordinates of */
14441 /*                  the central projection of P onto the un- */
14442 /*                  derlying planar triangle if P is in the */
14443 /*                  convex hull of the nodes.  These parame- */
14444 /*                  ters are not altered if I1 = 0. */
14445 
14446 /*       I1,I2,I3 = Counterclockwise-ordered vertex indexes */
14447 /*                  of a triangle containing P if P is con- */
14448 /*                  tained in a triangle.  If P is not in the */
14449 /*                  convex hull of the nodes, I1 and I2 are */
14450 /*                  the rightmost and leftmost (boundary) */
14451 /*                  nodes that are visible from P, and */
14452 /*                  I3 = 0.  (If all boundary nodes are vis- */
14453 /*                  ible from P, then I1 and I2 coincide.) */
14454 /*                  I1 = I2 = I3 = 0 if P and all of the */
14455 /*                  nodes are coplanar (lie on a common great */
14456 /*                  circle. */
14457 
14458 /* Modules required by TRFIND:  JRAND, LSTPTR, STORE */
14459 
14460 /* Intrinsic function called by TRFIND:  ABS */
14461 
14462 /* *********************************************************** */
14463 
14464 
14465     /* Parameter adjustments */
14466     --p;
14467     --lend;
14468     --z__;
14469     --y;
14470     --x;
14471     --list;
14472     --lptr;
14473 
14474     /* Function Body */
14475 
14476 /* Local parameters: */
14477 
14478 /* EPS =      Machine precision */
14479 /* IX,IY,IZ = int seeds for JRAND */
14480 /* LP =       LIST pointer */
14481 /* N0,N1,N2 = Nodes in counterclockwise order defining a */
14482 /*              cone (with vertex N0) containing P, or end- */
14483 /*              points of a boundary edge such that P Right */
14484 /*              N1->N2 */
14485 /* N1S,N2S =  Initially-determined values of N1 and N2 */
14486 /* N3,N4 =    Nodes opposite N1->N2 and N2->N1, respectively */
14487 /* NEXT =     Candidate for I1 or I2 when P is exterior */
14488 /* NF,NL =    First and last neighbors of N0, or first */
14489 /*              (rightmost) and last (leftmost) nodes */
14490 /*              visible from P when P is exterior to the */
14491 /*              triangulation */
14492 /* PTN1 =     Scalar product <P,N1> */
14493 /* PTN2 =     Scalar product <P,N2> */
14494 /* Q =        (N2 X N1) X N2  or  N1 X (N2 X N1) -- used in */
14495 /*              the boundary traversal when P is exterior */
14496 /* S12 =      Scalar product <N1,N2> */
14497 /* TOL =      Tolerance (multiple of EPS) defining an upper */
14498 /*              bound on the magnitude of a negative bary- */
14499 /*              centric coordinate (B1 or B2) for P in a */
14500 /*              triangle -- used to avoid an infinite number */
14501 /*              of restarts with 0 <= B3 < EPS and B1 < 0 or */
14502 /*              B2 < 0 but small in magnitude */
14503 /* XP,YP,ZP = Local variables containing P(1), P(2), and P(3) */
14504 /* X0,Y0,Z0 = Dummy arguments for DET */
14505 /* X1,Y1,Z1 = Dummy arguments for DET */
14506 /* X2,Y2,Z2 = Dummy arguments for DET */
14507 
14508 /* Statement function: */
14509 
14510 /* DET(X1,...,Z0) .GE. 0 if and only if (X0,Y0,Z0) is in the */
14511 /*                       (closed) left hemisphere defined by */
14512 /*                       the plane containing (0,0,0), */
14513 /*                       (X1,Y1,Z1), and (X2,Y2,Z2), where */
14514 /*                       left is defined relative to an ob- */
14515 /*                       server at (X1,Y1,Z1) facing */
14516 /*                       (X2,Y2,Z2). */
14517 
14518 
14519 /* Initialize variables. */
14520 
14521     xp = p[1];
14522     yp = p[2];
14523     zp = p[3];
14524     n0 = *nst;
14525     if (n0 < 1 || n0 > *n) {
14526         n0 = jrand_(n, &ix, &iy, &iz);
14527     }
14528 
14529 /* Compute the relative machine precision EPS and TOL. */
14530 
14531     eps = 1.;
14532 L1:
14533     eps /= 2.;
14534     d__1 = eps + 1.;
14535     if (store_(&d__1) > 1.) {
14536         goto L1;
14537     }
14538     eps *= 2.;
14539     tol = eps * 4.;
14540 
14541 /* Set NF and NL to the first and last neighbors of N0, and */
14542 /*   initialize N1 = NF. */
14543 
14544 L2:
14545     lp = lend[n0];
14546     nl = list[lp];
14547     lp = lptr[lp];
14548     nf = list[lp];
14549     n1 = nf;
14550 
14551 /* Find a pair of adjacent neighbors N1,N2 of N0 that define */
14552 /*   a wedge containing P:  P LEFT N0->N1 and P RIGHT N0->N2. */
14553 
14554     if (nl > 0) {
14555 
14556 /*   N0 is an interior node.  Find N1. */
14557 
14558 L3:
14559         if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14560                 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14561                 -1e-10) {
14562             lp = lptr[lp];
14563             n1 = list[lp];
14564             if (n1 == nl) {
14565                 goto L6;
14566             }
14567             goto L3;
14568         }
14569     } else {
14570 
14571 /*   N0 is a boundary node.  Test for P exterior. */
14572 
14573         nl = -nl;
14574         if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14575                 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14576                 -1e-10) {
14577 
14578 /*   P is to the right of the boundary edge N0->NF. */
14579 
14580             n1 = n0;
14581             n2 = nf;
14582             goto L9;
14583         }
14584         if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14585                 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14586                 -1e-10) {
14587 
14588 /*   P is to the right of the boundary edge NL->N0. */
14589 
14590             n1 = nl;
14591             n2 = n0;
14592             goto L9;
14593         }
14594     }
14595 
14596 /* P is to the left of arcs N0->N1 and NL->N0.  Set N2 to the */
14597 /*   next neighbor of N0 (following N1). */
14598 
14599 L4:
14600     lp = lptr[lp];
14601     n2 = (i__1 = list[lp], abs(i__1));
14602     if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14603             n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14604         goto L7;
14605     }
14606     n1 = n2;
14607     if (n1 != nl) {
14608         goto L4;
14609     }
14610     if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14611             nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14612         goto L6;
14613     }
14614 
14615 /* P is left of or on arcs N0->NB for all neighbors NB */
14616 /*   of N0.  Test for P = +/-N0. */
14617 
14618     d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14619     if (store_(&d__2) < 1. - eps * 4.) {
14620 
14621 /*   All points are collinear iff P Left NB->N0 for all */
14622 /*     neighbors NB of N0.  Search the neighbors of N0. */
14623 /*     Note:  N1 = NL and LP points to NL. */
14624 
14625 L5:
14626         if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14627                 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14628                 -1e-10) {
14629             lp = lptr[lp];
14630             n1 = (i__1 = list[lp], abs(i__1));
14631             if (n1 == nl) {
14632                 goto L14;
14633             }
14634             goto L5;
14635         }
14636     }
14637 
14638 /* P is to the right of N1->N0, or P = +/-N0.  Set N0 to N1 */
14639 /*   and start over. */
14640 
14641     n0 = n1;
14642     goto L2;
14643 
14644 /* P is between arcs N0->N1 and N0->NF. */
14645 
14646 L6:
14647     n2 = nf;
14648 
14649 /* P is contained in a wedge defined by geodesics N0-N1 and */
14650 /*   N0-N2, where N1 is adjacent to N2.  Save N1 and N2 to */
14651 /*   test for cycling. */
14652 
14653 L7:
14654     n3 = n0;
14655     n1s = n1;
14656     n2s = n2;
14657 
14658 /* Top of edge-hopping loop: */
14659 
14660 L8:
14661 
14662     *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14663             x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14664      if (*b3 < -1e-10) {
14665 
14666 /*   Set N4 to the first neighbor of N2 following N1 (the */
14667 /*     node opposite N2->N1) unless N1->N2 is a boundary arc. */
14668 
14669         lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14670         if (list[lp] < 0) {
14671             goto L9;
14672         }
14673         lp = lptr[lp];
14674         n4 = (i__1 = list[lp], abs(i__1));
14675 
14676 /*   Define a new arc N1->N2 which intersects the geodesic */
14677 /*     N0-P. */
14678         if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14679                 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14680                 -1e-10) {
14681             n3 = n2;
14682             n2 = n4;
14683             n1s = n1;
14684             if (n2 != n2s && n2 != n0) {
14685                 goto L8;
14686             }
14687         } else {
14688             n3 = n1;
14689             n1 = n4;
14690             n2s = n2;
14691             if (n1 != n1s && n1 != n0) {
14692                 goto L8;
14693             }
14694         }
14695 
14696 /*   The starting node N0 or edge N1-N2 was encountered */
14697 /*     again, implying a cycle (infinite loop).  Restart */
14698 /*     with N0 randomly selected. */
14699 
14700         n0 = jrand_(n, &ix, &iy, &iz);
14701         goto L2;
14702     }
14703 
14704 /* P is in (N1,N2,N3) unless N0, N1, N2, and P are collinear */
14705 /*   or P is close to -N0. */
14706 
14707     if (*b3 >= eps) {
14708 
14709 /*   B3 .NE. 0. */
14710 
14711         *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14712                  - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14713         *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14714                  - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14715         if (*b1 < -tol || *b2 < -tol) {
14716 
14717 /*   Restart with N0 randomly selected. */
14718 
14719             n0 = jrand_(n, &ix, &iy, &iz);
14720             goto L2;
14721         }
14722     } else {
14723 
14724 /*   B3 = 0 and thus P lies on N1->N2. Compute */
14725 /*     B1 = Det(P,N2 X N1,N2) and B2 = Det(P,N1,N2 X N1). */
14726 
14727         *b3 = 0.;
14728         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14729         ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14730         ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14731         *b1 = ptn1 - s12 * ptn2;
14732         *b2 = ptn2 - s12 * ptn1;
14733         if (*b1 < -tol || *b2 < -tol) {
14734 
14735 /*   Restart with N0 randomly selected. */
14736 
14737             n0 = jrand_(n, &ix, &iy, &iz);
14738             goto L2;
14739         }
14740     }
14741 
14742 /* P is in (N1,N2,N3). */
14743 
14744     *i1 = n1;
14745     *i2 = n2;
14746     *i3 = n3;
14747     if (*b1 < 0.f) {
14748         *b1 = 0.f;
14749     }
14750     if (*b2 < 0.f) {
14751         *b2 = 0.f;
14752     }
14753     return 0;
14754 
14755 /* P Right N1->N2, where N1->N2 is a boundary edge. */
14756 /*   Save N1 and N2, and set NL = 0 to indicate that */
14757 /*   NL has not yet been found. */
14758 
14759 L9:
14760     n1s = n1;
14761     n2s = n2;
14762     nl = 0;
14763 
14764 /*           Counterclockwise Boundary Traversal: */
14765 
14766 L10:
14767 
14768     lp = lend[n2];
14769     lp = lptr[lp];
14770     next = list[lp];
14771      if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14772              - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14773             >= -1e-10) {
14774 
14775 /*   N2 is the rightmost visible node if P Forward N2->N1 */
14776 /*     or NEXT Forward N2->N1.  Set Q to (N2 X N1) X N2. */
14777 
14778         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14779         q[0] = x[n1] - s12 * x[n2];
14780         q[1] = y[n1] - s12 * y[n2];
14781         q[2] = z__[n1] - s12 * z__[n2];
14782         if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14783             goto L11;
14784         }
14785         if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14786             goto L11;
14787         }
14788 
14789 /*   N1, N2, NEXT, and P are nearly collinear, and N2 is */
14790 /*     the leftmost visible node. */
14791 
14792         nl = n2;
14793     }
14794 
14795 /* Bottom of counterclockwise loop: */
14796 
14797     n1 = n2;
14798     n2 = next;
14799     if (n2 != n1s) {
14800         goto L10;
14801     }
14802 
14803 /* All boundary nodes are visible from P. */
14804 
14805     *i1 = n1s;
14806     *i2 = n1s;
14807     *i3 = 0;
14808     return 0;
14809 
14810 /* N2 is the rightmost visible node. */
14811 
14812 L11:
14813     nf = n2;
14814     if (nl == 0) {
14815 
14816 /* Restore initial values of N1 and N2, and begin the search */
14817 /*   for the leftmost visible node. */
14818 
14819         n2 = n2s;
14820         n1 = n1s;
14821 
14822 /*           Clockwise Boundary Traversal: */
14823 
14824 L12:
14825         lp = lend[n1];
14826         next = -list[lp];
14827         if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14828                 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14829                  y[next]) >= -1e-10) {
14830 
14831 /*   N1 is the leftmost visible node if P or NEXT is */
14832 /*     forward of N1->N2.  Compute Q = N1 X (N2 X N1). */
14833 
14834             s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14835             q[0] = x[n2] - s12 * x[n1];
14836             q[1] = y[n2] - s12 * y[n1];
14837             q[2] = z__[n2] - s12 * z__[n1];
14838             if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14839                 goto L13;
14840             }
14841             if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14842                 goto L13;
14843             }
14844 
14845 /*   P, NEXT, N1, and N2 are nearly collinear and N1 is the */
14846 /*     rightmost visible node. */
14847 
14848             nf = n1;
14849         }
14850 
14851 /* Bottom of clockwise loop: */
14852 
14853         n2 = n1;
14854         n1 = next;
14855         if (n1 != n1s) {
14856             goto L12;
14857         }
14858 
14859 /* All boundary nodes are visible from P. */
14860 
14861         *i1 = n1;
14862         *i2 = n1;
14863         *i3 = 0;
14864         return 0;
14865 
14866 /* N1 is the leftmost visible node. */
14867 
14868 L13:
14869         nl = n1;
14870     }
14871 
14872 /* NF and NL have been found. */
14873 
14874     *i1 = nf;
14875     *i2 = nl;
14876     *i3 = 0;
14877     return 0;
14878 
14879 /* All points are collinear (coplanar). */
14880 
14881 L14:
14882     *i1 = 0;
14883     *i2 = 0;
14884     *i3 = 0;
14885     return 0;
14886 } /* trfind_ */
14887 
14888 /* Subroutine */ int trlist_(int *n, int *list, int *lptr,
14889         int *lend, int *nrow, int *nt, int *ltri, int *
14890         ier)
14891 {
14892     /* System generated locals */
14893     int ltri_dim1, ltri_offset, i__1, i__2;
14894 
14895     /* Local variables */
14896     static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
14897             lpl, isv;
14898     static long int arcs;
14899     static int lpln1;
14900 
14901 
14902 /* *********************************************************** */
14903 
14904 /*                                              From STRIPACK */
14905 /*                                            Robert J. Renka */
14906 /*                                  Dept. of Computer Science */
14907 /*                                       Univ. of North Texas */
14908 /*                                           renka@cs.unt.edu */
14909 /*                                                   07/20/96 */
14910 
14911 /*   This subroutine converts a triangulation data structure */
14912 /* from the linked list created by Subroutine TRMESH to a */
14913 /* triangle list. */
14914 
14915 /* On input: */
14916 
14917 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14918 
14919 /*       LIST,LPTR,LEND = Linked list data structure defin- */
14920 /*                        ing the triangulation.  Refer to */
14921 /*                        Subroutine TRMESH. */
14922 
14923 /*       NROW = Number of rows (entries per triangle) re- */
14924 /*              served for the triangle list LTRI.  The value */
14925 /*              must be 6 if only the vertex indexes and */
14926 /*              neighboring triangle indexes are to be */
14927 /*              stored, or 9 if arc indexes are also to be */
14928 /*              assigned and stored.  Refer to LTRI. */
14929 
14930 /* The above parameters are not altered by this routine. */
14931 
14932 /*       LTRI = int array of length at least NROW*NT, */
14933 /*              where NT is at most 2N-4.  (A sufficient */
14934 /*              length is 12N if NROW=6 or 18N if NROW=9.) */
14935 
14936 /* On output: */
14937 
14938 /*       NT = Number of triangles in the triangulation unless */
14939 /*            IER .NE. 0, in which case NT = 0.  NT = 2N-NB-2 */
14940 /*            if NB .GE. 3 or 2N-4 if NB = 0, where NB is the */
14941 /*            number of boundary nodes. */
14942 
14943 /*       LTRI = NROW by NT array whose J-th column contains */
14944 /*              the vertex nodal indexes (first three rows), */
14945 /*              neighboring triangle indexes (second three */
14946 /*              rows), and, if NROW = 9, arc indexes (last */
14947 /*              three rows) associated with triangle J for */
14948 /*              J = 1,...,NT.  The vertices are ordered */
14949 /*              counterclockwise with the first vertex taken */
14950 /*              to be the one with smallest index.  Thus, */
14951 /*              LTRI(2,J) and LTRI(3,J) are larger than */
14952 /*              LTRI(1,J) and index adjacent neighbors of */
14953 /*              node LTRI(1,J).  For I = 1,2,3, LTRI(I+3,J) */
14954 /*              and LTRI(I+6,J) index the triangle and arc, */
14955 /*              respectively, which are opposite (not shared */
14956 /*              by) node LTRI(I,J), with LTRI(I+3,J) = 0 if */
14957 /*              LTRI(I+6,J) indexes a boundary arc.  Vertex */
14958 /*              indexes range from 1 to N, triangle indexes */
14959 /*              from 0 to NT, and, if included, arc indexes */
14960 /*              from 1 to NA, where NA = 3N-NB-3 if NB .GE. 3 */
14961 /*              or 3N-6 if NB = 0.  The triangles are or- */
14962 /*              dered on first (smallest) vertex indexes. */
14963 
14964 /*       IER = Error indicator. */
14965 /*             IER = 0 if no errors were encountered. */
14966 /*             IER = 1 if N or NROW is outside its valid */
14967 /*                     range on input. */
14968 /*             IER = 2 if the triangulation data structure */
14969 /*                     (LIST,LPTR,LEND) is invalid.  Note, */
14970 /*                     however, that these arrays are not */
14971 /*                     completely tested for validity. */
14972 
14973 /* Modules required by TRLIST:  None */
14974 
14975 /* Intrinsic function called by TRLIST:  ABS */
14976 
14977 /* *********************************************************** */
14978 
14979 
14980 /* Local parameters: */
14981 
14982 /* ARCS =     long int variable with value TRUE iff are */
14983 /*              indexes are to be stored */
14984 /* I,J =      LTRI row indexes (1 to 3) associated with */
14985 /*              triangles KT and KN, respectively */
14986 /* I1,I2,I3 = Nodal indexes of triangle KN */
14987 /* ISV =      Variable used to permute indexes I1,I2,I3 */
14988 /* KA =       Arc index and number of currently stored arcs */
14989 /* KN =       Index of the triangle that shares arc I1-I2 */
14990 /*              with KT */
14991 /* KT =       Triangle index and number of currently stored */
14992 /*              triangles */
14993 /* LP =       LIST pointer */
14994 /* LP2 =      Pointer to N2 as a neighbor of N1 */
14995 /* LPL =      Pointer to the last neighbor of I1 */
14996 /* LPLN1 =    Pointer to the last neighbor of N1 */
14997 /* N1,N2,N3 = Nodal indexes of triangle KT */
14998 /* NM2 =      N-2 */
14999 
15000 
15001 /* Test for invalid input parameters. */
15002 
15003     /* Parameter adjustments */
15004     --lend;
15005     --list;
15006     --lptr;
15007     ltri_dim1 = *nrow;
15008     ltri_offset = 1 + ltri_dim1;
15009     ltri -= ltri_offset;
15010 
15011     /* Function Body */
15012     if (*n < 3 || (*nrow != 6 && *nrow != 9)) {
15013         goto L11;
15014     }
15015 
15016 /* Initialize parameters for loop on triangles KT = (N1,N2, */
15017 /*   N3), where N1 < N2 and N1 < N3. */
15018 
15019 /*   ARCS = TRUE iff arc indexes are to be stored. */
15020 /*   KA,KT = Numbers of currently stored arcs and triangles. */
15021 /*   NM2 = Upper bound on candidates for N1. */
15022 
15023     arcs = *nrow == 9;
15024     ka = 0;
15025     kt = 0;
15026     nm2 = *n - 2;
15027 
15028 /* Loop on nodes N1. */
15029 
15030     i__1 = nm2;
15031     for (n1 = 1; n1 <= i__1; ++n1) {
15032 
15033 /* Loop on pairs of adjacent neighbors (N2,N3).  LPLN1 points */
15034 /*   to the last neighbor of N1, and LP2 points to N2. */
15035 
15036         lpln1 = lend[n1];
15037         lp2 = lpln1;
15038 L1:
15039         lp2 = lptr[lp2];
15040         n2 = list[lp2];
15041         lp = lptr[lp2];
15042         n3 = (i__2 = list[lp], abs(i__2));
15043         if (n2 < n1 || n3 < n1) {
15044             goto L8;
15045         }
15046 
15047 /* Add a new triangle KT = (N1,N2,N3). */
15048 
15049         ++kt;
15050         ltri[kt * ltri_dim1 + 1] = n1;
15051         ltri[kt * ltri_dim1 + 2] = n2;
15052         ltri[kt * ltri_dim1 + 3] = n3;
15053 
15054 /* Loop on triangle sides (I2,I1) with neighboring triangles */
15055 /*   KN = (I1,I2,I3). */
15056 
15057         for (i__ = 1; i__ <= 3; ++i__) {
15058             if (i__ == 1) {
15059                 i1 = n3;
15060                 i2 = n2;
15061             } else if (i__ == 2) {
15062                 i1 = n1;
15063                 i2 = n3;
15064             } else {
15065                 i1 = n2;
15066                 i2 = n1;
15067             }
15068 
15069 /* Set I3 to the neighbor of I1 that follows I2 unless */
15070 /*   I2->I1 is a boundary arc. */
15071 
15072             lpl = lend[i1];
15073             lp = lptr[lpl];
15074 L2:
15075             if (list[lp] == i2) {
15076                 goto L3;
15077             }
15078             lp = lptr[lp];
15079             if (lp != lpl) {
15080                 goto L2;
15081             }
15082 
15083 /*   I2 is the last neighbor of I1 unless the data structure */
15084 /*     is invalid.  Bypass the search for a neighboring */
15085 /*     triangle if I2->I1 is a boundary arc. */
15086 
15087             if ((i__2 = list[lp], abs(i__2)) != i2) {
15088                 goto L12;
15089             }
15090             kn = 0;
15091             if (list[lp] < 0) {
15092                 goto L6;
15093             }
15094 
15095 /*   I2->I1 is not a boundary arc, and LP points to I2 as */
15096 /*     a neighbor of I1. */
15097 
15098 L3:
15099             lp = lptr[lp];
15100             i3 = (i__2 = list[lp], abs(i__2));
15101 
15102 /* Find J such that LTRI(J,KN) = I3 (not used if KN > KT), */
15103 /*   and permute the vertex indexes of KN so that I1 is */
15104 /*   smallest. */
15105 
15106             if (i1 < i2 && i1 < i3) {
15107                 j = 3;
15108             } else if (i2 < i3) {
15109                 j = 2;
15110                 isv = i1;
15111                 i1 = i2;
15112                 i2 = i3;
15113                 i3 = isv;
15114             } else {
15115                 j = 1;
15116                 isv = i1;
15117                 i1 = i3;
15118                 i3 = i2;
15119                 i2 = isv;
15120             }
15121 
15122 /* Test for KN > KT (triangle index not yet assigned). */
15123 
15124             if (i1 > n1) {
15125                 goto L7;
15126             }
15127 
15128 /* Find KN, if it exists, by searching the triangle list in */
15129 /*   reverse order. */
15130 
15131             for (kn = kt - 1; kn >= 1; --kn) {
15132                 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15133                          == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15134                     goto L5;
15135                 }
15136 /* L4: */
15137             }
15138             goto L7;
15139 
15140 /* Store KT as a neighbor of KN. */
15141 
15142 L5:
15143             ltri[j + 3 + kn * ltri_dim1] = kt;
15144 
15145 /* Store KN as a neighbor of KT, and add a new arc KA. */
15146 
15147 L6:
15148             ltri[i__ + 3 + kt * ltri_dim1] = kn;
15149             if (arcs) {
15150                 ++ka;
15151                 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15152                 if (kn != 0) {
15153                     ltri[j + 6 + kn * ltri_dim1] = ka;
15154                 }
15155             }
15156 L7:
15157             ;
15158         }
15159 
15160 /* Bottom of loop on triangles. */
15161 
15162 L8:
15163         if (lp2 != lpln1) {
15164             goto L1;
15165         }
15166 /* L9: */
15167     }
15168 
15169 /* No errors encountered. */
15170 
15171     *nt = kt;
15172     *ier = 0;
15173     return 0;
15174 
15175 /* Invalid input parameter. */
15176 
15177 L11:
15178     *nt = 0;
15179     *ier = 1;
15180     return 0;
15181 
15182 /* Invalid triangulation data structure:  I1 is a neighbor of */
15183 /*   I2, but I2 is not a neighbor of I1. */
15184 
15185 L12:
15186     *nt = 0;
15187     *ier = 2;
15188     return 0;
15189 } /* trlist_ */
15190 
15191 /* Subroutine */ int trlprt_(int *n, double *x, double *y,
15192         double *z__, int *iflag, int *nrow, int *nt, int *
15193         ltri, int *lout)
15194 {
15195     /* Initialized data */
15196 
15197     static int nmax = 9999;
15198     static int nlmax = 58;
15199 
15200     /* System generated locals */
15201     int ltri_dim1, ltri_offset, i__1;
15202 
15203     /* Local variables */
15204     static int i__, k, na, nb, nl, lun;
15205 
15206 
15207 /* *********************************************************** */
15208 
15209 /*                                              From STRIPACK */
15210 /*                                            Robert J. Renka */
15211 /*                                  Dept. of Computer Science */
15212 /*                                       Univ. of North Texas */
15213 /*                                           renka@cs.unt.edu */
15214 /*                                                   07/02/98 */
15215 
15216 /*   This subroutine prints the triangle list created by Sub- */
15217 /* routine TRLIST and, optionally, the nodal coordinates */
15218 /* (either latitude and longitude or Cartesian coordinates) */
15219 /* on long int unit LOUT.  The numbers of boundary nodes, */
15220 /* triangles, and arcs are also printed. */
15221 
15222 
15223 /* On input: */
15224 
15225 /*       N = Number of nodes in the triangulation. */
15226 /*           3 .LE. N .LE. 9999. */
15227 
15228 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15229 /*               coordinates of the nodes if IFLAG = 0, or */
15230 /*               (X and Y only) arrays of length N containing */
15231 /*               longitude and latitude, respectively, if */
15232 /*               IFLAG > 0, or unused dummy parameters if */
15233 /*               IFLAG < 0. */
15234 
15235 /*       IFLAG = Nodal coordinate option indicator: */
15236 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
15237 /*                         Cartesian coordinates) are to be */
15238 /*                         printed (to 6 decimal places). */
15239 /*               IFLAG > 0 if only X and Y (assumed to con- */
15240 /*                         tain longitude and latitude) are */
15241 /*                         to be printed (to 6 decimal */
15242 /*                         places). */
15243 /*               IFLAG < 0 if only the adjacency lists are to */
15244 /*                         be printed. */
15245 
15246 /*       NROW = Number of rows (entries per triangle) re- */
15247 /*              served for the triangle list LTRI.  The value */
15248 /*              must be 6 if only the vertex indexes and */
15249 /*              neighboring triangle indexes are stored, or 9 */
15250 /*              if arc indexes are also stored. */
15251 
15252 /*       NT = Number of triangles in the triangulation. */
15253 /*            1 .LE. NT .LE. 9999. */
15254 
15255 /*       LTRI = NROW by NT array whose J-th column contains */
15256 /*              the vertex nodal indexes (first three rows), */
15257 /*              neighboring triangle indexes (second three */
15258 /*              rows), and, if NROW = 9, arc indexes (last */
15259 /*              three rows) associated with triangle J for */
15260 /*              J = 1,...,NT. */
15261 
15262 /*       LOUT = long int unit number for output.  If LOUT is */
15263 /*              not in the range 0 to 99, output is written */
15264 /*              to unit 6. */
15265 
15266 /* Input parameters are not altered by this routine. */
15267 
15268 /* On output: */
15269 
15270 /*   The triangle list and nodal coordinates (as specified by */
15271 /* IFLAG) are written to unit LOUT. */
15272 
15273 /* Modules required by TRLPRT:  None */
15274 
15275 /* *********************************************************** */
15276 
15277     /* Parameter adjustments */
15278     --z__;
15279     --y;
15280     --x;
15281     ltri_dim1 = *nrow;
15282     ltri_offset = 1 + ltri_dim1;
15283     ltri -= ltri_offset;
15284 
15285     /* Function Body */
15286 
15287 /* Local parameters: */
15288 
15289 /* I =     DO-loop, nodal index, and row index for LTRI */
15290 /* K =     DO-loop and triangle index */
15291 /* LUN =   long int unit number for output */
15292 /* NA =    Number of triangulation arcs */
15293 /* NB =    Number of boundary nodes */
15294 /* NL =    Number of lines printed on the current page */
15295 /* NLMAX = Maximum number of print lines per page (except */
15296 /*           for the last page which may have two addi- */
15297 /*           tional lines) */
15298 /* NMAX =  Maximum value of N and NT (4-digit format) */
15299 
15300     lun = *lout;
15301     if (lun < 0 || lun > 99) {
15302         lun = 6;
15303     }
15304 
15305 /* Print a heading and test for invalid input. */
15306 
15307 /*      WRITE (LUN,100) N */
15308     nl = 3;
15309     if (*n < 3 || *n > nmax || (*nrow != 6 && *nrow != 9) || *nt < 1 || *nt >
15310             nmax) {
15311 
15312 /* Print an error message and exit. */
15313 
15314 /*        WRITE (LUN,110) N, NROW, NT */
15315         return 0;
15316     }
15317     if (*iflag == 0) {
15318 
15319 /* Print X, Y, and Z. */
15320 
15321 /*        WRITE (LUN,101) */
15322         nl = 6;
15323         i__1 = *n;
15324         for (i__ = 1; i__ <= i__1; ++i__) {
15325             if (nl >= nlmax) {
15326 /*            WRITE (LUN,108) */
15327                 nl = 0;
15328             }
15329 /*          WRITE (LUN,103) I, X(I), Y(I), Z(I) */
15330             ++nl;
15331 /* L1: */
15332         }
15333     } else if (*iflag > 0) {
15334 
15335 /* Print X (longitude) and Y (latitude). */
15336 
15337 /*        WRITE (LUN,102) */
15338         nl = 6;
15339         i__1 = *n;
15340         for (i__ = 1; i__ <= i__1; ++i__) {
15341             if (nl >= nlmax) {
15342 /*            WRITE (LUN,108) */
15343                 nl = 0;
15344             }
15345 /*          WRITE (LUN,104) I, X(I), Y(I) */
15346             ++nl;
15347 /* L2: */
15348         }
15349     }
15350 
15351 /* Print the triangulation LTRI. */
15352 
15353     if (nl > nlmax / 2) {
15354 /*        WRITE (LUN,108) */
15355         nl = 0;
15356     }
15357     if (*nrow == 6) {
15358 /*        WRITE (LUN,105) */
15359     } else {
15360 /*        WRITE (LUN,106) */
15361     }
15362     nl += 5;
15363     i__1 = *nt;
15364     for (k = 1; k <= i__1; ++k) {
15365         if (nl >= nlmax) {
15366 /*          WRITE (LUN,108) */
15367             nl = 0;
15368         }
15369 /*        WRITE (LUN,107) K, (LTRI(I,K), I = 1,NROW) */
15370         ++nl;
15371 /* L3: */
15372     }
15373 
15374 /* Print NB, NA, and NT (boundary nodes, arcs, and */
15375 /*   triangles). */
15376 
15377     nb = (*n << 1) - *nt - 2;
15378     if (nb < 3) {
15379         nb = 0;
15380         na = *n * 3 - 6;
15381     } else {
15382         na = *nt + *n - 1;
15383     }
15384 /*      WRITE (LUN,109) NB, NA, NT */
15385     return 0;
15386 
15387 /* Print formats: */
15388 
15389 /*  100 FORMAT (///18X,'STRIPACK (TRLIST) Output,  N = ',I4) */
15390 /*  101 FORMAT (//8X,'Node',10X,'X(Node)',10X,'Y(Node)',10X, */
15391 /*     .        'Z(Node)'//) */
15392 /*  102 FORMAT (//16X,'Node',8X,'Longitude',9X,'Latitude'//) */
15393 /*  103 FORMAT (8X,I4,3D17.6) */
15394 /*  104 FORMAT (16X,I4,2D17.6) */
15395 /*  105 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors'/ */
15396 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15397 /*     .        'KT2',4X,'KT3'/) */
15398 /*  106 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors', */
15399 /*     .        14X,'Arcs'/ */
15400 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15401 /*     .        'KT2',4X,'KT3',4X,'KA1',4X,'KA2',4X,'KA3'/) */
15402 /*  107 FORMAT (2X,I4,2X,6(3X,I4),3(2X,I5)) */
15403 /*  108 FORMAT (///) */
15404 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
15405 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
15406 /*     .        ' Triangles') */
15407 /*  110 FORMAT (//1X,10X,'*** Invalid Parameter:  N =',I5, */
15408 /*     .        ', NROW =',I5,', NT =',I5,' ***') */
15409 } /* trlprt_ */
15410 
15411 /* Subroutine */ int trmesh_(int *n, double *x, double *y,
15412         double *z__, int *list, int *lptr, int *lend, int
15413         *lnew, int *near__, int *next, double *dist, int *ier)
15414 {
15415     /* System generated locals */
15416     int i__1, i__2;
15417 
15418     /* Local variables */
15419     static double d__;
15420     static int i__, j, k;
15421     static double d1, d2, d3;
15422     static int i0, lp, nn, lpl;
15423     extern long int left_(double *, double *, double *, double
15424             *, double *, double *, double *, double *,
15425             double *);
15426     static int nexti;
15427     extern /* Subroutine */ int addnod_(int *, int *, double *,
15428             double *, double *, int *, int *, int *,
15429             int *, int *);
15430 
15431 
15432 /* *********************************************************** */
15433 
15434 /*                                              From STRIPACK */
15435 /*                                            Robert J. Renka */
15436 /*                                  Dept. of Computer Science */
15437 /*                                       Univ. of North Texas */
15438 /*                                           renka@cs.unt.edu */
15439 /*                                                   03/04/03 */
15440 
15441 /*   This subroutine creates a Delaunay triangulation of a */
15442 /* set of N arbitrarily distributed points, referred to as */
15443 /* nodes, on the surface of the unit sphere.  The Delaunay */
15444 /* triangulation is defined as a set of (spherical) triangles */
15445 /* with the following five properties: */
15446 
15447 /*  1)  The triangle vertices are nodes. */
15448 /*  2)  No triangle contains a node other than its vertices. */
15449 /*  3)  The interiors of the triangles are pairwise disjoint. */
15450 /*  4)  The union of triangles is the convex hull of the set */
15451 /*        of nodes (the smallest convex set that contains */
15452 /*        the nodes).  If the nodes are not contained in a */
15453 /*        single hemisphere, their convex hull is the en- */
15454 /*        tire sphere and there are no boundary nodes. */
15455 /*        Otherwise, there are at least three boundary nodes. */
15456 /*  5)  The interior of the circumcircle of each triangle */
15457 /*        contains no node. */
15458 
15459 /* The first four properties define a triangulation, and the */
15460 /* last property results in a triangulation which is as close */
15461 /* as possible to equiangular in a certain sense and which is */
15462 /* uniquely defined unless four or more nodes lie in a common */
15463 /* plane.  This property makes the triangulation well-suited */
15464 /* for solving closest-point problems and for triangle-based */
15465 /* interpolation. */
15466 
15467 /*   The algorithm has expected time complexity O(N*log(N)) */
15468 /* for most nodal distributions. */
15469 
15470 /*   Spherical coordinates (latitude and longitude) may be */
15471 /* converted to Cartesian coordinates by Subroutine TRANS. */
15472 
15473 /*   The following is a list of the software package modules */
15474 /* which a user may wish to call directly: */
15475 
15476 /*  ADDNOD - Updates the triangulation by appending a new */
15477 /*             node. */
15478 
15479 /*  AREAS  - Returns the area of a spherical triangle. */
15480 
15481 /*  AREAV  - Returns the area of a Voronoi region associated */
15482 /*           with an interior node without requiring that the */
15483 /*           entire Voronoi diagram be computed and stored. */
15484 
15485 /*  BNODES - Returns an array containing the indexes of the */
15486 /*             boundary nodes (if any) in counterclockwise */
15487 /*             order.  Counts of boundary nodes, triangles, */
15488 /*             and arcs are also returned. */
15489 
15490 /*  CIRCLE - Computes the coordinates of a sequence of uni- */
15491 /*           formly spaced points on the unit circle centered */
15492 /*           at (0,0). */
15493 
15494 /*  CIRCUM - Returns the circumcenter of a spherical trian- */
15495 /*             gle. */
15496 
15497 /*  CRLIST - Returns the set of triangle circumcenters */
15498 /*             (Voronoi vertices) and circumradii associated */
15499 /*             with a triangulation. */
15500 
15501 /*  DELARC - Deletes a boundary arc from a triangulation. */
15502 
15503 /*  DELNOD - Updates the triangulation with a nodal deletion. */
15504 
15505 /*  EDGE   - Forces an arbitrary pair of nodes to be connec- */
15506 /*             ted by an arc in the triangulation. */
15507 
15508 /*  GETNP  - Determines the ordered sequence of L closest */
15509 /*             nodes to a given node, along with the associ- */
15510 /*             ated distances. */
15511 
15512 /*  INSIDE - Locates a point relative to a polygon on the */
15513 /*             surface of the sphere. */
15514 
15515 /*  INTRSC - Returns the point of intersection between a */
15516 /*             pair of great circle arcs. */
15517 
15518 /*  JRAND  - Generates a uniformly distributed pseudo-random */
15519 /*             int. */
15520 
15521 /*  LEFT   - Locates a point relative to a great circle. */
15522 
15523 /*  NEARND - Returns the index of the nearest node to an */
15524 /*             arbitrary point, along with its squared */
15525 /*             distance. */
15526 
15527 /*  PROJCT - Applies a perspective-depth projection to a */
15528 /*             point in 3-space. */
15529 
15530 /*  SCOORD - Converts a point from Cartesian coordinates to */
15531 /*             spherical coordinates. */
15532 
15533 /*  STORE  - Forces a value to be stored in main memory so */
15534 /*             that the precision of floating point numbers */
15535 /*             in memory locations rather than registers is */
15536 /*             computed. */
15537 
15538 /*  TRANS  - Transforms spherical coordinates into Cartesian */
15539 /*             coordinates on the unit sphere for input to */
15540 /*             Subroutine TRMESH. */
15541 
15542 /*  TRLIST - Converts the triangulation data structure to a */
15543 /*             triangle list more suitable for use in a fin- */
15544 /*             ite element code. */
15545 
15546 /*  TRLPRT - Prints the triangle list created by Subroutine */
15547 /*             TRLIST. */
15548 
15549 /*  TRMESH - Creates a Delaunay triangulation of a set of */
15550 /*             nodes. */
15551 
15552 /*  TRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15553 /*             file containing a triangulation plot. */
15554 
15555 /*  TRPRNT - Prints the triangulation data structure and, */
15556 /*             optionally, the nodal coordinates. */
15557 
15558 /*  VRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15559 /*             file containing a Voronoi diagram plot. */
15560 
15561 
15562 /* On input: */
15563 
15564 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15565 
15566 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15567 /*               coordinates of distinct nodes.  (X(K),Y(K), */
15568 /*               Z(K)) is referred to as node K, and K is re- */
15569 /*               ferred to as a nodal index.  It is required */
15570 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
15571 /*               K.  The first three nodes must not be col- */
15572 /*               linear (lie on a common great circle). */
15573 
15574 /* The above parameters are not altered by this routine. */
15575 
15576 /*       LIST,LPTR = Arrays of length at least 6N-12. */
15577 
15578 /*       LEND = Array of length at least N. */
15579 
15580 /*       NEAR,NEXT,DIST = Work space arrays of length at */
15581 /*                        least N.  The space is used to */
15582 /*                        efficiently determine the nearest */
15583 /*                        triangulation node to each un- */
15584 /*                        processed node for use by ADDNOD. */
15585 
15586 /* On output: */
15587 
15588 /*       LIST = Set of nodal indexes which, along with LPTR, */
15589 /*              LEND, and LNEW, define the triangulation as a */
15590 /*              set of N adjacency lists -- counterclockwise- */
15591 /*              ordered sequences of neighboring nodes such */
15592 /*              that the first and last neighbors of a bound- */
15593 /*              ary node are boundary nodes (the first neigh- */
15594 /*              bor of an interior node is arbitrary).  In */
15595 /*              order to distinguish between interior and */
15596 /*              boundary nodes, the last neighbor of each */
15597 /*              boundary node is represented by the negative */
15598 /*              of its index. */
15599 
15600 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
15601 /*              correspondence with the elements of LIST. */
15602 /*              LIST(LPTR(I)) indexes the node which follows */
15603 /*              LIST(I) in cyclical counterclockwise order */
15604 /*              (the first neighbor follows the last neigh- */
15605 /*              bor). */
15606 
15607 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
15608 /*              points to the last neighbor of node K for */
15609 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
15610 /*              only if K is a boundary node. */
15611 
15612 /*       LNEW = Pointer to the first empty location in LIST */
15613 /*              and LPTR (list length plus one).  LIST, LPTR, */
15614 /*              LEND, and LNEW are not altered if IER < 0, */
15615 /*              and are incomplete if IER > 0. */
15616 
15617 /*       NEAR,NEXT,DIST = Garbage. */
15618 
15619 /*       IER = Error indicator: */
15620 /*             IER =  0 if no errors were encountered. */
15621 /*             IER = -1 if N < 3 on input. */
15622 /*             IER = -2 if the first three nodes are */
15623 /*                      collinear. */
15624 /*             IER =  L if nodes L and M coincide for some */
15625 /*                      M > L.  The data structure represents */
15626 /*                      a triangulation of nodes 1 to M-1 in */
15627 /*                      this case. */
15628 
15629 /* Modules required by TRMESH:  ADDNOD, BDYADD, COVSPH, */
15630 /*                                INSERT, INTADD, JRAND, */
15631 /*                                LEFT, LSTPTR, STORE, SWAP, */
15632 /*                                SWPTST, TRFIND */
15633 
15634 /* Intrinsic function called by TRMESH:  ABS */
15635 
15636 /* *********************************************************** */
15637 
15638 
15639 /* Local parameters: */
15640 
15641 /* D =        (Negative cosine of) distance from node K to */
15642 /*              node I */
15643 /* D1,D2,D3 = Distances from node K to nodes 1, 2, and 3, */
15644 /*              respectively */
15645 /* I,J =      Nodal indexes */
15646 /* I0 =       Index of the node preceding I in a sequence of */
15647 /*              unprocessed nodes:  I = NEXT(I0) */
15648 /* K =        Index of node to be added and DO-loop index: */
15649 /*              K > 3 */
15650 /* LP =       LIST index (pointer) of a neighbor of K */
15651 /* LPL =      Pointer to the last neighbor of K */
15652 /* NEXTI =    NEXT(I) */
15653 /* NN =       Local copy of N */
15654 
15655     /* Parameter adjustments */
15656     --dist;
15657     --next;
15658     --near__;
15659     --lend;
15660     --z__;
15661     --y;
15662     --x;
15663     --list;
15664     --lptr;
15665 
15666     /* Function Body */
15667     nn = *n;
15668     if (nn < 3) {
15669         *ier = -1;
15670         return 0;
15671     }
15672 
15673 /* Store the first triangle in the linked list. */
15674 
15675     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15676             z__[3])) {
15677 
15678 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
15679 
15680         list[1] = 3;
15681         lptr[1] = 2;
15682         list[2] = -2;
15683         lptr[2] = 1;
15684         lend[1] = 2;
15685 
15686         list[3] = 1;
15687         lptr[3] = 4;
15688         list[4] = -3;
15689         lptr[4] = 3;
15690         lend[2] = 4;
15691 
15692         list[5] = 2;
15693         lptr[5] = 6;
15694         list[6] = -1;
15695         lptr[6] = 5;
15696         lend[3] = 6;
15697 
15698     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15699             y[3], &z__[3])) {
15700 
15701 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
15702 /*     i.e., node 3 lies in the left hemisphere defined by */
15703 /*     arc 1->2. */
15704 
15705         list[1] = 2;
15706         lptr[1] = 2;
15707         list[2] = -3;
15708         lptr[2] = 1;
15709         lend[1] = 2;
15710 
15711         list[3] = 3;
15712         lptr[3] = 4;
15713         list[4] = -1;
15714         lptr[4] = 3;
15715         lend[2] = 4;
15716 
15717         list[5] = 1;
15718         lptr[5] = 6;
15719         list[6] = -2;
15720         lptr[6] = 5;
15721         lend[3] = 6;
15722 
15723     } else {
15724 
15725 /*   The first three nodes are collinear. */
15726 
15727         *ier = -2;
15728         return 0;
15729     }
15730 
15731 /* Initialize LNEW and test for N = 3. */
15732 
15733     *lnew = 7;
15734     if (nn == 3) {
15735         *ier = 0;
15736         return 0;
15737     }
15738 
15739 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
15740 /*   used to obtain an expected-time (N*log(N)) incremental */
15741 /*   algorithm by enabling constant search time for locating */
15742 /*   each new node in the triangulation. */
15743 
15744 /* For each unprocessed node K, NEAR(K) is the index of the */
15745 /*   triangulation node closest to K (used as the starting */
15746 /*   point for the search in Subroutine TRFIND) and DIST(K) */
15747 /*   is an increasing function of the arc length (angular */
15748 /*   distance) between nodes K and NEAR(K):  -Cos(a) for arc */
15749 /*   length a. */
15750 
15751 /* Since it is necessary to efficiently find the subset of */
15752 /*   unprocessed nodes associated with each triangulation */
15753 /*   node J (those that have J as their NEAR entries), the */
15754 /*   subsets are stored in NEAR and NEXT as follows:  for */
15755 /*   each node J in the triangulation, I = NEAR(J) is the */
15756 /*   first unprocessed node in J's set (with I = 0 if the */
15757 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
15758 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
15759 /*   set are initially ordered by increasing indexes (which */
15760 /*   maximizes efficiency) but that ordering is not main- */
15761 /*   tained as the data structure is updated. */
15762 
15763 /* Initialize the data structure for the single triangle. */
15764 
15765     near__[1] = 0;
15766     near__[2] = 0;
15767     near__[3] = 0;
15768     for (k = nn; k >= 4; --k) {
15769         d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15770         d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15771         d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15772         if (d1 <= d2 && d1 <= d3) {
15773             near__[k] = 1;
15774             dist[k] = d1;
15775             next[k] = near__[1];
15776             near__[1] = k;
15777         } else if (d2 <= d1 && d2 <= d3) {
15778             near__[k] = 2;
15779             dist[k] = d2;
15780             next[k] = near__[2];
15781             near__[2] = k;
15782         } else {
15783             near__[k] = 3;
15784             dist[k] = d3;
15785             next[k] = near__[3];
15786             near__[3] = k;
15787         }
15788 /* L1: */
15789     }
15790 
15791 /* Add the remaining nodes */
15792 
15793     i__1 = nn;
15794     for (k = 4; k <= i__1; ++k) {
15795         addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15796                 lend[1], lnew, ier);
15797         if (*ier != 0) {
15798             return 0;
15799         }
15800 
15801 /* Remove K from the set of unprocessed nodes associated */
15802 /*   with NEAR(K). */
15803 
15804         i__ = near__[k];
15805         if (near__[i__] == k) {
15806             near__[i__] = next[k];
15807         } else {
15808             i__ = near__[i__];
15809 L2:
15810             i0 = i__;
15811             i__ = next[i0];
15812             if (i__ != k) {
15813                 goto L2;
15814             }
15815             next[i0] = next[k];
15816         }
15817         near__[k] = 0;
15818 
15819 /* Loop on neighbors J of node K. */
15820 
15821         lpl = lend[k];
15822         lp = lpl;
15823 L3:
15824         lp = lptr[lp];
15825         j = (i__2 = list[lp], abs(i__2));
15826 
15827 /* Loop on elements I in the sequence of unprocessed nodes */
15828 /*   associated with J:  K is a candidate for replacing J */
15829 /*   as the nearest triangulation node to I.  The next value */
15830 /*   of I in the sequence, NEXT(I), must be saved before I */
15831 /*   is moved because it is altered by adding I to K's set. */
15832 
15833         i__ = near__[j];
15834 L4:
15835         if (i__ == 0) {
15836             goto L5;
15837         }
15838         nexti = next[i__];
15839 
15840 /* Test for the distance from I to K less than the distance */
15841 /*   from I to J. */
15842 
15843         d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15844         if (d__ < dist[i__]) {
15845 
15846 /* Replace J by K as the nearest triangulation node to I: */
15847 /*   update NEAR(I) and DIST(I), and remove I from J's set */
15848 /*   of unprocessed nodes and add it to K's set. */
15849 
15850             near__[i__] = k;
15851             dist[i__] = d__;
15852             if (i__ == near__[j]) {
15853                 near__[j] = nexti;
15854             } else {
15855                 next[i0] = nexti;
15856             }
15857             next[i__] = near__[k];
15858             near__[k] = i__;
15859         } else {
15860             i0 = i__;
15861         }
15862 
15863 /* Bottom of loop on I. */
15864 
15865         i__ = nexti;
15866         goto L4;
15867 
15868 /* Bottom of loop on neighbors J. */
15869 
15870 L5:
15871         if (lp != lpl) {
15872             goto L3;
15873         }
15874 /* L6: */
15875     }
15876     return 0;
15877 } /* trmesh_ */
15878 
15879 /* Subroutine */ int trplot_(int *lun, double *pltsiz, double *
15880         elat, double *elon, double *a, int *n, double *x,
15881         double *y, double *z__, int *list, int *lptr, int
15882         *lend, char *, long int *numbr, int *ier, short )
15883 {
15884     /* Initialized data */
15885 
15886     static long int annot = TRUE_;
15887     static double fsizn = 10.;
15888     static double fsizt = 16.;
15889     static double tol = .5;
15890 
15891     /* System generated locals */
15892     int i__1, i__2;
15893     double d__1;
15894 
15895     /* Builtin functions */
15896     //double atan(double), sin(double);
15897     //int i_dnnt(double *);
15898     //double cos(double), sqrt(double);
15899 
15900     /* Local variables */
15901     static double t;
15902     static int n0, n1;
15903     static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
15904     static int ir, lp;
15905     static double ex, ey, ez, wr, tx, ty;
15906     static int lpl;
15907     static double wrs;
15908     static int ipx1, ipx2, ipy1, ipy2, nseg;
15909     extern /* Subroutine */ int drwarc_(int *, double *, double *,
15910              double *, int *);
15911 
15912 
15913 /* *********************************************************** */
15914 
15915 /*                                              From STRIPACK */
15916 /*                                            Robert J. Renka */
15917 /*                                  Dept. of Computer Science */
15918 /*                                       Univ. of North Texas */
15919 /*                                           renka@cs.unt.edu */
15920 /*                                                   03/04/03 */
15921 
15922 /*   This subroutine creates a level-2 Encapsulated Post- */
15923 /* script (EPS) file containing a graphical display of a */
15924 /* triangulation of a set of nodes on the surface of the unit */
15925 /* sphere.  The visible portion of the triangulation is */
15926 /* projected onto the plane that contains the origin and has */
15927 /* normal defined by a user-specified eye-position. */
15928 
15929 
15930 /* On input: */
15931 
15932 /*       LUN = long int unit number in the range 0 to 99. */
15933 /*             The unit should be opened with an appropriate */
15934 /*             file name before the call to this routine. */
15935 
15936 /*       PLTSIZ = Plot size in inches.  A circular window in */
15937 /*                the projection plane is mapped to a circu- */
15938 /*                lar viewport with diameter equal to .88* */
15939 /*                PLTSIZ (leaving room for labels outside the */
15940 /*                viewport).  The viewport is centered on the */
15941 /*                8.5 by 11 inch page, and its boundary is */
15942 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
15943 
15944 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
15945 /*                   the center of projection E (the center */
15946 /*                   of the plot).  The projection plane is */
15947 /*                   the plane that contains the origin and */
15948 /*                   has E as unit normal.  In a rotated */
15949 /*                   coordinate system for which E is the */
15950 /*                   north pole, the projection plane con- */
15951 /*                   tains the equator, and only northern */
15952 /*                   hemisphere nodes are visible (from the */
15953 /*                   point at infinity in the direction E). */
15954 /*                   These are projected orthogonally onto */
15955 /*                   the projection plane (by zeroing the z- */
15956 /*                   component in the rotated coordinate */
15957 /*                   system).  ELAT and ELON must be in the */
15958 /*                   range -90 to 90 and -180 to 180, respec- */
15959 /*                   tively. */
15960 
15961 /*       A = Angular distance in degrees from E to the boun- */
15962 /*           dary of a circular window against which the */
15963 /*           triangulation is clipped.  The projected window */
15964 /*           is a disk of radius r = Sin(A) centered at the */
15965 /*           origin, and only visible nodes whose projections */
15966 /*           are within distance r of the origin are included */
15967 /*           in the plot.  Thus, if A = 90, the plot includes */
15968 /*           the entire hemisphere centered at E.  0 .LT. A */
15969 /*           .LE. 90. */
15970 
15971 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15972 
15973 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15974 /*               coordinates of the nodes (unit vectors). */
15975 
15976 /*       LIST,LPTR,LEND = Data structure defining the trian- */
15977 /*                        gulation.  Refer to Subroutine */
15978 /*                        TRMESH. */
15979 
15980 /*       TITLE = Type CHARACTER variable or constant contain- */
15981 /*               ing a string to be centered above the plot. */
15982 /*               The string must be enclosed in parentheses; */
15983 /*               i.e., the first and last characters must be */
15984 /*               '(' and ')', respectively, but these are not */
15985 /*               displayed.  TITLE may have at most 80 char- */
15986 /*               acters including the parentheses. */
15987 
15988 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
15989 /*               nodal indexes are plotted next to the nodes. */
15990 
15991 /* Input parameters are not altered by this routine. */
15992 
15993 /* On output: */
15994 
15995 /*       IER = Error indicator: */
15996 /*             IER = 0 if no errors were encountered. */
15997 /*             IER = 1 if LUN, PLTSIZ, or N is outside its */
15998 /*                     valid range. */
15999 /*             IER = 2 if ELAT, ELON, or A is outside its */
16000 /*                     valid range. */
16001 /*             IER = 3 if an error was encountered in writing */
16002 /*                     to unit LUN. */
16003 
16004 /*   The values in the data statement below may be altered */
16005 /* in order to modify various plotting options. */
16006 
16007 /* Module required by TRPLOT:  DRWARC */
16008 
16009 /* Intrinsic functions called by TRPLOT:  ABS, ATAN, COS, */
16010 /*                                          DBLE, NINT, SIN, */
16011 /*                                          SQRT */
16012 
16013 /* *********************************************************** */
16014 
16015 
16016     /* Parameter adjustments */
16017     --lend;
16018     --z__;
16019     --y;
16020     --x;
16021     --list;
16022     --lptr;
16023 
16024     /* Function Body */
16025 
16026 /* Local parameters: */
16027 
16028 /* ANNOT =     long int variable with value TRUE iff the plot */
16029 /*               is to be annotated with the values of ELAT, */
16030 /*               ELON, and A */
16031 /* CF =        Conversion factor for degrees to radians */
16032 /* CT =        Cos(ELAT) */
16033 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16034 /* FSIZN =     Font size in points for labeling nodes with */
16035 /*               their indexes if NUMBR = TRUE */
16036 /* FSIZT =     Font size in points for the title (and */
16037 /*               annotation if ANNOT = TRUE) */
16038 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16039 /*               left corner of the bounding box or viewport */
16040 /*               box */
16041 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16042 /*               right corner of the bounding box or viewport */
16043 /*               box */
16044 /* IR =        Half the width (height) of the bounding box or */
16045 /*               viewport box in points -- viewport radius */
16046 /* LP =        LIST index (pointer) */
16047 /* LPL =       Pointer to the last neighbor of N0 */
16048 /* N0 =        Index of a node whose incident arcs are to be */
16049 /*               drawn */
16050 /* N1 =        Neighbor of N0 */
16051 /* NSEG =      Number of line segments used by DRWARC in a */
16052 /*               polygonal approximation to a projected edge */
16053 /* P0 =        Coordinates of N0 in the rotated coordinate */
16054 /*               system or label location (first two */
16055 /*               components) */
16056 /* P1 =        Coordinates of N1 in the rotated coordinate */
16057 /*               system or intersection of edge N0-N1 with */
16058 /*               the equator (in the rotated coordinate */
16059 /*               system) */
16060 /* R11...R23 = Components of the first two rows of a rotation */
16061 /*               that maps E to the north pole (0,0,1) */
16062 /* SF =        Scale factor for mapping world coordinates */
16063 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16064 /*               to viewport coordinates in [IPX1,IPX2] X */
16065 /*               [IPY1,IPY2] */
16066 /* T =         Temporary variable */
16067 /* TOL =       Maximum distance in points between a projected */
16068 /*               triangulation edge and its approximation by */
16069 /*               a polygonal curve */
16070 /* TX,TY =     Translation vector for mapping world coordi- */
16071 /*               nates to viewport coordinates */
16072 /* WR =        Window radius r = Sin(A) */
16073 /* WRS =       WR**2 */
16074 
16075 
16076 /* Test for invalid parameters. */
16077 
16078     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16079         goto L11;
16080     }
16081     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16082         goto L12;
16083     }
16084 
16085 /* Compute a conversion factor CF for degrees to radians */
16086 /*   and compute the window radius WR. */
16087 
16088     cf = atan(1.) / 45.;
16089     wr = sin(cf * *a);
16090     wrs = wr * wr;
16091 
16092 /* Compute the lower left (IPX1,IPY1) and upper right */
16093 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16094 /*   The coordinates, specified in default user space units */
16095 /*   (points, at 72 points/inch with origin at the lower */
16096 /*   left corner of the page), are chosen to preserve the */
16097 /*   square aspect ratio, and to center the plot on the 8.5 */
16098 /*   by 11 inch page.  The center of the page is (306,396), */
16099 /*   and IR = PLTSIZ/2 in points. */
16100 
16101     d__1 = *pltsiz * 36.;
16102     ir = i_dnnt(&d__1);
16103     ipx1 = 306 - ir;
16104     ipx2 = ir + 306;
16105     ipy1 = 396 - ir;
16106     ipy2 = ir + 396;
16107 
16108 /* Output header comments. */
16109 
16110 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16111 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16112 /*     .        '%%BoundingBox:',4I4/ */
16113 /*     .        '%%Title:  Triangulation'/ */
16114 /*     .        '%%Creator:  STRIPACK'/ */
16115 /*     .        '%%EndComments') */
16116 
16117 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16118 /*   of a viewport box obtained by shrinking the bounding box */
16119 /*   by 12% in each dimension. */
16120 
16121     d__1 = (double) ir * .88;
16122     ir = i_dnnt(&d__1);
16123     ipx1 = 306 - ir;
16124     ipx2 = ir + 306;
16125     ipy1 = 396 - ir;
16126     ipy2 = ir + 396;
16127 
16128 /* Set the line thickness to 2 points, and draw the */
16129 /*   viewport boundary. */
16130 
16131     t = 2.;
16132 /*      WRITE (LUN,110,ERR=13) T */
16133 /*      WRITE (LUN,120,ERR=13) IR */
16134 /*      WRITE (LUN,130,ERR=13) */
16135 /*  110 FORMAT (F12.6,' setlinewidth') */
16136 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16137 /*  130 FORMAT ('stroke') */
16138 
16139 /* Set up an affine mapping from the window box [-WR,WR] X */
16140 /*   [-WR,WR] to the viewport box. */
16141 
16142     sf = (double) ir / wr;
16143     tx = ipx1 + sf * wr;
16144     ty = ipy1 + sf * wr;
16145 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16146 /*  140 FORMAT (2F12.6,' translate'/ */
16147 /*    .        2F12.6,' scale') */
16148 
16149 /* The line thickness must be changed to reflect the new */
16150 /*   scaling which is applied to all subsequent output. */
16151 /*   Set it to 1.0 point. */
16152 
16153     t = 1. / sf;
16154 /*      WRITE (LUN,110,ERR=13) T */
16155 
16156 /* Save the current graphics state, and set the clip path to */
16157 /*   the boundary of the window. */
16158 
16159 /*      WRITE (LUN,150,ERR=13) */
16160 /*      WRITE (LUN,160,ERR=13) WR */
16161 /*      WRITE (LUN,170,ERR=13) */
16162 /*  150 FORMAT ('gsave') */
16163 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
16164 /*  170 FORMAT ('clip newpath') */
16165 
16166 /* Compute the Cartesian coordinates of E and the components */
16167 /*   of a rotation R which maps E to the north pole (0,0,1). */
16168 /*   R is taken to be a rotation about the z-axis (into the */
16169 /*   yz-plane) followed by a rotation about the x-axis chosen */
16170 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
16171 /*   E is the north or south pole. */
16172 
16173 /*           ( R11  R12  0   ) */
16174 /*       R = ( R21  R22  R23 ) */
16175 /*           ( EX   EY   EZ  ) */
16176 
16177     t = cf * *elon;
16178     ct = cos(cf * *elat);
16179     ex = ct * cos(t);
16180     ey = ct * sin(t);
16181     ez = sin(cf * *elat);
16182     if (ct != 0.) {
16183         r11 = -ey / ct;
16184         r12 = ex / ct;
16185     } else {
16186         r11 = 0.;
16187         r12 = 1.;
16188     }
16189     r21 = -ez * r12;
16190     r22 = ez * r11;
16191     r23 = ct;
16192 
16193 /* Loop on visible nodes N0 that project to points */
16194 /*   (P0(1),P0(2)) in the window. */
16195 
16196     i__1 = *n;
16197     for (n0 = 1; n0 <= i__1; ++n0) {
16198         p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16199         if (p0[2] < 0.) {
16200             goto L3;
16201         }
16202         p0[0] = r11 * x[n0] + r12 * y[n0];
16203         p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16204         if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16205             goto L3;
16206         }
16207         lpl = lend[n0];
16208         lp = lpl;
16209 
16210 /* Loop on neighbors N1 of N0.  LPL points to the last */
16211 /*   neighbor of N0.  Copy the components of N1 into P. */
16212 
16213 L1:
16214         lp = lptr[lp];
16215         n1 = (i__2 = list[lp], abs(i__2));
16216         p1[0] = r11 * x[n1] + r12 * y[n1];
16217         p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16218         p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16219         if (p1[2] < 0.) {
16220 
16221 /*   N1 is a 'southern hemisphere' point.  Move it to the */
16222 /*     intersection of edge N0-N1 with the equator so that */
16223 /*     the edge is clipped properly.  P1(3) is set to 0. */
16224 
16225             p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16226             p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16227             t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16228             p1[0] /= t;
16229             p1[1] /= t;
16230         }
16231 
16232 /*   If node N1 is in the window and N1 < N0, bypass edge */
16233 /*     N0->N1 (since edge N1->N0 has already been drawn). */
16234 
16235         if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16236             goto L2;
16237         }
16238 
16239 /*   Add the edge to the path.  (TOL is converted to world */
16240 /*     coordinates.) */
16241 
16242         if (p1[2] < 0.) {
16243             p1[2] = 0.;
16244         }
16245         d__1 = tol / sf;
16246         drwarc_(lun, p0, p1, &d__1, &nseg);
16247 
16248 /* Bottom of loops. */
16249 
16250 L2:
16251         if (lp != lpl) {
16252             goto L1;
16253         }
16254 L3:
16255         ;
16256     }
16257 
16258 /* Paint the path and restore the saved graphics state (with */
16259 /*   no clip path). */
16260 
16261 /*      WRITE (LUN,130,ERR=13) */
16262 /*      WRITE (LUN,190,ERR=13) */
16263 /*  190 FORMAT ('grestore') */
16264     if (*numbr) {
16265 
16266 /* Nodes in the window are to be labeled with their indexes. */
16267 /*   Convert FSIZN from points to world coordinates, and */
16268 /*   output the commands to select a font and scale it. */
16269 
16270         t = fsizn / sf;
16271 /*        WRITE (LUN,200,ERR=13) T */
16272 /*  200   FORMAT ('/Helvetica findfont'/ */
16273 /*     .          F12.6,' scalefont setfont') */
16274 
16275 /* Loop on visible nodes N0 that project to points */
16276 /*   P0 = (P0(1),P0(2)) in the window. */
16277 
16278         i__1 = *n;
16279         for (n0 = 1; n0 <= i__1; ++n0) {
16280             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16281                 goto L4;
16282             }
16283             p0[0] = r11 * x[n0] + r12 * y[n0];
16284             p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16285             if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16286                 goto L4;
16287             }
16288 
16289 /*   Move to P0 and draw the label N0.  The first character */
16290 /*     will will have its lower left corner about one */
16291 /*     character width to the right of the nodal position. */
16292 
16293 /*          WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16294 /*          WRITE (LUN,220,ERR=13) N0 */
16295 /*  210     FORMAT (2F12.6,' moveto') */
16296 /*  220     FORMAT ('(',I3,') show') */
16297 L4:
16298             ;
16299         }
16300     }
16301 
16302 /* Convert FSIZT from points to world coordinates, and output */
16303 /*   the commands to select a font and scale it. */
16304 
16305     t = fsizt / sf;
16306 /*      WRITE (LUN,200,ERR=13) T */
16307 
16308 /* Display TITLE centered above the plot: */
16309 
16310     p0[1] = wr + t * 3.;
16311 /*      WRITE (LUN,230,ERR=13) TITLE, P0(2) */
16312 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
16313 /*     .        ' moveto') */
16314 /*      WRITE (LUN,240,ERR=13) TITLE */
16315 /*  240 FORMAT (A80/'  show') */
16316     if (annot) {
16317 
16318 /* Display the window center and radius below the plot. */
16319 
16320         p0[0] = -wr;
16321         p0[1] = -wr - 50. / sf;
16322 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16323 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
16324         p0[1] -= t * 2.;
16325 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16326 /*        WRITE (LUN,260,ERR=13) A */
16327 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
16328 /*     .          ',  ELON = ',F8.2,') show') */
16329 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
16330     }
16331 
16332 /* Paint the path and output the showpage command and */
16333 /*   end-of-file indicator. */
16334 
16335 /*      WRITE (LUN,270,ERR=13) */
16336 /*  270 FORMAT ('stroke'/ */
16337 /*     .        'showpage'/ */
16338 /*     .        '%%EOF') */
16339 
16340 /* HP's interpreters require a one-byte End-of-PostScript-Job */
16341 /*   indicator (to eliminate a timeout error message): */
16342 /*   ASCII 4. */
16343 
16344 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
16345 /*  280 FORMAT (A1) */
16346 
16347 /* No error encountered. */
16348 
16349     *ier = 0;
16350     return 0;
16351 
16352 /* Invalid input parameter LUN, PLTSIZ, or N. */
16353 
16354 L11:
16355     *ier = 1;
16356     return 0;
16357 
16358 /* Invalid input parameter ELAT, ELON, or A. */
16359 
16360 L12:
16361     *ier = 2;
16362     return 0;
16363 
16364 /* Error writing to unit LUN. */
16365 
16366 /* L13: */
16367     *ier = 3;
16368     return 0;
16369 } /* trplot_ */
16370 
16371 /* Subroutine */ int trprnt_(int *n, double *x, double *y,
16372         double *z__, int *iflag, int *list, int *lptr,
16373         int *lend, int *lout)
16374 {
16375     /* Initialized data */
16376 
16377     static int nmax = 9999;
16378     static int nlmax = 58;
16379 
16380     /* System generated locals */
16381     int i__1;
16382 
16383     /* Local variables */
16384     static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16385             400];
16386 
16387 
16388 /* *********************************************************** */
16389 
16390 /*                                              From STRIPACK */
16391 /*                                            Robert J. Renka */
16392 /*                                  Dept. of Computer Science */
16393 /*                                       Univ. of North Texas */
16394 /*                                           renka@cs.unt.edu */
16395 /*                                                   07/25/98 */
16396 
16397 /*   This subroutine prints the triangulation adjacency lists */
16398 /* created by Subroutine TRMESH and, optionally, the nodal */
16399 /* coordinates (either latitude and longitude or Cartesian */
16400 /* coordinates) on long int unit LOUT.  The list of neighbors */
16401 /* of a boundary node is followed by index 0.  The numbers of */
16402 /* boundary nodes, triangles, and arcs are also printed. */
16403 
16404 
16405 /* On input: */
16406 
16407 /*       N = Number of nodes in the triangulation.  N .GE. 3 */
16408 /*           and N .LE. 9999. */
16409 
16410 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16411 /*               coordinates of the nodes if IFLAG = 0, or */
16412 /*               (X and Y only) arrays of length N containing */
16413 /*               longitude and latitude, respectively, if */
16414 /*               IFLAG > 0, or unused dummy parameters if */
16415 /*               IFLAG < 0. */
16416 
16417 /*       IFLAG = Nodal coordinate option indicator: */
16418 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
16419 /*                         Cartesian coordinates) are to be */
16420 /*                         printed (to 6 decimal places). */
16421 /*               IFLAG > 0 if only X and Y (assumed to con- */
16422 /*                         tain longitude and latitude) are */
16423 /*                         to be printed (to 6 decimal */
16424 /*                         places). */
16425 /*               IFLAG < 0 if only the adjacency lists are to */
16426 /*                         be printed. */
16427 
16428 /*       LIST,LPTR,LEND = Data structure defining the trian- */
16429 /*                        gulation.  Refer to Subroutine */
16430 /*                        TRMESH. */
16431 
16432 /*       LOUT = long int unit for output.  If LOUT is not in */
16433 /*              the range 0 to 99, output is written to */
16434 /*              long int unit 6. */
16435 
16436 /* Input parameters are not altered by this routine. */
16437 
16438 /* On output: */
16439 
16440 /*   The adjacency lists and nodal coordinates (as specified */
16441 /* by IFLAG) are written to unit LOUT. */
16442 
16443 /* Modules required by TRPRNT:  None */
16444 
16445 /* *********************************************************** */
16446 
16447     /* Parameter adjustments */
16448     --lend;
16449     --z__;
16450     --y;
16451     --x;
16452     --list;
16453     --lptr;
16454 
16455     /* Function Body */
16456 
16457 /* Local parameters: */
16458 
16459 /* I =     NABOR index (1 to K) */
16460 /* INC =   Increment for NL associated with an adjacency list */
16461 /* K =     Counter and number of neighbors of NODE */
16462 /* LP =    LIST pointer of a neighbor of NODE */
16463 /* LPL =   Pointer to the last neighbor of NODE */
16464 /* LUN =   long int unit for output (copy of LOUT) */
16465 /* NA =    Number of arcs in the triangulation */
16466 /* NABOR = Array containing the adjacency list associated */
16467 /*           with NODE, with zero appended if NODE is a */
16468 /*           boundary node */
16469 /* NB =    Number of boundary nodes encountered */
16470 /* ND =    Index of a neighbor of NODE (or negative index) */
16471 /* NL =    Number of lines that have been printed on the */
16472 /*           current page */
16473 /* NLMAX = Maximum number of print lines per page (except */
16474 /*           for the last page which may have two addi- */
16475 /*           tional lines) */
16476 /* NMAX =  Upper bound on N (allows 4-digit indexes) */
16477 /* NODE =  Index of a node and DO-loop index (1 to N) */
16478 /* NN =    Local copy of N */
16479 /* NT =    Number of triangles in the triangulation */
16480 
16481     nn = *n;
16482     lun = *lout;
16483     if (lun < 0 || lun > 99) {
16484         lun = 6;
16485     }
16486 
16487 /* Print a heading and test the range of N. */
16488 
16489 /*      WRITE (LUN,100) NN */
16490     if (nn < 3 || nn > nmax) {
16491 
16492 /* N is outside its valid range. */
16493 
16494 /*        WRITE (LUN,110) */
16495         return 0;
16496     }
16497 
16498 /* Initialize NL (the number of lines printed on the current */
16499 /*   page) and NB (the number of boundary nodes encountered). */
16500 
16501     nl = 6;
16502     nb = 0;
16503     if (*iflag < 0) {
16504 
16505 /* Print LIST only.  K is the number of neighbors of NODE */
16506 /*   that have been stored in NABOR. */
16507 
16508 /*        WRITE (LUN,101) */
16509         i__1 = nn;
16510         for (node = 1; node <= i__1; ++node) {
16511             lpl = lend[node];
16512             lp = lpl;
16513             k = 0;
16514 
16515 L1:
16516             ++k;
16517             lp = lptr[lp];
16518             nd = list[lp];
16519             nabor[k - 1] = nd;
16520             if (lp != lpl) {
16521                 goto L1;
16522             }
16523             if (nd <= 0) {
16524 
16525 /*   NODE is a boundary node.  Correct the sign of the last */
16526 /*     neighbor, add 0 to the end of the list, and increment */
16527 /*     NB. */
16528 
16529                 nabor[k - 1] = -nd;
16530                 ++k;
16531                 nabor[k - 1] = 0;
16532                 ++nb;
16533             }
16534 
16535 /*   Increment NL and print the list of neighbors. */
16536 
16537             inc = (k - 1) / 14 + 2;
16538             nl += inc;
16539             if (nl > nlmax) {
16540 /*            WRITE (LUN,108) */
16541                 nl = inc;
16542             }
16543 /*          WRITE (LUN,104) NODE, (NABOR(I), I = 1,K) */
16544 /*          IF (K .NE. 14) */
16545 /*           WRITE (LUN,107) */
16546 /* L2: */
16547         }
16548     } else if (*iflag > 0) {
16549 
16550 /* Print X (longitude), Y (latitude), and LIST. */
16551 
16552 /*        WRITE (LUN,102) */
16553         i__1 = nn;
16554         for (node = 1; node <= i__1; ++node) {
16555             lpl = lend[node];
16556             lp = lpl;
16557             k = 0;
16558 
16559 L3:
16560             ++k;
16561             lp = lptr[lp];
16562             nd = list[lp];
16563             nabor[k - 1] = nd;
16564             if (lp != lpl) {
16565                 goto L3;
16566             }
16567             if (nd <= 0) {
16568 
16569 /*   NODE is a boundary node. */
16570 
16571                 nabor[k - 1] = -nd;
16572                 ++k;
16573                 nabor[k - 1] = 0;
16574                 ++nb;
16575             }
16576 
16577 /*   Increment NL and print X, Y, and NABOR. */
16578 
16579             inc = (k - 1) / 8 + 2;
16580             nl += inc;
16581             if (nl > nlmax) {
16582 /*            WRITE (LUN,108) */
16583                 nl = inc;
16584             }
16585 /*          WRITE (LUN,105) NODE, X(NODE), Y(NODE), (NABOR(I), I = 1,K) */
16586 /*          IF (K .NE. 8) */
16587 /*           PRINT *,K */
16588 /*           WRITE (LUN,107) */
16589 /* L4: */
16590         }
16591     } else {
16592 
16593 /* Print X, Y, Z, and LIST. */
16594 
16595 /*        WRITE (LUN,103) */
16596         i__1 = nn;
16597         for (node = 1; node <= i__1; ++node) {
16598             lpl = lend[node];
16599             lp = lpl;
16600             k = 0;
16601 
16602 L5:
16603             ++k;
16604             lp = lptr[lp];
16605             nd = list[lp];
16606             nabor[k - 1] = nd;
16607             if (lp != lpl) {
16608                 goto L5;
16609             }
16610             if (nd <= 0) {
16611 
16612 /*   NODE is a boundary node. */
16613 
16614                 nabor[k - 1] = -nd;
16615                 ++k;
16616                 nabor[k - 1] = 0;
16617                 ++nb;
16618             }
16619 
16620 /*   Increment NL and print X, Y, Z, and NABOR. */
16621 
16622             inc = (k - 1) / 5 + 2;
16623             nl += inc;
16624             if (nl > nlmax) {
16625 /*            WRITE (LUN,108) */
16626                 nl = inc;
16627             }
16628 /*          WRITE (LUN,106) NODE, X(NODE), Y(NODE),Z(NODE), (NABOR(I), I = 1,K) */
16629 /*          IF (K .NE. 5) */
16630 /*           print *,K */
16631 /*           WRITE (LUN,107) */
16632 /* L6: */
16633         }
16634     }
16635 
16636 /* Print NB, NA, and NT (boundary nodes, arcs, and */
16637 /*   triangles). */
16638 
16639     if (nb != 0) {
16640         na = nn * 3 - nb - 3;
16641         nt = (nn << 1) - nb - 2;
16642     } else {
16643         na = nn * 3 - 6;
16644         nt = (nn << 1) - 4;
16645     }
16646 /*      WRITE (LUN,109) NB, NA, NT */
16647     return 0;
16648 
16649 /* Print formats: */
16650 
16651 /*  100 FORMAT (///15X,'STRIPACK Triangulation Data ', */
16652 /*     .        'Structure,  N = ',I5//) */
16653 /*  101 FORMAT (1X,'Node',31X,'Neighbors of Node'//) */
16654 /*  102 FORMAT (1X,'Node',5X,'Longitude',6X,'Latitude', */
16655 /*     .        18X,'Neighbors of Node'//) */
16656 /*  103 FORMAT (1X,'Node',5X,'X(Node)',8X,'Y(Node)',8X, */
16657 /*     .        'Z(Node)',11X,'Neighbors of Node'//) */
16658 /*  104 FORMAT (1X,I4,4X,14I5/(1X,8X,14I5)) */
16659 /*  105 FORMAT (1X,I4,2D15.6,4X,8I5/(1X,38X,8I5)) */
16660 /*  106 FORMAT (1X,I4,3D15.6,4X,5I5/(1X,53X,5I5)) */
16661 /*  107 FORMAT (1X) */
16662 /*  108 FORMAT (///) */
16663 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
16664 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
16665 /*     .        ' Triangles') */
16666 /*  110 FORMAT (1X,10X,'*** N is outside its valid', */
16667 /*     .        ' range ***') */
16668 } /* trprnt_ */
16669 
16670 /* Subroutine */ int vrplot_(int *lun, double *pltsiz, double *
16671         elat, double *elon, double *a, int *n, double *x,
16672         double *y, double *z__, int *nt, int *listc, int *
16673         lptr, int *lend, double *xc, double *yc, double *zc,
16674         char *, long int *numbr, int *ier, short)
16675 {
16676     /* Initialized data */
16677 
16678     static long int annot = TRUE_;
16679     static double fsizn = 10.;
16680     static double fsizt = 16.;
16681     static double tol = .5;
16682 
16683     /* System generated locals */
16684     int i__1;
16685     double d__1;
16686 
16687     /* Builtin functions */
16688     //double atan(double), sin(double);
16689     //int i_dnnt(double *);
16690     //double cos(double), sqrt(double);
16691 
16692     /* Local variables */
16693     static double t;
16694     static int n0;
16695     static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16696             sf;
16697     static int ir, lp;
16698     static double ex, ey, ez, wr, tx, ty;
16699     static long int in1, in2;
16700     static int kv1, kv2, lpl;
16701     static double wrs;
16702     static int ipx1, ipx2, ipy1, ipy2, nseg;
16703     extern /* Subroutine */ int drwarc_(int *, double *, double *,
16704              double *, int *);
16705 
16706 
16707 /* *********************************************************** */
16708 
16709 /*                                              From STRIPACK */
16710 /*                                            Robert J. Renka */
16711 /*                                  Dept. of Computer Science */
16712 /*                                       Univ. of North Texas */
16713 /*                                           renka@cs.unt.edu */
16714 /*                                                   03/04/03 */
16715 
16716 /*   This subroutine creates a level-2 Encapsulated Post- */
16717 /* script (EPS) file containing a graphical depiction of a */
16718 /* Voronoi diagram of a set of nodes on the unit sphere. */
16719 /* The visible portion of the diagram is projected orthog- */
16720 /* onally onto the plane that contains the origin and has */
16721 /* normal defined by a user-specified eye-position. */
16722 
16723 /*   The parameters defining the Voronoi diagram may be com- */
16724 /* puted by Subroutine CRLIST. */
16725 
16726 
16727 /* On input: */
16728 
16729 /*       LUN = long int unit number in the range 0 to 99. */
16730 /*             The unit should be opened with an appropriate */
16731 /*             file name before the call to this routine. */
16732 
16733 /*       PLTSIZ = Plot size in inches.  A circular window in */
16734 /*                the projection plane is mapped to a circu- */
16735 /*                lar viewport with diameter equal to .88* */
16736 /*                PLTSIZ (leaving room for labels outside the */
16737 /*                viewport).  The viewport is centered on the */
16738 /*                8.5 by 11 inch page, and its boundary is */
16739 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
16740 
16741 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
16742 /*                   the center of projection E (the center */
16743 /*                   of the plot).  The projection plane is */
16744 /*                   the plane that contains the origin and */
16745 /*                   has E as unit normal.  In a rotated */
16746 /*                   coordinate system for which E is the */
16747 /*                   north pole, the projection plane con- */
16748 /*                   tains the equator, and only northern */
16749 /*                   hemisphere points are visible (from the */
16750 /*                   point at infinity in the direction E). */
16751 /*                   These are projected orthogonally onto */
16752 /*                   the projection plane (by zeroing the z- */
16753 /*                   component in the rotated coordinate */
16754 /*                   system).  ELAT and ELON must be in the */
16755 /*                   range -90 to 90 and -180 to 180, respec- */
16756 /*                   tively. */
16757 
16758 /*       A = Angular distance in degrees from E to the boun- */
16759 /*           dary of a circular window against which the */
16760 /*           Voronoi diagram is clipped.  The projected win- */
16761 /*           dow is a disk of radius r = Sin(A) centered at */
16762 /*           the origin, and only visible vertices whose */
16763 /*           projections are within distance r of the origin */
16764 /*           are included in the plot.  Thus, if A = 90, the */
16765 /*           plot includes the entire hemisphere centered at */
16766 /*           E.  0 .LT. A .LE. 90. */
16767 
16768 /*       N = Number of nodes (Voronoi centers) and Voronoi */
16769 /*           regions.  N .GE. 3. */
16770 
16771 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16772 /*               coordinates of the nodes (unit vectors). */
16773 
16774 /*       NT = Number of Voronoi region vertices (triangles, */
16775 /*            including those in the extended triangulation */
16776 /*            if the number of boundary nodes NB is nonzero): */
16777 /*            NT = 2*N-4. */
16778 
16779 /*       LISTC = Array of length 3*NT containing triangle */
16780 /*               indexes (indexes to XC, YC, and ZC) stored */
16781 /*               in 1-1 correspondence with LIST/LPTR entries */
16782 /*               (or entries that would be stored in LIST for */
16783 /*               the extended triangulation):  the index of */
16784 /*               triangle (N1,N2,N3) is stored in LISTC(K), */
16785 /*               LISTC(L), and LISTC(M), where LIST(K), */
16786 /*               LIST(L), and LIST(M) are the indexes of N2 */
16787 /*               as a neighbor of N1, N3 as a neighbor of N2, */
16788 /*               and N1 as a neighbor of N3.  The Voronoi */
16789 /*               region associated with a node is defined by */
16790 /*               the CCW-ordered sequence of circumcenters in */
16791 /*               one-to-one correspondence with its adjacency */
16792 /*               list (in the extended triangulation). */
16793 
16794 /*       LPTR = Array of length 3*NT = 6*N-12 containing a */
16795 /*              set of pointers (LISTC indexes) in one-to-one */
16796 /*              correspondence with the elements of LISTC. */
16797 /*              LISTC(LPTR(I)) indexes the triangle which */
16798 /*              follows LISTC(I) in cyclical counterclockwise */
16799 /*              order (the first neighbor follows the last */
16800 /*              neighbor). */
16801 
16802 /*       LEND = Array of length N containing a set of */
16803 /*              pointers to triangle lists.  LP = LEND(K) */
16804 /*              points to a triangle (indexed by LISTC(LP)) */
16805 /*              containing node K for K = 1 to N. */
16806 
16807 /*       XC,YC,ZC = Arrays of length NT containing the */
16808 /*                  Cartesian coordinates of the triangle */
16809 /*                  circumcenters (Voronoi vertices). */
16810 /*                  XC(I)**2 + YC(I)**2 + ZC(I)**2 = 1. */
16811 
16812 /*       TITLE = Type CHARACTER variable or constant contain- */
16813 /*               ing a string to be centered above the plot. */
16814 /*               The string must be enclosed in parentheses; */
16815 /*               i.e., the first and last characters must be */
16816 /*               '(' and ')', respectively, but these are not */
16817 /*               displayed.  TITLE may have at most 80 char- */
16818 /*               acters including the parentheses. */
16819 
16820 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
16821 /*               nodal indexes are plotted at the Voronoi */
16822 /*               region centers. */
16823 
16824 /* Input parameters are not altered by this routine. */
16825 
16826 /* On output: */
16827 
16828 /*       IER = Error indicator: */
16829 /*             IER = 0 if no errors were encountered. */
16830 /*             IER = 1 if LUN, PLTSIZ, N, or NT is outside */
16831 /*                     its valid range. */
16832 /*             IER = 2 if ELAT, ELON, or A is outside its */
16833 /*                     valid range. */
16834 /*             IER = 3 if an error was encountered in writing */
16835 /*                     to unit LUN. */
16836 
16837 /* Module required by VRPLOT:  DRWARC */
16838 
16839 /* Intrinsic functions called by VRPLOT:  ABS, ATAN, COS, */
16840 /*                                          DBLE, NINT, SIN, */
16841 /*                                          SQRT */
16842 
16843 /* *********************************************************** */
16844 
16845 
16846     /* Parameter adjustments */
16847     --lend;
16848     --z__;
16849     --y;
16850     --x;
16851     --zc;
16852     --yc;
16853     --xc;
16854     --listc;
16855     --lptr;
16856 
16857     /* Function Body */
16858 
16859 /* Local parameters: */
16860 
16861 /* ANNOT =     long int variable with value TRUE iff the plot */
16862 /*               is to be annotated with the values of ELAT, */
16863 /*               ELON, and A */
16864 /* CF =        Conversion factor for degrees to radians */
16865 /* CT =        Cos(ELAT) */
16866 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16867 /* FSIZN =     Font size in points for labeling nodes with */
16868 /*               their indexes if NUMBR = TRUE */
16869 /* FSIZT =     Font size in points for the title (and */
16870 /*               annotation if ANNOT = TRUE) */
16871 /* IN1,IN2 =   long int variables with value TRUE iff the */
16872 /*               projections of vertices KV1 and KV2, respec- */
16873 /*               tively, are inside the window */
16874 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16875 /*               left corner of the bounding box or viewport */
16876 /*               box */
16877 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16878 /*               right corner of the bounding box or viewport */
16879 /*               box */
16880 /* IR =        Half the width (height) of the bounding box or */
16881 /*               viewport box in points -- viewport radius */
16882 /* KV1,KV2 =   Endpoint indexes of a Voronoi edge */
16883 /* LP =        LIST index (pointer) */
16884 /* LPL =       Pointer to the last neighbor of N0 */
16885 /* N0 =        Index of a node */
16886 /* NSEG =      Number of line segments used by DRWARC in a */
16887 /*               polygonal approximation to a projected edge */
16888 /* P1 =        Coordinates of vertex KV1 in the rotated */
16889 /*               coordinate system */
16890 /* P2 =        Coordinates of vertex KV2 in the rotated */
16891 /*               coordinate system or intersection of edge */
16892 /*               KV1-KV2 with the equator (in the rotated */
16893 /*               coordinate system) */
16894 /* R11...R23 = Components of the first two rows of a rotation */
16895 /*               that maps E to the north pole (0,0,1) */
16896 /* SF =        Scale factor for mapping world coordinates */
16897 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16898 /*               to viewport coordinates in [IPX1,IPX2] X */
16899 /*               [IPY1,IPY2] */
16900 /* T =         Temporary variable */
16901 /* TOL =       Maximum distance in points between a projected */
16902 /*               Voronoi edge and its approximation by a */
16903 /*               polygonal curve */
16904 /* TX,TY =     Translation vector for mapping world coordi- */
16905 /*               nates to viewport coordinates */
16906 /* WR =        Window radius r = Sin(A) */
16907 /* WRS =       WR**2 */
16908 /* X0,Y0 =     Projection plane coordinates of node N0 or */
16909 /*               label location */
16910 
16911 
16912 /* Test for invalid parameters. */
16913 
16914     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
16915             nt != 2 * *n - 4) {
16916         goto L11;
16917     }
16918     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16919         goto L12;
16920     }
16921 
16922 /* Compute a conversion factor CF for degrees to radians */
16923 /*   and compute the window radius WR. */
16924 
16925     cf = atan(1.) / 45.;
16926     wr = sin(cf * *a);
16927     wrs = wr * wr;
16928 
16929 /* Compute the lower left (IPX1,IPY1) and upper right */
16930 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16931 /*   The coordinates, specified in default user space units */
16932 /*   (points, at 72 points/inch with origin at the lower */
16933 /*   left corner of the page), are chosen to preserve the */
16934 /*   square aspect ratio, and to center the plot on the 8.5 */
16935 /*   by 11 inch page.  The center of the page is (306,396), */
16936 /*   and IR = PLTSIZ/2 in points. */
16937 
16938     d__1 = *pltsiz * 36.;
16939     ir = i_dnnt(&d__1);
16940     ipx1 = 306 - ir;
16941     ipx2 = ir + 306;
16942     ipy1 = 396 - ir;
16943     ipy2 = ir + 396;
16944 
16945 /* Output header comments. */
16946 
16947 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16948 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16949 /*     .        '%%BoundingBox:',4I4/ */
16950 /*     .        '%%Title:  Voronoi diagram'/ */
16951 /*     .        '%%Creator:  STRIPACK'/ */
16952 /*     .        '%%EndComments') */
16953 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16954 /*   of a viewport box obtained by shrinking the bounding box */
16955 /*   by 12% in each dimension. */
16956 
16957     d__1 = (double) ir * .88;
16958     ir = i_dnnt(&d__1);
16959     ipx1 = 306 - ir;
16960     ipx2 = ir + 306;
16961     ipy1 = 396 - ir;
16962     ipy2 = ir + 396;
16963 
16964 /* Set the line thickness to 2 points, and draw the */
16965 /*   viewport boundary. */
16966 
16967     t = 2.;
16968 /*      WRITE (LUN,110,ERR=13) T */
16969 /*      WRITE (LUN,120,ERR=13) IR */
16970 /*      WRITE (LUN,130,ERR=13) */
16971 /*  110 FORMAT (F12.6,' setlinewidth') */
16972 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16973 /*  130 FORMAT ('stroke') */
16974 
16975 /* Set up an affine mapping from the window box [-WR,WR] X */
16976 /*   [-WR,WR] to the viewport box. */
16977 
16978     sf = (double) ir / wr;
16979     tx = ipx1 + sf * wr;
16980     ty = ipy1 + sf * wr;
16981 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16982 /*  140 FORMAT (2F12.6,' translate'/ */
16983 /*     .        2F12.6,' scale') */
16984 
16985 /* The line thickness must be changed to reflect the new */
16986 /*   scaling which is applied to all subsequent output. */
16987 /*   Set it to 1.0 point. */
16988 
16989     t = 1. / sf;
16990 /*      WRITE (LUN,110,ERR=13) T */
16991 
16992 /* Save the current graphics state, and set the clip path to */
16993 /*   the boundary of the window. */
16994 
16995 /*      WRITE (LUN,150,ERR=13) */
16996 /*      WRITE (LUN,160,ERR=13) WR */
16997 /*      WRITE (LUN,170,ERR=13) */
16998 /*  150 FORMAT ('gsave') */
16999 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
17000 /*  170 FORMAT ('clip newpath') */
17001 
17002 /* Compute the Cartesian coordinates of E and the components */
17003 /*   of a rotation R which maps E to the north pole (0,0,1). */
17004 /*   R is taken to be a rotation about the z-axis (into the */
17005 /*   yz-plane) followed by a rotation about the x-axis chosen */
17006 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
17007 /*   E is the north or south pole. */
17008 
17009 /*           ( R11  R12  0   ) */
17010 /*       R = ( R21  R22  R23 ) */
17011 /*           ( EX   EY   EZ  ) */
17012 
17013     t = cf * *elon;
17014     ct = cos(cf * *elat);
17015     ex = ct * cos(t);
17016     ey = ct * sin(t);
17017     ez = sin(cf * *elat);
17018     if (ct != 0.) {
17019         r11 = -ey / ct;
17020         r12 = ex / ct;
17021     } else {
17022         r11 = 0.;
17023         r12 = 1.;
17024     }
17025     r21 = -ez * r12;
17026     r22 = ez * r11;
17027     r23 = ct;
17028 
17029 /* Loop on nodes (Voronoi centers) N0. */
17030 /*   LPL indexes the last neighbor of N0. */
17031 
17032     i__1 = *n;
17033     for (n0 = 1; n0 <= i__1; ++n0) {
17034         lpl = lend[n0];
17035 
17036 /* Set KV2 to the first (and last) vertex index and compute */
17037 /*   its coordinates P2 in the rotated coordinate system. */
17038 
17039         kv2 = listc[lpl];
17040         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17041         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17042         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17043 
17044 /*   IN2 = TRUE iff KV2 is in the window. */
17045 
17046         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17047 
17048 /* Loop on neighbors N1 of N0.  For each triangulation edge */
17049 /*   N0-N1, KV1-KV2 is the corresponding Voronoi edge. */
17050 
17051         lp = lpl;
17052 L1:
17053         lp = lptr[lp];
17054         kv1 = kv2;
17055         p1[0] = p2[0];
17056         p1[1] = p2[1];
17057         p1[2] = p2[2];
17058         in1 = in2;
17059         kv2 = listc[lp];
17060 
17061 /*   Compute the new values of P2 and IN2. */
17062 
17063         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17064         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17065         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17066         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17067 
17068 /* Add edge KV1-KV2 to the path iff both endpoints are inside */
17069 /*   the window and KV2 > KV1, or KV1 is inside and KV2 is */
17070 /*   outside (so that the edge is drawn only once). */
17071 
17072         if (! in1 || (in2 && kv2 <= kv1)) {
17073             goto L2;
17074         }
17075         if (p2[2] < 0.) {
17076 
17077 /*   KV2 is a 'southern hemisphere' point.  Move it to the */
17078 /*     intersection of edge KV1-KV2 with the equator so that */
17079 /*     the edge is clipped properly.  P2(3) is set to 0. */
17080 
17081             p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17082             p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17083             t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17084             p2[0] /= t;
17085             p2[1] /= t;
17086         }
17087 
17088 /*   Add the edge to the path.  (TOL is converted to world */
17089 /*     coordinates.) */
17090 
17091         if (p2[2] < 0.) {
17092             p2[2] = 0.f;
17093         }
17094         d__1 = tol / sf;
17095         drwarc_(lun, p1, p2, &d__1, &nseg);
17096 
17097 /* Bottom of loops. */
17098 
17099 L2:
17100         if (lp != lpl) {
17101             goto L1;
17102         }
17103 /* L3: */
17104     }
17105 
17106 /* Paint the path and restore the saved graphics state (with */
17107 /*   no clip path). */
17108 
17109 /*      WRITE (LUN,130,ERR=13) */
17110 /*      WRITE (LUN,190,ERR=13) */
17111 /*  190 FORMAT ('grestore') */
17112     if (*numbr) {
17113 
17114 /* Nodes in the window are to be labeled with their indexes. */
17115 /*   Convert FSIZN from points to world coordinates, and */
17116 /*   output the commands to select a font and scale it. */
17117 
17118         t = fsizn / sf;
17119 /*        WRITE (LUN,200,ERR=13) T */
17120 /*  200   FORMAT ('/Helvetica findfont'/ */
17121 /*     .          F12.6,' scalefont setfont') */
17122 
17123 /* Loop on visible nodes N0 that project to points (X0,Y0) in */
17124 /*   the window. */
17125 
17126         i__1 = *n;
17127         for (n0 = 1; n0 <= i__1; ++n0) {
17128             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17129                 goto L4;
17130             }
17131             x0 = r11 * x[n0] + r12 * y[n0];
17132             y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17133             if (x0 * x0 + y0 * y0 > wrs) {
17134                 goto L4;
17135             }
17136 
17137 /*   Move to (X0,Y0), and draw the label N0 with the origin */
17138 /*     of the first character at (X0,Y0). */
17139 
17140 /*          WRITE (LUN,210,ERR=13) X0, Y0 */
17141 /*          WRITE (LUN,220,ERR=13) N0 */
17142 /*  210     FORMAT (2F12.6,' moveto') */
17143 /*  220     FORMAT ('(',I3,') show') */
17144 L4:
17145             ;
17146         }
17147     }
17148 
17149 /* Convert FSIZT from points to world coordinates, and output */
17150 /*   the commands to select a font and scale it. */
17151 
17152     t = fsizt / sf;
17153 /*      WRITE (LUN,200,ERR=13) T */
17154 
17155 /* Display TITLE centered above the plot: */
17156 
17157     y0 = wr + t * 3.;
17158 /*      WRITE (LUN,230,ERR=13) TITLE, Y0 */
17159 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
17160 /*     .        ' moveto') */
17161 /*      WRITE (LUN,240,ERR=13) TITLE */
17162 /*  240 FORMAT (A80/'  show') */
17163     if (annot) {
17164 
17165 /* Display the window center and radius below the plot. */
17166 
17167         x0 = -wr;
17168         y0 = -wr - 50. / sf;
17169 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17170 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
17171         y0 -= t * 2.;
17172 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17173 /*        WRITE (LUN,260,ERR=13) A */
17174 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
17175 /*     .          ',  ELON = ',F8.2,') show') */
17176 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
17177     }
17178 
17179 /* Paint the path and output the showpage command and */
17180 /*   end-of-file indicator. */
17181 
17182 /*      WRITE (LUN,270,ERR=13) */
17183 /*  270 FORMAT ('stroke'/ */
17184 /*     .        'showpage'/ */
17185 /*     .        '%%EOF') */
17186 
17187 /* HP's interpreters require a one-byte End-of-PostScript-Job */
17188 /*   indicator (to eliminate a timeout error message): */
17189 /*   ASCII 4. */
17190 
17191 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
17192 /*  280 FORMAT (A1) */
17193 
17194 /* No error encountered. */
17195 
17196     *ier = 0;
17197     return 0;
17198 
17199 /* Invalid input parameter LUN, PLTSIZ, N, or NT. */
17200 
17201 L11:
17202     *ier = 1;
17203     return 0;
17204 
17205 /* Invalid input parameter ELAT, ELON, or A. */
17206 
17207 L12:
17208     *ier = 2;
17209     return 0;
17210 
17211 /* Error writing to unit LUN. */
17212 
17213 /* L13: */
17214     *ier = 3;
17215     return 0;
17216 } /* vrplot_ */
17217 
17218 /* Subroutine */ int random_(int *ix, int *iy, int *iz,
17219         double *rannum)
17220 {
17221     static double x;
17222 
17223 
17224 /*   This routine returns pseudo-random numbers uniformly */
17225 /* distributed in the interval (0,1).  int seeds IX, IY, */
17226 /* and IZ should be initialized to values in the range 1 to */
17227 /* 30,000 before the first call to RANDOM, and should not */
17228 /* be altered between subsequent calls (unless a sequence */
17229 /* of random numbers is to be repeated by reinitializing the */
17230 /* seeds). */
17231 
17232 /* Reference:  B. A. Wichmann and I. D. Hill, An Efficient */
17233 /*             and Portable Pseudo-random Number Generator, */
17234 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
17235 /*             pp. 188-190. */
17236 
17237     *ix = *ix * 171 % 30269;
17238     *iy = *iy * 172 % 30307;
17239     *iz = *iz * 170 % 30323;
17240     x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17241             double) (*iz) / 30323.;
17242     *rannum = x - (int) x;
17243     return 0;
17244 } /* random_ */
17245 
17246 #undef TRUE_
17247 #undef FALSE_
17248 #undef abs
17249 
17250 /*################################################################################################
17251 ##########  strid.f -- translated by f2c (version 20030320). ###################################
17252 ######   You must link the resulting object file with the libraries: #############################
17253 ####################    -lf2c -lm   (in that order)   ############################################
17254 ################################################################################################*/
17255 
17256 
17257 
17258 EMData* Util::mult_scalar(EMData* img, float scalar)
17259 {
17260         ENTERFUNC;
17261         /* Exception Handle */
17262         if (!img) {
17263                 throw NullPointerException("NULL input image");
17264         }
17265         /* ============  output = scalar*input  ================== */
17266 
17267         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17268         size_t size = (size_t)nx*ny*nz;
17269         EMData * img2 = img->copy_head();
17270         float *img_ptr  =img->get_data();
17271         float *img2_ptr = img2->get_data();
17272         for (size_t i=0;i<size;++i)img2_ptr[i] = img_ptr[i]*scalar;
17273         img2->update();
17274 
17275         if(img->is_complex()) {
17276                 img2->set_complex(true);
17277                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17278         }
17279         EXITFUNC;
17280         return img2;
17281 }
17282 
17283 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17284 {
17285         ENTERFUNC;
17286         /* Exception Handle */
17287         if (!img) {
17288                 throw NullPointerException("NULL input image");
17289         }
17290         /* ==============   output = img + scalar*img1   ================ */
17291 
17292         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17293         size_t size = (size_t)nx*ny*nz;
17294         EMData * img2 = img->copy_head();
17295         float *img_ptr  =img->get_data();
17296         float *img2_ptr = img2->get_data();
17297         float *img1_ptr = img1->get_data();
17298         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17299         img2->update();
17300         if(img->is_complex()) {
17301                 img2->set_complex(true);
17302                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17303         }
17304 
17305         EXITFUNC;
17306         return img2;
17307 }
17308 
17309 EMData* Util::addn_img(EMData* img, EMData* img1)
17310 {
17311         ENTERFUNC;
17312         /* Exception Handle */
17313         if (!img) {
17314                 throw NullPointerException("NULL input image");
17315         }
17316         /* ==============   output = img + img1   ================ */
17317 
17318         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17319         size_t size = (size_t)nx*ny*nz;
17320         EMData * img2 = img->copy_head();
17321         float *img_ptr  =img->get_data();
17322         float *img2_ptr = img2->get_data();
17323         float *img1_ptr = img1->get_data();
17324         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17325         img2->update();
17326         if(img->is_complex()) {
17327                 img2->set_complex(true);
17328                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17329         }
17330 
17331         EXITFUNC;
17332         return img2;
17333 }
17334 
17335 EMData* Util::subn_img(EMData* img, EMData* img1)
17336 {
17337         ENTERFUNC;
17338         /* Exception Handle */
17339         if (!img) {
17340                 throw NullPointerException("NULL input image");
17341         }
17342         /* ==============   output = img - img1   ================ */
17343 
17344         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17345         size_t size = (size_t)nx*ny*nz;
17346         EMData * img2 = img->copy_head();
17347         float *img_ptr  =img->get_data();
17348         float *img2_ptr = img2->get_data();
17349         float *img1_ptr = img1->get_data();
17350         for (size_t i=0;i<size;++i) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17351         img2->update();
17352         if(img->is_complex()) {
17353                 img2->set_complex(true);
17354                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17355         }
17356 
17357         EXITFUNC;
17358         return img2;
17359 }
17360 
17361 EMData* Util::muln_img(EMData* img, EMData* img1)
17362 {
17363         ENTERFUNC;
17364         /* Exception Handle */
17365         if (!img) {
17366                 throw NullPointerException("NULL input image");
17367         }
17368         /* ==============   output = img * img1   ================ */
17369 
17370         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17371         size_t size = (size_t)nx*ny*nz;
17372         EMData * img2 = img->copy_head();
17373         float *img_ptr  =img->get_data();
17374         float *img2_ptr = img2->get_data();
17375         float *img1_ptr = img1->get_data();
17376         if(img->is_complex()) {
17377                 for (size_t i=0; i<size; i+=2) {
17378                         img2_ptr[i]   = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17379                         img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17380                 }
17381                 img2->set_complex(true);
17382                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17383         } else {
17384                 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17385                 img2->update();
17386         }
17387 
17388         EXITFUNC;
17389         return img2;
17390 }
17391 
17392 EMData* Util::divn_img(EMData* img, EMData* img1)
17393 {
17394         ENTERFUNC;
17395         /* Exception Handle */
17396         if (!img) {
17397                 throw NullPointerException("NULL input image");
17398         }
17399         /* ==============   output = img / img1   ================ */
17400 
17401         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17402         size_t size = (size_t)nx*ny*nz;
17403         EMData * img2 = img->copy_head();
17404         float *img_ptr  =img->get_data();
17405         float *img2_ptr = img2->get_data();
17406         float *img1_ptr = img1->get_data();
17407         if(img->is_complex()) {
17408                 float  sq2;
17409                 for (size_t i=0; i<size; i+=2) {
17410                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17411                         img2_ptr[i]   = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17412                         img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17413                 }
17414                 img2->set_complex(true);
17415                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17416         } else {
17417                 for (size_t i=0; i<size; ++i) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17418                 img2->update();
17419         }
17420 
17421         EXITFUNC;
17422         return img2;
17423 }
17424 
17425 EMData* Util::divn_filter(EMData* img, EMData* img1)
17426 {
17427         ENTERFUNC;
17428         /* Exception Handle */
17429         if (!img) {
17430                 throw NullPointerException("NULL input image");
17431         }
17432         /* ========= img /= img1 ===================== */
17433 
17434         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17435         size_t size = (size_t)nx*ny*nz;
17436         EMData * img2 = img->copy_head();
17437         float *img_ptr  =img->get_data();
17438         float *img1_ptr = img1->get_data();
17439         float *img2_ptr = img2->get_data();
17440         if(img->is_complex()) {
17441                 for (size_t i=0; i<size; i+=2) {
17442                         if(img1_ptr[i] > 1.e-10f) {
17443                         img2_ptr[i]   = img_ptr[i]  /img1_ptr[i];
17444                         img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17445                         } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17446                 }
17447         } else  throw ImageFormatException("Only Fourier image allowed");
17448 
17449         img->update();
17450 
17451         EXITFUNC;
17452         return img2;
17453 }
17454 
17455 void Util::mul_scalar(EMData* img, float scalar)
17456 {
17457         ENTERFUNC;
17458         /* Exception Handle */
17459         if (!img) {
17460                 throw NullPointerException("NULL input image");
17461         }
17462         /* ============  output = scalar*input  ================== */
17463 
17464         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17465         size_t size = (size_t)nx*ny*nz;
17466         float *img_ptr  =img->get_data();
17467         for (size_t i=0;i<size;++i) img_ptr[i] *= scalar;
17468         img->update();
17469 
17470         EXITFUNC;
17471 }
17472 
17473 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17474 {
17475         ENTERFUNC;
17476         /* Exception Handle */
17477         if (!img) {
17478                 throw NullPointerException("NULL input image");
17479         }
17480         /* ==============   img += scalar*img1   ================ */
17481 
17482         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17483         size_t size = (size_t)nx*ny*nz;
17484         float *img_ptr  =img->get_data();
17485         float *img1_ptr = img1->get_data();
17486         for (size_t i=0;i<size;++i)img_ptr[i] += img1_ptr[i]*scalar;
17487         img1->update();
17488 
17489         EXITFUNC;
17490 }
17491 
17492 void Util::add_img(EMData* img, EMData* img1)
17493 {
17494         ENTERFUNC;
17495         /* Exception Handle */
17496         if (!img) {
17497                 throw NullPointerException("NULL input image");
17498         }
17499         /* ========= img += img1 ===================== */
17500 
17501         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17502         size_t size = (size_t)nx*ny*nz;
17503         float *img_ptr  = img->get_data();
17504         float *img1_ptr = img1->get_data();
17505         for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i];
17506         img->update();
17507 
17508         EXITFUNC;
17509 }
17510 
17511 void Util::add_img_abs(EMData* img, EMData* img1)
17512 {
17513         ENTERFUNC;
17514         /* Exception Handle */
17515         if (!img) {
17516                 throw NullPointerException("NULL input image");
17517         }
17518         /* ========= img += img1 ===================== */
17519 
17520         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17521         size_t size = (size_t)nx*ny*nz;
17522         float *img_ptr  = img->get_data();
17523         float *img1_ptr = img1->get_data();
17524         for (size_t i=0;i<size;++i) img_ptr[i] += abs(img1_ptr[i]);
17525         img->update();
17526 
17527         EXITFUNC;
17528 }
17529 
17530 void Util::add_img2(EMData* img, EMData* img1)
17531 {
17532         ENTERFUNC;
17533         /* Exception Handle */
17534         if (!img) {
17535                 throw NullPointerException("NULL input image");
17536         }
17537         /* ========= img += img1**2 ===================== */
17538 
17539         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17540         size_t size = (size_t)nx*ny*nz;
17541         float *img_ptr  = img->get_data();
17542         float *img1_ptr = img1->get_data();
17543         if(img->is_complex()) {
17544                 for (size_t i=0; i<size; i+=2) img_ptr[i] += img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1] ;
17545         } else {
17546                 for (size_t i=0;i<size;++i) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17547         }
17548         img->update();
17549 
17550         EXITFUNC;
17551 }
17552 
17553 void Util::sub_img(EMData* img, EMData* img1)
17554 {
17555         ENTERFUNC;
17556         /* Exception Handle */
17557         if (!img) {
17558                 throw NullPointerException("NULL input image");
17559         }
17560         /* ========= img -= img1 ===================== */
17561 
17562         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17563         size_t size = (size_t)nx*ny*nz;
17564         float *img_ptr  = img->get_data();
17565         float *img1_ptr = img1->get_data();
17566         for (size_t i=0;i<size;++i) img_ptr[i] -= img1_ptr[i];
17567         img->update();
17568 
17569         EXITFUNC;
17570 }
17571 
17572 void Util::mul_img(EMData* img, EMData* img1)
17573 {
17574         ENTERFUNC;
17575         /* Exception Handle */
17576         if (!img) {
17577                 throw NullPointerException("NULL input image");
17578         }
17579         /* ========= img *= img1 ===================== */
17580 
17581         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17582         size_t size = (size_t)nx*ny*nz;
17583         float *img_ptr  = img->get_data();
17584         float *img1_ptr = img1->get_data();
17585         if(img->is_complex()) {
17586                 for (size_t i=0; i<size; i+=2) {
17587                         float tmp     = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17588                         img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17589                         img_ptr[i]   = tmp;
17590 
17591                 }
17592         } else {
17593                 for (size_t i=0;i<size;++i) img_ptr[i] *= img1_ptr[i];
17594         }
17595         img->update();
17596 
17597         EXITFUNC;
17598 }
17599 
17600 void Util::div_img(EMData* img, EMData* img1)
17601 {
17602         ENTERFUNC;
17603         /* Exception Handle */
17604         if (!img) {
17605                 throw NullPointerException("NULL input image");
17606         }
17607         /* ========= img /= img1 ===================== */
17608 
17609         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17610         size_t size = (size_t)nx*ny*nz;
17611         float *img_ptr  = img->get_data();
17612         float *img1_ptr = img1->get_data();
17613         if(img->is_complex()) {
17614                 float  sq2;
17615                 for (size_t i=0; i<size; i+=2) {
17616                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17617                         float tmp    = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17618                         img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17619                         img_ptr[i]   = tmp;
17620                 }
17621         } else {
17622                 for (size_t i=0; i<size; ++i) img_ptr[i] /= img1_ptr[i];
17623         }
17624         img->update();
17625 
17626         EXITFUNC;
17627 }
17628 
17629 void Util::div_filter(EMData* img, EMData* img1)
17630 {
17631         ENTERFUNC;
17632         /* Exception Handle */
17633         if (!img) {
17634                 throw NullPointerException("NULL input image");
17635         }
17636         /* ========= img /= img1 ===================== */
17637 
17638         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17639         size_t size = (size_t)nx*ny*nz;
17640         float *img_ptr  = img->get_data();
17641         float *img1_ptr = img1->get_data();
17642         if(img->is_complex()) {
17643                 for (size_t i=0; i<size; i+=2) {
17644                         if(img1_ptr[i] > 1.e-10f) {
17645                         img_ptr[i]   /= img1_ptr[i];
17646                         img_ptr[i+1] /= img1_ptr[i];
17647                         } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17648                 }
17649         } else throw ImageFormatException("Only Fourier image allowed");
17650 
17651         img->update();
17652 
17653         EXITFUNC;
17654 }
17655 
17656 #define img_ptr(i,j,k)  img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*(size_t)nxo]
17657 
17658 EMData* Util::pack_complex_to_real(EMData* img)
17659 {
17660         ENTERFUNC;
17661         /* Exception Handle */
17662         if (!img) {
17663                 throw NullPointerException("NULL input image");
17664         }
17665         /* ==============   img is modulus of a complex image in FFT format (so its imaginary parts are zero),
17666                               output is img packed into real image with Friedel part added,   ================ */
17667 
17668         int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17669         int nx = nxo - 2 + img->is_fftodd();
17670         int lsd2 = (nx + 2 - nx%2) / 2; // Extended x-dimension of the complex image
17671         int nyt, nzt;
17672         int nx2 = nx/2;
17673         int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17674         int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17675         int nx2p = nx2+nx%2;
17676         int ny2p = ny2+ny%2;
17677         int nz2p = nz2+nz%2;
17678         EMData& power = *(new EMData()); // output image
17679         power.set_size(nx, ny, nz);
17680         power.set_array_offsets(-nx2,-ny2,-nz2);
17681         //img->set_array_offsets(1,1,1);
17682         float *img_ptr  = img->get_data();
17683         for (int iz = 1; iz <= nz; iz++) {
17684                 int jz=iz-1;
17685                 if(jz>=nz2p) jz=jz-nzt;
17686                 for (int iy = 1; iy <= ny; iy++) {
17687                         int jy=iy-1;
17688                         if(jy>=ny2p) jy=jy-nyt;
17689                         for (int ix = 1; ix <= lsd2; ix++) {
17690                                 int jx=ix-1;
17691                                 if(jx>=nx2p) jx=jx-nx;
17692                                 power(jx,jy,jz) = img_ptr(ix,iy,iz); //real(img->cmplx(ix,iy,iz));
17693                         }
17694                 }
17695         }
17696 //  Create the Friedel related half
17697         int  nzb, nze, nyb, nye, nxb, nxe;
17698         nxb =-nx2+(nx+1)%2;
17699         nxe = nx2-(nx+1)%2;
17700         if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17701         if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17702         for (int iz = nzb; iz <= nze; iz++) {
17703                 for (int iy = nyb; iy <= nye; iy++) {
17704                         for (int ix = 1; ix <= nxe; ix++) { // Note this loop begins with 1 - FFT should create correct Friedel related 0 plane
17705                                 power(-ix,-iy,-iz) = power(ix,iy,iz);
17706                         }
17707                 }
17708         }
17709         if(ny2 != 0)  {
17710                 if(nz2 != 0)  {
17711                         if(nz%2 == 0) {  //if nz even, fix the first slice
17712                                 for (int iy = nyb; iy <= nye; iy++) {
17713                                         for (int ix = nxb; ix <= -1; ix++) {
17714                                                 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17715                                         }
17716                                 }
17717                                 if(ny%2 == 0) {  //if ny even, fix the first line
17718                                         for (int ix = nxb; ix <= -1; ix++) {
17719                                                 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17720                                         }
17721                                 }
17722                         }
17723                 }
17724                 if(ny%2 == 0) {  //if ny even, fix the first column
17725                         for (int iz = nzb; iz <= nze; iz++) {
17726                                 for (int ix = nxb; ix <= -1; ix++) {
17727                                         power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17728                                 }
17729                         }
17730                 }
17731 
17732         }
17733         power.update();
17734         power.set_array_offsets(0,0,0);
17735         return &power;
17736 }
17737 #undef  img_ptr
17738 
17739 float Util::ang_n(float peakp, string mode, int maxrin)
17740 {
17741     if (mode == "f" || mode == "F")
17742         return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17743     else
17744         return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17745 }
17746 
17747 
17748 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17749 {
17750     float* data = ring->get_data();
17751     float av=0.0;
17752     float sq=0.0;
17753     float nn=0.0;
17754     int nring = numr.size()/3;
17755     for( int i=0; i < nring; ++i )
17756     {
17757         int numr3i = numr[3*i+2];
17758         int numr2i = numr[3*i+1]-1;
17759         float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17760         for( int j=0; j < numr3i; ++j )
17761         {
17762             int jc = numr2i+j;
17763             av += data[jc] * w;
17764             sq += data[jc] * data[jc] * w;
17765             nn += w;
17766         }
17767     }
17768 
17769     float avg = av/nn;
17770     float sgm = sqrt( (sq-av*av/nn)/nn );
17771     size_t n = (size_t)ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17772     for( size_t i=0; i < n; ++i )
17773     {
17774         data[i] -= avg;
17775         data[i] /= sgm;
17776     }
17777 
17778     ring->update();
17779 }
17780 
17781 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17782                 float xrng, float yrng, float step, string mode,
17783                 vector<int>numr, float cnx, float cny) {
17784 
17785     // Manually extract.
17786 /*    vector< EMAN::EMData* > crefim;
17787     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17788     crefim.reserve(crefim_len);
17789 
17790     for(std::size_t i=0;i<crefim_len;i++) {
17791         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17792         crefim.push_back(proxy());
17793     }
17794 */
17795 
17796         size_t crefim_len = crefim.size();
17797 
17798         int   ky = int(2*yrng/step+0.5)/2;
17799         int   kx = int(2*xrng/step+0.5)/2;
17800         int   iref, nref=0, mirror=0;
17801         float iy, ix, sx=0, sy=0;
17802         float peak = -1.0E23f;
17803         float ang=0.0f;
17804         for (int i = -ky; i <= ky; i++) {
17805                 iy = i * step ;
17806                 for (int j = -kx; j <= kx; j++) {
17807                         ix = j*step ;
17808                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17809 
17810                         Normalize_ring( cimage, numr );
17811 
17812                         Frngs(cimage, numr);
17813                         //  compare with all reference images
17814                         // for iref in xrange(len(crefim)):
17815                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17816                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17817                                 double qn = retvals["qn"];
17818                                 double qm = retvals["qm"];
17819                                 if(qn >= peak || qm >= peak) {
17820                                         sx = -ix;
17821                                         sy = -iy;
17822                                         nref = iref;
17823                                         if (qn >= qm) {
17824                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17825                                                 peak = static_cast<float>(qn);
17826                                                 mirror = 0;
17827                                         } else {
17828                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17829                                                 peak = static_cast<float>(qm);
17830                                                 mirror = 1;
17831                                         }
17832                                 }
17833                         }  delete cimage; cimage = 0;
17834                 }
17835         }
17836         float co, so, sxs, sys;
17837         co = static_cast<float>( cos(ang*pi/180.0) );
17838         so = static_cast<float>( -sin(ang*pi/180.0) );
17839         sxs = sx*co - sy*so;
17840         sys = sx*so + sy*co;
17841         vector<float> res;
17842         res.push_back(ang);
17843         res.push_back(sxs);
17844         res.push_back(sys);
17845         res.push_back(static_cast<float>(mirror));
17846         res.push_back(static_cast<float>(nref));
17847         res.push_back(peak);
17848         return res;
17849 }
17850 
17851 vector<float> Util::multiref_polar_ali_2d_peaklist(EMData* image, const vector< EMData* >& crefim,
17852                 float xrng, float yrng, float step, string mode,
17853                 vector<int>numr, float cnx, float cny) {
17854 
17855         size_t crefim_len = crefim.size();
17856 
17857         int   ky = int(2*yrng/step+0.5)/2;
17858         int   kx = int(2*xrng/step+0.5)/2;
17859         float iy, ix;
17860         vector<float> peak(crefim_len*5, -1.0e23f);
17861         for (int i = -ky; i <= ky; i++) {
17862                 iy = i * step ;
17863                 for (int j = -kx; j <= kx; j++) {
17864                         ix = j*step ;
17865                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17866                         Normalize_ring( cimage, numr );
17867                         Frngs(cimage, numr);
17868                         for (int iref = 0; iref < (int)crefim_len; iref++) {
17869                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17870                                 double qn = retvals["qn"];
17871                                 double qm = retvals["qm"];
17872                                 if(qn >= peak[iref*5] || qm >= peak[iref*5]) {
17873                                         if (qn >= qm) {
17874                                                 peak[iref*5] = static_cast<float>(qn);
17875                                                 peak[iref*5+1] = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17876                                                 peak[iref*5+2] = -ix;
17877                                                 peak[iref*5+3] = -iy;
17878                                                 peak[iref*5+4] = 0;
17879                                         } else {
17880                                                 peak[iref*5] = static_cast<float>(qm);
17881                                                 peak[iref*5+1] = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17882                                                 peak[iref*5+2] = -ix;
17883                                                 peak[iref*5+3] = -iy;
17884                                                 peak[iref*5+4] = 1;
17885                                         }
17886                                 }
17887                         }  delete cimage; cimage = 0;
17888                 }
17889         }
17890         for (int iref = 0; iref < (int)crefim_len; iref++) {
17891                 float ang = peak[iref*5+1];
17892                 float sx = peak[iref*5+2];
17893                 float sy = peak[iref*5+3];
17894                 float co =  cos(ang*pi/180.0);
17895                 float so = -sin(ang*pi/180.0);
17896                 float sxs = sx*co - sy*so;
17897                 float sys = sx*so + sy*co;
17898                 peak[iref*5+2] = sxs;
17899                 peak[iref*5+3] = sys;
17900         }
17901         return peak;
17902 }
17903 
17904 vector<int> Util::assign_groups(vector<float> d, int nref, int nima) {
17905 
17906         int kt = nref;
17907         unsigned int maxasi = nima/nref;
17908 //      vector<int> id_list[nref];
17909         vector< vector<int> > id_list;
17910         id_list.resize(nref);
17911         int group;
17912         while (kt > 0) {
17913                 int l = -1;
17914                 float dmax = -1.e22;
17915                 for (int i=0; i<nref*nima; i++)
17916                         if (d[i] > dmax) {
17917                                 dmax = d[i];
17918                                 l = i;
17919                         }
17920                 group = l/nima;
17921                 int ima  = l%nima;
17922                 id_list[group].push_back(ima);
17923                 if (kt > 1) {
17924                         if (id_list[group].size() < maxasi) group = -1;
17925                         else kt -= 1;
17926                 } else {
17927                         if (id_list[group].size() < maxasi+nima%nref) group = -1;
17928                         else kt -= 1;
17929                 }
17930                 for (int iref=0; iref<nref; iref++) d[iref*nima+ima] = -1.e20;
17931                 if (group != -1) {
17932                         for (int im=0; im<nima; im++) d[group*nima+im] = -1.e20;
17933                 }
17934         }
17935 
17936         vector<int> id_list_1; 
17937         for (int iref=0; iref<nref; iref++)
17938                 for (int im=0; im<maxasi; im++)
17939                         id_list_1.push_back(id_list[iref][im]);
17940         for (int im=maxasi; im<maxasi+nima%nref; im++)
17941                         id_list_1.push_back(id_list[group][im]);
17942         id_list_1.push_back(group);
17943         return id_list_1;
17944 }
17945 
17946 
17947 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
17948                 float xrng, float yrng, float step, string mode,
17949                 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
17950 
17951     // Manually extract.
17952 /*    vector< EMAN::EMData* > crefim;
17953     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17954     crefim.reserve(crefim_len);
17955 
17956     for(std::size_t i=0;i<crefim_len;i++) {
17957         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17958         crefim.push_back(proxy());
17959     }
17960 */
17961 
17962         size_t crefim_len = crefim.size();
17963 
17964         int   ky = int(2*yrng/step+0.5)/2;
17965         int   kx = int(2*xrng/step+0.5)/2;
17966         int   iref, nref=0, mirror=0;
17967         float iy, ix, sx=0, sy=0;
17968         float peak = -1.0E23f;
17969         float ang=0.0f;
17970         for (int i = -ky; i <= ky; i++) {
17971                 iy = i * step ;
17972                 for (int j = -kx; j <= kx; j++) {
17973                         ix = j*step ;
17974                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17975 
17976                         Normalize_ring( cimage, numr );
17977 
17978                         Frngs(cimage, numr);
17979                         //  compare with all reference images
17980                         // for iref in xrange(len(crefim)):
17981                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17982                                 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
17983                                 double qn = retvals["qn"];
17984                                 double qm = retvals["qm"];
17985                                 if(qn >= peak || qm >= peak) {
17986                                         sx = -ix;
17987                                         sy = -iy;
17988                                         nref = iref;
17989                                         if (qn >= qm) {
17990                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17991                                                 peak = static_cast<float>(qn);
17992                                                 mirror = 0;
17993                                         } else {
17994                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17995                                                 peak = static_cast<float>(qm);
17996                                                 mirror = 1;
17997                                         }
17998                                 }
17999                         }  delete cimage; cimage = 0;
18000                 }
18001         }
18002         float co, so, sxs, sys;
18003         co = static_cast<float>( cos(ang*pi/180.0) );
18004         so = static_cast<float>( -sin(ang*pi/180.0) );
18005         sxs = sx*co - sy*so;
18006         sys = sx*so + sy*co;
18007         vector<float> res;
18008         res.push_back(ang);
18009         res.push_back(sxs);
18010         res.push_back(sys);
18011         res.push_back(static_cast<float>(mirror));
18012         res.push_back(static_cast<float>(nref));
18013         res.push_back(peak);
18014         return res;
18015 }
18016 
18017 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
18018                 float xrng, float yrng, float step, string mode,
18019                 vector< int >numr, float cnx, float cny) {
18020 
18021     // Manually extract.
18022 /*    vector< EMAN::EMData* > crefim;
18023     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18024     crefim.reserve(crefim_len);
18025 
18026     for(std::size_t i=0;i<crefim_len;i++) {
18027         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18028         crefim.push_back(proxy());
18029     }
18030 */
18031         size_t crefim_len = crefim.size();
18032 
18033         int   ky = int(2*yrng/step+0.5)/2;
18034         int   kx = int(2*xrng/step+0.5)/2;
18035         int   iref, nref=0;
18036         float iy, ix, sx=0, sy=0;
18037         float peak = -1.0E23f;
18038         float ang=0.0f;
18039         for (int i = -ky; i <= ky; i++) {
18040                 iy = i * step ;
18041                 for (int j = -kx; j <= kx; j++) {
18042                         ix = j*step ;
18043                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18044                         Frngs(cimage, numr);
18045                         //  compare with all reference images
18046                         // for iref in xrange(len(crefim)):
18047                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18048                                 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
18049                                 double qn = retvals["qn"];
18050                                 if(qn >= peak) {
18051                                         sx = -ix;
18052                                         sy = -iy;
18053                                         nref = iref;
18054                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18055                                         peak = static_cast<float>(qn);
18056                                 }
18057                         }  delete cimage; cimage = 0;
18058                 }
18059         }
18060         float co, so, sxs, sys;
18061         co = static_cast<float>( cos(ang*pi/180.0) );
18062         so = static_cast<float>( -sin(ang*pi/180.0) );
18063         sxs = sx*co - sy*so;
18064         sys = sx*so + sy*co;
18065         vector<float> res;
18066         res.push_back(ang);
18067         res.push_back(sxs);
18068         res.push_back(sys);
18069         res.push_back(static_cast<float>(nref));
18070         res.push_back(peak);
18071         return res;
18072 }
18073 
18074 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
18075                 float xrng, float yrng, float step, float ant, string mode,
18076                 vector<int>numr, float cnx, float cny) {
18077 
18078     // Manually extract.
18079 /*    vector< EMAN::EMData* > crefim;
18080     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18081     crefim.reserve(crefim_len);
18082 
18083     for(std::size_t i=0;i<crefim_len;i++) {
18084         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18085         crefim.push_back(proxy());
18086     }
18087 */
18088         size_t crefim_len = crefim.size();
18089         const float qv = static_cast<float>( pi/180.0 );
18090 
18091         Transform * t = image->get_attr("xform.projection");
18092         Dict d = t->get_params("spider");
18093         if(t) {delete t; t=0;}
18094         float phi = d["phi"];
18095         float theta = d["theta"];
18096         int   ky = int(2*yrng/step+0.5)/2;
18097         int   kx = int(2*xrng/step+0.5)/2;
18098         int   iref, nref=0, mirror=0;
18099         float iy, ix, sx=0, sy=0;
18100         float peak = -1.0E23f;
18101         float ang=0.0f;
18102         float imn1 = sin(theta*qv)*cos(phi*qv);
18103         float imn2 = sin(theta*qv)*sin(phi*qv);
18104         float imn3 = cos(theta*qv);
18105         vector<float> n1(crefim_len);
18106         vector<float> n2(crefim_len);
18107         vector<float> n3(crefim_len);
18108         for ( iref = 0; iref < (int)crefim_len; iref++) {
18109                         n1[iref] = crefim[iref]->get_attr("n1");
18110                         n2[iref] = crefim[iref]->get_attr("n2");
18111                         n3[iref] = crefim[iref]->get_attr("n3");
18112         }
18113         for (int i = -ky; i <= ky; i++) {
18114             iy = i * step ;
18115             for (int j = -kx; j <= kx; j++) {
18116                 ix = j*step;
18117                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18118 
18119                 Normalize_ring( cimage, numr );
18120 
18121                 Frngs(cimage, numr);
18122                 //  compare with all reference images
18123                 // for iref in xrange(len(crefim)):
18124                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18125                         if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18126                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18127                                 double qn = retvals["qn"];
18128                                 double qm = retvals["qm"];
18129                                 if(qn >= peak || qm >= peak) {
18130                                         sx = -ix;
18131                                         sy = -iy;
18132                                         nref = iref;
18133                                         if (qn >= qm) {
18134                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18135                                                 peak = static_cast<float>( qn );
18136                                                 mirror = 0;
18137                                         } else {
18138                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18139                                                 peak = static_cast<float>( qm );
18140                                                 mirror = 1;
18141                                         }
18142                                 }
18143                         }
18144                 }  delete cimage; cimage = 0;
18145             }
18146         }
18147         float co, so, sxs, sys;
18148         if(peak == -1.0E23) {
18149                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18150                 nref = -1;
18151         } else {
18152                 co =  cos(ang*qv);
18153                 so = -sin(ang*qv);
18154                 sxs = sx*co - sy*so;
18155                 sys = sx*so + sy*co;
18156         }
18157         vector<float> res;
18158         res.push_back(ang);
18159         res.push_back(sxs);
18160         res.push_back(sys);
18161         res.push_back(static_cast<float>(mirror));
18162         res.push_back(static_cast<float>(nref));
18163         res.push_back(peak);
18164         return res;
18165 }
18166 
18167 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18168                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18169                 vector<int>numr, float cnx, float cny) {
18170 
18171     // Manually extract.
18172 /*    vector< EMAN::EMData* > crefim;
18173     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18174     crefim.reserve(crefim_len);
18175 
18176     for(std::size_t i=0;i<crefim_len;i++) {
18177         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18178         crefim.push_back(proxy());
18179     }
18180 */
18181         size_t crefim_len = crefim.size();
18182         const float qv = static_cast<float>(pi/180.0);
18183 
18184         Transform* t = image->get_attr("xform.projection");
18185         Dict d = t->get_params("spider");
18186         if(t) {delete t; t=0;}
18187         float phi = d["phi"];
18188         float theta = d["theta"];
18189         float psi = d["psi"];
18190         int ky = int(2*yrng/step+0.5)/2;
18191         int kx = int(2*xrng/step+0.5)/2;
18192         int iref, nref = 0, mirror = 0;
18193         float iy, ix, sx = 0, sy = 0;
18194         float peak = -1.0E23f;
18195         float ang = 0.0f;
18196         float imn1 = sin(theta*qv)*cos(phi*qv);
18197         float imn2 = sin(theta*qv)*sin(phi*qv);
18198         float imn3 = cos(theta*qv);
18199         vector<float> n1(crefim_len);
18200         vector<float> n2(crefim_len);
18201         vector<float> n3(crefim_len);
18202         for (iref = 0; iref < (int)crefim_len; iref++) {
18203                         n1[iref] = crefim[iref]->get_attr("n1");
18204                         n2[iref] = crefim[iref]->get_attr("n2");
18205                         n3[iref] = crefim[iref]->get_attr("n3");
18206         }
18207         bool nomirror = (theta<90.0) || ((theta==90.0) && (psi<psi_max));
18208         if (!nomirror) {
18209                 phi = fmod(phi+540.0f, 360.0f);
18210                 theta = 180-theta;
18211                 psi = fmod(540.0f-psi, 360.0f);
18212         }
18213         for (int i = -ky; i <= ky; i++) {
18214             iy = i * step ;
18215             for (int j = -kx; j <= kx; j++) {
18216                 ix = j*step;
18217                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18218 
18219                 Normalize_ring(cimage, numr);
18220 
18221                 Frngs(cimage, numr);
18222                 //  compare with all reference images
18223                 // for iref in xrange(len(crefim)):
18224                 for (iref = 0; iref < (int)crefim_len; iref++) {
18225                         if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18226                                 if (nomirror) {
18227                                         Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 0);
18228                                         double qn = retvals["qn"];
18229                                         if (qn >= peak) {
18230                                                 sx = -ix;
18231                                                 sy = -iy;
18232                                                 nref = iref;
18233                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18234                                                 peak = static_cast<float>(qn);
18235                                                 mirror = 0;
18236                                         }
18237                                 } else {
18238                                         Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 1);
18239                                         double qn = retvals["qn"];
18240                                         if (qn >= peak) {
18241                                                 sx = -ix;
18242                                                 sy = -iy;
18243                                                 nref = iref;
18244                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18245                                                 peak = static_cast<float>(qn);
18246                                                 mirror = 1;
18247                                         }
18248                                 }
18249                         }
18250                 }  delete cimage; cimage = 0;
18251             }
18252         }
18253         float co, so, sxs, sys;
18254         if(peak == -1.0E23) {
18255                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18256                 nref = -1;
18257         } else {
18258                 co =  cos(ang*qv);
18259                 so = -sin(ang*qv);
18260                 sxs = sx*co - sy*so;
18261                 sys = sx*so + sy*co;
18262         }
18263         vector<float> res;
18264         res.push_back(ang);
18265         res.push_back(sxs);
18266         res.push_back(sys);
18267         res.push_back(static_cast<float>(mirror));
18268         res.push_back(static_cast<float>(nref));
18269         res.push_back(peak);
18270         return res;
18271 }
18272 
18273 
18274 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18275                 float xrng, float yrng, float step, float psi_max, string mode,
18276                 vector<int>numr, float cnx, float cny, int ynumber) {
18277 
18278         size_t crefim_len = crefim.size();
18279 
18280         int   iref, nref=0, mirror=0;
18281         float iy, ix, sx=0, sy=0;
18282         float peak = -1.0E23f;
18283         float ang=0.0f;
18284         int   kx = int(2*xrng/step+0.5)/2;
18285         //if ynumber==-1, use the old code which process x and y direction equally.
18286         if(ynumber==-1) {
18287                 int   ky = int(2*yrng/step+0.5)/2;
18288                 for (int i = -ky; i <= ky; i++) {
18289                         iy = i * step ;
18290                         for (int j = -kx; j <= kx; j++)  {
18291                                 ix = j*step ;
18292                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18293 
18294                                 Normalize_ring( cimage, numr );
18295 
18296                                 Frngs(cimage, numr);
18297                                 //  compare with all reference images
18298                                 // for iref in xrange(len(crefim)):
18299                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18300                                         Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18301                                         double qn = retvals["qn"];
18302                                         double qm = retvals["qm"];
18303                                         if(qn >= peak || qm >= peak) {
18304                                                 sx = -ix;
18305                                                 sy = -iy;
18306                                                 nref = iref;
18307                                                 if (qn >= qm) {
18308                                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18309                                                         peak = static_cast<float>(qn);
18310                                                         mirror = 0;
18311                                                 } else {
18312                                                         ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18313                                                         peak = static_cast<float>(qm);
18314                                                         mirror = 1;
18315                                                 }
18316                                         }
18317                                 }  
18318                                 delete cimage; cimage = 0;
18319                         }
18320                    }
18321         }
18322         //if ynumber is given, it should be even. We need to check whether it is zero
18323         else if(ynumber==0) {
18324                 sy = 0.0f;
18325                 for (int j = -kx; j <= kx; j++) {
18326                         ix = j*step ;
18327                         EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18328 
18329                         Normalize_ring( cimage, numr );
18330 
18331                         Frngs(cimage, numr);
18332                         //  compare with all reference images
18333                         // for iref in xrange(len(crefim)):
18334                         for ( iref = 0; iref < (int)crefim_len; iref++)  {
18335                                 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18336                                 double qn = retvals["qn"];
18337                                 double qm = retvals["qm"];
18338                                 if(qn >= peak || qm >= peak) {
18339                                         sx = -ix;
18340                                         nref = iref;
18341                                         if (qn >= qm) {
18342                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18343                                                 peak = static_cast<float>(qn);
18344                                                 mirror = 0;
18345                                         } else {
18346                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18347                                                 peak = static_cast<float>(qm);
18348                                                 mirror = 1;
18349                                         }
18350                                 }
18351                         } 
18352                         delete cimage; cimage = 0;
18353                 }                       
18354         } else {
18355                 int   ky = int(ynumber/2);              
18356                 float stepy=2*yrng/ynumber;
18357                 //std::cout<<"yrng="<<yrng<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
18358                 for (int i = -ky+1; i <= ky; i++) {
18359                         iy = i * stepy ;
18360                         for (int j = -kx; j <= kx; j++) {
18361                                 ix = j*step ;
18362                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18363 
18364                                 Normalize_ring( cimage, numr );
18365 
18366                                 Frngs(cimage, numr);
18367                                 //  compare with all reference images
18368                                 // for iref in xrange(len(crefim)):
18369                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18370                                         Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18371                                         double qn = retvals["qn"];
18372                                         double qm = retvals["qm"];
18373                                         if(qn >= peak || qm >= peak) {
18374                                                 sx = -ix;
18375                                                 sy = -iy;
18376                                                 nref = iref;
18377                                                 if (qn >= qm) {
18378                                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18379                                                         peak = static_cast<float>(qn);
18380                                                         mirror = 0;
18381                                                 } else {
18382                                                         ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18383                                                         peak = static_cast<float>(qm);
18384                                                         mirror = 1;
18385                                                 }
18386                                         }
18387                                 }
18388                                 delete cimage; cimage = 0;
18389                         }
18390                 }
18391         }
18392         float co, so, sxs, sys;
18393         co = static_cast<float>( cos(ang*pi/180.0) );
18394         so = static_cast<float>( -sin(ang*pi/180.0) );
18395         sxs = sx*co - sy*so;
18396         sys = sx*so + sy*co;
18397         vector<float> res;
18398         res.push_back(ang);
18399         res.push_back(sxs);
18400         res.push_back(sys);
18401         res.push_back(static_cast<float>(mirror));
18402         res.push_back(static_cast<float>(nref));
18403         res.push_back(peak);
18404         return res;
18405 }
18406 
18407 void  Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
18408                         float xrng, float yrng, float step, string mode,
18409                         vector< int >numr, float cnx, float cny,
18410                         EMData *peaks, EMData *peakm) {
18411 
18412         int   maxrin = numr[numr.size()-1];
18413 
18414         int   ky = int(2*yrng/step+0.5)/2;
18415         int   kx = int(2*xrng/step+0.5)/2;
18416 
18417         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18418         float *p_ccf1ds = peaks->get_data();
18419 
18420         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18421         float *p_ccf1dm = peakm->get_data();
18422 
18423         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18424                 p_ccf1ds[i] = -1.e20f;
18425                 p_ccf1dm[i] = -1.e20f;
18426         }
18427 
18428         for (int i = -ky; i <= ky; i++) {
18429                 float iy = i * step;
18430                 for (int j = -kx; j <= kx; j++) {
18431                         float ix = j*step;
18432                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18433                         Frngs(cimage, numr);
18434                         Crosrng_msg_vec(crefim, cimage, numr,
18435                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18436                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18437                         delete cimage; cimage = 0;
18438                 }
18439         }
18440         return;
18441 }
18442 
18443 void  Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
18444      float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
18445      EMData *peaks_compress, EMData *peakm_compress) {
18446 
18447         int   maxrin = numr[numr.size()-1];
18448 
18449         int   ky = int(2*yrng/step+0.5)/2;
18450         int   kx = int(2*xrng/step+0.5)/2;
18451 
18452         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18453         float *p_ccf1ds = peaks->get_data();
18454 
18455         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18456         float *p_ccf1dm = peakm->get_data();
18457 
18458         peaks_compress->set_size(maxrin, 1, 1);
18459         float *p_ccf1ds_compress = peaks_compress->get_data();
18460 
18461         peakm_compress->set_size(maxrin, 1, 1);
18462         float *p_ccf1dm_compress = peakm_compress->get_data();
18463 
18464         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18465                 p_ccf1ds[i] = -1.e20f;
18466                 p_ccf1dm[i] = -1.e20f;
18467         }
18468 
18469         for (int i = -ky; i <= ky; i++) {
18470                 float iy = i * step;
18471                 for (int j = -kx; j <= kx; j++) {
18472                         float ix = j*step;
18473                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18474                         Frngs(cimage, numr);
18475                         Crosrng_msg_vec(crefim, cimage, numr,
18476                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18477                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18478                         delete cimage; cimage = 0;
18479                 }
18480         }
18481         for (int x=0; x<maxrin; x++) {
18482                 float maxs = -1.0e22f;
18483                 float maxm = -1.0e22f;
18484                 for (int i=1; i<=2*ky+1; i++) {
18485                         for (int j=1; j<=2*kx+1; j++) {
18486                                 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
18487                                 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
18488                         }
18489                 }
18490                 p_ccf1ds_compress[x] = maxs;
18491                 p_ccf1dm_compress[x] = maxm;
18492         }
18493         return;
18494 }
18495 
18496 struct ccf_point
18497 {
18498     float value;
18499     int i;
18500     int j;
18501     int k;
18502     int mirror;
18503 };
18504 
18505 
18506 struct ccf_value
18507 {
18508     bool operator()( const ccf_point& a, const ccf_point& b )
18509     {
18510         return a.value > b.value;
18511     }
18512 };
18513 
18514 
18515 vector<float>  Util::ali2d_ccf_list(EMData* image, EMData* crefim,
18516                         float xrng, float yrng, float step, string mode,
18517                         vector< int >numr, float cnx, float cny, double T) {
18518 
18519         int   maxrin = numr[numr.size()-1];
18520 
18521         int   ky = int(2*yrng/step+0.5)/2;
18522         int   kx = int(2*xrng/step+0.5)/2;
18523 
18524         float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
18525         float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
18526         int vol = maxrin*(2*kx+1)*(2*ky+1);
18527         vector<ccf_point> ccf(2*vol);
18528         ccf_point temp;
18529 
18530         int index = 0;
18531         for (int i = -ky; i <= ky; i++) {
18532                 float iy = i * step;
18533                 for (int j = -kx; j <= kx; j++) {
18534                         float ix = j*step;
18535                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18536                         Frngs(cimage, numr);
18537                         Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
18538                         for (int k=0; k<maxrin; k++) {
18539                                 temp.value = p_ccf1ds[k];
18540                                 temp.i = k;
18541                                 temp.j = j;
18542                                 temp.k = i;
18543                                 temp.mirror = 0;
18544                                 ccf[index] = temp;
18545                                 index++;
18546                                 temp.value = p_ccf1dm[k];
18547                                 temp.mirror = 1;
18548                                 ccf[index] = temp;
18549                                 index++;
18550                         }
18551                         delete cimage; cimage = 0;
18552                 }
18553         }
18554 
18555         delete p_ccf1ds;
18556         delete p_ccf1dm;
18557         std::sort(ccf.begin(), ccf.end(), ccf_value());
18558 
18559         double qt = (double)ccf[0].value;
18560         vector <double> p(2*vol), cp(2*vol);
18561 
18562         double sump = 0.0;
18563         for (int i=0; i<2*vol; i++) {
18564                 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
18565                 sump += p[i];
18566         }
18567         for (int i=0; i<2*vol; i++) {
18568                 p[i] /= sump;
18569         }
18570         for (int i=1; i<2*vol; i++) {
18571                 p[i] += p[i-1];
18572         }
18573         p[2*vol-1] = 2.0;
18574 
18575         float t = get_frand(0.0f, 1.0f);
18576         int select = 0;
18577         while (p[select] < t)   select += 1;
18578 
18579         vector<float> a(6);
18580         a[0] = ccf[select].value;
18581         a[1] = (float)ccf[select].i;
18582         a[2] = (float)ccf[select].j;
18583         a[3] = (float)ccf[select].k;
18584         a[4] = (float)ccf[select].mirror;
18585         a[5] = (float)select;
18586         return a;
18587 }
18588 
18589 
18590 /*
18591 void Util::multiref_peaks_ali(EMData* image, const vector< EMData* >& crefim,
18592                         float xrng, float yrng, float step, string mode,
18593                         vector< int >numr, float cnx, float cny,
18594                         EMData *peaks, EMData *peakm, int nphi, int ntheta) {
18595 
18596 // formerly known as apmq
18597     // Determine shift and rotation between image and many reference
18598     // images (crefim, weights have to be applied) quadratic
18599     // interpolation
18600 
18601 
18602     // Manually extract.
18603 *//*    vector< EMAN::EMData* > crefim;
18604     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18605     crefim.reserve(crefim_len);
18606 
18607     for(std::size_t i=0;i<crefim_len;i++) {
18608         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18609         crefim.push_back(proxy());
18610     }
18611 */
18612 /*
18613         int   maxrin = numr[numr.size()-1];
18614 
18615         size_t crefim_len = crefim.size();
18616 
18617         int   iref;
18618         int   ky = int(2*yrng/step+0.5)/2;
18619         int   kx = int(2*xrng/step+0.5)/2;
18620         int   tkx = 2*kx+3;
18621         int   tky = 2*ky+3;
18622 
18623         peaks->set_size(maxrin, nphi, ntheta, tkx, tky);
18624         float *p_ccf1ds = peaks->get_data();
18625 
18626 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k+(l*nz))*ny))*nx]
18627 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k*ny))*nx]
18628         peakm->set_size(maxrin, nphi, ntheta, tkx, tky);
18629         float *p_ccf1dm = peakm->get_data();
18630 
18631         for ( int i = 0; i<maxrin*(int)crefim_len*(2*kx+3)*(2*ky+3); i++) {
18632                 p_ccf1ds[i] = -1.e20f;
18633                 p_ccf1dm[i] = -1.e20f;
18634         }
18635 
18636         float  iy, ix;
18637         for (int i = -ky; i <= ky; i++) {
18638                 iy = i * step ;
18639                 for (int j = -kx; j <= kx; j++) {
18640                         ix = j*step ;
18641                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18642                         Frngs(cimage, numr);
18643                         //  compare with all reference images
18644                         // for iref in xrange(len(crefim)):
18645                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18646                                 Crosrng_msg_vec(crefim[iref], cimage, numr,
18647                                         p_ccf1ds+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin,
18648                                         p_ccf1dm+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin);
18649                         }
18650                         delete cimage; cimage = 0;
18651                 }
18652         }
18653         return;
18654 }
18655 */
18656 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18657 
18658         EMData *rot;
18659 
18660         const int nmax=3, mmax=3;
18661         char task[60], csave[60];
18662         long int lsave[4];
18663         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18664         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];
18665         long int SIXTY=60;
18666 
18667         //     We wish to have no output.
18668         iprint = -1;
18669 
18670         //c     We specify the tolerances in the stopping criteria.
18671         factr=1.0e1;
18672         pgtol=1.0e-5;
18673 
18674         //     We specify the dimension n of the sample problem and the number
18675         //        m of limited memory corrections stored.  (n and m should not
18676         //        exceed the limits nmax and mmax respectively.)
18677         n=3;
18678         m=3;
18679 
18680         //     We now provide nbd which defines the bounds on the variables:
18681         //                    l   specifies the lower bounds,
18682         //                    u   specifies the upper bounds.
18683         //                    x   specifies the initial guess
18684         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18685         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18686         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18687 
18688 
18689         //     We start the iteration by initializing task.
18690         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18691         strcpy(task,"START");
18692         for (int i=5;i<60;i++)  task[i]=' ';
18693 
18694         //     This is the call to the L-BFGS-B code.
18695         // (* call the L-BFGS-B routine with task='START' once before loop *)
18696         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18697         //int step = 1;
18698 
18699         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18700         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18701 
18702                 if (strncmp(task,"FG",2)==0) {
18703                 //   the minimization routine has returned to request the
18704                 //   function f and gradient g values at the current x
18705 
18706                 //        Compute function value f for the sample problem.
18707                 rot = new EMData();
18708                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
18709                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18710                 //f = -f;
18711                 delete rot;
18712 
18713                 //        Compute gradient g for the sample problem.
18714                 float dt = 1.0e-3f;
18715                 rot = new EMData();
18716                 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
18717                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18718                 //f1 = -f1;
18719                 g[0] = (f1-f)/dt;
18720                 delete rot;
18721 
18722                 dt = 1.0e-2f;
18723                 rot = new EMData();
18724                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
18725                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18726                 //f2 = -f2;
18727                 g[1] = (f2-f)/dt;
18728                 delete rot;
18729 
18730                 rot = new EMData();
18731                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
18732                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18733                 //f3 = -f3;
18734                 g[2] = (f3-f)/dt;
18735                 delete rot;
18736                 }
18737 
18738                 //c          go back to the minimization routine.
18739                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18740                 //step++;
18741         }
18742 
18743         //printf("Total step is %d\n", step);
18744         vector<float> res;
18745         res.push_back(static_cast<float>(x[0]));
18746         res.push_back(static_cast<float>(x[1]));
18747         res.push_back(static_cast<float>(x[2]));
18748         //res.push_back(step);
18749         return res;
18750 }
18751 
18752 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
18753 
18754         EMData *rot;
18755 
18756         const int nmax=3, mmax=3;
18757         char task[60], csave[60];
18758         long int lsave[4];
18759         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18760         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];
18761         long int SIXTY=60;
18762 
18763         //     We wish to have no output.
18764         iprint = -1;
18765 
18766         //c     We specify the tolerances in the stopping criteria.
18767         factr=1.0e1;
18768         pgtol=1.0e-5;
18769 
18770         //     We specify the dimension n of the sample problem and the number
18771         //        m of limited memory corrections stored.  (n and m should not
18772         //        exceed the limits nmax and mmax respectively.)
18773         n=3;
18774         m=3;
18775 
18776         //     We now provide nbd which defines the bounds on the variables:
18777         //                    l   specifies the lower bounds,
18778         //                    u   specifies the upper bounds.
18779         //                    x   specifies the initial guess
18780         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18781         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18782         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18783 
18784 
18785         //     We start the iteration by initializing task.
18786         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18787         strcpy(task,"START");
18788         for (int i=5;i<60;i++)  task[i]=' ';
18789 
18790         //     This is the call to the L-BFGS-B code.
18791         // (* call the L-BFGS-B routine with task='START' once before loop *)
18792         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18793         //int step = 1;
18794 
18795         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18796         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18797 
18798                 if (strncmp(task,"FG",2)==0) {
18799                 //   the minimization routine has returned to request the
18800                 //   function f and gradient g values at the current x
18801 
18802                 //        Compute function value f for the sample problem.
18803                 rot = new EMData();
18804                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18805                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18806                 //f = -f;
18807                 delete rot;
18808 
18809                 //        Compute gradient g for the sample problem.
18810                 float dt = 1.0e-3f;
18811                 rot = new EMData();
18812                 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18813                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18814                 //f1 = -f1;
18815                 g[0] = (f1-f)/dt;
18816                 delete rot;
18817 
18818                 rot = new EMData();
18819                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
18820                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18821                 //f2 = -f2;
18822                 g[1] = (f2-f)/dt;
18823                 delete rot;
18824 
18825                 rot = new EMData();
18826                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
18827                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18828                 //f3 = -f3;
18829                 g[2] = (f3-f)/dt;
18830                 delete rot;
18831                 }
18832 
18833                 //c          go back to the minimization routine.
18834                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18835                 //step++;
18836         }
18837 
18838         //printf("Total step is %d\n", step);
18839         vector<float> res;
18840         res.push_back(static_cast<float>(x[0]));
18841         res.push_back(static_cast<float>(x[1]));
18842         res.push_back(static_cast<float>(x[2]));
18843         //res.push_back(step);
18844         return res;
18845 }
18846 
18847 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) {
18848 
18849         EMData *proj, *proj2;
18850 
18851         const int nmax=5, mmax=5;
18852         char task[60], csave[60];
18853         long int lsave[4];
18854         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18855         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];
18856         long int SIXTY=60;
18857 
18858         //     We wish to have no output.
18859         iprint = -1;
18860 
18861         //c     We specify the tolerances in the stopping criteria.
18862         factr=1.0e1;
18863         pgtol=1.0e-5;
18864 
18865         //     We specify the dimension n of the sample problem and the number
18866         //        m of limited memory corrections stored.  (n and m should not
18867         //        exceed the limits nmax and mmax respectively.)
18868         n=5;
18869         m=5;
18870 
18871         //     We now provide nbd which defines the bounds on the variables:
18872         //                    l   specifies the lower bounds,
18873         //                    u   specifies the upper bounds.
18874         //                    x   specifies the initial guess
18875         x[0] = phi;     nbd[0] = 2;     l[0] = phi-2.0;         u[0] = phi+2.0;
18876         x[1] = theta;   nbd[1] = 2;     l[1] = theta-2.0;       u[1] = theta+2.0;
18877         x[2] = psi;     nbd[2] = 2;     l[2] = psi-2.0;         u[2] = psi+2.0;
18878         x[3] = sxs;     nbd[3] = 2;     l[3] = sxs-2.0;         u[3] = sxs+2.0;
18879         x[4] = sys;     nbd[4] = 2;     l[4] = sys-2.0;         u[4] = sys+2.0;
18880 
18881 
18882         //     We start the iteration by initializing task.
18883         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18884         strcpy(task,"START");
18885         for (int i=5;i<60;i++)  task[i]=' ';
18886 
18887         //     This is the call to the L-BFGS-B code.
18888         // (* call the L-BFGS-B routine with task='START' once before loop *)
18889         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18890         int step = 1;
18891 
18892         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18893         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18894 
18895                 if (strncmp(task,"FG",2)==0) {
18896                 //   the minimization routine has returned to request the
18897                 //   function f and gradient g values at the current x
18898 
18899                 //        Compute function value f for the sample problem.
18900                 proj = new EMData();
18901                 proj2 = new EMData();
18902                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18903                 proj->fft_shuffle();
18904                 proj->center_origin_fft();
18905                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18906                 proj->do_ift_inplace();
18907                 int M = proj->get_ysize()/2;
18908                 proj2 = proj->window_center(M);
18909                 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18910                 //f = -f;
18911                 delete proj;
18912                 delete proj2;
18913 
18914                 //        Compute gradient g for the sample problem.
18915                 float dt = 1.0e-3f;
18916                 proj = new EMData();
18917                 proj2 = new EMData();
18918                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
18919                 proj->fft_shuffle();
18920                 proj->center_origin_fft();
18921                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18922                 proj->do_ift_inplace();
18923                 proj2 = proj->window_center(M);
18924                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18925                 //ft = -ft;
18926                 delete proj;
18927                 delete proj2;
18928                 g[0] = (ft-f)/dt;
18929 
18930                 proj = new EMData();
18931                 proj2 = new EMData();
18932                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
18933                 proj->fft_shuffle();
18934                 proj->center_origin_fft();
18935                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18936                 proj->do_ift_inplace();
18937                 proj2 = proj->window_center(M);
18938                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18939                 //ft = -ft;
18940                 delete proj;
18941                 delete proj2;
18942                 g[1] = (ft-f)/dt;
18943 
18944                 proj = new EMData();
18945                 proj2 = new EMData();
18946                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
18947                 proj->fft_shuffle();
18948                 proj->center_origin_fft();
18949                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18950                 proj->do_ift_inplace();
18951                 proj2 = proj->window_center(M);
18952                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18953                 //ft = -ft;
18954                 delete proj;
18955                 delete proj2;
18956                 g[2] = (ft-f)/dt;
18957 
18958                 proj = new EMData();
18959                 proj2 = new EMData();
18960                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18961                 proj->fft_shuffle();
18962                 proj->center_origin_fft();
18963                 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
18964                 proj->do_ift_inplace();
18965                 proj2 = proj->window_center(M);
18966                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18967                 //ft = -ft;
18968                 delete proj;
18969                 delete proj2;
18970                 g[3] = (ft-f)/dt;
18971 
18972                 proj = new EMData();
18973                 proj2 = new EMData();
18974                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18975                 proj->fft_shuffle();
18976                 proj->center_origin_fft();
18977                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
18978                 proj->do_ift_inplace();
18979                 proj2 = proj->window_center(M);
18980                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18981                 //ft = -ft;
18982                 delete proj;
18983                 delete proj2;
18984                 g[4] = (ft-f)/dt;
18985                 }
18986 
18987                 //c          go back to the minimization routine.
18988                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18989                 step++;
18990         }
18991 
18992         //printf("Total step is %d\n", step);
18993         vector<float> res;
18994         res.push_back(static_cast<float>(x[0]));
18995         res.push_back(static_cast<float>(x[1]));
18996         res.push_back(static_cast<float>(x[2]));
18997         res.push_back(static_cast<float>(x[3]));
18998         res.push_back(static_cast<float>(x[4]));
18999         //res.push_back(step);
19000         return res;
19001 }
19002 
19003 
19004 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
19005 
19006         double  x[4];
19007         int n;
19008         int l = 3;
19009         int m = 200;
19010         double e = 1e-9;
19011         double step = 0.01;
19012         float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
19013 
19014         x[1] = ang;
19015         x[2] = sxs;
19016         x[3] = sys;
19017 
19018         Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);   // Call steepest descent optimization subroutine
19019         //printf("Took %d steps\n", n);
19020 
19021         vector<float> res;
19022         res.push_back(static_cast<float>(x[1]));
19023         res.push_back(static_cast<float>(x[2]));
19024         res.push_back(static_cast<float>(x[3]));
19025         res.push_back(static_cast<float>(n));
19026         return res;
19027 }
19028 
19029 vector<float> Util::multi_align_error(vector<float> args, vector<float> all_ali_params) {
19030         
19031         const int nmax=args.size(), mmax=nmax;
19032         char task[60], csave[60];
19033         long int lsave[4];
19034         long int n, m, iprint, isave[44];
19035         long int* nbd = new long int[nmax];
19036         long int* iwa = new long int[3*nmax];
19037         double f, factr, pgtol;
19038         double* x = new double[nmax];
19039         double* l = new double[nmax];
19040         double* u = new double[nmax];
19041         double* g = new double[nmax];
19042         double dsave[29];
19043         double* wa = new double[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
19044         long int SIXTY=60;
19045 
19046         int num_ali = nmax/3+1;
19047         int nima = all_ali_params.size()/(num_ali*4);
19048         
19049         //     We wish to have no output.
19050         iprint = -1;
19051 
19052         //c     We specify the tolerances in the stopping criteria.
19053         factr=1.0e1;
19054         pgtol=1.0e-9;
19055 
19056         //     We specify the dimension n of the sample problem and the number
19057         //        m of limited memory corrections stored.  (n and m should not
19058         //        exceed the limits nmax and mmax respectively.)
19059         n=nmax;
19060         m=mmax;
19061 
19062         //     We now provide nbd which defines the bounds on the variables:
19063         //                    l   specifies the lower bounds,
19064         //                    u   specifies the upper bounds.
19065         //                    x   specifies the initial guess
19066         for (int i=0; i<nmax; i++) {
19067                 x[i] = args[i]; 
19068                 nbd[i] = 0;
19069         }
19070         
19071         //     We start the iteration by initializing task.
19072         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19073         strcpy(task,"START");
19074         for (int i=5;i<60;i++)  task[i]=' ';
19075 
19076         //     This is the call to the L-BFGS-B code.
19077         // (* call the L-BFGS-B routine with task='START' once before loop *)
19078         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19079         int step = 1;
19080 
19081         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19082         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19083 
19084                 if (strncmp(task,"FG",2)==0) {
19085                 //   the minimization routine has returned to request the
19086                 //   function f and gradient g values at the current x
19087 
19088                 //        Compute function value f for the sample problem.
19089                 f = multi_align_error_func(x, all_ali_params, nima, num_ali);
19090 
19091                 //        Compute gradient g for the sample problem.
19092                 multi_align_error_dfunc(x, all_ali_params, nima, num_ali, g);
19093                 }
19094 
19095                 //c          go back to the minimization routine.
19096                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19097                 step++;
19098         }
19099 
19100         //printf("Total step is %d\n", step);
19101         vector<float> res;
19102         for (int i=0; i<nmax; i++) res.push_back(static_cast<float>(x[i]));
19103         res.push_back(f);
19104         
19105         delete[] nbd;
19106         delete[] iwa;
19107         delete[] x;
19108         delete[] l;
19109         delete[] u;
19110         delete[] g;
19111         delete[] wa;
19112         
19113         return res;
19114 
19115 }
19116 
19117 float Util::multi_align_error_func(double* x, vector<float> all_ali_params, int nima, int num_ali) {
19118 
19119         float x1 = 1.0;
19120         float y1 = 0.0;
19121         float x2 = 0.0;
19122         float y2 = 1.0;
19123 
19124         float all_var = 0;
19125         float* x1_new = new float[num_ali];
19126         float* y1_new = new float[num_ali];
19127         float* x2_new = new float[num_ali];
19128         float* y2_new = new float[num_ali];
19129 
19130         for (int i=0; i<nima; i++) {
19131                 float alpha2 = all_ali_params[(num_ali-1)*(nima*4)+i*4];
19132                 float sx2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+1];
19133                 float sy2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+2];
19134                 
19135                 rot_shift(x1, y1, alpha2, sx2, sy2, x1_new+num_ali-1, y1_new+num_ali-1);
19136                 rot_shift(x2, y2, alpha2, sx2, sy2, x2_new+num_ali-1, y2_new+num_ali-1);
19137                 for (int j=0; j<num_ali-1; j++) {
19138                         float alpha1 = all_ali_params[j*(nima*4)+i*4];
19139                         float sx1 = all_ali_params[j*(nima*4)+i*4+1];
19140                         float sy1 = all_ali_params[j*(nima*4)+i*4+2];
19141                         int mirror1 = static_cast<int>(all_ali_params[j*(nima*4)+i*4+3]);
19142 
19143                         float alphai = x[j*3];
19144                         float sxi = x[j*3+1];
19145                         float syi = x[j*3+2];
19146 
19147                         float alpha12, sx12, sy12;
19148                         int mirror12;
19149                         if (mirror1 == 0) {
19150                                 alpha12 = fmod(alpha1+alphai, 360.0f);
19151                                 rot_shift(sx1, sy1, alphai, sxi, syi, &sx12, &sy12);
19152                                 mirror12 = 0;
19153                         } else {
19154                                 alpha12 = fmod(alpha1-alphai, 360.0f);
19155                                 rot_shift(sx1, sy1, -alphai, -sxi, syi, &sx12, &sy12);
19156                                 mirror12 = 1;
19157                         }
19158 
19159                         rot_shift(x1, y1, alpha12, sx12, sy12, x1_new+j, y1_new+j);
19160                         rot_shift(x2, y2, alpha12, sx12, sy12, x2_new+j, y2_new+j);
19161                 }
19162                 
19163                 float p = var(x1_new, num_ali)+var(y1_new, num_ali)+var(x2_new, num_ali)+var(y2_new, num_ali);
19164                 all_var += p;
19165         }
19166         delete[] x1_new;
19167         delete[] y1_new;
19168         delete[] x2_new;
19169         delete[] y2_new;
19170         return all_var/static_cast<float>(nima);
19171 }
19172 
19173 void Util::multi_align_error_dfunc(double* x, vector<float> all_ali_params, int nima, int num_ali, double* g) {
19174 
19175         
19176         for (int i=0; i<num_ali*3-3; i++) g[i] = 0.0;
19177         
19178         float x1 = 1.0;
19179         float y1 = 0.0;
19180         float x2 = 0.0;
19181         float y2 = 1.0;
19182 
19183         float* x1_new = new float[num_ali];
19184         float* y1_new = new float[num_ali];
19185         float* x2_new = new float[num_ali];
19186         float* y2_new = new float[num_ali];
19187 
19188         float* alpha12_0 = new float[num_ali-1];
19189         float* dalpha12 = new float[num_ali-1];
19190         float* dsx12 = new float[num_ali-1];
19191         float* dsy12 = new float[num_ali-1];
19192         float* mirror1_0 = new float[num_ali-1];
19193 
19194         for (int i=0; i<nima; i++) {
19195                 
19196                 float alpha2 = all_ali_params[(num_ali-1)*(nima*4)+i*4];
19197                 float sx2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+1];
19198                 float sy2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+2];
19199                 
19200                 rot_shift(x1, y1, alpha2, sx2, sy2, x1_new+num_ali-1, y1_new+num_ali-1);
19201                 rot_shift(x2, y2, alpha2, sx2, sy2, x2_new+num_ali-1, y2_new+num_ali-1);
19202                 
19203                 for (int j=0; j<num_ali-1; j++) {
19204                         float alpha1 = all_ali_params[j*(nima*4)+i*4];
19205                         float sx1 = all_ali_params[j*(nima*4)+i*4+1];
19206                         float sy1 = all_ali_params[j*(nima*4)+i*4+2];
19207                         int mirror1 = static_cast<int>(all_ali_params[j*(nima*4)+i*4+3]);
19208 
19209                         float alphai = x[j*3];
19210                         float sxi = x[j*3+1];
19211                         float syi = x[j*3+2];
19212 
19213                         float cosi = cos(alphai/180.0f*M_PI);
19214                         float sini = sin(alphai/180.0f*M_PI);
19215                         
19216                         float alpha12, sx12, sy12;
19217                         int mirror12;
19218                         if (mirror1 == 0) {
19219                                 alpha12 = fmod(alpha1+alphai, 360.0f);
19220                                 rot_shift(sx1, sy1, alphai, sxi, syi, &sx12, &sy12);
19221                                 mirror12 = 0;
19222                         } else {
19223                                 alpha12 = fmod(alpha1-alphai, 360.0f);
19224                                 rot_shift(sx1, sy1, -alphai, -sxi, syi, &sx12, &sy12);
19225                                 mirror12 = 1;
19226                         }
19227 
19228                         rot_shift(x1, y1, alpha12, sx12, sy12, x1_new+j, y1_new+j);
19229                         rot_shift(x2, y2, alpha12, sx12, sy12, x2_new+j, y2_new+j);
19230                 
19231                         alpha12_0[j] = alpha12;
19232                         mirror1_0[j] = mirror1;
19233                         if (mirror1 == 0) {
19234                                 dalpha12[j] = M_PI/180.0f;
19235                                 dsx12[j] = (-sini*sx1+cosi*sy1)/180.0f*M_PI;
19236                                 dsy12[j] = (-cosi*sx1-sini*sy1)/180.0f*M_PI;
19237                         } else {
19238                                 dalpha12[j] = -M_PI/180.0f;
19239                                 dsx12[j] = (sini*(-sx1)-cosi*sy1)/180.0f*M_PI;
19240                                 dsy12[j] = (-cosi*(-sx1)-sini*sy1)/180.0f*M_PI;
19241                         }
19242                 }
19243 
19244                 for (int j=0; j<num_ali-1; j++) {
19245                         float cosa = cos(alpha12_0[j]/180.0f*M_PI);
19246                         float sina = sin(alpha12_0[j]/180.0f*M_PI);
19247                         float diffx1 = x1_new[j]-mean(x1_new, num_ali);
19248                         float diffx2 = x2_new[j]-mean(x2_new, num_ali);
19249                         float diffy1 = y1_new[j]-mean(y1_new, num_ali);
19250                         float diffy2 = y2_new[j]-mean(y2_new, num_ali);
19251 
19252                         float p = diffx1*((-x1*sina+y1*cosa)*dalpha12[j]+dsx12[j])+diffx2*((-x2*sina+y2*cosa)*dalpha12[j]+dsx12[j])+diffy1*((-x1*cosa-y1*sina)*dalpha12[j]+dsy12[j])+diffy2*((-x2*cosa-y2*sina)*dalpha12[j]+dsy12[j]);
19253                         g[j*3] += p;
19254                 
19255                         p = diffx1+diffx2;
19256                         if (mirror1_0[j] == 0) g[j*3+1] += p;
19257                         else g[j*3+1] -= p;
19258 
19259                         p = diffy1+diffy2;
19260                         g[j*3+2] += p;
19261                 }
19262         }
19263 
19264         delete[] x1_new;
19265         delete[] y1_new;
19266         delete[] x2_new;
19267         delete[] y2_new;
19268         delete[] alpha12_0;
19269         delete[] dalpha12;
19270         delete[] dsx12;
19271         delete[] dsy12;
19272         delete[] mirror1_0;
19273         
19274 }
19275 
19276 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
19277 
19278         EMData *rot= new EMData();
19279         float ccc;
19280 
19281         rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
19282         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19283         delete rot;
19284         return ccc;
19285 }
19286 
19287 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19288 
19289         double  x[4];
19290         int n;
19291         int l = 3;
19292         int m = 200;
19293         double e = 1e-9;
19294         double step = 0.001;
19295         float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
19296 
19297         x[1] = ang;
19298         x[2] = sxs;
19299         x[3] = sys;
19300 
19301         Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);   // Call steepest descent optimization subroutine
19302         //printf("Took %d steps\n", n);
19303 
19304         vector<float> res;
19305         res.push_back(static_cast<float>(x[1]));
19306         res.push_back(static_cast<float>(x[2]));
19307         res.push_back(static_cast<float>(x[3]));
19308         res.push_back(static_cast<float>(n));
19309         return res;
19310 }
19311 
19312 
19313 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
19314 
19315         EMData *rot= new EMData();
19316         float ccc;
19317 
19318         rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
19319         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19320         delete rot;
19321         return ccc;
19322 }
19323 
19324 #define img_ptr(i,j,k)  img_ptr[i+(j+(k*ny))*(size_t)nx]
19325 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*(size_t)nx]
19326 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
19327 {
19328         ENTERFUNC;
19329         /* Exception Handle */
19330         if (!img) {
19331                 throw NullPointerException("NULL input image");
19332         }
19333 
19334         int newx, newy, newz;
19335         bool  keep_going;
19336         cout << " entered   " <<endl;
19337         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
19338         //int size = nx*ny*nz;
19339         EMData * img2 = new EMData();
19340         img2->set_size(nx,ny,nz);
19341         img2->to_zero();
19342         float *img_ptr  =img->get_data();
19343         float *img2_ptr = img2->get_data();
19344         int r2 = ro*ro;
19345         int r3 = r2*ro;
19346         int ri2 = ri*ri;
19347         int ri3 = ri2*ri;
19348 
19349         int n2 = nx/2;
19350 
19351         for (int k=-n2; k<=n2; k++) {           //cout << " k   "<<k <<endl;
19352                 float z2 = static_cast<float>(k*k);
19353                 for (int j=-n2; j<=n2; j++) {
19354                         float y2 = z2 + j*j;
19355                         if(y2 <= r2) {
19356                                                                                         //cout << "  j  "<<j <<endl;
19357 
19358                                 for (int i=-n2; i<=n2; i++) {
19359                                         float x2 = y2 + i*i;
19360                                         if(x2 <= r3) {
19361                                                                                         //cout << "  i  "<<i <<endl;
19362                                                 int ib = i+n2; int jb = j+n2; int kb = k+n2;
19363                                                 if(x2 >= ri3) {
19364                                                         //  this is the outer shell, here points can only vanish
19365                                                         if(img_ptr(ib,jb,kb) == 1.0f) {
19366                                                                 //cout << "  1  "<<ib <<endl;
19367                                                                 if(Util::get_frand(0.0f, 1.0f) > qprob){
19368                                                                         img2_ptr(ib,jb,kb) = 0.0f;
19369                                                                         keep_going = true;
19370                                                                 //cout << "  try  "<<ib <<endl;
19371                                                                         while(keep_going) {
19372                                                                                 newx = Util::get_irand(-ro,ro);
19373                                                                                 newy = Util::get_irand(-ro,ro);
19374                                                                                 newz = Util::get_irand(-ro,ro);
19375                                                                                 if(newx*newx+newy*newy+newz*newz <= r3) {
19376                                                                                         newx += n2; newy += n2; newz += n2;
19377                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
19378                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
19379                                                                                                 keep_going = false; }
19380                                                                                 }
19381                                                                         }
19382                                                                 }   else  img2_ptr(ib,jb,kb) = 1.0f;
19383                                                         }
19384                                                 }  else  {
19385                                                         // this is inner shell, the point can only move (or vanish, if all neighbors exist)
19386                                                         if(img_ptr(ib,jb,kb) == 1.0) {
19387                                                                 if(Util::get_frand(0.0f,1.0f) > qprob) {
19388                                                                         //  find out the number of neighbors
19389                                                                         float  numn = -1.0f;  // we already know the central one is 1
19390                                                                         for (newz = -1; newz <= 1; newz++)
19391                                                                                 for (newy = -1; newy <= 1; newy++)
19392                                                                                         for (newx = -1; newx <= 1; newx++)
19393                                                                                                 numn += img_ptr(ib+newx,jb+newy,kb+newz);
19394                                                                         img2_ptr(ib,jb,kb) = 0.0;
19395                                                                         if(numn == 26.0f) {
19396                                                                                 //  all neighbors exist, it has to vanish
19397                                                                                 keep_going = true;
19398                                                                                 while(keep_going) {
19399                                                                                         newx = Util::get_irand(-ro,ro);
19400                                                                                         newy = Util::get_irand(-ro,ro);
19401                                                                                         newz = Util::get_irand(-ro,ro);
19402                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
19403                                                                                                 newx += n2; newy += n2; newz += n2;
19404                                                                                                 if( img_ptr(newx,newy,newz) == 0.0f) {
19405                                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
19406                                                                                                                 if(newx*newx+newy*newy+newz*newz < r3) {
19407                                                                                                                         newx += n2; newy += n2; newz += n2;
19408                                                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
19409                                                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
19410                                                                                                                                 keep_going = false; }
19411                                                                                                                 }
19412                                                                                                         }
19413                                                                                                 }
19414                                                                                         }
19415                                                                                 }
19416                                                                         }  else if(numn == 25.0f) {
19417                                                                                 // there is only one empty neighbor, move there
19418                                                                                 for (newz = -1; newz <= 1; newz++) {
19419                                                                                         for (newy = -1; newy <= 1; newy++) {
19420                                                                                                 for (newx = -1; newx <= 1; newx++) {
19421                                                                                                         if( newx != 0 && newy != 0 && newz != 0) {
19422                                                                                                                 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
19423                                                                                                                         img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
19424                                                                                                                         }
19425                                                                                                         }
19426                                                                                                 }
19427                                                                                         }
19428                                                                                 }
19429                                                                         }  else {
19430                                                                                 //  more than one neighbor is zero, select randomly one and move there
19431                                                                                 keep_going = true;
19432                                                                                 while(keep_going) {
19433                                                                                         newx = Util::get_irand(-1,1);
19434                                                                                         newy = Util::get_irand(-1,1);
19435                                                                                         newz = Util::get_irand(-1,1);
19436                                                                                         if(newx != 0 && newy != 0 && newz != 0)  {
19437                                                                                                 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
19438                                                                                                         img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;//?????
19439                                                                                                         keep_going = false;
19440                                                                                                 }
19441                                                                                         }
19442                                                                                 }
19443                                                                         }
19444                                                                 }  else img2_ptr(ib,jb,kb) = 1.0f;
19445                                                         }
19446                                                 }
19447                                         }
19448                                 }
19449                         }
19450                 }
19451         }
19452         //for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + Util::get_irand(1,7);
19453         img2->update();
19454 
19455         EXITFUNC;
19456         return img2;
19457 }
19458 #undef img_ptr
19459 #undef img2_ptr
19460 
19461 struct point3d_t
19462 {
19463         point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
19464 
19465         int x;
19466         int y;
19467         int z;
19468 };
19469 
19470 
19471 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
19472 {
19473         int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
19474         int noff = 6;
19475 
19476         int nx = visited->get_xsize();
19477         int ny = visited->get_ysize();
19478         int nz = visited->get_zsize();
19479 
19480         vector< point3d_t > pts;
19481         pts.push_back( point3d_t(ix, iy, iz) );
19482         visited->set_value_at( ix, iy, iz, (float)grpid );
19483 
19484         int start = 0;
19485         int end = pts.size();
19486 
19487         while( end > start ) {
19488                 for(int i=start; i < end; ++i ) {
19489                         int ix = pts[i].x;
19490                         int iy = pts[i].y;
19491                         int iz = pts[i].z;
19492 
19493                         for( int j=0; j < noff; ++j ) {
19494                                 int jx = ix + offs[j][0];
19495                                 int jy = iy + offs[j][1];
19496                                 int jz = iz + offs[j][2];
19497 
19498                                 if( jx < 0 || jx >= nx ) continue;
19499                                 if( jy < 0 || jy >= ny ) continue;
19500                                 if( jz < 0 || jz >= nz ) continue;
19501 
19502 
19503                                 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
19504                                     pts.push_back( point3d_t(jx, jy, jz) );
19505                                     visited->set_value_at( jx, jy, jz, (float)grpid );
19506                                 }
19507 
19508                         }
19509                 }
19510 
19511                 start = end;
19512                 end = pts.size();
19513         }
19514         return pts.size();
19515 }
19516 
19517 
19518 EMData* Util::get_biggest_cluster( EMData* mg )
19519 {
19520         int nx = mg->get_xsize();
19521         int ny = mg->get_ysize();
19522         int nz = mg->get_zsize();
19523 
19524         EMData* visited = new EMData();
19525         visited->set_size( nx, ny, nz );
19526         visited->to_zero();
19527         int grpid = 0;
19528         int maxgrp = 0;
19529         int maxsize = 0;
19530         for( int iz=0; iz < nz; ++iz ) {
19531                 for( int iy=0; iy < ny; ++iy ) {
19532                         for( int ix=0; ix < nx; ++ix ) {
19533                                 if( (*mg)(ix, iy, iz)==0.0 ) continue;
19534 
19535                                 if( (*visited)(ix, iy, iz) > 0.0 ) {
19536                                         // visited before, must be in other group.
19537                                         continue;
19538                                 }
19539 
19540                                 grpid++;
19541                                 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
19542                                 if( grpsize > maxsize ) {
19543                                         maxsize = grpsize;
19544                                         maxgrp = grpid;
19545                                 }
19546                         }
19547                 }
19548         }
19549 
19550         Assert( maxgrp > 0 );
19551 
19552         int npoint = 0;
19553         EMData* result = new EMData();
19554         result->set_size( nx, ny, nz );
19555         result->to_zero();
19556 
19557         for( int iz=0; iz < nz; ++iz ) {
19558                 for( int iy=0; iy < ny; ++iy ) {
19559                         for( int ix=0; ix < nx; ++ix ) {
19560                                 if( (*visited)(ix, iy, iz)==maxgrp ) {
19561                                         (*result)(ix,iy,iz) = 1.0;
19562                                         npoint++;
19563                                 }
19564                         }
19565                 }
19566         }
19567 
19568         Assert( npoint==maxsize );
19569         delete visited;
19570         return result;
19571 
19572 }
19573 
19574 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)
19575 {
19576         int   ix, iy, iz;
19577         int   i,  j, k;
19578         int   nr2, nl2;
19579         float  dzz, az, ak;
19580         float  scx, scy, scz;
19581         int offset = 2 - nx%2;
19582         int lsm = nx + offset;
19583         EMData* ctf_img1 = new EMData();
19584         ctf_img1->set_size(lsm, ny, nz);
19585         float freq = 1.0f/(2.0f*ps);
19586         scx = 2.0f/float(nx);
19587         if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
19588         if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
19589         nr2 = ny/2 ;
19590         nl2 = nz/2 ;
19591         for ( k=0; k<nz;k++) {
19592                 iz = k;  if(k>nl2) iz=k-nz;
19593                 for ( j=0; j<ny;j++) {
19594                         iy = j;  if(j>nr2) iy=j - ny;
19595                         for ( i=0; i<lsm/2; i++) {
19596                                 ix=i;
19597                                 ak=pow(ix*ix*scx*scx+iy*scy*iy*scy+iz*scz*iz*scz, 0.5f)*freq;
19598                                 if(ak!=0) az=0.0; else az=M_PI;
19599                                 dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f));
19600                                 (*ctf_img1) (i*2,j,k)   = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
19601                                 (*ctf_img1) (i*2+1,j,k) = 0.0f;
19602                         }
19603                 }
19604         }
19605         ctf_img1->update();
19606         ctf_img1->set_complex(true);
19607         ctf_img1->set_ri(true);
19608         //ctf_img1->attr_dict["is_complex"] = 1;
19609         //ctf_img1->attr_dict["is_ri"] = 1;
19610         if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
19611         return ctf_img1;
19612 }
19613 /*
19614 #define  cent(i)     out[i+N]
19615 #define  assign(i)   out[i]
19616 vector<float> Util::cluster_pairwise(EMData* d, int K) {
19617 
19618         int nx = d->get_xsize();
19619         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19620         vector<float> out(N+K+2);
19621         if(N*(N-1)/2 != nx) {
19622                 //print  "  incorrect dimension"
19623                 return out;}
19624         //  assign random objects as centers
19625         for(int i=0; i<N; i++) assign(i) = float(i);
19626         // shuffle
19627         for(int i=0; i<N; i++) {
19628                 int j = Util::get_irand(0,N-1);
19629                 float temp = assign(i);
19630                 assign(i) = assign(j);
19631                 assign(j) = temp;
19632         }
19633         for(int k=0; k<K; k++) cent(k) = float(assign(k));
19634         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
19635         //
19636         for(int i=0; i<N; i++) assign(i) = 0.0f;
19637         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
19638         bool change = true;
19639         int it = -1;
19640         while(change && disp < dispold) {
19641                 change = false;
19642                 dispold = disp;
19643                 it++;
19644                 //cout<<"Iteration:  "<<it<<endl;
19645                 // dispersion is a sum of distance from objects to object center
19646                 disp = 0.0f;
19647                 for(int i=0; i<N; i++) {
19648                         qm = 1.0e23f;
19649                         for(int k=0; k<K; k++) {
19650                                 if(float(i) == cent(k)) {
19651                                         qm = 0.0f;
19652                                         na = (float)k;
19653                                 } else {
19654                                         float dt = (*d)(mono(i,int(cent(k))));
19655                                         if(dt < qm) {
19656                                                 qm = dt;
19657                                                 na = (float)k;
19658                                         }
19659                                 }
19660                         }
19661                         disp += qm;
19662                         if(na != assign(i)) {
19663                                 assign(i) = na;
19664                                 change = true;
19665                         }
19666                 }
19667         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
19668                 //print disp
19669                 //print  assign
19670                 // find centers
19671                 for(int k=0; k<K; k++) {
19672                         qm = 1.0e23f;
19673                         for(int i=0; i<N; i++) {
19674                                 if(assign(i) == float(k)) {
19675                                         float q = 0.0;
19676                                         for(int j=0; j<N; j++) {
19677                                                 if(assign(j) == float(k)) {
19678                                                                 //it cannot be the same object
19679                                                         if(i != j)  q += (*d)(mono(i,j));
19680                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
19681                                                 }
19682                                         }
19683                                         if(q < qm) {
19684                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
19685                                                 qm = q;
19686                                                 cent(k) = float(i);
19687                                         }
19688                                 }
19689                         }
19690                 }
19691         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
19692         }
19693         out[N+K] = disp;
19694         out[N+K+1] = float(it);
19695         return  out;
19696 }
19697 #undef  cent
19698 #undef  assign
19699 */
19700 #define  cent(i)     out[i+N]
19701 #define  assign(i)   out[i]
19702 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
19703         int nx = d->get_xsize();
19704         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19705         vector<float> out(N+K+2);
19706         if(N*(N-1)/2 != nx) {
19707                 //print  "  incorrect dimension"
19708                 return out;}
19709         //  assign random objects as centers
19710         for(int i=0; i<N; i++) assign(i) = float(i);
19711         // shuffle
19712         for(int i=0; i<N; i++) {
19713                 int j = Util::get_irand(0,N-1);
19714                 float temp = assign(i);
19715                 assign(i) = assign(j);
19716                 assign(j) = temp;
19717         }
19718         for(int k=0; k<K; k++) cent(k) = float(assign(k));
19719         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
19720         //
19721         for(int i=0; i<N; i++) assign(i) = 0.0f;
19722         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
19723         bool change = true;
19724         int it = -1;
19725         int ct = -1;
19726         while ((change && disp < dispold) || ct > 0) {
19727 
19728                 change = false;
19729                 dispold = disp;
19730                 it++;
19731 
19732                 // dispersion is a sum of distance from objects to object center
19733                 disp = 0.0f;
19734                 ct = 0;
19735                 for(int i=0; i<N; i++) {
19736                         qm = 1.0e23f;
19737                         for(int k=0; k<K; k++) {
19738                                 if(float(i) == cent(k)) {
19739                                         qm = 0.0f;
19740                                         na = (float)k;
19741                                 } else {
19742                                         float dt = (*d)(mono(i,int(cent(k))));
19743                                         if(dt < qm) {
19744                                                 qm = dt;
19745                                                 na = (float)k;
19746                                         }
19747                                 }
19748                         }
19749 
19750 
19751                         // Simulated annealing
19752                         if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
19753                             na = (float)(Util::get_irand(0, K));
19754                             qm = (*d)(mono(i,int(na)));
19755                             ct++;
19756                         }
19757 
19758                         disp += qm;
19759 
19760                         if(na != assign(i)) {
19761                                 assign(i) = na;
19762                                 change = true;
19763                         }
19764                 }
19765 
19766                 //cout<<"Iteration:  "<<it<< " disp " <<disp << " T " << T << " disturb " << ct << endl;
19767                 T = T*F;
19768 
19769         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
19770                 //print disp
19771                 //print  assign
19772                 // find centers
19773                 for(int k=0; k<K; k++) {
19774                         qm = 1.0e23f;
19775                         for(int i=0; i<N; i++) {
19776                                 if(assign(i) == float(k)) {
19777                                         float q = 0.0;
19778                                         for(int j=0; j<N; j++) {
19779                                                 if(assign(j) == float(k)) {
19780                                                                 //it cannot be the same object
19781                                                         if(i != j)  q += (*d)(mono(i,j));
19782                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
19783                                                 }
19784                                         }
19785                                         if(q < qm) {
19786                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
19787                                                 qm = q;
19788                                                 cent(k) = float(i);
19789                                         }
19790                                 }
19791                         }
19792                 }
19793         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
19794         }
19795         out[N+K] = disp;
19796         out[N+K+1] = float(it);
19797         return  out;
19798 }
19799 #undef  cent
19800 #undef  assign
19801 /*
19802 #define  groupping(i,k)   group[i + k*m]
19803 vector<float> Util::cluster_equalsize(EMData* d, int m) {
19804         int nx = d->get_xsize();
19805         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19806         int K = N/m;
19807         //cout<<"  K  "<<K<<endl;
19808         vector<float> group(N+1);
19809         if(N*(N-1)/2 != nx) {
19810                 //print  "  incorrect dimension"
19811                 return group;}
19812         bool active[N];
19813         for(int i=0; i<N; i++) active[i] = true;
19814 
19815         float dm, qd;
19816         int   ppi, ppj;
19817         for(int k=0; k<K; k++) {
19818                 // find two most similiar objects among active
19819                 cout<<"  k  "<<k<<endl;
19820                 dm = 1.0e23;
19821                 for(int i=1; i<N; i++) {
19822                         if(active[i]) {
19823                                 for(int j=0; j<i; j++) {
19824                                         if(active[j]) {
19825                                                 qd = (*d)(mono(i,j));
19826                                                 if(qd < dm) {
19827                                                         dm = qd;
19828                                                         ppi = i;
19829                                                         ppj = j;
19830                                                 }
19831                                         }
19832                                 }
19833                         }
19834                 }
19835                 groupping(0,k) = float(ppi);
19836                 groupping(1,k) = float(ppj);
19837                 active[ppi] = false;
19838                 active[ppj] = false;
19839 
19840                 // find progressively objects most similar to those in the current list
19841                 for(int l=2; l<m; l++) {
19842                         //cout<<"  l  "<<l<<endl;
19843                         dm = 1.0e23;
19844                         for(int i=0; i<N; i++) {
19845                                 if(active[i]) {
19846                                         qd = 0.0;
19847                                         for(int j=0; j<l; j++) { //j in groupping[k]:
19848                         //cout<<"  groupping(j,k)  "<<groupping(j,k)<<"   "<<j<<endl;
19849                                                 int jj = int(groupping(j,k));
19850                         //cout<<"   "<<jj<<endl;
19851                                                 qd += (*d)(mono(i,jj));
19852                                         }
19853                                         if(qd < dm) {
19854                                                 dm = qd;
19855                                                 ppi = i;
19856                                         }
19857                                 }
19858                         }
19859                         groupping(l,k) = float(ppi);
19860                         active[ppi] = false;
19861                         //cout<<"  k  "<<k<<"  l  "<<l<<"  ppi  "<<ppi<<"  dm  "<<dm<<endl;
19862                 }
19863                 //for(int j=k*m; j<(k+1)*m; j++) cout<<group[j]<<endl;
19864         }
19865         // there might be remaining objects when N is not divisible by m, simply put them in one group
19866         if(N%m != 0) {
19867                 int j = K*m;
19868                 K++;
19869                 //cout<<"  N%m  "<<N%m<<"   "<<K<<endl;
19870                 for(int i=0; i<N; i++) {
19871                         if(active[i]) {
19872                                 group[j] = float(i);
19873                 //cout<<"  j,i  "<<j<<"   "<<i<<"   "<<K-1 <<"   "<< group[j]<<endl;
19874                                 j++;
19875                         }
19876                 }
19877         }
19878 
19879         int  cent[K];
19880          // find centers
19881         for(int k=0; k<K; k++) {
19882                 float qm = 1.0e23f;
19883                 for(int i=0; i<N; i++) {
19884                         if(group[i] == float(k)) {
19885                                 qd = 0.0;
19886                                 for(int j=0; j<N; j++) {
19887                                         if(group[j] == float(k)) {
19888                                                 //it cannot be the same object
19889                                                 if(i != j)  qd += (*d)(mono(i,j));
19890                                         }
19891                                 }
19892                                 if(qd < qm) {
19893                                         qm = qd;
19894                                         cent[k] = i;
19895                                 }
19896                         }
19897                 }
19898         }
19899         // dispersion is a sum of distances from objects to object center
19900         float disp = 0.0f;
19901         for(int i=0; i<N; i++) {
19902                 for(int k=0; k<K; k++) if(i == cent[k]) disp += (*d)(mono(i,cent[k]));
19903         }
19904         group[N] = disp;
19905         return  group;
19906 }
19907 #undef  groupping
19908 */
19909 
19910 vector<float> Util::cluster_equalsize(EMData* d) {
19911         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
19912         int nx = d->get_xsize();
19913         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19914         int K = N/2;
19915         vector<float> group(N);
19916         if(N*(N-1)/2 != nx) {
19917                 //print  "  incorrect dimension"
19918                 return group;}
19919         //bool active[N];       //this does not compile in VS2005. --Grant Tang
19920         bool * active = new bool[N];
19921         for(int i=0; i<N; i++) active[i] = true;
19922 
19923         float dm, qd;
19924         int   ppi = 0, ppj = 0;
19925         for(int k=0; k<K; k++) {
19926                 // find pairs of most similiar objects among active
19927                 //cout<<"  k  "<<k<<endl;
19928                 dm = 1.0e23f;
19929                 for(int i=1; i<N; i++) {
19930                         if(active[i]) {
19931                                 for(int j=0; j<i; j++) {
19932                                         if(active[j]) {
19933                                                 qd = (*d)(i*(i - 1)/2 + j);
19934                                                 if(qd < dm) {
19935                                                         dm = qd;
19936                                                         ppi = i;
19937                                                         ppj = j;
19938                                                 }
19939                                         }
19940                                 }
19941                         }
19942                 }
19943                 group[2*k] = float(ppi);
19944                 group[1+2*k] = float(ppj);
19945                 active[ppi] = false;
19946                 active[ppj] = false;
19947         }
19948 
19949         delete [] active;
19950         active = NULL;
19951         return  group;
19952 }
19953 /*
19954 #define son(i,j)=i*(i-1)/2+j
19955 vector<float> Util::cluster_equalsize(EMData* d) {
19956         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
19957         int nx = d->get_xsize();
19958         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19959         int K = N/2;
19960         vector<float> group(N);
19961         if(N*(N-1)/2 != nx) {
19962                 //print  "  incorrect dimension"
19963                 return group;}
19964         //bool active[N];
19965         int  active[N];
19966         for(int i=0; i<N; i++) active[i] = i;
19967 
19968         float dm, qd;
19969         int   ppi = 0, ppj = 0, ln = N;
19970         for(int k=0; k<K; k++) {
19971                 // find pairs of most similiar objects among active
19972                 //cout<<"  k:  "<<k<<endl;
19973                 dm = 1.0e23;
19974                 for(int i=1; i<ln; i++) {
19975                         for(int j=0; j<i; j++) {
19976                                 //qd = (*d)(mono(active[i],active[j]));
19977                                 qd = (*d)(active[i]*(active[i] - 1)/2 + active[j]);
19978                                 if(qd < dm) {
19979                                         dm = qd;
19980                                         ppi = i;
19981                                         ppj = j;
19982                                 }
19983                         }
19984                 }
19985                 group[2*k]   = float(active[ppi]);
19986                 group[1+2*k] = float(active[ppj]);
19987                 //  Shorten the list
19988                 if(ppi > ln-3 || ppj > ln - 3) {
19989                         if(ppi > ln-3 && ppj > ln - 3) {
19990                         } else if(ppi > ln-3) {
19991                                 if(ppi == ln -1) active[ppj] = active[ln-2];
19992                                 else             active[ppj] = active[ln-1];
19993                         } else { // ppj>ln-3
19994                                 if(ppj == ln -1) active[ppi] = active[ln-2];
19995                                 else             active[ppi] = active[ln-1];
19996                         }
19997                 } else {
19998                         active[ppi] = active[ln-1];
19999                         active[ppj] = active[ln-2];
20000                 }
20001                 ln = ln - 2;
20002         }
20003         return  group;
20004 }
20005 
20006 */
20007 #define data(i,j) group[i*ny+j]
20008 vector<float> Util::vareas(EMData* d) {
20009         const float step=0.001f;
20010         int ny = d->get_ysize();
20011         //  input emdata should have size 2xN, where N is number of points
20012         //  output vector should be 2xN, first element is the number of elements
20013         //  associated with this point, second is 0 is the element is touching the border, 1 if it is interior
20014         vector<float> group(2*ny);
20015         for(int i=0; i<2*ny; i++) group[i] = 0.0f;
20016         int K = int(1.0f/step) +1;
20017         int hit = 0;
20018         for(int kx=0; kx<=K; kx++) {
20019                 float tx = kx*step;
20020                 for(int ky=0; ky<=K; ky++) {
20021                         float ty = ky*step;
20022                         float dm = 1.0e23f;
20023                         for(int i=0; i<ny; i++) {
20024                                 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
20025                                 if( qd < dm) {
20026                                         dm = qd;
20027                                         hit = i;
20028                                 }
20029                         }
20030                         data(0,hit) += 1.0f;
20031                         if(kx == 0 || ky == 0 || kx == K || ky == K)  data(1,hit) = 1.0f;
20032                 }
20033         }
20034         return  group;
20035 }
20036 #undef data
20037 
20038 EMData* Util::get_slice(EMData *vol, int dim, int index) {
20039 
20040         int nx = vol->get_xsize();
20041         int ny = vol->get_ysize();
20042         int nz = vol->get_zsize();
20043         float *vol_data = vol->get_data();
20044         int new_nx, new_ny;
20045 
20046         if (nz == 1)
20047                 throw ImageDimensionException("Error: Input must be a 3-D object");
20048         if ((dim < 1) || (dim > 3))
20049                 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
20050         if (((dim == 1) && (index < 0 || index > nx-1)) ||
20051           ((dim == 1) && (index < 0 || index > nx-1)) ||
20052           ((dim == 1) && (index < 0 || index > nx-1)))
20053                 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
20054 
20055         if (dim == 1) {
20056                 new_nx = ny;
20057                 new_ny = nz;
20058         } else if (dim == 2) {
20059                 new_nx = nx;
20060                 new_ny = nz;
20061         } else {
20062                 new_nx = nx;
20063                 new_ny = ny;
20064         }
20065 
20066         EMData *slice = new EMData();
20067         slice->set_size(new_nx, new_ny, 1);
20068         float *slice_data = slice->get_data();
20069 
20070         if (dim == 1) {
20071                 for (int x=0; x<new_nx; x++)
20072                         for (int y=0; y<new_ny; y++)
20073                                 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
20074         } else if (dim == 2) {
20075                 for (int x=0; x<new_nx; x++)
20076                         for (int y=0; y<new_ny; y++)
20077                                 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
20078         } else {
20079                 for (int x=0; x<new_nx; x++)
20080                         for (int y=0; y<new_ny; y++)
20081                                 slice_data[y*new_nx+x] = vol_data[((size_t)index*ny+y)*nx+x];
20082         }
20083 
20084         return slice;
20085 }
20086 
20087 void Util::image_mutation(EMData *img, float mutation_rate) {
20088         int nx = img->get_xsize();
20089         float min = img->get_attr("minimum");
20090         float max = img->get_attr("maximum");
20091         float* img_data = img->get_data();
20092         array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
20093         return;
20094 }
20095 
20096 
20097 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20098 
20099         if (is_mirror != 0) {
20100                 for (int i=0; i<len_list; i++) {
20101                         int r = rand()%10000;
20102                         float f = r/10000.0f;
20103                         if (f < mutation_rate) list[i] = 1-list[i];
20104                 }
20105         } else {
20106                 map<int, vector<int> >  graycode;
20107                 map<vector<int>, int> rev_graycode;
20108                 vector <int> gray;
20109 
20110                 int K=1;
20111                 for (int i=0; i<L; i++) K*=2;
20112 
20113                 for (int k=0; k<K; k++) {
20114                         int shift = 0;
20115                         vector <int> gray;
20116                         for (int i=L-1; i>-1; i--) {
20117                                 int t = ((k>>i)%2-shift)%2;
20118                                 gray.push_back(t);
20119                                 shift += t-2;
20120                         }
20121                         graycode[k] = gray;
20122                         rev_graycode[gray] = k;
20123                 }
20124 
20125                 float gap = (K-1)/(max_val-min_val);
20126                 for (int i=0; i<len_list; i++) {
20127                         float val = list[i];
20128                         if (val < min_val) { val = min_val; }
20129                         else if  (val > max_val) { val = max_val; }
20130                         int k = int((val-min_val)*gap+0.5);
20131                         vector<int> gray = graycode[k];
20132                         bool changed = false;
20133                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20134                                 int r = rand()%10000;
20135                                 float f = r/10000.0f;
20136                                 if (f < mutation_rate) {
20137                                         *p = 1-*p;
20138                                         changed = true;
20139                                 }
20140                         }
20141                         if (changed) {
20142                                 k = rev_graycode[gray];
20143                                 list[i] = k/gap+min_val;
20144                         }
20145                 }
20146         }
20147 
20148 }
20149 
20150 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20151 
20152         if (is_mirror != 0) {
20153                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20154                         int r = rand()%10000;
20155                         float f = r/10000.0f;
20156                         if (f < mutation_rate) *q = 1-*q;
20157                 }
20158         } else {
20159                 map<int, vector<int> >  graycode;
20160                 map<vector<int>, int> rev_graycode;
20161                 vector <int> gray;
20162 
20163                 int K=1;
20164                 for (int i=0; i<L; i++) K*=2;
20165 
20166                 for (int k=0; k<K; k++) {
20167                         int shift = 0;
20168                         vector <int> gray;
20169                         for (int i=L-1; i>-1; i--) {
20170                                 int t = ((k>>i)%2-shift)%2;
20171                                 gray.push_back(t);
20172                                 shift += t-2;
20173                         }
20174                         graycode[k] = gray;
20175                         rev_graycode[gray] = k;
20176                 }
20177 
20178                 float gap = (K-1)/(max_val-min_val);
20179                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20180                         float val = *q;
20181                         if (val < min_val) { val = min_val; }
20182                         else if  (val > max_val) { val = max_val; }
20183                         int k = int((val-min_val)*gap+0.5);
20184                         vector<int> gray = graycode[k];
20185                         bool changed = false;
20186                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20187                                 int r = rand()%10000;
20188                                 float f = r/10000.0f;
20189                                 if (f < mutation_rate) {
20190                                         *p = 1-*p;
20191                                         changed = true;
20192                                 }
20193                         }
20194                         if (changed) {
20195                                 k = rev_graycode[gray];
20196                                 *q = k/gap+min_val;
20197                         }
20198                 }
20199         }
20200         return list;
20201 }
20202 
20203 
20204 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
20205         //cout<<"sanitycheck called\n";
20206         int total_cost = *output;
20207         int num_matches = *(output+1);
20208 
20209         int cost=0;
20210         int* intx;
20211         int intx_size;
20212         int* intx_next(0);
20213         int intx_next_size = 0;
20214         int curclass;
20215         int curclass_size;
20216         //cout<<"cost by match: [";
20217         for(int i = 0; i < num_matches; i++){
20218                 curclass = *(output+2+ i*nParts);
20219                 // check feasibility
20220                 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
20221                 *(argParts + Indices[curclass]+1) = -5;
20222                 // initialize intx to the number of elements in class curclass of partition 0 of the i-th match
20223                 curclass_size = *(dimClasses+curclass)-2;
20224                 intx = new int[curclass_size];
20225                 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
20226                 intx_size = curclass_size;
20227 
20228                 for (int j=1; j < nParts; j++){
20229                       curclass = *(output+2+ i*nParts+j);
20230                       if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
20231                       *(argParts + Indices[j*K+curclass]+1)=-5;
20232                       // compute the intersection of intx and class curclass of partition j of the i-th match
20233                       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);
20234                       intx_next = new int[intx_next_size];
20235                       Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
20236                       delete[] intx;
20237                       intx=intx_next;
20238                       intx_size= intx_next_size;
20239                       if (j==nParts-1) delete[] intx_next;
20240                 }
20241 
20242                 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
20243                 //cout <<intx_next_size<<",";
20244                 cost = cost + intx_next_size;
20245         }
20246         //cout<<"]\n";
20247         if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
20248 
20249         return 1;
20250 
20251 }
20252 
20253 
20254 // Given J, returns the J matches with the largest weight
20255 // matchlist has room for J matches
20256 // costlist has J elements to record cost of the J largest matches
20257 
20258 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int* costlist, int J){
20259         
20260         // some temp variables
20261         bool flag = 0;
20262         int nintx;
20263         int* dummy(0);
20264         //int* ret;
20265         int* curbranch = new int[nParts];
20266         
20267         //initialize costlist to all 0
20268         for(int jit= 0; jit< J; jit++) *(costlist+jit) = 0;
20269         
20270         
20271         for(int a=0; a<K; a++)
20272         {
20273         
20274                 // check that class a of partition 0 is active and has greater than T elements. If not the case, then skip to the next class
20275                 if (*(argParts + Indices[a] + 1) < 1) continue;
20276                 if (*(dimClasses + a)-2 <= T) continue;
20277 
20278                 // initial pruning: for each partition j>0, set the partition to inactive if its intersection with class a of partition 0 is less than new T
20279 
20280                 for( int i=1; i < nParts; i++){
20281                         flag = 0; // if flag stays 0 then no class in this partition has more than T objects in common with a, which implies no feasible match (> T) with class a of part 0 is possible.
20282                         for(int j=0; j < K; j++){
20283                                 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
20284                                 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);
20285                                 if (nintx > T) flag=1;
20286                                 else *(argParts + Indices[i*K+j] + 1) =-4;
20287                         }
20288                         if (flag==0) {break;}
20289                 }
20290 
20291                 // explore determines J matchs with the largest weight greater than T where class in partition 0 is class a
20292                 *curbranch = a;
20293 
20294                 if (flag > 0) // Each partition has one or more active class
20295                         Util::explore2(argParts, Indices, dimClasses, nParts, K, T, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2,
20296                         *(dimClasses+a)-2,0, J, matchlist, costlist, curbranch);
20297                         
20298                 // take all the classes marked as -4 and remark it as 1 in preparation for next round
20299                 for( int i=1; i < nParts; i++){
20300                         for(int j=0; j < K; j++){
20301                                 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
20302 
20303                         }
20304                 }
20305         }
20306         
20307         delete[] curbranch;
20308 }
20309 
20310 // returns J largest matches
20311 void Util::explore2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* curintx, int size_curintx, int* next, int size_next, int depth, int J, int* matchlist, int*costlist, int* curbranch){
20312 
20313 // depth is the level which is going to be explored in the current iteration
20314         int* curintx2(0);
20315         int nintx = size_curintx;
20316         
20317         
20318         // 2. take the intx of next and cur. Prune if <= T
20319         if (depth >0){
20320                 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
20321                 if (nintx <= T) return; //prune!
20322         }
20323 
20324         // 1. we're at a leaf with weight > T, so determine if there is any empty space. If so, put it in. If not, determine if current cost is larger than any of the cost in matchlist, if so, replace the  smallest one in matchlist
20325         if (depth == (nParts-1)) {
20326                 
20327                 int replace = 0;
20328                 int ind_smallest = -1;
20329                 int smallest_cost = -1;
20330                 
20331                 for (int jit = 0; jit < J; jit++){
20332                         if (*(costlist+jit) < nintx){
20333                                 replace = 1;
20334                                 if (ind_smallest == -1) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20335                                 if (*(costlist+jit) < smallest_cost) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20336                         }       
20337                 }
20338                 
20339                 if (replace > 0){
20340                         // replace the smallest cost in matchlist with the current stuff
20341                         *(costlist + ind_smallest) = nintx;
20342                         for (int xit = 0; xit < nParts; xit++)
20343                                 *(matchlist + ind_smallest*nParts + xit) = *(curbranch+xit);
20344                                 
20345                 }
20346                 
20347                 return; 
20348         }
20349         
20350 
20351         // 3. have not yet reached a leaf, and current weight is still greather than T, so keep on going.
20352 
20353         if (depth > 0){
20354                 curintx2 = new int[nintx]; // put the intersection set in here
20355                 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
20356         }
20357 
20358         if (depth == 0){
20359                 // set curintx2 to curintx
20360                 curintx2 = new int[size_curintx];
20361                 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
20362         }
20363 
20364 
20365         // recursion (non-leaf case)
20366         depth=depth+1;
20367         // we now consider each of the classes in partition depth and recurse upon each of them
20368         for (int i=0; i < K; i++){
20369 
20370                 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue; // class is not active so move on
20371                 size_next = (*(dimClasses + depth*K+i ))-2;
20372                 if (size_next <= T) continue;
20373                 *(curbranch+depth) = i;
20374                 Util::explore2(argParts,Indices, dimClasses, nParts, K, T, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth,J, matchlist,
20375                         costlist, curbranch);
20376                 
20377         }
20378 
20379         delete[] curintx2;
20380 }
20381 
20382 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
20383         //cout<<"initial_prune\n";
20384         // simple initial pruning. For class indClass of partition indPart:
20385         // For each class of partition which is not indPart, see if there is a class in the partition with which indClass has intersection greater than T
20386         // If for some partition (not equal to indPart) for which there is no such class, then return 0 immediately.
20387 
20388         // 1. For each class of partition which is not indPart, remove the class from Parts if its intx with indClass of indPart is not gt T
20389 
20390         // remember when calling k_means_cont_table, the first element of each class is an index, and actual number of elements in the class
20391         // (as stored in dimClasses) should be decremented by 1 accordingly, and second is dummy variable
20392 
20393         int* dummy(0);
20394         int* cref;
20395         int cref_size;
20396         int* ccomp;
20397         int ccomp_size;
20398         int nintx;
20399         for (int i=0; i < nParts; i++){
20400                 for (int j =0; j < K; j++){
20401 
20402                         // consider class Parts[i][j]
20403                         cref = Parts[i][j];//incr by 1 since first element is index and second is dummy
20404                         cref_size = (*(dimClasses+i*K+(*cref)))-2;
20405 
20406 
20407                         if (cref_size <= T){
20408 
20409                                 *cref = -1;
20410                                 continue;
20411                         }
20412                         bool done = 0;
20413                         for (int a = 0; a < nParts; a++){
20414                                 if (a == i) continue; //consider all classes not in partition i and set to inactive all those classes whose intx with cref is not gt T
20415                                 bool hasActive=0;
20416                                 for (unsigned int b=0; b < Parts[a].size(); b++){
20417                                         // get the card of the intx between Parts[i][j] and Parts[a][b] using k_means_cont_table
20418                                         // remember first element of each class is the index of the class
20419                                         ccomp = Parts[a][b];
20420                                         ccomp_size= (*(dimClasses+a*K+(*ccomp)))-2;
20421                                         nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
20422 
20423 
20424                                         if (nintx <= T)
20425                                                 *(ccomp+1) = 0; // class Parts[a][b] is 'inactive' for cref
20426                                         else{
20427                                                 *(ccomp+1)=1; // class Parts[a][b] is 'active' for cref
20428                                                 hasActive=1;
20429                                         }
20430                                 }
20431                                 // see if partition a has at least one active class.if not then we're done with cref
20432                                 if (hasActive < 1){
20433                                    done=1;
20434                                    break;
20435                                 }
20436 
20437                         }
20438 
20439                         if (done > 0){
20440                                 // remove class j from partition i
20441 
20442                                 *cref = -1; // mark for deletion later
20443                                 continue; // move on to class Parts[i][j+1]
20444                         }
20445 
20446                         // now we adopt more aggressive measures. we find the match with the largest weight which has class Parts[i][j] for partition i.
20447                         // We get rid of Parts[i][j] if this weight is not gt T as no other feasible match containing class Parts[i][j] can be gt T.
20448 
20449                         // (To implement later:) To reduce complexity, determine the order the partitions are to be explored based on the cardinality of the active classes of each partition.
20450                         // Note that if there are ALOT of partitions, then sorting the partititions may actually take longer than the time saved by ordering hte
20451                         // partitions. However, we do not have a lot of partitions, (usually around 10), so sorting time will be trivial compared to exploring time.
20452 
20453                         // explore returns one if there is a feasible matching containing class cref which has weight gt T, and otherwise returns 0
20454                         //bool found = 1;
20455                         bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
20456 
20457                         if (found<1){ // There is NO feasible matching with class j (cref)  with weight greater than T, so delete this class from Parts
20458                                 // Parts[i].erase(Parts[i].begin()+j);
20459                                 *cref = -1;
20460                         }
20461                 }
20462 
20463                 // Erase from Parts[i] all the classes that's being designated for erasure
20464 
20465                 for (int d = K-1; d > -1; d--){
20466                         if (*(Parts[i][d]) < 0) Parts[i].erase(Parts[i].begin()+d);
20467                 }
20468 
20469         }
20470         //cout <<"number of classes left in each partition after initial prune\n";      
20471         // Print out how many classes are left in each partition
20472         //for (int i =0; i < nParts; i++)
20473         //      cout << Parts[i].size()<<", ";
20474         //cout << "\n";
20475 }
20476 
20477 
20478 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) {
20479 
20480 
20481         if (size_next <= T) return 0;
20482 
20483         // take the intx of next and cur
20484         int* curintx2(0);
20485         int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
20486         if (nintx <= T) return 0;
20487 
20488         int old_depth=depth;
20489         if (depth == partref) depth = depth + 1; // we skip classes in partref
20490         if (depth == (nParts)) { if (old_depth>0) return 1;}
20491 
20492         // have not yet reached a leaf, and current weight is still greather than T, so keep on going.
20493 
20494         curintx2 = new int[nintx]; // put the intersection set in here
20495         Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
20496 
20497         // if (old_depth > 0) {delete[] curintx;} // don't delete when depth == 0 because curintx in that case is a pointer into Parts
20498 
20499         // we now consider each of the classes in partition (depth+1) in turn
20500         bool gt_thresh;
20501         int num_classes = (Parts[depth]).size(); // (TO DO) have to figure out how many classes partition (depth) has since some may have being removed from before iterations
20502 
20503         for (int i=0; i < num_classes; i++){
20504                 if (*(Parts[depth][i]+1) < 1) continue; // class is not active so move on
20505                 size_next = (*(dimClasses + depth*K+(*(Parts[depth][i])) ))-2;
20506                 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
20507                 if (gt_thresh) return 1;
20508         }
20509         delete[] curintx2;
20510         return 0;
20511 }
20512 
20513 
20514 
20515 
20516 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int n_guesses, int LARGEST_CLASS, int J,
20517 int max_branching, float stmult, int branchfunc, int LIM) {
20518 
20519         
20520         // Indices is an nParts*K int array storing the index (into argparts) of the first element of the i-th class of the j-th partition
20521         // So Indices[j*K + i] is the offset from argparts of the first element of the first element  of the i-th class of the j-th partition
20522         // Make a vector of nParts vectors of K int* each
20523          int* Indices = new int[nParts*K];
20524          int ind_c = 0;
20525          for (int i=0; i < nParts; i++){
20526                  for(int j = 0; j < K; j++){
20527                          Indices[i*K + j] = ind_c;
20528                          ind_c = ind_c + *(dimClasses+i*K + j);
20529 
20530                  }
20531          }
20532 
20533         // do initial pruning on argParts and return the pruned partitions
20534 
20535         // Make a vector of nParts vectors of K int* each
20536         vector <vector <int*> > Parts(nParts,vector<int*>(K));
20537         ind_c = 0;
20538         int argParts_size=0;
20539         for (int i=0; i < nParts; i++){
20540                 for(int j = 0; j < K; j++){
20541                         Parts[i][j]=argParts + ind_c;
20542                         ind_c = ind_c + *(dimClasses+i*K + j);
20543                         argParts_size = argParts_size + *(dimClasses+i*K + j);
20544 
20545                 }
20546         }
20547 
20548         // in the following we call initial_prune with Parts which is a vector. This is not the most
20549         // efficient since vector does not allow for direct addressing. But since initial_prune doesn't have very high complexity, and
20550         // the running time for 7 partitions with 288 classes per partition is a couple of minutes at most, i'll just leave it for now.....
20551 
20552         // comment out for testing
20553         Util::initial_prune(Parts, dimClasses, nParts, K,T);
20554         for(int i = 0; i < nParts; i++){
20555                 for(int j=0; j < K; j++){
20556                         *(argParts + Indices[i*K + j]+1) = -1;
20557                 }
20558         }
20559 
20560         int num_classes;
20561         int old_index;
20562         for(int i=0; i<nParts; i++){
20563                 num_classes = Parts[i].size();// number of classes in partition i after pruning
20564                 for (int j=0; j < num_classes; j++){
20565                         old_index = *(Parts[i][j]);
20566                         //cout << "old_index: " << old_index<<"\n";
20567                         *(argParts + Indices[i*K + old_index]+1) = 1;
20568                 }
20569         }
20570 
20571 
20572         // if we're not doing mpi then keep going and call branchMPI and return the output
20573         //cout <<"begin partition matching\n";
20574         //int* dummy(0);
20575         int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T,0,n_guesses,LARGEST_CLASS, J, max_branching, stmult,
20576         branchfunc, LIM);
20577         
20578         //cout<<"total cost: "<<*output<<"\n";
20579         //cout<<"number of matches: "<<*(output+1)<<"\n";
20580         // now go check if the matches are sensical! i.e, if the matches are feasible, if the sum of the match weights in output is equal to *output, and if each match in output has weight at least T
20581         bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
20582 
20583         // something is wrong with output of branchMPI!
20584         if (correct < 1){
20585                 cout << "something is wrong with output of branchMPI!\n";
20586                 vector<int> ret(1);
20587                 ret[0]=-1;
20588                 return ret;
20589         }
20590 
20591         // output is not nonsense, so now put it into a single dimension vector and return
20592         // output is an int array, the first element is the cost of the output solution, the second element is the total number of matches in the solution
20593         // and the rest is the list of matches. output is one dimensional
20594 
20595         int output_size = 2+ *(output+1) * nParts;
20596         vector<int> ret(output_size);
20597         for (int i = 0; i < output_size; i++){
20598                 ret[i]=*(output+i);
20599         }
20600         return ret;
20601 
20602 }
20603 
20604 
20605 int branch_all=0;
20606 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int curlevel,int n_guesses, int
20607 LARGEST_CLASS, int J, int max_branching, float stmult, int branchfunc, int LIM) {
20608 
20609 //*************************************
20610 //testing search2
20611 if (1 == 0){
20612 cout <<"begin test search2\n";
20613 int* matchlist = new int[J*nParts];
20614 int* costlist = new int[J];
20615 for (int jit = 0; jit < nParts; jit++) *(costlist+jit) = 0;
20616 Util::search2(argParts,Indices, dimClasses, nParts, K,  T, matchlist,costlist,J);
20617 
20618 for (int jit = 0; jit < J; jit++) {
20619   cout << *(costlist +jit)<<": ";
20620   for (int yit = 0; yit < nParts; yit++)
20621         cout << *(matchlist + jit*nParts + yit)<<",";
20622   cout <<"\n";  
20623 
20624 }
20625 cout <<"end test search2\n";
20626 int* ret = new int[1];
20627 *ret=1;
20628 delete [] matchlist;
20629 delete [] costlist;
20630 return ret;
20631 }
20632 //**************************************
20633 
20634         // Base Case: we're at a leaf, no more feasible matches possible
20635         if (curlevel > K -1){
20636                 int* res = new int[2];
20637                 *res = 0;
20638                 *(res+1)=0;
20639                 return res;
20640         }
20641 
20642         // branch dynamically depending on results of search 2!
20643         
20644         int* matchlist = new int[J*nParts];
20645         int* costlist = new int[J];
20646         Util::search2(argParts,Indices, dimClasses, nParts, K,  T, matchlist,costlist,J);
20647         
20648         
20649         // each class in the matches found by findTopLargest is encoded by the original index of the first element of the class in argPart
20650         // each match contains nParts classes, with the i-th class belonging to the i-th partition.
20651 
20652         // if there are no feasible matches with cost gt T, then return 0
20653         for (int jit = 0; jit < J ; jit++){
20654         
20655                 if (costlist[jit] > T) break;
20656                 if (jit == J-1){
20657                         int* res = new int[2];
20658                         *res = 0;
20659                         *(res+1)=0;
20660                         return res;
20661                 }
20662         }
20663         
20664 
20665         
20666         // note that costlist and matchlist are NOT sorted by weight, and branch factor takes care of that...
20667         if (curlevel==0) branch_all = 0;
20668         int* newcostlist;
20669         int* newmatchlist;
20670         
20671         int nBranches = -1;
20672         
20673         if (branchfunc == 2)
20674                 nBranches = branch_factor_2(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM); // branch based on distribution of top J (weighted) matches  with cost > T
20675 
20676         if (branchfunc == 3)
20677                 nBranches = branch_factor_3(costlist,matchlist,J, T, nParts, curlevel, max_branching, K, LIM); // branch based on distribution of top J (weighted) matches  with cost > T
20678 
20679         if (branchfunc == 4)
20680                 nBranches = branch_factor_4(costlist,matchlist,J, T, nParts, curlevel, max_branching, stmult); // branch based on distribution of top J (weighted) matches  with cost > T
20681 
20682         newcostlist= new int[nBranches];
20683         newmatchlist = new int[nBranches*nParts];
20684         for (int i=0; i<nBranches; i++){
20685                 *(newcostlist + i) = *(costlist+i);
20686                 for (int j=0; j< nParts; j++)
20687                         *(newmatchlist + i*nParts + j) = *(matchlist + i*nParts+j);
20688         }
20689 
20690         delete[] costlist;
20691         delete[] matchlist;
20692         
20693         int* maxreturn = new int[2];//initialize to placeholder
20694         *maxreturn=0;
20695         *(maxreturn+1)=0;
20696         // some temporary variables
20697         int old_index;
20698         int totalcost;
20699         int nmatches;
20700         //int offset;
20701 
20702         for(int i=0; i < nBranches ; i++){
20703 
20704                 // consider the i-th match returned by findTopLargest
20705                 //if (newcostlist[i] <= T) continue;
20706 
20707                 // mark the classes in the i-th match of matchlist as taken (using the dummy variable and -2), and then call branch again on argParts.
20708                 // After branch returns, compute overall cost, unmark  the classes just marked as 1 again in preparation for next loop.
20709 
20710                 for(int j=0; j < nParts; j++){
20711                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
20712                         old_index=newmatchlist[i*nParts + j];
20713                         *(argParts + Indices[j*K+old_index] + 1) = -2;
20714                 }
20715 
20716                 
20717                 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T,curlevel+1,n_guesses, LARGEST_CLASS,
20718                 J,max_branching, stmult,branchfunc, LIM);
20719                 
20720                 // first element of ret is the total cost of all the matches in ret, and second element is the number of matches in ret
20721                 totalcost = newcostlist[i] + *ret;
20722 
20723                 //if (curlevel == 0) {
20724                 //      cout <<"totalcost*****************************************************************: "<<totalcost<<", costlist["<<i<<"]="<<newcostlist[i]<<", *ret="<<*ret<<", level: "<<curlevel<<"\n";
20725                         
20726                 //}
20727                 if (totalcost > *maxreturn) // option 1
20728                 {
20729                         nmatches = 1 + *(ret+1);
20730                         delete[] maxreturn; // get rid of the old maxreturn
20731                         maxreturn = new int[2+nmatches*nParts];
20732                         *maxreturn = totalcost;
20733                         
20734                         *(maxreturn + 1)= nmatches;
20735                         int nret = 2+(nmatches-1)*nParts;
20736                         for(int iret=2; iret <nret;iret++) *(maxreturn+iret)=*(ret+iret);
20737                         for(int imax=0; imax<nParts;imax++) *(maxreturn+nret+imax)=newmatchlist[i*nParts + imax];
20738                 }
20739 
20740 
20741                 delete[] ret;
20742 
20743                 // unmark the marked classes in preparation for the next iteration
20744 
20745                 for(int j=0; j < nParts; j++){
20746                         old_index=newmatchlist[i*nParts + j];
20747                         *(argParts + Indices[j*K+old_index] + 1) = 1;
20748                 }
20749 
20750         }
20751 
20752         delete[] newmatchlist;
20753         delete[] newcostlist;
20754         
20755         return maxreturn;
20756 
20757 }
20758 
20759 int* costlist_global;
20760 
20761 // make global costlist
20762 bool jiafunc(int i, int j){
20763         return (costlist_global[j] < costlist_global[i]) ;
20764 
20765 }
20766 
20767 // Given J matches, branch always on the first one (i.e., the one with the largest weight, so the worst case we just end up doing greedy).
20768 // Branch on the second one only if it is INFEASIBLE with the first, so you know it will never occur in any branching beginning with the first.
20769 // Branch on subsequent ones only if its infeasible with ALL the ones which we have previously decided to branch on.
20770 // The other option is to use LIM - so we branch on a match if its infeasible with at least LIM matches which we have previously decoded to branch on.
20771 // For now, LIM is defaulted to -1, which means we branch on a match only if it is infeasible to ALL matches we have previously decided to branch on.
20772 int Util::branch_factor_2(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int LIM){
20773         
20774         int ntot=0;
20775         for (int jit=0; jit < J; jit++){
20776                 if (*(costlist+jit) > T) ntot++;
20777         }
20778 
20779         int cur;
20780         // sort matchlist by cost
20781         int* indx = new int[J];
20782         for (int jit=0; jit < J; jit++) indx[jit]=jit;
20783         vector<int> myindx (indx, indx+J);
20784         vector<int>::iterator it;
20785         costlist_global=costlist;
20786         sort(myindx.begin(), myindx.begin()+J, jiafunc);
20787 
20788         // put matchlist in the order of mycost
20789         int* templist = new int[J];
20790         int* temp2list = new int[J*nParts];
20791         int next = 0;
20792         
20793         for (it=myindx.begin(); it!=myindx.end();++it){
20794                 cur = *(costlist + *it);
20795                 if (cur > T){
20796                         
20797                         templist[next] = cur;
20798                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
20799                         next = next + 1;
20800                 }
20801         }
20802         
20803         for (int jit=0; jit < ntot; jit++){
20804                 *(costlist+jit)=*(templist + jit);
20805                 //cout <<*(costlist+jit)<<", ";
20806                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
20807         }
20808         //cout <<"\n";
20809         
20810         delete [] indx;
20811         //compute the average 
20812         
20813         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
20814         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
20815         
20816         
20817         int B=1;
20818         int B_init=B;
20819         int infeasible=0;
20820         
20821         for (int i=B_init; i<ntot; i++){
20822                 if (i==ntot) continue;
20823                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
20824                 // branch on
20825                 infeasible = 0;
20826                 if (LIM < 0) LIM = B;
20827                 for (int j=0; j<B; j++){
20828                         
20829                         for (int vit=0; vit<nParts; vit++){
20830                                 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
20831                         }
20832                         if (infeasible >= LIM) break;
20833                 }
20834                 
20835                 if (infeasible >= LIM){
20836                         *(costlist+B)=*(templist+i);
20837                         for (int vit=0; vit < nParts; vit++)
20838                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
20839                         B=B+1;  
20840                 }
20841         }
20842         
20843         delete [] templist;
20844         delete [] temp2list;
20845         //cout<<"**************************************** "<<B<<" ***************************\n";
20846         
20847         if (branch_all < max_branching){
20848                 if (B>1)
20849                         {branch_all = branch_all + B -1 ; }
20850         }
20851         else B=1;
20852         
20853         return B;
20854         
20855 
20856 }
20857 
20858 
20859 // similar to branch_factor_2 except we branch on a match if it is infeasible with all other matches in matchlist (not just the ones we branch on). LIM plays similar role here.
20860 int Util::branch_factor_3(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, int, int LIM){
20861         
20862         int ntot=0;
20863         for (int jit=0; jit < J; jit++){
20864                 if (*(costlist+jit) > T) ntot++;
20865         }
20866 
20867         int cur;
20868         // sort matchlist by cost
20869         int* indx = new int[J];
20870         for (int jit=0; jit < J; jit++) indx[jit]=jit;
20871         vector<int> myindx (indx, indx+J);
20872         vector<int>::iterator it;
20873         costlist_global=costlist;
20874         sort(myindx.begin(), myindx.begin()+J, jiafunc);
20875 
20876         // put matchlist in the order of mycost
20877         int* templist = new int[J];
20878         int* temp2list = new int[J*nParts];
20879         int next = 0;
20880         
20881         for (it=myindx.begin(); it!=myindx.end();++it){
20882                 cur = *(costlist + *it);
20883                 if (cur > T){
20884                         
20885                         templist[next] = cur;
20886                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
20887                         next = next + 1;
20888                 }
20889         }
20890         
20891         for (int jit=0; jit < ntot; jit++){
20892                 *(costlist+jit)=*(templist + jit);
20893                 //cout <<*(costlist+jit)<<", ";
20894                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
20895         }
20896         //cout <<"\n";
20897         
20898         delete [] indx;
20899         //compute the average 
20900         
20901         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
20902         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
20903         
20904         
20905         int B=1;
20906         int B_init=B;
20907         int infeasible=0;
20908         // if we're near the bottom of the tree then explore more... this is because the larger weights are not likely to change much,
20909         // whereas the smaller ones can have many permutations
20910         if (LIM < 0) LIM = ntot-1;
20911         for (int i=B_init; i<ntot; i++){
20912                 if (i==ntot) continue;
20913                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
20914                 // branch on
20915                 infeasible = 0;
20916                 
20917                 for (int j=0; j<ntot; j++){
20918                         if (j == i) continue;
20919                         for (int vit=0; vit<nParts; vit++){
20920                                 if (temp2list[i*nParts+vit] == temp2list[j*nParts+vit]) {infeasible++; break;}
20921                         }
20922                         if (infeasible >= LIM) break;
20923                 }
20924                 
20925                 if (infeasible >= LIM){
20926                         *(costlist+B)=*(templist+i);
20927                         for (int vit=0; vit < nParts; vit++)
20928                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
20929                         B=B+1;  
20930                 }
20931         }
20932         
20933         delete [] templist;
20934         delete [] temp2list;
20935         //cout<<"**************************************** "<<B<<" ***************************\n";
20936         
20937         
20938         if (branch_all < max_branching){
20939                 if (B>1)
20940                         {branch_all = branch_all + B-1;}
20941         }
20942         else B=1;
20943         
20944         return B;
20945         
20946 
20947 }
20948 
20949 // We branch based on distribution of the cost of the J largest matches. Roughly speaking, if there is a match which has significantly larger weight than others, then we branch just on that
20950 // match. Otherwise, we branch on similar weighted matches.
20951 // As before we always branch on the match with the largest cost so worst case we'll get greedy.
20952 // We compute standard dev of the J costs, and then if the difference between the cost of a match and the largest cost is within stmult*standard dev, then we branch on it.
20953 int Util::branch_factor_4(int* costlist, int* matchlist, int J, int T, int nParts, int, int max_branching, float stmult){
20954         int sum=0;
20955         float average =0;
20956         int ntot=0;
20957         for (int jit=0; jit < J; jit++){
20958                 if (*(costlist+jit) > T) {ntot++; sum = sum +*(costlist+jit);}
20959         }
20960         average = ((float)sum)/((float)ntot);
20961         int cur;
20962         // sort matchlist by cost
20963         int* indx = new int[J];
20964         for (int jit=0; jit < J; jit++) indx[jit]=jit;
20965         vector<int> myindx (indx, indx+J);
20966         vector<int>::iterator it;
20967         costlist_global=costlist;
20968         sort(myindx.begin(), myindx.begin()+J, jiafunc);
20969 
20970         // put matchlist in the order of mycost
20971         int* templist = new int[J];
20972         int* temp2list = new int[J*nParts];
20973         int next = 0;
20974         
20975         for (it=myindx.begin(); it!=myindx.end();++it){
20976                 cur = *(costlist + *it);
20977                 if (cur > T){
20978                         
20979                         templist[next] = cur;
20980                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
20981                         next = next + 1;
20982                 }
20983         }
20984         
20985         for (int jit=0; jit < ntot; jit++){
20986                 *(costlist+jit)=*(templist + jit);
20987                 //cout <<*(costlist+jit)<<", ";
20988                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
20989         }
20990         //cout <<"\n";
20991         
20992         delete [] indx;
20993         delete [] templist;
20994         delete [] temp2list;
20995         
20996         if (ntot == 1) return 1;
20997         
20998         // look at the average, standard dev etc. If standard dev very small, i.e., costs very similar, then branch on the similar
20999         // costs
21000         float sq_sum=0.0;
21001         //cout <<"costlist:";
21002         for (int i=0; i< ntot; i++){
21003                 sq_sum = sq_sum + (float) pow((float) *(costlist+i) - average, (float)2.0);
21004                 //cout <<*(costlist+i)<<", ";
21005         }       
21006         //cout <<"\n";
21007         
21008         float variance = sq_sum/ntot;
21009         float stdev = (float)pow((float)variance,(float)0.5);
21010         
21011         //cout <<"stdev: "<<int(stdev)<<"\n";
21012         
21013         int B=1;
21014         int largest = *costlist;
21015         //cout <<"largest: "<<largest<<"\n";
21016         for (int i=1; i<ntot; i++){
21017                 int cur = *(costlist+i);
21018                 if (largest-cur < (float)(stdev*stmult)) B++;
21019                 else break;
21020         
21021         }
21022         //cout <<"B: "<<B<<"\n";
21023         if (branch_all < max_branching){
21024                 if (B>1)
21025                         {branch_all = branch_all + B-1;}
21026         }
21027         else B=1;
21028         
21029         return B;
21030         
21031 
21032 }

Generated on Thu Mar 10 22:58:31 2011 for EMAN2 by  doxygen 1.3.9.1