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 < nx*ny*nz; i++) {
00115               if (maskptr[i]>0.5f == flip) {
00116                 Sum1 += Volptr[i];
00117                 Sum2 += Volptr[i]*Volptr[i];
00118                 MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00119                 MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00120                 count++;
00121               }
00122         }
00123 
00124         if (count == 0) {
00125                 LOGERR("Invalid mask");
00126                 throw ImageFormatException( "Invalid mask");
00127         }
00128 
00129        float avg = static_cast<float>(Sum1/count);
00130        float sig2 = static_cast<float>(Sum2 - count*avg*avg)/(count-1);
00131        float sig = sqrt(sig2);
00132 
00133        stats.push_back(avg);
00134        stats.push_back(sig);
00135        stats.push_back(MIN);
00136        stats.push_back(MAX);
00137 
00138        return stats;
00139 }
00140 
00141 
00142 //----------------------------------------------------------------------------------------------------------
00143 
00144 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00145 {
00146         ENTERFUNC;
00147 
00148         if (!EMUtil::is_same_size(V1, V2)) {
00149                 LOGERR("images not same size");
00150                 throw ImageFormatException( "images not same size");
00151         }
00152 
00153         size_t nx = V1->get_xsize();
00154         size_t ny = V1->get_ysize();
00155         size_t nz = V1->get_zsize();
00156         size_t size = nx*ny*nz;
00157 
00158         EMData *BD = new EMData();
00159         BD->set_size(nx, ny, nz);
00160 
00161         float *params = new float[2];
00162 
00163         float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00164         long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00165         int nvox = 0L;
00166 
00167         V1ptr = V1->get_data();
00168         V2ptr = V2->get_data();
00169         BDptr = BD->get_data();
00170 
00171 
00172         if(!mask){
00173                 EMData * Mask = new EMData();
00174                 Mask->set_size(nx,ny,nz);
00175                 Mask->to_one();
00176                 MASKptr = Mask->get_data();
00177         } else {
00178                 if (!EMUtil::is_same_size(V1, mask)) {
00179                         LOGERR("mask not same size");
00180                         throw ImageFormatException( "mask not same size");
00181                 }
00182 
00183                 MASKptr = mask->get_data();
00184         }
00185 
00186 
00187 
00188 //       calculation of S1,S2,S3,S3,nvox
00189 
00190         for (size_t i = 0L;i < size; i++) {
00191               if (MASKptr[i]>0.5f) {
00192                S1 += V1ptr[i]*V2ptr[i];
00193                S2 += V1ptr[i]*V1ptr[i];
00194                S3 += V2ptr[i];
00195                S4 += V1ptr[i];
00196                nvox ++;
00197               }
00198         }
00199 
00200         if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00201                 A =1.0f ;
00202         } else {
00203                 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00204         }
00205         B = static_cast<float> (A*S4  -  S3)/nvox;
00206 
00207         // calculation of the difference image
00208 
00209         for (size_t i = 0L;i < size; i++) {
00210              if (MASKptr[i]>0.5f) {
00211                BDptr[i] = A*V1ptr[i] -  B  - V2ptr[i];
00212              }  else  {
00213                BDptr[i] = 0.f;
00214              }
00215         }
00216 
00217         BD->update();
00218 
00219         params[0] = A;
00220         params[1] = B;
00221 
00222         Dict BDnParams;
00223         BDnParams["imdiff"] = BD;
00224         BDnParams["A"] = params[0];
00225         BDnParams["B"] = params[1];
00226 
00227         EXITFUNC;
00228         return BDnParams;
00229  }
00230 
00231 //----------------------------------------------------------------------------------------------------------
00232 
00233 
00234 
00235 EMData *Util::TwoDTestFunc(int Size, float p, float q,  float a, float b, int flag, float alphaDeg) //PRB
00236 {
00237         ENTERFUNC;
00238         int Mid= (Size+1)/2;
00239 
00240         if (flag==0) { // This is the real function
00241                 EMData* ImBW = new EMData();
00242                 ImBW->set_size(Size,Size,1);
00243                 ImBW->to_zero();
00244 
00245                 float tempIm;
00246                 float x,y;
00247 
00248                 for (int ix=(1-Mid);  ix<Mid; ix++){
00249                         for (int iy=(1-Mid);  iy<Mid; iy++){
00250                                 x = (float)ix;
00251                                 y = (float)iy;
00252                         tempIm= static_cast<float>( (1/(2*M_PI)) * cos(p*x)* cos(q*y) * exp(-.5*x*x/(a*a))* exp(-.5*y*y/(b*b)) );
00253                                 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00254                         }
00255                 }
00256                 ImBW->update();
00257                 ImBW->set_complex(false);
00258                 ImBW->set_ri(true);
00259 
00260                 return ImBW;
00261         }
00262         else if (flag==1) {  // This is the Fourier Transform
00263                 EMData* ImBWFFT = new EMData();
00264                 ImBWFFT ->set_size(2*Size,Size,1);
00265                 ImBWFFT ->to_zero();
00266 
00267                 float r,s;
00268 
00269                 for (int ir=(1-Mid);  ir<Mid; ir++){
00270                         for (int is=(1-Mid);  is<Mid; is++){
00271                                 r = (float)ir;
00272                                 s = (float)is;
00273                         (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00274                                 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00275                         }
00276                 }
00277                 ImBWFFT->update();
00278                 ImBWFFT->set_complex(true);
00279                 ImBWFFT->set_ri(true);
00280                 ImBWFFT->set_shuffled(true);
00281                 ImBWFFT->set_fftodd(true);
00282 
00283                 return ImBWFFT;
00284         }
00285         else if (flag==2 || flag==3) { //   This is the projection in Real Space
00286                 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00287                 float C=cos(alpha);
00288                 float S=sin(alpha);
00289                 float D= sqrt(S*S*b*b + C*C*a*a);
00290                 //float D2 = D*D;   PAP - to get rid of warning
00291 
00292                 float P = p * C *a*a/D ;
00293                 float Q = q * S *b*b/D ;
00294 
00295                 if (flag==2) {
00296                         EMData* pofalpha = new EMData();
00297                         pofalpha ->set_size(Size,1,1);
00298                         pofalpha ->to_zero();
00299 
00300                         float Norm0 =  D*(float)sqrt(2*pi);
00301                         float Norm1 =  exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00302                         float Norm2 =  exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00303                         float sD;
00304 
00305                         for (int is=(1-Mid);  is<Mid; is++){
00306                                 sD = is/D ;
00307                                 (*pofalpha)(is+Mid-1) =  Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00308                          + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00309                         }
00310                         pofalpha-> update();
00311                         pofalpha-> set_complex(false);
00312                         pofalpha-> set_ri(true);
00313 
00314                         return pofalpha;
00315                 }
00316                 if (flag==3) { // This is the projection in Fourier Space
00317                         float vD;
00318 
00319                         EMData* pofalphak = new EMData();
00320                         pofalphak ->set_size(2*Size,1,1);
00321                         pofalphak ->to_zero();
00322 
00323                         for (int iv=(1-Mid);  iv<Mid; iv++){
00324                                 vD = iv*D ;
00325                                 (*pofalphak)(2*(iv+Mid-1)) =  exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00326                         }
00327                         pofalphak-> update();
00328                         pofalphak-> set_complex(false);
00329                         pofalphak-> set_ri(true);
00330 
00331                         return pofalphak;
00332                 }
00333         }
00334         else if (flag==4) {
00335                 cout <<" FH under construction";
00336                 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00337                 EMData* TryFH= OutFT -> real2FH(4.0);
00338                 return TryFH;
00339         } else {
00340                 cout <<" flag must be 0,1,2,3, or 4";
00341         }
00342 
00343         EXITFUNC;
00344         return 0;
00345 }
00346 
00347 
00348 void Util::spline_mat(float *x, float *y, int n,  float *xq, float *yq, int m) //PRB
00349 {
00350 
00351         float x0= x[0];
00352         float x1= x[1];
00353         float x2= x[2];
00354         float y0= y[0];
00355         float y1= y[1];
00356         float y2= y[2];
00357         float yp1 =  (y1-y0)/(x1-x0) +  (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1)  ;
00358         float xn  = x[n];
00359         float xnm1= x[n-1];
00360         float xnm2= x[n-2];
00361         float yn  = y[n];
00362         float ynm1= y[n-1];
00363         float ynm2= y[n-2];
00364         float ypn=  (yn-ynm1)/(xn-xnm1) +  (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00365         float *y2d = new float[n];
00366         Util::spline(x,y,n,yp1,ypn,y2d);
00367         Util::splint(x,y,y2d,n,xq,yq,m); //PRB
00368         delete [] y2d;
00369         return;
00370 }
00371 
00372 
00373 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2) //PRB
00374 {
00375         int i,k;
00376         float p, qn, sig, un, *u;
00377         u = new float[n-1];
00378 
00379         if (yp1 > .99e30){
00380                 y2[0]=u[0]=0.0;
00381         } else {
00382                 y2[0]=-.5f;
00383                 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00384         }
00385 
00386         for (i=1; i < n-1; i++) {
00387                 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00388                 p = sig*y2[i-1] + 2.0f;
00389                 y2[i]  = (sig-1.0f)/p;
00390                 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) -  (y[i] - y[i-1] )/(x[i] -x[i-1]);
00391                 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00392         }
00393 
00394         if (ypn>.99e30){
00395                 qn=0; un=0;
00396         } else {
00397                 qn= .5f;
00398                 un= (3.0f/(x[n-1] -x[n-2])) * (ypn -  (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00399         }
00400         y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00401         for (k=n-2; k>=0; k--){
00402                 y2[k]=y2[k]*y2[k+1]+u[k];
00403         }
00404         delete [] u;
00405 }
00406 
00407 
00408 void Util::splint( float *xa, float *ya, float *y2a, int n,  float *xq, float *yq, int m) //PRB
00409 {
00410         int klo, khi, k;
00411         float h, b, a;
00412 
00413 //      klo=0; // can try to put here
00414         for (int j=0; j<m;j++){
00415                 klo=0;
00416                 khi=n-1;
00417                 while (khi-klo >1) {
00418                         k=(khi+klo) >>1;
00419                         if  (xa[k]>xq[j]){ khi=k;}
00420                         else { klo=k;}
00421                 }
00422                 h=xa[khi]- xa[klo];
00423                 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00424                 a =(xa[khi]-xq[j])/h;
00425                 b=(xq[j]-xa[klo])/h;
00426                 yq[j]=a*ya[klo] + b*ya[khi]
00427                         + ((a*a*a-a)*y2a[klo]
00428                              +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00429         }
00430 //      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]);
00431 }
00432 
00433 
00434 void Util::Radialize(int *PermMatTr, float *kValsSorted,   // PRB
00435                float *weightofkValsSorted, int Size, int *SizeReturned)
00436 {
00437         int iMax = (int) floor( (Size-1.0)/2 +.01);
00438         int CountMax = (iMax+2)*(iMax+1)/2;
00439         int Count=-1;
00440         float *kVals     = new float[CountMax];
00441         float *weightMat = new float[CountMax];
00442         int *PermMat     = new   int[CountMax];
00443         SizeReturned[0] = CountMax;
00444 
00445 //      printf("Aa \n");        fflush(stdout);
00446         for (int jkx=0; jkx< iMax+1; jkx++) {
00447                 for (int jky=0; jky< jkx+1; jky++) {
00448                         Count++;
00449                         kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00450                         weightMat[Count]=  1.0;
00451                         if (jkx!=0)  { weightMat[Count] *=2;}
00452                         if (jky!=0)  { weightMat[Count] *=2;}
00453                         if (jkx!=jky){ weightMat[Count] *=2;}
00454                         PermMat[Count]=Count+1;
00455                 }
00456         }
00457 
00458         int lkVals = Count+1;
00459 //      printf("Cc \n");fflush(stdout);
00460 
00461         sort_mat(&kVals[0],&kVals[Count],
00462              &PermMat[0],  &PermMat[Count]);  //PermMat is
00463                                 //also returned as well as kValsSorted
00464         fflush(stdout);
00465 
00466         int newInd;
00467 
00468         for (int iP=0; iP < lkVals ; iP++ ) {
00469                 newInd =  PermMat[iP];
00470                 PermMatTr[newInd-1] = iP+1;
00471         }
00472 
00473 //      printf("Ee \n"); fflush(stdout);
00474 
00475         int CountA=-1;
00476         int CountB=-1;
00477 
00478         while (CountB< (CountMax-1)) {
00479                 CountA++;
00480                 CountB++;
00481 //              printf("CountA=%d ; CountB=%d \n", CountA,CountB);fflush(stdout);
00482                 kValsSorted[CountA] = kVals[CountB] ;
00483                 if (CountB<(CountMax-1) ) {
00484                         while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001  ) {
00485                                 SizeReturned[0]--;
00486                                 for (int iP=0; iP < lkVals; iP++){
00487 //                                      printf("iP=%d \n", iP);fflush(stdout);
00488                                         if  (PermMatTr[iP]>CountA+1) {
00489                                                 PermMatTr[iP]--;
00490                                         }
00491                                 }
00492                                 CountB++;
00493                         }
00494                 }
00495         }
00496 
00497 
00498         for (int CountD=0; CountD < CountMax; CountD++) {
00499             newInd = PermMatTr[CountD];
00500             weightofkValsSorted[newInd-1] += weightMat[CountD];
00501         }
00502 
00503 }
00504 
00505 
00506 vector<float>
00507 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00508 {
00509         vector<float> angles;
00510         float psi = 0.0;
00511         if ((0.0 == t1)&&(0.0 == t2)||(t1 >= t2)) {
00512                 t1 = 0.0f;
00513                 t2 = 90.0f;
00514         }
00515         if ((0.0 == p1)&&(0.0 == p2)||(p1 >= p2)) {
00516                 p1 = 0.0f;
00517                 p2 = 359.9f;
00518         }
00519         bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00520         for (float theta = t1; theta <= t2; theta += delta) {
00521                 float detphi;
00522                 int lt;
00523                 if ((0.0 == theta)||(180.0 == theta)) {
00524                         detphi = 360.0f;
00525                         lt = 1;
00526                 } else {
00527                         detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00528                         lt = int((p2 - p1)/detphi)-1;
00529                         if (lt < 1) lt = 1;
00530                         detphi = (p2 - p1)/lt;
00531                 }
00532                 for (int i = 0; i < lt; i++) {
00533                         float phi = p1 + i*detphi;
00534                         if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00535                         angles.push_back(phi);
00536                         angles.push_back(theta);
00537                         angles.push_back(psi);
00538                 }
00539         }
00540         return angles;
00541 }
00542 
00543 
00544 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00545 /*float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00546 {
00547 
00548 //  purpose: quadratic interpolation
00549 //
00550 //  parameters:       xx,yy treated as circularly closed.
00551 //                    fdata - image 1..nxdata, 1..nydata
00552 //
00553 //                    f3    fc       f0, f1, f2, f3 are the values
00554 //                     +             at the grid points.  x is the
00555 //                     + x           point at which the function
00556 //              f2++++f0++++f1       is to be estimated. (it need
00557 //                     +             not be in the first quadrant).
00558 //                     +             fc - the outer corner point
00559 //                    f4             nearest x.
00560 c
00561 //                                   f0 is the value of the fdata at
00562 //                                   fdata(i,j), it is the interior mesh
00563 //                                   point nearest  x.
00564 //                                   the coordinates of f0 are (x0,y0),
00565 //                                   the coordinates of f1 are (xb,y0),
00566 //                                   the coordinates of f2 are (xa,y0),
00567 //                                   the coordinates of f3 are (x0,yb),
00568 //                                   the coordinates of f4 are (x0,ya),
00569 //                                   the coordinates of fc are (xc,yc),
00570 c
00571 //                   o               hxa, hxb are the mesh spacings
00572 //                   +               in the x-direction to the left
00573 //                  hyb              and right of the center point.
00574 //                   +
00575 //            ++hxa++o++hxb++o       hyb, hya are the mesh spacings
00576 //                   +               in the y-direction.
00577 //                  hya
00578 //                   +               hxc equals either  hxb  or  hxa
00579 //                   o               depending on where the corner
00580 //                                   point is located.
00581 c
00582 //                                   construct the interpolant
00583 //                                   f = f0 + c1*(x-x0) +
00584 //                                       c2*(x-x0)*(x-x1) +
00585 //                                       c3*(y-y0) + c4*(y-y0)*(y-y1)
00586 //                                       + c5*(x-x0)*(y-y0)
00587 //
00588 //
00589 
00590     float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00591     float quadri;
00592     int   i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00593 
00594     x = xx;
00595     y = yy;
00596 
00597     // circular closure
00598         while ( x < 1.0 ) x += nxdata;
00599         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00600         while ( y < 1.0 ) y += nydata;
00601         while ( y >= (float)(nydata+1) )  y -= nydata;
00602 
00603 
00604     i   = (int) x;
00605     j   = (int) y;
00606 
00607     dx0 = x - i;
00608     dy0 = y - j;
00609 
00610     ip1 = i + 1;
00611     im1 = i - 1;
00612     jp1 = j + 1;
00613     jm1 = j - 1;
00614 
00615     if (ip1 > nxdata) ip1 = ip1 - nxdata;
00616     if (im1 < 1)      im1 = im1 + nxdata;
00617     if (jp1 > nydata) jp1 = jp1 - nydata;
00618     if (jm1 < 1)      jm1 = jm1 + nydata;
00619 
00620     f0  = fdata(i,j);
00621     c1  = fdata(ip1,j) - f0;
00622     c2  = (c1 - f0 + fdata(im1,j)) * 0.5;
00623     c3  = fdata(i,jp1) - f0;
00624     c4  = (c3 - f0 + fdata(i,jm1)) * 0.5;
00625 
00626     dxb = dx0 - 1;
00627     dyb = dy0 - 1;
00628 
00629     // hxc & hyc are either 1 or -1
00630     if (dx0 >= 0) { hxc = 1; } else { hxc = -1; }
00631     if (dy0 >= 0) { hyc = 1; } else { hyc = -1; }
00632 
00633     ic  = i + hxc;
00634     jc  = j + hyc;
00635 
00636     if (ic > nxdata) { ic = ic - nxdata; }  else if (ic < 1) { ic = ic + nxdata; }
00637     if (jc > nydata) { jc = jc - nydata; } else if (jc < 1) { jc = jc + nydata; }
00638 
00639     c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0)) * c2
00640             - hyc * c3 - (hyc * (hyc - 1.0)) * c4) * (hxc * hyc));
00641 
00642     quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00643 
00644     return quadri;
00645 }*/
00646 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00647 {
00648 //  purpose: quadratic interpolation
00649 //  Optimized for speed, circular closer removed, checking of ranges removed
00650         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00651         float  quadri;
00652         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00653 
00654         x = xx;
00655         y = yy;
00656 
00657         //     any xx and yy
00658         while ( x < 1.0 )                 x += nxdata;
00659         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00660         while ( y < 1.0 )                 y += nydata;
00661         while ( y >= (float)(nydata+1) )  y -= nydata;
00662 
00663         i   = (int) x;
00664         j   = (int) y;
00665 
00666         dx0 = x - i;
00667         dy0 = y - j;
00668 
00669         ip1 = i + 1;
00670         im1 = i - 1;
00671         jp1 = j + 1;
00672         jm1 = j - 1;
00673 
00674         if (ip1 > nxdata) ip1 -= nxdata;
00675         if (im1 < 1)      im1 += nxdata;
00676         if (jp1 > nydata) jp1 -= nydata;
00677         if (jm1 < 1)      jm1 += nydata;
00678 
00679         f0  = fdata(i,j);
00680         c1  = fdata(ip1,j) - f0;
00681         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00682         c3  = fdata(i,jp1) - f0;
00683         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00684 
00685         dxb = dx0 - 1;
00686         dyb = dy0 - 1;
00687 
00688         // hxc & hyc are either 1 or -1
00689         if (dx0 >= 0) hxc = 1; else hxc = -1;
00690         if (dy0 >= 0) hyc = 1; else hyc = -1;
00691 
00692         ic  = i + hxc;
00693         jc  = j + hyc;
00694 
00695         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00696         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00697 
00698         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00699                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00700 
00701 
00702         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00703 
00704         return quadri;
00705 }
00706 
00707 #undef fdata
00708 
00709 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00710 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00711 {
00712 //  purpose: quadratic interpolation
00713 //  Optimized for speed, circular closer removed, checking of ranges removed
00714         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00715         float  quadri;
00716         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00717 
00718         x = xx;
00719         y = yy;
00720 
00721         // wrap around is not done circulantly; if (x,y) is not in the image, then x = xnew and y = ynew
00722         if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00723               x = (float)xnew;
00724                   y = (float)ynew;
00725      }
00726 
00727 
00728         i   = (int) x;
00729         j   = (int) y;
00730 
00731         dx0 = x - i;
00732         dy0 = y - j;
00733 
00734         ip1 = i + 1;
00735         im1 = i - 1;
00736         jp1 = j + 1;
00737         jm1 = j - 1;
00738 
00739         if (ip1 > nxdata) ip1 -= nxdata;
00740         if (im1 < 1)      im1 += nxdata;
00741         if (jp1 > nydata) jp1 -= nydata;
00742         if (jm1 < 1)      jm1 += nydata;
00743 
00744         f0  = fdata(i,j);
00745         c1  = fdata(ip1,j) - f0;
00746         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00747         c3  = fdata(i,jp1) - f0;
00748         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00749 
00750         dxb = dx0 - 1;
00751         dyb = dy0 - 1;
00752 
00753         // hxc & hyc are either 1 or -1
00754         if (dx0 >= 0) hxc = 1; else hxc = -1;
00755         if (dy0 >= 0) hyc = 1; else hyc = -1;
00756 
00757         ic  = i + hxc;
00758         jc  = j + hyc;
00759 
00760         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00761         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00762 
00763         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00764                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00765 
00766 
00767         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00768 
00769         return quadri;
00770 }
00771 
00772 #undef fdata
00773 
00774 
00775 float  Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00776         int K = kb.get_window_size();
00777         int kbmin = -K/2;
00778         int kbmax = -kbmin;
00779         int kbc = kbmax+1;
00780 
00781         float pixel =0.0f;
00782         float w=0.0f;
00783 
00784         delx = restrict1(delx, nx);
00785         int inxold = int(round(delx));
00786         if ( ny < 2 ) {  //1D
00787                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00788                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00789                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00790                 float tablex4 = kb.i0win_tab(delx-inxold);
00791                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00792                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00793                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00794 
00795                 int x1, x2, x3, x4, x5, x6, x7;
00796 
00797                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
00798                         x1 = (inxold-3+nx)%nx;
00799                         x2 = (inxold-2+nx)%nx;
00800                         x3 = (inxold-1+nx)%nx;
00801                         x4 = (inxold  +nx)%nx;
00802                         x5 = (inxold+1+nx)%nx;
00803                         x6 = (inxold+2+nx)%nx;
00804                         x7 = (inxold+3+nx)%nx;
00805                 } else {
00806                         x1 = inxold-3;
00807                         x2 = inxold-2;
00808                         x3 = inxold-1;
00809                         x4 = inxold;
00810                         x5 = inxold+1;
00811                         x6 = inxold+2;
00812                         x7 = inxold+3;
00813                 }
00814 
00815                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00816                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00817                         data[x7]*tablex7 ;
00818 
00819                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00820         } else if ( nz < 2 ) {  // 2D
00821                 dely = restrict1(dely, ny);
00822                 int inyold = int(round(dely));
00823                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00824                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00825                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00826                 float tablex4 = kb.i0win_tab(delx-inxold);
00827                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00828                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00829                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00830 
00831                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00832                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00833                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00834                 float tabley4 = kb.i0win_tab(dely-inyold);
00835                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00836                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00837                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00838 
00839                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00840 
00841                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
00842                         x1 = (inxold-3+nx)%nx;
00843                         x2 = (inxold-2+nx)%nx;
00844                         x3 = (inxold-1+nx)%nx;
00845                         x4 = (inxold  +nx)%nx;
00846                         x5 = (inxold+1+nx)%nx;
00847                         x6 = (inxold+2+nx)%nx;
00848                         x7 = (inxold+3+nx)%nx;
00849 
00850                         y1 = ((inyold-3+ny)%ny)*nx;
00851                         y2 = ((inyold-2+ny)%ny)*nx;
00852                         y3 = ((inyold-1+ny)%ny)*nx;
00853                         y4 = ((inyold  +ny)%ny)*nx;
00854                         y5 = ((inyold+1+ny)%ny)*nx;
00855                         y6 = ((inyold+2+ny)%ny)*nx;
00856                         y7 = ((inyold+3+ny)%ny)*nx;
00857                 } else {
00858                         x1 = inxold-3;
00859                         x2 = inxold-2;
00860                         x3 = inxold-1;
00861                         x4 = inxold;
00862                         x5 = inxold+1;
00863                         x6 = inxold+2;
00864                         x7 = inxold+3;
00865 
00866                         y1 = (inyold-3)*nx;
00867                         y2 = (inyold-2)*nx;
00868                         y3 = (inyold-1)*nx;
00869                         y4 = inyold*nx;
00870                         y5 = (inyold+1)*nx;
00871                         y6 = (inyold+2)*nx;
00872                         y7 = (inyold+3)*nx;
00873                 }
00874 
00875                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00876                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00877                              data[x7+y1]*tablex7 ) * tabley1 +
00878                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00879                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00880                              data[x7+y2]*tablex7 ) * tabley2 +
00881                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00882                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00883                              data[x7+y3]*tablex7 ) * tabley3 +
00884                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00885                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00886                              data[x7+y4]*tablex7 ) * tabley4 +
00887                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00888                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00889                              data[x7+y5]*tablex7 ) * tabley5 +
00890                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00891                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00892                              data[x7+y6]*tablex7 ) * tabley6 +
00893                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00894                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00895                              data[x7+y7]*tablex7 ) * tabley7;
00896 
00897                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00898                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00899         } else {  //  3D
00900                 dely = restrict1(dely, ny);
00901                 int inyold = int(Util::round(dely));
00902                 delz = restrict1(delz, nz);
00903                 int inzold = int(Util::round(delz));
00904 
00905                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00906                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00907                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00908                 float tablex4 = kb.i0win_tab(delx-inxold);
00909                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00910                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00911                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00912 
00913                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00914                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00915                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00916                 float tabley4 = kb.i0win_tab(dely-inyold);
00917                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00918                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00919                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00920 
00921                 float tablez1 = kb.i0win_tab(delz-inzold+3);
00922                 float tablez2 = kb.i0win_tab(delz-inzold+2);
00923                 float tablez3 = kb.i0win_tab(delz-inzold+1);
00924                 float tablez4 = kb.i0win_tab(delz-inzold);
00925                 float tablez5 = kb.i0win_tab(delz-inzold-1);
00926                 float tablez6 = kb.i0win_tab(delz-inzold-2);
00927                 float tablez7 = kb.i0win_tab(delz-inzold-3);
00928 
00929                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00930 
00931                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
00932                         x1 = (inxold-3+nx)%nx;
00933                         x2 = (inxold-2+nx)%nx;
00934                         x3 = (inxold-1+nx)%nx;
00935                         x4 = (inxold  +nx)%nx;
00936                         x5 = (inxold+1+nx)%nx;
00937                         x6 = (inxold+2+nx)%nx;
00938                         x7 = (inxold+3+nx)%nx;
00939 
00940                         y1 = ((inyold-3+ny)%ny)*nx;
00941                         y2 = ((inyold-2+ny)%ny)*nx;
00942                         y3 = ((inyold-1+ny)%ny)*nx;
00943                         y4 = ((inyold  +ny)%ny)*nx;
00944                         y5 = ((inyold+1+ny)%ny)*nx;
00945                         y6 = ((inyold+2+ny)%ny)*nx;
00946                         y7 = ((inyold+3+ny)%ny)*nx;
00947 
00948                         z1 = ((inzold-3+nz)%nz)*nx*ny;
00949                         z2 = ((inzold-2+nz)%nz)*nx*ny;
00950                         z3 = ((inzold-1+nz)%nz)*nx*ny;
00951                         z4 = ((inzold  +nz)%nz)*nx*ny;
00952                         z5 = ((inzold+1+nz)%nz)*nx*ny;
00953                         z6 = ((inzold+2+nz)%nz)*nx*ny;
00954                         z7 = ((inzold+3+nz)%nz)*nx*ny;
00955                 } else {
00956                         x1 = inxold-3;
00957                         x2 = inxold-2;
00958                         x3 = inxold-1;
00959                         x4 = inxold;
00960                         x5 = inxold+1;
00961                         x6 = inxold+2;
00962                         x7 = inxold+3;
00963 
00964                         y1 = (inyold-3)*nx;
00965                         y2 = (inyold-2)*nx;
00966                         y3 = (inyold-1)*nx;
00967                         y4 = inyold*nx;
00968                         y5 = (inyold+1)*nx;
00969                         y6 = (inyold+2)*nx;
00970                         y7 = (inyold+3)*nx;
00971 
00972                         z1 = (inzold-3)*nx*ny;
00973                         z2 = (inzold-2)*nx*ny;
00974                         z3 = (inzold-1)*nx*ny;
00975                         z4 = inzold*nx*ny;
00976                         z5 = (inzold+1)*nx*ny;
00977                         z6 = (inzold+2)*nx*ny;
00978                         z7 = (inzold+3)*nx*ny;
00979                 }
00980 
00981                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00982                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00983                              data[x7+y1+z1]*tablex7 ) * tabley1 +
00984                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00985                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00986                              data[x7+y2+z1]*tablex7 ) * tabley2 +
00987                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00988                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00989                              data[x7+y3+z1]*tablex7 ) * tabley3 +
00990                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00991                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00992                              data[x7+y4+z1]*tablex7 ) * tabley4 +
00993                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
00994                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
00995                              data[x7+y5+z1]*tablex7 ) * tabley5 +
00996                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
00997                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
00998                              data[x7+y6+z1]*tablex7 ) * tabley6 +
00999                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01000                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01001                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01002                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01003                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01004                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01005                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01006                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01007                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01008                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01009                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01010                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01011                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01012                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01013                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01014                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01015                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01016                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01017                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01018                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01019                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01020                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01021                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01022                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01023                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01024                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01025                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01026                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01027                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01028                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01029                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01030                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01031                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01032                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01033                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01034                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01035                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01036                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01037                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01038                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01039                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01040                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01041                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01042                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01043                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01044                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01045                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01046                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01047                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01048                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01049                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01050                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01051                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01052                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01053                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01054                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01055                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01056                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01057                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01058                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01059                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01060                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01061                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01062                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01063                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01064                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01065                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01066                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01067                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01068                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01069                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01070                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01071                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01072                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01073                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01074                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01075                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01076                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01077                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01078                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01079                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01080                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01081                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01082                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01083                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01084                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01085                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01086                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01087                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01088                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01089                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01090                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01091                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01092                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01093                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01094                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01095                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01096                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01097                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01098                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01099                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01100                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01101                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01102                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01103                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01104                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01105                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01106                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01107                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01108                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01109                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01110                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01111                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01112                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01113                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01114                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01115                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01116                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01117                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01118                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01119                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01120                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01121                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01122                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01123                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01124                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01125                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01126                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01127                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01128 
01129                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01130                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01131                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01132         }
01133         return pixel/w;
01134 }
01135 
01136 float  Util::get_pixel_conv_new_background(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb, int xnew, int ynew) {
01137         int K = kb.get_window_size();
01138         int kbmin = -K/2;
01139         int kbmax = -kbmin;
01140         int kbc = kbmax+1;
01141 
01142         float pixel =0.0f;
01143         float w=0.0f;
01144 
01145     float argdelx = delx; // adding this for 2D case where the wrap around is not done circulantly using restrict1.
01146         delx = restrict1(delx, nx);
01147         int inxold = int(round(delx));
01148         if ( ny < 2 ) {  //1D
01149                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01150                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01151                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01152                 float tablex4 = kb.i0win_tab(delx-inxold);
01153                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01154                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01155                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01156 
01157                 int x1, x2, x3, x4, x5, x6, x7;
01158 
01159                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
01160                         x1 = (inxold-3+nx)%nx;
01161                         x2 = (inxold-2+nx)%nx;
01162                         x3 = (inxold-1+nx)%nx;
01163                         x4 = (inxold  +nx)%nx;
01164                         x5 = (inxold+1+nx)%nx;
01165                         x6 = (inxold+2+nx)%nx;
01166                         x7 = (inxold+3+nx)%nx;
01167                 } else {
01168                         x1 = inxold-3;
01169                         x2 = inxold-2;
01170                         x3 = inxold-1;
01171                         x4 = inxold;
01172                         x5 = inxold+1;
01173                         x6 = inxold+2;
01174                         x7 = inxold+3;
01175                 }
01176 
01177                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01178                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01179                         data[x7]*tablex7 ;
01180 
01181                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01182         } else if ( nz < 2 ) {  // 2D
01183 
01184                 delx = argdelx;
01185                 // 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
01186                 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01187                 delx = (float)xnew*2.0f;
01188                 dely = (float)ynew*2.0f;
01189                 }
01190 
01191                 int inxold = int(round(delx));
01192                 int inyold = int(round(dely));
01193 
01194                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01195                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01196                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01197                 float tablex4 = kb.i0win_tab(delx-inxold);
01198                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01199                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01200                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01201 
01202                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01203                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01204                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01205                 float tabley4 = kb.i0win_tab(dely-inyold);
01206                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01207                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01208                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01209 
01210                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01211 
01212                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
01213                         x1 = (inxold-3+nx)%nx;
01214                         x2 = (inxold-2+nx)%nx;
01215                         x3 = (inxold-1+nx)%nx;
01216                         x4 = (inxold  +nx)%nx;
01217                         x5 = (inxold+1+nx)%nx;
01218                         x6 = (inxold+2+nx)%nx;
01219                         x7 = (inxold+3+nx)%nx;
01220 
01221                         y1 = ((inyold-3+ny)%ny)*nx;
01222                         y2 = ((inyold-2+ny)%ny)*nx;
01223                         y3 = ((inyold-1+ny)%ny)*nx;
01224                         y4 = ((inyold  +ny)%ny)*nx;
01225                         y5 = ((inyold+1+ny)%ny)*nx;
01226                         y6 = ((inyold+2+ny)%ny)*nx;
01227                         y7 = ((inyold+3+ny)%ny)*nx;
01228                 } else {
01229                         x1 = inxold-3;
01230                         x2 = inxold-2;
01231                         x3 = inxold-1;
01232                         x4 = inxold;
01233                         x5 = inxold+1;
01234                         x6 = inxold+2;
01235                         x7 = inxold+3;
01236 
01237                         y1 = (inyold-3)*nx;
01238                         y2 = (inyold-2)*nx;
01239                         y3 = (inyold-1)*nx;
01240                         y4 = inyold*nx;
01241                         y5 = (inyold+1)*nx;
01242                         y6 = (inyold+2)*nx;
01243                         y7 = (inyold+3)*nx;
01244                 }
01245 
01246                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01247                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01248                              data[x7+y1]*tablex7 ) * tabley1 +
01249                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01250                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01251                              data[x7+y2]*tablex7 ) * tabley2 +
01252                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01253                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01254                              data[x7+y3]*tablex7 ) * tabley3 +
01255                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01256                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01257                              data[x7+y4]*tablex7 ) * tabley4 +
01258                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01259                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01260                              data[x7+y5]*tablex7 ) * tabley5 +
01261                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01262                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01263                              data[x7+y6]*tablex7 ) * tabley6 +
01264                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01265                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01266                              data[x7+y7]*tablex7 ) * tabley7;
01267 
01268                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01269                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01270         } else {  //  3D
01271                 dely = restrict1(dely, ny);
01272                 int inyold = int(Util::round(dely));
01273                 delz = restrict1(delz, nz);
01274                 int inzold = int(Util::round(delz));
01275 
01276                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01277                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01278                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01279                 float tablex4 = kb.i0win_tab(delx-inxold);
01280                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01281                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01282                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01283 
01284                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01285                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01286                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01287                 float tabley4 = kb.i0win_tab(dely-inyold);
01288                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01289                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01290                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01291 
01292                 float tablez1 = kb.i0win_tab(delz-inzold+3);
01293                 float tablez2 = kb.i0win_tab(delz-inzold+2);
01294                 float tablez3 = kb.i0win_tab(delz-inzold+1);
01295                 float tablez4 = kb.i0win_tab(delz-inzold);
01296                 float tablez5 = kb.i0win_tab(delz-inzold-1);
01297                 float tablez6 = kb.i0win_tab(delz-inzold-2);
01298                 float tablez7 = kb.i0win_tab(delz-inzold-3);
01299 
01300                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01301 
01302                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
01303                         x1 = (inxold-3+nx)%nx;
01304                         x2 = (inxold-2+nx)%nx;
01305                         x3 = (inxold-1+nx)%nx;
01306                         x4 = (inxold  +nx)%nx;
01307                         x5 = (inxold+1+nx)%nx;
01308                         x6 = (inxold+2+nx)%nx;
01309                         x7 = (inxold+3+nx)%nx;
01310 
01311                         y1 = ((inyold-3+ny)%ny)*nx;
01312                         y2 = ((inyold-2+ny)%ny)*nx;
01313                         y3 = ((inyold-1+ny)%ny)*nx;
01314                         y4 = ((inyold  +ny)%ny)*nx;
01315                         y5 = ((inyold+1+ny)%ny)*nx;
01316                         y6 = ((inyold+2+ny)%ny)*nx;
01317                         y7 = ((inyold+3+ny)%ny)*nx;
01318 
01319                         z1 = ((inzold-3+nz)%nz)*nx*ny;
01320                         z2 = ((inzold-2+nz)%nz)*nx*ny;
01321                         z3 = ((inzold-1+nz)%nz)*nx*ny;
01322                         z4 = ((inzold  +nz)%nz)*nx*ny;
01323                         z5 = ((inzold+1+nz)%nz)*nx*ny;
01324                         z6 = ((inzold+2+nz)%nz)*nx*ny;
01325                         z7 = ((inzold+3+nz)%nz)*nx*ny;
01326                 } else {
01327                         x1 = inxold-3;
01328                         x2 = inxold-2;
01329                         x3 = inxold-1;
01330                         x4 = inxold;
01331                         x5 = inxold+1;
01332                         x6 = inxold+2;
01333                         x7 = inxold+3;
01334 
01335                         y1 = (inyold-3)*nx;
01336                         y2 = (inyold-2)*nx;
01337                         y3 = (inyold-1)*nx;
01338                         y4 = inyold*nx;
01339                         y5 = (inyold+1)*nx;
01340                         y6 = (inyold+2)*nx;
01341                         y7 = (inyold+3)*nx;
01342 
01343                         z1 = (inzold-3)*nx*ny;
01344                         z2 = (inzold-2)*nx*ny;
01345                         z3 = (inzold-1)*nx*ny;
01346                         z4 = inzold*nx*ny;
01347                         z5 = (inzold+1)*nx*ny;
01348                         z6 = (inzold+2)*nx*ny;
01349                         z7 = (inzold+3)*nx*ny;
01350                 }
01351 
01352                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01353                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01354                              data[x7+y1+z1]*tablex7 ) * tabley1 +
01355                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01356                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01357                              data[x7+y2+z1]*tablex7 ) * tabley2 +
01358                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01359                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01360                              data[x7+y3+z1]*tablex7 ) * tabley3 +
01361                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01362                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01363                              data[x7+y4+z1]*tablex7 ) * tabley4 +
01364                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01365                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01366                              data[x7+y5+z1]*tablex7 ) * tabley5 +
01367                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01368                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01369                              data[x7+y6+z1]*tablex7 ) * tabley6 +
01370                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01371                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01372                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01373                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01374                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01375                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01376                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01377                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01378                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01379                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01380                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01381                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01382                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01383                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01384                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01385                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01386                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01387                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01388                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01389                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01390                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01391                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01392                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01393                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01394                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01395                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01396                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01397                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01398                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01399                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01400                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01401                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01402                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01403                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01404                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01405                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01406                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01407                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01408                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01409                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01410                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01411                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01412                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01413                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01414                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01415                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01416                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01417                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01418                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01419                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01420                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01421                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01422                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01423                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01424                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01425                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01426                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01427                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01428                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01429                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01430                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01431                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01432                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01433                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01434                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01435                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01436                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01437                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01438                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01439                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01440                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01441                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01442                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01443                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01444                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01445                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01446                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01447                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01448                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01449                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01450                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01451                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01452                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01453                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01454                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01455                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01456                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01457                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01458                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01459                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01460                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01461                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01462                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01463                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01464                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01465                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01466                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01467                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01468                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01469                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01470                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01471                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01472                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01473                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01474                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01475                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01476                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01477                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01478                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01479                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01480                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01481                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01482                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01483                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01484                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01485                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01486                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01487                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01488                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01489                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01490                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01491                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01492                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01493                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01494                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01495                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01496                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01497                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01498                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01499 
01500                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01501                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01502                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01503         }
01504         return pixel/w;
01505 }
01506 
01507 /*
01508 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01509 
01510         int nxreal = nx - 2;
01511         if (nxreal != ny)
01512                 throw ImageDimensionException("extractpoint requires ny == nx");
01513         int nhalf = nxreal/2;
01514         int kbsize = kb.get_window_size();
01515         int kbmin = -kbsize/2;
01516         int kbmax = -kbmin;
01517         bool flip = (nuxnew < 0.f);
01518         if (flip) {
01519                 nuxnew *= -1;
01520                 nuynew *= -1;
01521         }
01522         // put (xnew,ynew) on a grid.  The indices will be wrong for
01523         // the Fourier elements in the image, but the grid sizing will
01524         // be correct.
01525         int ixn = int(Util::round(nuxnew));
01526         int iyn = int(Util::round(nuynew));
01527         // set up some temporary weighting arrays
01528         float* wy0 = new float[kbmax - kbmin + 1];
01529         float* wy = wy0 - kbmin; // wy[kbmin:kbmax]
01530         float* wx0 = new float[kbmax - kbmin + 1];
01531         float* wx = wx0 - kbmin;
01532         for (int i = kbmin; i <= kbmax; i++) {
01533                         int iyp = iyn + i;
01534                         wy[i] = kb.i0win_tab(nuynew - iyp);
01535                         int ixp = ixn + i;
01536                         wx[i] = kb.i0win_tab(nuxnew - ixp);
01537         }
01538         // restrict loops to non-zero elements
01539         int iymin = 0;
01540         for (int iy = kbmin; iy <= -1; iy++) {
01541                 if (wy[iy] != 0.f) {
01542                         iymin = iy;
01543                         break;
01544                 }
01545         }
01546         int iymax = 0;
01547         for (int iy = kbmax; iy >= 1; iy--) {
01548                 if (wy[iy] != 0.f) {
01549                         iymax = iy;
01550                         break;
01551                 }
01552         }
01553         int ixmin = 0;
01554         for (int ix = kbmin; ix <= -1; ix++) {
01555                 if (wx[ix] != 0.f) {
01556                         ixmin = ix;
01557                         break;
01558                 }
01559         }
01560         int ixmax = 0;
01561         for (int ix = kbmax; ix >= 1; ix--) {
01562                 if (wx[ix] != 0.f) {
01563                         ixmax = ix;
01564                         break;
01565                 }
01566         }
01567         float wsum = 0.0f;
01568         for (int iy = iymin; iy <= iymax; iy++)
01569                 for (int ix = ixmin; ix <= ixmax; ix++)
01570                         wsum += wx[ix]*wy[iy];
01571 
01572         complex<float> result(0.f,0.f);
01573         if ((ixn >= -kbmin) && (ixn <= nhalf-1-kbmax) && (iyn >= -nhalf-kbmin) && (iyn <= nhalf-1-kbmax)) {
01574                 // (xin,yin) not within window border from the edge
01575                 for (int iy = iymin; iy <= iymax; iy++) {
01576                         int iyp = iyn + iy;
01577                         for (int ix = ixmin; ix <= ixmax; ix++) {
01578                                 int ixp = ixn + ix;
01579                                 float w = wx[ix]*wy[iy];
01580                                 complex<float> val = fimage->cmplx(ixp,iyp);
01581                                 result += val*w;
01582                         }
01583                 }
01584         } else {
01585                 // points that "stick out"
01586                 for (int iy = iymin; iy <= iymax; iy++) {
01587                         int iyp = iyn + iy;
01588                         for (int ix = ixmin; ix <= ixmax; ix++) {
01589                                 int ixp = ixn + ix;
01590                                 bool mirror = false;
01591                                 int ixt= ixp, iyt= iyp;
01592                                 if (ixt < 0) {
01593                                         ixt = -ixt;
01594                                         iyt = -iyt;
01595                                         mirror = !mirror;
01596                                 }
01597                                 if (ixt > nhalf) {
01598                                         ixt = nxreal - ixt;
01599                                         iyt = -iyt;
01600                                         mirror = !mirror;
01601                                 }
01602                                 if (iyt > nhalf-1)  iyt -= nxreal;
01603                                 if (iyt < -nhalf)   iyt += nxreal;
01604                                 float w = wx[ix]*wy[iy];
01605                                 complex<float> val = fimage->cmplx(ixt,iyt);
01606                                 if (mirror)  result += conj(val)*w;
01607                                 else         result += val*w;
01608                         }
01609                 }
01610         }
01611         if (flip)  result = conj(result)/wsum;
01612         else result /= wsum;
01613         delete [] wx0;
01614         delete [] wy0;
01615         return result;
01616 }*/
01617 
01618 /*
01619 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01620 
01621         int nxreal = nx - 2;
01622         if (nxreal != ny)
01623                 throw ImageDimensionException("extractpoint requires ny == nx");
01624         int nhalf = nxreal/2;
01625         bool flip = false;
01626         if (nuxnew < 0.f) {
01627                 nuxnew *= -1;
01628                 nuynew *= -1;
01629                 flip = true;
01630         }
01631         if (nuynew >= nhalf-0.5)  {
01632                 nuynew -= nxreal;
01633         } else if (nuynew < -nhalf-0.5) {
01634                 nuynew += nxreal;
01635         }
01636 
01637         // put (xnew,ynew) on a grid.  The indices will be wrong for
01638         // the Fourier elements in the image, but the grid sizing will
01639         // be correct.
01640         int ixn = int(Util::round(nuxnew));
01641         int iyn = int(Util::round(nuynew));
01642 
01643         // set up some temporary weighting arrays
01644         static float wy[7];
01645         static float wx[7];
01646 
01647         float iynn = nuynew - iyn;
01648         wy[0] = kb.i0win_tab(iynn+3);
01649         wy[1] = kb.i0win_tab(iynn+2);
01650         wy[2] = kb.i0win_tab(iynn+1);
01651         wy[3] = kb.i0win_tab(iynn);
01652         wy[4] = kb.i0win_tab(iynn-1);
01653         wy[5] = kb.i0win_tab(iynn-2);
01654         wy[6] = kb.i0win_tab(iynn-3);
01655 
01656         float ixnn = nuxnew - ixn;
01657         wx[0] = kb.i0win_tab(ixnn+3);
01658         wx[1] = kb.i0win_tab(ixnn+2);
01659         wx[2] = kb.i0win_tab(ixnn+1);
01660         wx[3] = kb.i0win_tab(ixnn);
01661         wx[4] = kb.i0win_tab(ixnn-1);
01662         wx[5] = kb.i0win_tab(ixnn-2);
01663         wx[6] = kb.i0win_tab(ixnn-3);
01664 
01665         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]);
01666 
01667         complex<float> result(0.f,0.f);
01668         for (int iy = 0; iy < 7; iy++) {
01669                 int iyp = iyn + iy - 3 ;
01670                 for (int ix = 0; ix < 7; ix++) {
01671                         int ixp = ixn + ix - 3;
01672                         float w = wx[ix]*wy[iy];
01673                         complex<float> val = fimage->cmplx(ixp,iyp);
01674                         result += val*w;
01675                 }
01676         }
01677 
01678         if (flip)  result = conj(result)/wsum;
01679         else result /= wsum;
01680 
01681         return result;
01682 }*/
01683 
01684 
01685 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01686 
01687         int nxreal = nx - 2;
01688         if (nxreal != ny)
01689                 throw ImageDimensionException("extractpoint requires ny == nx");
01690         int nhalf = nxreal/2;
01691         bool flip = (nuxnew < 0.f);
01692         if (flip) {
01693                 nuxnew *= -1;
01694                 nuynew *= -1;
01695         }
01696         if (nuynew >= nhalf-0.5)  {
01697                 nuynew -= nxreal;
01698         } else if (nuynew < -nhalf-0.5) {
01699                 nuynew += nxreal;
01700         }
01701 
01702         // put (xnew,ynew) on a grid.  The indices will be wrong for
01703         // the Fourier elements in the image, but the grid sizing will
01704         // be correct.
01705         int ixn = int(Util::round(nuxnew));
01706         int iyn = int(Util::round(nuynew));
01707 
01708         // set up some temporary weighting arrays
01709         static float wy[7];
01710         static float wx[7];
01711 
01712         float iynn = nuynew - iyn;
01713         wy[0] = kb.i0win_tab(iynn+3);
01714         wy[1] = kb.i0win_tab(iynn+2);
01715         wy[2] = kb.i0win_tab(iynn+1);
01716         wy[3] = kb.i0win_tab(iynn);
01717         wy[4] = kb.i0win_tab(iynn-1);
01718         wy[5] = kb.i0win_tab(iynn-2);
01719         wy[6] = kb.i0win_tab(iynn-3);
01720 
01721         float ixnn = nuxnew - ixn;
01722         wx[0] = kb.i0win_tab(ixnn+3);
01723         wx[1] = kb.i0win_tab(ixnn+2);
01724         wx[2] = kb.i0win_tab(ixnn+1);
01725         wx[3] = kb.i0win_tab(ixnn);
01726         wx[4] = kb.i0win_tab(ixnn-1);
01727         wx[5] = kb.i0win_tab(ixnn-2);
01728         wx[6] = kb.i0win_tab(ixnn-3);
01729 
01730         float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01731 
01732         complex<float> result(0.f,0.f);
01733         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01734                 // (xin,yin) not within window border from the edge
01735                 for (int iy = 0; iy < 7; iy++) {
01736                         int iyp = iyn + iy - 3 ;
01737                         for (int ix = 0; ix < 7; ix++) {
01738                                 int ixp = ixn + ix - 3;
01739                                 float w = wx[ix]*wy[iy];
01740                                 complex<float> val = fimage->cmplx(ixp,iyp);
01741                                 result += val*w;
01742                         }
01743                 }
01744         } else {
01745                 // points that "stick out"
01746                 for (int iy = 0; iy < 7; iy++) {
01747                         int iyp = iyn + iy - 3;
01748                         for (int ix = 0; ix < 7; ix++) {
01749                                 int ixp = ixn + ix - 3;
01750                                 bool mirror = false;
01751                                 int ixt = ixp, iyt = iyp;
01752                                 if (ixt < 0) {
01753                                         ixt = -ixt;
01754                                         iyt = -iyt;
01755                                         mirror = !mirror;
01756                                 }
01757                                 if (ixt > nhalf) {
01758                                         ixt = nxreal - ixt;
01759                                         iyt = -iyt;
01760                                         mirror = !mirror;
01761                                 }
01762                                 if (iyt > nhalf-1)  iyt -= nxreal;
01763                                 if (iyt < -nhalf)   iyt += nxreal;
01764                                 float w = wx[ix]*wy[iy];
01765                                 complex<float> val = fimage->cmplx(ixt,iyt);
01766                                 if (mirror)  result += conj(val)*w;
01767                                 else         result += val*w;
01768                         }
01769                 }
01770         }
01771         if (flip)  result = conj(result)/wsum;
01772         else result /= wsum;
01773         return result;
01774 }
01775 
01776 /*
01777 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01778 
01779         int nxreal = nx - 2;
01780         if (nxreal != ny)
01781                 throw ImageDimensionException("extractpoint requires ny == nx");
01782         int nhalf = nxreal/2;
01783         bool flip = (nuxnew < 0.f);
01784         if (flip) {
01785                 nuxnew *= -1;
01786                 nuynew *= -1;
01787         }
01788         // put (xnew,ynew) on a grid.  The indices will be wrong for
01789         // the Fourier elements in the image, but the grid sizing will
01790         // be correct.
01791         int ixn = int(Util::round(nuxnew));
01792         int iyn = int(Util::round(nuynew));
01793         // set up some temporary weighting arrays
01794         static float wy[7];
01795         static float wx[7];
01796 
01797         float iynn = nuynew - iyn;
01798         wy[0] = kb.i0win_tab(iynn+3);
01799         wy[1] = kb.i0win_tab(iynn+2);
01800         wy[2] = kb.i0win_tab(iynn+1);
01801         wy[3] = kb.i0win_tab(iynn);
01802         wy[4] = kb.i0win_tab(iynn-1);
01803         wy[5] = kb.i0win_tab(iynn-2);
01804         wy[6] = kb.i0win_tab(iynn-3);
01805 
01806         float ixnn = nuxnew - ixn;
01807         wx[0] = kb.i0win_tab(ixnn+3);
01808         wx[1] = kb.i0win_tab(ixnn+2);
01809         wx[2] = kb.i0win_tab(ixnn+1);
01810         wx[3] = kb.i0win_tab(ixnn);
01811         wx[4] = kb.i0win_tab(ixnn-1);
01812         wx[5] = kb.i0win_tab(ixnn-2);
01813         wx[6] = kb.i0win_tab(ixnn-3);
01814 
01815         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]);
01816 
01817         complex<float> result(0.f,0.f);
01818 
01819         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01820                 // (xin,yin) not within window border from the edge
01821                 result = ( fimage->cmplx(ixn-3,iyn-3)*wx[0] +
01822                            fimage->cmplx(ixn-2,iyn-3)*wx[1] +
01823                            fimage->cmplx(ixn-1,iyn-3)*wx[2] +
01824                            fimage->cmplx(ixn+0,iyn-3)*wx[3] +
01825                            fimage->cmplx(ixn+1,iyn-3)*wx[4] +
01826                            fimage->cmplx(ixn+2,iyn-3)*wx[5] +
01827                            fimage->cmplx(ixn+3,iyn-3)*wx[6] )*wy[0] +
01828                            ( fimage->cmplx(ixn-3,iyn-2)*wx[0] +
01829                            fimage->cmplx(ixn-2,iyn-2)*wx[1] +
01830                            fimage->cmplx(ixn-1,iyn-2)*wx[2] +
01831                            fimage->cmplx(ixn+0,iyn-2)*wx[3] +
01832                            fimage->cmplx(ixn+1,iyn-2)*wx[4] +
01833                            fimage->cmplx(ixn+2,iyn-2)*wx[5] +
01834                            fimage->cmplx(ixn+3,iyn-2)*wx[6] )*wy[1] +
01835                            ( fimage->cmplx(ixn-3,iyn-1)*wx[0] +
01836                            fimage->cmplx(ixn-2,iyn-1)*wx[1] +
01837                            fimage->cmplx(ixn-1,iyn-1)*wx[2] +
01838                            fimage->cmplx(ixn+0,iyn-1)*wx[3] +
01839                            fimage->cmplx(ixn+1,iyn-1)*wx[4] +
01840                            fimage->cmplx(ixn+2,iyn-1)*wx[5] +
01841                            fimage->cmplx(ixn+3,iyn-1)*wx[6] )*wy[2] +
01842                            ( fimage->cmplx(ixn-3,iyn+0)*wx[0] +
01843                            fimage->cmplx(ixn-2,iyn+0)*wx[1] +
01844                            fimage->cmplx(ixn-1,iyn+0)*wx[2] +
01845                            fimage->cmplx(ixn+0,iyn+0)*wx[3] +
01846                            fimage->cmplx(ixn+1,iyn+0)*wx[4] +
01847                            fimage->cmplx(ixn+2,iyn+0)*wx[5] +
01848                            fimage->cmplx(ixn+3,iyn+0)*wx[6] )*wy[3] +
01849                            ( fimage->cmplx(ixn-3,iyn+1)*wx[0] +
01850                            fimage->cmplx(ixn-2,iyn+1)*wx[1] +
01851                            fimage->cmplx(ixn-1,iyn+1)*wx[2] +
01852                            fimage->cmplx(ixn+0,iyn+1)*wx[3] +
01853                            fimage->cmplx(ixn+1,iyn+1)*wx[4] +
01854                            fimage->cmplx(ixn+2,iyn+1)*wx[5] +
01855                            fimage->cmplx(ixn+3,iyn+1)*wx[6] )*wy[4] +
01856                            ( fimage->cmplx(ixn-3,iyn+2)*wx[0] +
01857                            fimage->cmplx(ixn-2,iyn+2)*wx[1] +
01858                            fimage->cmplx(ixn-1,iyn+2)*wx[2] +
01859                            fimage->cmplx(ixn+0,iyn+2)*wx[3] +
01860                            fimage->cmplx(ixn+1,iyn+2)*wx[4] +
01861                            fimage->cmplx(ixn+2,iyn+2)*wx[5] +
01862                            fimage->cmplx(ixn+3,iyn+2)*wx[6] )*wy[5] +
01863                            ( fimage->cmplx(ixn-3,iyn+3)*wx[0] +
01864                            fimage->cmplx(ixn-2,iyn+3)*wx[1] +
01865                            fimage->cmplx(ixn-1,iyn+3)*wx[2] +
01866                            fimage->cmplx(ixn+0,iyn+3)*wx[3] +
01867                            fimage->cmplx(ixn+1,iyn+3)*wx[4] +
01868                            fimage->cmplx(ixn+2,iyn+3)*wx[5] +
01869                            fimage->cmplx(ixn+3,iyn+3)*wx[6] )*wy[6];
01870 
01871         } else {
01872                 // points that "stick out"
01873                 for (int iy = 0; iy < 7; iy++) {
01874                         int iyp = iyn + iy - 3;
01875                         for (int ix = 0; ix < 7; ix++) {
01876                                 int ixp = ixn + ix - 3;
01877                                 bool mirror = false;
01878                                 int ixt= ixp, iyt= iyp;
01879                                 if (ixt < 0) {
01880                                         ixt = -ixt;
01881                                         iyt = -iyt;
01882                                         mirror = !mirror;
01883                                 }
01884                                 if (ixt > nhalf) {
01885                                         ixt = nxreal - ixt;
01886                                         iyt = -iyt;
01887                                         mirror = !mirror;
01888                                 }
01889                                 if (iyt > nhalf-1)  iyt -= nxreal;
01890                                 if (iyt < -nhalf)   iyt += nxreal;
01891                                 float w = wx[ix]*wy[iy];
01892                                 complex<float> val = fimage->cmplx(ixt,iyt);
01893                                 if (mirror)  result += conj(val)*w;
01894                                 else         result += val*w;
01895                         }
01896                 }
01897         }
01898         if (flip)  result = conj(result)/wsum;
01899         else result /= wsum;
01900         return result;
01901 }*/
01902 
01903 
01904 float Util::triquad(float R, float S, float T, float* fdata)
01905 {
01906 
01907     const float C2 = 0.5f;    //1.0 / 2.0;
01908     const float C4 = 0.25f;   //1.0 / 4.0;
01909     const float C8 = 0.125f;  //1.0 / 8.0;
01910 
01911     float  RS   = R * S;
01912     float  ST   = S * T;
01913     float  RT   = R * T;
01914     float  RST  = R * ST;
01915 
01916     float  RSQ  = 1-R*R;
01917     float  SSQ  = 1-S*S;
01918     float  TSQ  = 1-T*T;
01919 
01920     float  RM1  = (1-R);
01921     float  SM1  = (1-S);
01922     float  TM1  = (1-T);
01923 
01924     float  RP1  = (1+R);
01925     float  SP1  = (1+S);
01926     float  TP1  = (1+T);
01927 
01928     float triquad =
01929     (-C8) * RST * RM1  * SM1  * TM1 * fdata[0] +
01930         ( C4) * ST  * RSQ  * SM1  * TM1 * fdata[1] +
01931         ( C8) * RST * RP1  * SM1  * TM1 * fdata[2] +
01932         ( C4) * RT  * RM1  * SSQ  * TM1 * fdata[3] +
01933         (-C2) * T   * RSQ  * SSQ  * TM1 * fdata[4] +
01934         (-C4) * RT  * RP1  * SSQ  * TM1 * fdata[5] +
01935         ( C8) * RST * RM1  * SP1  * TM1 * fdata[6] +
01936         (-C4) * ST  * RSQ  * SP1  * TM1 * fdata[7] +
01937         (-C8) * RST * RP1  * SP1  * TM1 * fdata[8] +
01938 //
01939         ( C4) * RS  * RM1  * SM1  * TSQ * fdata[9]  +
01940         (-C2) * S   * RSQ  * SM1  * TSQ * fdata[10] +
01941         (-C4) * RS  * RP1  * SM1  * TSQ * fdata[11] +
01942         (-C2) * R   * RM1  * SSQ  * TSQ * fdata[12] +
01943                       RSQ  * SSQ  * TSQ * fdata[13] +
01944         ( C2) * R   * RP1  * SSQ  * TSQ * fdata[14] +
01945         (-C4) * RS  * RM1  * SP1  * TSQ * fdata[15] +
01946         ( C2) * S   * RSQ  * SP1  * TSQ * fdata[16] +
01947         ( C4) * RS  * RP1  * SP1  * TSQ * fdata[17] +
01948  //
01949         ( C8) * RST * RM1  * SM1  * TP1 * fdata[18] +
01950         (-C4) * ST  * RSQ  * SM1  * TP1 * fdata[19] +
01951         (-C8) * RST * RP1  * SM1  * TP1 * fdata[20] +
01952         (-C4) * RT  * RM1  * SSQ  * TP1 * fdata[21] +
01953         ( C2) * T   * RSQ  * SSQ  * TP1 * fdata[22] +
01954         ( C4) * RT  * RP1  * SSQ  * TP1 * fdata[23] +
01955         (-C8) * RST * RM1  * SP1  * TP1 * fdata[24] +
01956         ( C4) * ST  * RSQ  * SP1  * TP1 * fdata[25] +
01957         ( C8) * RST * RP1  * SP1  * TP1 * fdata[26]   ;
01958      return triquad;
01959 }
01960 
01961 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01962                 : M(M_), fc(fc_), ntable(ntable_) {
01963         // Sinc-Blackman kernel
01964         build_sBtable();
01965 }
01966 
01967 void Util::sincBlackman::build_sBtable() {
01968         sBtable.resize(ntable+1);
01969         int ltab = int(round(float(ntable)/1.25f));
01970         int M2 = M/2;
01971         fltb = float(ltab)/M2;
01972         for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01973         float x = 1.0e-7f;
01974         sBtable[0] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01975         for (int i=1; i <= ltab; i++) {
01976                 x = float(i)/fltb;
01977                 sBtable[i] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01978                 //cout << "  "<<x<<"  "<<sBtable[i] <<endl;
01979         }
01980 }
01981 
01982 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01983                                          int N_, float vtable_, int ntable_)
01984                 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01985                   ntable(ntable_) {
01986         // Default values are alpha=1.25, K=6, r=0.5, v = K/2
01987         if (0.f == v) v = float(K)/2;
01988         if (0.f == vtable) vtable = v;
01989         alphar = alpha*r;
01990         fac = static_cast<float>(twopi)*alphar*v;
01991         vadjust = 1.0f*v;
01992         facadj = static_cast<float>(twopi)*alphar*vadjust;
01993         build_I0table();
01994 }
01995 
01996 float Util::KaiserBessel::i0win(float x) const {
01997         float val0 = float(gsl_sf_bessel_I0(facadj));
01998         float absx = fabs(x);
01999         if (absx > vadjust) return 0.f;
02000         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02001         float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02002         return res;
02003 }
02004 
02005 void Util::KaiserBessel::build_I0table() {
02006         i0table.resize(ntable+1); // i0table[0:ntable]
02007         int ltab = int(round(float(ntable)/1.25f));
02008         fltb = float(ltab)/(K/2);
02009         float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02010         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02011         for (int i=0; i <= ltab; i++) {
02012                 float s = float(i)/fltb/N;
02013                 if (s < vadjust) {
02014                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02015                         i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02016                 } else {
02017                         i0table[i] = 0.f;
02018                 }
02019 //              cout << "  "<<s*N<<"  "<<i0table[i] <<endl;
02020         }
02021 }
02022 
02023 float Util::KaiserBessel::I0table_maxerror() {
02024         float maxdiff = 0.f;
02025         for (int i = 1; i <= ntable; i++) {
02026                 float diff = fabs(i0table[i] - i0table[i-1]);
02027                 if (diff > maxdiff) maxdiff = diff;
02028         }
02029         return maxdiff;
02030 }
02031 
02032 float Util::KaiserBessel::sinhwin(float x) const {
02033         float val0 = sinh(fac)/fac;
02034         float absx = fabs(x);
02035         if (0.0 == x) {
02036                 float res = 1.0f;
02037                 return res;
02038         } else if (absx == alphar) {
02039                 return 1.0f/val0;
02040         } else if (absx < alphar) {
02041                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02042                 float facrt = fac*rt;
02043                 float res = (sinh(facrt)/facrt)/val0;
02044                 return res;
02045         } else {
02046                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02047                 float facrt = fac*rt;
02048                 float res = (sin(facrt)/facrt)/val0;
02049                 return res;
02050         }
02051 }
02052 
02053 float Util::FakeKaiserBessel::i0win(float x) const {
02054         float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02055         float absx = fabs(x);
02056         if (absx > vadjust) return 0.f;
02057         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02058         float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02059         return res;
02060 }
02061 
02062 void Util::FakeKaiserBessel::build_I0table() {
02063         i0table.resize(ntable+1); // i0table[0:ntable]
02064         int ltab = int(round(float(ntable)/1.1f));
02065         fltb = float(ltab)/(K/2);
02066         float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02067         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02068         for (int i=0; i <= ltab; i++) {
02069                 float s = float(i)/fltb/N;
02070                 if (s < vadjust) {
02071                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02072                         i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02073                 } else {
02074                         i0table[i] = 0.f;
02075                 }
02076         }
02077 }
02078 
02079 float Util::FakeKaiserBessel::sinhwin(float x) const {
02080         float val0 = sinh(fac)/fac;
02081         float absx = fabs(x);
02082         if (0.0 == x) {
02083                 float res = 1.0f;
02084                 return res;
02085         } else if (absx == alphar) {
02086                 return 1.0f/val0;
02087         } else if (absx < alphar) {
02088                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02089                 float facrt = fac*rt;
02090                 float res = (sinh(facrt)/facrt)/val0;
02091                 return res;
02092         } else {
02093                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02094                 float facrt = fac*rt;
02095                 float res = (sin(facrt)/facrt)/val0;
02096                 return res;
02097         }
02098 }
02099 
02100 #if 0 // 1-st order KB window
02101 float Util::FakeKaiserBessel::sinhwin(float x) const {
02102         //float val0 = sinh(fac)/fac;
02103         float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02104         float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02105         float absx = fabs(x);
02106         if (0.0 == x) {
02107                 //float res = 1.0f;
02108                 float res = val0;
02109                 return res;
02110         } else if (absx == alphar) {
02111                 //return 1.0f/val0;
02112                 return prefix;
02113         } else if (absx < alphar) {
02114                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02115                 //float facrt = fac*rt;
02116                 float facrt = facadj*rt;
02117                 //float res = (sinh(facrt)/facrt)/val0;
02118                 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02119                 return res;
02120         } else {
02121                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02122                 //float facrt = fac*rt;
02123                 float facrt = facadj*rt;
02124                 //float res = (sin(facrt)/facrt)/val0;
02125                 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02126                 return res;
02127         }
02128 }
02129 #endif // 0
02130 
02131 
02132 
02133 #define  circ(i)         circ[i-1]
02134 #define  numr(i,j)       numr[(j-1)*3 + i-1]
02135 #define  xim(i,j)        xim[(j-1)*nsam + i-1]
02136 
02137 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02138         int nsam = image->get_xsize();
02139         int nrow = image->get_ysize();
02140         int nring = numr.size()/3;
02141         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02142         EMData* out = new EMData();
02143         out->set_size(lcirc,1,1);
02144         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02145         float *xim  = image->get_data();
02146         float *circ = out->get_data();
02147 /*   alrq(image->get_data(), nsam, nrow, &numr[0], out->get_data(), lcirc, nring, cmode);
02148    return out;
02149 }
02150 void Util::alrq(float *xim,  int nsam , int nrow , int *numr,
02151           float *circ, int lcirc, int nring, char mode)
02152 {*/
02153 /*
02154 c
02155 c  purpose:
02156 c
02157 c  resmaple to polar coordinates
02158 c
02159 */
02160         //  dimension         xim(nsam,nrow),circ(lcirc)
02161         //  integer           numr(3,nring)
02162 
02163         double dfi, dpi;
02164         int    ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02165         float  yq, xold, yold, fi, x, y;
02166 
02167         ns2 = nsam/2+1;
02168         nr2 = nrow/2+1;
02169         dpi = 2.0*atan(1.0);
02170 
02171         for (i=1;i<=nring;i++) {
02172                 // radius of the ring
02173                 inr = numr(1,i);
02174                 yq  = static_cast<float>(inr);
02175                 l   = numr(3,i);
02176                 if (mode == 'h' || mode == 'H')  lt = l/2;
02177                 else                             lt = l/4;
02178 
02179                 nsim           = lt-1;
02180                 dfi            = dpi/(nsim+1);
02181                 kcirc          = numr(2,i);
02182                 xold           = 0.0f;
02183                 yold           = static_cast<float>(inr);
02184                 circ(kcirc)    = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02185                 xold           = static_cast<float>(inr);
02186                 yold           = 0.0f;
02187                 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02188 
02189                 if (mode == 'f' || mode == 'F') {
02190                         xold              = 0.0f;
02191                         yold              = static_cast<float>(-inr);
02192                         circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02193                         xold              = static_cast<float>(-inr);
02194                         yold              = 0.0f;
02195                         circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02196                 }
02197 
02198                 for (j=1;j<=nsim;j++) {
02199                         fi               = static_cast<float>(dfi*j);
02200                         x                = sin(fi)*yq;
02201                         y                = cos(fi)*yq;
02202                         xold             = x;
02203                         yold             = y;
02204                         circ(j+kcirc)    = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02205                         xold             =  y;
02206                         yold             = -x;
02207                         circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02208 
02209                         if (mode == 'f' || mode == 'F')  {
02210                                 xold                = -x;
02211                                 yold                = -y;
02212                                 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02213                                 xold                = -y;
02214                                 yold                =  x;
02215                                 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02216                         }
02217                 }
02218         }
02219         return  out;
02220 }
02221 
02222 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02223         int nsam = image->get_xsize();
02224         int nrow = image->get_ysize();
02225         int nring = numr.size()/3;
02226         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02227         EMData* out = new EMData();
02228         out->set_size(lcirc,1,1);
02229         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02230         float *xim  = image->get_data();
02231         float *circ = out->get_data();
02232         double dpi, dfi;
02233         int    it, jt, inr, l, nsim, kcirc, lt;
02234         float  xold, yold, fi, x, y;
02235 
02236         //     cns2 and cnr2 are predefined centers
02237         //     no need to set to zero, all elements are defined
02238         dpi = 2*atan(1.0);
02239         for (it=1; it<=nring; it++) {
02240                 // radius of the ring
02241                 inr = numr(1,it);
02242 
02243                 // "F" means a full circle interpolation
02244                 // "H" means a half circle interpolation
02245 
02246                 l = numr(3,it);
02247                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02248                 else                              lt = l / 4;
02249 
02250                 nsim  = lt - 1;
02251                 dfi   = dpi / (nsim+1);
02252                 kcirc = numr(2,it);
02253                 xold  = 0.0f+cns2;
02254                 yold  = inr+cnr2;
02255 
02256                 Assert( kcirc <= lcirc );
02257                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on 90 degree
02258 
02259                 xold  = inr+cns2;
02260                 yold  = 0.0f+cnr2;
02261                 Assert( lt+kcirc <= lcirc );
02262                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 0 degree
02263 
02264                 if ( mode == 'f' || mode == 'F' ) {
02265                         xold = 0.0f+cns2;
02266                         yold = -inr+cnr2;
02267                         Assert( lt+lt+kcirc <= lcirc );
02268                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 270 degree
02269 
02270                         xold = -inr+cns2;
02271                         yold = 0.0f+cnr2;
02272                         Assert(lt+lt+lt+kcirc <= lcirc );
02273                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on 180 degree
02274                 }
02275 
02276                 for (jt=1; jt<=nsim; jt++) {
02277                         fi   = static_cast<float>(dfi * jt);
02278                         x    = sin(fi) * inr;
02279                         y    = cos(fi) * inr;
02280 
02281                         xold = x+cns2;
02282                         yold = y+cnr2;
02283 
02284                         Assert( jt+kcirc <= lcirc );
02285                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);      // Sampling on the first quadrant
02286 
02287                         xold = y+cns2;
02288                         yold = -x+cnr2;
02289 
02290                         Assert( jt+lt+kcirc <= lcirc );
02291                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on the fourth quadrant
02292 
02293                         if ( mode == 'f' || mode == 'F' ) {
02294                                 xold = -x+cns2;
02295                                 yold = -y+cnr2;
02296 
02297                                 Assert( jt+lt+lt+kcirc <= lcirc );
02298                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on the third quadrant
02299 
02300                                 xold = -y+cns2;
02301                                 yold = x+cnr2;
02302 
02303                                 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02304                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on the second quadrant
02305                         }
02306                 } // end for jt
02307         } //end for it
02308         return out;
02309 }
02310 
02311 float Util::bilinear(float xold, float yold, int nsam, int nrow, float* xim)
02312 {
02313 /*
02314 c  purpose: linear interpolation
02315   Optimized for speed, circular closer removed, checking of ranges removed
02316 */
02317     float bilinear;
02318     int   ixold, iyold;
02319 
02320 /*
02321         float xdif, ydif, xrem, yrem;
02322         ixold   = (int) floor(xold);
02323         iyold   = (int) floor(yold);
02324         ydif = yold - iyold;
02325         yrem = 1.0f - ydif;
02326 
02327         //  May want to insert if?
02328 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02329 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02330 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02331         xdif = xold - ixold;
02332         xrem = 1.0f- xdif;
02333 //                 RBUF(K) = YDIF*(BUF(NADDR+NSAM)*XREM
02334 //     &                    +BUF(NADDR+NSAM+1)*XDIF)
02335 //     &                    +YREM*(BUF(NADDR)*XREM + BUF(NADDR+1)*XDIF)
02336         bilinear = ydif*(xim(ixold,iyold+1)*xrem + xim(ixold+1,iyold+1)*xdif) +
02337                                         yrem*(xim(ixold,iyold)*xrem+xim(ixold+1,iyold)*xdif);
02338 
02339     return bilinear;
02340 }
02341 */
02342         float xdif, ydif;
02343 
02344         ixold   = (int) xold;
02345         iyold   = (int) yold;
02346         ydif = yold - iyold;
02347 
02348         //  May want to insert it?
02349 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02350 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02351 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02352         xdif = xold - ixold;
02353         bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02354                    xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02355                            ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02356 
02357         return bilinear;
02358 }
02359 
02360 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02361              int  *numr, float *circ, int , int  nring, char  mode) {
02362         double dpi, dfi;
02363         int    it, jt, inr, l, nsim, kcirc, lt;
02364         float   xold, yold, fi, x, y;
02365 
02366         //     cns2 and cnr2 are predefined centers
02367         //     no need to set to zero, all elements are defined
02368 
02369         dpi = 2*atan(1.0);
02370         for (it=1; it<=nring; it++) {
02371                 // radius of the ring
02372                 inr = numr(1,it);
02373 
02374                 l = numr(3,it);
02375                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02376                 else                              lt = l / 4;
02377 
02378                 nsim  = lt - 1;
02379                 dfi   = dpi / (nsim+1);
02380                 kcirc = numr(2,it);
02381 
02382 
02383                 xold  = 0.0f+cns2;
02384                 yold  = inr+cnr2;
02385 
02386                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02387 
02388                 xold  = inr+cns2;
02389                 yold  = 0.0f+cnr2;
02390                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02391 
02392                 if ( mode == 'f' || mode == 'F' ) {
02393                         xold = 0.0f+cns2;
02394                         yold = -inr+cnr2;
02395                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02396 
02397                         xold = -inr+cns2;
02398                         yold = 0.0f+cnr2;
02399                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02400                 }
02401 
02402                 for (jt=1; jt<=nsim; jt++) {
02403                         fi   = static_cast<float>(dfi * jt);
02404                         x    = sin(fi) * inr;
02405                         y    = cos(fi) * inr;
02406 
02407                         xold = x+cns2;
02408                         yold = y+cnr2;
02409                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02410 
02411                         xold = y+cns2;
02412                         yold = -x+cnr2;
02413                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02414 
02415                         if ( mode == 'f' || mode == 'F' ) {
02416                                 xold = -x+cns2;
02417                                 yold = -y+cnr2;
02418                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02419 
02420                                 xold = -y+cns2;
02421                                 yold = x+cnr2;
02422                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02423                         }
02424                 } // end for jt
02425         } //end for it
02426 }
02427 /*
02428 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02429              int  *numr, float *circ, int lcirc, int  nring, char  mode)
02430 {
02431    double dpi, dfi;
02432    int    it, jt, inr, l, nsim, kcirc, lt, xold, yold;
02433    float  yq, fi, x, y;
02434 
02435    //     cns2 and cnr2 are predefined centers
02436    //     no need to set to zero, all elements are defined
02437 
02438    dpi = 2*atan(1.0);
02439    for (it=1; it<=nring; it++) {
02440       // radius of the ring
02441       inr = numr(1,it);
02442       yq  = inr;
02443 
02444       l = numr(3,it);
02445       if ( mode == 'h' || mode == 'H' ) {
02446          lt = l / 2;
02447       }
02448       else { // if ( mode == 'f' || mode == 'F' )
02449          lt = l / 4;
02450       }
02451 
02452       nsim  = lt - 1;
02453       dfi   = dpi / (nsim+1);
02454       kcirc = numr(2,it);
02455 
02456 
02457         xold = (int) (0.0+cns2);
02458         yold = (int) (inr+cnr2);
02459 
02460         circ(kcirc) = xim(xold, yold);
02461 
02462       xold = (int) (inr+cns2);
02463       yold = (int) (0.0+cnr2);
02464       circ(lt+kcirc) = xim(xold, yold);
02465 
02466       if ( mode == 'f' || mode == 'F' ) {
02467          xold  = (int) (0.0+cns2);
02468          yold = (int) (-inr+cnr2);
02469          circ(lt+lt+kcirc) = xim(xold, yold);
02470 
02471          xold  = (int) (-inr+cns2);
02472          yold = (int) (0.0+cnr2);
02473          circ(lt+lt+lt+kcirc) = xim(xold, yold);
02474       }
02475 
02476       for (jt=1; jt<=nsim; jt++) {
02477          fi   = dfi * jt;
02478          x    = sin(fi) * yq;
02479          y    = cos(fi) * yq;
02480 
02481          xold  = (int) (x+cns2);
02482          yold = (int) (y+cnr2);
02483          circ(jt+kcirc) = xim(xold, yold);
02484 
02485          xold  = (int) (y+cns2);
02486          yold = (int) (-x+cnr2);
02487          circ(jt+lt+kcirc) = xim(xold, yold);
02488 
02489          if ( mode == 'f' || mode == 'F' ) {
02490             xold  = (int) (-x+cns2);
02491             yold = (int) (-y+cnr2);
02492             circ(jt+lt+lt+kcirc) = xim(xold, yold);
02493 
02494             xold  = (int) (-y+cns2);
02495             yold = (int) (x+cnr2);
02496             circ(jt+lt+lt+lt+kcirc) = xim(xold, yold);
02497          }
02498       } // end for jt
02499    } //end for it
02500 }
02501 */
02502 //xim((int) floor(xold), (int) floor(yold))
02503 #undef  xim
02504 
02505 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02506 // input image is twice the size of the original image
02507         int nring = numr.size()/3;
02508         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02509         EMData* out = new EMData();
02510         out->set_size(lcirc,1,1);
02511         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02512         float *circ = out->get_data();
02513         float *fimage = image->get_data();
02514         int nx = image->get_xsize();
02515         int ny = image->get_ysize();
02516         int nz = image->get_zsize();
02517         double dpi, dfi;
02518         int    it, jt, inr, l, nsim, kcirc, lt;
02519         float  yq, xold, yold, fi, x, y;
02520 
02521         //     cns2 and cnr2 are predefined centers
02522         //     no need to set to zero, all elements are defined
02523 
02524         dpi = 2*atan(1.0);
02525         for (it=1;it<=nring;it++) {
02526                 // radius of the ring
02527                 inr = numr(1,it);
02528                 yq  = static_cast<float>(inr);
02529 
02530                 l = numr(3,it);
02531                 if ( mode == 'h' || mode == 'H' )  lt = l / 2;
02532                 else                               lt = l / 4;
02533 
02534                 nsim  = lt - 1;
02535                 dfi   = dpi / (nsim+1);
02536                 kcirc = numr(2,it);
02537                 xold  = 0.0f;
02538                 yold  = static_cast<float>(inr);
02539                 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02540 //      circ(kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02541 
02542                 xold  = static_cast<float>(inr);
02543                 yold  = 0.0f;
02544                 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02545 //      circ(lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02546 
02547         if ( mode == 'f' || mode == 'F' ) {
02548                 xold = 0.0f;
02549                 yold = static_cast<float>(-inr);
02550                 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02551 //         circ(lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02552 
02553                 xold = static_cast<float>(-inr);
02554                 yold = 0.0f;
02555                 circ(lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02556 //         circ(lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02557         }
02558 
02559         for (jt=1;jt<=nsim;jt++) {
02560                 fi   = static_cast<float>(dfi * jt);
02561                 x    = sin(fi) * yq;
02562                 y    = cos(fi) * yq;
02563 
02564                 xold = x;
02565                 yold = y;
02566                 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02567 //         circ(jt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02568 
02569                 xold = y;
02570                 yold = -x;
02571                 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02572 //         circ(jt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02573 
02574         if ( mode == 'f' || mode == 'F' ) {
02575                 xold = -x;
02576                 yold = -y;
02577                 circ(jt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02578 //            circ(jt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02579 
02580                 xold = -y;
02581                 yold = x;
02582                 circ(jt+lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02583 //            circ(jt+lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02584         }
02585         } // end for jt
02586         } //end for it
02587         return  out;
02588 }
02589 
02590 /*
02591 
02592         A set of 1-D power-of-two FFTs
02593         Pawel & Chao 01/20/06
02594 
02595 fftr_q(xcmplx,nv)
02596   single precision
02597 
02598  dimension xcmplx(2,iabs(nv)/2);
02599  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02600  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02601 
02602 
02603 fftr_d(xcmplx,nv)
02604   double precision
02605 
02606  dimension xcmplx(2,iabs(nv)/2);
02607  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02608  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02609 
02610 
02611 
02612 */
02613 #define  tab1(i)      tab1[i-1]
02614 #define  xcmplx(i,j)  xcmplx [(j-1)*2 + i-1]
02615 #define  br(i)        br[i-1]
02616 #define  bi(i)        bi[i-1]
02617 //-----------------------------------------
02618 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02619 {
02620         double rni,sgn,tr1,tr2,ti1,ti2;
02621         double cc,c,ss,s,t,x2,x3,x4,x5;
02622         int    b3,b4,b5,b6,b7,b56;
02623         int    n, k, l, j, i, ix0, ix1, status=0;
02624 
02625         const double tab1[] = {
02626                 9.58737990959775e-5,
02627                 1.91747597310703e-4,
02628                 3.83495187571395e-4,
02629                 7.66990318742704e-4,
02630                 1.53398018628476e-3,
02631                 3.06795676296598e-3,
02632                 6.13588464915449e-3,
02633                 1.22715382857199e-2,
02634                 2.45412285229123e-2,
02635                 4.90676743274181e-2,
02636                 9.80171403295604e-2,
02637                 1.95090322016128e-1,
02638                 3.82683432365090e-1,
02639                 7.07106781186546e-1,
02640                 1.00000000000000,
02641         };
02642 
02643         n=(int)pow(2.0f,ln);
02644 
02645         k=abs(ks);
02646         l=16-ln;
02647         b3=n*k;
02648         b6=b3;
02649         b7=k;
02650         if (ks > 0) {
02651                 sgn=1.0f;
02652         } else {
02653                 sgn=-1.0f;
02654                 rni=1.0f/(float)(n);
02655                 j=1;
02656                 for (i=1; i<=n; i++) {
02657                         br(j)=br(j)*rni;
02658                         bi(j)=bi(j)*rni;
02659                         j=j+k;
02660                 }
02661         }
02662 
02663 L12:
02664    b6=b6/2;
02665    b5=b6;
02666    b4=2*b6;
02667    b56=b5-b6;
02668 
02669 L14:
02670    tr1=br(b5+1);
02671    ti1=bi(b5+1);
02672    tr2=br(b56+1);
02673    ti2=bi(b56+1);
02674 
02675    br(b5+1)=tr2-tr1;
02676    bi(b5+1)=ti2-ti1;
02677    br(b56+1)=tr1+tr2;
02678    bi(b56+1)=ti1+ti2;
02679 
02680    b5=b5+b4;
02681    b56=b5-b6;
02682    if ( b5 <= b3 )  goto  L14;
02683    if ( b6 == b7 )  goto  L20;
02684 
02685    b4=b7;
02686    cc=2.0f*pow(tab1(l),2);
02687    c=1.0f-cc;
02688    l++;
02689    ss=sgn*tab1(l);
02690    s=ss;
02691 
02692 L16:
02693    b5=b6+b4;
02694    b4=2*b6;
02695    b56=b5-b6;
02696 
02697 L18:
02698    tr1=br(b5+1);
02699    ti1=bi(b5+1);
02700    tr2=br(b56+1);
02701    ti2=bi(b56+1);
02702    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02703    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02704    br(b56+1)=tr1+tr2;
02705    bi(b56+1)=ti1+ti2;
02706 
02707    b5=b5+b4;
02708    b56=b5-b6;
02709    if ( b5 <= b3 )  goto  L18;
02710    b4=b5-b6;
02711    b5=b4-b3;
02712    c=-c;
02713    b4=b6-b5;
02714    if ( b5 < b4 )  goto  L16;
02715    b4=b4+b7;
02716    if ( b4 >= b5 ) goto  L12;
02717 
02718    t=c-cc*c-ss*s;
02719    s=s+ss*c-cc*s;
02720    c=t;
02721    goto  L16;
02722 
02723 L20:
02724    ix0=b3/2;
02725    b3=b3-b7;
02726    b4=0;
02727    b5=0;
02728    b6=ix0;
02729    ix1=0;
02730    if (b6 == b7) goto EXIT;
02731 
02732 L22:
02733    b4=b3-b4;
02734    b5=b3-b5;
02735    x2=br(b4+1);
02736    x3=br(b5+1);
02737    x4=bi(b4+1);
02738    x5=bi(b5+1);
02739    br(b4+1)=x3;
02740    br(b5+1)=x2;
02741    bi(b4+1)=x5;
02742    bi(b5+1)=x4;
02743    if(b6 < b4)  goto  L22;
02744 
02745 L24:
02746    b4=b4+b7;
02747    b5=b6+b5;
02748    x2=br(b4+1);
02749    x3=br(b5+1);
02750    x4=bi(b4+1);
02751    x5=bi(b5+1);
02752    br(b4+1)=x3;
02753    br(b5+1)=x2;
02754    bi(b4+1)=x5;
02755    bi(b5+1)=x4;
02756    ix0=b6;
02757 
02758 L26:
02759    ix0=ix0/2;
02760    ix1=ix1-ix0;
02761    if( ix1 >= 0)  goto L26;
02762 
02763    ix0=2*ix0;
02764    b4=b4+b7;
02765    ix1=ix1+ix0;
02766    b5=ix1;
02767    if ( b5 >= b4)  goto  L22;
02768    if ( b4 < b6)   goto  L24;
02769 
02770 EXIT:
02771    status = 0;
02772 }
02773 
02774 // -----------------------------------------------------------------
02775 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02776 {
02777         //  dimension  br(1),bi(1)
02778 
02779         int b3,b4,b5,b6,b7,b56;
02780         int n, k, l, j, i, ix0, ix1;
02781         float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02782         int status=0;
02783 
02784         const float tab1[] = {
02785                 9.58737990959775e-5f,
02786                 1.91747597310703e-4f,
02787                 3.83495187571395e-4f,
02788                 7.66990318742704e-4f,
02789                 1.53398018628476e-3f,
02790                 3.06795676296598e-3f,
02791                 6.13588464915449e-3f,
02792                 1.22715382857199e-2f,
02793                 2.45412285229123e-2f,
02794                 4.90676743274181e-2f,
02795                 9.80171403295604e-2f,
02796                 1.95090322016128e-1f,
02797                 3.82683432365090e-1f,
02798                 7.07106781186546e-1f,
02799                 1.00000000000000f,
02800         };
02801 
02802         n=(int)pow(2.0f,ln);
02803 
02804         k=abs(ks);
02805         l=16-ln;
02806         b3=n*k;
02807         b6=b3;
02808         b7=k;
02809         if( ks > 0 ) {
02810                 sgn=1.0f;
02811         } else {
02812                 sgn=-1.0f;
02813                 rni=1.0f/(float)n;
02814                 j=1;
02815                 for (i=1; i<=n; i++) {
02816                         br(j)=br(j)*rni;
02817                         bi(j)=bi(j)*rni;
02818                         j=j+k;
02819                 }
02820         }
02821 L12:
02822    b6=b6/2;
02823    b5=b6;
02824    b4=2*b6;
02825    b56=b5-b6;
02826 L14:
02827    tr1=br(b5+1);
02828    ti1=bi(b5+1);
02829 
02830    tr2=br(b56+1);
02831    ti2=bi(b56+1);
02832 
02833    br(b5+1)=tr2-tr1;
02834    bi(b5+1)=ti2-ti1;
02835    br(b56+1)=tr1+tr2;
02836    bi(b56+1)=ti1+ti2;
02837 
02838    b5=b5+b4;
02839    b56=b5-b6;
02840    if ( b5 <= b3 )  goto  L14;
02841    if ( b6 == b7 )  goto  L20;
02842 
02843    b4=b7;
02844    cc=2.0f*pow(tab1(l),2);
02845    c=1.0f-cc;
02846    l++;
02847    ss=sgn*tab1(l);
02848    s=ss;
02849 L16:
02850    b5=b6+b4;
02851    b4=2*b6;
02852    b56=b5-b6;
02853 L18:
02854    tr1=br(b5+1);
02855    ti1=bi(b5+1);
02856    tr2=br(b56+1);
02857    ti2=bi(b56+1);
02858    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02859    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02860    br(b56+1)=tr1+tr2;
02861    bi(b56+1)=ti1+ti2;
02862 
02863    b5=b5+b4;
02864    b56=b5-b6;
02865    if(b5 <= b3)  goto L18;
02866    b4=b5-b6;
02867    b5=b4-b3;
02868    c=-c;
02869    b4=b6-b5;
02870    if(b5 < b4)  goto  L16;
02871    b4=b4+b7;
02872    if(b4 >= b5) goto  L12;
02873 
02874    t=c-cc*c-ss*s;
02875    s=s+ss*c-cc*s;
02876    c=t;
02877    goto  L16;
02878 L20:
02879    ix0=b3/2;
02880    b3=b3-b7;
02881    b4=0;
02882    b5=0;
02883    b6=ix0;
02884    ix1=0;
02885    if ( b6 == b7) goto EXIT;
02886 L22:
02887    b4=b3-b4;
02888    b5=b3-b5;
02889    x2=br(b4+1);
02890    x3=br(b5+1);
02891    x4=bi(b4+1);
02892    x5=bi(b5+1);
02893    br(b4+1)=x3;
02894    br(b5+1)=x2;
02895    bi(b4+1)=x5;
02896    bi(b5+1)=x4;
02897    if (b6 < b4) goto  L22;
02898 L24:
02899    b4=b4+b7;
02900    b5=b6+b5;
02901    x2=br(b4+1);
02902    x3=br(b5+1);
02903    x4=bi(b4+1);
02904    x5=bi(b5+1);
02905    br(b4+1)=x3;
02906    br(b5+1)=x2;
02907    bi(b4+1)=x5;
02908    bi(b5+1)=x4;
02909    ix0=b6;
02910 L26:
02911    ix0=ix0/2;
02912    ix1=ix1-ix0;
02913    if(ix1 >= 0)  goto  L26;
02914 
02915    ix0=2*ix0;
02916    b4=b4+b7;
02917    ix1=ix1+ix0;
02918    b5=ix1;
02919    if (b5 >= b4)  goto  L22;
02920    if (b4 < b6)   goto  L24;
02921 EXIT:
02922    status = 0;
02923 }
02924 
02925 void  Util::fftr_q(float *xcmplx, int nv)
02926 {
02927    // dimension xcmplx(2,1); xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02928 
02929         int nu, inv, nu1, n, isub, n2, i1, i2, i;
02930         float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02931 
02932         const float tab1[] = {
02933                 9.58737990959775e-5f,
02934                 1.91747597310703e-4f,
02935                 3.83495187571395e-4f,
02936                 7.66990318742704e-4f,
02937                 1.53398018628476e-3f,
02938                 3.06795676296598e-3f,
02939                 6.13588464915449e-3f,
02940                 1.22715382857199e-2f,
02941                 2.45412285229123e-2f,
02942                 4.90676743274181e-2f,
02943                 9.80171403295604e-2f,
02944                 1.95090322016128e-1f,
02945                 3.82683432365090e-1f,
02946                 7.07106781186546e-1f,
02947                 1.00000000000000f,
02948         };
02949 
02950         nu=abs(nv);
02951         inv=nv/nu;
02952         nu1=nu-1;
02953         n=(int)pow(2.f,nu1);
02954         isub=16-nu1;
02955 
02956         ss=-tab1(isub);
02957         cc=-2.0f*pow(tab1(isub-1),2.f);
02958         c=1.0f;
02959         s=0.0f;
02960         n2=n/2;
02961         if ( inv > 0) {
02962                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02963                 tr=xcmplx(1,1);
02964                 ti=xcmplx(2,1);
02965                 xcmplx(1,1)=tr+ti;
02966                 xcmplx(2,1)=tr-ti;
02967                 for (i=1;i<=n2;i++) {
02968                         i1=i+1;
02969                         i2=n-i+1;
02970                         tr1=xcmplx(1,i1);
02971                         tr2=xcmplx(1,i2);
02972                         ti1=xcmplx(2,i1);
02973                         ti2=xcmplx(2,i2);
02974                         t=(cc*c-ss*s)+c;
02975                         s=(cc*s+ss*c)+s;
02976                         c=t;
02977                         xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02978                         xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02979                         xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02980                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02981                 }
02982         } else {
02983                 tr=xcmplx(1,1);
02984                 ti=xcmplx(2,1);
02985                 xcmplx(1,1)=0.5f*(tr+ti);
02986                 xcmplx(2,1)=0.5f*(tr-ti);
02987                 for (i=1; i<=n2; i++) {
02988                         i1=i+1;
02989                         i2=n-i+1;
02990                         tr1=xcmplx(1,i1);
02991                         tr2=xcmplx(1,i2);
02992                         ti1=xcmplx(2,i1);
02993                         ti2=xcmplx(2,i2);
02994                         t=(cc*c-ss*s)+c;
02995                         s=(cc*s+ss*c)+s;
02996                         c=t;
02997                         xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
02998                         xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
02999                         xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03000                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03001                 }
03002                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03003         }
03004 }
03005 
03006 // -------------------------------------------
03007 void  Util::fftr_d(double *xcmplx, int nv)
03008 {
03009         // double precision  x(2,1)
03010         int    i1, i2,  nu, inv, nu1, n, isub, n2, i;
03011         double tr1,tr2,ti1,ti2,tr,ti;
03012         double cc,c,ss,s,t;
03013         const double tab1[] = {
03014                 9.58737990959775e-5,
03015                 1.91747597310703e-4,
03016                 3.83495187571395e-4,
03017                 7.66990318742704e-4,
03018                 1.53398018628476e-3,
03019                 3.06795676296598e-3,
03020                 6.13588464915449e-3,
03021                 1.22715382857199e-2,
03022                 2.45412285229123e-2,
03023                 4.90676743274181e-2,
03024                 9.80171403295604e-2,
03025                 1.95090322016128e-1,
03026                 3.82683432365090e-1,
03027                 7.07106781186546e-1,
03028                 1.00000000000000,
03029         };
03030 
03031         nu=abs(nv);
03032         inv=nv/nu;
03033         nu1=nu-1;
03034         n=(int)pow(2.0f,nu1);
03035         isub=16-nu1;
03036         ss=-tab1(isub);
03037         cc=-2.0*pow(tab1(isub-1),2);
03038         c=1.0f;
03039         s=0.0f;
03040         n2=n/2;
03041 
03042         if ( inv > 0 ) {
03043                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03044                 tr=xcmplx(1,1);
03045                 ti=xcmplx(2,1);
03046                 xcmplx(1,1)=tr+ti;
03047                 xcmplx(2,1)=tr-ti;
03048                 for (i=1;i<=n2;i++) {
03049                         i1=i+1;
03050                         i2=n-i+1;
03051                         tr1=xcmplx(1,i1);
03052                         tr2=xcmplx(1,i2);
03053                         ti1=xcmplx(2,i1);
03054                         ti2=xcmplx(2,i2);
03055                         t=(cc*c-ss*s)+c;
03056                         s=(cc*s+ss*c)+s;
03057                         c=t;
03058                         xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03059                         xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03060                         xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03061                         xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03062                 }
03063         } else {
03064                 tr=xcmplx(1,1);
03065                 ti=xcmplx(2,1);
03066                 xcmplx(1,1)=0.5*(tr+ti);
03067                 xcmplx(2,1)=0.5*(tr-ti);
03068                 for (i=1; i<=n2; i++) {
03069                         i1=i+1;
03070                         i2=n-i+1;
03071                         tr1=xcmplx(1,i1);
03072                         tr2=xcmplx(1,i2);
03073                         ti1=xcmplx(2,i1);
03074                         ti2=xcmplx(2,i2);
03075                         t=(cc*c-ss*s)+c;
03076                         s=(cc*s+ss*c)+s;
03077                         c=t;
03078                         xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03079                         xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03080                         xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03081                         xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03082                 }
03083                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03084         }
03085 }
03086 #undef  tab1
03087 #undef  xcmplx
03088 #undef  br
03089 #undef  bi
03090 
03091 
03092 void Util::Frngs(EMData* circp, vector<int> numr){
03093         int nring = numr.size()/3;
03094         float *circ = circp->get_data();
03095         int i, l;
03096         for (i=1; i<=nring;i++) {
03097 
03098 #ifdef _WIN32
03099                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03100 #else
03101                 l=(int)(log2(numr(3,i)));
03102 #endif  //_WIN32
03103 
03104                 fftr_q(&circ(numr(2,i)),l);
03105         }
03106 }
03107 
03108 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03109         int nring = numr.size()/3;
03110         float *circ = circp->get_data();
03111         int i, l;
03112         for (i=1; i<=nring;i++) {
03113 
03114 #ifdef _WIN32
03115                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03116 #else
03117                 l=(int)(log2(numr(3,i)));
03118 #endif  //_WIN32
03119 
03120                 fftr_q(&circ(numr(2,i)),-l);
03121         }
03122 }
03123 #undef  circ
03124 
03125 #define  b(i)            b[i-1]
03126 void Util::prb1d(double *b, int npoint, float *pos) {
03127         double  c2,c3;
03128         int     nhalf;
03129 
03130         nhalf = npoint/2 + 1;
03131         *pos  = 0.0;
03132 
03133         if (npoint == 7) {
03134                 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03135                      - 6.*b(6) + 31.*b(7);
03136                 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03137         }
03138         else if (npoint == 5) {
03139                 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03140                    + 46.*b(5) ) / (-70.);
03141                 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03142         }
03143         else if (npoint == 3) {
03144                 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03145                 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03146         }
03147         //else if (npoint == 9) {
03148         else  { // at least one has to be true!!
03149                 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03150                      - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03151                      + 1092.*b(9) ) / (-4620.);
03152                 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03153                      - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03154         }
03155         if (c3 != 0.0)  *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03156 }
03157 #undef  b
03158 
03159 #define  circ1(i)        circ1[i-1]
03160 #define  circ2(i)        circ2[i-1]
03161 #define  t(i)            t[i-1]
03162 #define  q(i)            q[i-1]
03163 #define  b(i)            b[i-1]
03164 #define  t7(i)           t7[i-1]
03165 Dict Util::Crosrng_e(EMData*  circ1p, EMData* circ2p, vector<int> numr, int neg) {
03166         //  neg = 0 straight,  neg = 1 mirrored
03167         int nring = numr.size()/3;
03168         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03169         int maxrin = numr[numr.size()-1];
03170         double qn;   float  tot;
03171         float *circ1 = circ1p->get_data();
03172         float *circ2 = circ2p->get_data();
03173 /*
03174 c checks single position, neg is flag for checking mirrored position
03175 c
03176 c  input - fourier transforms of rings!
03177 c  first set is conjugated (mirrored) if neg
03178 c  circ1 already multiplied by weights!
03179 c       automatic arrays
03180         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03181         double precision  q(maxrin)
03182         double precision  t7(-3:3)
03183 */
03184         float *t;
03185         double t7[7], *q;
03186         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03187         float  pos;
03188 
03189 #ifdef _WIN32
03190         ip = -(int)(log((float)maxrin)/log(2.0f));
03191 #else
03192         ip = -(int) (log2(maxrin));
03193 #endif  //_WIN32
03194 
03195         q = (double*)calloc(maxrin, sizeof(double));
03196         t = (float*)calloc(maxrin, sizeof(float));
03197 
03198 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03199         for (i=1; i<=nring; i++) {
03200                 numr3i = numr(3,i);
03201                 numr2i = numr(2,i);
03202 
03203                 t(1) = (circ1(numr2i)) * circ2(numr2i);
03204 
03205                 if (numr3i != maxrin) {
03206                          // test .ne. first for speed on some compilers
03207                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03208                         t(2)            = 0.0;
03209 
03210                         if (neg) {
03211                                 // first set is conjugated (mirrored)
03212                                 for (j=3;j<=numr3i;j=j+2) {
03213                                         jc = j+numr2i-1;
03214                                         t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03215                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03216                                 }
03217                         } else {
03218                                 for (j=3;j<=numr3i;j=j+2) {
03219                                         jc = j+numr2i-1;
03220                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03221                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03222                                 }
03223                         }
03224                         for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03225                 } else {
03226                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03227                         if (neg) {
03228                                 // first set is conjugated (mirrored)
03229                                 for (j=3;j<=maxrin;j=j+2) {
03230                                         jc = j+numr2i-1;
03231                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03232                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03233                                 }
03234                         } else {
03235                                 for (j=3;j<=maxrin;j=j+2) {
03236                                         jc = j+numr2i-1;
03237                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03238                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03239                                 }
03240                         }
03241                         for (j = 1; j <= maxrin; j++) q(j) += t(j);
03242                 }
03243         }
03244 
03245         fftr_d(q,ip);
03246 
03247         qn = -1.0e20;
03248         for (j=1;j<=maxrin;j++) {
03249            if (q(j) >= qn) {
03250                   qn = q(j); jtot = j;
03251            }
03252         }
03253 
03254         for (k=-3; k<=3; k++) {
03255                 j = (jtot+k+maxrin-1)%maxrin + 1;
03256                 t7(k+4) = q(j);
03257         }
03258 
03259         prb1d(t7,7,&pos);
03260 
03261         tot = (float)jtot + pos;
03262 
03263         if (q) free(q);
03264         if (t) free(t);
03265 
03266         Dict retvals;
03267         retvals["qn"] = qn;
03268         retvals["tot"] = tot;
03269         return  retvals;
03270 }
03271 
03272 Dict Util::Crosrng_ew(EMData*  circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03273    //  neg = 0 straight,  neg = 1 mirrored
03274         int nring = numr.size()/3;
03275         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03276         int maxrin = numr[numr.size()-1];
03277         double qn;   float  tot;
03278         float *circ1 = circ1p->get_data();
03279         float *circ2 = circ2p->get_data();
03280 /*
03281 c checks single position, neg is flag for checking mirrored position
03282 c
03283 c  input - fourier transforms of rings!
03284 c  first set is conjugated (mirrored) if neg
03285 c  multiplication by weights!
03286 c       automatic arrays
03287         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03288         double precision  q(maxrin)
03289         double precision  t7(-3:3)
03290 */
03291         float *t;
03292         double t7[7], *q;
03293         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03294         float  pos;
03295 
03296 #ifdef _WIN32
03297         ip = -(int)(log((float)maxrin)/log(2.0f));
03298 #else
03299         ip = -(int) (log2(maxrin));
03300 #endif  //_WIN32
03301 
03302         q = (double*)calloc(maxrin, sizeof(double));
03303         t = (float*)calloc(maxrin, sizeof(float));
03304 
03305 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03306         for (i=1;i<=nring;i++) {
03307                 numr3i = numr(3,i);
03308                 numr2i = numr(2,i);
03309 
03310                 t(1) = circ1(numr2i) * circ2(numr2i);
03311 
03312                 if (numr3i != maxrin) {
03313                         // test .ne. first for speed on some compilers
03314                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03315                         t(2)      = 0.0;
03316 
03317                         if (neg) {
03318                                 // first set is conjugated (mirrored)
03319                                 for (j=3; j<=numr3i; j=j+2) {
03320                                         jc = j+numr2i-1;
03321                                         t(j)   =  (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03322                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03323                                 }
03324                         } else {
03325                                 for (j=3; j<=numr3i; j=j+2) {
03326                                         jc = j+numr2i-1;
03327                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03328                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03329                                 }
03330                         }
03331                         for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03332                 } else {
03333                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03334                         if (neg) {
03335                                 // first set is conjugated (mirrored)
03336                                 for (j=3; j<=maxrin; j=j+2) {
03337                                         jc = j+numr2i-1;
03338                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03339                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03340                                 }
03341                         } else {
03342                                 for (j=3; j<=maxrin; j=j+2) {
03343                                 jc = j+numr2i-1;
03344                                 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03345                                 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03346                                 }
03347                         }
03348                         for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03349                 }
03350         }
03351 
03352         fftr_d(q,ip);
03353 
03354         qn = -1.0e20;
03355         for (j=1;j<=maxrin;j++) {
03356                 //cout << j << "  " << q(j) << endl;
03357                 if (q(j) >= qn) {
03358                         qn = q(j);
03359                         jtot = j;
03360                 }
03361         }
03362 
03363         for (k=-3; k<=3; k++) {
03364                 j = (jtot+k+maxrin-1)%maxrin + 1;
03365                 t7(k+4) = q(j);
03366         }
03367 
03368         prb1d(t7,7,&pos);
03369 
03370         tot = (float)jtot + pos;
03371 
03372         //if (q) free(q);
03373         if (t) free(t);
03374 
03375         Dict retvals;
03376         //tot = 1;
03377         //qn = q(1);
03378         retvals["qn"] = qn;
03379         retvals["tot"] = tot;
03380 
03381         if (q) free(q);
03382 
03383         return  retvals;
03384 }
03385 
03386 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03387         int nring = numr.size()/3;
03388         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03389         int maxrin = numr[numr.size()-1];
03390         double qn; float tot; double qm; float tmt;
03391         float *circ1 = circ1p->get_data();
03392         float *circ2 = circ2p->get_data();
03393 /*
03394 c
03395 c  checks both straight & mirrored positions
03396 c
03397 c  input - fourier transforms of rings!!
03398 c  circ1 already multiplied by weights!
03399 c
03400 */
03401 
03402         // dimension             circ1(lcirc),circ2(lcirc)
03403 
03404         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03405         double *t, *q, t7[7];
03406 
03407         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03408         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03409 
03410         qn  = 0.0f;
03411         qm  = 0.0f;
03412         tot = 0.0f;
03413         tmt = 0.0f;
03414 #ifdef _WIN32
03415         ip = -(int)(log((float)maxrin)/log(2.0f));
03416 #else
03417         ip = -(int)(log2(maxrin));
03418 #endif  //_WIN32
03419   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03420 
03421         //  c - straight  = circ1 * conjg(circ2)
03422         //  zero q array
03423 
03424         q = (double*)calloc(maxrin,sizeof(double));
03425 
03426         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03427         //   zero t array
03428         t = (double*)calloc(maxrin,sizeof(double));
03429 
03430    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03431         for (i=1; i<=nring; i++) {
03432 
03433                 numr3i = numr(3,i);   // Number of samples of this ring
03434                 numr2i = numr(2,i);   // The beginning point of this ring
03435 
03436                 t1   = circ1(numr2i) * circ2(numr2i);
03437                 q(1) += t1;
03438                 t(1) += t1;
03439 
03440                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03441                 if (numr3i == maxrin)  {
03442                         q(2) += t1;
03443                         t(2) += t1;
03444                 } else {
03445                         q(numr3i+1) += t1;
03446                         t(numr3i+1) += t1;
03447                 }
03448 
03449                 for (j=3; j<=numr3i; j += 2) {
03450                         jc     = j+numr2i-1;
03451 
03452 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03453 //                                ----- -----    ----- -----
03454 //                                 t1     t2      t3    t4
03455 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03456 //                                    ----- -----    ----- -----
03457 //                                     t1    t2       t3    t4
03458 
03459                         c1     = circ1(jc);
03460                         c2     = circ1(jc+1);
03461                         d1     = circ2(jc);
03462                         d2     = circ2(jc+1);
03463 
03464                         t1     = c1 * d1;
03465                         t2     = c2 * d2;
03466                         t3     = c1 * d2;
03467                         t4     = c2 * d1;
03468 
03469                         q(j)   += t1 + t2;
03470                         q(j+1) += -t3 + t4;
03471                         t(j)   += t1 - t2;
03472                         t(j+1) += -t3 - t4;
03473                 }
03474         }
03475         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03476         fftr_d(q,ip);
03477 
03478         qn  = -1.0e20;
03479         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03480                 if (q(j) >= qn) {
03481                         qn  = q(j);
03482                         jtot = j;
03483                 }
03484         }
03485 
03486         for (k=-3; k<=3; k++) {
03487                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03488                 t7(k+4) = q(j);
03489         }
03490 
03491         // interpolate
03492         prb1d(t7,7,&pos);
03493         tot = (float)(jtot)+pos;
03494         // Do not interpolate
03495         //tot = (float)(jtot);
03496 
03497         // mirrored
03498         fftr_d(t,ip);
03499 
03500         // find angle
03501         qm = -1.0e20;
03502         for (j=1; j<=maxrin;j++) {//cout <<"  "<<j<<"   "<<t(j) <<endl;
03503                 if ( t(j) >= qm ) {
03504                         qm   = t(j);
03505                         jtot = j;
03506                 }
03507         }
03508 
03509         for (k=-3; k<=3; k++)  {
03510                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03511                 t7(k+4) = t(j);
03512         }
03513 
03514         // interpolate
03515 
03516         prb1d(t7,7,&pos);
03517         tmt = float(jtot) + pos;
03518         // Do not interpolate
03519         //tmt = float(jtot);
03520 
03521         free(t);
03522         free(q);
03523 
03524         Dict retvals;
03525         retvals["qn"] = qn;
03526         retvals["tot"] = tot;
03527         retvals["qm"] = qm;
03528         retvals["tmt"] = tmt;
03529         return retvals;
03530 }
03531 
03532 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03533         int nring = numr.size()/3;
03534         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03535         int maxrin = numr[numr.size()-1];
03536         double qn; float tot; double qm; float tmt;
03537         float *circ1 = circ1p->get_data();
03538         float *circ2 = circ2p->get_data();
03539 /*
03540 c
03541 c  checks both straight & mirrored positions
03542 c
03543 c  input - fourier transforms of rings!!
03544 c  circ1 already multiplied by weights!
03545 c
03546 */
03547 
03548         // dimension             circ1(lcirc),circ2(lcirc)
03549 
03550         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03551         double *t, *q;
03552 
03553         int   ip, jc, numr3i, numr2i, i, j, jtot = 0;
03554         float t1, t2, t3, t4, c1, c2, d1, d2;
03555 
03556         qn  = 0.0f;
03557         qm  = 0.0f;
03558         tot = 0.0f;
03559         tmt = 0.0f;
03560 #ifdef _WIN32
03561         ip = -(int)(log((float)maxrin)/log(2.0f));
03562 #else
03563         ip = -(int)(log2(maxrin));
03564 #endif  //_WIN32
03565   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03566 
03567         //  c - straight  = circ1 * conjg(circ2)
03568         //  zero q array
03569 
03570         q = (double*)calloc(maxrin,sizeof(double));
03571 
03572         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03573         //   zero t array
03574         t = (double*)calloc(maxrin,sizeof(double));
03575 
03576    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03577         for (i=1; i<=nring; i++) {
03578 
03579                 numr3i = numr(3,i);   // Number of samples of this ring
03580                 numr2i = numr(2,i);   // The beginning point of this ring
03581 
03582                 t1   = circ1(numr2i) * circ2(numr2i);
03583                 q(1) += t1;
03584                 t(1) += t1;
03585 
03586                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03587                 if (numr3i == maxrin)  {
03588                         q(2) += t1;
03589                         t(2) += t1;
03590                 } else {
03591                         q(numr3i+1) += t1;
03592                         t(numr3i+1) += t1;
03593                 }
03594 
03595                 for (j=3; j<=numr3i; j += 2) {
03596                         jc     = j+numr2i-1;
03597 
03598 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03599 //                                ----- -----    ----- -----
03600 //                                 t1     t2      t3    t4
03601 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03602 //                                    ----- -----    ----- -----
03603 //                                     t1    t2       t3    t4
03604 
03605                         c1     = circ1(jc);
03606                         c2     = circ1(jc+1);
03607                         d1     = circ2(jc);
03608                         d2     = circ2(jc+1);
03609 
03610                         t1     = c1 * d1;
03611                         t2     = c2 * d2;
03612                         t3     = c1 * d2;
03613                         t4     = c2 * d1;
03614 
03615                         q(j)   += t1 + t2;
03616                         q(j+1) += -t3 + t4;
03617                         t(j)   += t1 - t2;
03618                         t(j+1) += -t3 - t4;
03619                 }
03620         }
03621         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03622         fftr_d(q,ip);
03623 
03624         qn  = -1.0e20;
03625 
03626         int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03627         int jstep = static_cast<int>(delta/360.0*maxrin);
03628         if (jstep < 1) { jstep = 1; }
03629 
03630         for (j=jstart; j<=maxrin; j+=jstep) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03631                 if (q(j) >= qn) {
03632                         qn  = q(j);
03633                         jtot = j;
03634                 }
03635         }
03636 
03637         //for (k=-3; k<=3; k++) {
03638         //      j = ((jtot+k+maxrin-1)%maxrin)+1;
03639         //      t7(k+4) = q(j);
03640         //}
03641 
03642         // interpolate
03643         //prb1d(t7,7,&pos);
03644         //tot = (float)(jtot)+pos;
03645         // Do not interpolate
03646         tot = (float)(jtot);
03647 
03648         // mirrored
03649         fftr_d(t,ip);
03650 
03651         // find angle
03652         qm = -1.0e20;
03653         for (j=jstart; j<=maxrin;j+=jstep) {//cout <<"  "<<j<<" "<<t(j) <<endl;
03654                 if ( t(j) >= qm ) {
03655                         qm   = t(j);
03656                         jtot = j;
03657                 }
03658         }
03659 
03660         //for (k=-3; k<=3; k++)  {
03661         //      j = ((jtot+k+maxrin-1)%maxrin) + 1;
03662         //      t7(k+4) = t(j);
03663         //}
03664 
03665         // interpolate
03666 
03667         //prb1d(t7,7,&pos);
03668         //tmt = float(jtot) + pos;
03669         // Do not interpolate
03670         tmt = float(jtot);
03671 
03672         free(t);
03673         free(q);
03674 
03675         Dict retvals;
03676         retvals["qn"] = qn;
03677         retvals["tot"] = tot;
03678         retvals["qm"] = qm;
03679         retvals["tmt"] = tmt;
03680         return retvals;
03681 }
03682 
03683 
03684 Dict Util::Crosrng_psi_0_180(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03685         int nring = numr.size()/3;
03686         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03687         int maxrin = numr[numr.size()-1];
03688         double qn; float tot; double qm; float tmt;
03689         float *circ1 = circ1p->get_data();
03690         float *circ2 = circ2p->get_data();
03691 
03692         // dimension             circ1(lcirc),circ2(lcirc)
03693 
03694         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03695         double *t, *q, t7[7];
03696 
03697         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03698         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03699 
03700         qn  = 0.0f;
03701         qm  = 0.0f;
03702         tot = 0.0f;
03703         tmt = 0.0f;
03704 #ifdef _WIN32
03705         ip = -(int)(log((float)maxrin)/log(2.0f));
03706 #else
03707         ip = -(int)(log2(maxrin));
03708 #endif  //_WIN32
03709   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03710 
03711         //  c - straight  = circ1 * conjg(circ2)
03712         //  zero q array
03713 
03714         q = (double*)calloc(maxrin,sizeof(double));
03715 
03716         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03717         //   zero t array
03718         t = (double*)calloc(maxrin,sizeof(double));
03719 
03720    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03721         for (i=1; i<=nring; i++) {
03722 
03723                 numr3i = numr(3,i);   // Number of samples of this ring
03724                 numr2i = numr(2,i);   // The beginning point of this ring
03725 
03726                 t1   = circ1(numr2i) * circ2(numr2i);
03727                 q(1) += t1;
03728                 t(1) += t1;
03729 
03730                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03731                 if (numr3i == maxrin)  {
03732                         q(2) += t1;
03733                         t(2) += t1;
03734                 } else {
03735                         q(numr3i+1) += t1;
03736                         t(numr3i+1) += t1;
03737                 }
03738 
03739                 for (j=3; j<=numr3i; j += 2) {
03740                         jc     = j+numr2i-1;
03741 
03742 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03743 //                                ----- -----    ----- -----
03744 //                                 t1     t2      t3    t4
03745 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03746 //                                    ----- -----    ----- -----
03747 //                                     t1    t2       t3    t4
03748 
03749                         c1     = circ1(jc);
03750                         c2     = circ1(jc+1);
03751                         d1     = circ2(jc);
03752                         d2     = circ2(jc+1);
03753 
03754                         t1     = c1 * d1;
03755                         t2     = c2 * d2;
03756                         t3     = c1 * d2;
03757                         t4     = c2 * d1;
03758 
03759                         q(j)   += t1 + t2;
03760                         q(j+1) += -t3 + t4;
03761                         t(j)   += t1 - t2;
03762                         t(j+1) += -t3 - t4;
03763                 }
03764         }
03765         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03766         fftr_d(q,ip);
03767 
03768         int psi_range  = int(psi_max/360.0*maxrin+0.5);
03769         const int psi_0 = 0;
03770         int psi_180    = int(  180.0/360.0*maxrin+0.5);
03771 
03772         qn  = -1.0e20;
03773         for (k=-psi_range; k<=psi_range; k++) {
03774                 j = (k+psi_0+maxrin-1)%maxrin+1;//string modemo = "f";cout <<" 90   "<<j<<"   "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03775                 if (q(j) >= qn) {
03776                         qn  = q(j);
03777                         jtot = j;
03778                 }
03779         }
03780 
03781         for (k=-psi_range; k<=psi_range; k++) {
03782                 j = (k+psi_180+maxrin-1)%maxrin+1; //cout <<" 270  "<<j<<"  "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03783                 if (q(j) >= qn) {
03784                         qn  = q(j);
03785                         jtot = j;
03786                 }
03787         }
03788 
03789         for (k=-3; k<=3; k++) {
03790                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03791                 t7(k+4) = q(j);
03792         }
03793 
03794         // interpolate
03795         prb1d(t7,7,&pos);
03796         tot = (float)(jtot)+pos;
03797         // Do not interpolate
03798         //tot = (float)(jtot);
03799 
03800         // mirrored
03801         fftr_d(t,ip);
03802 
03803         // find angle
03804         qm = -1.0e20;
03805         for (k=-psi_range; k<=psi_range; k++) {
03806                 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;
03807                 if (t(j) >= qm) {
03808                         qm  = t(j);
03809                         jtot = j;
03810                 }
03811         }
03812 
03813         for (k=-psi_range; k<=psi_range; k++) {
03814                 j = (k+psi_180+maxrin-1)%maxrin+1; //cout <<" 270  "<<j<<"  "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03815                 if (t(j) >= qm) {
03816                         qm  = t(j);
03817                         jtot = j;
03818                 }
03819         }
03820 
03821         for (k=-3; k<=3; k++)  {
03822                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03823                 t7(k+4) = t(j);
03824         }
03825 
03826         // interpolate
03827 
03828         prb1d(t7,7,&pos);
03829         tmt = float(jtot) + pos;
03830         // Do not interpolate
03831         //tmt = float(jtot);
03832 
03833         free(t);
03834         free(q);
03835 
03836         Dict retvals;
03837         retvals["qn"] = qn;
03838         retvals["tot"] = tot;
03839         retvals["qm"] = qm;
03840         retvals["tmt"] = tmt;
03841         return retvals;
03842 }
03843 
03844 
03845 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag) {
03846 // flag 0 - straignt, 1 - mirror
03847 
03848         int nring = numr.size()/3;
03849         int maxrin = numr[numr.size()-1];
03850         double qn; float tot; double qm; float tmt;
03851         float *circ1 = circ1p->get_data();
03852         float *circ2 = circ2p->get_data();
03853 
03854         double *q, t7[7];
03855 
03856         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03857         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03858 
03859         qn  = 0.0f;
03860         qm  = 0.0f;
03861         tot = 0.0f;
03862         tmt = 0.0f;
03863 #ifdef _WIN32
03864         ip = -(int)(log((float)maxrin)/log(2.0f));
03865 #else
03866         ip = -(int)(log2(maxrin));
03867 #endif  //_WIN32
03868 
03869         //  c - straight  = circ1 * conjg(circ2)
03870         //  zero q array
03871 
03872         q = (double*)calloc(maxrin,sizeof(double));
03873 
03874    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03875         if (flag==0) {
03876                 for (i=1; i<=nring; i++) {
03877 
03878                         numr3i = numr(3,i);   // Number of samples of this ring
03879                         numr2i = numr(2,i);   // The beginning point of this ring
03880 
03881                         t1   = circ1(numr2i) * circ2(numr2i);
03882                         q(1) += t1;
03883 
03884                         t1   = circ1(numr2i+1) * circ2(numr2i+1);
03885                         if (numr3i == maxrin)  {
03886                                 q(2) += t1;
03887                         } else {
03888                                 q(numr3i+1) += t1;
03889                         }
03890 
03891                         for (j=3; j<=numr3i; j += 2) {
03892                                 jc     = j+numr2i-1;
03893 
03894         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03895         //                                ----- -----    ----- -----
03896         //                                 t1     t2      t3    t4
03897 
03898                                 c1     = circ1(jc);
03899                                 c2     = circ1(jc+1);
03900                                 d1     = circ2(jc);
03901                                 d2     = circ2(jc+1);
03902 
03903                                 t1     = c1 * d1;
03904                                 t3     = c1 * d2;
03905                                 t2     = c2 * d2;
03906                                 t4     = c2 * d1;
03907 
03908                                 q(j)   += t1 + t2;
03909                                 q(j+1) += -t3 + t4;
03910                         }
03911                 }
03912         } else {
03913                 for (i=1; i<=nring; i++) {
03914 
03915                         numr3i = numr(3,i);   // Number of samples of this ring
03916                         numr2i = numr(2,i);   // The beginning point of this ring
03917 
03918                         t1   = circ1(numr2i) * circ2(numr2i);
03919                         q(1) += t1;
03920 
03921                         t1   = circ1(numr2i+1) * circ2(numr2i+1);
03922                         if (numr3i == maxrin)  {
03923                                 q(2) += t1;
03924                         } else {
03925                                 q(numr3i+1) += t1;
03926                         }
03927 
03928                         for (j=3; j<=numr3i; j += 2) {
03929                                 jc     = j+numr2i-1;
03930 
03931         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03932         //                                ----- -----    ----- -----
03933         //                                 t1     t2      t3    t4
03934 
03935                                 c1     = circ1(jc);
03936                                 c2     = circ1(jc+1);
03937                                 d1     = circ2(jc);
03938                                 d2     = circ2(jc+1);
03939 
03940                                 t1     = c1 * d1;
03941                                 t3     = c1 * d2;
03942                                 t2     = c2 * d2;
03943                                 t4     = c2 * d1;
03944 
03945                                 q(j)   += t1 - t2;
03946                                 q(j+1) += -t3 - t4;
03947                         }
03948                 }
03949         }
03950         fftr_d(q,ip);
03951 
03952         qn  = -1.0e20;
03953         int psi_pos = int(psi/360.0*maxrin+0.5);
03954 
03955         for (k=-5; k<=5; k++) {
03956                 j = (psi_pos+maxrin-1)%maxrin+1;
03957                 if (q(j) >= qn) {
03958                         qn  = q(j);
03959                         jtot = j;
03960                 }
03961         }
03962 
03963         for (k=-3; k<=3; k++) {
03964                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03965                 t7(k+4) = q(j);
03966         }
03967 
03968         // interpolate
03969         prb1d(t7,7,&pos);
03970         tot = (float)(jtot)+pos;
03971         free(q);
03972 
03973         Dict retvals;
03974         retvals["qn"] = qn;
03975         retvals["tot"] = tot;
03976         return retvals;
03977 }
03978 
03979 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03980         int nring = numr.size()/3;
03981         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03982         int maxrin = numr[numr.size()-1];
03983         double qn; float tot;
03984         float *circ1 = circ1p->get_data();
03985         float *circ2 = circ2p->get_data();
03986 /*
03987 c
03988 c  checks only straight position
03989 c
03990 c  input - fourier transforms of rings!!
03991 c  circ1 already multiplied by weights!
03992 c
03993 */
03994 
03995         // dimension             circ1(lcirc),circ2(lcirc)
03996 
03997         // q(maxrin), t7(-3:3)  //maxrin+2 removed
03998         double *q, t7[7];
03999 
04000         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04001         float c1, c2, d1, d2, pos;
04002 
04003         qn  = 0.0;
04004         tot = 0.0;
04005 #ifdef _WIN32
04006         ip = -(int)(log((float)maxrin)/log(2.0f));
04007 #else
04008    ip = -(int)(log2(maxrin));
04009 #endif  //_WIN32
04010         //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
04011 
04012         //  c - straight  = circ1 * conjg(circ2)
04013         //  zero q array
04014 
04015         q = (double*)calloc(maxrin,sizeof(double));
04016 
04017                         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04018         for (i=1; i<=nring; i++) {
04019 
04020                 numr3i = numr(3,i);   // Number of samples of this ring
04021                 numr2i = numr(2,i);   // The beginning point of this ring
04022 
04023                 q(1) += circ1(numr2i) * circ2(numr2i);
04024 
04025                 if (numr3i == maxrin)   q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04026                 else  q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04027 
04028                 for (j=3; j<=numr3i; j += 2) {
04029                         jc     = j+numr2i-1;
04030 
04031 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
04032 //                                ----- -----    ----- -----
04033 //                                 t1     t2      t3    t4
04034 
04035                         c1     = circ1(jc);
04036                         c2     = circ1(jc+1);
04037                         d1     = circ2(jc);
04038                         d2     = circ2(jc+1);
04039 
04040                         q(j)   += c1 * d1 + c2 * d2;
04041                         q(j+1) += -c1 * d2 + c2 * d1;
04042                 }
04043         }
04044 //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<endl;
04045         fftr_d(q,ip);
04046 
04047         qn  = -1.0e20;
04048         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
04049                 if (q(j) >= qn) {
04050                         qn  = q(j);
04051                         jtot = j;
04052                 }
04053         }
04054 
04055         for (k=-3; k<=3; k++)  {
04056                 j = ((jtot+k+maxrin-1)%maxrin)+1;
04057                 t7(k+4) = q(j);
04058         }
04059 
04060         // interpolate
04061         prb1d(t7,7,&pos);
04062         tot = (float)(jtot)+pos;
04063         // Do not interpolate
04064         //*tot = (float)(jtot);
04065 
04066         free(q);
04067 
04068         Dict retvals;
04069         retvals["qn"] = qn;
04070         retvals["tot"] = tot;
04071         return retvals;
04072 }
04073 
04074 #define  dout(i,j)        dout[i+maxrin*j]
04075 #define  circ1b(i)        circ1b[i-1]
04076 #define  circ2b(i)        circ2b[i-1]
04077 
04078 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04079 
04080    // dimension         circ1(lcirc),circ2(lcirc)
04081 
04082         int   ip, jc, numr3i, numr2i, i, j;
04083         float t1, t2, t3, t4, c1, c2, d1, d2;
04084 
04085         int nring = numr.size()/3;
04086         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04087         int maxrin = numr[numr.size()-1];
04088 
04089         float* circ1b = circ1->get_data();
04090         float* circ2b = circ2->get_data();
04091 
04092         // t(maxrin), q(maxrin)  // removed +2
04093         double *t, *q;
04094 
04095         q = (double*)calloc(maxrin,sizeof(double));
04096         t = (double*)calloc(maxrin,sizeof(double));
04097 
04098 #ifdef _WIN32
04099         ip = -(int)(log((float)maxrin)/log(2.0f));
04100 #else
04101         ip = -(int)(log2(maxrin));
04102 #endif  //_WIN32
04103 
04104         //  q - straight  = circ1 * conjg(circ2)
04105 
04106         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04107 
04108         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04109 
04110         for (i=1; i<=nring; i++) {
04111 
04112                 numr3i = numr(3,i);
04113                 numr2i = numr(2,i);
04114 
04115                 t1   = circ1b(numr2i) * circ2b(numr2i);
04116                 q(1) = q(1)+t1;
04117                 t(1) = t(1)+t1;
04118 
04119                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04120                 if (numr3i == maxrin)  {
04121                         q(2) += t1;
04122                         t(2) += t1;
04123                 } else {
04124                         q(numr3i+1) += t1;
04125                         t(numr3i+1) += t1;
04126                 }
04127 
04128                 for (j=3; j<=numr3i; j=j+2) {
04129                         jc     = j+numr2i-1;
04130 
04131                         c1     = circ1b(jc);
04132                         c2     = circ1b(jc+1);
04133                         d1     = circ2b(jc);
04134                         d2     = circ2b(jc+1);
04135 
04136                         t1     = c1 * d1;
04137                         t3     = c1 * d2;
04138                         t2     = c2 * d2;
04139                         t4     = c2 * d1;
04140 
04141                         q(j)   += t1 + t2;
04142                         q(j+1) += - t3 + t4;
04143                         t(j)   += t1 - t2;
04144                         t(j+1) += - t3 - t4;
04145                 }
04146         }
04147 
04148         // straight
04149         fftr_d(q,ip);
04150 
04151         // mirrored
04152         fftr_d(t,ip);
04153 
04154         EMData* out = new EMData();
04155         out->set_size(maxrin,2,1);
04156         float *dout = out->get_data();
04157         for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04158         //out->set_size(maxrin,1,1);
04159         //float *dout = out->get_data();
04160         //for (int i=0; i<maxrin; i++) {dout(i,0)=q[i];}
04161         free(t);
04162         free(q);
04163         return out;
04164 }
04165 
04166 
04167 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04168 
04169         int maxrin = numr[numr.size()-1];
04170 
04171         vector<float> r(2*maxrin);
04172 
04173         Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04174 
04175         return r;
04176 }
04177 
04178 #define  dout(i,j)        dout[i+maxrin*j]
04179 #define  circ1b(i)        circ1b[i-1]
04180 #define  circ2b(i)        circ2b[i-1]
04181 
04182 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04183 
04184    // dimension         circ1(lcirc),circ2(lcirc)
04185 
04186         int   ip, jc, numr3i, numr2i, i, j;
04187         float t1, t2, t3, t4, c1, c2, d1, d2;
04188 
04189         int nring = numr.size()/3;
04190         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04191         int maxrin = numr[numr.size()-1];
04192 
04193         float* circ1b = circ1->get_data();
04194         float* circ2b = circ2->get_data();
04195 
04196 #ifdef _WIN32
04197         ip = -(int)(log((float)maxrin)/log(2.0f));
04198 #else
04199         ip = -(int)(log2(maxrin));
04200 #endif  //_WIN32
04201         for (int i=1; i<=maxrin; i++)  {q(i) = 0.0f; t(i) = 0.0f;}
04202 
04203         //  q - straight  = circ1 * conjg(circ2)
04204 
04205         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04206 
04207         for (i=1; i<=nring; i++) {
04208 
04209                 numr3i = numr(3,i);
04210                 numr2i = numr(2,i);
04211 
04212                 t1   = circ1b(numr2i) * circ2b(numr2i);
04213                 q(1) += t1;
04214                 t(1) += t1;
04215 
04216                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04217                 if (numr3i == maxrin)  {
04218                         q(2) += t1;
04219                         t(2) += t1;
04220                 } else {
04221                         q(numr3i+1) += t1;
04222                         t(numr3i+1) += t1;
04223                 }
04224 
04225                 for (j=3; j<=numr3i; j=j+2) {
04226                         jc     = j+numr2i-1;
04227 
04228                         c1     = circ1b(jc);
04229                         c2     = circ1b(jc+1);
04230                         d1     = circ2b(jc);
04231                         d2     = circ2b(jc+1);
04232 
04233                         t1     = c1 * d1;
04234                         t3     = c1 * d2;
04235                         t2     = c2 * d2;
04236                         t4     = c2 * d1;
04237 
04238                         q(j)   += t1 + t2;
04239                         q(j+1) += -t3 + t4;
04240                         t(j)   += t1 - t2;
04241                         t(j+1) += -t3 - t4;
04242                 }
04243         }
04244         // straight
04245         fftr_q(q,ip);
04246         //for (int i=0; i<maxrin; i++) cout<<i<<"  B    "<<q[i]<<"       "<<t[i]<<endl;
04247 
04248         // mirrored
04249         fftr_q(t,ip);
04250 }
04251 
04252 
04253 
04254 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04255 {
04256 
04257         int   ip, jc, numr3i, numr2i, i, j;
04258         float t1, t2, t3, t4, c1, c2, d1, d2;
04259 
04260         int nring = numr.size()/3;
04261         int maxrin = numr[numr.size()-1];
04262 
04263         float* circ1b = circ1->get_data();
04264         float* circ2b = circ2->get_data();
04265 
04266         double *q;
04267 
04268         q = (double*)calloc(maxrin,sizeof(double));
04269 
04270 #ifdef _WIN32
04271         ip = -(int)(log((float)maxrin)/log(2.0f));
04272 #else
04273         ip = -(int)(log2(maxrin));
04274 #endif  //_WIN32
04275 
04276          //  q - straight  = circ1 * conjg(circ2)
04277 
04278         for (i=1;i<=nring;i++) {
04279 
04280                 numr3i = numr(3,i);
04281                 numr2i = numr(2,i);
04282 
04283                 t1   = circ1b(numr2i) * circ2b(numr2i);
04284                 q(1) = q(1)+t1;
04285 
04286                 if (numr3i == maxrin)  {
04287                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04288                         q(2) = q(2)+t1;
04289                 } else {
04290                         t1              = circ1b(numr2i+1) * circ2b(numr2i+1);
04291                         q(numr3i+1) = q(numr3i+1)+t1;
04292                 }
04293 
04294                 for (j=3;j<=numr3i;j=j+2) {
04295                         jc     = j+numr2i-1;
04296 
04297                         c1     = circ1b(jc);
04298                         c2     = circ1b(jc+1);
04299                         d1     = circ2b(jc);
04300                         d2     = circ2b(jc+1);
04301 
04302                         t1     = c1 * d1;
04303                         t3     = c1 * d2;
04304                         t2     = c2 * d2;
04305                         t4     = c2 * d1;
04306 
04307                         q(j)   = q(j)   + t1 + t2;
04308                         q(j+1) = q(j+1) - t3 + t4;
04309                 }
04310         }
04311 
04312         // straight
04313         fftr_d(q,ip);
04314 
04315         EMData* out = new EMData();
04316         out->set_size(maxrin,1,1);
04317         float *dout = out->get_data();
04318         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04319         free(q);
04320         return out;
04321 
04322 }
04323 
04324 
04325 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04326 {
04327 
04328         int   ip, jc, numr3i, numr2i, i, j;
04329         float t1, t2, t3, t4, c1, c2, d1, d2;
04330 
04331         int nring = numr.size()/3;
04332         int maxrin = numr[numr.size()-1];
04333 
04334         float* circ1b = circ1->get_data();
04335         float* circ2b = circ2->get_data();
04336 
04337         double *t;
04338 
04339         t = (double*)calloc(maxrin,sizeof(double));
04340 
04341 #ifdef _WIN32
04342         ip = -(int)(log((float)maxrin)/log(2.0f));
04343 #else
04344         ip = -(int)(log2(maxrin));
04345 #endif  //_WIN32
04346 
04347          //   t - mirrored  = conjg(circ1) * conjg(circ2)
04348 
04349         for (i=1;i<=nring;i++) {
04350 
04351                 numr3i = numr(3,i);
04352                 numr2i = numr(2,i);
04353 
04354                 t1   = circ1b(numr2i) * circ2b(numr2i);
04355                 t(1) = t(1)+t1;
04356 
04357                 if (numr3i == maxrin)  {
04358                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04359                         t(2) = t(2)+t1;
04360                 }
04361 
04362                 for (j=3;j<=numr3i;j=j+2) {
04363                         jc     = j+numr2i-1;
04364 
04365                         c1     = circ1b(jc);
04366                         c2     = circ1b(jc+1);
04367                         d1     = circ2b(jc);
04368                         d2     = circ2b(jc+1);
04369 
04370                         t1     = c1 * d1;
04371                         t3     = c1 * d2;
04372                         t2     = c2 * d2;
04373                         t4     = c2 * d1;
04374 
04375                         t(j)   = t(j)   + t1 - t2;
04376                         t(j+1) = t(j+1) - t3 - t4;
04377                 }
04378         }
04379 
04380         // mirrored
04381         fftr_d(t,ip);
04382 
04383         EMData* out = new EMData();
04384         out->set_size(maxrin,1,1);
04385         float *dout = out->get_data();
04386         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04387         free(t);
04388         return out;
04389 
04390 }
04391 
04392 #undef circ1b
04393 #undef circ2b
04394 #undef dout
04395 
04396 #undef  circ1
04397 #undef  circ2
04398 #undef  t
04399 #undef  q
04400 #undef  b
04401 #undef  t7
04402 
04403 
04404 #define    QUADPI                   3.141592653589793238462643383279502884197
04405 #define    PI2                      2*QUADPI
04406 
04407 float Util::ener(EMData* ave, vector<int> numr) {
04408         ENTERFUNC;
04409         long double ener,en;
04410 
04411         int nring = numr.size()/3;
04412         float *aveptr = ave->get_data();
04413 
04414         ener = 0.0;
04415         for (int i=1; i<=nring; i++) {
04416                 int numr3i = numr(3,i);
04417                 int np     = numr(2,i)-1;
04418                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04419                 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04420                 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04421                 ener += en/numr3i;
04422         }
04423         EXITFUNC;
04424         return static_cast<float>(ener);
04425 }
04426 
04427 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04428         ENTERFUNC;
04429         long double ener, en;
04430         float arg, cs, si;
04431 
04432         int nima = data.size();
04433         int nring = numr.size()/3;
04434         int maxrin = numr(3,nring);
04435 
04436         ener = 0.0;
04437         for (int i=1; i<=nring; i++) {
04438                 int numr3i = numr(3,i);
04439                 int np     = numr(2,i)-1;
04440                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04441                 float temp1 = 0.0, temp2 = 0.0;
04442                 for (int kk=0; kk<nima; kk++) {
04443                         float *ptr = data[kk]->get_data();
04444                         temp1 += ptr[np];
04445                         temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04446                 }
04447                 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04448                 for (int j=2; j<numr3i; j+=2) {
04449                         float tempr = 0.0, tempi = 0.0;
04450                         for (int kk=0; kk<nima; kk++) {
04451                                 float *ptr = data[kk]->get_data();
04452                                 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04453                                 cs = cos(arg);
04454                                 si = sin(arg);
04455                                 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04456                                 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04457                         }
04458                         en += tq*(tempr*tempr+tempi*tempi);
04459                 }
04460                 ener += en/numr3i;
04461         }
04462         EXITFUNC;
04463         return static_cast<float>(ener);
04464 }
04465 
04466 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04467         int nring = numr.size()/3;
04468         float *ave = avep->get_data();
04469         float *dat = datp->get_data();
04470         int i, j, numr3i, np;
04471         float  arg, cs, si;
04472         int maxrin = numr(3,nring);
04473         if(mirror == 1) { //for mirrored data has to be conjugated
04474                 for (i=1; i<=nring; i++) {
04475                         numr3i = numr(3,i);
04476                         np     = numr(2,i)-1;
04477                         ave[np]   += dat[np];
04478                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04479                         for (j=2; j<numr3i; j=j+2) {
04480                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04481                                 cs = cos(arg);
04482                                 si = sin(arg);
04483                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04484                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04485                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04486                         }
04487                 }
04488         } else {
04489                 for (i=1; i<=nring; i++) {
04490                         numr3i = numr(3,i);
04491                         np     = numr(2,i)-1;
04492                         ave[np]   += dat[np];
04493                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04494                         for (j=2; j<numr3i; j=j+2) {
04495                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04496                                 cs = cos(arg);
04497                                 si = sin(arg);
04498                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04499                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04500                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04501                         }
04502                 }
04503         }
04504         avep->update();
04505         EXITFUNC;
04506 }
04507 
04508 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04509         int nring = numr.size()/3;
04510         float *ave = avep->get_data();
04511         float *dat = datp->get_data();
04512         int i, j, numr3i, np;
04513         float  arg, cs, si;
04514         int maxrin = numr(3,nring);
04515         if(mirror == 1) { //for mirrored data has to be conjugated
04516                 for (i=1; i<=nring; i++) {
04517                         numr3i = numr(3,i);
04518                         np     = numr(2,i)-1;
04519                         ave[np]   -= dat[np];
04520                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04521                         for (j=2; j<numr3i; j=j+2) {
04522                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04523                                 cs = cos(arg);
04524                                 si = sin(arg);
04525                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04526                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04527                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04528                         }
04529                 }
04530         } else {
04531                 for (i=1; i<=nring; i++) {
04532                         numr3i = numr(3,i);
04533                         np     = numr(2,i)-1;
04534                         ave[np]   -= dat[np];
04535                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04536                         for (j=2; j<numr3i; j=j+2) {
04537                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04538                                 cs = cos(arg);
04539                                 si = sin(arg);
04540                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04541                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04542                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04543                         }
04544                 }
04545         }
04546         avep->update();
04547         EXITFUNC;
04548 }
04549 
04550 
04551 #undef    QUADPI
04552 #undef    PI2
04553 
04554 #undef  numr
04555 #undef  circ
04556 
04557 
04558 #define QUADPI   3.141592653589793238462643383279502884197
04559 #define PI2      QUADPI*2
04560 #define deg_rad  QUADPI/180.0
04561 #define rad_deg  180.0/QUADPI
04562 
04563 struct ori_t
04564 {
04565     int iphi;
04566     int itht;
04567     int id;
04568 };
04569 
04570 
04571 struct cmpang
04572 {
04573     bool operator()( const ori_t& a, const ori_t& b )
04574     {
04575         if( a.itht != b.itht )
04576         {
04577             return a.itht < b.itht;
04578         }
04579 
04580         return a.iphi < b.iphi;
04581     }
04582 };
04583 
04584 
04585 vector<double> Util::cml_weights(const vector<float>& cml){
04586         static const int NBIN = 100;
04587         int nline=cml.size()/2;
04588         vector<double> weights(nline);
04589 
04590         vector<ori_t> angs(nline);
04591         for( int i=0; i < nline; ++i ) {
04592                 angs[i].iphi = int( NBIN*cml[2*i] );
04593                 angs[i].itht = int( NBIN*cml[2*i+1] );
04594                 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04595                 angs[i].id = i;
04596         }
04597 
04598         //std::cout << "# of angs: " << angs.size() << std::endl;
04599 
04600         std::sort( angs.begin(), angs.end(), cmpang() );
04601 
04602         vector<float> newphi;
04603         vector<float> newtht;
04604         vector< vector<int> > indices;
04605 
04606         int curt_iphi = -1;
04607         int curt_itht = -1;
04608         for(unsigned int i=0 ;i < angs.size(); ++i ) {
04609                 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04610                         Assert( indices.size() > 0 );
04611                         indices.back().push_back(angs[i].id);
04612                 } else {
04613                         curt_iphi = angs[i].iphi;
04614                         curt_itht = angs[i].itht;
04615 
04616                         newphi.push_back( float(curt_iphi)/NBIN );
04617                         newtht.push_back( float(curt_itht)/NBIN );
04618                         indices.push_back( vector<int>(1,angs[i].id) );
04619                 }
04620         }
04621 
04622         //std::cout << "# of indpendent ang: " << newphi.size() << std::endl;
04623 
04624 
04625         int num_agl = newphi.size();
04626 
04627         if(num_agl>2) {
04628                 vector<double> w=Util::vrdg(newphi, newtht);
04629 
04630                 Assert( w.size()==newphi.size() );
04631                 Assert( indices.size()==newphi.size() );
04632 
04633                 for(unsigned int i=0; i < newphi.size(); ++i ) {
04634                     /*
04635                     std::cout << "phi,tht,w,n: ";
04636                     std::cout << boost::format( "%10.3f" ) % newphi[i] << " ";
04637                     std::cout << boost::format( "%10.3f" ) % newtht[i] << " ";
04638                     std::cout << boost::format( "%8.6f"  ) % w[i] << " ";
04639                     std::cout << indices[i].size() << "(";
04640                     */
04641 
04642                     for(unsigned int j=0; j < indices[i].size(); ++j ) {
04643                             int id = indices[i][j];
04644                             weights[id] = w[i]/indices[i].size();
04645                             //std::cout << id << " ";
04646                     }
04647 
04648                     //std::cout << ")" << std::endl;
04649 
04650                 }
04651         } else {
04652                 cout<<"warning in Util.cml_weights"<<endl;
04653                 double val = PI2/float(nline);
04654                 for(int i=0; i<nline; i++)  weights[i]=val;
04655         }
04656 
04657         return weights;
04658 
04659 }
04660 
04661 /****************************************************
04662  * New code for common-lines
04663  ****************************************************/
04664 
04665 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04666 {
04667         int i;
04668         int nx=img->get_xsize();
04669         float *img_ptr  = img->get_data();
04670         float *line_ptr = line->get_data();
04671         for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04672         img->update();
04673 }
04674 
04675 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04676     int j;
04677     int nx = sino->get_xsize();
04678     int i = nx * pos_line;
04679     float r1, r2;
04680     float *line_ptr = line->get_data();
04681     float *sino_ptr = sino->get_data();
04682     for (j=ilf;j<=ihf; j += 2) {
04683         r1 = line_ptr[j];
04684         r2 = line_ptr[j + 1];
04685         sino_ptr[i + j - ilf] = r1;
04686         sino_ptr[i + j - ilf + 1] = r2;
04687         sino_ptr[i + nx * nblines + j - ilf] = r1;
04688         sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04689     }
04690     sino->update();
04691 }
04692 
04693 vector<double> Util::cml_init_rot(vector<float> Ori){
04694     int nb_ori = Ori.size() / 4;
04695     int i, ind;
04696     float ph, th, ps;
04697     double cph, cth, cps, sph, sth, sps;
04698     vector<double> Rot(nb_ori*9);
04699     for (i=0; i<nb_ori; ++i){
04700         ind = i*4;
04701         // spider convention phi=psi-90, psi=phi+90
04702         ph = Ori[ind+2]-90;
04703         th = Ori[ind+1];
04704         ps = Ori[ind]+90;
04705         ph *= deg_rad;
04706         th *= deg_rad;
04707         ps *= deg_rad;
04708         // pre-calculate some trigo stuffs
04709         cph = cos(ph);
04710         cth = cos(th);
04711         cps = cos(ps);
04712         sph = sin(ph);
04713         sth = sin(th);
04714         sps = sin(ps);
04715         // fill rotation matrix
04716         ind = i*9;
04717         Rot[ind] = cph*cps-cth*sps*sph;
04718         Rot[ind+1] = cph*sps+cth*cps*sph;
04719         Rot[ind+2] = sth*sph;
04720         Rot[ind+3] = -sph*cps-cth*sps*cph;
04721         Rot[ind+4] = -sph*sps+cth*cps*cph;
04722         Rot[ind+5] = sth*cph;
04723         Rot[ind+6] = sth*sps;
04724         Rot[ind+7] = -sth*cps;
04725         Rot[ind+8] = cth;
04726     }
04727 
04728     return Rot;
04729 }
04730 
04731 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04732     float ph, ps;
04733     double cph, cth, cps, sph, sth, sps;
04734     int ind = iprj*9;
04735     // spider convention phi=psi-90, psi=phi+90
04736     ph = nps-90;
04737     ps = nph+90;
04738     ph *= deg_rad;
04739     th *= deg_rad;
04740     ps *= deg_rad;
04741     // pre-calculate some trigo stuffs
04742     cph = cos(ph);
04743     cth = cos(th);
04744     cps = cos(ps);
04745     sph = sin(ph);
04746     sth = sin(th);
04747     sps = sin(ps);
04748     // fill rotation matrix
04749     Rot[ind] = (float)(cph*cps-cth*sps*sph);
04750     Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04751     Rot[ind+2] = (float)(sth*sph);
04752     Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04753     Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04754     Rot[ind+5] = (float)(sth*cph);
04755     Rot[ind+6] = (float)(sth*sps);
04756     Rot[ind+7] = (float)(-sth*cps);
04757     Rot[ind+8] = (float)(cth);
04758 
04759     return Rot;
04760 }
04761 
04762 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04763     vector<int> com(2*(n_prj - 1));
04764     int a = i_prj*9;
04765     int i, b, c;
04766     int n1=0, n2=0;
04767     float vmax = 1 - 1.0e-6f;
04768     double r11, r12, r13, r23, r31, r32, r33;
04769 
04770     c = 0;
04771     for (i=0; i<n_prj; ++i){
04772         if (i!=i_prj){
04773             b = i*9;
04774             // this is equivalent to R = A*B'
04775             r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04776             r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04777             r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04778             r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04779             r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04780             r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04781             r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04782             if (r33 > vmax) {
04783                 n2 = 270;
04784                 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04785             }
04786             else if (r33 < -vmax) {
04787                 n2 = 270;
04788                 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04789             } else {
04790                 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04791                 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04792                 if (n1 < 0) {n1 += 360;}
04793                 if (n2 <= 0) {n2 = abs(n2);}
04794                 else {n2 = 360 - n2;}
04795             }
04796 
04797             if (n1 >= 360){n1 = n1 % 360;}
04798             if (n2 >= 360){n2 = n2 % 360;}
04799 
04800             // store common-lines
04801             b = c*2;
04802             com[b] = n1;
04803             com[b+1] = n2;
04804             ++c;
04805         }
04806     }
04807 
04808     return com;
04809 
04810 }
04811 
04812 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int n_prj, int n_lines) {
04813     vector<int> com(2*n_lines);
04814     int a=0, b, c, l;
04815     int n1=0, n2=0, mem=-1;
04816     float vmax = 1 - 1.0e-6f;
04817     double r11, r12, r13, r23, r31, r32, r33;
04818     c = 0;
04819     for (l=0; l<n_lines; ++l){
04820         c = 2*l;
04821         if (seq[c]!=mem){
04822             mem = seq[c];
04823             a = seq[c]*9;
04824         }
04825         b = seq[c+1]*9;
04826 
04827         // this is equivalent to R = A*B'
04828         r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04829         r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04830         r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04831         r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04832         r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04833         r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04834         r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04835         if (r33 > vmax) {
04836             n2 = 270;
04837             n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04838         }
04839         else if (r33 < -vmax) {
04840             n2 = 270;
04841             n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04842         } else {
04843             n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04844             n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04845             if (n1 < 0) {n1 += 360;}
04846             if (n2 <= 0) {n2 = abs(n2);}
04847             else {n2 = 360 - n2;}
04848         }
04849         if (n1 >= 360){n1 = n1 % 360;}
04850         if (n2 >= 360){n2 = n2 % 360;}
04851 
04852         // store common-lines
04853         com[c] = n1;
04854         com[c+1] = n2;
04855     }
04856 
04857     return com;
04858 
04859 }
04860 
04861 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int nprj, int nlines){
04862     // seq is the pairwise index ij: 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04863     vector<double> cml(2*nlines); // [phi, theta] / line
04864     float ph1, th1;
04865     float ph2, th2;
04866     double nx, ny, nz;
04867     double norm;
04868     double sth1=0, sph1=0, cth1=0, cph1=0;
04869     double sth2, sph2, cth2, cph2;
04870     int l, ind, c;
04871     int mem = -1;
04872     for (l=0; l<nlines; ++l){
04873         c = 2*l;
04874         if (seq[c]!=mem){
04875             mem = seq[c];
04876             ind = 4*seq[c];
04877             ph1 = Ori[ind]*deg_rad;
04878             th1 = Ori[ind+1]*deg_rad;
04879             sth1 = sin(th1);
04880             sph1 = sin(ph1);
04881             cth1 = cos(th1);
04882             cph1 = cos(ph1);
04883         }
04884         ind = 4*seq[c+1];
04885         ph2 = Ori[ind]*deg_rad;
04886         th2 = Ori[ind+1]*deg_rad;
04887         sth2 = sin(th2);
04888         cth2 = cos(th2);
04889         sph2 = sin(ph2);
04890         cph2 = cos(ph2);
04891         // cross product
04892         nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04893         ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04894         nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04895         norm = sqrt(nx*nx+ny*ny+nz*nz);
04896         nx /= norm;
04897         ny /= norm;
04898         nz /= norm;
04899         // apply mirror if need
04900         if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04901         // compute theta and phi
04902         cml[c+1] = acos(nz);
04903         if (cml[c+1] == 0) {cml[c] = 0;}
04904         else {
04905             cml[c+1] *= rad_deg;
04906             if (cml[c+1] > 89.99) {cml[c+1] = 89.99;} // this fix some pb in Voronoi
04907             cml[c] = rad_deg * atan2(nx, ny);
04908             cml[c] = fmod(360 + cml[c], 360);
04909 
04910         }
04911     }
04912 
04913     return cml;
04914 }
04915 
04916 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04917     double res = 0;
04918     double buf = 0;
04919     float* line_1;
04920     float* line_2;
04921     int i, n, ind;
04922     int lnlen = data[0]->get_xsize();
04923     for (n=0; n<n_lines; ++n) {
04924         ind = n*2;
04925         line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04926         line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04927         buf = 0;
04928         for (i=0; i<lnlen; ++i) {
04929             buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04930         }
04931         res += buf * weights[n];
04932     }
04933 
04934     return res;
04935 
04936 }
04937 
04938 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04939                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04940     // res: [best_disc, best_ipsi]
04941     // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04942     // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
04943     vector<double> res(2);
04944     int lnlen = data[0]->get_xsize();
04945     int end = 2*(n_prj-1);
04946     double disc, buf, bdisc, tmp;
04947     int n, i, ipsi, ind, bipsi, c;
04948     float* line_1;
04949     float* line_2;
04950     bdisc = 1.0e6;
04951     bipsi = -1;
04952     // loop psi
04953     for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04954         // discrepancy
04955         disc = 0;
04956         c = 0;
04957         for (n=0; n<n_prj; ++n) {
04958             if(n!=iprj) {
04959                 ind = 2*c;
04960                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04961                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04962                 buf = 0;
04963                 for (i=0; i<lnlen; ++i) {
04964                     tmp = line_1[i]-line_2[i];
04965                     buf += tmp*tmp;
04966                 }
04967                 disc += buf * weights[iw[c]];
04968                 ++c;
04969             }
04970         }
04971         // select the best value
04972         if (disc <= bdisc) {
04973             bdisc = disc;
04974             bipsi = ipsi;
04975         }
04976         // update common-lines
04977         for (i=0; i<end; i+=2){
04978             com[i] += d_psi;
04979             if (com[i] >= n_psi) {com[i] = com[i] % n_psi;}
04980         }
04981     }
04982     res[0] = bdisc;
04983     res[1] = float(bipsi);
04984 
04985     return res;
04986 }
04987 
04988 #undef  QUADPI
04989 #undef  PI2
04990 #undef  deg_rad
04991 #undef  rad_deg
04992 
04993 /****************************************************
04994  * END OF NEW CODE FOR COMMON-LINES
04995  ****************************************************/
04996 
04997 // helper function for k-means
04998 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
04999         ENTERFUNC;
05000 
05001         int nima = data.size();
05002         vector<float> res(nima);
05003         double result = 0.;
05004         double valmin = 1.0e20;
05005         int valpos = -1;
05006 
05007         for (int kk=0; kk<nima; kk++){
05008         result = 0;
05009 
05010         float *y_data = data[kk]->get_data();
05011         float *x_data = image->get_data();
05012         long totsize = image->get_xsize()*image->get_ysize();
05013         for (long i = 0; i < totsize; i++) {
05014             double temp = x_data[i]- y_data[i];
05015             result += temp*temp;
05016         }
05017         result /= totsize;
05018         res[kk] = (float)result;
05019 
05020         if(result<valmin) {valmin = result; valpos = kk;}
05021 
05022         }
05023 
05024         Dict retvals;
05025         retvals["dist"] = res;
05026         retvals["pos"]  = valpos;
05027 
05028         EXITFUNC;
05029         return retvals;
05030 
05031 }
05032 
05033 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05034         ENTERFUNC;
05035 
05036         int nima = data.size();
05037         vector<float> res(nima);
05038         double result = 0.;
05039         double valmin = 1.0e20;
05040         int valpos = -1;
05041 
05042         for (int kk=0; kk<nima; kk++){
05043         result = 0;
05044         //validate_input_args(image, data[kk]);
05045 
05046         float *y_data = data[kk]->get_data();
05047         float *x_data = image->get_data();
05048 
05049         // Implemented by PAP  01/09/06 - please do not change.  If in doubts, write/call me.
05050         int nx  = data[kk]->get_xsize();
05051         int ny  = data[kk]->get_ysize();
05052         nx = (nx - 2 + data[kk]->is_fftodd()); // nx is the real-space size of the input image
05053         int lsd2 = (nx + 2 - nx%2) ; // Extended x-dimension of the complex image
05054 
05055         int ixb = 2*((nx+1)%2);
05056         int iyb = ny%2;
05057         int iz = 0;
05058 
05059         for ( int iy = 0; iy <= ny-1; iy++) {
05060             for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05061                 int ii = ix + (iy  + iz * ny)* lsd2;
05062                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05063             }
05064         }
05065         for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05066             int ii = (iy  + iz * ny)* lsd2;
05067             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05068             result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05069         }
05070         if(nx%2 == 0) {
05071             for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05072                 int ii = lsd2 - 2 + (iy  + iz * ny)* lsd2;
05073                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05074                 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05075             }
05076 
05077         }
05078         result *= 2;
05079         result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05080         if(ny%2 == 0) {
05081             int ii = (ny/2  + iz * ny)* lsd2;
05082             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05083         }
05084         if(nx%2 == 0) {
05085             int ii = lsd2 - 2 + (0  + iz * ny)* lsd2;
05086             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05087             if(ny%2 == 0) {
05088                 int ii = lsd2 - 2 +(ny/2  + iz * ny)* lsd2;
05089                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05090             }
05091         }
05092 
05093         result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05094         res[kk] = (float)result;
05095 
05096         if(result<valmin) {valmin = result; valpos = kk;}
05097 
05098         }
05099 
05100         Dict retvals;
05101         retvals["dist"] = res;
05102         retvals["pos"]  = valpos;
05103 
05104         EXITFUNC;
05105         return retvals;
05106 }
05107 
05108 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05109     long int d2 = group2[s2 - 1] - group2[0];
05110     long int p2 = 0;
05111     long int i1 = 0;
05112     long int i2 = 0;
05113     long int max = 0;
05114     long int cont = 0;
05115     long int i = 0;
05116     int stop1 = 0;
05117     int stop2 = 0;
05118 
05119     for (i=0; i<s1; i++) {
05120         p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05121         if (p2 >= s2) {p2 = s2 - 1;}
05122         i1 = p2;
05123         i2 = p2;
05124         max = s2;
05125         if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05126 
05127         stop1 = 0;
05128         stop2 = 0;
05129         while (max--) {
05130             if (group1[i] == group2[i1]) {
05131                 if (flag) {stb[cont] = group1[i];}
05132                 cont++;
05133                 break;
05134             }
05135             if (group2[i1] < group1[i]) {stop1=1;}
05136             if (group1[i] == group2[i2]) {
05137                 if (flag) {stb[cont] = group1[i];}
05138                 cont++;
05139                 break;
05140             }
05141             if (group2[i2] > group1[i]) {stop2=1;}
05142             //printf("i1 %li i2 %li    v2 %i v2 %i   stop1 %i stop2 %i\n", i1, i2, group2[i1], group2[i2], stop1, stop2);
05143 
05144             if (stop1 & stop2) {break;}
05145             i1--;
05146             i2++;
05147             if (i1 < 0) {i1 = 0;}
05148             if (i2 >= s2) {i2 = s2 - 1;}
05149         }
05150         //printf("v1: %i    ite: %li   cont: %li\n", group1[i], s2-max, cont);
05151     }
05152 
05153     return cont;
05154 }
05155 
05156 
05157 
05158 #define old_ptr(i,j,k)          old_ptr[i+(j+(k*ny))*nx]
05159 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*new_nx]
05160 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05161 {
05162         /* Exception Handle */
05163         if (!img) {
05164                 throw NullPointerException("NULL input image");
05165         }
05166         /* ============================== */
05167 
05168         // Get the size of the input image
05169         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05170         /* ============================== */
05171 
05172 
05173         /* Exception Handle */
05174         if ((x_step-1 > nx/2 || y_step-1 > ny/2 || z_step-1 > nz/2) || (x_step-1)<0 || (y_step-1)<0 || (z_step-1)<0)
05175         {
05176                 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05177                 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05178         }
05179         /* ============================== */
05180 
05181 
05182         /*    Calculation of the start point */
05183         int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05184         /* ============================*/
05185 
05186 
05187         /* Calculation of the size of the decimated image */
05188         int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05189         int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05190         int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05191         if(r1>1){r1=1;}
05192         if(r2>1){r2=1;}
05193         if(r3>1){r3=1;}
05194         int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05195         /* ===========================================*/
05196 
05197 
05198         EMData* img2 = new EMData();
05199         img2->set_size(new_nx,new_ny,new_nz);
05200         float *new_ptr = img2->get_data();
05201         float *old_ptr = img->get_data();
05202         int iptr, jptr, kptr = 0;
05203         for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05204                 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05205                         for (int i=new_st_x; i<nx; i+=x_step) {
05206                                 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05207                         iptr++;}
05208                 jptr++;}
05209         kptr++;}
05210         img2->update();
05211         return img2;
05212 }
05213 #undef old_ptr
05214 #undef new_ptr
05215 
05216 #define inp(i,j,k)  inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*nx]
05217 #define outp(i,j,k) outp[i+(j+(k*new_ny))*new_nx]
05218 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05219 {
05220         /* Exception Handle */
05221         if (!img) throw NullPointerException("NULL input image");
05222         /* ============================== */
05223 
05224         // Get the size of the input image
05225         int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05226         /* ============================== */
05227 
05228         /* Exception Handle */
05229         if(new_nx>nx || new_ny>ny || new_nz>nz)
05230                 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size.");
05231         if((nx/2)-(new_nx/2)+x_offset<0 || (ny/2)-(new_ny/2)+y_offset<0 || (nz/2)-(new_nz/2)+z_offset<0)
05232                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05233         if(x_offset>((nx-(nx/2))-(new_nx-(new_nx/2))) || y_offset>((ny-(ny/2))-(new_ny-(new_ny/2))) || z_offset>((nz-(nz/2))-(new_nz-(new_nz/2))))
05234                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05235         /* ============================== */
05236 
05237         /*    Calculation of the start point */
05238         int  new_st_x = nx/2-new_nx/2 + x_offset,
05239              new_st_y = ny/2-new_ny/2 + y_offset,
05240              new_st_z = nz/2-new_nz/2 + z_offset;
05241         /* ============================== */
05242 
05243         /* Exception Handle */
05244         if (new_st_x<0 || new_st_y<0 || new_st_z<0)   //  WHAT HAPPENS WITH THE END POINT CHECK??  PAP
05245                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05246         /* ============================== */
05247 
05248         EMData* wind = img->copy_head();
05249         wind->set_size(new_nx, new_ny, new_nz);
05250         float *outp=wind->get_data();
05251         float *inp=img->get_data();
05252 
05253         for (int k=0; k<new_nz; k++)
05254                 for(int j=0; j<new_ny; j++)
05255                         for(int i=0; i<new_nx; i++)
05256                                 outp(i,j,k) = inp(i,j,k);
05257         wind->update();
05258         return wind;
05259 }
05260 #undef inp
05261 #undef outp
05262 
05263 #define inp(i,j,k) inp[i+(j+(k*ny))*nx]
05264 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*new_nx]
05265 EMData *Util::pad(EMData* img,int new_nx, int new_ny, int new_nz, int x_offset, int y_offset, int z_offset,char *params)
05266 {
05267         /* Exception Handle */
05268         if (!img)  throw NullPointerException("NULL input image");
05269         /* ============================== */
05270 
05271         // Get the size of the input image
05272         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05273         /* ============================== */
05274 
05275         /* Exception Handle */
05276         if(new_nx<nx || new_ny<ny || new_nz<nz)
05277                 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05278         if((new_nx/2)-(nx/2)+x_offset<0 || (new_ny/2)-(ny/2)+y_offset<0 || (new_nz/2)-(nz/2)+z_offset<0)
05279                 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05280         if(x_offset>((new_nx-(new_nx/2))-(nx-(nx/2))) || y_offset>((new_ny-(new_ny/2))-(ny-(ny/2))) || z_offset>((new_nz-(new_nz/2))-(nz-(nz/2))))
05281                 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05282         /* ============================== */
05283 
05284         EMData* pading = img->copy_head();
05285         pading->set_size(new_nx, new_ny, new_nz);
05286         float *inp  = img->get_data();
05287         float *outp = pading->get_data();
05288 
05289 
05290         /* Calculation of the average and the circumference values for background substitution
05291         =======================================================================================*/
05292         float background;
05293 
05294         if (strcmp(params,"average")==0) background = img->get_attr("mean");
05295         else if (strcmp(params,"circumference")==0) {
05296                 float sum1=0.0f;
05297                 int cnt=0;
05298                 for(int i=0;i<nx;i++) {
05299                         sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05300                         cnt+=2;
05301                 }
05302                 if(nz-1 == 0) {
05303                         for (int j=1;j<ny-1;j++) {
05304                                 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05305                                 cnt+=2;
05306                         }
05307                 } else {
05308                         for (int k=1;k<nz-1;k++) {
05309                                 for (int j=1;j<ny-1;j++) {
05310                                         sum1 += inp(1,j,0) + inp(nx-1,j,0);
05311                                         cnt+=2;
05312                                 }
05313                         }
05314                 }
05315                 background = sum1/cnt;
05316         } else {
05317                 background = static_cast<float>( atof( params ) );
05318         }
05319         /*=====================================================================================*/
05320 
05321          /*Initial Padding */
05322         int new_st_x=0,new_st_y=0,new_st_z=0;
05323         for (int k=0;k<new_nz;k++)
05324                 for(int j=0;j<new_ny;j++)
05325                         for (int i=0;i<new_nx;i++)
05326                                 outp(i,j,k)=background;
05327         /*============================== */
05328 
05329         /*    Calculation of the start point */
05330         new_st_x=int((new_nx/2-nx/2)  + x_offset);
05331         new_st_y=int((new_ny/2-ny/2)  + y_offset);
05332         new_st_z=int((new_nz/2-nz/2)  + z_offset);
05333         /* ============================== */
05334 
05335         for (int k=0;k<nz;k++)
05336                 for(int j=0;j<ny;j++)
05337                         for(int i=0;i<nx;i++)
05338                                 outp(i,j,k)=inp(i,j,k);
05339         pading->update();
05340         return pading;
05341 }
05342 #undef inp
05343 #undef outp
05344 //-------------------------------------------------------------------------------------------------------------------------------------------------------------
05345 
05346 void Util::colreverse(float* beg, float* end, int nx) {
05347         float* tmp = new float[nx];
05348         int n = (end - beg)/nx;
05349         int nhalf = n/2;
05350         for (int i = 0; i < nhalf; i++) {
05351                 // swap col i and col n-1-i
05352                 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05353                 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05354                 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05355         }
05356         delete[] tmp;
05357 }
05358 
05359 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05360 {
05361         int nxy = nx*ny;
05362         colreverse(beg, end, nxy);
05363 }
05364 
05365 
05366 void Util::cyclicshift(EMData *image, Dict params) {
05367 
05368         if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05369 
05370         int dx = params["dx"];
05371         int dy = params["dy"];
05372         int dz = params["dz"];
05373 
05374         // The reverse trick we're using shifts to the left (a negative shift)
05375         int nx = image->get_xsize();
05376         dx %= nx;
05377         if (dx < 0) dx += nx;
05378         int ny = image->get_ysize();
05379         dy %= ny;
05380         if (dy < 0) dy += ny;
05381         int nz = image->get_zsize();
05382         dz %= nz;
05383         if (dz < 0) dz += nz;
05384 
05385         int mx = -(dx - nx);
05386         int my = -(dy - ny);
05387         int mz = -(dz - nz);
05388 
05389         float* data = image->get_data();
05390         // x-reverses
05391         if (mx != 0) {
05392                 for (int iz = 0; iz < nz; iz++)
05393                        for (int iy = 0; iy < ny; iy++) {
05394                                 // reverses for column iy
05395                                 int offset = nx*iy + nx*ny*iz; // starting location for column iy in slice iz
05396                                 reverse(&data[offset],&data[offset+mx]);
05397                                 reverse(&data[offset+mx],&data[offset+nx]);
05398                                 reverse(&data[offset],&data[offset+nx]);
05399                         }
05400         }
05401         // y-reverses
05402         if (my != 0) {
05403                 for (int iz = 0; iz < nz; iz++) {
05404                         int offset = nx*ny*iz;
05405                         colreverse(&data[offset], &data[offset + my*nx], nx);
05406                         colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05407                         colreverse(&data[offset], &data[offset + ny*nx], nx);
05408                 }
05409         }
05410         if (mz != 0) {
05411                 slicereverse(&data[0], &data[mz*ny*nx], nx, ny);
05412                 slicereverse(&data[mz*ny*nx], &data[nz*ny*nx], nx, ny);
05413                 slicereverse(&data[0], &data[nz*ny*nx], nx ,ny);
05414         }
05415         image->update();
05416 }
05417 
05418 //-----------------------------------------------------------------------------------------------------------------------
05419 
05420 
05421 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05422 {
05423         if (image->is_complex())
05424                 throw ImageFormatException("Cannot do histogram on Fourier image");
05425         //float hmax, hmin;
05426         float *imageptr=0, *maskptr=0;
05427         int nx=image->get_xsize();
05428         int ny=image->get_ysize();
05429         int nz=image->get_zsize();
05430 
05431         if(mask != NULL){
05432                 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05433                         throw ImageDimensionException("The size of mask image should be of same size as the input image");
05434                 maskptr =mask->get_data();
05435         }
05436         if( nbins == 0) nbins = nx;
05437         vector <float> freq(2*nbins, 0.0);
05438 
05439         imageptr=image->get_data();
05440         if( hmin == hmax ) {
05441                 if(mask == NULL) {
05442                         hmax = image->get_attr("maximum");
05443                         hmin = image->get_attr("minimum");
05444                 } else {
05445                         bool  First = true;
05446                         for (int i = 0;i < nx*ny*nz; i++) {
05447                         if (maskptr[i]>=0.5f) {
05448                                         if(First) {
05449                                                 hmax = imageptr[i];
05450                                                 hmin = imageptr[i];
05451                                                 First = false;
05452                                         } else {
05453                                                 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05454                                                 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05455                                         }
05456                                 }
05457                         }
05458                 }
05459         }
05460         float hdiff = hmax - hmin;
05461         float ff = (nbins-1)/hdiff;
05462         for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05463         if(mask == NULL) {
05464                 for(int i = 0; i < nx*ny*nz; i++) {
05465                         int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05466                         if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05467                 }
05468         } else {
05469                 for(int i = 0; i < nx*ny*nz; i++) {
05470                         if(maskptr[i] >= 0.5) {
05471                                 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05472                                 if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05473                         }
05474                 }
05475         }
05476         return freq;
05477 }
05478 
05479 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05480 {
05481         /* Exception Handle */
05482         if (img->is_complex() || ref->is_complex())
05483                 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05484 
05485         if(mask != NULL){
05486                 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05487                         throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05488         /* ===================================================== */
05489 
05490         /* Image size calculation */
05491         int size_ref = ((ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05492         int size_img = ((img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05493         /* ===================================================== */
05494 
05495         /* The reference image attributes */
05496         float *ref_ptr = ref->get_data();
05497         float ref_h_min = ref->get_attr("minimum");
05498         float ref_h_max = ref->get_attr("maximum");
05499         float ref_h_avg = ref->get_attr("mean");
05500         float ref_h_sig = ref->get_attr("sigma");
05501         /* ===================================================== */
05502 
05503         /* Input image under mask attributes */
05504         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05505 
05506         vector<float> img_data = Util::infomask(img, mask);
05507         float img_avg = img_data[0];
05508         float img_sig = img_data[1];
05509 
05510         /* The image under mask -- size calculation */
05511         int cnt=0;
05512         for(int i=0;i<size_img;i++)
05513                 if (mask_ptr[i]>0.5f)
05514                                 cnt++;
05515         /* ===================================================== */
05516 
05517         /* Histogram of reference image calculation */
05518         float ref_h_diff = ref_h_max - ref_h_min;
05519 
05520         #ifdef _WIN32
05521                 int hist_len = _cpp_min((int)size_ref/16,_cpp_min((int)size_img/16,256));
05522         #else
05523                 int hist_len = std::min((int)size_ref/16,std::min((int)size_img/16,256));
05524         #endif  //_WIN32
05525 
05526         float *ref_freq_bin = new float[3*hist_len];
05527 
05528         //initialize value in each bin to zero
05529         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05530 
05531         for (int i = 0;i < size_ref;i++) {
05532                 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05533                 ref_freq_bin[L]++;
05534         }
05535         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05536 
05537         //Parameters Calculation (i.e) 'A' x + 'B'
05538         float A = ref_h_sig/img_sig;
05539         float B = ref_h_avg - (A*img_avg);
05540 
05541         vector<float> args;
05542         args.push_back(A);
05543         args.push_back(B);
05544 
05545         vector<float> scale;
05546         scale.push_back(1.e-7f*A);
05547         scale.push_back(-1.e-7f*B);
05548 
05549         vector<float> ref_freq_hist;
05550         for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05551 
05552         vector<float> data;
05553         data.push_back(ref_h_diff);
05554         data.push_back(ref_h_min);
05555 
05556         Dict parameter;
05557 
05558         /* Parameters displaying the arguments A & B, and the scaling function and the data's */
05559         parameter["args"] = args;
05560         parameter["scale"]= scale;
05561         parameter["data"] = data;
05562         parameter["ref_freq_bin"] = ref_freq_hist;
05563         parameter["size_img"]=size_img;
05564         parameter["hist_len"]=hist_len;
05565         /* ===================================================== */
05566 
05567         return parameter;
05568 }
05569 
05570 
05571 float Util::hist_comp_freq(float PA,float PB,int size_img, int hist_len, EMData *img, vector<float> ref_freq_hist, EMData *mask, float ref_h_diff, float ref_h_min)
05572 {
05573         float *img_ptr = img->get_data();
05574         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05575 
05576         int *img_freq_bin = new int[3*hist_len];
05577         for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05578         for(int i = 0;i < size_img;i++) {
05579                 if(mask_ptr[i] > 0.5f) {
05580                         float img_xn = img_ptr[i]*PA + PB;
05581                         int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05582                         if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05583                 }
05584         }
05585         int freq_hist = 0;
05586 
05587         for(int i = 0;i < (3*hist_len);i++) freq_hist += (int)pow((float)((int)ref_freq_hist[i] - (int)img_freq_bin[i]),2.f);
05588         freq_hist = (-freq_hist);
05589         return static_cast<float>(freq_hist);
05590 }
05591 //------------------------------------------------------------------------------------------------------------------------------------------------------------------
05592 #define    QUADPI                       3.141592653589793238462643383279502884197
05593 #define    DGR_TO_RAD                   QUADPI/180
05594 #define    DM(I)                        DM          [I-1]
05595 #define    SS(I)                        SS          [I-1]
05596 Dict Util::CANG(float PHI,float THETA,float PSI)
05597 {
05598         double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05599         vector<float>   DM,SS;
05600 
05601         for(int i =0;i<9;i++) DM.push_back(0);
05602 
05603         for(int i =0;i<6;i++) SS.push_back(0);
05604 
05605         CPHI = cos(double(PHI)*DGR_TO_RAD);
05606         SPHI = sin(double(PHI)*DGR_TO_RAD);
05607         CTHE = cos(double(THETA)*DGR_TO_RAD);
05608         STHE = sin(double(THETA)*DGR_TO_RAD);
05609         CPSI = cos(double(PSI)*DGR_TO_RAD);
05610         SPSI = sin(double(PSI)*DGR_TO_RAD);
05611 
05612         SS(1) = float(CPHI);
05613         SS(2) = float(SPHI);
05614         SS(3) = float(CTHE);
05615         SS(4) = float(STHE);
05616         SS(5) = float(CPSI);
05617         SS(6) = float(SPSI);
05618 
05619         DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05620         DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05621         DM(3) = float(-STHE*CPSI);
05622         DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05623         DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05624         DM(6) = float(STHE*SPSI);
05625         DM(7) = float(STHE*CPHI);
05626         DM(8) = float(STHE*SPHI);
05627         DM(9) = float(CTHE);
05628 
05629         Dict DMnSS;
05630         DMnSS["DM"] = DM;
05631         DMnSS["SS"] = SS;
05632 
05633         return(DMnSS);
05634 }
05635 #undef SS
05636 #undef DM
05637 #undef QUADPI
05638 #undef DGR_TO_RAD
05639 //-----------------------------------------------------------------------------------------------------------------------
05640 #define    DM(I)                        DM[I-1]
05641 #define    B(i,j)                       Bptr[i-1+((j-1)*NSAM)]
05642 #define    CUBE(i,j,k)                  CUBEptr[(i-1)+((j-1)+((k-1)*NY3D))*NX3D]
05643 
05644 void Util::BPCQ(EMData *B,EMData *CUBE, vector<float> DM)
05645 {
05646 
05647         float  *Bptr = B->get_data();
05648         float  *CUBEptr = CUBE->get_data();
05649 
05650         int NSAM,NROW,NX3D,NY3D,NZC,KZ,IQX,IQY,LDPX,LDPY,LDPZ,LDPNMX,LDPNMY,NZ1;
05651         float DIPX,DIPY,XB,YB,XBB,YBB;
05652 
05653         Transform * t = B->get_attr("xform.projection");
05654         Dict d = t->get_params("spider");
05655         if(t) {delete t; t=0;}
05656         //  Unsure about sign of shifts, check later PAP 06/28/09
05657         float x_shift = d[ "tx" ];
05658         float y_shift = d[ "ty" ];
05659         x_shift = -x_shift;
05660         y_shift = -y_shift;
05661 
05662         NSAM = B->get_xsize();
05663         NROW = B->get_ysize();
05664         NX3D = CUBE->get_xsize();
05665         NY3D = CUBE->get_ysize();
05666         NZC  = CUBE->get_zsize();
05667 
05668 
05669         LDPX   = NX3D/2 +1;
05670         LDPY   = NY3D/2 +1;
05671         LDPZ   = NZC/2 +1;
05672         LDPNMX = NSAM/2 +1;
05673         LDPNMY = NROW/2 +1;
05674         NZ1    = 1;
05675 
05676         for(int K=1;K<=NZC;K++) {
05677                 KZ=K-1+NZ1;
05678                 for(int J=1;J<=NY3D;J++) {
05679                         XBB = (1-LDPX)*DM(1)+(J-LDPY)*DM(2)+(KZ-LDPZ)*DM(3);
05680                         YBB = (1-LDPX)*DM(4)+(J-LDPY)*DM(5)+(KZ-LDPZ)*DM(6);
05681                         for(int I=1;I<=NX3D;I++) {
05682                                 XB  = (I-1)*DM(1)+XBB-x_shift;
05683                                 IQX = int(XB+float(LDPNMX));
05684                                 if (IQX <1 || IQX >= NSAM) continue;
05685                                 YB  = (I-1)*DM(4)+YBB-y_shift;
05686                                 IQY = int(YB+float(LDPNMY));
05687                                 if (IQY<1 || IQY>=NROW)  continue;
05688                                 DIPX = XB+LDPNMX-IQX;
05689                                 DIPY = YB+LDPNMY-IQY;
05690 
05691                                 CUBE(I,J,K) = CUBE(I,J,K)+B(IQX,IQY)+DIPY*(B(IQX,IQY+1)-B(IQX,IQY))+DIPX*(B(IQX+1,IQY)-B(IQX,IQY)+DIPY*(B(IQX+1,IQY+1)-B(IQX+1,IQY)-B(IQX,IQY+1)+B(IQX,IQY)));
05692                         }
05693                 }
05694         }
05695 }
05696 
05697 #undef DM
05698 #undef B
05699 #undef CUBE
05700 
05701 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05702 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
05703 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
05704 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
05705 
05706 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05707 {
05708         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05709         float WW,OX,OY;
05710 
05711         NSAM = PROJ->get_xsize();
05712         NROW = PROJ->get_ysize();
05713         int ntotal = NSAM*NROW;
05714         float q = 2.0f;
05715         float qt = 8.0f/q;
05716         //  Fix for padding 2x
05717         int ipad = 1;
05718         NSAM *= ipad;
05719         NROW *= ipad;
05720         NNNN = NSAM+2-(NSAM%2);
05721         int NX2 = NSAM/2;
05722         NR2  = NROW/2;
05723 
05724         NANG = int(SS.size())/6;
05725 
05726         EMData* W = new EMData();
05727         int Wnx = NNNN/2;
05728         W->set_size(Wnx,NROW,1);
05729         W->to_zero();
05730         float *Wptr = W->get_data();
05731         float *PROJptr = PROJ->get_data();
05732         for (L=1; L<=NANG; L++) {
05733                 float  tmp1 = SS(3,K)*SS(4,L)*(SS(1,K)*SS(1,L) + SS(2,K)*SS(2,L)) - SS(3,L)*SS(4,K);
05734                 float  tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) ); 
05735                 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05736                 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05737                 if(OX < 0.0f) {
05738                         OX = -OX;
05739                         OY = -OY;
05740                 }
05741 
05742                 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f ) {
05743                         for(int J=1;J<=NROW;J++) {
05744                                 JY = (J-1);
05745                                 if(JY > NR2) JY -= NROW;
05746 #ifdef _WIN32
05747                                 int xma = _cpp_min(int(0.5f+(q-JY*OY)/OX),NX2);
05748                                 int xmi = _cpp_max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05749 #else
05750                                 int xma = std::min(int(0.5f+(q-JY*OY)/OX),NX2);
05751                                 int xmi = std::max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05752 #endif  //_WIN32
05753                                 if( xmi <= xma) {
05754                                         for(int I=xmi;I<=xma;I++) {
05755                                                 float Y = fabs(OX*I + OY*JY);
05756                                                 W(I+1,J) += exp(-qt*Y*Y);
05757         //cout << " L   "<<L << " I   "<<I << " JY   "<<JY << " ARG   "<<qt*Y*Y <<endl;
05758                                         }
05759                                 }
05760                         }
05761                 } else {
05762                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
05763                 }
05764         }
05765         EMData* proj_in = PROJ;
05766 
05767         PROJ = PROJ->norm_pad( false, ipad);
05768         PROJ->do_fft_inplace();
05769         PROJ->update();
05770         //cout << " x   "<<PROJ->get_xsize() << " y   "<<PROJ->get_ysize() <<endl;
05771         PROJptr = PROJ->get_data();
05772 
05773         float WNRMinv,temp;
05774         float osnr = 1.0f/SNR;
05775         WNRMinv = 1.0f/W(1,1);
05776         for(int J=1;J<=NROW;J++)  {
05777                 JY = J-1;
05778                 if( JY > NR2)  JY -= NROW;
05779                 float sy = JY;
05780                 sy /= NROW;
05781                 sy *= sy;
05782                 for(int I=1;I<=NNNN;I+=2) {
05783                         KX           = (I+1)/2;
05784                         temp         = W(KX,J)*WNRMinv;
05785                         WW           = temp/(temp*temp + osnr);
05786                         // This is supposed to fix fall-off due to Gaussian function in the weighting function
05787                         float sx = KX-1;
05788                         sx /= NSAM;
05789                         WW *= exp(qt*(sy + sx*sx));
05790                         PROJ(I,J)   *= WW;
05791                         PROJ(I+1,J) *= WW;
05792                 }
05793         }
05794         delete W; W = 0;
05795         PROJ->do_ift_inplace();
05796         PROJ->depad();
05797 
05798         float* data_src = PROJ->get_data();
05799         float* data_dst = proj_in->get_data();
05800 
05801         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
05802 
05803         proj_in->update();
05804 
05805         delete PROJ;
05806 }
05807 /*
05808 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05809 {
05810         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05811         float WW,OX,OY,Y;
05812 
05813         NSAM = PROJ->get_xsize();
05814         NROW = PROJ->get_ysize();
05815         //  Fix for padding 2x
05816         int ntotal = NSAM*NROW;
05817         int ipad = 1;
05818         NSAM *= ipad;
05819         NROW *= ipad;
05820         NNNN = NSAM+2-(NSAM%2);
05821         NR2  = NROW/2;
05822 
05823         NANG = int(SS.size())/6;
05824 
05825         EMData* W = new EMData();
05826         int Wnx = NNNN/2;
05827         W->set_size(Wnx,NROW,1);
05828         W->to_zero();
05829         float *Wptr = W->get_data();
05830         float *PROJptr = PROJ->get_data();
05831         for (L=1; L<=NANG; L++) {
05832                 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);
05833                 float  tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) ); 
05834                 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05835                 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05836         //cout << " OX   "<<OX << " OY   "<<OY <<endl;
05837 
05838                 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f) {
05839                         for(int J=1;J<=NROW;J++) {
05840                                 JY = (J-1);
05841                                 if(JY > NR2) JY=JY-NROW;
05842                                 for(int I=1;I<=NNNN/2;I++) {
05843                                         Y =  fabs(OX * (I-1) + OY * JY);
05844                                         if(Y < 2.0f) {
05845                                         W(I,J) += exp(-4*Y*Y);
05846         cout << " L   "<<L << " I   "<<I-1 << " JY   "<<JY << " ARG   "<<4*Y*Y<<endl;}
05847                                 }
05848                         }
05849                 } else {
05850                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
05851                 }
05852         }
05853         EMData* proj_in = PROJ;
05854 
05855         PROJ = PROJ->norm_pad( false, ipad);
05856         PROJ->do_fft_inplace();
05857         PROJ->update();
05858         //cout << " x   "<<PROJ->get_xsize() << " y   "<<PROJ->get_ysize() <<endl;
05859         PROJptr = PROJ->get_data();
05860 
05861         float WNRMinv,temp;
05862         float osnr = 1.0f/SNR;
05863         WNRMinv = 1.0f/W(1,1);
05864         for(int J=1;J<=NROW;J++)
05865                 for(int I=1;I<=NNNN;I+=2) {
05866                         KX           = (I+1)/2;
05867                         temp         = W(KX,J)*WNRMinv;
05868                         WW           = temp/(temp*temp + osnr);
05869                         PROJ(I,J)   *= WW;
05870                         PROJ(I+1,J) *= WW;
05871                 }
05872         delete W; W = 0;
05873         PROJ->do_ift_inplace();
05874         PROJ->depad();
05875 
05876         float* data_src = PROJ->get_data();
05877         float* data_dst = proj_in->get_data();
05878 
05879         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
05880 
05881         proj_in->update();
05882 
05883         delete PROJ;
05884 }
05885 */
05886 #undef PROJ
05887 #undef W
05888 #undef SS
05889 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05890 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
05891 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
05892 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
05893 #define    RI(i,j)                      RI          [(i-1) + ((j-1)*3)]
05894 #define    CC(i)                        CC          [i-1]
05895 #define    CP(i)                        CP          [i-1]
05896 #define    VP(i)                        VP          [i-1]
05897 #define    VV(i)                        VV          [i-1]
05898 #define    AMAX1(i,j)                   i>j?i:j
05899 #define    AMIN1(i,j)                   i<j?i:j
05900 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
05901 {
05902         float rad2deg =(180.0f/3.1415926f);
05903         float deg2rad = (3.1415926f/180.0f);
05904 
05905         int NSAM,NROW,NNNN,NR2,NANG,L,JY;
05906 
05907         NSAM = PROJ->get_xsize();
05908         NROW = PROJ->get_ysize();
05909         NNNN = NSAM+2-(NSAM%2);
05910         NR2  = NROW/2;
05911         NANG = int(SS.size())/6;
05912 
05913         float RI[9];
05914         RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
05915         RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
05916         RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
05917         RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
05918         RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
05919         RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
05920         RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
05921         RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
05922         RI(3,3)=SS(3,NUMP);
05923 
05924         float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
05925 
05926         EMData* W = new EMData();
05927         int Wnx = NNNN/2;
05928         W->set_size(NNNN/2,NROW,1);
05929         W->to_one();
05930         float *Wptr = W->get_data();
05931 
05932         float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
05933 
05934         for (L=1; L<=NANG; L++) {
05935                 if (L != NUMP) {
05936                         CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
05937                         CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
05938                         CC(3)=SS(1,L)*SS(4,L)*SS(2,NUMP)*SS(4,NUMP)-SS(2,L)*SS(4,L)*SS(1,NUMP)*SS(4,NUMP);
05939 
05940                         TMP = sqrt(CC(1)*CC(1) +  CC(2)*CC(2) + CC(3)*CC(3));
05941                         CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
05942                         ALPHA=rad2deg*float(asin(CCN));
05943                         if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
05944                         if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
05945                         if(ALPHA<1.0E-6) {
05946                                 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
05947                         } else {
05948                                 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
05949                                 CC(1)   = CC(1)/CCN;CC(2)   = CC(2)/CCN;CC(3)   = CC(3)/CCN;
05950                                 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
05951                                 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
05952                                 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
05953                                 CP(1)   = 0.0;CP(2) = 0.0;
05954                                 VP(1)   = 0.0;VP(2) = 0.0;
05955 
05956                                 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
05957                                 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
05958                                 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
05959                                 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
05960 
05961                                 TMP = CP(1)*VP(2)-CP(2)*VP(1);
05962 
05963                                 //     PREVENT TMP TO BE TOO SMALL, SIGN IS IRRELEVANT
05964                                 TMP = AMAX1(1.0E-4f,fabs(TMP));
05965                                 float tmpinv = 1.0f/TMP;
05966                                 for(int J=1;J<=NROW;J++) {
05967                                         JY = (J-1);
05968                                         if (JY>NR2)  JY=JY-NROW;
05969                                         for(int I=1;I<=NNNN/2;I++) {
05970                                                 FV     = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
05971                                                 RT     = 1.0f-FV/FM;
05972                                                 W(I,J) += ((RT>0.0f)*RT);
05973                                         }
05974                                 }
05975                         }
05976                 }
05977         }
05978 
05979         EMData* proj_in = PROJ;
05980 
05981         PROJ = PROJ->norm_pad( false, 1);
05982         PROJ->do_fft_inplace();
05983         PROJ->update();
05984         float *PROJptr = PROJ->get_data();
05985 
05986         int KX;
05987         float WW;
05988         for(int J=1; J<=NROW; J++)
05989                 for(int I=1; I<=NNNN; I+=2) {
05990                         KX          =  (I+1)/2;
05991                         WW          =  1.0f/W(KX,J);
05992                         PROJ(I,J)   = PROJ(I,J)*WW;
05993                         PROJ(I+1,J) = PROJ(I+1,J)*WW;
05994                 }
05995         delete W; W = 0;
05996         PROJ->do_ift_inplace();
05997         PROJ->depad();
05998 
05999         float* data_src = PROJ->get_data();
06000         float* data_dst = proj_in->get_data();
06001 
06002         int ntotal = NSAM*NROW;
06003         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
06004 
06005         proj_in->update();
06006         delete PROJ;
06007 }
06008 #undef   AMAX1
06009 #undef   AMIN1
06010 #undef   RI
06011 #undef   CC
06012 #undef   CP
06013 #undef   VV
06014 #undef   VP
06015 
06016 
06017 #undef   W
06018 #undef   SS
06019 #undef   PROJ
06020 
06021 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06022 {
06023         float cst  = cs*1.0e7f;
06024 
06025         wgh /= 100.0;
06026         float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06027         float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06028         float ak2 = ak*ak;
06029         float g1 = dzz*1.0e4f*lambda*ak2;
06030         float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06031 
06032         float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06033         if(b_factor != 0.0f)  ctfv *= exp(-b_factor*ak2/4.0f);
06034 
06035         return ctfv;
06036 }
06037 
06038 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06039 {
06040         /***********
06041         ***get the size of the image for validation purpose
06042         **************/
06043         int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();  //Aren't  these  implied?  Please check and let me know, PAP.
06044         /********
06045         ***Exception Handle
06046         *************/
06047         if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06048                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06049 
06050         int i, size = nx*ny*nz;
06051 
06052         float* img_ptr = image->get_data();
06053         float* mask_ptr = mask->get_data();
06054 
06055         int ln=0;  //length of the output image = number of points under the mask.
06056         for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06057 
06058         EMData* new_image = new EMData();
06059         new_image->set_size(ln,1,1); /* set size of the new image */
06060         float *new_ptr    = new_image->get_data();
06061 
06062         ln=-1;
06063         for(i = 0;i < size;i++){
06064                 if(mask_ptr[i] > 0.5f) {
06065                         ln++;
06066                         new_ptr[ln]=img_ptr[i];
06067                 }
06068         }
06069 
06070         return new_image;
06071 }
06072 
06073 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06074 {
06075         /********
06076         ***Exception Handle
06077         *************/
06078         if(mask == NULL)
06079                 throw ImageDimensionException("The mask cannot be an null image");
06080 
06081         /***********
06082         ***get the size of the mask
06083         **************/
06084         int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06085 
06086         int i,size = nx*ny*nz;                   /* loop counters */
06087         /* new image declaration */
06088         EMData *new_image = new EMData();
06089         new_image->set_size(nx,ny,nz);           /* set the size of new image */
06090         float *new_ptr  = new_image->get_data(); /* set size of the new image */
06091         float *mask_ptr = mask->get_data();      /* assign a pointer to the mask image */
06092         float *img_ptr  = image->get_data();     /* assign a pointer to the 1D image */
06093         int count = 0;
06094         float sum_under_mask = 0.0 ;
06095         for(i = 0;i < size;i++){
06096                         if(mask_ptr[i] > 0.5f){
06097                                 new_ptr[i] = img_ptr[count];
06098                                 sum_under_mask += img_ptr[count];
06099                                 count++;
06100                                 if( count > image->get_xsize() ) {
06101                                     throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06102                                 }
06103                         }
06104         }
06105 
06106         if( count > image->get_xsize() ) {
06107             throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06108         }
06109 
06110         float avg_under_mask = sum_under_mask / count;
06111         for(i = 0;i < size;i++) {
06112                 if(mask_ptr[i] <= 0.5f)  new_ptr[i] = avg_under_mask;
06113         }
06114         new_image->update();
06115         return new_image;
06116 }
06117 
06118 
06119 
06120 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06121 {
06122         vector<float>new_peak;
06123         int n1=peak1.size()/3;
06124         float p_size2=p_size*p_size;
06125         for (int i=0;i<n1;++i) {
06126                 vector<float>::iterator it2= peak1.begin()+3*i;
06127                 bool push_back1=true;
06128                 int n2=peak2.size()/3;
06129                 /*cout<<"peak2 size==="<<n2<<"i====="<<i<<endl;
06130                        cout<<"new peak size==="<<new_peak.size()/3<<endl;*/
06131                 if(n2 ==0) {
06132                         new_peak.push_back(*it2);
06133                         new_peak.push_back(*(it2+1));
06134                         new_peak.push_back(*(it2+2));
06135                 } else  {
06136                         int j=0;
06137                         while (j< n2-1 ) {
06138                                 vector<float>::iterator it3= peak2.begin()+3*j;
06139                                 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06140                                 if(d2< p_size2 ) {
06141                                         if( (*it2)<(*it3) ) {
06142                                                 new_peak.push_back(*it3);
06143                                                 new_peak.push_back(*(it3+1));
06144                                                 new_peak.push_back(*(it3+2));
06145                                                 peak2.erase(it3);
06146                                                 peak2.erase(it3);
06147                                                 peak2.erase(it3);
06148                                                 push_back1=false;
06149                                         } else {
06150                                                 peak2.erase(it3);
06151                                                 peak2.erase(it3);
06152                                                 peak2.erase(it3);
06153                                         }
06154                                 } else  j=j+1;
06155                                 n2=peak2.size()/3;
06156                         }
06157                         if(push_back1) {
06158                                 new_peak.push_back(*it2);
06159                                 new_peak.push_back(*(it2+1));
06160                                 new_peak.push_back(*(it2+2));
06161                         }
06162                 }
06163         }
06164         return new_peak;
06165 }
06166 
06167 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06168 {
06169         // n size of the covariance/correlation matrix
06170         // covmat --- covariance/correlation matrix (n by n)
06171         // eigval --- returns eigenvalues
06172         // eigvec --- returns eigenvectors
06173 
06174         ENTERFUNC;
06175 
06176         int i;
06177 
06178         // make a copy of covmat so that it will not be overwritten
06179         for ( i = 0 ; i < n * n ; i++ )   eigvec[i] = covmat[i];
06180 
06181         char NEEDV = 'V';
06182         char UPLO = 'U';
06183         int lwork = -1;
06184         int info = 0;
06185         float *work, wsize;
06186 
06187         //  query to get optimal workspace
06188         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06189         lwork = (int)wsize;
06190 
06191         work = (float *)calloc(lwork, sizeof(float));
06192         //  calculate eigs
06193         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06194         free(work);
06195         EXITFUNC;
06196         return info;
06197 }
06198 
06199 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06200 {
06201 
06202         ENTERFUNC;
06203         int len = covmatpy.size();
06204         float *eigvec;
06205         float *eigval;
06206         float *covmat;
06207         int status = 0;
06208         eigval = (float*)calloc(ncov,sizeof(float));
06209         eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06210         covmat = (float*)calloc(ncov*ncov, sizeof(float));
06211 
06212         const float *covmat_ptr;
06213         covmat_ptr = &covmatpy[0];
06214         for(int i=0;i<len;i++){
06215             covmat[i] = covmat_ptr[i];
06216         }
06217 
06218         status = Util::coveig(ncov, covmat, eigval, eigvec);
06219 
06220         vector<float> eigval_py(ncov);
06221         const float *eigval_ptr;
06222         eigval_ptr = &eigval[0];
06223         for(int i=0;i<ncov;i++){
06224             eigval_py[i] = eigval_ptr[i];
06225         }
06226 
06227         vector<float> eigvec_py(ncov*ncov);
06228         const float *eigvec_ptr;
06229         eigvec_ptr = &eigvec[0];
06230         for(int i=0;i<ncov*ncov;i++){
06231             eigvec_py[i] = eigvec_ptr[i];
06232         }
06233 
06234         Dict res;
06235         res["eigval"] = eigval_py;
06236         res["eigvec"] = eigvec_py;
06237 
06238         EXITFUNC;
06239         return res;
06240 }
06241 
06242 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06243 {
06244         int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06245 
06246         k=(int)pw.size();
06247         l=0;
06248         m=k;
06249         n2=n+2;
06250         n1=n+1;
06251         klmd=k+l+m;
06252         klm2d= k+l+m+2;
06253         nklmd=k+l+m+n;
06254         n2d=n+2;
06255         /*size has to be increased when N is large*/
06256         n_larg=klmd*2;
06257         klm2d=n_larg+klm2d;
06258         klmd=n_larg+klmd;
06259         nklmd=n_larg+nklmd;
06260         int size_q=klm2d*n2d;
06261         int size_cu=nklmd*2;
06262         static int i__;
06263 
06264          double *q ;
06265          double *x ;
06266          double *res;
06267          double *cu;
06268          float *q2;
06269          float *pw_;
06270          long int *iu;
06271          double *s;
06272          q = (double*)calloc(size_q,sizeof(double));
06273          x = (double*)calloc(n2d,sizeof(double));
06274          res = (double*)calloc(klmd,sizeof(double));
06275          cu =(double*)calloc(size_cu,sizeof(double));
06276          s = (double*)calloc(klmd,sizeof(double));
06277          q2 = (float*)calloc(size_q,sizeof(float));
06278          iu = (long int*)calloc(size_cu,sizeof(long int));
06279          pw_ = (float*)calloc(k,sizeof(float));
06280 
06281         for( i__ =0;i__<k;++i__)
06282                 {
06283                 pw_[i__]=log(pw[i__]); }
06284         long int l_k=k;
06285         long int l_n=n;
06286         long int l_iswi=iswi;
06287         vector<float> cl1_res;
06288         cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06289         free(q);
06290         free(x);
06291         free(res);
06292         free(s);
06293         free(cu);
06294         free(q2);
06295         free(iu);
06296         free(pw_);
06297         return cl1_res;
06298 }
06299 vector<float> Util::call_cl1(long int *k, long int *n, float *ps, long int *iswi, float *pw, float *q2,double *q, double *x, double *res, double *cu, double *s, long int *iu)
06300 {
06301     long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06302     float r__1;
06303     int tmp__i;
06304     long int i__, j;
06305     --s;
06306     --res;
06307     iu -= 3;
06308     cu -= 3;
06309     --x;
06310     long int klm2d;
06311     klm2d= *k+*k+2;
06312     klm2d=klm2d+klm2d;
06313     q_dim1 = klm2d;
06314     q_offset = 1 + q_dim1;
06315     q -= q_offset;
06316     q2_dim1 = klm2d;
06317     q2_offset = 1 + q2_dim1;
06318     q2 -= q2_offset;
06319     i__2=0;
06320     i__1 = *n - 1;
06321     tmp__i=0;
06322     for (j = 1; j <= i__1; ++j) {
06323         i__2 = *k;
06324         tmp__i+=1;
06325         for (i__ = 1; i__ <= i__2; ++i__) {
06326             r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06327             q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06328         }
06329     }
06330     for  (i__ = 1; i__ <= i__2; ++i__)
06331       { q2[i__ + *n * q2_dim1] = 1.f;
06332             q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06333         }
06334    vector<float> fit_res;
06335    fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06336    return fit_res;
06337 }
06338 vector<float> Util::lsfit(long int *ks,long int *n, long int *klm2d, long int *iswi,float *q1,double *q, double *x, double *res, double *cu, double *s, long int *iu)
06339 {
06340     /* System generated locals */
06341     long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06342 
06343     /* Local variables */
06344     long int i__, j, m, n1, ii, jj;
06345     double tmp;
06346     vector<float> p;
06347     --x;
06348     q_dim1 = *klm2d;
06349     q_offset = 1 + q_dim1;
06350     q -= q_offset;
06351     q1_dim1 = *klm2d;
06352     q1_offset = 1 + q1_dim1;
06353     q1 -= q1_offset;
06354     --s;
06355     --res;
06356     iu -= 3;
06357     cu -= 3;
06358 
06359     /* Function Body */
06360     long int l = 0;
06361 
06362 /* C==ZHONG HUANG,JULY,12,02;L=0,1,2,3,4,5,6 correspond to different equality constraints */
06363     m = *ks;
06364     n1 = *n + 1;
06365     if (*iswi == 1) {
06366         i__1 = n1;
06367         for (jj = 1; jj <= i__1; ++jj) {
06368             i__2 = *ks;
06369             for (ii = 1; ii <= i__2; ++ii) {
06370         /*      q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];*/
06371 
06372                 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06373                         ;
06374             }
06375         }
06376     } else if (*iswi == 2) {
06377         i__1 = *ks;
06378         for (ii = 1; ii <= i__1; ++ii) {
06379             i__2 = n1;
06380             for (jj = 1; jj <= i__2; ++jj) {
06381                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06382                 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06383             }
06384         }
06385     } else if (*iswi == 3) {
06386         l = 2;
06387         i__1 = n1;
06388         for (jj = 1; jj <= i__1; ++jj) {
06389             i__2 = *ks + 2;
06390             for (ii = 1; ii <= i__2; ++ii) {
06391                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06392             }
06393             i__2 = *ks;
06394             for (ii = 1; ii <= i__2; ++ii) {
06395                 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06396             }
06397         }
06398     } else if (*iswi == 4) {
06399         l = 2;
06400         i__1 = n1;
06401         for (jj = 1; jj <= i__1; ++jj) {
06402             i__2 = *ks + 2;
06403             for (ii = 1; ii <= i__2; ++ii) {
06404                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06405             }
06406             i__2 = *ks;
06407             for (ii = 1; ii <= i__2; ++ii) {
06408                 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06409             }
06410         }
06411     } else if (*iswi == 5) {
06412         l = 1;
06413         i__1 = n1;
06414         for (jj = 1; jj <= i__1; ++jj) {
06415             i__2 = *ks + 1;
06416             for (ii = 1; ii <= i__2; ++ii) {
06417                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06418             }
06419             i__2 = *ks;
06420             for (ii = 1; ii <= i__2; ++ii) {
06421                 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06422             }
06423         }
06424     } else if (*iswi == 6) {
06425         l = 1;
06426         i__1 = n1;
06427         for (jj = 1; jj <= i__1; ++jj) {
06428             i__2 = *ks + 1;
06429             for (ii = 1; ii <= i__2; ++ii) {
06430                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06431             }
06432             i__2 = *ks;
06433             for (ii = 1; ii <= i__2; ++ii) {
06434                 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06435             }
06436         }
06437     } else if (*iswi == 7) {
06438         l = 3;
06439         i__1 = n1;
06440         for (jj = 1; jj <= i__1; ++jj) {
06441             i__2 = *ks + 3;
06442             for (ii = 1; ii <= i__2; ++ii) {
06443                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06444             }
06445             i__2 = *ks;
06446             for (ii = 1; ii <= i__2; ++ii) {
06447                 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06448             }
06449         }
06450     } else if (*iswi == 8) {
06451         l = 4;
06452         i__1 = n1;
06453         for (jj = 1; jj <= i__1; ++jj) {
06454             i__2 = *ks + 4;
06455             for (ii = 1; ii <= i__2; ++ii) {
06456                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06457             }
06458             i__2 = *ks;
06459             for (ii = 1; ii <= i__2; ++ii) {
06460                 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06461             }
06462         }
06463     }
06464 
06465     Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06466     i__1 = *ks;
06467     int tmp__j=0;
06468     for (i__ = 1; i__ <= i__1; ++i__) {
06469         tmp = 0.f;
06470         i__2 = *n - 1;
06471         for (j = 1; j <= i__2; ++j) {
06472         tmp__j=j;
06473             tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06474         }
06475         tmp += x[*n];
06476         p.push_back(static_cast<float>(exp(tmp)));
06477         p.push_back(q1[i__ + q1_dim1]);
06478     }
06479     i__2=*n;
06480     for (i__=1;i__<=i__2;++i__)
06481         { p.push_back(static_cast<float>(x[i__]));}
06482     return p;
06483 }
06484 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06485         double *q, double *x, double *res, double *cu, long int *iu, double *s)
06486 {
06487 
06488     long int q_dim1, q_offset, i__1, i__2;
06489     double d__1;
06490 
06491     static long int i__, j;
06492     static double z__;
06493     static long int n1, n2, ia, ii, kk, in, nk, js;
06494     static double sn, zu, zv;
06495     static long int nk1, klm, nkl, jmn, jpn;
06496     static double cuv;
06497     static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06498     static float xmin;
06499     static double xmax;
06500     static long int iout;
06501     static double xsum;
06502     static long int iineg, maxit;
06503     static double toler;
06504     static float error;
06505     static double pivot;
06506     static long int kforce, iphase;
06507     static double tpivot;
06508 
06509     --s;
06510     --res;
06511     iu -= 3;
06512     cu -= 3;
06513     --x;
06514     q_dim1 = *klm2d;
06515     q_offset = 1 + q_dim1;
06516     q -= q_offset;
06517 
06518     /* Function Body */
06519     maxit = 500;
06520     kode = 0;
06521     toler = 1e-4f;
06522     iter = 0;
06523     n1 = *n + 1;
06524     n2 = *n + 2;
06525     nk = *n + *k;
06526     nk1 = nk + 1;
06527     nkl = nk + *l;
06528     nkl1 = nkl + 1;
06529     klm = *k + *l + *m;
06530     klm1 = klm + 1;
06531     klm2 = klm + 2;
06532     nklm = *n + klm;
06533     kforce = 1;
06534     iter = 0;
06535     js = 1;
06536     ia = 0;
06537 /* SET UP LABELS IN Q. */
06538     i__1 = *n;
06539     for (j = 1; j <= i__1; ++j) {
06540         q[klm2 + j * q_dim1] = (double) j;
06541 /* L10: */
06542     }
06543     i__1 = klm;
06544     for (i__ = 1; i__ <= i__1; ++i__) {
06545         q[i__ + n2 * q_dim1] = (double) (*n + i__);
06546         if (q[i__ + n1 * q_dim1] >= 0.f) {
06547             goto L30;
06548         }
06549         i__2 = n2;
06550         for (j = 1; j <= i__2; ++j) {
06551             q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06552 /* L20: */
06553         }
06554 L30:
06555         ;
06556     }
06557 /* SET UP PHASE 1 COSTS. */
06558     iphase = 2;
06559     i__1 = nklm;
06560     for (j = 1; j <= i__1; ++j) {
06561         cu[(j << 1) + 1] = 0.f;
06562         cu[(j << 1) + 2] = 0.f;
06563         iu[(j << 1) + 1] = 0;
06564         iu[(j << 1) + 2] = 0;
06565 /* L40: */
06566     }
06567     if (*l == 0) {
06568         goto L60;
06569     }
06570     i__1 = nkl;
06571     for (j = nk1; j <= i__1; ++j) {
06572         cu[(j << 1) + 1] = 1.f;
06573         cu[(j << 1) + 2] = 1.f;
06574         iu[(j << 1) + 1] = 1;
06575         iu[(j << 1) + 2] = 1;
06576 /* L50: */
06577     }
06578     iphase = 1;
06579 L60:
06580     if (*m == 0) {
06581         goto L80;
06582     }
06583     i__1 = nklm;
06584     for (j = nkl1; j <= i__1; ++j) {
06585         cu[(j << 1) + 2] = 1.f;
06586         iu[(j << 1) + 2] = 1;
06587         jmn = j - *n;
06588         if (q[jmn + n2 * q_dim1] < 0.f) {
06589             iphase = 1;
06590         }
06591 /* L70: */
06592     }
06593 L80:
06594     if (kode == 0) {
06595         goto L150;
06596     }
06597     i__1 = *n;
06598     for (j = 1; j <= i__1; ++j) {
06599         if ((d__1 = x[j]) < 0.) {
06600             goto L90;
06601         } else if (d__1 == 0) {
06602             goto L110;
06603         } else {
06604             goto L100;
06605         }
06606 L90:
06607         cu[(j << 1) + 1] = 1.f;
06608         iu[(j << 1) + 1] = 1;
06609         goto L110;
06610 L100:
06611         cu[(j << 1) + 2] = 1.f;
06612         iu[(j << 1) + 2] = 1;
06613 L110:
06614         ;
06615     }
06616     i__1 = *k;
06617     for (j = 1; j <= i__1; ++j) {
06618         jpn = j + *n;
06619         if ((d__1 = res[j]) < 0.) {
06620             goto L120;
06621         } else if (d__1 == 0) {
06622             goto L140;
06623         } else {
06624             goto L130;
06625         }
06626 L120:
06627         cu[(jpn << 1) + 1] = 1.f;
06628         iu[(jpn << 1) + 1] = 1;
06629         if (q[j + n2 * q_dim1] > 0.f) {
06630             iphase = 1;
06631         }
06632         goto L140;
06633 L130:
06634         cu[(jpn << 1) + 2] = 1.f;
06635         iu[(jpn << 1) + 2] = 1;
06636         if (q[j + n2 * q_dim1] < 0.f) {
06637             iphase = 1;
06638         }
06639 L140:
06640         ;
06641     }
06642 L150:
06643     if (iphase == 2) {
06644         goto L500;
06645     }
06646 /* COMPUTE THE MARGINAL COSTS. */
06647 L160:
06648     i__1 = n1;
06649     for (j = js; j <= i__1; ++j) {
06650         xsum = 0.;
06651         i__2 = klm;
06652         for (i__ = 1; i__ <= i__2; ++i__) {
06653             ii = (long int) q[i__ + n2 * q_dim1];
06654             if (ii < 0) {
06655                 goto L170;
06656             }
06657             z__ = cu[(ii << 1) + 1];
06658             goto L180;
06659 L170:
06660             iineg = -ii;
06661             z__ = cu[(iineg << 1) + 2];
06662 L180:
06663             xsum += q[i__ + j * q_dim1] * z__;
06664 /*  180       XSUM = XSUM + Q(I,J)*Z */
06665 /* L190: */
06666         }
06667         q[klm1 + j * q_dim1] = xsum;
06668 /* L200: */
06669     }
06670     i__1 = *n;
06671     for (j = js; j <= i__1; ++j) {
06672         ii = (long int) q[klm2 + j * q_dim1];
06673         if (ii < 0) {
06674             goto L210;
06675         }
06676         z__ = cu[(ii << 1) + 1];
06677         goto L220;
06678 L210:
06679         iineg = -ii;
06680         z__ = cu[(iineg << 1) + 2];
06681 L220:
06682         q[klm1 + j * q_dim1] -= z__;
06683 /* L230: */
06684     }
06685 /* DETERMINE THE VECTOR TO ENTER THE BASIS. */
06686 L240:
06687     xmax = 0.f;
06688     if (js > *n) {
06689         goto L490;
06690     }
06691     i__1 = *n;
06692     for (j = js; j <= i__1; ++j) {
06693         zu = q[klm1 + j * q_dim1];
06694         ii = (long int) q[klm2 + j * q_dim1];
06695         if (ii > 0) {
06696             goto L250;
06697         }
06698         ii = -ii;
06699         zv = zu;
06700         zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06701         goto L260;
06702 L250:
06703         zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06704 L260:
06705         if (kforce == 1 && ii > *n) {
06706             goto L280;
06707         }
06708         if (iu[(ii << 1) + 1] == 1) {
06709             goto L270;
06710         }
06711         if (zu <= xmax) {
06712             goto L270;
06713         }
06714         xmax = zu;
06715         in = j;
06716 L270:
06717         if (iu[(ii << 1) + 2] == 1) {
06718             goto L280;
06719         }
06720         if (zv <= xmax) {
06721             goto L280;
06722         }
06723         xmax = zv;
06724         in = j;
06725 L280:
06726         ;
06727     }
06728     if (xmax <= toler) {
06729         goto L490;
06730     }
06731     if (q[klm1 + in * q_dim1] == xmax) {
06732         goto L300;
06733     }
06734     i__1 = klm2;
06735     for (i__ = 1; i__ <= i__1; ++i__) {
06736         q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06737 /* L290: */
06738     }
06739     q[klm1 + in * q_dim1] = xmax;
06740 /* DETERMINE THE VECTOR TO LEAVE THE BASIS. */
06741 L300:
06742     if (iphase == 1 || ia == 0) {
06743         goto L330;
06744     }
06745     xmax = 0.f;
06746     i__1 = ia;
06747     for (i__ = 1; i__ <= i__1; ++i__) {
06748         z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06749         if (z__ <= xmax) {
06750             goto L310;
06751         }
06752         xmax = z__;
06753         iout = i__;
06754 L310:
06755         ;
06756     }
06757     if (xmax <= toler) {
06758         goto L330;
06759     }
06760     i__1 = n2;
06761     for (j = 1; j <= i__1; ++j) {
06762         z__ = q[ia + j * q_dim1];
06763         q[ia + j * q_dim1] = q[iout + j * q_dim1];
06764         q[iout + j * q_dim1] = z__;
06765 /* L320: */
06766     }
06767     iout = ia;
06768     --ia;
06769     pivot = q[iout + in * q_dim1];
06770     goto L420;
06771 L330:
06772     kk = 0;
06773     i__1 = klm;
06774     for (i__ = 1; i__ <= i__1; ++i__) {
06775         z__ = q[i__ + in * q_dim1];
06776         if (z__ <= toler) {
06777             goto L340;
06778         }
06779         ++kk;
06780         res[kk] = q[i__ + n1 * q_dim1] / z__;
06781         s[kk] = (double) i__;
06782 L340:
06783         ;
06784     }
06785 L350:
06786     if (kk > 0) {
06787         goto L360;
06788     }
06789     kode = 2;
06790     goto L590;
06791 L360:
06792     xmin = static_cast<float>( res[1] );
06793     iout = (long int) s[1];
06794     j = 1;
06795     if (kk == 1) {
06796         goto L380;
06797     }
06798     i__1 = kk;
06799     for (i__ = 2; i__ <= i__1; ++i__) {
06800         if (res[i__] >= xmin) {
06801             goto L370;
06802         }
06803         j = i__;
06804         xmin = static_cast<float>( res[i__] );
06805         iout = (long int) s[i__];
06806 L370:
06807         ;
06808     }
06809     res[j] = res[kk];
06810     s[j] = s[kk];
06811 L380:
06812     --kk;
06813     pivot = q[iout + in * q_dim1];
06814     ii = (long int) q[iout + n2 * q_dim1];
06815     if (iphase == 1) {
06816         goto L400;
06817     }
06818     if (ii < 0) {
06819         goto L390;
06820     }
06821     if (iu[(ii << 1) + 2] == 1) {
06822         goto L420;
06823     }
06824     goto L400;
06825 L390:
06826     iineg = -ii;
06827     if (iu[(iineg << 1) + 1] == 1) {
06828         goto L420;
06829     }
06830 /* 400 II = IABS(II) */
06831 L400:
06832     ii = abs(ii);
06833     cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06834     if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06835         goto L420;
06836     }
06837 /* BYPASS INTERMEDIATE VERTICES. */
06838     i__1 = n1;
06839     for (j = js; j <= i__1; ++j) {
06840         z__ = q[iout + j * q_dim1];
06841         q[klm1 + j * q_dim1] -= z__ * cuv;
06842         q[iout + j * q_dim1] = -z__;
06843 /* L410: */
06844     }
06845     q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06846     goto L350;
06847 /* GAUSS-JORDAN ELIMINATION. */
06848 L420:
06849     if (iter < maxit) {
06850         goto L430;
06851     }
06852     kode = 3;
06853     goto L590;
06854 L430:
06855     ++iter;
06856     i__1 = n1;
06857     for (j = js; j <= i__1; ++j) {
06858         if (j != in) {
06859             q[iout + j * q_dim1] /= pivot;
06860         }
06861 /* L440: */
06862     }
06863 /* IF PERMITTED, USE SUBROUTINE COL OF THE DESCRIPTION */
06864 /* SECTION AND REPLACE THE FOLLOWING SEVEN STATEMENTS DOWN */
06865 /* TO AND INCLUDING STATEMENT NUMBER 460 BY.. */
06866 /*     DO 460 J=JS,N1 */
06867 /*        IF(J .EQ. IN) GO TO 460 */
06868 /*        Z = -Q(IOUT,J) */
06869 /*        CALL COL(Q(1,J), Q(1,IN), Z, IOUT, KLM1) */
06870 /* 460 CONTINUE */
06871     i__1 = n1;
06872     for (j = js; j <= i__1; ++j) {
06873         if (j == in) {
06874             goto L460;
06875         }
06876         z__ = -q[iout + j * q_dim1];
06877         i__2 = klm1;
06878         for (i__ = 1; i__ <= i__2; ++i__) {
06879             if (i__ != iout) {
06880                 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06881             }
06882 /* L450: */
06883         }
06884 L460:
06885         ;
06886     }
06887     tpivot = -pivot;
06888     i__1 = klm1;
06889     for (i__ = 1; i__ <= i__1; ++i__) {
06890         if (i__ != iout) {
06891             q[i__ + in * q_dim1] /= tpivot;
06892         }
06893 /* L470: */
06894     }
06895     q[iout + in * q_dim1] = 1.f / pivot;
06896     z__ = q[iout + n2 * q_dim1];
06897     q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
06898     q[klm2 + in * q_dim1] = z__;
06899     ii = (long int) abs(z__);
06900     if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
06901         goto L240;
06902     }
06903     i__1 = klm2;
06904     for (i__ = 1; i__ <= i__1; ++i__) {
06905         z__ = q[i__ + in * q_dim1];
06906         q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
06907         q[i__ + js * q_dim1] = z__;
06908 /* L480: */
06909     }
06910     ++js;
06911     goto L240;
06912 /* TEST FOR OPTIMALITY. */
06913 L490:
06914     if (kforce == 0) {
06915         goto L580;
06916     }
06917     if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
06918         goto L500;
06919     }
06920     kforce = 0;
06921     goto L240;
06922 /* SET UP PHASE 2 COSTS. */
06923 L500:
06924     iphase = 2;
06925     i__1 = nklm;
06926     for (j = 1; j <= i__1; ++j) {
06927         cu[(j << 1) + 1] = 0.f;
06928         cu[(j << 1) + 2] = 0.f;
06929 /* L510: */
06930     }
06931     i__1 = nk;
06932     for (j = n1; j <= i__1; ++j) {
06933         cu[(j << 1) + 1] = 1.f;
06934         cu[(j << 1) + 2] = 1.f;
06935 /* L520: */
06936     }
06937     i__1 = klm;
06938     for (i__ = 1; i__ <= i__1; ++i__) {
06939         ii = (long int) q[i__ + n2 * q_dim1];
06940         if (ii > 0) {
06941             goto L530;
06942         }
06943         ii = -ii;
06944         if (iu[(ii << 1) + 2] == 0) {
06945             goto L560;
06946         }
06947         cu[(ii << 1) + 2] = 0.f;
06948         goto L540;
06949 L530:
06950         if (iu[(ii << 1) + 1] == 0) {
06951             goto L560;
06952         }
06953         cu[(ii << 1) + 1] = 0.f;
06954 L540:
06955         ++ia;
06956         i__2 = n2;
06957         for (j = 1; j <= i__2; ++j) {
06958             z__ = q[ia + j * q_dim1];
06959             q[ia + j * q_dim1] = q[i__ + j * q_dim1];
06960             q[i__ + j * q_dim1] = z__;
06961 /* L550: */
06962         }
06963 L560:
06964         ;
06965     }
06966     goto L160;
06967 L570:
06968     if (q[klm1 + n1 * q_dim1] <= toler) {
06969         goto L500;
06970     }
06971     kode = 1;
06972     goto L590;
06973 L580:
06974     if (iphase == 1) {
06975         goto L570;
06976     }
06977 /* PREPARE OUTPUT. */
06978     kode = 0;
06979 L590:
06980     xsum = 0.;
06981     i__1 = *n;
06982     for (j = 1; j <= i__1; ++j) {
06983         x[j] = 0.f;
06984 /* L600: */
06985     }
06986     i__1 = klm;
06987     for (i__ = 1; i__ <= i__1; ++i__) {
06988         res[i__] = 0.f;
06989 /* L610: */
06990     }
06991     i__1 = klm;
06992     for (i__ = 1; i__ <= i__1; ++i__) {
06993         ii = (long int) q[i__ + n2 * q_dim1];
06994         sn = 1.f;
06995         if (ii > 0) {
06996             goto L620;
06997         }
06998         ii = -ii;
06999         sn = -1.f;
07000 L620:
07001         if (ii > *n) {
07002             goto L630;
07003         }
07004         x[ii] = sn * q[i__ + n1 * q_dim1];
07005         goto L640;
07006 L630:
07007         iimn = ii - *n;
07008         res[iimn] = sn * q[i__ + n1 * q_dim1];
07009         if (ii >= n1 && ii <= nk) {
07010             xsum += q[i__ + n1 * q_dim1];
07011         }
07012 L640:
07013         ;
07014     }
07015     error = (float)xsum;
07016     return;
07017 }
07018 
07019 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07020 {
07021         int j,d;
07022         EMData * e = new EMData();
07023         float *eptr, *imgptr;
07024         imgptr = img->get_data();
07025         float SSE = 0.f;
07026         for (j = 0 ; j < N ; j++) {
07027                 e->read_image(images,S[j]);
07028                 eptr = e->get_data();
07029                 for (d = 0; d < size; d++) {
07030                         SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07031                 }
07032         delete e;
07033         return SSE;
07034 }
07035 
07036 
07037 #define         mymax(x,y)              (((x)>(y))?(x):(y))
07038 #define         mymin(x,y)              (((x)<(y))?(x):(y))
07039 #define         sign(x,y)               (((((y)>0)?(1):(-1))*(y!=0))*(x))
07040 
07041 
07042 #define         quadpi                  3.141592653589793238462643383279502884197
07043 #define         dgr_to_rad              quadpi/180
07044 #define         deg_to_rad              quadpi/180
07045 #define         rad_to_deg              180/quadpi
07046 #define         rad_to_dgr              180/quadpi
07047 #define         TRUE                    1
07048 #define         FALSE                   0
07049 
07050 
07051 #define theta(i)                theta   [i-1]
07052 #define phi(i)                  phi     [i-1]
07053 #define weight(i)               weight  [i-1]
07054 #define lband(i)                lband   [i-1]
07055 #define ts(i)                   ts      [i-1]
07056 #define thetast(i)              thetast [i-1]
07057 #define key(i)                  key     [i-1]
07058 
07059 
07060 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07061 {
07062 
07063         ENTERFUNC;
07064 
07065         if ( th.size() != ph.size() ) {
07066                 LOGERR("images not same size");
07067                 throw ImageFormatException( "images not same size");
07068         }
07069 
07070         // rand_seed
07071         srand(10);
07072 
07073         int i,*key;
07074         int len = th.size();
07075         double *theta,*phi,*weight;
07076         theta   =       (double*) calloc(len,sizeof(double));
07077         phi     =       (double*) calloc(len,sizeof(double));
07078         weight  =       (double*) calloc(len,sizeof(double));
07079         key     =       (int*) calloc(len,sizeof(int));
07080         const float *thptr, *phptr;
07081 
07082         thptr = &th[0];
07083         phptr = &ph[0];
07084         for(i=1;i<=len;i++){
07085                 key(i) = i;
07086                 weight(i) = 0.0;
07087         }
07088 
07089         for(i = 0;i<len;i++){
07090                 theta[i] = thptr[i];
07091                 phi[i]   = phptr[i];
07092         }
07093 
07094         //  sort by theta
07095         Util::hsortd(theta, phi, key, len, 1);
07096 
07097         //Util::voronoidiag(theta,phi, weight, len);
07098         Util::voronoi(phi, theta, weight, len);
07099 
07100         //sort by key
07101         Util::hsortd(weight, weight, key, len, 2);
07102 
07103         free(theta);
07104         free(phi);
07105         free(key);
07106         vector<double> wt;
07107         double count = 0;
07108         for(i=1; i<= len; i++)
07109         {
07110                 wt.push_back(weight(i));
07111                 count += weight(i);
07112         }
07113 
07114         //if( abs(count-6.28) > 0.1 )
07115         //{
07116         //    printf("Warning: SUM OF VORONOI CELLS AREAS IS %lf, should 2*PI\n", count);
07117         //}
07118 
07119         free(weight);
07120 
07121         EXITFUNC;
07122         return wt;
07123 
07124 }
07125 
07126 struct  tmpstruct{
07127         double theta1,phi1;
07128         int key1;
07129         };
07130 
07131 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07132 {
07133         ENTERFUNC;
07134         vector<tmpstruct> tmp(len);
07135         int i;
07136         for(i = 1;i<=len;i++)
07137         {
07138                 tmp[i-1].theta1 = theta(i);
07139                 tmp[i-1].phi1 = phi(i);
07140                 tmp[i-1].key1 = key(i);
07141         }
07142 
07143         if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07144         if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07145 
07146         for(i = 1;i<=len;i++)
07147         {
07148                 theta(i) = tmp[i-1].theta1;
07149                 phi(i)   = tmp[i-1].phi1;
07150                 key(i)   = tmp[i-1].key1;
07151         }
07152         EXITFUNC;
07153 }
07154 
07155 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07156 {
07157         return(tmp1.theta1 < tmp2.theta1);
07158 }
07159 
07160 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07161 {
07162         return(tmp1.key1 < tmp2.key1);
07163 }
07164 
07165 /******************  VORONOI DIAGRAM **********************************/
07166 /*
07167 void Util::voronoidiag(double *theta,double *phi,double* weight,int n)
07168 {
07169         ENTERFUNC;
07170 
07171         int     *lband;
07172         double  aat=0.0f,*ts;
07173         double  aa,acum,area;
07174         int     last;
07175         int numth       =       1;
07176         int nbt         =       1;//mymax((int)(sqrt((n/500.0))) , 3);
07177 
07178         int i,it,l,k;
07179         int nband,lb,low,medium,lhigh,lbw,lenw;
07180 
07181 
07182         lband   =       (int*)calloc(nbt,sizeof(int));
07183         ts      =       (double*)calloc(nbt,sizeof(double));
07184 
07185         if(lband == NULL || ts == NULL ){
07186                 fprintf(stderr,"memory allocation failure!\n");
07187                 exit(1);
07188         }
07189 
07190         nband=nbt;
07191 
07192         while(nband>0){
07193                 Util::angstep(ts,nband);
07194 
07195                 l=1;
07196                 for(i=1;i<=n;i++){
07197                         if(theta(i)>ts(l)){
07198                                 lband(l)=i;
07199                                 l=l+1;
07200                                 if(l>nband)  exit(1);
07201                         }
07202                 }
07203 
07204                 l=1;
07205                 for(i=1;i<=n;i++){
07206                         if(theta(i)>ts(l)){
07207                                 lband(l)=i;
07208                                 l=l+1;
07209                                 if(l>nband)  exit(1);
07210                         }
07211                 }
07212 
07213                 lband(l)=n+1;
07214                 acum=0.0;
07215                 for(it=l;it>=1;it-=numth){
07216                         for(i=it;i>=mymax(1,it-numth+1);i--){
07217                         if(i==l) last   =        TRUE;
07218                         else     last   =        FALSE;
07219 
07220                         if(l==1){
07221                                 lb=1;
07222                                 low=1;
07223                                 medium=n+1;
07224                                 lhigh=n-lb+1;
07225                                 lbw=1;
07226                         }
07227                         else if(i==1){
07228                                 lb=1;
07229                                 low=1;
07230                                 medium=lband(1);
07231                                 lhigh=lband(2)-1;
07232                                 lbw=1;
07233                         }
07234                         else if(i==l){
07235                                 if(l==2)        lb=1;
07236                                 else            lb=lband(l-2);
07237                                 low=lband(l-1)-lb+1;
07238                                 medium=lband(l)-lb+1;
07239                                 lhigh=n-lb+1;
07240                                 lbw=lband(i-1);
07241                         }
07242                         else{
07243                                 if(i==2)        lb=1;
07244                                 else            lb=lband(i-2);
07245                                 low=lband(i-1)-lb+1;
07246                                 medium=lband(i)-lb+1;
07247                                 lhigh=lband(i+1)-1-lb+1;
07248                                 lbw=lband(i-1);
07249                         }
07250                         lenw=medium-low;
07251 
07252 
07253                         Util::voronoi(&phi(lb),&theta(lb),&weight(lbw),lenw,low,medium,lhigh,last);
07254 
07255 
07256                         if(nband>1){
07257                                 if(i==1)        area=quadpi*2.0*(1.0-cos(ts(1)*dgr_to_rad));
07258                                 else            area=quadpi*2.0*(cos(ts(i-1)*dgr_to_rad)-cos(ts(i)*dgr_to_rad));
07259 
07260                                 aa = 0.0;
07261                                 for(k = lbw;k<=lbw+lenw-1;k++)
07262                                         aa = aa+weight(k);
07263 
07264                                 acum=acum+aa;
07265                                 aat=aa/area;
07266                                 }
07267 
07268                         }
07269                         for(i=it;mymax(1,it-numth+1);i--){
07270                         if(fabs(aat-1.0)>0.02){
07271                                 nband=mymax(0,mymin( (int)(((float)nband) * 0.75) ,nband-1) );
07272                                 goto  label2;
07273                                 }
07274                         }
07275                 acum=acum/quadpi/2.0;
07276                 exit(1);
07277 label2:
07278 
07279                 continue;
07280                 }
07281 
07282         free(ts);
07283         free(lband);
07284 
07285         }
07286 
07287         EXITFUNC;
07288 }
07289 
07290 
07291 void Util::angstep(double* thetast,int len){
07292 
07293         ENTERFUNC;
07294 
07295         double t1,t2,tmp;
07296         int i;
07297         if(len>1){
07298                 t1=0;
07299                 for(i=1;i<=len-1;i++){
07300                         tmp=cos(t1)-1.0/((float)len);
07301                         t2=acos(sign(mymin(1.0,fabs(tmp)),tmp));
07302                         thetast(i)=t2 * rad_to_deg;
07303                         t1=t2;
07304                 }
07305         }
07306         thetast(len)=90.0;
07307 
07308         EXITFUNC;
07309 }
07310 */
07311 /*
07312 void Util::voronoi(double *phi, double *theta, double *weight, int lenw, int low, int medium, int nt, int last)
07313 {
07314 
07315         ENTERFUNC;
07316         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07317         int nt6, n, ier,nout,lnew,mdup,nd;
07318         int i,k,mt,status;
07319 
07320 
07321         double *ds, *x, *y, *z;
07322         double tol=1.0e-8;
07323         double a;
07324 
07325         if(last){
07326                 if(medium>nt)  n = nt+nt;
07327                 else           n = nt+nt-medium+1;
07328         }
07329         else{
07330                 n=nt;
07331         }
07332 
07333         nt6 = n*6;
07334 
07335         list = (int*)calloc(nt6,sizeof(int));
07336         lptr = (int*)calloc(nt6,sizeof(int));
07337         lend = (int*)calloc(n  ,sizeof(int));
07338         iwk  = (int*)calloc(n  ,sizeof(int));
07339         good = (int*)calloc(n  ,sizeof(int));
07340         key  = (int*)calloc(n  ,sizeof(int));
07341         indx = (int*)calloc(n  ,sizeof(int));
07342         lcnt = (int*)calloc(n  ,sizeof(int));
07343 
07344         ds      =       (double*) calloc(n,sizeof(double));
07345         x       =       (double*) calloc(n,sizeof(double));
07346         y       =       (double*) calloc(n,sizeof(double));
07347         z       =       (double*) calloc(n,sizeof(double));
07348 
07349         if (list == NULL ||
07350         lptr == NULL ||
07351         lend == NULL ||
07352         iwk  == NULL ||
07353         good == NULL ||
07354         key  == NULL ||
07355         indx == NULL ||
07356         lcnt == NULL ||
07357         x    == NULL ||
07358         y    == NULL ||
07359         z    == NULL ||
07360         ds   == NULL) {
07361                 printf("memory allocation failure!\n");
07362                 exit(1);
07363         }
07364 
07365 
07366 
07367         for(i = 1;i<=nt;i++){
07368                 x[i-1] = theta(i);
07369                 y[i-1] = phi(i);
07370         }
07371 
07372 
07373 
07374         if (last) {
07375                 for(i=nt+1;i<=n;i++){
07376                         x[i-1]=180.0-x[2*nt-i];
07377                         y[i-1]=180.0+y[2*nt-i];
07378                 }
07379         }
07380 
07381 
07382         Util::disorder2(x,y,key,n);
07383 
07384         Util::ang_to_xyz(x,y,z,n);
07385 
07386 
07387         //  Make sure that first three are no colinear
07388         label1:
07389         for(k=0; k<2; k++){
07390                 for(i=k+1; i<3; i++){
07391                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol){
07392                                 Util::flip23(x, y, z, key, k, n);
07393                                 goto label1;
07394                         }
07395                 }
07396         }
07397 
07398 
07399         status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew,indx,lcnt, iwk, good, ds, &ier);
07400 
07401 
07402         if (status != 0) {
07403                 printf(" error in trmsh3 \n");
07404                 exit(1);
07405         }
07406 
07407 
07408         mdup=n-nout;
07409         if (ier == -2) {
07410                 printf("*** Error in TRMESH:the first three nodes are collinear***\n");
07411                 exit(1);
07412         }
07413         else if (ier > 0) {
07414                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07415                 exit(1);
07416         }
07417 
07418         nd=0;
07419         for (k=1;k<=n;k++){
07420                 if (indx[k-1]>0){
07421                         nd++;
07422                         good[nd-1]=k;
07423                 }
07424         }
07425 
07426 
07427         for(i = 1;i<=nout;i++) {
07428                 k=good[i-1];
07429                 if (key[k-1] >= low && key[k-1]<medium){
07430                         a = Util::areav_(&i,&nout,x,y,z,list,lptr,lend,&ier);
07431                         if (ier != 0){
07432                                 weight[key[k-1]-low] =-1.0;
07433                         }
07434                         else {
07435                                 weight[key[k-1]-low]=a/lcnt[i-1];
07436                         }
07437                 }
07438         }
07439 
07440 // Fill out the duplicated weights
07441         for(i = 1;i<=n;i++){
07442                 mt=-indx[i-1];
07443                 if (mt>0){
07444                         k=good[mt-1];
07445 //  This is a duplicated entry, get the already calculated
07446 //   weight and assign it.
07447                         if (key[i-1]>=low && key[i-1]<medium){
07448 //  Is it already calculated weight??
07449                                 if(key[k-1]>=low && key[k-1]<medium){
07450                                         weight[key[i-1]-low]=weight[key[k-1]-low];
07451                                 }
07452                                 else{
07453 //  No, the weight is from the outside of valid region, calculate it anyway
07454                                         a = Util::areav_(&mt, &nout, x, y, z, list, lptr, lend, &ier);
07455                                         if (ier != 0){
07456                                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07457                                                 weight[key[i-1]-low] =-1.0;
07458                                         }
07459                                         else {
07460                                                 weight[key[i-1]-low] = a/lcnt[mt-1];
07461                                         }
07462                                 }
07463                         }
07464                 }
07465         }
07466 
07467 
07468         free(list);
07469         free(lend);
07470         free(iwk);
07471         free(good);
07472         free(key);
07473 
07474         free(indx);
07475         free(lcnt);
07476         free(ds);
07477         free(x);
07478         free(y);
07479         free(z);
07480         EXITFUNC;
07481 }
07482 */
07483 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07484 {
07485 
07486         ENTERFUNC;
07487 
07488         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07489         int nt6, n, ier, nout, lnew, mdup, nd;
07490         int i,k,mt,status;
07491 
07492 
07493         double *ds, *x, *y, *z;
07494         double tol  = 1.0e-8;
07495         double dtol = 15;
07496         double a;
07497 
07498         /*if(last){
07499                 if(medium>nt)  n = nt+nt;
07500                 else           n = nt+nt-medium+1;
07501         }
07502         else{
07503                 n=nt;
07504         }*/
07505 
07506         n = nt + nt;
07507 
07508         nt6 = n*6;
07509 
07510         list = (int*)calloc(nt6,sizeof(int));
07511         lptr = (int*)calloc(nt6,sizeof(int));
07512         lend = (int*)calloc(n  ,sizeof(int));
07513         iwk  = (int*)calloc(n  ,sizeof(int));
07514         good = (int*)calloc(n  ,sizeof(int));
07515         key  = (int*)calloc(n  ,sizeof(int));
07516         indx = (int*)calloc(n  ,sizeof(int));
07517         lcnt = (int*)calloc(n  ,sizeof(int));
07518 
07519         ds      =       (double*) calloc(n,sizeof(double));
07520         x       =       (double*) calloc(n,sizeof(double));
07521         y       =       (double*) calloc(n,sizeof(double));
07522         z       =       (double*) calloc(n,sizeof(double));
07523 
07524         if (list == NULL ||
07525         lptr == NULL ||
07526         lend == NULL ||
07527         iwk  == NULL ||
07528         good == NULL ||
07529         key  == NULL ||
07530         indx == NULL ||
07531         lcnt == NULL ||
07532         x    == NULL ||
07533         y    == NULL ||
07534         z    == NULL ||
07535         ds   == NULL) {
07536                 printf("memory allocation failure!\n");
07537                 exit(1);
07538         }
07539 
07540         bool colinear=true;
07541         while(colinear)
07542         {
07543 
07544         L1:
07545             for(i = 0; i<nt; i++){
07546                 x[i] = theta[i];
07547                 y[i] = phi[i];
07548                 x[nt+i] = 180.0 - x[i];
07549                 y[nt+i] = 180.0 + y[i];
07550             }
07551 
07552             Util::disorder2(x, y, key, n);
07553 
07554             // check if the first three angles are not close, else shuffle
07555             double val;
07556             for(k=0; k<2; k++){
07557                 for(i=k+1; i<3; i++){
07558                     val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07559                     if( val  < dtol) {
07560                         goto L1;
07561                     }
07562                 }
07563             }
07564 
07565             Util::ang_to_xyz(x, y, z, n);
07566 
07567             //  Make sure that first three has no duplication
07568             bool dupnode=true;
07569             dupnode=true;
07570             while(dupnode)
07571             {
07572                 for(k=0; k<2; k++){
07573                     for(i=k+1; i<3; i++){
07574                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07575                                 Util::flip23(x, y, z, key, k, n);
07576                                 continue;
07577                         }
07578                     }
07579                 }
07580                 dupnode = false;
07581             }
07582 
07583 
07584             ier = 0;
07585 
07586             status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07587 
07588             if (status != 0) {
07589                 printf(" error in trmsh3 \n");
07590                 exit(1);
07591             }
07592 
07593             if (ier > 0) {
07594                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07595                 exit(1);
07596             }
07597 
07598             mdup=n-nout;
07599             if (ier == -2) {
07600                 //printf("in TRMESH:the first three nodes are colinear*** disorder again\n");
07601             }
07602             else
07603             {
07604                 colinear=false;
07605             }
07606         }
07607 
07608 
07609         Assert( ier != -2 );
07610 //  Create a list of unique nodes GOOD, the numbers refer to locations on the full list
07611 //  INDX contains node numbers from the squeezed list
07612         nd=0;
07613         for (k=1; k<=n; k++){
07614                 if (indx[k-1]>0) {
07615                         nd++;
07616                         good[nd-1]=k;
07617                 }
07618         }
07619 
07620 //
07621 // *** Compute the Voronoi region areas.
07622 //
07623         for(i = 1; i<=nout; i++) {
07624                 k=good[i-1];
07625                 //  We only need n weights from hemisphere
07626                 if (key[k-1] <= nt) {
07627 //  CALCULATE THE AREA
07628                         a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07629                         if (ier != 0){
07630 //  We set the weight to -1, this will signal the error in the calling
07631 //   program, as the area will turn out incorrect
07632                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07633                                 weight[key[k-1]-1] =-1.0;
07634                         } else {
07635 //  Assign the weight
07636                                 weight[key[k-1]-1]=a/lcnt[i-1];
07637                         }
07638                 }
07639         }
07640 
07641 
07642 // Fill out the duplicated weights
07643         for(i = 1; i<=n; i++){
07644                 mt =- indx[i-1];
07645                 if (mt>0){
07646                         k = good[mt-1];
07647 //  This is a duplicated entry, get the already calculated
07648 //   weight and assign it.
07649                 //  We only need n weights from hemisphere
07650                         if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07651                         }
07652         }
07653 
07654         free(list);
07655         free(lend);
07656         free(iwk);
07657         free(good);
07658         free(key);
07659         free(lptr);
07660         free(indx);
07661         free(lcnt);
07662         free(ds);
07663         free(x);
07664         free(y);
07665         free(z);
07666 
07667 
07668         EXITFUNC;
07669 }
07670 
07671 void Util::disorder2(double *x,double *y, int *key, int len)
07672 {
07673         ENTERFUNC;
07674         int k, i;
07675         for(i=0; i<len; i++) key[i]=i+1;
07676 
07677         for(i = 0; i<len;i++){
07678                 k = rand()%len;
07679                 std::swap(key[k], key[i]);
07680                 std::swap(x[k], x[i]);
07681                 std::swap(y[k], y[i]);
07682         }
07683         EXITFUNC;
07684 }
07685 
07686 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07687 {
07688         ENTERFUNC;
07689         double costheta,sintheta,cosphi,sinphi;
07690         for(int i = 0;  i<len;  i++)
07691         {
07692                 cosphi = cos(y[i]*dgr_to_rad);
07693                 sinphi = sin(y[i]*dgr_to_rad);
07694                 if(fabs(x[i]-90.0)< 1.0e-5){
07695                         x[i] = cosphi;
07696                         y[i] = sinphi;
07697                         z[i] = 0.0;
07698                 }
07699                 else{
07700                         costheta = cos(x[i]*dgr_to_rad);
07701                         sintheta = sin(x[i]*dgr_to_rad);
07702                         x[i] = cosphi*sintheta;
07703                         y[i] = sinphi*sintheta;
07704                         z[i] = costheta;
07705                 }
07706         }
07707         EXITFUNC;
07708 }
07709 
07710 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07711 {
07712         ENTERFUNC;
07713         int i = k;
07714         while( i == k )  i = rand()%len;
07715         std::swap(key[i], key[k]);
07716         std::swap(x[i], x[k]);
07717         std::swap(y[i], y[k]);
07718         std::swap(z[i], z[k]);
07719         EXITFUNC;
07720 }
07721 
07722 
07723 #undef  mymax
07724 #undef  mymin
07725 #undef  sign
07726 #undef  quadpi
07727 #undef  dgr_to_rad
07728 #undef  deg_to_rad
07729 #undef  rad_to_deg
07730 #undef  rad_to_dgr
07731 #undef  TRUE
07732 #undef  FALSE
07733 #undef  theta
07734 #undef  phi
07735 #undef  weight
07736 #undef  lband
07737 #undef  ts
07738 #undef  thetast
07739 #undef  key
07740 
07741 
07742 /*################################################################################################
07743 ##########  strid.f -- translated by f2c (version 20030320). ###################################
07744 ######   You must link the resulting object file with the libraries: #############################
07745 ####################    -lf2c -lm   (in that order)   ############################################
07746 ################################################################################################*/
07747 
07748 /* Common Block Declarations */
07749 
07750 
07751 #define TRUE_ (1)
07752 #define FALSE_ (0)
07753 #define abs(x) ((x) >= 0 ? (x) : -(x))
07754 
07755 struct stcom_{
07756     double y;
07757 };
07758 stcom_ stcom_1;
07759 #ifdef KR_headers
07760 double floor();
07761 int i_dnnt(x) double *x;
07762 #else
07763 int i_dnnt(double *x)
07764 #endif
07765 {
07766         return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07767 }
07768 
07769 
07770 
07771 
07772 /* ____________________STRID______________________________________ */
07773 /* Subroutine */ int Util::trmsh3_(int *n0, double *tol, double *x,
07774         double *y, double *z__, int *n, int *list, int *
07775         lptr, int *lend, int *lnew, int *indx, int *lcnt,
07776         int *near__, int *next, double *dist, int *ier)
07777 {
07778     /* System generated locals */
07779     int i__1, i__2;
07780 
07781     /* Local variables */
07782     static double d__;
07783     static int i__, j;
07784     static double d1, d2, d3;
07785     static int i0, lp, kt, ku, lpl, nku;
07786     extern long int left_(double *, double *, double *, double
07787             *, double *, double *, double *, double *,
07788             double *);
07789     static int nexti;
07790     extern /* Subroutine */ int addnod_(int *, int *, double *,
07791             double *, double *, int *, int *, int *,
07792             int *, int *);
07793 
07794 
07795 /* *********************************************************** */
07796 
07797 /*                                              From STRIPACK */
07798 /*                                            Robert J. Renka */
07799 /*                                  Dept. of Computer Science */
07800 /*                                       Univ. of North Texas */
07801 /*                                           renka@cs.unt.edu */
07802 /*                                                   01/20/03 */
07803 
07804 /*   This is an alternative to TRMESH with the inclusion of */
07805 /* an efficient means of removing duplicate or nearly dupli- */
07806 /* cate nodes. */
07807 
07808 /*   This subroutine creates a Delaunay triangulation of a */
07809 /* set of N arbitrarily distributed points, referred to as */
07810 /* nodes, on the surface of the unit sphere.  Refer to Sub- */
07811 /* routine TRMESH for definitions and a list of additional */
07812 /* subroutines.  This routine is an alternative to TRMESH */
07813 /* with the inclusion of an efficient means of removing dup- */
07814 /* licate or nearly duplicate nodes. */
07815 
07816 /*   The algorithm has expected time complexity O(N*log(N)) */
07817 /* for random nodal distributions. */
07818 
07819 
07820 /* On input: */
07821 
07822 /*       N0 = Number of nodes, possibly including duplicates. */
07823 /*            N0 .GE. 3. */
07824 
07825 /*       TOL = Tolerance defining a pair of duplicate nodes: */
07826 /*             bound on the deviation from 1 of the cosine of */
07827 /*             the angle between the nodes.  Note that */
07828 /*             |1-cos(A)| is approximately A*A/2. */
07829 
07830 /* The above parameters are not altered by this routine. */
07831 
07832 /*       X,Y,Z = Arrays of length at least N0 containing the */
07833 /*               Cartesian coordinates of nodes.  (X(K),Y(K), */
07834 /*               Z(K)) is referred to as node K, and K is re- */
07835 /*               ferred to as a nodal index.  It is required */
07836 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
07837 /*               K.  The first three nodes must not be col- */
07838 /*               linear (lie on a common great circle). */
07839 
07840 /*       LIST,LPTR = Arrays of length at least 6*N0-12. */
07841 
07842 /*       LEND = Array of length at least N0. */
07843 
07844 /*       INDX = Array of length at least N0. */
07845 
07846 /*       LCNT = Array of length at least N0 (length N is */
07847 /*              sufficient). */
07848 
07849 /*       NEAR,NEXT,DIST = Work space arrays of length at */
07850 /*                        least N0.  The space is used to */
07851 /*                        efficiently determine the nearest */
07852 /*                        triangulation node to each un- */
07853 /*                        processed node for use by ADDNOD. */
07854 
07855 /* On output: */
07856 
07857 /*       N = Number of nodes in the triangulation.  3 .LE. N */
07858 /*           .LE. N0, or N = 0 if IER < 0. */
07859 
07860 /*       X,Y,Z = Arrays containing the Cartesian coordinates */
07861 /*               of the triangulation nodes in the first N */
07862 /*               locations.  The original array elements are */
07863 /*               shifted down as necessary to eliminate dup- */
07864 /*               licate nodes. */
07865 
07866 /*       LIST = Set of nodal indexes which, along with LPTR, */
07867 /*              LEND, and LNEW, define the triangulation as a */
07868 /*              set of N adjacency lists -- counterclockwise- */
07869 /*              ordered sequences of neighboring nodes such */
07870 /*              that the first and last neighbors of a bound- */
07871 /*              ary node are boundary nodes (the first neigh- */
07872 /*              bor of an interior node is arbitrary).  In */
07873 /*              order to distinguish between interior and */
07874 /*              boundary nodes, the last neighbor of each */
07875 /*              boundary node is represented by the negative */
07876 /*              of its index. */
07877 
07878 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
07879 /*              correspondence with the elements of LIST. */
07880 /*              LIST(LPTR(I)) indexes the node which follows */
07881 /*              LIST(I) in cyclical counterclockwise order */
07882 /*              (the first neighbor follows the last neigh- */
07883 /*              bor). */
07884 
07885 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
07886 /*              points to the last neighbor of node K for */
07887 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
07888 /*              only if K is a boundary node. */
07889 
07890 /*       LNEW = Pointer to the first empty location in LIST */
07891 /*              and LPTR (list length plus one).  LIST, LPTR, */
07892 /*              LEND, and LNEW are not altered if IER < 0, */
07893 /*              and are incomplete if IER > 0. */
07894 
07895 /*       INDX = Array of output (triangulation) nodal indexes */
07896 /*              associated with input nodes.  For I = 1 to */
07897 /*              N0, INDX(I) is the index (for X, Y, and Z) of */
07898 /*              the triangulation node with the same (or */
07899 /*              nearly the same) coordinates as input node I. */
07900 
07901 /*       LCNT = Array of int weights (counts) associated */
07902 /*              with the triangulation nodes.  For I = 1 to */
07903 /*              N, LCNT(I) is the number of occurrences of */
07904 /*              node I in the input node set, and thus the */
07905 /*              number of duplicates is LCNT(I)-1. */
07906 
07907 /*       NEAR,NEXT,DIST = Garbage. */
07908 
07909 /*       IER = Error indicator: */
07910 /*             IER =  0 if no errors were encountered. */
07911 /*             IER = -1 if N0 < 3 on input. */
07912 /*             IER = -2 if the first three nodes are */
07913 /*                      collinear. */
07914 /*             IER = -3 if Subroutine ADDNOD returns an error */
07915 /*                      flag.  This should not occur. */
07916 
07917 /* Modules required by TRMSH3:  ADDNOD, BDYADD, COVSPH, */
07918 /*                                INSERT, INTADD, JRAND, */
07919 /*                                LEFT, LSTPTR, STORE, SWAP, */
07920 /*                                SWPTST, TRFIND */
07921 
07922 /* Intrinsic function called by TRMSH3:  ABS */
07923 
07924 /* *********************************************************** */
07925 
07926 
07927 /* Local parameters: */
07928 
07929 /* D =        (Negative cosine of) distance from node KT to */
07930 /*              node I */
07931 /* D1,D2,D3 = Distances from node KU to nodes 1, 2, and 3, */
07932 /*              respectively */
07933 /* I,J =      Nodal indexes */
07934 /* I0 =       Index of the node preceding I in a sequence of */
07935 /*              unprocessed nodes:  I = NEXT(I0) */
07936 /* KT =       Index of a triangulation node */
07937 /* KU =       Index of an unprocessed node and DO-loop index */
07938 /* LP =       LIST index (pointer) of a neighbor of KT */
07939 /* LPL =      Pointer to the last neighbor of KT */
07940 /* NEXTI =    NEXT(I) */
07941 /* NKU =      NEAR(KU) */
07942 
07943     /* Parameter adjustments */
07944     --dist;
07945     --next;
07946     --near__;
07947     --indx;
07948     --lend;
07949     --z__;
07950     --y;
07951     --x;
07952     --list;
07953     --lptr;
07954     --lcnt;
07955 
07956     /* Function Body */
07957     if (*n0 < 3) {
07958         *n = 0;
07959         *ier = -1;
07960         return 0;
07961     }
07962 
07963 /* Store the first triangle in the linked list. */
07964 
07965     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
07966             z__[3])) {
07967 
07968 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
07969 
07970         list[1] = 3;
07971         lptr[1] = 2;
07972         list[2] = -2;
07973         lptr[2] = 1;
07974         lend[1] = 2;
07975 
07976         list[3] = 1;
07977         lptr[3] = 4;
07978         list[4] = -3;
07979         lptr[4] = 3;
07980         lend[2] = 4;
07981 
07982         list[5] = 2;
07983         lptr[5] = 6;
07984         list[6] = -1;
07985         lptr[6] = 5;
07986         lend[3] = 6;
07987 
07988     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
07989             y[3], &z__[3])) {
07990 
07991 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
07992 /*     i.e., node 3 lies in the left hemisphere defined by */
07993 /*     arc 1->2. */
07994 
07995         list[1] = 2;
07996         lptr[1] = 2;
07997         list[2] = -3;
07998         lptr[2] = 1;
07999         lend[1] = 2;
08000 
08001         list[3] = 3;
08002         lptr[3] = 4;
08003         list[4] = -1;
08004         lptr[4] = 3;
08005         lend[2] = 4;
08006 
08007         list[5] = 1;
08008         lptr[5] = 6;
08009         list[6] = -2;
08010         lptr[6] = 5;
08011         lend[3] = 6;
08012 
08013 
08014     } else {
08015 
08016 /*   The first three nodes are collinear. */
08017 
08018         *n = 0;
08019         *ier = -2;
08020         return 0;
08021     }
08022 
08023     //printf("pass check colinear\n");
08024 
08025 /* Initialize LNEW, INDX, and LCNT, and test for N = 3. */
08026 
08027     *lnew = 7;
08028     indx[1] = 1;
08029     indx[2] = 2;
08030     indx[3] = 3;
08031     lcnt[1] = 1;
08032     lcnt[2] = 1;
08033     lcnt[3] = 1;
08034     if (*n0 == 3) {
08035         *n = 3;
08036         *ier = 0;
08037         return 0;
08038     }
08039 
08040 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
08041 /*   used to obtain an expected-time (N*log(N)) incremental */
08042 /*   algorithm by enabling constant search time for locating */
08043 /*   each new node in the triangulation. */
08044 
08045 /* For each unprocessed node KU, NEAR(KU) is the index of the */
08046 /*   triangulation node closest to KU (used as the starting */
08047 /*   point for the search in Subroutine TRFIND) and DIST(KU) */
08048 /*   is an increasing function of the arc length (angular */
08049 /*   distance) between nodes KU and NEAR(KU):  -Cos(a) for */
08050 /*   arc length a. */
08051 
08052 /* Since it is necessary to efficiently find the subset of */
08053 /*   unprocessed nodes associated with each triangulation */
08054 /*   node J (those that have J as their NEAR entries), the */
08055 /*   subsets are stored in NEAR and NEXT as follows:  for */
08056 /*   each node J in the triangulation, I = NEAR(J) is the */
08057 /*   first unprocessed node in J's set (with I = 0 if the */
08058 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
08059 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
08060 /*   set are initially ordered by increasing indexes (which */
08061 /*   maximizes efficiency) but that ordering is not main- */
08062 /*   tained as the data structure is updated. */
08063 
08064 /* Initialize the data structure for the single triangle. */
08065 
08066     near__[1] = 0;
08067     near__[2] = 0;
08068     near__[3] = 0;
08069     for (ku = *n0; ku >= 4; --ku) {
08070         d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08071         d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08072         d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08073         if (d1 <= d2 && d1 <= d3) {
08074             near__[ku] = 1;
08075             dist[ku] = d1;
08076             next[ku] = near__[1];
08077             near__[1] = ku;
08078         } else if (d2 <= d1 && d2 <= d3) {
08079             near__[ku] = 2;
08080             dist[ku] = d2;
08081             next[ku] = near__[2];
08082             near__[2] = ku;
08083         } else {
08084             near__[ku] = 3;
08085             dist[ku] = d3;
08086             next[ku] = near__[3];
08087             near__[3] = ku;
08088         }
08089 /* L1: */
08090     }
08091 
08092 /* Loop on unprocessed nodes KU.  KT is the number of nodes */
08093 /*   in the triangulation, and NKU = NEAR(KU). */
08094 
08095     kt = 3;
08096     i__1 = *n0;
08097     for (ku = 4; ku <= i__1; ++ku) {
08098         nku = near__[ku];
08099 
08100 /* Remove KU from the set of unprocessed nodes associated */
08101 /*   with NEAR(KU). */
08102         i__ = nku;
08103         if (near__[i__] == ku) {
08104             near__[i__] = next[ku];
08105         } else {
08106             i__ = near__[i__];
08107 L2:
08108             i0 = i__;
08109             i__ = next[i0];
08110             if (i__ != ku) {
08111                 goto L2;
08112             }
08113             next[i0] = next[ku];
08114         }
08115         near__[ku] = 0;
08116 
08117 /* Bypass duplicate nodes. */
08118 
08119         if (dist[ku] <= *tol - 1.) {
08120             indx[ku] = -nku;
08121             ++lcnt[nku];
08122             goto L6;
08123         }
08124 
08125 
08126 /* Add a new triangulation node KT with LCNT(KT) = 1. */
08127         ++kt;
08128         x[kt] = x[ku];
08129         y[kt] = y[ku];
08130         z__[kt] = z__[ku];
08131         indx[ku] = kt;
08132         lcnt[kt] = 1;
08133         addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08134                 , lnew, ier);
08135         if (*ier != 0) {
08136             *n = 0;
08137             *ier = -3;
08138             return 0;
08139         }
08140 
08141 /* Loop on neighbors J of node KT. */
08142 
08143         lpl = lend[kt];
08144         lp = lpl;
08145 L3:
08146         lp = lptr[lp];
08147         j = (i__2 = list[lp], abs(i__2));
08148 
08149 /* Loop on elements I in the sequence of unprocessed nodes */
08150 /*   associated with J:  KT is a candidate for replacing J */
08151 /*   as the nearest triangulation node to I.  The next value */
08152 /*   of I in the sequence, NEXT(I), must be saved before I */
08153 /*   is moved because it is altered by adding I to KT's set. */
08154 
08155         i__ = near__[j];
08156 L4:
08157         if (i__ == 0) {
08158             goto L5;
08159         }
08160         nexti = next[i__];
08161 
08162 /* Test for the distance from I to KT less than the distance */
08163 /*   from I to J. */
08164 
08165         d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08166         if (d__ < dist[i__]) {
08167 
08168 /* Replace J by KT as the nearest triangulation node to I: */
08169 /*   update NEAR(I) and DIST(I), and remove I from J's set */
08170 /*   of unprocessed nodes and add it to KT's set. */
08171 
08172             near__[i__] = kt;
08173             dist[i__] = d__;
08174             if (i__ == near__[j]) {
08175                 near__[j] = nexti;
08176             } else {
08177                 next[i0] = nexti;
08178             }
08179             next[i__] = near__[kt];
08180             near__[kt] = i__;
08181         } else {
08182             i0 = i__;
08183         }
08184 
08185 /* Bottom of loop on I. */
08186 
08187         i__ = nexti;
08188         goto L4;
08189 
08190 /* Bottom of loop on neighbors J. */
08191 
08192 L5:
08193         if (lp != lpl) {
08194             goto L3;
08195         }
08196 L6:
08197         ;
08198     }
08199     *n = kt;
08200     *ier = 0;
08201     return 0;
08202 } /* trmsh3_ */
08203 
08204 /* stripack.dbl sent by Robert on 06/03/03 */
08205 /* Subroutine */ int addnod_(int *nst, int *k, double *x,
08206         double *y, double *z__, int *list, int *lptr, int
08207         *lend, int *lnew, int *ier)
08208 {
08209     /* Initialized data */
08210 
08211     static double tol = 0.;
08212 
08213     /* System generated locals */
08214     int i__1;
08215 
08216     /* Local variables */
08217     static int l;
08218     static double p[3], b1, b2, b3;
08219     static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08220     extern /* Subroutine */ int swap_(int *, int *, int *,
08221             int *, int *, int *, int *, int *);
08222     static int lpo1s;
08223     extern /* Subroutine */ int bdyadd_(int *, int *, int *,
08224             int *, int *, int *, int *), intadd_(int *,
08225             int *, int *, int *, int *, int *, int *,
08226             int *), trfind_(int *, double *, int *,
08227             double *, double *, double *, int *, int *,
08228             int *, double *, double *, double *, int *,
08229             int *, int *), covsph_(int *, int *, int *,
08230             int *, int *, int *);
08231     extern int lstptr_(int *, int *, int *, int *);
08232     extern long int swptst_(int *, int *, int *, int *,
08233             double *, double *, double *);
08234 
08235 
08236 /* *********************************************************** */
08237 
08238 /*                                              From STRIPACK */
08239 /*                                            Robert J. Renka */
08240 /*                                  Dept. of Computer Science */
08241 /*                                       Univ. of North Texas */
08242 /*                                           renka@cs.unt.edu */
08243 /*                                                   01/08/03 */
08244 
08245 /*   This subroutine adds node K to a triangulation of the */
08246 /* convex hull of nodes 1,...,K-1, producing a triangulation */
08247 /* of the convex hull of nodes 1,...,K. */
08248 
08249 /*   The algorithm consists of the following steps:  node K */
08250 /* is located relative to the triangulation (TRFIND), its */
08251 /* index is added to the data structure (INTADD or BDYADD), */
08252 /* and a sequence of swaps (SWPTST and SWAP) are applied to */
08253 /* the arcs opposite K so that all arcs incident on node K */
08254 /* and opposite node K are locally optimal (satisfy the cir- */
08255 /* cumcircle test).  Thus, if a Delaunay triangulation is */
08256 /* input, a Delaunay triangulation will result. */
08257 
08258 
08259 /* On input: */
08260 
08261 /*       NST = Index of a node at which TRFIND begins its */
08262 /*             search.  Search time depends on the proximity */
08263 /*             of this node to K.  If NST < 1, the search is */
08264 /*             begun at node K-1. */
08265 
08266 /*       K = Nodal index (index for X, Y, Z, and LEND) of the */
08267 /*           new node to be added.  K .GE. 4. */
08268 
08269 /*       X,Y,Z = Arrays of length .GE. K containing Car- */
08270 /*               tesian coordinates of the nodes. */
08271 /*               (X(I),Y(I),Z(I)) defines node I for */
08272 /*               I = 1,...,K. */
08273 
08274 /* The above parameters are not altered by this routine. */
08275 
08276 /*       LIST,LPTR,LEND,LNEW = Data structure associated with */
08277 /*                             the triangulation of nodes 1 */
08278 /*                             to K-1.  The array lengths are */
08279 /*                             assumed to be large enough to */
08280 /*                             add node K.  Refer to Subrou- */
08281 /*                             tine TRMESH. */
08282 
08283 /* On output: */
08284 
08285 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
08286 /*                             the addition of node K as the */
08287 /*                             last entry unless IER .NE. 0 */
08288 /*                             and IER .NE. -3, in which case */
08289 /*                             the arrays are not altered. */
08290 
08291 /*       IER = Error indicator: */
08292 /*             IER =  0 if no errors were encountered. */
08293 /*             IER = -1 if K is outside its valid range */
08294 /*                      on input. */
08295 /*             IER = -2 if all nodes (including K) are col- */
08296 /*                      linear (lie on a common geodesic). */
08297 /*             IER =  L if nodes L and K coincide for some */
08298 /*                      L < K.  Refer to TOL below. */
08299 
08300 /* Modules required by ADDNOD:  BDYADD, COVSPH, INSERT, */
08301 /*                                INTADD, JRAND, LSTPTR, */
08302 /*                                STORE, SWAP, SWPTST, */
08303 /*                                TRFIND */
08304 
08305 /* Intrinsic function called by ADDNOD:  ABS */
08306 
08307 /* *********************************************************** */
08308 
08309 
08310 /* Local parameters: */
08311 
08312 /* B1,B2,B3 = Unnormalized barycentric coordinates returned */
08313 /*              by TRFIND. */
08314 /* I1,I2,I3 = Vertex indexes of a triangle containing K */
08315 /* IN1 =      Vertex opposite K:  first neighbor of IO2 */
08316 /*              that precedes IO1.  IN1,IO1,IO2 are in */
08317 /*              counterclockwise order. */
08318 /* IO1,IO2 =  Adjacent neighbors of K defining an arc to */
08319 /*              be tested for a swap */
08320 /* IST =      Index of node at which TRFIND begins its search */
08321 /* KK =       Local copy of K */
08322 /* KM1 =      K-1 */
08323 /* L =        Vertex index (I1, I2, or I3) returned in IER */
08324 /*              if node K coincides with a vertex */
08325 /* LP =       LIST pointer */
08326 /* LPF =      LIST pointer to the first neighbor of K */
08327 /* LPO1 =     LIST pointer to IO1 */
08328 /* LPO1S =    Saved value of LPO1 */
08329 /* P =        Cartesian coordinates of node K */
08330 /* TOL =      Tolerance defining coincident nodes:  bound on */
08331 /*              the deviation from 1 of the cosine of the */
08332 /*              angle between the nodes. */
08333 /*              Note that |1-cos(A)| is approximately A*A/2. */
08334 
08335     /* Parameter adjustments */
08336     --lend;
08337     --z__;
08338     --y;
08339     --x;
08340     --list;
08341     --lptr;
08342 
08343     /* Function Body */
08344 
08345     kk = *k;
08346     if (kk < 4) {
08347         goto L3;
08348     }
08349 
08350 /* Initialization: */
08351     km1 = kk - 1;
08352     ist = *nst;
08353     if (ist < 1) {
08354         ist = km1;
08355     }
08356     p[0] = x[kk];
08357     p[1] = y[kk];
08358     p[2] = z__[kk];
08359 
08360 /* Find a triangle (I1,I2,I3) containing K or the rightmost */
08361 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
08362 /*   from node K. */
08363     trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08364             , &b1, &b2, &b3, &i1, &i2, &i3);
08365 
08366 /*   Test for collinear or (nearly) duplicate nodes. */
08367 
08368     if (i1 == 0) {
08369         goto L4;
08370     }
08371     l = i1;
08372     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08373         goto L5;
08374     }
08375     l = i2;
08376     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08377         goto L5;
08378     }
08379     if (i3 != 0) {
08380         l = i3;
08381         if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08382             goto L5;
08383         }
08384         intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08385     } else {
08386         if (i1 != i2) {
08387             bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08388         } else {
08389             covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08390         }
08391     }
08392     *ier = 0;
08393 
08394 /* Initialize variables for optimization of the */
08395 /*   triangulation. */
08396     lp = lend[kk];
08397     lpf = lptr[lp];
08398     io2 = list[lpf];
08399     lpo1 = lptr[lpf];
08400     io1 = (i__1 = list[lpo1], abs(i__1));
08401 
08402 /* Begin loop:  find the node opposite K. */
08403 
08404 L1:
08405     lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08406     if (list[lp] < 0) {
08407         goto L2;
08408     }
08409     lp = lptr[lp];
08410     in1 = (i__1 = list[lp], abs(i__1));
08411 
08412 /* Swap test:  if a swap occurs, two new arcs are */
08413 /*             opposite K and must be tested. */
08414 
08415     lpo1s = lpo1;
08416     if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08417         goto L2;
08418     }
08419     swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08420     if (lpo1 == 0) {
08421 
08422 /*   A swap is not possible because KK and IN1 are already */
08423 /*     adjacent.  This error in SWPTST only occurs in the */
08424 /*     neutral case and when there are nearly duplicate */
08425 /*     nodes. */
08426 
08427         lpo1 = lpo1s;
08428         goto L2;
08429     }
08430     io1 = in1;
08431     goto L1;
08432 
08433 /* No swap occurred.  Test for termination and reset */
08434 /*   IO2 and IO1. */
08435 
08436 L2:
08437     if (lpo1 == lpf || list[lpo1] < 0) {
08438         return 0;
08439     }
08440     io2 = io1;
08441     lpo1 = lptr[lpo1];
08442     io1 = (i__1 = list[lpo1], abs(i__1));
08443     goto L1;
08444 
08445 /* KK < 4. */
08446 
08447 L3:
08448     *ier = -1;
08449     return 0;
08450 
08451 /* All nodes are collinear. */
08452 
08453 L4:
08454     *ier = -2;
08455     return 0;
08456 
08457 /* Nodes L and K coincide. */
08458 
08459 L5:
08460     *ier = l;
08461     return 0;
08462 } /* addnod_ */
08463 
08464 double angle_(double *v1, double *v2, double *v3)
08465 {
08466     /* System generated locals */
08467     double ret_val;
08468 
08469     /* Builtin functions */
08470     //double sqrt(double), acos(double);
08471 
08472     /* Local variables */
08473     static double a;
08474     static int i__;
08475     static double ca, s21, s23, u21[3], u23[3];
08476     extern long int left_(double *, double *, double *, double
08477             *, double *, double *, double *, double *,
08478             double *);
08479 
08480 
08481 /* *********************************************************** */
08482 
08483 /*                                              From STRIPACK */
08484 /*                                            Robert J. Renka */
08485 /*                                  Dept. of Computer Science */
08486 /*                                       Univ. of North Texas */
08487 /*                                           renka@cs.unt.edu */
08488 /*                                                   06/03/03 */
08489 
08490 /*   Given a sequence of three nodes (V1,V2,V3) on the sur- */
08491 /* face of the unit sphere, this function returns the */
08492 /* interior angle at V2 -- the dihedral angle between the */
08493 /* plane defined by V2 and V3 (and the origin) and the plane */
08494 /* defined by V2 and V1 or, equivalently, the angle between */
08495 /* the normals V2 X V3 and V2 X V1.  Note that the angle is */
08496 /* in the range 0 to Pi if V3 Left V1->V2, Pi to 2*Pi other- */
08497 /* wise.  The surface area of a spherical polygon with CCW- */
08498 /* ordered vertices V1, V2, ..., Vm is Asum - (m-2)*Pi, where */
08499 /* Asum is the sum of the m interior angles computed from the */
08500 /* sequences (Vm,V1,V2), (V1,V2,V3), (V2,V3,V4), ..., */
08501 /* (Vm-1,Vm,V1). */
08502 
08503 
08504 /* On input: */
08505 
08506 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08507 /*                  sian coordinates of unit vectors.  These */
08508 /*                  vectors, if nonzero, are implicitly */
08509 /*                  scaled to have length 1. */
08510 
08511 /* Input parameters are not altered by this function. */
08512 
08513 /* On output: */
08514 
08515 /*       ANGLE = Angle defined above, or 0 if V2 X V1 = 0 or */
08516 /*               V2 X V3 = 0. */
08517 
08518 /* Module required by ANGLE:  LEFT */
08519 
08520 /* Intrinsic functions called by ANGLE:  ACOS, SQRT */
08521 
08522 /* *********************************************************** */
08523 
08524 
08525 /* Local parameters: */
08526 
08527 /* A =       Interior angle at V2 */
08528 /* CA =      cos(A) */
08529 /* I =       DO-loop index and index for U21 and U23 */
08530 /* S21,S23 = Sum of squared components of U21 and U23 */
08531 /* U21,U23 = Unit normal vectors to the planes defined by */
08532 /*             pairs of triangle vertices */
08533 
08534 
08535 /* Compute cross products U21 = V2 X V1 and U23 = V2 X V3. */
08536 
08537     /* Parameter adjustments */
08538     --v3;
08539     --v2;
08540     --v1;
08541 
08542     /* Function Body */
08543     u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08544     u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08545     u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08546 
08547     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08548     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08549     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08550 
08551 /* Normalize U21 and U23 to unit vectors. */
08552 
08553     s21 = 0.;
08554     s23 = 0.;
08555     for (i__ = 1; i__ <= 3; ++i__) {
08556         s21 += u21[i__ - 1] * u21[i__ - 1];
08557         s23 += u23[i__ - 1] * u23[i__ - 1];
08558 /* L1: */
08559     }
08560 
08561 /* Test for a degenerate triangle associated with collinear */
08562 /*   vertices. */
08563 
08564     if (s21 == 0. || s23 == 0.) {
08565         ret_val = 0.;
08566         return ret_val;
08567     }
08568     s21 = sqrt(s21);
08569     s23 = sqrt(s23);
08570     for (i__ = 1; i__ <= 3; ++i__) {
08571         u21[i__ - 1] /= s21;
08572         u23[i__ - 1] /= s23;
08573 /* L2: */
08574     }
08575 
08576 /* Compute the angle A between normals: */
08577 
08578 /*   CA = cos(A) = <U21,U23> */
08579 
08580     ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08581     if (ca < -1.) {
08582         ca = -1.;
08583     }
08584     if (ca > 1.) {
08585         ca = 1.;
08586     }
08587     a = acos(ca);
08588 
08589 /* Adjust A to the interior angle:  A > Pi iff */
08590 /*   V3 Right V1->V2. */
08591 
08592     if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08593             , &v3[3])) {
08594         a = acos(-1.) * 2. - a;
08595     }
08596     ret_val = a;
08597     return ret_val;
08598 } /* angle_ */
08599 
08600 double areas_(double *v1, double *v2, double *v3)
08601 {
08602     /* System generated locals */
08603     double ret_val;
08604 
08605     /* Builtin functions */
08606     //double sqrt(double), acos(double);
08607 
08608     /* Local variables */
08609     static int i__;
08610     static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08611             ca2, ca3;
08612 
08613 
08614 /* *********************************************************** */
08615 
08616 /*                                              From STRIPACK */
08617 /*                                            Robert J. Renka */
08618 /*                                  Dept. of Computer Science */
08619 /*                                       Univ. of North Texas */
08620 /*                                           renka@cs.unt.edu */
08621 /*                                                   06/22/98 */
08622 
08623 /*   This function returns the area of a spherical triangle */
08624 /* on the unit sphere. */
08625 
08626 
08627 /* On input: */
08628 
08629 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08630 /*                  sian coordinates of unit vectors (the */
08631 /*                  three triangle vertices in any order). */
08632 /*                  These vectors, if nonzero, are implicitly */
08633 /*                  scaled to have length 1. */
08634 
08635 /* Input parameters are not altered by this function. */
08636 
08637 /* On output: */
08638 
08639 /*       AREAS = Area of the spherical triangle defined by */
08640 /*               V1, V2, and V3 in the range 0 to 2*PI (the */
08641 /*               area of a hemisphere).  AREAS = 0 (or 2*PI) */
08642 /*               if and only if V1, V2, and V3 lie in (or */
08643 /*               close to) a plane containing the origin. */
08644 
08645 /* Modules required by AREAS:  None */
08646 
08647 /* Intrinsic functions called by AREAS:  ACOS, SQRT */
08648 
08649 /* *********************************************************** */
08650 
08651 
08652 /* Local parameters: */
08653 
08654 /* A1,A2,A3 =    Interior angles of the spherical triangle */
08655 /* CA1,CA2,CA3 = cos(A1), cos(A2), and cos(A3), respectively */
08656 /* I =           DO-loop index and index for Uij */
08657 /* S12,S23,S31 = Sum of squared components of U12, U23, U31 */
08658 /* U12,U23,U31 = Unit normal vectors to the planes defined by */
08659 /*                 pairs of triangle vertices */
08660 
08661 
08662 /* Compute cross products Uij = Vi X Vj. */
08663 
08664     /* Parameter adjustments */
08665     --v3;
08666     --v2;
08667     --v1;
08668 
08669     /* Function Body */
08670     u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08671     u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08672     u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08673 
08674     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08675     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08676     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08677 
08678     u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08679     u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08680     u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08681 
08682 /* Normalize Uij to unit vectors. */
08683 
08684     s12 = 0.;
08685     s23 = 0.;
08686     s31 = 0.;
08687     for (i__ = 1; i__ <= 3; ++i__) {
08688         s12 += u12[i__ - 1] * u12[i__ - 1];
08689         s23 += u23[i__ - 1] * u23[i__ - 1];
08690         s31 += u31[i__ - 1] * u31[i__ - 1];
08691 /* L2: */
08692     }
08693 
08694 /* Test for a degenerate triangle associated with collinear */
08695 /*   vertices. */
08696 
08697     if (s12 == 0. || s23 == 0. || s31 == 0.) {
08698         ret_val = 0.;
08699         return ret_val;
08700     }
08701     s12 = sqrt(s12);
08702     s23 = sqrt(s23);
08703     s31 = sqrt(s31);
08704     for (i__ = 1; i__ <= 3; ++i__) {
08705         u12[i__ - 1] /= s12;
08706         u23[i__ - 1] /= s23;
08707         u31[i__ - 1] /= s31;
08708 /* L3: */
08709     }
08710 
08711 /* Compute interior angles Ai as the dihedral angles between */
08712 /*   planes: */
08713 /*           CA1 = cos(A1) = -<U12,U31> */
08714 /*           CA2 = cos(A2) = -<U23,U12> */
08715 /*           CA3 = cos(A3) = -<U31,U23> */
08716 
08717     ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08718     ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08719     ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08720     if (ca1 < -1.) {
08721         ca1 = -1.;
08722     }
08723     if (ca1 > 1.) {
08724         ca1 = 1.;
08725     }
08726     if (ca2 < -1.) {
08727         ca2 = -1.;
08728     }
08729     if (ca2 > 1.) {
08730         ca2 = 1.;
08731     }
08732     if (ca3 < -1.) {
08733         ca3 = -1.;
08734     }
08735     if (ca3 > 1.) {
08736         ca3 = 1.;
08737     }
08738     a1 = acos(ca1);
08739     a2 = acos(ca2);
08740     a3 = acos(ca3);
08741 
08742 /* Compute AREAS = A1 + A2 + A3 - PI. */
08743 
08744     ret_val = a1 + a2 + a3 - acos(-1.);
08745     if (ret_val < 0.) {
08746         ret_val = 0.;
08747     }
08748     return ret_val;
08749 } /* areas_ */
08750 
08751 double Util::areav_(int *k, int *n, double *x, double *y,
08752         double *z__, int *list, int *lptr, int *lend, int
08753         *ier)
08754 {
08755     /* Initialized data */
08756 
08757     static double amax = 6.28;
08758 
08759     /* System generated locals */
08760     double ret_val;
08761 
08762     /* Local variables */
08763     static double a, c0[3], c2[3], c3[3];
08764     static int n1, n2, n3;
08765     static double v1[3], v2[3], v3[3];
08766     static int lp, lpl, ierr;
08767     static double asum;
08768     extern double areas_(double *, double *, double *);
08769     static long int first;
08770     extern /* Subroutine */ int circum_(double *, double *,
08771             double *, double *, int *);
08772 
08773 
08774 /* *********************************************************** */
08775 
08776 /*                                            Robert J. Renka */
08777 /*                                  Dept. of Computer Science */
08778 /*                                       Univ. of North Texas */
08779 /*                                           renka@cs.unt.edu */
08780 /*                                                   10/25/02 */
08781 
08782 /*   Given a Delaunay triangulation and the index K of an */
08783 /* interior node, this subroutine returns the (surface) area */
08784 /* of the Voronoi region associated with node K.  The Voronoi */
08785 /* region is the polygon whose vertices are the circumcenters */
08786 /* of the triangles that contain node K, where a triangle */
08787 /* circumcenter is the point (unit vector) lying at the same */
08788 /* angular distance from the three vertices and contained in */
08789 /* the same hemisphere as the vertices. */
08790 
08791 
08792 /* On input: */
08793 
08794 /*       K = Nodal index in the range 1 to N. */
08795 
08796 /*       N = Number of nodes in the triangulation.  N > 3. */
08797 
08798 /*       X,Y,Z = Arrays of length N containing the Cartesian */
08799 /*               coordinates of the nodes (unit vectors). */
08800 
08801 /*       LIST,LPTR,LEND = Data structure defining the trian- */
08802 /*                        gulation.  Refer to Subroutine */
08803 /*                        TRMESH. */
08804 
08805 /* Input parameters are not altered by this function. */
08806 
08807 /* On output: */
08808 
08809 /*       AREAV = Area of Voronoi region K unless IER > 0, */
08810 /*               in which case AREAV = 0. */
08811 
08812 /*       IER = Error indicator: */
08813 /*             IER = 0 if no errors were encountered. */
08814 /*             IER = 1 if K or N is outside its valid range */
08815 /*                     on input. */
08816 /*             IER = 2 if K indexes a boundary node. */
08817 /*             IER = 3 if an error flag is returned by CIRCUM */
08818 /*                     (null triangle). */
08819 /*             IER = 4 if AREAS returns a value greater than */
08820 /*                     AMAX (defined below). */
08821 
08822 /* Modules required by AREAV:  AREAS, CIRCUM */
08823 
08824 /* *********************************************************** */
08825 
08826 
08827 /* Maximum valid triangle area is less than 2*Pi: */
08828 
08829     /* Parameter adjustments */
08830     --lend;
08831     --z__;
08832     --y;
08833     --x;
08834     --list;
08835     --lptr;
08836 
08837     /* Function Body */
08838 
08839 /* Test for invalid input. */
08840 
08841     if (*k < 1 || *k > *n || *n <= 3) {
08842         goto L11;
08843     }
08844 
08845 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
08846 /*   FIRST = TRUE only for the first triangle. */
08847 /*   The Voronoi region area is accumulated in ASUM. */
08848 
08849     n1 = *k;
08850     v1[0] = x[n1];
08851     v1[1] = y[n1];
08852     v1[2] = z__[n1];
08853     lpl = lend[n1];
08854     n3 = list[lpl];
08855     if (n3 < 0) {
08856         goto L12;
08857     }
08858     lp = lpl;
08859     first = TRUE_;
08860     asum = 0.;
08861 
08862 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
08863 
08864 L1:
08865     n2 = n3;
08866     lp = lptr[lp];
08867     n3 = list[lp];
08868     v2[0] = x[n2];
08869     v2[1] = y[n2];
08870     v2[2] = z__[n2];
08871     v3[0] = x[n3];
08872     v3[1] = y[n3];
08873     v3[2] = z__[n3];
08874     if (first) {
08875 
08876 /* First triangle:  compute the circumcenter C3 and save a */
08877 /*   copy in C0. */
08878 
08879         circum_(v1, v2, v3, c3, &ierr);
08880         if (ierr != 0) {
08881             goto L13;
08882         }
08883         c0[0] = c3[0];
08884         c0[1] = c3[1];
08885         c0[2] = c3[2];
08886         first = FALSE_;
08887     } else {
08888 
08889 /* Set C2 to C3, compute the new circumcenter C3, and compute */
08890 /*   the area A of triangle (V1,C2,C3). */
08891 
08892         c2[0] = c3[0];
08893         c2[1] = c3[1];
08894         c2[2] = c3[2];
08895         circum_(v1, v2, v3, c3, &ierr);
08896         if (ierr != 0) {
08897             goto L13;
08898         }
08899         a = areas_(v1, c2, c3);
08900         if (a > amax) {
08901             goto L14;
08902         }
08903         asum += a;
08904     }
08905 
08906 /* Bottom on loop on neighbors of K. */
08907 
08908     if (lp != lpl) {
08909         goto L1;
08910     }
08911 
08912 /* Compute the area of triangle (V1,C3,C0). */
08913 
08914     a = areas_(v1, c3, c0);
08915     if (a > amax) {
08916         goto L14;
08917     }
08918     asum += a;
08919 
08920 /* No error encountered. */
08921 
08922     *ier = 0;
08923     ret_val = asum;
08924     return ret_val;
08925 
08926 /* Invalid input. */
08927 
08928 L11:
08929     *ier = 1;
08930     ret_val = 0.;
08931     return ret_val;
08932 
08933 /* K indexes a boundary node. */
08934 
08935 L12:
08936     *ier = 2;
08937     ret_val = 0.;
08938     return ret_val;
08939 
08940 /* Error in CIRCUM. */
08941 
08942 L13:
08943     *ier = 3;
08944     ret_val = 0.;
08945     return ret_val;
08946 
08947 /* AREAS value larger than AMAX. */
08948 
08949 L14:
08950     *ier = 4;
08951     ret_val = 0.;
08952     return ret_val;
08953 } /* areav_ */
08954 
08955 double areav_new__(int *k, int *n, double *x, double *y,
08956         double *z__, int *list, int *lptr, int *lend, int
08957         *ier)
08958 {
08959     /* System generated locals */
08960     double ret_val = 0;
08961 
08962     /* Builtin functions */
08963     //double acos(double);
08964 
08965     /* Local variables */
08966     static int m;
08967     static double c1[3], c2[3], c3[3];
08968     static int n1, n2, n3;
08969     static double v1[3], v2[3], v3[3];
08970     static int lp;
08971     static double c1s[3], c2s[3];
08972     static int lpl, ierr;
08973     static double asum;
08974     extern double angle_(double *, double *, double *);
08975     static float areav;
08976     extern /* Subroutine */ int circum_(double *, double *,
08977             double *, double *, int *);
08978 
08979 
08980 /* *********************************************************** */
08981 
08982 /*                                            Robert J. Renka */
08983 /*                                  Dept. of Computer Science */
08984 /*                                       Univ. of North Texas */
08985 /*                                           renka@cs.unt.edu */
08986 /*                                                   06/03/03 */
08987 
08988 /*   Given a Delaunay triangulation and the index K of an */
08989 /* interior node, this subroutine returns the (surface) area */
08990 /* of the Voronoi region associated with node K.  The Voronoi */
08991 /* region is the polygon whose vertices are the circumcenters */
08992 /* of the triangles that contain node K, where a triangle */
08993 /* circumcenter is the point (unit vector) lying at the same */
08994 /* angular distance from the three vertices and contained in */
08995 /* the same hemisphere as the vertices.  The Voronoi region */
08996 /* area is computed as Asum-(m-2)*Pi, where m is the number */
08997 /* of Voronoi vertices (neighbors of K) and Asum is the sum */
08998 /* of interior angles at the vertices. */
08999 
09000 
09001 /* On input: */
09002 
09003 /*       K = Nodal index in the range 1 to N. */
09004 
09005 /*       N = Number of nodes in the triangulation.  N > 3. */
09006 
09007 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09008 /*               coordinates of the nodes (unit vectors). */
09009 
09010 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09011 /*                        gulation.  Refer to Subroutine */
09012 /*                        TRMESH. */
09013 
09014 /* Input parameters are not altered by this function. */
09015 
09016 /* On output: */
09017 
09018 /*       AREAV = Area of Voronoi region K unless IER > 0, */
09019 /*               in which case AREAV = 0. */
09020 
09021 /*       IER = Error indicator: */
09022 /*             IER = 0 if no errors were encountered. */
09023 /*             IER = 1 if K or N is outside its valid range */
09024 /*                     on input. */
09025 /*             IER = 2 if K indexes a boundary node. */
09026 /*             IER = 3 if an error flag is returned by CIRCUM */
09027 /*                     (null triangle). */
09028 
09029 /* Modules required by AREAV:  ANGLE, CIRCUM */
09030 
09031 /* Intrinsic functions called by AREAV:  ACOS, DBLE */
09032 
09033 /* *********************************************************** */
09034 
09035 
09036 /* Test for invalid input. */
09037 
09038     /* Parameter adjustments */
09039     --lend;
09040     --z__;
09041     --y;
09042     --x;
09043     --list;
09044     --lptr;
09045 
09046     /* Function Body */
09047     if (*k < 1 || *k > *n || *n <= 3) {
09048         goto L11;
09049     }
09050 
09051 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
09052 /*   The number of neighbors and the sum of interior angles */
09053 /*   are accumulated in M and ASUM, respectively. */
09054 
09055     n1 = *k;
09056     v1[0] = x[n1];
09057     v1[1] = y[n1];
09058     v1[2] = z__[n1];
09059     lpl = lend[n1];
09060     n3 = list[lpl];
09061     if (n3 < 0) {
09062         goto L12;
09063     }
09064     lp = lpl;
09065     m = 0;
09066     asum = 0.;
09067 
09068 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
09069 
09070 L1:
09071     ++m;
09072     n2 = n3;
09073     lp = lptr[lp];
09074     n3 = list[lp];
09075     v2[0] = x[n2];
09076     v2[1] = y[n2];
09077     v2[2] = z__[n2];
09078     v3[0] = x[n3];
09079     v3[1] = y[n3];
09080     v3[2] = z__[n3];
09081     if (m == 1) {
09082 
09083 /* First triangle:  compute the circumcenter C2 and save a */
09084 /*   copy in C1S. */
09085 
09086         circum_(v1, v2, v3, c2, &ierr);
09087         if (ierr != 0) {
09088             goto L13;
09089         }
09090         c1s[0] = c2[0];
09091         c1s[1] = c2[1];
09092         c1s[2] = c2[2];
09093     } else if (m == 2) {
09094 
09095 /* Second triangle:  compute the circumcenter C3 and save a */
09096 /*   copy in C2S. */
09097 
09098         circum_(v1, v2, v3, c3, &ierr);
09099         if (ierr != 0) {
09100             goto L13;
09101         }
09102         c2s[0] = c3[0];
09103         c2s[1] = c3[1];
09104         c2s[2] = c3[2];
09105     } else {
09106 
09107 /* Set C1 to C2, set C2 to C3, compute the new circumcenter */
09108 /*   C3, and compute the interior angle at C2 from the */
09109 /*   sequence of vertices (C1,C2,C3). */
09110 
09111         c1[0] = c2[0];
09112         c1[1] = c2[1];
09113         c1[2] = c2[2];
09114         c2[0] = c3[0];
09115         c2[1] = c3[1];
09116         c2[2] = c3[2];
09117         circum_(v1, v2, v3, c3, &ierr);
09118         if (ierr != 0) {
09119             goto L13;
09120         }
09121         asum += angle_(c1, c2, c3);
09122     }
09123 
09124 /* Bottom on loop on neighbors of K. */
09125 
09126     if (lp != lpl) {
09127         goto L1;
09128     }
09129 
09130 /* C3 is the last vertex.  Compute its interior angle from */
09131 /*   the sequence (C2,C3,C1S). */
09132 
09133     asum += angle_(c2, c3, c1s);
09134 
09135 /* Compute the interior angle at C1S from */
09136 /*   the sequence (C3,C1S,C2S). */
09137 
09138     asum += angle_(c3, c1s, c2s);
09139 
09140 /* No error encountered. */
09141 
09142     *ier = 0;
09143     ret_val = asum - (double) (m - 2) * acos(-1.);
09144     return ret_val;
09145 
09146 /* Invalid input. */
09147 
09148 L11:
09149     *ier = 1;
09150     areav = 0.f;
09151     return ret_val;
09152 
09153 /* K indexes a boundary node. */
09154 
09155 L12:
09156     *ier = 2;
09157     areav = 0.f;
09158     return ret_val;
09159 
09160 /* Error in CIRCUM. */
09161 
09162 L13:
09163     *ier = 3;
09164     areav = 0.f;
09165     return ret_val;
09166 } /* areav_new__ */
09167 
09168 /* Subroutine */ int bdyadd_(int *kk, int *i1, int *i2, int *
09169         list, int *lptr, int *lend, int *lnew)
09170 {
09171     static int k, n1, n2, lp, lsav, nsav, next;
09172     extern /* Subroutine */ int insert_(int *, int *, int *,
09173             int *, int *);
09174 
09175 
09176 /* *********************************************************** */
09177 
09178 /*                                              From STRIPACK */
09179 /*                                            Robert J. Renka */
09180 /*                                  Dept. of Computer Science */
09181 /*                                       Univ. of North Texas */
09182 /*                                           renka@cs.unt.edu */
09183 /*                                                   07/11/96 */
09184 
09185 /*   This subroutine adds a boundary node to a triangulation */
09186 /* of a set of KK-1 points on the unit sphere.  The data */
09187 /* structure is updated with the insertion of node KK, but no */
09188 /* optimization is performed. */
09189 
09190 /*   This routine is identical to the similarly named routine */
09191 /* in TRIPACK. */
09192 
09193 
09194 /* On input: */
09195 
09196 /*       KK = Index of a node to be connected to the sequence */
09197 /*            of all visible boundary nodes.  KK .GE. 1 and */
09198 /*            KK must not be equal to I1 or I2. */
09199 
09200 /*       I1 = First (rightmost as viewed from KK) boundary */
09201 /*            node in the triangulation that is visible from */
09202 /*            node KK (the line segment KK-I1 intersects no */
09203 /*            arcs. */
09204 
09205 /*       I2 = Last (leftmost) boundary node that is visible */
09206 /*            from node KK.  I1 and I2 may be determined by */
09207 /*            Subroutine TRFIND. */
09208 
09209 /* The above parameters are not altered by this routine. */
09210 
09211 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09212 /*                             created by Subroutine TRMESH. */
09213 /*                             Nodes I1 and I2 must be in- */
09214 /*                             cluded in the triangulation. */
09215 
09216 /* On output: */
09217 
09218 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09219 /*                             the addition of node KK.  Node */
09220 /*                             KK is connected to I1, I2, and */
09221 /*                             all boundary nodes in between. */
09222 
09223 /* Module required by BDYADD:  INSERT */
09224 
09225 /* *********************************************************** */
09226 
09227 
09228 /* Local parameters: */
09229 
09230 /* K =     Local copy of KK */
09231 /* LP =    LIST pointer */
09232 /* LSAV =  LIST pointer */
09233 /* N1,N2 = Local copies of I1 and I2, respectively */
09234 /* NEXT =  Boundary node visible from K */
09235 /* NSAV =  Boundary node visible from K */
09236 
09237     /* Parameter adjustments */
09238     --lend;
09239     --lptr;
09240     --list;
09241 
09242     /* Function Body */
09243     k = *kk;
09244     n1 = *i1;
09245     n2 = *i2;
09246 
09247 /* Add K as the last neighbor of N1. */
09248 
09249     lp = lend[n1];
09250     lsav = lptr[lp];
09251     lptr[lp] = *lnew;
09252     list[*lnew] = -k;
09253     lptr[*lnew] = lsav;
09254     lend[n1] = *lnew;
09255     ++(*lnew);
09256     next = -list[lp];
09257     list[lp] = next;
09258     nsav = next;
09259 
09260 /* Loop on the remaining boundary nodes between N1 and N2, */
09261 /*   adding K as the first neighbor. */
09262 
09263 L1:
09264     lp = lend[next];
09265     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09266     if (next == n2) {
09267         goto L2;
09268     }
09269     next = -list[lp];
09270     list[lp] = next;
09271     goto L1;
09272 
09273 /* Add the boundary nodes between N1 and N2 as neighbors */
09274 /*   of node K. */
09275 
09276 L2:
09277     lsav = *lnew;
09278     list[*lnew] = n1;
09279     lptr[*lnew] = *lnew + 1;
09280     ++(*lnew);
09281     next = nsav;
09282 
09283 L3:
09284     if (next == n2) {
09285         goto L4;
09286     }
09287     list[*lnew] = next;
09288     lptr[*lnew] = *lnew + 1;
09289     ++(*lnew);
09290     lp = lend[next];
09291     next = list[lp];
09292     goto L3;
09293 
09294 L4:
09295     list[*lnew] = -n2;
09296     lptr[*lnew] = lsav;
09297     lend[k] = *lnew;
09298     ++(*lnew);
09299     return 0;
09300 } /* bdyadd_ */
09301 
09302 /* Subroutine */ int bnodes_(int *n, int *list, int *lptr,
09303         int *lend, int *nodes, int *nb, int *na, int *nt)
09304 {
09305     /* System generated locals */
09306     int i__1;
09307 
09308     /* Local variables */
09309     static int k, n0, lp, nn, nst;
09310 
09311 
09312 /* *********************************************************** */
09313 
09314 /*                                              From STRIPACK */
09315 /*                                            Robert J. Renka */
09316 /*                                  Dept. of Computer Science */
09317 /*                                       Univ. of North Texas */
09318 /*                                           renka@cs.unt.edu */
09319 /*                                                   06/26/96 */
09320 
09321 /*   Given a triangulation of N nodes on the unit sphere */
09322 /* created by Subroutine TRMESH, this subroutine returns an */
09323 /* array containing the indexes (if any) of the counterclock- */
09324 /* wise-ordered sequence of boundary nodes -- the nodes on */
09325 /* the boundary of the convex hull of the set of nodes.  (The */
09326 /* boundary is empty if the nodes do not lie in a single */
09327 /* hemisphere.)  The numbers of boundary nodes, arcs, and */
09328 /* triangles are also returned. */
09329 
09330 
09331 /* On input: */
09332 
09333 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09334 
09335 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09336 /*                        gulation.  Refer to Subroutine */
09337 /*                        TRMESH. */
09338 
09339 /* The above parameters are not altered by this routine. */
09340 
09341 /*       NODES = int array of length at least NB */
09342 /*               (NB .LE. N). */
09343 
09344 /* On output: */
09345 
09346 /*       NODES = Ordered sequence of boundary node indexes */
09347 /*               in the range 1 to N (in the first NB loca- */
09348 /*               tions). */
09349 
09350 /*       NB = Number of boundary nodes. */
09351 
09352 /*       NA,NT = Number of arcs and triangles, respectively, */
09353 /*               in the triangulation. */
09354 
09355 /* Modules required by BNODES:  None */
09356 
09357 /* *********************************************************** */
09358 
09359 
09360 /* Local parameters: */
09361 
09362 /* K =   NODES index */
09363 /* LP =  LIST pointer */
09364 /* N0 =  Boundary node to be added to NODES */
09365 /* NN =  Local copy of N */
09366 /* NST = First element of nodes (arbitrarily chosen to be */
09367 /*         the one with smallest index) */
09368 
09369     /* Parameter adjustments */
09370     --lend;
09371     --list;
09372     --lptr;
09373     --nodes;
09374 
09375     /* Function Body */
09376     nn = *n;
09377 
09378 /* Search for a boundary node. */
09379 
09380     i__1 = nn;
09381     for (nst = 1; nst <= i__1; ++nst) {
09382         lp = lend[nst];
09383         if (list[lp] < 0) {
09384             goto L2;
09385         }
09386 /* L1: */
09387     }
09388 
09389 /* The triangulation contains no boundary nodes. */
09390 
09391     *nb = 0;
09392     *na = (nn - 2) * 3;
09393     *nt = nn - (2<<1);
09394     return 0;
09395 
09396 /* NST is the first boundary node encountered.  Initialize */
09397 /*   for traversal of the boundary. */
09398 
09399 L2:
09400     nodes[1] = nst;
09401     k = 1;
09402     n0 = nst;
09403 
09404 /* Traverse the boundary in counterclockwise order. */
09405 
09406 L3:
09407     lp = lend[n0];
09408     lp = lptr[lp];
09409     n0 = list[lp];
09410     if (n0 == nst) {
09411         goto L4;
09412     }
09413     ++k;
09414     nodes[k] = n0;
09415     goto L3;
09416 
09417 /* Store the counts. */
09418 
09419 L4:
09420     *nb = k;
09421     *nt = (*n << 1) - *nb - 2;
09422     *na = *nt + *n - 1;
09423     return 0;
09424 } /* bnodes_ */
09425 
09426 /* Subroutine */ int circle_(int *k, double *xc, double *yc,
09427         int *ier)
09428 {
09429     /* System generated locals */
09430     int i__1;
09431 
09432     /* Builtin functions */
09433     //double atan(double), cos(double), sin(double);
09434 
09435     /* Local variables */
09436     static double a, c__;
09437     static int i__;
09438     static double s;
09439     static int k2, k3;
09440     static double x0, y0;
09441     static int kk, np1;
09442 
09443 
09444 /* *********************************************************** */
09445 
09446 /*                                              From STRIPACK */
09447 /*                                            Robert J. Renka */
09448 /*                                  Dept. of Computer Science */
09449 /*                                       Univ. of North Texas */
09450 /*                                           renka@cs.unt.edu */
09451 /*                                                   04/06/90 */
09452 
09453 /*   This subroutine computes the coordinates of a sequence */
09454 /* of N equally spaced points on the unit circle centered at */
09455 /* (0,0).  An N-sided polygonal approximation to the circle */
09456 /* may be plotted by connecting (XC(I),YC(I)) to (XC(I+1), */
09457 /* YC(I+1)) for I = 1,...,N, where XC(N+1) = XC(1) and */
09458 /* YC(N+1) = YC(1).  A reasonable value for N in this case */
09459 /* is 2*PI*R, where R is the radius of the circle in device */
09460 /* coordinates. */
09461 
09462 
09463 /* On input: */
09464 
09465 /*       K = Number of points in each quadrant, defining N as */
09466 /*           4K.  K .GE. 1. */
09467 
09468 /*       XC,YC = Arrays of length at least N+1 = 4K+1. */
09469 
09470 /* K is not altered by this routine. */
09471 
09472 /* On output: */
09473 
09474 /*       XC,YC = Cartesian coordinates of the points on the */
09475 /*               unit circle in the first N+1 locations. */
09476 /*               XC(I) = cos(A*(I-1)), YC(I) = sin(A*(I-1)), */
09477 /*               where A = 2*PI/N.  Note that XC(N+1) = XC(1) */
09478 /*               and YC(N+1) = YC(1). */
09479 
09480 /*       IER = Error indicator: */
09481 /*             IER = 0 if no errors were encountered. */
09482 /*             IER = 1 if K < 1 on input. */
09483 
09484 /* Modules required by CIRCLE:  None */
09485 
09486 /* Intrinsic functions called by CIRCLE:  ATAN, COS, DBLE, */
09487 /*                                          SIN */
09488 
09489 /* *********************************************************** */
09490 
09491 
09492 /* Local parameters: */
09493 
09494 /* I =     DO-loop index and index for XC and YC */
09495 /* KK =    Local copy of K */
09496 /* K2 =    K*2 */
09497 /* K3 =    K*3 */
09498 /* NP1 =   N+1 = 4*K + 1 */
09499 /* A =     Angular separation between adjacent points */
09500 /* C,S =   Cos(A) and sin(A), respectively, defining a */
09501 /*           rotation through angle A */
09502 /* X0,Y0 = Cartesian coordinates of a point on the unit */
09503 /*           circle in the first quadrant */
09504 
09505     /* Parameter adjustments */
09506     --yc;
09507     --xc;
09508 
09509     /* Function Body */
09510     kk = *k;
09511     k2 = kk << 1;
09512     k3 = kk * 3;
09513     np1 = (kk << 2) + 1;
09514 
09515 /* Test for invalid input, compute A, C, and S, and */
09516 /*   initialize (X0,Y0) to (1,0). */
09517 
09518     if (kk < 1) {
09519         goto L2;
09520     }
09521     a = atan(1.) * 2. / (double) kk;
09522     c__ = cos(a);
09523     s = sin(a);
09524     x0 = 1.;
09525     y0 = 0.;
09526 
09527 /* Loop on points (X0,Y0) in the first quadrant, storing */
09528 /*   the point and its reflections about the x axis, the */
09529 /*   y axis, and the line y = -x. */
09530 
09531     i__1 = kk;
09532     for (i__ = 1; i__ <= i__1; ++i__) {
09533         xc[i__] = x0;
09534         yc[i__] = y0;
09535         xc[i__ + kk] = -y0;
09536         yc[i__ + kk] = x0;
09537         xc[i__ + k2] = -x0;
09538         yc[i__ + k2] = -y0;
09539         xc[i__ + k3] = y0;
09540         yc[i__ + k3] = -x0;
09541 
09542 /*   Rotate (X0,Y0) counterclockwise through angle A. */
09543 
09544         x0 = c__ * x0 - s * y0;
09545         y0 = s * x0 + c__ * y0;
09546 /* L1: */
09547     }
09548 
09549 /* Store the coordinates of the first point as the last */
09550 /*   point. */
09551 
09552     xc[np1] = xc[1];
09553     yc[np1] = yc[1];
09554     *ier = 0;
09555     return 0;
09556 
09557 /* K < 1. */
09558 
09559 L2:
09560     *ier = 1;
09561     return 0;
09562 } /* circle_ */
09563 
09564 /* Subroutine */ int circum_(double *v1, double *v2, double *v3,
09565         double *c__, int *ier)
09566 {
09567     /* Builtin functions */
09568     //double sqrt(double);
09569 
09570     /* Local variables */
09571     static int i__;
09572     static double e1[3], e2[3], cu[3], cnorm;
09573 
09574 
09575 /* *********************************************************** */
09576 
09577 /*                                              From STRIPACK */
09578 /*                                            Robert J. Renka */
09579 /*                                  Dept. of Computer Science */
09580 /*                                       Univ. of North Texas */
09581 /*                                           renka@cs.unt.edu */
09582 /*                                                   10/27/02 */
09583 
09584 /*   This subroutine returns the circumcenter of a spherical */
09585 /* triangle on the unit sphere:  the point on the sphere sur- */
09586 /* face that is equally distant from the three triangle */
09587 /* vertices and lies in the same hemisphere, where distance */
09588 /* is taken to be arc-length on the sphere surface. */
09589 
09590 
09591 /* On input: */
09592 
09593 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
09594 /*                  sian coordinates of the three triangle */
09595 /*                  vertices (unit vectors) in CCW order. */
09596 
09597 /* The above parameters are not altered by this routine. */
09598 
09599 /*       C = Array of length 3. */
09600 
09601 /* On output: */
09602 
09603 /*       C = Cartesian coordinates of the circumcenter unless */
09604 /*           IER > 0, in which case C is not defined.  C = */
09605 /*           (V2-V1) X (V3-V1) normalized to a unit vector. */
09606 
09607 /*       IER = Error indicator: */
09608 /*             IER = 0 if no errors were encountered. */
09609 /*             IER = 1 if V1, V2, and V3 lie on a common */
09610 /*                     line:  (V2-V1) X (V3-V1) = 0. */
09611 /*             (The vertices are not tested for validity.) */
09612 
09613 /* Modules required by CIRCUM:  None */
09614 
09615 /* Intrinsic function called by CIRCUM:  SQRT */
09616 
09617 /* *********************************************************** */
09618 
09619 
09620 /* Local parameters: */
09621 
09622 /* CNORM = Norm of CU:  used to compute C */
09623 /* CU =    Scalar multiple of C:  E1 X E2 */
09624 /* E1,E2 = Edges of the underlying planar triangle: */
09625 /*           V2-V1 and V3-V1, respectively */
09626 /* I =     DO-loop index */
09627 
09628     /* Parameter adjustments */
09629     --c__;
09630     --v3;
09631     --v2;
09632     --v1;
09633 
09634     /* Function Body */
09635     for (i__ = 1; i__ <= 3; ++i__) {
09636         e1[i__ - 1] = v2[i__] - v1[i__];
09637         e2[i__ - 1] = v3[i__] - v1[i__];
09638 /* L1: */
09639     }
09640 
09641 /* Compute CU = E1 X E2 and CNORM**2. */
09642 
09643     cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09644     cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09645     cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09646     cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09647 
09648 /* The vertices lie on a common line if and only if CU is */
09649 /*   the zero vector. */
09650 
09651     if (cnorm != 0.) {
09652 
09653 /*   No error:  compute C. */
09654 
09655         cnorm = sqrt(cnorm);
09656         for (i__ = 1; i__ <= 3; ++i__) {
09657             c__[i__] = cu[i__ - 1] / cnorm;
09658 /* L2: */
09659         }
09660 
09661 /* If the vertices are nearly identical, the problem is */
09662 /*   ill-conditioned and it is possible for the computed */
09663 /*   value of C to be 180 degrees off:  <C,V1> near -1 */
09664 /*   when it should be positive. */
09665 
09666         if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09667             c__[1] = -c__[1];
09668             c__[2] = -c__[2];
09669             c__[3] = -c__[3];
09670         }
09671         *ier = 0;
09672     } else {
09673 
09674 /*   CU = 0. */
09675 
09676         *ier = 1;
09677     }
09678     return 0;
09679 } /* circum_ */
09680 
09681 /* Subroutine */ int covsph_(int *kk, int *n0, int *list, int
09682         *lptr, int *lend, int *lnew)
09683 {
09684     static int k, lp, nst, lsav, next;
09685     extern /* Subroutine */ int insert_(int *, int *, int *,
09686             int *, int *);
09687 
09688 
09689 /* *********************************************************** */
09690 
09691 /*                                              From STRIPACK */
09692 /*                                            Robert J. Renka */
09693 /*                                  Dept. of Computer Science */
09694 /*                                       Univ. of North Texas */
09695 /*                                           renka@cs.unt.edu */
09696 /*                                                   07/17/96 */
09697 
09698 /*   This subroutine connects an exterior node KK to all */
09699 /* boundary nodes of a triangulation of KK-1 points on the */
09700 /* unit sphere, producing a triangulation that covers the */
09701 /* sphere.  The data structure is updated with the addition */
09702 /* of node KK, but no optimization is performed.  All boun- */
09703 /* dary nodes must be visible from node KK. */
09704 
09705 
09706 /* On input: */
09707 
09708 /*       KK = Index of the node to be connected to the set of */
09709 /*            all boundary nodes.  KK .GE. 4. */
09710 
09711 /*       N0 = Index of a boundary node (in the range 1 to */
09712 /*            KK-1).  N0 may be determined by Subroutine */
09713 /*            TRFIND. */
09714 
09715 /* The above parameters are not altered by this routine. */
09716 
09717 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09718 /*                             created by Subroutine TRMESH. */
09719 /*                             Node N0 must be included in */
09720 /*                             the triangulation. */
09721 
09722 /* On output: */
09723 
09724 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09725 /*                             the addition of node KK as the */
09726 /*                             last entry.  The updated */
09727 /*                             triangulation contains no */
09728 /*                             boundary nodes. */
09729 
09730 /* Module required by COVSPH:  INSERT */
09731 
09732 /* *********************************************************** */
09733 
09734 
09735 /* Local parameters: */
09736 
09737 /* K =     Local copy of KK */
09738 /* LP =    LIST pointer */
09739 /* LSAV =  LIST pointer */
09740 /* NEXT =  Boundary node visible from K */
09741 /* NST =   Local copy of N0 */
09742 
09743     /* Parameter adjustments */
09744     --lend;
09745     --lptr;
09746     --list;
09747 
09748     /* Function Body */
09749     k = *kk;
09750     nst = *n0;
09751 
09752 /* Traverse the boundary in clockwise order, inserting K as */
09753 /*   the first neighbor of each boundary node, and converting */
09754 /*   the boundary node to an interior node. */
09755 
09756     next = nst;
09757 L1:
09758     lp = lend[next];
09759     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09760     next = -list[lp];
09761     list[lp] = next;
09762     if (next != nst) {
09763         goto L1;
09764     }
09765 
09766 /* Traverse the boundary again, adding each node to K's */
09767 /*   adjacency list. */
09768 
09769     lsav = *lnew;
09770 L2:
09771     lp = lend[next];
09772     list[*lnew] = next;
09773     lptr[*lnew] = *lnew + 1;
09774     ++(*lnew);
09775     next = list[lp];
09776     if (next != nst) {
09777         goto L2;
09778     }
09779 
09780     lptr[*lnew - 1] = lsav;
09781     lend[k] = *lnew - 1;
09782     return 0;
09783 } /* covsph_ */
09784 
09785 /* Subroutine */ int crlist_(int *n, int *ncol, double *x,
09786         double *y, double *z__, int *list, int *lend, int
09787         *lptr, int *lnew, int *ltri, int *listc, int *nb,
09788         double *xc, double *yc, double *zc, double *rc,
09789         int *ier)
09790 {
09791     /* System generated locals */
09792     int i__1, i__2;
09793 
09794     /* Builtin functions */
09795     //double acos(double);
09796 
09797     /* Local variables */
09798     static double c__[3], t;
09799     static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09800     static double v1[3], v2[3], v3[3];
09801     static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09802              lpn;
09803     static long int swp;
09804     static int ierr;
09805     extern /* Subroutine */ int circum_(double *, double *,
09806             double *, double *, int *);
09807     extern int lstptr_(int *, int *, int *, int *);
09808     extern long int swptst_(int *, int *, int *, int *,
09809             double *, double *, double *);
09810 
09811 
09812 /* *********************************************************** */
09813 
09814 /*                                              From STRIPACK */
09815 /*                                            Robert J. Renka */
09816 /*                                  Dept. of Computer Science */
09817 /*                                       Univ. of North Texas */
09818 /*                                           renka@cs.unt.edu */
09819 /*                                                   03/05/03 */
09820 
09821 /*   Given a Delaunay triangulation of nodes on the surface */
09822 /* of the unit sphere, this subroutine returns the set of */
09823 /* triangle circumcenters corresponding to Voronoi vertices, */
09824 /* along with the circumradii and a list of triangle indexes */
09825 /* LISTC stored in one-to-one correspondence with LIST/LPTR */
09826 /* entries. */
09827 
09828 /*   A triangle circumcenter is the point (unit vector) lying */
09829 /* at the same angular distance from the three vertices and */
09830 /* contained in the same hemisphere as the vertices.  (Note */
09831 /* that the negative of a circumcenter is also equidistant */
09832 /* from the vertices.)  If the triangulation covers the sur- */
09833 /* face, the Voronoi vertices are the circumcenters of the */
09834 /* triangles in the Delaunay triangulation.  LPTR, LEND, and */
09835 /* LNEW are not altered in this case. */
09836 
09837 /*   On the other hand, if the nodes are contained in a sin- */
09838 /* gle hemisphere, the triangulation is implicitly extended */
09839 /* to the entire surface by adding pseudo-arcs (of length */
09840 /* greater than 180 degrees) between boundary nodes forming */
09841 /* pseudo-triangles whose 'circumcenters' are included in the */
09842 /* list.  This extension to the triangulation actually con- */
09843 /* sists of a triangulation of the set of boundary nodes in */
09844 /* which the swap test is reversed (a non-empty circumcircle */
09845 /* test).  The negative circumcenters are stored as the */
09846 /* pseudo-triangle 'circumcenters'.  LISTC, LPTR, LEND, and */
09847 /* LNEW contain a data structure corresponding to the ex- */
09848 /* tended triangulation (Voronoi diagram), but LIST is not */
09849 /* altered in this case.  Thus, if it is necessary to retain */
09850 /* the original (unextended) triangulation data structure, */
09851 /* copies of LPTR and LNEW must be saved before calling this */
09852 /* routine. */
09853 
09854 
09855 /* On input: */
09856 
09857 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09858 /*           Note that, if N = 3, there are only two Voronoi */
09859 /*           vertices separated by 180 degrees, and the */
09860 /*           Voronoi regions are not well defined. */
09861 
09862 /*       NCOL = Number of columns reserved for LTRI.  This */
09863 /*              must be at least NB-2, where NB is the number */
09864 /*              of boundary nodes. */
09865 
09866 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09867 /*               coordinates of the nodes (unit vectors). */
09868 
09869 /*       LIST = int array containing the set of adjacency */
09870 /*              lists.  Refer to Subroutine TRMESH. */
09871 
09872 /*       LEND = Set of pointers to ends of adjacency lists. */
09873 /*              Refer to Subroutine TRMESH. */
09874 
09875 /* The above parameters are not altered by this routine. */
09876 
09877 /*       LPTR = Array of pointers associated with LIST.  Re- */
09878 /*              fer to Subroutine TRMESH. */
09879 
09880 /*       LNEW = Pointer to the first empty location in LIST */
09881 /*              and LPTR (list length plus one). */
09882 
09883 /*       LTRI = int work space array dimensioned 6 by */
09884 /*              NCOL, or unused dummy parameter if NB = 0. */
09885 
09886 /*       LISTC = int array of length at least 3*NT, where */
09887 /*               NT = 2*N-4 is the number of triangles in the */
09888 /*               triangulation (after extending it to cover */
09889 /*               the entire surface if necessary). */
09890 
09891 /*       XC,YC,ZC,RC = Arrays of length NT = 2*N-4. */
09892 
09893 /* On output: */
09894 
09895 /*       LPTR = Array of pointers associated with LISTC: */
09896 /*              updated for the addition of pseudo-triangles */
09897 /*              if the original triangulation contains */
09898 /*              boundary nodes (NB > 0). */
09899 
09900 /*       LNEW = Pointer to the first empty location in LISTC */
09901 /*              and LPTR (list length plus one).  LNEW is not */
09902 /*              altered if NB = 0. */
09903 
09904 /*       LTRI = Triangle list whose first NB-2 columns con- */
09905 /*              tain the indexes of a clockwise-ordered */
09906 /*              sequence of vertices (first three rows) */
09907 /*              followed by the LTRI column indexes of the */
09908 /*              triangles opposite the vertices (or 0 */
09909 /*              denoting the exterior region) in the last */
09910 /*              three rows.  This array is not generally of */
09911 /*              any use. */
09912 
09913 /*       LISTC = Array containing triangle indexes (indexes */
09914 /*               to XC, YC, ZC, and RC) stored in 1-1 corres- */
09915 /*               pondence with LIST/LPTR entries (or entries */
09916 /*               that would be stored in LIST for the */
09917 /*               extended triangulation):  the index of tri- */
09918 /*               angle (N1,N2,N3) is stored in LISTC(K), */
09919 /*               LISTC(L), and LISTC(M), where LIST(K), */
09920 /*               LIST(L), and LIST(M) are the indexes of N2 */
09921 /*               as a neighbor of N1, N3 as a neighbor of N2, */
09922 /*               and N1 as a neighbor of N3.  The Voronoi */
09923 /*               region associated with a node is defined by */
09924 /*               the CCW-ordered sequence of circumcenters in */
09925 /*               one-to-one correspondence with its adjacency */
09926 /*               list (in the extended triangulation). */
09927 
09928 /*       NB = Number of boundary nodes unless IER = 1. */
09929 
09930 /*       XC,YC,ZC = Arrays containing the Cartesian coordi- */
09931 /*                  nates of the triangle circumcenters */
09932 /*                  (Voronoi vertices).  XC(I)**2 + YC(I)**2 */
09933 /*                  + ZC(I)**2 = 1.  The first NB-2 entries */
09934 /*                  correspond to pseudo-triangles if NB > 0. */
09935 
09936 /*       RC = Array containing circumradii (the arc lengths */
09937 /*            or angles between the circumcenters and associ- */
09938 /*            ated triangle vertices) in 1-1 correspondence */
09939 /*            with circumcenters. */
09940 
09941 /*       IER = Error indicator: */
09942 /*             IER = 0 if no errors were encountered. */
09943 /*             IER = 1 if N < 3. */
09944 /*             IER = 2 if NCOL < NB-2. */
09945 /*             IER = 3 if a triangle is degenerate (has ver- */
09946 /*                     tices lying on a common geodesic). */
09947 
09948 /* Modules required by CRLIST:  CIRCUM, LSTPTR, SWPTST */
09949 
09950 /* Intrinsic functions called by CRLIST:  ABS, ACOS */
09951 
09952 /* *********************************************************** */
09953 
09954 
09955 /* Local parameters: */
09956 
09957 /* C =         Circumcenter returned by Subroutine CIRCUM */
09958 /* I1,I2,I3 =  Permutation of (1,2,3):  LTRI row indexes */
09959 /* I4 =        LTRI row index in the range 1 to 3 */
09960 /* IERR =      Error flag for calls to CIRCUM */
09961 /* KT =        Triangle index */
09962 /* KT1,KT2 =   Indexes of a pair of adjacent pseudo-triangles */
09963 /* KT11,KT12 = Indexes of the pseudo-triangles opposite N1 */
09964 /*               and N2 as vertices of KT1 */
09965 /* KT21,KT22 = Indexes of the pseudo-triangles opposite N1 */
09966 /*               and N2 as vertices of KT2 */
09967 /* LP,LPN =    LIST pointers */
09968 /* LPL =       LIST pointer of the last neighbor of N1 */
09969 /* N0 =        Index of the first boundary node (initial */
09970 /*               value of N1) in the loop on boundary nodes */
09971 /*               used to store the pseudo-triangle indexes */
09972 /*               in LISTC */
09973 /* N1,N2,N3 =  Nodal indexes defining a triangle (CCW order) */
09974 /*               or pseudo-triangle (clockwise order) */
09975 /* N4 =        Index of the node opposite N2 -> N1 */
09976 /* NM2 =       N-2 */
09977 /* NN =        Local copy of N */
09978 /* NT =        Number of pseudo-triangles:  NB-2 */
09979 /* SWP =       long int variable set to TRUE in each optimiza- */
09980 /*               tion loop (loop on pseudo-arcs) iff a swap */
09981 /*               is performed */
09982 /* V1,V2,V3 =  Vertices of triangle KT = (N1,N2,N3) sent to */
09983 /*               Subroutine CIRCUM */
09984 
09985     /* Parameter adjustments */
09986     --lend;
09987     --z__;
09988     --y;
09989     --x;
09990     ltri -= 7;
09991     --list;
09992     --lptr;
09993     --listc;
09994     --xc;
09995     --yc;
09996     --zc;
09997     --rc;
09998 
09999     /* Function Body */
10000     nn = *n;
10001     *nb = 0;
10002     nt = 0;
10003     if (nn < 3) {
10004         goto L21;
10005     }
10006 
10007 /* Search for a boundary node N1. */
10008 
10009     i__1 = nn;
10010     for (n1 = 1; n1 <= i__1; ++n1) {
10011         lp = lend[n1];
10012         if (list[lp] < 0) {
10013             goto L2;
10014         }
10015 /* L1: */
10016     }
10017 
10018 /* The triangulation already covers the sphere. */
10019 
10020     goto L9;
10021 
10022 /* There are NB .GE. 3 boundary nodes.  Add NB-2 pseudo- */
10023 /*   triangles (N1,N2,N3) by connecting N3 to the NB-3 */
10024 /*   boundary nodes to which it is not already adjacent. */
10025 
10026 /*   Set N3 and N2 to the first and last neighbors, */
10027 /*     respectively, of N1. */
10028 
10029 L2:
10030     n2 = -list[lp];
10031     lp = lptr[lp];
10032     n3 = list[lp];
10033 
10034 /*   Loop on boundary arcs N1 -> N2 in clockwise order, */
10035 /*     storing triangles (N1,N2,N3) in column NT of LTRI */
10036 /*     along with the indexes of the triangles opposite */
10037 /*     the vertices. */
10038 
10039 L3:
10040     ++nt;
10041     if (nt <= *ncol) {
10042         ltri[nt * 6 + 1] = n1;
10043         ltri[nt * 6 + 2] = n2;
10044         ltri[nt * 6 + 3] = n3;
10045         ltri[nt * 6 + 4] = nt + 1;
10046         ltri[nt * 6 + 5] = nt - 1;
10047         ltri[nt * 6 + 6] = 0;
10048     }
10049     n1 = n2;
10050     lp = lend[n1];
10051     n2 = -list[lp];
10052     if (n2 != n3) {
10053         goto L3;
10054     }
10055 
10056     *nb = nt + 2;
10057     if (*ncol < nt) {
10058         goto L22;
10059     }
10060     ltri[nt * 6 + 4] = 0;
10061     if (nt == 1) {
10062         goto L7;
10063     }
10064 
10065 /* Optimize the exterior triangulation (set of pseudo- */
10066 /*   triangles) by applying swaps to the pseudo-arcs N1-N2 */
10067 /*   (pairs of adjacent pseudo-triangles KT1 and KT2 > KT1). */
10068 /*   The loop on pseudo-arcs is repeated until no swaps are */
10069 /*   performed. */
10070 
10071 L4:
10072     swp = FALSE_;
10073     i__1 = nt - 1;
10074     for (kt1 = 1; kt1 <= i__1; ++kt1) {
10075         for (i3 = 1; i3 <= 3; ++i3) {
10076             kt2 = ltri[i3 + 3 + kt1 * 6];
10077             if (kt2 <= kt1) {
10078                 goto L5;
10079             }
10080 
10081 /*   The LTRI row indexes (I1,I2,I3) of triangle KT1 = */
10082 /*     (N1,N2,N3) are a cyclical permutation of (1,2,3). */
10083 
10084             if (i3 == 1) {
10085                 i1 = 2;
10086                 i2 = 3;
10087             } else if (i3 == 2) {
10088                 i1 = 3;
10089                 i2 = 1;
10090             } else {
10091                 i1 = 1;
10092                 i2 = 2;
10093             }
10094             n1 = ltri[i1 + kt1 * 6];
10095             n2 = ltri[i2 + kt1 * 6];
10096             n3 = ltri[i3 + kt1 * 6];
10097 
10098 /*   KT2 = (N2,N1,N4) for N4 = LTRI(I,KT2), where */
10099 /*     LTRI(I+3,KT2) = KT1. */
10100 
10101             if (ltri[kt2 * 6 + 4] == kt1) {
10102                 i4 = 1;
10103             } else if (ltri[kt2 * 6 + 5] == kt1) {
10104                 i4 = 2;
10105             } else {
10106                 i4 = 3;
10107             }
10108             n4 = ltri[i4 + kt2 * 6];
10109 
10110 /*   The empty circumcircle test is reversed for the pseudo- */
10111 /*     triangles.  The reversal is implicit in the clockwise */
10112 /*     ordering of the vertices. */
10113 
10114             if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10115                 goto L5;
10116             }
10117 
10118 /*   Swap arc N1-N2 for N3-N4.  KTij is the triangle opposite */
10119 /*     Nj as a vertex of KTi. */
10120 
10121             swp = TRUE_;
10122             kt11 = ltri[i1 + 3 + kt1 * 6];
10123             kt12 = ltri[i2 + 3 + kt1 * 6];
10124             if (i4 == 1) {
10125                 i2 = 2;
10126                 i1 = 3;
10127             } else if (i4 == 2) {
10128                 i2 = 3;
10129                 i1 = 1;
10130             } else {
10131                 i2 = 1;
10132                 i1 = 2;
10133             }
10134             kt21 = ltri[i1 + 3 + kt2 * 6];
10135             kt22 = ltri[i2 + 3 + kt2 * 6];
10136             ltri[kt1 * 6 + 1] = n4;
10137             ltri[kt1 * 6 + 2] = n3;
10138             ltri[kt1 * 6 + 3] = n1;
10139             ltri[kt1 * 6 + 4] = kt12;
10140             ltri[kt1 * 6 + 5] = kt22;
10141             ltri[kt1 * 6 + 6] = kt2;
10142             ltri[kt2 * 6 + 1] = n3;
10143             ltri[kt2 * 6 + 2] = n4;
10144             ltri[kt2 * 6 + 3] = n2;
10145             ltri[kt2 * 6 + 4] = kt21;
10146             ltri[kt2 * 6 + 5] = kt11;
10147             ltri[kt2 * 6 + 6] = kt1;
10148 
10149 /*   Correct the KT11 and KT22 entries that changed. */
10150 
10151             if (kt11 != 0) {
10152                 i4 = 4;
10153                 if (ltri[kt11 * 6 + 4] != kt1) {
10154                     i4 = 5;
10155                     if (ltri[kt11 * 6 + 5] != kt1) {
10156                         i4 = 6;
10157                     }
10158                 }
10159                 ltri[i4 + kt11 * 6] = kt2;
10160             }
10161             if (kt22 != 0) {
10162                 i4 = 4;
10163                 if (ltri[kt22 * 6 + 4] != kt2) {
10164                     i4 = 5;
10165                     if (ltri[kt22 * 6 + 5] != kt2) {
10166                         i4 = 6;
10167                     }
10168                 }
10169                 ltri[i4 + kt22 * 6] = kt1;
10170             }
10171 L5:
10172             ;
10173         }
10174 /* L6: */
10175     }
10176     if (swp) {
10177         goto L4;
10178     }
10179 
10180 /* Compute and store the negative circumcenters and radii of */
10181 /*   the pseudo-triangles in the first NT positions. */
10182 
10183 L7:
10184     i__1 = nt;
10185     for (kt = 1; kt <= i__1; ++kt) {
10186         n1 = ltri[kt * 6 + 1];
10187         n2 = ltri[kt * 6 + 2];
10188         n3 = ltri[kt * 6 + 3];
10189         v1[0] = x[n1];
10190         v1[1] = y[n1];
10191         v1[2] = z__[n1];
10192         v2[0] = x[n2];
10193         v2[1] = y[n2];
10194         v2[2] = z__[n2];
10195         v3[0] = x[n3];
10196         v3[1] = y[n3];
10197         v3[2] = z__[n3];
10198         circum_(v2, v1, v3, c__, &ierr);
10199         if (ierr != 0) {
10200             goto L23;
10201         }
10202 
10203 /*   Store the negative circumcenter and radius (computed */
10204 /*     from <V1,C>). */
10205 
10206         xc[kt] = -c__[0];
10207         yc[kt] = -c__[1];
10208         zc[kt] = -c__[2];
10209         t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10210         if (t < -1.) {
10211             t = -1.;
10212         }
10213         if (t > 1.) {
10214             t = 1.;
10215         }
10216         rc[kt] = acos(t);
10217 /* L8: */
10218     }
10219 
10220 /* Compute and store the circumcenters and radii of the */
10221 /*   actual triangles in positions KT = NT+1, NT+2, ... */
10222 /*   Also, store the triangle indexes KT in the appropriate */
10223 /*   LISTC positions. */
10224 
10225 L9:
10226     kt = nt;
10227 
10228 /*   Loop on nodes N1. */
10229 
10230     nm2 = nn - 2;
10231     i__1 = nm2;
10232     for (n1 = 1; n1 <= i__1; ++n1) {
10233         lpl = lend[n1];
10234         lp = lpl;
10235         n3 = list[lp];
10236 
10237 /*   Loop on adjacent neighbors N2,N3 of N1 for which N2 > N1 */
10238 /*     and N3 > N1. */
10239 
10240 L10:
10241         lp = lptr[lp];
10242         n2 = n3;
10243         n3 = (i__2 = list[lp], abs(i__2));
10244         if (n2 <= n1 || n3 <= n1) {
10245             goto L11;
10246         }
10247         ++kt;
10248 
10249 /*   Compute the circumcenter C of triangle KT = (N1,N2,N3). */
10250 
10251         v1[0] = x[n1];
10252         v1[1] = y[n1];
10253         v1[2] = z__[n1];
10254         v2[0] = x[n2];
10255         v2[1] = y[n2];
10256         v2[2] = z__[n2];
10257         v3[0] = x[n3];
10258         v3[1] = y[n3];
10259         v3[2] = z__[n3];
10260         circum_(v1, v2, v3, c__, &ierr);
10261         if (ierr != 0) {
10262             goto L23;
10263         }
10264 
10265 /*   Store the circumcenter, radius and triangle index. */
10266 
10267         xc[kt] = c__[0];
10268         yc[kt] = c__[1];
10269         zc[kt] = c__[2];
10270         t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10271         if (t < -1.) {
10272             t = -1.;
10273         }
10274         if (t > 1.) {
10275             t = 1.;
10276         }
10277         rc[kt] = acos(t);
10278 
10279 /*   Store KT in LISTC(LPN), where Abs(LIST(LPN)) is the */
10280 /*     index of N2 as a neighbor of N1, N3 as a neighbor */
10281 /*     of N2, and N1 as a neighbor of N3. */
10282 
10283         lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10284         listc[lpn] = kt;
10285         lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10286         listc[lpn] = kt;
10287         lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10288         listc[lpn] = kt;
10289 L11:
10290         if (lp != lpl) {
10291             goto L10;
10292         }
10293 /* L12: */
10294     }
10295     if (nt == 0) {
10296         goto L20;
10297     }
10298 
10299 /* Store the first NT triangle indexes in LISTC. */
10300 
10301 /*   Find a boundary triangle KT1 = (N1,N2,N3) with a */
10302 /*     boundary arc opposite N3. */
10303 
10304     kt1 = 0;
10305 L13:
10306     ++kt1;
10307     if (ltri[kt1 * 6 + 4] == 0) {
10308         i1 = 2;
10309         i2 = 3;
10310         i3 = 1;
10311         goto L14;
10312     } else if (ltri[kt1 * 6 + 5] == 0) {
10313         i1 = 3;
10314         i2 = 1;
10315         i3 = 2;
10316         goto L14;
10317     } else if (ltri[kt1 * 6 + 6] == 0) {
10318         i1 = 1;
10319         i2 = 2;
10320         i3 = 3;
10321         goto L14;
10322     }
10323     goto L13;
10324 L14:
10325     n1 = ltri[i1 + kt1 * 6];
10326     n0 = n1;
10327 
10328 /*   Loop on boundary nodes N1 in CCW order, storing the */
10329 /*     indexes of the clockwise-ordered sequence of triangles */
10330 /*     that contain N1.  The first triangle overwrites the */
10331 /*     last neighbor position, and the remaining triangles, */
10332 /*     if any, are appended to N1's adjacency list. */
10333 
10334 /*   A pointer to the first neighbor of N1 is saved in LPN. */
10335 
10336 L15:
10337     lp = lend[n1];
10338     lpn = lptr[lp];
10339     listc[lp] = kt1;
10340 
10341 /*   Loop on triangles KT2 containing N1. */
10342 
10343 L16:
10344     kt2 = ltri[i2 + 3 + kt1 * 6];
10345     if (kt2 != 0) {
10346 
10347 /*   Append KT2 to N1's triangle list. */
10348 
10349         lptr[lp] = *lnew;
10350         lp = *lnew;
10351         listc[lp] = kt2;
10352         ++(*lnew);
10353 
10354 /*   Set KT1 to KT2 and update (I1,I2,I3) such that */
10355 /*     LTRI(I1,KT1) = N1. */
10356 
10357         kt1 = kt2;
10358         if (ltri[kt1 * 6 + 1] == n1) {
10359             i1 = 1;
10360             i2 = 2;
10361             i3 = 3;
10362         } else if (ltri[kt1 * 6 + 2] == n1) {
10363             i1 = 2;
10364             i2 = 3;
10365             i3 = 1;
10366         } else {
10367             i1 = 3;
10368             i2 = 1;
10369             i3 = 2;
10370         }
10371         goto L16;
10372     }
10373 
10374 /*   Store the saved first-triangle pointer in LPTR(LP), set */
10375 /*     N1 to the next boundary node, test for termination, */
10376 /*     and permute the indexes:  the last triangle containing */
10377 /*     a boundary node is the first triangle containing the */
10378 /*     next boundary node. */
10379 
10380     lptr[lp] = lpn;
10381     n1 = ltri[i3 + kt1 * 6];
10382     if (n1 != n0) {
10383         i4 = i3;
10384         i3 = i2;
10385         i2 = i1;
10386         i1 = i4;
10387         goto L15;
10388     }
10389 
10390 /* No errors encountered. */
10391 
10392 L20:
10393     *ier = 0;
10394     return 0;
10395 
10396 /* N < 3. */
10397 
10398 L21:
10399     *ier = 1;
10400     return 0;
10401 
10402 /* Insufficient space reserved for LTRI. */
10403 
10404 L22:
10405     *ier = 2;
10406     return 0;
10407 
10408 /* Error flag returned by CIRCUM: KT indexes a null triangle. */
10409 
10410 L23:
10411     *ier = 3;
10412     return 0;
10413 } /* crlist_ */
10414 
10415 /* Subroutine */ int delarc_(int *n, int *io1, int *io2, int *
10416         list, int *lptr, int *lend, int *lnew, int *ier)
10417 {
10418     /* System generated locals */
10419     int i__1;
10420 
10421     /* Local variables */
10422     static int n1, n2, n3, lp, lph, lpl;
10423     extern /* Subroutine */ int delnb_(int *, int *, int *,
10424             int *, int *, int *, int *, int *);
10425     extern int lstptr_(int *, int *, int *, int *);
10426 
10427 
10428 /* *********************************************************** */
10429 
10430 /*                                              From STRIPACK */
10431 /*                                            Robert J. Renka */
10432 /*                                  Dept. of Computer Science */
10433 /*                                       Univ. of North Texas */
10434 /*                                           renka@cs.unt.edu */
10435 /*                                                   07/17/96 */
10436 
10437 /*   This subroutine deletes a boundary arc from a triangula- */
10438 /* tion.  It may be used to remove a null triangle from the */
10439 /* convex hull boundary.  Note, however, that if the union of */
10440 /* triangles is rendered nonconvex, Subroutines DELNOD, EDGE, */
10441 /* and TRFIND (and hence ADDNOD) may fail.  Also, Function */
10442 /* NEARND should not be called following an arc deletion. */
10443 
10444 /*   This routine is identical to the similarly named routine */
10445 /* in TRIPACK. */
10446 
10447 
10448 /* On input: */
10449 
10450 /*       N = Number of nodes in the triangulation.  N .GE. 4. */
10451 
10452 /*       IO1,IO2 = Indexes (in the range 1 to N) of a pair of */
10453 /*                 adjacent boundary nodes defining the arc */
10454 /*                 to be removed. */
10455 
10456 /* The above parameters are not altered by this routine. */
10457 
10458 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
10459 /*                             created by Subroutine TRMESH. */
10460 
10461 /* On output: */
10462 
10463 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10464 /*                             the removal of arc IO1-IO2 */
10465 /*                             unless IER > 0. */
10466 
10467 /*       IER = Error indicator: */
10468 /*             IER = 0 if no errors were encountered. */
10469 /*             IER = 1 if N, IO1, or IO2 is outside its valid */
10470 /*                     range, or IO1 = IO2. */
10471 /*             IER = 2 if IO1-IO2 is not a boundary arc. */
10472 /*             IER = 3 if the node opposite IO1-IO2 is al- */
10473 /*                     ready a boundary node, and thus IO1 */
10474 /*                     or IO2 has only two neighbors or a */
10475 /*                     deletion would result in two triangu- */
10476 /*                     lations sharing a single node. */
10477 /*             IER = 4 if one of the nodes is a neighbor of */
10478 /*                     the other, but not vice versa, imply- */
10479 /*                     ing an invalid triangulation data */
10480 /*                     structure. */
10481 
10482 /* Module required by DELARC:  DELNB, LSTPTR */
10483 
10484 /* Intrinsic function called by DELARC:  ABS */
10485 
10486 /* *********************************************************** */
10487 
10488 
10489 /* Local parameters: */
10490 
10491 /* LP =       LIST pointer */
10492 /* LPH =      LIST pointer or flag returned by DELNB */
10493 /* LPL =      Pointer to the last neighbor of N1, N2, or N3 */
10494 /* N1,N2,N3 = Nodal indexes of a triangle such that N1->N2 */
10495 /*              is the directed boundary edge associated */
10496 /*              with IO1-IO2 */
10497 
10498     /* Parameter adjustments */
10499     --lend;
10500     --list;
10501     --lptr;
10502 
10503     /* Function Body */
10504     n1 = *io1;
10505     n2 = *io2;
10506 
10507 /* Test for errors, and set N1->N2 to the directed boundary */
10508 /*   edge associated with IO1-IO2:  (N1,N2,N3) is a triangle */
10509 /*   for some N3. */
10510 
10511     if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10512         *ier = 1;
10513         return 0;
10514     }
10515 
10516     lpl = lend[n2];
10517     if (-list[lpl] != n1) {
10518         n1 = n2;
10519         n2 = *io1;
10520         lpl = lend[n2];
10521         if (-list[lpl] != n1) {
10522             *ier = 2;
10523             return 0;
10524         }
10525     }
10526 
10527 /* Set N3 to the node opposite N1->N2 (the second neighbor */
10528 /*   of N1), and test for error 3 (N3 already a boundary */
10529 /*   node). */
10530 
10531     lpl = lend[n1];
10532     lp = lptr[lpl];
10533     lp = lptr[lp];
10534     n3 = (i__1 = list[lp], abs(i__1));
10535     lpl = lend[n3];
10536     if (list[lpl] <= 0) {
10537         *ier = 3;
10538         return 0;
10539     }
10540 
10541 /* Delete N2 as a neighbor of N1, making N3 the first */
10542 /*   neighbor, and test for error 4 (N2 not a neighbor */
10543 /*   of N1).  Note that previously computed pointers may */
10544 /*   no longer be valid following the call to DELNB. */
10545 
10546     delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10547     if (lph < 0) {
10548         *ier = 4;
10549         return 0;
10550     }
10551 
10552 /* Delete N1 as a neighbor of N2, making N3 the new last */
10553 /*   neighbor. */
10554 
10555     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10556 
10557 /* Make N3 a boundary node with first neighbor N2 and last */
10558 /*   neighbor N1. */
10559 
10560     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10561     lend[n3] = lp;
10562     list[lp] = -n1;
10563 
10564 /* No errors encountered. */
10565 
10566     *ier = 0;
10567     return 0;
10568 } /* delarc_ */
10569 
10570 /* Subroutine */ int delnb_(int *n0, int *nb, int *n, int *
10571         list, int *lptr, int *lend, int *lnew, int *lph)
10572 {
10573     /* System generated locals */
10574     int i__1;
10575 
10576     /* Local variables */
10577     static int i__, lp, nn, lpb, lpl, lpp, lnw;
10578 
10579 
10580 /* *********************************************************** */
10581 
10582 /*                                              From STRIPACK */
10583 /*                                            Robert J. Renka */
10584 /*                                  Dept. of Computer Science */
10585 /*                                       Univ. of North Texas */
10586 /*                                           renka@cs.unt.edu */
10587 /*                                                   07/29/98 */
10588 
10589 /*   This subroutine deletes a neighbor NB from the adjacency */
10590 /* list of node N0 (but N0 is not deleted from the adjacency */
10591 /* list of NB) and, if NB is a boundary node, makes N0 a */
10592 /* boundary node.  For pointer (LIST index) LPH to NB as a */
10593 /* neighbor of N0, the empty LIST,LPTR location LPH is filled */
10594 /* in with the values at LNEW-1, pointer LNEW-1 (in LPTR and */
10595 /* possibly in LEND) is changed to LPH, and LNEW is decremen- */
10596 /* ted.  This requires a search of LEND and LPTR entailing an */
10597 /* expected operation count of O(N). */
10598 
10599 /*   This routine is identical to the similarly named routine */
10600 /* in TRIPACK. */
10601 
10602 
10603 /* On input: */
10604 
10605 /*       N0,NB = Indexes, in the range 1 to N, of a pair of */
10606 /*               nodes such that NB is a neighbor of N0. */
10607 /*               (N0 need not be a neighbor of NB.) */
10608 
10609 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
10610 
10611 /* The above parameters are not altered by this routine. */
10612 
10613 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10614 /*                             triangulation. */
10615 
10616 /* On output: */
10617 
10618 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10619 /*                             the removal of NB from the ad- */
10620 /*                             jacency list of N0 unless */
10621 /*                             LPH < 0. */
10622 
10623 /*       LPH = List pointer to the hole (NB as a neighbor of */
10624 /*             N0) filled in by the values at LNEW-1 or error */
10625 /*             indicator: */
10626 /*             LPH > 0 if no errors were encountered. */
10627 /*             LPH = -1 if N0, NB, or N is outside its valid */
10628 /*                      range. */
10629 /*             LPH = -2 if NB is not a neighbor of N0. */
10630 
10631 /* Modules required by DELNB:  None */
10632 
10633 /* Intrinsic function called by DELNB:  ABS */
10634 
10635 /* *********************************************************** */
10636 
10637 
10638 /* Local parameters: */
10639 
10640 /* I =   DO-loop index */
10641 /* LNW = LNEW-1 (output value of LNEW) */
10642 /* LP =  LIST pointer of the last neighbor of NB */
10643 /* LPB = Pointer to NB as a neighbor of N0 */
10644 /* LPL = Pointer to the last neighbor of N0 */
10645 /* LPP = Pointer to the neighbor of N0 that precedes NB */
10646 /* NN =  Local copy of N */
10647 
10648     /* Parameter adjustments */
10649     --lend;
10650     --list;
10651     --lptr;
10652 
10653     /* Function Body */
10654     nn = *n;
10655 
10656 /* Test for error 1. */
10657 
10658     if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10659         *lph = -1;
10660         return 0;
10661     }
10662 
10663 /*   Find pointers to neighbors of N0: */
10664 
10665 /*     LPL points to the last neighbor, */
10666 /*     LPP points to the neighbor NP preceding NB, and */
10667 /*     LPB points to NB. */
10668 
10669     lpl = lend[*n0];
10670     lpp = lpl;
10671     lpb = lptr[lpp];
10672 L1:
10673     if (list[lpb] == *nb) {
10674         goto L2;
10675     }
10676     lpp = lpb;
10677     lpb = lptr[lpp];
10678     if (lpb != lpl) {
10679         goto L1;
10680     }
10681 
10682 /*   Test for error 2 (NB not found). */
10683 
10684     if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10685         *lph = -2;
10686         return 0;
10687     }
10688 
10689 /*   NB is the last neighbor of N0.  Make NP the new last */
10690 /*     neighbor and, if NB is a boundary node, then make N0 */
10691 /*     a boundary node. */
10692 
10693     lend[*n0] = lpp;
10694     lp = lend[*nb];
10695     if (list[lp] < 0) {
10696         list[lpp] = -list[lpp];
10697     }
10698     goto L3;
10699 
10700 /*   NB is not the last neighbor of N0.  If NB is a boundary */
10701 /*     node and N0 is not, then make N0 a boundary node with */
10702 /*     last neighbor NP. */
10703 
10704 L2:
10705     lp = lend[*nb];
10706     if (list[lp] < 0 && list[lpl] > 0) {
10707         lend[*n0] = lpp;
10708         list[lpp] = -list[lpp];
10709     }
10710 
10711 /*   Update LPTR so that the neighbor following NB now fol- */
10712 /*     lows NP, and fill in the hole at location LPB. */
10713 
10714 L3:
10715     lptr[lpp] = lptr[lpb];
10716     lnw = *lnew - 1;
10717     list[lpb] = list[lnw];
10718     lptr[lpb] = lptr[lnw];
10719     for (i__ = nn; i__ >= 1; --i__) {
10720         if (lend[i__] == lnw) {
10721             lend[i__] = lpb;
10722             goto L5;
10723         }
10724 /* L4: */
10725     }
10726 
10727 L5:
10728     i__1 = lnw - 1;
10729     for (i__ = 1; i__ <= i__1; ++i__) {
10730         if (lptr[i__] == lnw) {
10731             lptr[i__] = lpb;
10732         }
10733 /* L6: */
10734     }
10735 
10736 /* No errors encountered. */
10737 
10738     *lnew = lnw;
10739     *lph = lpb;
10740     return 0;
10741 } /* delnb_ */
10742 
10743 /* Subroutine */ int delnod_(int *k, int *n, double *x,
10744         double *y, double *z__, int *list, int *lptr, int
10745         *lend, int *lnew, int *lwk, int *iwk, int *ier)
10746 {
10747     /* System generated locals */
10748     int i__1;
10749 
10750     /* Local variables */
10751     static int i__, j, n1, n2;
10752     static double x1, x2, y1, y2, z1, z2;
10753     static int nl, lp, nn, nr;
10754     static double xl, yl, zl, xr, yr, zr;
10755     static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10756     extern long int left_(double *, double *, double *, double
10757             *, double *, double *, double *, double *,
10758             double *);
10759     static long int bdry;
10760     static int ierr, lwkl;
10761     extern /* Subroutine */ int swap_(int *, int *, int *,
10762             int *, int *, int *, int *, int *), delnb_(
10763             int *, int *, int *, int *, int *, int *,
10764             int *, int *);
10765     extern int nbcnt_(int *, int *);
10766     extern /* Subroutine */ int optim_(double *, double *, double
10767             *, int *, int *, int *, int *, int *, int
10768             *, int *);
10769     static int nfrst;
10770     extern int lstptr_(int *, int *, int *, int *);
10771 
10772 
10773 /* *********************************************************** */
10774 
10775 /*                                              From STRIPACK */
10776 /*                                            Robert J. Renka */
10777 /*                                  Dept. of Computer Science */
10778 /*                                       Univ. of North Texas */
10779 /*                                           renka@cs.unt.edu */
10780 /*                                                   11/30/99 */
10781 
10782 /*   This subroutine deletes node K (along with all arcs */
10783 /* incident on node K) from a triangulation of N nodes on the */
10784 /* unit sphere, and inserts arcs as necessary to produce a */
10785 /* triangulation of the remaining N-1 nodes.  If a Delaunay */
10786 /* triangulation is input, a Delaunay triangulation will */
10787 /* result, and thus, DELNOD reverses the effect of a call to */
10788 /* Subroutine ADDNOD. */
10789 
10790 
10791 /* On input: */
10792 
10793 /*       K = Index (for X, Y, and Z) of the node to be */
10794 /*           deleted.  1 .LE. K .LE. N. */
10795 
10796 /* K is not altered by this routine. */
10797 
10798 /*       N = Number of nodes in the triangulation on input. */
10799 /*           N .GE. 4.  Note that N will be decremented */
10800 /*           following the deletion. */
10801 
10802 /*       X,Y,Z = Arrays of length N containing the Cartesian */
10803 /*               coordinates of the nodes in the triangula- */
10804 /*               tion. */
10805 
10806 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10807 /*                             triangulation.  Refer to Sub- */
10808 /*                             routine TRMESH. */
10809 
10810 /*       LWK = Number of columns reserved for IWK.  LWK must */
10811 /*             be at least NNB-3, where NNB is the number of */
10812 /*             neighbors of node K, including an extra */
10813 /*             pseudo-node if K is a boundary node. */
10814 
10815 /*       IWK = int work array dimensioned 2 by LWK (or */
10816 /*             array of length .GE. 2*LWK). */
10817 
10818 /* On output: */
10819 
10820 /*       N = Number of nodes in the triangulation on output. */
10821 /*           The input value is decremented unless 1 .LE. IER */
10822 /*           .LE. 4. */
10823 
10824 /*       X,Y,Z = Updated arrays containing nodal coordinates */
10825 /*               (with elements K+1,...,N+1 shifted up one */
10826 /*               position, thus overwriting element K) unless */
10827 /*               1 .LE. IER .LE. 4. */
10828 
10829 /*       LIST,LPTR,LEND,LNEW = Updated triangulation data */
10830 /*                             structure reflecting the dele- */
10831 /*                             tion unless 1 .LE. IER .LE. 4. */
10832 /*                             Note that the data structure */
10833 /*                             may have been altered if IER > */
10834 /*                             3. */
10835 
10836 /*       LWK = Number of IWK columns required unless IER = 1 */
10837 /*             or IER = 3. */
10838 
10839 /*       IWK = Indexes of the endpoints of the new arcs added */
10840 /*             unless LWK = 0 or 1 .LE. IER .LE. 4.  (Arcs */
10841 /*             are associated with columns, or pairs of */
10842 /*             adjacent elements if IWK is declared as a */
10843 /*             singly-subscripted array.) */
10844 
10845 /*       IER = Error indicator: */
10846 /*             IER = 0 if no errors were encountered. */
10847 /*             IER = 1 if K or N is outside its valid range */
10848 /*                     or LWK < 0 on input. */
10849 /*             IER = 2 if more space is required in IWK. */
10850 /*                     Refer to LWK. */
10851 /*             IER = 3 if the triangulation data structure is */
10852 /*                     invalid on input. */
10853 /*             IER = 4 if K indexes an interior node with */
10854 /*                     four or more neighbors, none of which */
10855 /*                     can be swapped out due to collineari- */
10856 /*                     ty, and K cannot therefore be deleted. */
10857 /*             IER = 5 if an error flag (other than IER = 1) */
10858 /*                     was returned by OPTIM.  An error */
10859 /*                     message is written to the standard */
10860 /*                     output unit in this case. */
10861 /*             IER = 6 if error flag 1 was returned by OPTIM. */
10862 /*                     This is not necessarily an error, but */
10863 /*                     the arcs may not be optimal. */
10864 
10865 /*   Note that the deletion may result in all remaining nodes */
10866 /* being collinear.  This situation is not flagged. */
10867 
10868 /* Modules required by DELNOD:  DELNB, LEFT, LSTPTR, NBCNT, */
10869 /*                                OPTIM, SWAP, SWPTST */
10870 
10871 /* Intrinsic function called by DELNOD:  ABS */
10872 
10873 /* *********************************************************** */
10874 
10875 
10876 /* Local parameters: */
10877 
10878 /* BDRY =    long int variable with value TRUE iff N1 is a */
10879 /*             boundary node */
10880 /* I,J =     DO-loop indexes */
10881 /* IERR =    Error flag returned by OPTIM */
10882 /* IWL =     Number of IWK columns containing arcs */
10883 /* LNW =     Local copy of LNEW */
10884 /* LP =      LIST pointer */
10885 /* LP21 =    LIST pointer returned by SWAP */
10886 /* LPF,LPL = Pointers to the first and last neighbors of N1 */
10887 /* LPH =     Pointer (or flag) returned by DELNB */
10888 /* LPL2 =    Pointer to the last neighbor of N2 */
10889 /* LPN =     Pointer to a neighbor of N1 */
10890 /* LWKL =    Input value of LWK */
10891 /* N1 =      Local copy of K */
10892 /* N2 =      Neighbor of N1 */
10893 /* NFRST =   First neighbor of N1:  LIST(LPF) */
10894 /* NIT =     Number of iterations in OPTIM */
10895 /* NR,NL =   Neighbors of N1 preceding (to the right of) and */
10896 /*             following (to the left of) N2, respectively */
10897 /* NN =      Number of nodes in the triangulation */
10898 /* NNB =     Number of neighbors of N1 (including a pseudo- */
10899 /*             node representing the boundary if N1 is a */
10900 /*             boundary node) */
10901 /* X1,Y1,Z1 = Coordinates of N1 */
10902 /* X2,Y2,Z2 = Coordinates of N2 */
10903 /* XL,YL,ZL = Coordinates of NL */
10904 /* XR,YR,ZR = Coordinates of NR */
10905 
10906 
10907 /* Set N1 to K and NNB to the number of neighbors of N1 (plus */
10908 /*   one if N1 is a boundary node), and test for errors.  LPF */
10909 /*   and LPL are LIST indexes of the first and last neighbors */
10910 /*   of N1, IWL is the number of IWK columns containing arcs, */
10911 /*   and BDRY is TRUE iff N1 is a boundary node. */
10912 
10913     /* Parameter adjustments */
10914     iwk -= 3;
10915     --lend;
10916     --lptr;
10917     --list;
10918     --z__;
10919     --y;
10920     --x;
10921 
10922     /* Function Body */
10923     n1 = *k;
10924     nn = *n;
10925     if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
10926         goto L21;
10927     }
10928     lpl = lend[n1];
10929     lpf = lptr[lpl];
10930     nnb = nbcnt_(&lpl, &lptr[1]);
10931     bdry = list[lpl] < 0;
10932     if (bdry) {
10933         ++nnb;
10934     }
10935     if (nnb < 3) {
10936         goto L23;
10937     }
10938     lwkl = *lwk;
10939     *lwk = nnb - 3;
10940     if (lwkl < *lwk) {
10941         goto L22;
10942     }
10943     iwl = 0;
10944     if (nnb == 3) {
10945         goto L3;
10946     }
10947 
10948 /* Initialize for loop on arcs N1-N2 for neighbors N2 of N1, */
10949 /*   beginning with the second neighbor.  NR and NL are the */
10950 /*   neighbors preceding and following N2, respectively, and */
10951 /*   LP indexes NL.  The loop is exited when all possible */
10952 /*   swaps have been applied to arcs incident on N1. */
10953 
10954     x1 = x[n1];
10955     y1 = y[n1];
10956     z1 = z__[n1];
10957     nfrst = list[lpf];
10958     nr = nfrst;
10959     xr = x[nr];
10960     yr = y[nr];
10961     zr = z__[nr];
10962     lp = lptr[lpf];
10963     n2 = list[lp];
10964     x2 = x[n2];
10965     y2 = y[n2];
10966     z2 = z__[n2];
10967     lp = lptr[lp];
10968 
10969 /* Top of loop:  set NL to the neighbor following N2. */
10970 
10971 L1:
10972     nl = (i__1 = list[lp], abs(i__1));
10973     if (nl == nfrst && bdry) {
10974         goto L3;
10975     }
10976     xl = x[nl];
10977     yl = y[nl];
10978     zl = z__[nl];
10979 
10980 /*   Test for a convex quadrilateral.  To avoid an incorrect */
10981 /*     test caused by collinearity, use the fact that if N1 */
10982 /*     is a boundary node, then N1 LEFT NR->NL and if N2 is */
10983 /*     a boundary node, then N2 LEFT NL->NR. */
10984 
10985     lpl2 = lend[n2];
10986     if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
10987             list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
10988             z2)))) {
10989 
10990 /*   Nonconvex quadrilateral -- no swap is possible. */
10991 
10992         nr = n2;
10993         xr = x2;
10994         yr = y2;
10995         zr = z2;
10996         goto L2;
10997     }
10998 
10999 /*   The quadrilateral defined by adjacent triangles */
11000 /*     (N1,N2,NL) and (N2,N1,NR) is convex.  Swap in */
11001 /*     NL-NR and store it in IWK unless NL and NR are */
11002 /*     already adjacent, in which case the swap is not */
11003 /*     possible.  Indexes larger than N1 must be decremented */
11004 /*     since N1 will be deleted from X, Y, and Z. */
11005 
11006     swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11007     if (lp21 == 0) {
11008         nr = n2;
11009         xr = x2;
11010         yr = y2;
11011         zr = z2;
11012         goto L2;
11013     }
11014     ++iwl;
11015     if (nl <= n1) {
11016         iwk[(iwl << 1) + 1] = nl;
11017     } else {
11018         iwk[(iwl << 1) + 1] = nl - 1;
11019     }
11020     if (nr <= n1) {
11021         iwk[(iwl << 1) + 2] = nr;
11022     } else {
11023         iwk[(iwl << 1) + 2] = nr - 1;
11024     }
11025 
11026 /*   Recompute the LIST indexes and NFRST, and decrement NNB. */
11027 
11028     lpl = lend[n1];
11029     --nnb;
11030     if (nnb == 3) {
11031         goto L3;
11032     }
11033     lpf = lptr[lpl];
11034     nfrst = list[lpf];
11035     lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11036     if (nr == nfrst) {
11037         goto L2;
11038     }
11039 
11040 /*   NR is not the first neighbor of N1. */
11041 /*     Back up and test N1-NR for a swap again:  Set N2 to */
11042 /*     NR and NR to the previous neighbor of N1 -- the */
11043 /*     neighbor of NR which follows N1.  LP21 points to NL */
11044 /*     as a neighbor of NR. */
11045 
11046     n2 = nr;
11047     x2 = xr;
11048     y2 = yr;
11049     z2 = zr;
11050     lp21 = lptr[lp21];
11051     lp21 = lptr[lp21];
11052     nr = (i__1 = list[lp21], abs(i__1));
11053     xr = x[nr];
11054     yr = y[nr];
11055     zr = z__[nr];
11056     goto L1;
11057 
11058 /*   Bottom of loop -- test for termination of loop. */
11059 
11060 L2:
11061     if (n2 == nfrst) {
11062         goto L3;
11063     }
11064     n2 = nl;
11065     x2 = xl;
11066     y2 = yl;
11067     z2 = zl;
11068     lp = lptr[lp];
11069     goto L1;
11070 
11071 /* Delete N1 and all its incident arcs.  If N1 is an interior */
11072 /*   node and either NNB > 3 or NNB = 3 and N2 LEFT NR->NL, */
11073 /*   then N1 must be separated from its neighbors by a plane */
11074 /*   containing the origin -- its removal reverses the effect */
11075 /*   of a call to COVSPH, and all its neighbors become */
11076 /*   boundary nodes.  This is achieved by treating it as if */
11077 /*   it were a boundary node (setting BDRY to TRUE, changing */
11078 /*   a sign in LIST, and incrementing NNB). */
11079 
11080 L3:
11081     if (! bdry) {
11082         if (nnb > 3) {
11083             bdry = TRUE_;
11084         } else {
11085             lpf = lptr[lpl];
11086             nr = list[lpf];
11087             lp = lptr[lpf];
11088             n2 = list[lp];
11089             nl = list[lpl];
11090             bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11091                     x[n2], &y[n2], &z__[n2]);
11092         }
11093         if (bdry) {
11094 
11095 /*   IF a boundary node already exists, then N1 and its */
11096 /*     neighbors cannot be converted to boundary nodes. */
11097 /*     (They must be collinear.)  This is a problem if */
11098 /*     NNB > 3. */
11099 
11100             i__1 = nn;
11101             for (i__ = 1; i__ <= i__1; ++i__) {
11102                 if (list[lend[i__]] < 0) {
11103                     bdry = FALSE_;
11104                     goto L5;
11105                 }
11106 /* L4: */
11107             }
11108             list[lpl] = -list[lpl];
11109             ++nnb;
11110         }
11111     }
11112 L5:
11113     if (! bdry && nnb > 3) {
11114         goto L24;
11115     }
11116 
11117 /* Initialize for loop on neighbors.  LPL points to the last */
11118 /*   neighbor of N1.  LNEW is stored in local variable LNW. */
11119 
11120     lp = lpl;
11121     lnw = *lnew;
11122 
11123 /* Loop on neighbors N2 of N1, beginning with the first. */
11124 
11125 L6:
11126     lp = lptr[lp];
11127     n2 = (i__1 = list[lp], abs(i__1));
11128     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11129     if (lph < 0) {
11130         goto L23;
11131     }
11132 
11133 /*   LP and LPL may require alteration. */
11134 
11135     if (lpl == lnw) {
11136         lpl = lph;
11137     }
11138     if (lp == lnw) {
11139         lp = lph;
11140     }
11141     if (lp != lpl) {
11142         goto L6;
11143     }
11144 
11145 /* Delete N1 from X, Y, Z, and LEND, and remove its adjacency */
11146 /*   list from LIST and LPTR.  LIST entries (nodal indexes) */
11147 /*   which are larger than N1 must be decremented. */
11148 
11149     --nn;
11150     if (n1 > nn) {
11151         goto L9;
11152     }
11153     i__1 = nn;
11154     for (i__ = n1; i__ <= i__1; ++i__) {
11155         x[i__] = x[i__ + 1];
11156         y[i__] = y[i__ + 1];
11157         z__[i__] = z__[i__ + 1];
11158         lend[i__] = lend[i__ + 1];
11159 /* L7: */
11160     }
11161 
11162     i__1 = lnw - 1;
11163     for (i__ = 1; i__ <= i__1; ++i__) {
11164         if (list[i__] > n1) {
11165             --list[i__];
11166         }
11167         if (list[i__] < -n1) {
11168             ++list[i__];
11169         }
11170 /* L8: */
11171     }
11172 
11173 /*   For LPN = first to last neighbors of N1, delete the */
11174 /*     preceding neighbor (indexed by LP). */
11175 
11176 /*   Each empty LIST,LPTR location LP is filled in with the */
11177 /*     values at LNW-1, and LNW is decremented.  All pointers */
11178 /*     (including those in LPTR and LEND) with value LNW-1 */
11179 /*     must be changed to LP. */
11180 
11181 /*  LPL points to the last neighbor of N1. */
11182 
11183 L9:
11184     if (bdry) {
11185         --nnb;
11186     }
11187     lpn = lpl;
11188     i__1 = nnb;
11189     for (j = 1; j <= i__1; ++j) {
11190         --lnw;
11191         lp = lpn;
11192         lpn = lptr[lp];
11193         list[lp] = list[lnw];
11194         lptr[lp] = lptr[lnw];
11195         if (lptr[lpn] == lnw) {
11196             lptr[lpn] = lp;
11197         }
11198         if (lpn == lnw) {
11199             lpn = lp;
11200         }
11201         for (i__ = nn; i__ >= 1; --i__) {
11202             if (lend[i__] == lnw) {
11203                 lend[i__] = lp;
11204                 goto L11;
11205             }
11206 /* L10: */
11207         }
11208 
11209 L11:
11210         for (i__ = lnw - 1; i__ >= 1; --i__) {
11211             if (lptr[i__] == lnw) {
11212                 lptr[i__] = lp;
11213             }
11214 /* L12: */
11215         }
11216 /* L13: */
11217     }
11218 
11219 /* Update N and LNEW, and optimize the patch of triangles */
11220 /*   containing K (on input) by applying swaps to the arcs */
11221 /*   in IWK. */
11222 
11223     *n = nn;
11224     *lnew = lnw;
11225     if (iwl > 0) {
11226         nit = iwl << 2;
11227         optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11228                 nit, &iwk[3], &ierr);
11229         if (ierr != 0 && ierr != 1) {
11230             goto L25;
11231         }
11232         if (ierr == 1) {
11233             goto L26;
11234         }
11235     }
11236 
11237 /* Successful termination. */
11238 
11239     *ier = 0;
11240     return 0;
11241 
11242 /* Invalid input parameter. */
11243 
11244 L21:
11245     *ier = 1;
11246     return 0;
11247 
11248 /* Insufficient space reserved for IWK. */
11249 
11250 L22:
11251     *ier = 2;
11252     return 0;
11253 
11254 /* Invalid triangulation data structure.  NNB < 3 on input or */
11255 /*   N2 is a neighbor of N1 but N1 is not a neighbor of N2. */
11256 
11257 L23:
11258     *ier = 3;
11259     return 0;
11260 
11261 /* N1 is interior but NNB could not be reduced to 3. */
11262 
11263 L24:
11264     *ier = 4;
11265     return 0;
11266 
11267 /* Error flag (other than 1) returned by OPTIM. */
11268 
11269 L25:
11270     *ier = 5;
11271 /*      WRITE (*,100) NIT, IERR */
11272 /*  100 FORMAT (//5X,'*** Error in OPTIM (called from ', */
11273 /*     .        'DELNOD):  NIT = ',I4,', IER = ',I1,' ***'/) */
11274     return 0;
11275 
11276 /* Error flag 1 returned by OPTIM. */
11277 
11278 L26:
11279     *ier = 6;
11280     return 0;
11281 } /* delnod_ */
11282 
11283 /* Subroutine */ int drwarc_(int *, double *p, double *q,
11284         double *tol, int *nseg)
11285 {
11286     /* System generated locals */
11287     int i__1;
11288     double d__1;
11289 
11290     /* Builtin functions */
11291     //double sqrt(double);
11292 
11293     /* Local variables */
11294     static int i__, k;
11295     static double s, p1[3], p2[3], u1, u2, v1, v2;
11296     static int na;
11297     static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11298 
11299 
11300 /* *********************************************************** */
11301 
11302 /*                                              From STRIPACK */
11303 /*                                            Robert J. Renka */
11304 /*                                  Dept. of Computer Science */
11305 /*                                       Univ. of North Texas */
11306 /*                                           renka@cs.unt.edu */
11307 /*                                                   03/04/03 */
11308 
11309 /*   Given unit vectors P and Q corresponding to northern */
11310 /* hemisphere points (with positive third components), this */
11311 /* subroutine draws a polygonal line which approximates the */
11312 /* projection of arc P-Q onto the plane containing the */
11313 /* equator. */
11314 
11315 /*   The line segment is drawn by writing a sequence of */
11316 /* 'moveto' and 'lineto' Postscript commands to unit LUN.  It */
11317 /* is assumed that an open file is attached to the unit, */
11318 /* header comments have been written to the file, a window- */
11319 /* to-viewport mapping has been established, etc. */
11320 
11321 /* On input: */
11322 
11323 /*       LUN = long int unit number in the range 0 to 99. */
11324 
11325 /*       P,Q = Arrays of length 3 containing the endpoints of */
11326 /*             the arc to be drawn. */
11327 
11328 /*       TOL = Maximum distance in world coordinates between */
11329 /*             the projected arc and polygonal line. */
11330 
11331 /* Input parameters are not altered by this routine. */
11332 
11333 /* On output: */
11334 
11335 /*       NSEG = Number of line segments in the polygonal */
11336 /*              approximation to the projected arc.  This is */
11337 /*              a decreasing function of TOL.  NSEG = 0 and */
11338 /*              no drawing is performed if P = Q or P = -Q */
11339 /*              or an error is encountered in writing to unit */
11340 /*              LUN. */
11341 
11342 /* STRIPACK modules required by DRWARC:  None */
11343 
11344 /* Intrinsic functions called by DRWARC:  ABS, DBLE, SQRT */
11345 
11346 /* *********************************************************** */
11347 
11348 
11349 /* Local parameters: */
11350 
11351 /* DP =    (Q-P)/NSEG */
11352 /* DU,DV = Components of the projection Q'-P' of arc P->Q */
11353 /*           onto the projection plane */
11354 /* ENRM =  Euclidean norm (or squared norm) of Q'-P' or PM */
11355 /* ERR =   Orthogonal distance from the projected midpoint */
11356 /*           PM' to the line defined by P' and Q': */
11357 /*           |Q'-P' X PM'-P'|/|Q'-P'| */
11358 /* I,K =   DO-loop indexes */
11359 /* NA =    Number of arcs (segments) in the partition of P-Q */
11360 /* P1,P2 = Pairs of adjacent points in a uniform partition of */
11361 /*           arc P-Q into NSEG segments; obtained by normal- */
11362 /*           izing PM values */
11363 /* PM =    Midpoint of arc P-Q or a point P + k*DP in a */
11364 /*           uniform partition of the line segment P-Q into */
11365 /*           NSEG segments */
11366 /* S =     Scale factor 1/NA */
11367 /* U1,V1 = Components of P' */
11368 /* U2,V2 = Components of Q' */
11369 /* UM,VM = Components of the midpoint PM' */
11370 
11371 
11372 /* Compute the midpoint PM of arc P-Q. */
11373 
11374     /* Parameter adjustments */
11375     --q;
11376     --p;
11377 
11378     /* Function Body */
11379     enrm = 0.;
11380     for (i__ = 1; i__ <= 3; ++i__) {
11381         pm[i__ - 1] = p[i__] + q[i__];
11382         enrm += pm[i__ - 1] * pm[i__ - 1];
11383 /* L1: */
11384     }
11385     if (enrm == 0.) {
11386         goto L5;
11387     }
11388     enrm = sqrt(enrm);
11389     pm[0] /= enrm;
11390     pm[1] /= enrm;
11391     pm[2] /= enrm;
11392 
11393 /* Project P, Q, and PM to P' = (U1,V1), Q' = (U2,V2), and */
11394 /*   PM' = (UM,VM), respectively. */
11395 
11396     u1 = p[1];
11397     v1 = p[2];
11398     u2 = q[1];
11399     v2 = q[2];
11400     um = pm[0];
11401     vm = pm[1];
11402 
11403 /* Compute the orthogonal distance ERR from PM' to the line */
11404 /*   defined by P' and Q'.  This is the maximum deviation */
11405 /*   between the projected arc and the line segment.  It is */
11406 /*   undefined if P' = Q'. */
11407 
11408     du = u2 - u1;
11409     dv = v2 - v1;
11410     enrm = du * du + dv * dv;
11411     if (enrm == 0.) {
11412         goto L5;
11413     }
11414     err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11415 
11416 /* Compute the number of arcs into which P-Q will be parti- */
11417 /*   tioned (the number of line segments to be drawn): */
11418 /*   NA = ERR/TOL. */
11419 
11420     na = (int) (err / *tol + 1.);
11421 
11422 /* Initialize for loop on arcs P1-P2, where the intermediate */
11423 /*   points are obtained by normalizing PM = P + k*DP for */
11424 /*   DP = (Q-P)/NA */
11425 
11426     s = 1. / (double) na;
11427     for (i__ = 1; i__ <= 3; ++i__) {
11428         dp[i__ - 1] = s * (q[i__] - p[i__]);
11429         pm[i__ - 1] = p[i__];
11430         p1[i__ - 1] = p[i__];
11431 /* L2: */
11432     }
11433 
11434 /* Loop on arcs P1-P2, drawing the line segments associated */
11435 /*   with the projected endpoints. */
11436 
11437     i__1 = na - 1;
11438     for (k = 1; k <= i__1; ++k) {
11439         enrm = 0.;
11440         for (i__ = 1; i__ <= 3; ++i__) {
11441             pm[i__ - 1] += dp[i__ - 1];
11442             enrm += pm[i__ - 1] * pm[i__ - 1];
11443 /* L3: */
11444         }
11445         if (enrm == 0.) {
11446             goto L5;
11447         }
11448         enrm = sqrt(enrm);
11449         p2[0] = pm[0] / enrm;
11450         p2[1] = pm[1] / enrm;
11451         p2[2] = pm[2] / enrm;
11452 /*        WRITE (LUN,100,ERR=5) P1(1), P1(2), P2(1), P2(2) */
11453 /*  100   FORMAT (2F12.6,' moveto',2F12.6,' lineto') */
11454         p1[0] = p2[0];
11455         p1[1] = p2[1];
11456         p1[2] = p2[2];
11457 /* L4: */
11458     }
11459 /*      WRITE (LUN,100,ERR=5) P1(1), P1(2), Q(1), Q(2) */
11460 
11461 /* No error encountered. */
11462 
11463     *nseg = na;
11464     return 0;
11465 
11466 /* Invalid input value of P or Q. */
11467 
11468 L5:
11469     *nseg = 0;
11470     return 0;
11471 } /* drwarc_ */
11472 
11473 /* Subroutine */ int edge_(int *in1, int *in2, double *x,
11474         double *y, double *z__, int *lwk, int *iwk, int *
11475         list, int *lptr, int *lend, int *ier)
11476 {
11477     /* System generated locals */
11478     int i__1;
11479 
11480     /* Local variables */
11481     static int i__, n0, n1, n2;
11482     static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11483     static int nl, lp, nr;
11484     static double dp12;
11485     static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11486     static double dp1l, dp2l, dp1r, dp2r;
11487     extern long int left_(double *, double *, double *, double
11488             *, double *, double *, double *, double *,
11489             double *);
11490     static int ierr;
11491     extern /* Subroutine */ int swap_(int *, int *, int *,
11492             int *, int *, int *, int *, int *);
11493     static int next, iwcp1, n1lst, iwend;
11494     extern /* Subroutine */ int optim_(double *, double *, double
11495             *, int *, int *, int *, int *, int *, int
11496             *, int *);
11497     static int n1frst;
11498 
11499 
11500 /* *********************************************************** */
11501 
11502 /*                                              From STRIPACK */
11503 /*                                            Robert J. Renka */
11504 /*                                  Dept. of Computer Science */
11505 /*                                       Univ. of North Texas */
11506 /*                                           renka@cs.unt.edu */
11507 /*                                                   07/30/98 */
11508 
11509 /*   Given a triangulation of N nodes and a pair of nodal */
11510 /* indexes IN1 and IN2, this routine swaps arcs as necessary */
11511 /* to force IN1 and IN2 to be adjacent.  Only arcs which */
11512 /* intersect IN1-IN2 are swapped out.  If a Delaunay triangu- */
11513 /* lation is input, the resulting triangulation is as close */
11514 /* as possible to a Delaunay triangulation in the sense that */
11515 /* all arcs other than IN1-IN2 are locally optimal. */
11516 
11517 /*   A sequence of calls to EDGE may be used to force the */
11518 /* presence of a set of edges defining the boundary of a non- */
11519 /* convex and/or multiply connected region, or to introduce */
11520 /* barriers into the triangulation.  Note that Subroutine */
11521 /* GETNP will not necessarily return closest nodes if the */
11522 /* triangulation has been constrained by a call to EDGE. */
11523 /* However, this is appropriate in some applications, such */
11524 /* as triangle-based interpolation on a nonconvex domain. */
11525 
11526 
11527 /* On input: */
11528 
11529 /*       IN1,IN2 = Indexes (of X, Y, and Z) in the range 1 to */
11530 /*                 N defining a pair of nodes to be connected */
11531 /*                 by an arc. */
11532 
11533 /*       X,Y,Z = Arrays of length N containing the Cartesian */
11534 /*               coordinates of the nodes. */
11535 
11536 /* The above parameters are not altered by this routine. */
11537 
11538 /*       LWK = Number of columns reserved for IWK.  This must */
11539 /*             be at least NI -- the number of arcs that */
11540 /*             intersect IN1-IN2.  (NI is bounded by N-3.) */
11541 
11542 /*       IWK = int work array of length at least 2*LWK. */
11543 
11544 /*       LIST,LPTR,LEND = Data structure defining the trian- */
11545 /*                        gulation.  Refer to Subroutine */
11546 /*                        TRMESH. */
11547 
11548 /* On output: */
11549 
11550 /*       LWK = Number of arcs which intersect IN1-IN2 (but */
11551 /*             not more than the input value of LWK) unless */
11552 /*             IER = 1 or IER = 3.  LWK = 0 if and only if */
11553 /*             IN1 and IN2 were adjacent (or LWK=0) on input. */
11554 
11555 /*       IWK = Array containing the indexes of the endpoints */
11556 /*             of the new arcs other than IN1-IN2 unless */
11557 /*             IER > 0 or LWK = 0.  New arcs to the left of */
11558 /*             IN1->IN2 are stored in the first K-1 columns */
11559 /*             (left portion of IWK), column K contains */
11560 /*             zeros, and new arcs to the right of IN1->IN2 */
11561 /*             occupy columns K+1,...,LWK.  (K can be deter- */
11562 /*             mined by searching IWK for the zeros.) */
11563 
11564 /*       LIST,LPTR,LEND = Data structure updated if necessary */
11565 /*                        to reflect the presence of an arc */
11566 /*                        connecting IN1 and IN2 unless IER > */
11567 /*                        0.  The data structure has been */
11568 /*                        altered if IER >= 4. */
11569 
11570 /*       IER = Error indicator: */
11571 /*             IER = 0 if no errors were encountered. */
11572 /*             IER = 1 if IN1 < 1, IN2 < 1, IN1 = IN2, */
11573 /*                     or LWK < 0 on input. */
11574 /*             IER = 2 if more space is required in IWK. */
11575 /*                     Refer to LWK. */
11576 /*             IER = 3 if IN1 and IN2 could not be connected */
11577 /*                     due to either an invalid data struc- */
11578 /*                     ture or collinear nodes (and floating */
11579 /*                     point error). */
11580 /*             IER = 4 if an error flag other than IER = 1 */
11581 /*                     was returned by OPTIM. */
11582 /*             IER = 5 if error flag 1 was returned by OPTIM. */
11583 /*                     This is not necessarily an error, but */
11584 /*                     the arcs other than IN1-IN2 may not */
11585 /*                     be optimal. */
11586 
11587 /*   An error message is written to the standard output unit */
11588 /* in the case of IER = 3 or IER = 4. */
11589 
11590 /* Modules required by EDGE:  LEFT, LSTPTR, OPTIM, SWAP, */
11591 /*                              SWPTST */
11592 
11593 /* Intrinsic function called by EDGE:  ABS */
11594 
11595 /* *********************************************************** */
11596 
11597 
11598 /* Local parameters: */
11599 
11600 /* DPij =     Dot product <Ni,Nj> */
11601 /* I =        DO-loop index and column index for IWK */
11602 /* IERR =     Error flag returned by Subroutine OPTIM */
11603 /* IWC =      IWK index between IWF and IWL -- NL->NR is */
11604 /*              stored in IWK(1,IWC)->IWK(2,IWC) */
11605 /* IWCP1 =    IWC + 1 */
11606 /* IWEND =    Input or output value of LWK */
11607 /* IWF =      IWK (column) index of the first (leftmost) arc */
11608 /*              which intersects IN1->IN2 */
11609 /* IWL =      IWK (column) index of the last (rightmost) are */
11610 /*              which intersects IN1->IN2 */
11611 /* LFT =      Flag used to determine if a swap results in the */
11612 /*              new arc intersecting IN1-IN2 -- LFT = 0 iff */
11613 /*              N0 = IN1, LFT = -1 implies N0 LEFT IN1->IN2, */
11614 /*              and LFT = 1 implies N0 LEFT IN2->IN1 */
11615 /* LP =       List pointer (index for LIST and LPTR) */
11616 /* LP21 =     Unused parameter returned by SWAP */
11617 /* LPL =      Pointer to the last neighbor of IN1 or NL */
11618 /* N0 =       Neighbor of N1 or node opposite NR->NL */
11619 /* N1,N2 =    Local copies of IN1 and IN2 */
11620 /* N1FRST =   First neighbor of IN1 */
11621 /* N1LST =    (Signed) last neighbor of IN1 */
11622 /* NEXT =     Node opposite NL->NR */
11623 /* NIT =      Flag or number of iterations employed by OPTIM */
11624 /* NL,NR =    Endpoints of an arc which intersects IN1-IN2 */
11625 /*              with NL LEFT IN1->IN2 */
11626 /* X0,Y0,Z0 = Coordinates of N0 */
11627 /* X1,Y1,Z1 = Coordinates of IN1 */
11628 /* X2,Y2,Z2 = Coordinates of IN2 */
11629 
11630 
11631 /* Store IN1, IN2, and LWK in local variables and test for */
11632 /*   errors. */
11633 
11634     /* Parameter adjustments */
11635     --lend;
11636     --lptr;
11637     --list;
11638     iwk -= 3;
11639     --z__;
11640     --y;
11641     --x;
11642 
11643     /* Function Body */
11644     n1 = *in1;
11645     n2 = *in2;
11646     iwend = *lwk;
11647     if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11648         goto L31;
11649     }
11650 
11651 /* Test for N2 as a neighbor of N1.  LPL points to the last */
11652 /*   neighbor of N1. */
11653 
11654     lpl = lend[n1];
11655     n0 = (i__1 = list[lpl], abs(i__1));
11656     lp = lpl;
11657 L1:
11658     if (n0 == n2) {
11659         goto L30;
11660     }
11661     lp = lptr[lp];
11662     n0 = list[lp];
11663     if (lp != lpl) {
11664         goto L1;
11665     }
11666 
11667 /* Initialize parameters. */
11668 
11669     iwl = 0;
11670     nit = 0;
11671 
11672 /* Store the coordinates of N1 and N2. */
11673 
11674 L2:
11675     x1 = x[n1];
11676     y1 = y[n1];
11677     z1 = z__[n1];
11678     x2 = x[n2];
11679     y2 = y[n2];
11680     z2 = z__[n2];
11681 
11682 /* Set NR and NL to adjacent neighbors of N1 such that */
11683 /*   NR LEFT N2->N1 and NL LEFT N1->N2, */
11684 /*   (NR Forward N1->N2 or NL Forward N1->N2), and */
11685 /*   (NR Forward N2->N1 or NL Forward N2->N1). */
11686 
11687 /*   Initialization:  Set N1FRST and N1LST to the first and */
11688 /*     (signed) last neighbors of N1, respectively, and */
11689 /*     initialize NL to N1FRST. */
11690 
11691     lpl = lend[n1];
11692     n1lst = list[lpl];
11693     lp = lptr[lpl];
11694     n1frst = list[lp];
11695     nl = n1frst;
11696     if (n1lst < 0) {
11697         goto L4;
11698     }
11699 
11700 /*   N1 is an interior node.  Set NL to the first candidate */
11701 /*     for NR (NL LEFT N2->N1). */
11702 
11703 L3:
11704     if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11705         goto L4;
11706     }
11707     lp = lptr[lp];
11708     nl = list[lp];
11709     if (nl != n1frst) {
11710         goto L3;
11711     }
11712 
11713 /*   All neighbors of N1 are strictly left of N1->N2. */
11714 
11715     goto L5;
11716 
11717 /*   NL = LIST(LP) LEFT N2->N1.  Set NR to NL and NL to the */
11718 /*     following neighbor of N1. */
11719 
11720 L4:
11721     nr = nl;
11722     lp = lptr[lp];
11723     nl = (i__1 = list[lp], abs(i__1));
11724     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11725 
11726 /*   NL LEFT N1->N2 and NR LEFT N2->N1.  The Forward tests */
11727 /*     are employed to avoid an error associated with */
11728 /*     collinear nodes. */
11729 
11730         dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11731         dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11732         dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11733         dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11734         dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11735         if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11736                 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11737             goto L6;
11738         }
11739 
11740 /*   NL-NR does not intersect N1-N2.  However, there is */
11741 /*     another candidate for the first arc if NL lies on */
11742 /*     the line N1-N2. */
11743 
11744         if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11745             goto L5;
11746         }
11747     }
11748 
11749 /*   Bottom of loop. */
11750 
11751     if (nl != n1frst) {
11752         goto L4;
11753     }
11754 
11755 /* Either the triangulation is invalid or N1-N2 lies on the */
11756 /*   convex hull boundary and an edge NR->NL (opposite N1 and */
11757 /*   intersecting N1-N2) was not found due to floating point */
11758 /*   error.  Try interchanging N1 and N2 -- NIT > 0 iff this */
11759 /*   has already been done. */
11760 
11761 L5:
11762     if (nit > 0) {
11763         goto L33;
11764     }
11765     nit = 1;
11766     n1 = n2;
11767     n2 = *in1;
11768     goto L2;
11769 
11770 /* Store the ordered sequence of intersecting edges NL->NR in */
11771 /*   IWK(1,IWL)->IWK(2,IWL). */
11772 
11773 L6:
11774     ++iwl;
11775     if (iwl > iwend) {
11776         goto L32;
11777     }
11778     iwk[(iwl << 1) + 1] = nl;
11779     iwk[(iwl << 1) + 2] = nr;
11780 
11781 /*   Set NEXT to the neighbor of NL which follows NR. */
11782 
11783     lpl = lend[nl];
11784     lp = lptr[lpl];
11785 
11786 /*   Find NR as a neighbor of NL.  The search begins with */
11787 /*     the first neighbor. */
11788 
11789 L7:
11790     if (list[lp] == nr) {
11791         goto L8;
11792     }
11793     lp = lptr[lp];
11794     if (lp != lpl) {
11795         goto L7;
11796     }
11797 
11798 /*   NR must be the last neighbor, and NL->NR cannot be a */
11799 /*     boundary edge. */
11800 
11801     if (list[lp] != nr) {
11802         goto L33;
11803     }
11804 
11805 /*   Set NEXT to the neighbor following NR, and test for */
11806 /*     termination of the store loop. */
11807 
11808 L8:
11809     lp = lptr[lp];
11810     next = (i__1 = list[lp], abs(i__1));
11811     if (next == n2) {
11812         goto L9;
11813     }
11814 
11815 /*   Set NL or NR to NEXT. */
11816 
11817     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11818         nl = next;
11819     } else {
11820         nr = next;
11821     }
11822     goto L6;
11823 
11824 /* IWL is the number of arcs which intersect N1-N2. */
11825 /*   Store LWK. */
11826 
11827 L9:
11828     *lwk = iwl;
11829     iwend = iwl;
11830 
11831 /* Initialize for edge swapping loop -- all possible swaps */
11832 /*   are applied (even if the new arc again intersects */
11833 /*   N1-N2), arcs to the left of N1->N2 are stored in the */
11834 /*   left portion of IWK, and arcs to the right are stored in */
11835 /*   the right portion.  IWF and IWL index the first and last */
11836 /*   intersecting arcs. */
11837 
11838     iwf = 1;
11839 
11840 /* Top of loop -- set N0 to N1 and NL->NR to the first edge. */
11841 /*   IWC points to the arc currently being processed.  LFT */
11842 /*   .LE. 0 iff N0 LEFT N1->N2. */
11843 
11844 L10:
11845     lft = 0;
11846     n0 = n1;
11847     x0 = x1;
11848     y0 = y1;
11849     z0 = z1;
11850     nl = iwk[(iwf << 1) + 1];
11851     nr = iwk[(iwf << 1) + 2];
11852     iwc = iwf;
11853 
11854 /*   Set NEXT to the node opposite NL->NR unless IWC is the */
11855 /*     last arc. */
11856 
11857 L11:
11858     if (iwc == iwl) {
11859         goto L21;
11860     }
11861     iwcp1 = iwc + 1;
11862     next = iwk[(iwcp1 << 1) + 1];
11863     if (next != nl) {
11864         goto L16;
11865     }
11866     next = iwk[(iwcp1 << 1) + 2];
11867 
11868 /*   NEXT RIGHT N1->N2 and IWC .LT. IWL.  Test for a possible */
11869 /*     swap. */
11870 
11871     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11872             z__[next])) {
11873         goto L14;
11874     }
11875     if (lft >= 0) {
11876         goto L12;
11877     }
11878     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11879             z__[next])) {
11880         goto L14;
11881     }
11882 
11883 /*   Replace NL->NR with N0->NEXT. */
11884 
11885     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11886     iwk[(iwc << 1) + 1] = n0;
11887     iwk[(iwc << 1) + 2] = next;
11888     goto L15;
11889 
11890 /*   Swap NL-NR for N0-NEXT, shift columns IWC+1,...,IWL to */
11891 /*     the left, and store N0-NEXT in the right portion of */
11892 /*     IWK. */
11893 
11894 L12:
11895     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11896     i__1 = iwl;
11897     for (i__ = iwcp1; i__ <= i__1; ++i__) {
11898         iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11899         iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11900 /* L13: */
11901     }
11902     iwk[(iwl << 1) + 1] = n0;
11903     iwk[(iwl << 1) + 2] = next;
11904     --iwl;
11905     nr = next;
11906     goto L11;
11907 
11908 /*   A swap is not possible.  Set N0 to NR. */
11909 
11910 L14:
11911     n0 = nr;
11912     x0 = x[n0];
11913     y0 = y[n0];
11914     z0 = z__[n0];
11915     lft = 1;
11916 
11917 /*   Advance to the next arc. */
11918 
11919 L15:
11920     nr = next;
11921     ++iwc;
11922     goto L11;
11923 
11924 /*   NEXT LEFT N1->N2, NEXT .NE. N2, and IWC .LT. IWL. */
11925 /*     Test for a possible swap. */
11926 
11927 L16:
11928     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11929             z__[next])) {
11930         goto L19;
11931     }
11932     if (lft <= 0) {
11933         goto L17;
11934     }
11935     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11936             z__[next])) {
11937         goto L19;
11938     }
11939 
11940 /*   Replace NL->NR with NEXT->N0. */
11941 
11942     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11943     iwk[(iwc << 1) + 1] = next;
11944     iwk[(iwc << 1) + 2] = n0;
11945     goto L20;
11946 
11947 /*   Swap NL-NR for N0-NEXT, shift columns IWF,...,IWC-1 to */
11948 /*     the right, and store N0-NEXT in the left portion of */
11949 /*     IWK. */
11950 
11951 L17:
11952     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11953     i__1 = iwf;
11954     for (i__ = iwc - 1; i__ >= i__1; --i__) {
11955         iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11956         iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11957 /* L18: */
11958     }
11959     iwk[(iwf << 1) + 1] = n0;
11960     iwk[(iwf << 1) + 2] = next;
11961     ++iwf;
11962     goto L20;
11963 
11964 /*   A swap is not possible.  Set N0 to NL. */
11965 
11966 L19:
11967     n0 = nl;
11968     x0 = x[n0];
11969     y0 = y[n0];
11970     z0 = z__[n0];
11971     lft = -1;
11972 
11973 /*   Advance to the next arc. */
11974 
11975 L20:
11976     nl = next;
11977     ++iwc;
11978     goto L11;
11979 
11980 /*   N2 is opposite NL->NR (IWC = IWL). */
11981 
11982 L21:
11983     if (n0 == n1) {
11984         goto L24;
11985     }
11986     if (lft < 0) {
11987         goto L22;
11988     }
11989 
11990 /*   N0 RIGHT N1->N2.  Test for a possible swap. */
11991 
11992     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
11993         goto L10;
11994     }
11995 
11996 /*   Swap NL-NR for N0-N2 and store N0-N2 in the right */
11997 /*     portion of IWK. */
11998 
11999     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12000     iwk[(iwl << 1) + 1] = n0;
12001     iwk[(iwl << 1) + 2] = n2;
12002     --iwl;
12003     goto L10;
12004 
12005 /*   N0 LEFT N1->N2.  Test for a possible swap. */
12006 
12007 L22:
12008     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12009         goto L10;
12010     }
12011 
12012 /*   Swap NL-NR for N0-N2, shift columns IWF,...,IWL-1 to the */
12013 /*     right, and store N0-N2 in the left portion of IWK. */
12014 
12015     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12016     i__ = iwl;
12017 L23:
12018     iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12019     iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12020     --i__;
12021     if (i__ > iwf) {
12022         goto L23;
12023     }
12024     iwk[(iwf << 1) + 1] = n0;
12025     iwk[(iwf << 1) + 2] = n2;
12026     ++iwf;
12027     goto L10;
12028 
12029 /* IWF = IWC = IWL.  Swap out the last arc for N1-N2 and */
12030 /*   store zeros in IWK. */
12031 
12032 L24:
12033     swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12034     iwk[(iwc << 1) + 1] = 0;
12035     iwk[(iwc << 1) + 2] = 0;
12036 
12037 /* Optimization procedure -- */
12038 
12039     *ier = 0;
12040     if (iwc > 1) {
12041 
12042 /*   Optimize the set of new arcs to the left of IN1->IN2. */
12043 
12044         nit = iwc - (1<<2);
12045         i__1 = iwc - 1;
12046         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12047                 nit, &iwk[3], &ierr);
12048         if (ierr != 0 && ierr != 1) {
12049             goto L34;
12050         }
12051         if (ierr == 1) {
12052             *ier = 5;
12053         }
12054     }
12055     if (iwc < iwend) {
12056 
12057 /*   Optimize the set of new arcs to the right of IN1->IN2. */
12058 
12059         nit = iwend - (iwc<<2);
12060         i__1 = iwend - iwc;
12061         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12062                 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12063         if (ierr != 0 && ierr != 1) {
12064             goto L34;
12065         }
12066         if (ierr == 1) {
12067             goto L35;
12068         }
12069     }
12070     if (*ier == 5) {
12071         goto L35;
12072     }
12073 
12074 /* Successful termination (IER = 0). */
12075 
12076     return 0;
12077 
12078 /* IN1 and IN2 were adjacent on input. */
12079 
12080 L30:
12081     *ier = 0;
12082     return 0;
12083 
12084 /* Invalid input parameter. */
12085 
12086 L31:
12087     *ier = 1;
12088     return 0;
12089 
12090 /* Insufficient space reserved for IWK. */
12091 
12092 L32:
12093     *ier = 2;
12094     return 0;
12095 
12096 /* Invalid triangulation data structure or collinear nodes */
12097 /*   on convex hull boundary. */
12098 
12099 L33:
12100     *ier = 3;
12101 /*      WRITE (*,130) IN1, IN2 */
12102 /*  130 FORMAT (//5X,'*** Error in EDGE:  Invalid triangula', */
12103 /*     .        'tion or null triangles on boundary'/ */
12104 /*     .        9X,'IN1 =',I4,', IN2=',I4/) */
12105     return 0;
12106 
12107 /* Error flag (other than 1) returned by OPTIM. */
12108 
12109 L34:
12110     *ier = 4;
12111 /*      WRITE (*,140) NIT, IERR */
12112 /*  140 FORMAT (//5X,'*** Error in OPTIM (called from EDGE):', */
12113 /*     .        '  NIT = ',I4,', IER = ',I1,' ***'/) */
12114     return 0;
12115 
12116 /* Error flag 1 returned by OPTIM. */
12117 
12118 L35:
12119     *ier = 5;
12120     return 0;
12121 } /* edge_ */
12122 
12123 /* Subroutine */ int getnp_(double *x, double *y, double *z__,
12124         int *list, int *lptr, int *lend, int *l, int *
12125         npts, double *df, int *ier)
12126 {
12127     /* System generated locals */
12128     int i__1, i__2;
12129 
12130     /* Local variables */
12131     static int i__, n1;
12132     static double x1, y1, z1;
12133     static int nb, ni, lp, np, lm1;
12134     static double dnb, dnp;
12135     static int lpl;
12136 
12137 
12138 /* *********************************************************** */
12139 
12140 /*                                              From STRIPACK */
12141 /*                                            Robert J. Renka */
12142 /*                                  Dept. of Computer Science */
12143 /*                                       Univ. of North Texas */
12144 /*                                           renka@cs.unt.edu */
12145 /*                                                   07/28/98 */
12146 
12147 /*   Given a Delaunay triangulation of N nodes on the unit */
12148 /* sphere and an array NPTS containing the indexes of L-1 */
12149 /* nodes ordered by angular distance from NPTS(1), this sub- */
12150 /* routine sets NPTS(L) to the index of the next node in the */
12151 /* sequence -- the node, other than NPTS(1),...,NPTS(L-1), */
12152 /* that is closest to NPTS(1).  Thus, the ordered sequence */
12153 /* of K closest nodes to N1 (including N1) may be determined */
12154 /* by K-1 calls to GETNP with NPTS(1) = N1 and L = 2,3,...,K */
12155 /* for K .GE. 2. */
12156 
12157 /*   The algorithm uses the property of a Delaunay triangula- */
12158 /* tion that the K-th closest node to N1 is a neighbor of one */
12159 /* of the K-1 closest nodes to N1. */
12160 
12161 
12162 /* On input: */
12163 
12164 /*       X,Y,Z = Arrays of length N containing the Cartesian */
12165 /*               coordinates of the nodes. */
12166 
12167 /*       LIST,LPTR,LEND = Triangulation data structure.  Re- */
12168 /*                        fer to Subroutine TRMESH. */
12169 
12170 /*       L = Number of nodes in the sequence on output.  2 */
12171 /*           .LE. L .LE. N. */
12172 
12173 /* The above parameters are not altered by this routine. */
12174 
12175 /*       NPTS = Array of length .GE. L containing the indexes */
12176 /*              of the L-1 closest nodes to NPTS(1) in the */
12177 /*              first L-1 locations. */
12178 
12179 /* On output: */
12180 
12181 /*       NPTS = Array updated with the index of the L-th */
12182 /*              closest node to NPTS(1) in position L unless */
12183 /*              IER = 1. */
12184 
12185 /*       DF = Value of an increasing function (negative cos- */
12186 /*            ine) of the angular distance between NPTS(1) */
12187 /*            and NPTS(L) unless IER = 1. */
12188 
12189 /*       IER = Error indicator: */
12190 /*             IER = 0 if no errors were encountered. */
12191 /*             IER = 1 if L < 2. */
12192 
12193 /* Modules required by GETNP:  None */
12194 
12195 /* Intrinsic function called by GETNP:  ABS */
12196 
12197 /* *********************************************************** */
12198 
12199 
12200 /* Local parameters: */
12201 
12202 /* DNB,DNP =  Negative cosines of the angular distances from */
12203 /*              N1 to NB and to NP, respectively */
12204 /* I =        NPTS index and DO-loop index */
12205 /* LM1 =      L-1 */
12206 /* LP =       LIST pointer of a neighbor of NI */
12207 /* LPL =      Pointer to the last neighbor of NI */
12208 /* N1 =       NPTS(1) */
12209 /* NB =       Neighbor of NI and candidate for NP */
12210 /* NI =       NPTS(I) */
12211 /* NP =       Candidate for NPTS(L) */
12212 /* X1,Y1,Z1 = Coordinates of N1 */
12213 
12214     /* Parameter adjustments */
12215     --x;
12216     --y;
12217     --z__;
12218     --list;
12219     --lptr;
12220     --lend;
12221     --npts;
12222 
12223     /* Function Body */
12224     lm1 = *l - 1;
12225     if (lm1 < 1) {
12226         goto L6;
12227     }
12228     *ier = 0;
12229 
12230 /* Store N1 = NPTS(1) and mark the elements of NPTS. */
12231 
12232     n1 = npts[1];
12233     x1 = x[n1];
12234     y1 = y[n1];
12235     z1 = z__[n1];
12236     i__1 = lm1;
12237     for (i__ = 1; i__ <= i__1; ++i__) {
12238         ni = npts[i__];
12239         lend[ni] = -lend[ni];
12240 /* L1: */
12241     }
12242 
12243 /* Candidates for NP = NPTS(L) are the unmarked neighbors */
12244 /*   of nodes in NPTS.  DNP is initially greater than -cos(PI) */
12245 /*   (the maximum distance). */
12246 
12247     dnp = 2.;
12248 
12249 /* Loop on nodes NI in NPTS. */
12250 
12251     i__1 = lm1;
12252     for (i__ = 1; i__ <= i__1; ++i__) {
12253         ni = npts[i__];
12254         lpl = -lend[ni];
12255         lp = lpl;
12256 
12257 /* Loop on neighbors NB of NI. */
12258 
12259 L2:
12260         nb = (i__2 = list[lp], abs(i__2));
12261         if (lend[nb] < 0) {
12262             goto L3;
12263         }
12264 
12265 /* NB is an unmarked neighbor of NI.  Replace NP if NB is */
12266 /*   closer to N1. */
12267 
12268         dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12269         if (dnb >= dnp) {
12270             goto L3;
12271         }
12272         np = nb;
12273         dnp = dnb;
12274 L3:
12275         lp = lptr[lp];
12276         if (lp != lpl) {
12277             goto L2;
12278         }
12279 /* L4: */
12280     }
12281     npts[*l] = np;
12282     *df = dnp;
12283 
12284 /* Unmark the elements of NPTS. */
12285 
12286     i__1 = lm1;
12287     for (i__ = 1; i__ <= i__1; ++i__) {
12288         ni = npts[i__];
12289         lend[ni] = -lend[ni];
12290 /* L5: */
12291     }
12292     return 0;
12293 
12294 /* L is outside its valid range. */
12295 
12296 L6:
12297     *ier = 1;
12298     return 0;
12299 } /* getnp_ */
12300 
12301 /* Subroutine */ int insert_(int *k, int *lp, int *list, int *
12302         lptr, int *lnew)
12303 {
12304     static int lsav;
12305 
12306 
12307 /* *********************************************************** */
12308 
12309 /*                                              From STRIPACK */
12310 /*                                            Robert J. Renka */
12311 /*                                  Dept. of Computer Science */
12312 /*                                       Univ. of North Texas */
12313 /*                                           renka@cs.unt.edu */
12314 /*                                                   07/17/96 */
12315 
12316 /*   This subroutine inserts K as a neighbor of N1 following */
12317 /* N2, where LP is the LIST pointer of N2 as a neighbor of */
12318 /* N1.  Note that, if N2 is the last neighbor of N1, K will */
12319 /* become the first neighbor (even if N1 is a boundary node). */
12320 
12321 /*   This routine is identical to the similarly named routine */
12322 /* in TRIPACK. */
12323 
12324 
12325 /* On input: */
12326 
12327 /*       K = Index of the node to be inserted. */
12328 
12329 /*       LP = LIST pointer of N2 as a neighbor of N1. */
12330 
12331 /* The above parameters are not altered by this routine. */
12332 
12333 /*       LIST,LPTR,LNEW = Data structure defining the trian- */
12334 /*                        gulation.  Refer to Subroutine */
12335 /*                        TRMESH. */
12336 
12337 /* On output: */
12338 
12339 /*       LIST,LPTR,LNEW = Data structure updated with the */
12340 /*                        addition of node K. */
12341 
12342 /* Modules required by INSERT:  None */
12343 
12344 /* *********************************************************** */
12345 
12346 
12347     /* Parameter adjustments */
12348     --lptr;
12349     --list;
12350 
12351     /* Function Body */
12352     lsav = lptr[*lp];
12353     lptr[*lp] = *lnew;
12354     list[*lnew] = *k;
12355     lptr[*lnew] = lsav;
12356     ++(*lnew);
12357     return 0;
12358 } /* insert_ */
12359 
12360 long int inside_(double *p, int *lv, double *xv, double *yv,
12361         double *zv, int *nv, int *listv, int *ier)
12362 {
12363     /* Initialized data */
12364 
12365     static double eps = .001;
12366 
12367     /* System generated locals */
12368     int i__1;
12369     long int ret_val = 0;
12370 
12371     /* Builtin functions */
12372     //double sqrt(double);
12373 
12374     /* Local variables */
12375     static double b[3], d__;
12376     static int k, n;
12377     static double q[3];
12378     static int i1, i2, k0;
12379     static double v1[3], v2[3], cn[3], bp, bq;
12380     static int ni;
12381     static double pn[3], qn[3], vn[3];
12382     static int imx;
12383     static long int lft1, lft2, even;
12384     static int ierr;
12385     static long int pinr, qinr;
12386     static double qnrm, vnrm;
12387     extern /* Subroutine */ int intrsc_(double *, double *,
12388             double *, double *, int *);
12389 
12390 
12391 /* *********************************************************** */
12392 
12393 /*                                              From STRIPACK */
12394 /*                                            Robert J. Renka */
12395 /*                                  Dept. of Computer Science */
12396 /*                                       Univ. of North Texas */
12397 /*                                           renka@cs.unt.edu */
12398 /*                                                   12/27/93 */
12399 
12400 /*   This function locates a point P relative to a polygonal */
12401 /* region R on the surface of the unit sphere, returning */
12402 /* INSIDE = TRUE if and only if P is contained in R.  R is */
12403 /* defined by a cyclically ordered sequence of vertices which */
12404 /* form a positively-oriented simple closed curve.  Adjacent */
12405 /* vertices need not be distinct but the curve must not be */
12406 /* self-intersecting.  Also, while polygon edges are by defi- */
12407 /* nition restricted to a single hemisphere, R is not so */
12408 /* restricted.  Its interior is the region to the left as the */
12409 /* vertices are traversed in order. */
12410 
12411 /*   The algorithm consists of selecting a point Q in R and */
12412 /* then finding all points at which the great circle defined */
12413 /* by P and Q intersects the boundary of R.  P lies inside R */
12414 /* if and only if there is an even number of intersection */
12415 /* points between Q and P.  Q is taken to be a point immedi- */
12416 /* ately to the left of a directed boundary edge -- the first */
12417 /* one that results in no consistency-check failures. */
12418 
12419 /*   If P is close to the polygon boundary, the problem is */
12420 /* ill-conditioned and the decision may be incorrect.  Also, */
12421 /* an incorrect decision may result from a poor choice of Q */
12422 /* (if, for example, a boundary edge lies on the great cir- */
12423 /* cle defined by P and Q).  A more reliable result could be */
12424 /* obtained by a sequence of calls to INSIDE with the ver- */
12425 /* tices cyclically permuted before each call (to alter the */
12426 /* choice of Q). */
12427 
12428 
12429 /* On input: */
12430 
12431 /*       P = Array of length 3 containing the Cartesian */
12432 /*           coordinates of the point (unit vector) to be */
12433 /*           located. */
12434 
12435 /*       LV = Length of arrays XV, YV, and ZV. */
12436 
12437 /*       XV,YV,ZV = Arrays of length LV containing the Carte- */
12438 /*                  sian coordinates of unit vectors (points */
12439 /*                  on the unit sphere).  These values are */
12440 /*                  not tested for validity. */
12441 
12442 /*       NV = Number of vertices in the polygon.  3 .LE. NV */
12443 /*            .LE. LV. */
12444 
12445 /*       LISTV = Array of length NV containing the indexes */
12446 /*               (for XV, YV, and ZV) of a cyclically-ordered */
12447 /*               (and CCW-ordered) sequence of vertices that */
12448 /*               define R.  The last vertex (indexed by */
12449 /*               LISTV(NV)) is followed by the first (indexed */
12450 /*               by LISTV(1)).  LISTV entries must be in the */
12451 /*               range 1 to LV. */
12452 
12453 /* Input parameters are not altered by this function. */
12454 
12455 /* On output: */
12456 
12457 /*       INSIDE = TRUE if and only if P lies inside R unless */
12458 /*                IER .NE. 0, in which case the value is not */
12459 /*                altered. */
12460 
12461 /*       IER = Error indicator: */
12462 /*             IER = 0 if no errors were encountered. */
12463 /*             IER = 1 if LV or NV is outside its valid */
12464 /*                     range. */
12465 /*             IER = 2 if a LISTV entry is outside its valid */
12466 /*                     range. */
12467 /*             IER = 3 if the polygon boundary was found to */
12468 /*                     be self-intersecting.  This error will */
12469 /*                     not necessarily be detected. */
12470 /*             IER = 4 if every choice of Q (one for each */
12471 /*                     boundary edge) led to failure of some */
12472 /*                     internal consistency check.  The most */
12473 /*                     likely cause of this error is invalid */
12474 /*                     input:  P = (0,0,0), a null or self- */
12475 /*                     intersecting polygon, etc. */
12476 
12477 /* Module required by INSIDE:  INTRSC */
12478 
12479 /* Intrinsic function called by INSIDE:  SQRT */
12480 
12481 /* *********************************************************** */
12482 
12483 
12484 /* Local parameters: */
12485 
12486 /* B =         Intersection point between the boundary and */
12487 /*               the great circle defined by P and Q */
12488 /* BP,BQ =     <B,P> and <B,Q>, respectively, maximized over */
12489 /*               intersection points B that lie between P and */
12490 /*               Q (on the shorter arc) -- used to find the */
12491 /*               closest intersection points to P and Q */
12492 /* CN =        Q X P = normal to the plane of P and Q */
12493 /* D =         Dot product <B,P> or <B,Q> */
12494 /* EPS =       Parameter used to define Q as the point whose */
12495 /*               orthogonal distance to (the midpoint of) */
12496 /*               boundary edge V1->V2 is approximately EPS/ */
12497 /*               (2*Cos(A/2)), where <V1,V2> = Cos(A). */
12498 /* EVEN =      TRUE iff an even number of intersection points */
12499 /*               lie between P and Q (on the shorter arc) */
12500 /* I1,I2 =     Indexes (LISTV elements) of a pair of adjacent */
12501 /*               boundary vertices (endpoints of a boundary */
12502 /*               edge) */
12503 /* IERR =      Error flag for calls to INTRSC (not tested) */
12504 /* IMX =       Local copy of LV and maximum value of I1 and */
12505 /*               I2 */
12506 /* K =         DO-loop index and LISTV index */
12507 /* K0 =        LISTV index of the first endpoint of the */
12508 /*               boundary edge used to compute Q */
12509 /* LFT1,LFT2 = long int variables associated with I1 and I2 in */
12510 /*               the boundary traversal:  TRUE iff the vertex */
12511 /*               is strictly to the left of Q->P (<V,CN> > 0) */
12512 /* N =         Local copy of NV */
12513 /* NI =        Number of intersections (between the boundary */
12514 /*               curve and the great circle P-Q) encountered */
12515 /* PINR =      TRUE iff P is to the left of the directed */
12516 /*               boundary edge associated with the closest */
12517 /*               intersection point to P that lies between P */
12518 /*               and Q (a left-to-right intersection as */
12519 /*               viewed from Q), or there is no intersection */
12520 /*               between P and Q (on the shorter arc) */
12521 /* PN,QN =     P X CN and CN X Q, respectively:  used to */
12522 /*               locate intersections B relative to arc Q->P */
12523 /* Q =         (V1 + V2 + EPS*VN/VNRM)/QNRM, where V1->V2 is */
12524 /*               the boundary edge indexed by LISTV(K0) -> */
12525 /*               LISTV(K0+1) */
12526 /* QINR =      TRUE iff Q is to the left of the directed */
12527 /*               boundary edge associated with the closest */
12528 /*               intersection point to Q that lies between P */
12529 /*               and Q (a right-to-left intersection as */
12530 /*               viewed from Q), or there is no intersection */
12531 /*               between P and Q (on the shorter arc) */
12532 /* QNRM =      Euclidean norm of V1+V2+EPS*VN/VNRM used to */
12533 /*               compute (normalize) Q */
12534 /* V1,V2 =     Vertices indexed by I1 and I2 in the boundary */
12535 /*               traversal */
12536 /* VN =        V1 X V2, where V1->V2 is the boundary edge */
12537 /*               indexed by LISTV(K0) -> LISTV(K0+1) */
12538 /* VNRM =      Euclidean norm of VN */
12539 
12540     /* Parameter adjustments */
12541     --p;
12542     --zv;
12543     --yv;
12544     --xv;
12545     --listv;
12546 
12547     /* Function Body */
12548 
12549 /* Store local parameters, test for error 1, and initialize */
12550 /*   K0. */
12551 
12552     imx = *lv;
12553     n = *nv;
12554     if (n < 3 || n > imx) {
12555         goto L11;
12556     }
12557     k0 = 0;
12558     i1 = listv[1];
12559     if (i1 < 1 || i1 > imx) {
12560         goto L12;
12561     }
12562 
12563 /* Increment K0 and set Q to a point immediately to the left */
12564 /*   of the midpoint of edge V1->V2 = LISTV(K0)->LISTV(K0+1): */
12565 /*   Q = (V1 + V2 + EPS*VN/VNRM)/QNRM, where VN = V1 X V2. */
12566 
12567 L1:
12568     ++k0;
12569     if (k0 > n) {
12570         goto L14;
12571     }
12572     i1 = listv[k0];
12573     if (k0 < n) {
12574         i2 = listv[k0 + 1];
12575     } else {
12576         i2 = listv[1];
12577     }
12578     if (i2 < 1 || i2 > imx) {
12579         goto L12;
12580     }
12581     vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12582     vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12583     vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12584     vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12585     if (vnrm == 0.) {
12586         goto L1;
12587     }
12588     q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12589     q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12590     q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12591     qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12592     q[0] /= qnrm;
12593     q[1] /= qnrm;
12594     q[2] /= qnrm;
12595 
12596 /* Compute CN = Q X P, PN = P X CN, and QN = CN X Q. */
12597 
12598     cn[0] = q[1] * p[3] - q[2] * p[2];
12599     cn[1] = q[2] * p[1] - q[0] * p[3];
12600     cn[2] = q[0] * p[2] - q[1] * p[1];
12601     if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12602         goto L1;
12603     }
12604     pn[0] = p[2] * cn[2] - p[3] * cn[1];
12605     pn[1] = p[3] * cn[0] - p[1] * cn[2];
12606     pn[2] = p[1] * cn[1] - p[2] * cn[0];
12607     qn[0] = cn[1] * q[2] - cn[2] * q[1];
12608     qn[1] = cn[2] * q[0] - cn[0] * q[2];
12609     qn[2] = cn[0] * q[1] - cn[1] * q[0];
12610 
12611 /* Initialize parameters for the boundary traversal. */
12612 
12613     ni = 0;
12614     even = TRUE_;
12615     bp = -2.;
12616     bq = -2.;
12617     pinr = TRUE_;
12618     qinr = TRUE_;
12619     i2 = listv[n];
12620     if (i2 < 1 || i2 > imx) {
12621         goto L12;
12622     }
12623     lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12624 
12625 /* Loop on boundary arcs I1->I2. */
12626 
12627     i__1 = n;
12628     for (k = 1; k <= i__1; ++k) {
12629         i1 = i2;
12630         lft1 = lft2;
12631         i2 = listv[k];
12632         if (i2 < 1 || i2 > imx) {
12633             goto L12;
12634         }
12635         lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12636         if (lft1 == lft2) {
12637             goto L2;
12638         }
12639 
12640 /*   I1 and I2 are on opposite sides of Q->P.  Compute the */
12641 /*     point of intersection B. */
12642 
12643         ++ni;
12644         v1[0] = xv[i1];
12645         v1[1] = yv[i1];
12646         v1[2] = zv[i1];
12647         v2[0] = xv[i2];
12648         v2[1] = yv[i2];
12649         v2[2] = zv[i2];
12650         intrsc_(v1, v2, cn, b, &ierr);
12651 
12652 /*   B is between Q and P (on the shorter arc) iff */
12653 /*     B Forward Q->P and B Forward P->Q       iff */
12654 /*     <B,QN> > 0 and <B,PN> > 0. */
12655 
12656         if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12657                 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12658 
12659 /*   Update EVEN, BQ, QINR, BP, and PINR. */
12660 
12661             even = ! even;
12662             d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12663             if (d__ > bq) {
12664                 bq = d__;
12665                 qinr = lft2;
12666             }
12667             d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12668             if (d__ > bp) {
12669                 bp = d__;
12670                 pinr = lft1;
12671             }
12672         }
12673 L2:
12674         ;
12675     }
12676 
12677 /* Test for consistency:  NI must be even and QINR must be */
12678 /*   TRUE. */
12679 
12680     if (ni != ni / 2 << 1 || ! qinr) {
12681         goto L1;
12682     }
12683 
12684 /* Test for error 3:  different values of PINR and EVEN. */
12685 
12686     if (pinr != even) {
12687         goto L13;
12688     }
12689 
12690 /* No error encountered. */
12691 
12692     *ier = 0;
12693     ret_val = even;
12694     return ret_val;
12695 
12696 /* LV or NV is outside its valid range. */
12697 
12698 L11:
12699     *ier = 1;
12700     return ret_val;
12701 
12702 /* A LISTV entry is outside its valid range. */
12703 
12704 L12:
12705     *ier = 2;
12706     return ret_val;
12707 
12708 /* The polygon boundary is self-intersecting. */
12709 
12710 L13:
12711     *ier = 3;
12712     return ret_val;
12713 
12714 /* Consistency tests failed for all values of Q. */
12715 
12716 L14:
12717     *ier = 4;
12718     return ret_val;
12719 } /* inside_ */
12720 
12721 /* Subroutine */ int intadd_(int *kk, int *i1, int *i2, int *
12722         i3, int *list, int *lptr, int *lend, int *lnew)
12723 {
12724     static int k, n1, n2, n3, lp;
12725     extern /* Subroutine */ int insert_(int *, int *, int *,
12726             int *, int *);
12727     extern int lstptr_(int *, int *, int *, int *);
12728 
12729 
12730 /* *********************************************************** */
12731 
12732 /*                                              From STRIPACK */
12733 /*                                            Robert J. Renka */
12734 /*                                  Dept. of Computer Science */
12735 /*                                       Univ. of North Texas */
12736 /*                                           renka@cs.unt.edu */
12737 /*                                                   07/17/96 */
12738 
12739 /*   This subroutine adds an interior node to a triangulation */
12740 /* of a set of points on the unit sphere.  The data structure */
12741 /* is updated with the insertion of node KK into the triangle */
12742 /* whose vertices are I1, I2, and I3.  No optimization of the */
12743 /* triangulation is performed. */
12744 
12745 /*   This routine is identical to the similarly named routine */
12746 /* in TRIPACK. */
12747 
12748 
12749 /* On input: */
12750 
12751 /*       KK = Index of the node to be inserted.  KK .GE. 1 */
12752 /*            and KK must not be equal to I1, I2, or I3. */
12753 
12754 /*       I1,I2,I3 = Indexes of the counterclockwise-ordered */
12755 /*                  sequence of vertices of a triangle which */
12756 /*                  contains node KK. */
12757 
12758 /* The above parameters are not altered by this routine. */
12759 
12760 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
12761 /*                             triangulation.  Refer to Sub- */
12762 /*                             routine TRMESH.  Triangle */
12763 /*                             (I1,I2,I3) must be included */
12764 /*                             in the triangulation. */
12765 
12766 /* On output: */
12767 
12768 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
12769 /*                             the addition of node KK.  KK */
12770 /*                             will be connected to nodes I1, */
12771 /*                             I2, and I3. */
12772 
12773 /* Modules required by INTADD:  INSERT, LSTPTR */
12774 
12775 /* *********************************************************** */
12776 
12777 
12778 /* Local parameters: */
12779 
12780 /* K =        Local copy of KK */
12781 /* LP =       LIST pointer */
12782 /* N1,N2,N3 = Local copies of I1, I2, and I3 */
12783 
12784     /* Parameter adjustments */
12785     --lend;
12786     --lptr;
12787     --list;
12788 
12789     /* Function Body */
12790     k = *kk;
12791 
12792 /* Initialization. */
12793 
12794     n1 = *i1;
12795     n2 = *i2;
12796     n3 = *i3;
12797 
12798 /* Add K as a neighbor of I1, I2, and I3. */
12799 
12800     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12801     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12802     lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12803     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12804     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12805     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12806 
12807 /* Add I1, I2, and I3 as neighbors of K. */
12808 
12809     list[*lnew] = n1;
12810     list[*lnew + 1] = n2;
12811     list[*lnew + 2] = n3;
12812     lptr[*lnew] = *lnew + 1;
12813     lptr[*lnew + 1] = *lnew + 2;
12814     lptr[*lnew + 2] = *lnew;
12815     lend[k] = *lnew + 2;
12816     *lnew += 3;
12817     return 0;
12818 } /* intadd_ */
12819 
12820 /* Subroutine */ int intrsc_(double *p1, double *p2, double *cn,
12821         double *p, int *ier)
12822 {
12823     /* Builtin functions */
12824     //double sqrt(double);
12825 
12826     /* Local variables */
12827     static int i__;
12828     static double t, d1, d2, pp[3], ppn;
12829 
12830 
12831 /* *********************************************************** */
12832 
12833 /*                                              From STRIPACK */
12834 /*                                            Robert J. Renka */
12835 /*                                  Dept. of Computer Science */
12836 /*                                       Univ. of North Texas */
12837 /*                                           renka@cs.unt.edu */
12838 /*                                                   07/19/90 */
12839 
12840 /*   Given a great circle C and points P1 and P2 defining an */
12841 /* arc A on the surface of the unit sphere, where A is the */
12842 /* shorter of the two portions of the great circle C12 assoc- */
12843 /* iated with P1 and P2, this subroutine returns the point */
12844 /* of intersection P between C and C12 that is closer to A. */
12845 /* Thus, if P1 and P2 lie in opposite hemispheres defined by */
12846 /* C, P is the point of intersection of C with A. */
12847 
12848 
12849 /* On input: */
12850 
12851 /*       P1,P2 = Arrays of length 3 containing the Cartesian */
12852 /*               coordinates of unit vectors. */
12853 
12854 /*       CN = Array of length 3 containing the Cartesian */
12855 /*            coordinates of a nonzero vector which defines C */
12856 /*            as the intersection of the plane whose normal */
12857 /*            is CN with the unit sphere.  Thus, if C is to */
12858 /*            be the great circle defined by P and Q, CN */
12859 /*            should be P X Q. */
12860 
12861 /* The above parameters are not altered by this routine. */
12862 
12863 /*       P = Array of length 3. */
12864 
12865 /* On output: */
12866 
12867 /*       P = Point of intersection defined above unless IER */
12868 /*           .NE. 0, in which case P is not altered. */
12869 
12870 /*       IER = Error indicator. */
12871 /*             IER = 0 if no errors were encountered. */
12872 /*             IER = 1 if <CN,P1> = <CN,P2>.  This occurs */
12873 /*                     iff P1 = P2 or CN = 0 or there are */
12874 /*                     two intersection points at the same */
12875 /*                     distance from A. */
12876 /*             IER = 2 if P2 = -P1 and the definition of A is */
12877 /*                     therefore ambiguous. */
12878 
12879 /* Modules required by INTRSC:  None */
12880 
12881 /* Intrinsic function called by INTRSC:  SQRT */
12882 
12883 /* *********************************************************** */
12884 
12885 
12886 /* Local parameters: */
12887 
12888 /* D1 =  <CN,P1> */
12889 /* D2 =  <CN,P2> */
12890 /* I =   DO-loop index */
12891 /* PP =  P1 + T*(P2-P1) = Parametric representation of the */
12892 /*         line defined by P1 and P2 */
12893 /* PPN = Norm of PP */
12894 /* T =   D1/(D1-D2) = Parameter value chosen so that PP lies */
12895 /*         in the plane of C */
12896 
12897     /* Parameter adjustments */
12898     --p;
12899     --cn;
12900     --p2;
12901     --p1;
12902 
12903     /* Function Body */
12904     d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
12905     d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
12906 
12907     if (d1 == d2) {
12908         *ier = 1;
12909         return 0;
12910     }
12911 
12912 /* Solve for T such that <PP,CN> = 0 and compute PP and PPN. */
12913 
12914     t = d1 / (d1 - d2);
12915     ppn = 0.;
12916     for (i__ = 1; i__ <= 3; ++i__) {
12917         pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
12918         ppn += pp[i__ - 1] * pp[i__ - 1];
12919 /* L1: */
12920     }
12921 
12922 /* PPN = 0 iff PP = 0 iff P2 = -P1 (and T = .5). */
12923 
12924     if (ppn == 0.) {
12925         *ier = 2;
12926         return 0;
12927     }
12928     ppn = sqrt(ppn);
12929 
12930 /* Compute P = PP/PPN. */
12931 
12932     for (i__ = 1; i__ <= 3; ++i__) {
12933         p[i__] = pp[i__ - 1] / ppn;
12934 /* L2: */
12935     }
12936     *ier = 0;
12937     return 0;
12938 } /* intrsc_ */
12939 
12940 int jrand_(int *n, int *ix, int *iy, int *iz)
12941 {
12942     /* System generated locals */
12943     int ret_val;
12944 
12945     /* Local variables */
12946     static float u, x;
12947 
12948 
12949 /* *********************************************************** */
12950 
12951 /*                                              From STRIPACK */
12952 /*                                            Robert J. Renka */
12953 /*                                  Dept. of Computer Science */
12954 /*                                       Univ. of North Texas */
12955 /*                                           renka@cs.unt.edu */
12956 /*                                                   07/28/98 */
12957 
12958 /*   This function returns a uniformly distributed pseudo- */
12959 /* random int in the range 1 to N. */
12960 
12961 
12962 /* On input: */
12963 
12964 /*       N = Maximum value to be returned. */
12965 
12966 /* N is not altered by this function. */
12967 
12968 /*       IX,IY,IZ = int seeds initialized to values in */
12969 /*                  the range 1 to 30,000 before the first */
12970 /*                  call to JRAND, and not altered between */
12971 /*                  subsequent calls (unless a sequence of */
12972 /*                  random numbers is to be repeated by */
12973 /*                  reinitializing the seeds). */
12974 
12975 /* On output: */
12976 
12977 /*       IX,IY,IZ = Updated int seeds. */
12978 
12979 /*       JRAND = Random int in the range 1 to N. */
12980 
12981 /* Reference:  B. A. Wichmann and I. D. Hill, "An Efficient */
12982 /*             and Portable Pseudo-random Number Generator", */
12983 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
12984 /*             pp. 188-190. */
12985 
12986 /* Modules required by JRAND:  None */
12987 
12988 /* Intrinsic functions called by JRAND:  INT, MOD, float */
12989 
12990 /* *********************************************************** */
12991 
12992 
12993 /* Local parameters: */
12994 
12995 /* U = Pseudo-random number uniformly distributed in the */
12996 /*     interval (0,1). */
12997 /* X = Pseudo-random number in the range 0 to 3 whose frac- */
12998 /*       tional part is U. */
12999 
13000     *ix = *ix * 171 % 30269;
13001     *iy = *iy * 172 % 30307;
13002     *iz = *iz * 170 % 30323;
13003     x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13004             30323.f;
13005     u = x - (int) x;
13006     ret_val = (int) ((float) (*n) * u + 1.f);
13007     return ret_val;
13008 } /* jrand_ */
13009 
13010 long int left_(double *x1, double *y1, double *z1, double *x2,
13011         double *y2, double *z2, double *x0, double *y0,
13012         double *z0)
13013 {
13014     /* System generated locals */
13015     long int ret_val;
13016 
13017 
13018 /* *********************************************************** */
13019 
13020 /*                                              From STRIPACK */
13021 /*                                            Robert J. Renka */
13022 /*                                  Dept. of Computer Science */
13023 /*                                       Univ. of North Texas */
13024 /*                                           renka@cs.unt.edu */
13025 /*                                                   07/15/96 */
13026 
13027 /*   This function determines whether node N0 is in the */
13028 /* (closed) left hemisphere defined by the plane containing */
13029 /* N1, N2, and the origin, where left is defined relative to */
13030 /* an observer at N1 facing N2. */
13031 
13032 
13033 /* On input: */
13034 
13035 /*       X1,Y1,Z1 = Coordinates of N1. */
13036 
13037 /*       X2,Y2,Z2 = Coordinates of N2. */
13038 
13039 /*       X0,Y0,Z0 = Coordinates of N0. */
13040 
13041 /* Input parameters are not altered by this function. */
13042 
13043 /* On output: */
13044 
13045 /*       LEFT = TRUE if and only if N0 is in the closed */
13046 /*              left hemisphere. */
13047 
13048 /* Modules required by LEFT:  None */
13049 
13050 /* *********************************************************** */
13051 
13052 /* LEFT = TRUE iff <N0,N1 X N2> = det(N0,N1,N2) .GE. 0. */
13053 
13054     ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13055             *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13056 
13057 
13058     return ret_val;
13059 } /* left_ */
13060 
13061 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13062 {
13063     /* System generated locals */
13064     int ret_val;
13065 
13066     /* Local variables */
13067     static int nd, lp;
13068 
13069 
13070 /* *********************************************************** */
13071 
13072 /*                                              From STRIPACK */
13073 /*                                            Robert J. Renka */
13074 /*                                  Dept. of Computer Science */
13075 /*                                       Univ. of North Texas */
13076 /*                                           renka@cs.unt.edu */
13077 /*                                                   07/15/96 */
13078 
13079 /*   This function returns the index (LIST pointer) of NB in */
13080 /* the adjacency list for N0, where LPL = LEND(N0). */
13081 
13082 /*   This function is identical to the similarly named */
13083 /* function in TRIPACK. */
13084 
13085 
13086 /* On input: */
13087 
13088 /*       LPL = LEND(N0) */
13089 
13090 /*       NB = Index of the node whose pointer is to be re- */
13091 /*            turned.  NB must be connected to N0. */
13092 
13093 /*       LIST,LPTR = Data structure defining the triangula- */
13094 /*                   tion.  Refer to Subroutine TRMESH. */
13095 
13096 /* Input parameters are not altered by this function. */
13097 
13098 /* On output: */
13099 
13100 /*       LSTPTR = Pointer such that LIST(LSTPTR) = NB or */
13101 /*                LIST(LSTPTR) = -NB, unless NB is not a */
13102 /*                neighbor of N0, in which case LSTPTR = LPL. */
13103 
13104 /* Modules required by LSTPTR:  None */
13105 
13106 /* *********************************************************** */
13107 
13108 
13109 /* Local parameters: */
13110 
13111 /* LP = LIST pointer */
13112 /* ND = Nodal index */
13113 
13114     /* Parameter adjustments */
13115     --lptr;
13116     --list;
13117 
13118     /* Function Body */
13119     lp = lptr[*lpl];
13120 L1:
13121     nd = list[lp];
13122     if (nd == *nb) {
13123         goto L2;
13124     }
13125     lp = lptr[lp];
13126     if (lp != *lpl) {
13127         goto L1;
13128     }
13129 
13130 L2:
13131     ret_val = lp;
13132     return ret_val;
13133 } /* lstptr_ */
13134 
13135 int nbcnt_(int *lpl, int *lptr)
13136 {
13137     /* System generated locals */
13138     int ret_val;
13139 
13140     /* Local variables */
13141     static int k, lp;
13142 
13143 
13144 /* *********************************************************** */
13145 
13146 /*                                              From STRIPACK */
13147 /*                                            Robert J. Renka */
13148 /*                                  Dept. of Computer Science */
13149 /*                                       Univ. of North Texas */
13150 /*                                           renka@cs.unt.edu */
13151 /*                                                   07/15/96 */
13152 
13153 /*   This function returns the number of neighbors of a node */
13154 /* N0 in a triangulation created by Subroutine TRMESH. */
13155 
13156 /*   This function is identical to the similarly named */
13157 /* function in TRIPACK. */
13158 
13159 
13160 /* On input: */
13161 
13162 /*       LPL = LIST pointer to the last neighbor of N0 -- */
13163 /*             LPL = LEND(N0). */
13164 
13165 /*       LPTR = Array of pointers associated with LIST. */
13166 
13167 /* Input parameters are not altered by this function. */
13168 
13169 /* On output: */
13170 
13171 /*       NBCNT = Number of neighbors of N0. */
13172 
13173 /* Modules required by NBCNT:  None */
13174 
13175 /* *********************************************************** */
13176 
13177 
13178 /* Local parameters: */
13179 
13180 /* K =  Counter for computing the number of neighbors */
13181 /* LP = LIST pointer */
13182 
13183     /* Parameter adjustments */
13184     --lptr;
13185 
13186     /* Function Body */
13187     lp = *lpl;
13188     k = 1;
13189 
13190 L1:
13191     lp = lptr[lp];
13192     if (lp == *lpl) {
13193         goto L2;
13194     }
13195     ++k;
13196     goto L1;
13197 
13198 L2:
13199     ret_val = k;
13200     return ret_val;
13201 } /* nbcnt_ */
13202 
13203 int nearnd_(double *p, int *ist, int *n, double *x,
13204         double *y, double *z__, int *list, int *lptr, int
13205         *lend, double *al)
13206 {
13207     /* System generated locals */
13208     int ret_val, i__1;
13209 
13210     /* Builtin functions */
13211     //double acos(double);
13212 
13213     /* Local variables */
13214     static int l;
13215     static double b1, b2, b3;
13216     static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13217     static double ds1;
13218     static int lp1, lp2;
13219     static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13220     static int lpl;
13221     static double dsr;
13222     static int nst, listp[25], lptrp[25];
13223     extern /* Subroutine */ int trfind_(int *, double *, int *,
13224             double *, double *, double *, int *, int *,
13225             int *, double *, double *, double *, int *,
13226             int *, int *);
13227     extern int lstptr_(int *, int *, int *, int *);
13228 
13229 
13230 /* *********************************************************** */
13231 
13232 /*                                              From STRIPACK */
13233 /*                                            Robert J. Renka */
13234 /*                                  Dept. of Computer Science */
13235 /*                                       Univ. of North Texas */
13236 /*                                           renka@cs.unt.edu */
13237 /*                                                   07/28/98 */
13238 
13239 /*   Given a point P on the surface of the unit sphere and a */
13240 /* Delaunay triangulation created by Subroutine TRMESH, this */
13241 /* function returns the index of the nearest triangulation */
13242 /* node to P. */
13243 
13244 /*   The algorithm consists of implicitly adding P to the */
13245 /* triangulation, finding the nearest neighbor to P, and */
13246 /* implicitly deleting P from the triangulation.  Thus, it */
13247 /* is based on the fact that, if P is a node in a Delaunay */
13248 /* triangulation, the nearest node to P is a neighbor of P. */
13249 
13250 
13251 /* On input: */
13252 
13253 /*       P = Array of length 3 containing the Cartesian coor- */
13254 /*           dinates of the point P to be located relative to */
13255 /*           the triangulation.  It is assumed without a test */
13256 /*           that P(1)**2 + P(2)**2 + P(3)**2 = 1. */
13257 
13258 /*       IST = Index of a node at which TRFIND begins the */
13259 /*             search.  Search time depends on the proximity */
13260 /*             of this node to P. */
13261 
13262 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
13263 
13264 /*       X,Y,Z = Arrays of length N containing the Cartesian */
13265 /*               coordinates of the nodes. */
13266 
13267 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13268 /*                        gulation.  Refer to TRMESH. */
13269 
13270 /* Input parameters are not altered by this function. */
13271 
13272 /* On output: */
13273 
13274 /*       NEARND = Nodal index of the nearest node to P, or 0 */
13275 /*                if N < 3 or the triangulation data struc- */
13276 /*                ture is invalid. */
13277 
13278 /*       AL = Arc length (angular distance in radians) be- */
13279 /*            tween P and NEARND unless NEARND = 0. */
13280 
13281 /*       Note that the number of candidates for NEARND */
13282 /*       (neighbors of P) is limited to LMAX defined in */
13283 /*       the PARAMETER statement below. */
13284 
13285 /* Modules required by NEARND:  JRAND, LSTPTR, TRFIND, STORE */
13286 
13287 /* Intrinsic functions called by NEARND:  ABS, ACOS */
13288 
13289 /* *********************************************************** */
13290 
13291 
13292 /* Local parameters: */
13293 
13294 /* B1,B2,B3 =  Unnormalized barycentric coordinates returned */
13295 /*               by TRFIND */
13296 /* DS1 =       (Negative cosine of the) distance from P to N1 */
13297 /* DSR =       (Negative cosine of the) distance from P to NR */
13298 /* DX1,..DZ3 = Components of vectors used by the swap test */
13299 /* I1,I2,I3 =  Nodal indexes of a triangle containing P, or */
13300 /*               the rightmost (I1) and leftmost (I2) visible */
13301 /*               boundary nodes as viewed from P */
13302 /* L =         Length of LISTP/LPTRP and number of neighbors */
13303 /*               of P */
13304 /* LMAX =      Maximum value of L */
13305 /* LISTP =     Indexes of the neighbors of P */
13306 /* LPTRP =     Array of pointers in 1-1 correspondence with */
13307 /*               LISTP elements */
13308 /* LP =        LIST pointer to a neighbor of N1 and LISTP */
13309 /*               pointer */
13310 /* LP1,LP2 =   LISTP indexes (pointers) */
13311 /* LPL =       Pointer to the last neighbor of N1 */
13312 /* N1 =        Index of a node visible from P */
13313 /* N2 =        Index of an endpoint of an arc opposite P */
13314 /* N3 =        Index of the node opposite N1->N2 */
13315 /* NN =        Local copy of N */
13316 /* NR =        Index of a candidate for the nearest node to P */
13317 /* NST =       Index of the node at which TRFIND begins the */
13318 /*               search */
13319 
13320 
13321 /* Store local parameters and test for N invalid. */
13322 
13323     /* Parameter adjustments */
13324     --p;
13325     --lend;
13326     --z__;
13327     --y;
13328     --x;
13329     --list;
13330     --lptr;
13331 
13332     /* Function Body */
13333     nn = *n;
13334     if (nn < 3) {
13335         goto L6;
13336     }
13337     nst = *ist;
13338     if (nst < 1 || nst > nn) {
13339         nst = 1;
13340     }
13341 
13342 /* Find a triangle (I1,I2,I3) containing P, or the rightmost */
13343 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
13344 /*   from P. */
13345 
13346     trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13347             1], &b1, &b2, &b3, &i1, &i2, &i3);
13348 
13349 /* Test for collinear nodes. */
13350 
13351     if (i1 == 0) {
13352         goto L6;
13353     }
13354 
13355 /* Store the linked list of 'neighbors' of P in LISTP and */
13356 /*   LPTRP.  I1 is the first neighbor, and 0 is stored as */
13357 /*   the last neighbor if P is not contained in a triangle. */
13358 /*   L is the length of LISTP and LPTRP, and is limited to */
13359 /*   LMAX. */
13360 
13361     if (i3 != 0) {
13362         listp[0] = i1;
13363         lptrp[0] = 2;
13364         listp[1] = i2;
13365         lptrp[1] = 3;
13366         listp[2] = i3;
13367         lptrp[2] = 1;
13368         l = 3;
13369     } else {
13370         n1 = i1;
13371         l = 1;
13372         lp1 = 2;
13373         listp[l - 1] = n1;
13374         lptrp[l - 1] = lp1;
13375 
13376 /*   Loop on the ordered sequence of visible boundary nodes */
13377 /*     N1 from I1 to I2. */
13378 
13379 L1:
13380         lpl = lend[n1];
13381         n1 = -list[lpl];
13382         l = lp1;
13383         lp1 = l + 1;
13384         listp[l - 1] = n1;
13385         lptrp[l - 1] = lp1;
13386         if (n1 != i2 && lp1 < 25) {
13387             goto L1;
13388         }
13389         l = lp1;
13390         listp[l - 1] = 0;
13391         lptrp[l - 1] = 1;
13392     }
13393 
13394 /* Initialize variables for a loop on arcs N1-N2 opposite P */
13395 /*   in which new 'neighbors' are 'swapped' in.  N1 follows */
13396 /*   N2 as a neighbor of P, and LP1 and LP2 are the LISTP */
13397 /*   indexes of N1 and N2. */
13398 
13399     lp2 = 1;
13400     n2 = i1;
13401     lp1 = lptrp[0];
13402     n1 = listp[lp1 - 1];
13403 
13404 /* Begin loop:  find the node N3 opposite N1->N2. */
13405 
13406 L2:
13407     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13408     if (list[lp] < 0) {
13409         goto L3;
13410     }
13411     lp = lptr[lp];
13412     n3 = (i__1 = list[lp], abs(i__1));
13413 
13414 /* Swap test:  Exit the loop if L = LMAX. */
13415 
13416     if (l == 25) {
13417         goto L4;
13418     }
13419     dx1 = x[n1] - p[1];
13420     dy1 = y[n1] - p[2];
13421     dz1 = z__[n1] - p[3];
13422 
13423     dx2 = x[n2] - p[1];
13424     dy2 = y[n2] - p[2];
13425     dz2 = z__[n2] - p[3];
13426 
13427     dx3 = x[n3] - p[1];
13428     dy3 = y[n3] - p[2];
13429     dz3 = z__[n3] - p[3];
13430     if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13431             (dx2 * dy1 - dx1 * dy2) <= 0.) {
13432         goto L3;
13433     }
13434 
13435 /* Swap:  Insert N3 following N2 in the adjacency list for P. */
13436 /*        The two new arcs opposite P must be tested. */
13437 
13438     ++l;
13439     lptrp[lp2 - 1] = l;
13440     listp[l - 1] = n3;
13441     lptrp[l - 1] = lp1;
13442     lp1 = l;
13443     n1 = n3;
13444     goto L2;
13445 
13446 /* No swap:  Advance to the next arc and test for termination */
13447 /*           on N1 = I1 (LP1 = 1) or N1 followed by 0. */
13448 
13449 L3:
13450     if (lp1 == 1) {
13451         goto L4;
13452     }
13453     lp2 = lp1;
13454     n2 = n1;
13455     lp1 = lptrp[lp1 - 1];
13456     n1 = listp[lp1 - 1];
13457     if (n1 == 0) {
13458         goto L4;
13459     }
13460     goto L2;
13461 
13462 /* Set NR and DSR to the index of the nearest node to P and */
13463 /*   an increasing function (negative cosine) of its distance */
13464 /*   from P, respectively. */
13465 
13466 L4:
13467     nr = i1;
13468     dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13469     i__1 = l;
13470     for (lp = 2; lp <= i__1; ++lp) {
13471         n1 = listp[lp - 1];
13472         if (n1 == 0) {
13473             goto L5;
13474         }
13475         ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13476         if (ds1 < dsr) {
13477             nr = n1;
13478             dsr = ds1;
13479         }
13480 L5:
13481         ;
13482     }
13483     dsr = -dsr;
13484     if (dsr > 1.) {
13485         dsr = 1.;
13486     }
13487     *al = acos(dsr);
13488     ret_val = nr;
13489     return ret_val;
13490 
13491 /* Invalid input. */
13492 
13493 L6:
13494     ret_val = 0;
13495     return ret_val;
13496 } /* nearnd_ */
13497 
13498 /* Subroutine */ int optim_(double *x, double *y, double *z__,
13499         int *na, int *list, int *lptr, int *lend, int *
13500         nit, int *iwk, int *ier)
13501 {
13502     /* System generated locals */
13503     int i__1, i__2;
13504 
13505     /* Local variables */
13506     static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13507     static long int swp;
13508     static int iter;
13509     extern /* Subroutine */ int swap_(int *, int *, int *,
13510             int *, int *, int *, int *, int *);
13511     static int maxit;
13512     extern long int swptst_(int *, int *, int *, int *,
13513             double *, double *, double *);
13514 
13515 
13516 /* *********************************************************** */
13517 
13518 /*                                              From STRIPACK */
13519 /*                                            Robert J. Renka */
13520 /*                                  Dept. of Computer Science */
13521 /*                                       Univ. of North Texas */
13522 /*                                           renka@cs.unt.edu */
13523 /*                                                   07/30/98 */
13524 
13525 /*   Given a set of NA triangulation arcs, this subroutine */
13526 /* optimizes the portion of the triangulation consisting of */
13527 /* the quadrilaterals (pairs of adjacent triangles) which */
13528 /* have the arcs as diagonals by applying the circumcircle */
13529 /* test and appropriate swaps to the arcs. */
13530 
13531 /*   An iteration consists of applying the swap test and */
13532 /* swaps to all NA arcs in the order in which they are */
13533 /* stored.  The iteration is repeated until no swap occurs */
13534 /* or NIT iterations have been performed.  The bound on the */
13535 /* number of iterations may be necessary to prevent an */
13536 /* infinite loop caused by cycling (reversing the effect of a */
13537 /* previous swap) due to floating point inaccuracy when four */
13538 /* or more nodes are nearly cocircular. */
13539 
13540 
13541 /* On input: */
13542 
13543 /*       X,Y,Z = Arrays containing the nodal coordinates. */
13544 
13545 /*       NA = Number of arcs in the set.  NA .GE. 0. */
13546 
13547 /* The above parameters are not altered by this routine. */
13548 
13549 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13550 /*                        gulation.  Refer to Subroutine */
13551 /*                        TRMESH. */
13552 
13553 /*       NIT = Maximum number of iterations to be performed. */
13554 /*             NIT = 4*NA should be sufficient.  NIT .GE. 1. */
13555 
13556 /*       IWK = int array dimensioned 2 by NA containing */
13557 /*             the nodal indexes of the arc endpoints (pairs */
13558 /*             of endpoints are stored in columns). */
13559 
13560 /* On output: */
13561 
13562 /*       LIST,LPTR,LEND = Updated triangulation data struc- */
13563 /*                        ture reflecting the swaps. */
13564 
13565 /*       NIT = Number of iterations performed. */
13566 
13567 /*       IWK = Endpoint indexes of the new set of arcs */
13568 /*             reflecting the swaps. */
13569 
13570 /*       IER = Error indicator: */
13571 /*             IER = 0 if no errors were encountered. */
13572 /*             IER = 1 if a swap occurred on the last of */
13573 /*                     MAXIT iterations, where MAXIT is the */
13574 /*                     value of NIT on input.  The new set */
13575 /*                     of arcs is not necessarily optimal */
13576 /*                     in this case. */
13577 /*             IER = 2 if NA < 0 or NIT < 1 on input. */
13578 /*             IER = 3 if IWK(2,I) is not a neighbor of */
13579 /*                     IWK(1,I) for some I in the range 1 */
13580 /*                     to NA.  A swap may have occurred in */
13581 /*                     this case. */
13582 /*             IER = 4 if a zero pointer was returned by */
13583 /*                     Subroutine SWAP. */
13584 
13585 /* Modules required by OPTIM:  LSTPTR, SWAP, SWPTST */
13586 
13587 /* Intrinsic function called by OPTIM:  ABS */
13588 
13589 /* *********************************************************** */
13590 
13591 
13592 /* Local parameters: */
13593 
13594 /* I =       Column index for IWK */
13595 /* IO1,IO2 = Nodal indexes of the endpoints of an arc in IWK */
13596 /* ITER =    Iteration count */
13597 /* LP =      LIST pointer */
13598 /* LP21 =    Parameter returned by SWAP (not used) */
13599 /* LPL =     Pointer to the last neighbor of IO1 */
13600 /* LPP =     Pointer to the node preceding IO2 as a neighbor */
13601 /*             of IO1 */
13602 /* MAXIT =   Input value of NIT */
13603 /* N1,N2 =   Nodes opposite IO1->IO2 and IO2->IO1, */
13604 /*             respectively */
13605 /* NNA =     Local copy of NA */
13606 /* SWP =     Flag set to TRUE iff a swap occurs in the */
13607 /*             optimization loop */
13608 
13609     /* Parameter adjustments */
13610     --x;
13611     --y;
13612     --z__;
13613     iwk -= 3;
13614     --list;
13615     --lptr;
13616     --lend;
13617 
13618     /* Function Body */
13619     nna = *na;
13620     maxit = *nit;
13621     if (nna < 0 || maxit < 1) {
13622         goto L7;
13623     }
13624 
13625 /* Initialize iteration count ITER and test for NA = 0. */
13626 
13627     iter = 0;
13628     if (nna == 0) {
13629         goto L5;
13630     }
13631 
13632 /* Top of loop -- */
13633 /*   SWP = TRUE iff a swap occurred in the current iteration. */
13634 
13635 L1:
13636     if (iter == maxit) {
13637         goto L6;
13638     }
13639     ++iter;
13640     swp = FALSE_;
13641 
13642 /*   Inner loop on arcs IO1-IO2 -- */
13643 
13644     i__1 = nna;
13645     for (i__ = 1; i__ <= i__1; ++i__) {
13646         io1 = iwk[(i__ << 1) + 1];
13647         io2 = iwk[(i__ << 1) + 2];
13648 
13649 /*   Set N1 and N2 to the nodes opposite IO1->IO2 and */
13650 /*     IO2->IO1, respectively.  Determine the following: */
13651 
13652 /*     LPL = pointer to the last neighbor of IO1, */
13653 /*     LP = pointer to IO2 as a neighbor of IO1, and */
13654 /*     LPP = pointer to the node N2 preceding IO2. */
13655 
13656         lpl = lend[io1];
13657         lpp = lpl;
13658         lp = lptr[lpp];
13659 L2:
13660         if (list[lp] == io2) {
13661             goto L3;
13662         }
13663         lpp = lp;
13664         lp = lptr[lpp];
13665         if (lp != lpl) {
13666             goto L2;
13667         }
13668 
13669 /*   IO2 should be the last neighbor of IO1.  Test for no */
13670 /*     arc and bypass the swap test if IO1 is a boundary */
13671 /*     node. */
13672 
13673         if ((i__2 = list[lp], abs(i__2)) != io2) {
13674             goto L8;
13675         }
13676         if (list[lp] < 0) {
13677             goto L4;
13678         }
13679 
13680 /*   Store N1 and N2, or bypass the swap test if IO1 is a */
13681 /*     boundary node and IO2 is its first neighbor. */
13682 
13683 L3:
13684         n2 = list[lpp];
13685         if (n2 < 0) {
13686             goto L4;
13687         }
13688         lp = lptr[lp];
13689         n1 = (i__2 = list[lp], abs(i__2));
13690 
13691 /*   Test IO1-IO2 for a swap, and update IWK if necessary. */
13692 
13693         if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13694             goto L4;
13695         }
13696         swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13697         if (lp21 == 0) {
13698             goto L9;
13699         }
13700         swp = TRUE_;
13701         iwk[(i__ << 1) + 1] = n1;
13702         iwk[(i__ << 1) + 2] = n2;
13703 L4:
13704         ;
13705     }
13706     if (swp) {
13707         goto L1;
13708     }
13709 
13710 /* Successful termination. */
13711 
13712 L5:
13713     *nit = iter;
13714     *ier = 0;
13715     return 0;
13716 
13717 /* MAXIT iterations performed without convergence. */
13718 
13719 L6:
13720     *nit = maxit;
13721     *ier = 1;
13722     return 0;
13723 
13724 /* Invalid input parameter. */
13725 
13726 L7:
13727     *nit = 0;
13728     *ier = 2;
13729     return 0;
13730 
13731 /* IO2 is not a neighbor of IO1. */
13732 
13733 L8:
13734     *nit = iter;
13735     *ier = 3;
13736     return 0;
13737 
13738 /* Zero pointer returned by SWAP. */
13739 
13740 L9:
13741     *nit = iter;
13742     *ier = 4;
13743     return 0;
13744 } /* optim_ */
13745 
13746 /* Subroutine */ int projct_(double *px, double *py, double *pz,
13747         double *ox, double *oy, double *oz, double *ex,
13748         double *ey, double *ez, double *vx, double *vy,
13749         double *vz, long int *init, double *x, double *y,
13750         double *z__, int *ier)
13751 {
13752     /* Builtin functions */
13753     //double sqrt(double);
13754 
13755     /* Local variables */
13756     static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13757             oes, xoe, yoe, zoe, xep, yep, zep;
13758 
13759 
13760 /* *********************************************************** */
13761 
13762 /*                        From PLTPACK, SCRPLOT, and STRIPACK */
13763 /*                                            Robert J. Renka */
13764 /*                                  Dept. of Computer Science */
13765 /*                                       Univ. of North Texas */
13766 /*                                           renka@cs.unt.edu */
13767 /*                                                   07/18/90 */
13768 
13769 /*   Given a projection plane and associated coordinate sys- */
13770 /* tem defined by an origin O, eye position E, and up-vector */
13771 /* V, this subroutine applies a perspective depth transform- */
13772 /* ation T to a point P = (PX,PY,PZ), returning the point */
13773 /* T(P) = (X,Y,Z), where X and Y are the projection plane */
13774 /* coordinates of the point that lies in the projection */
13775 /* plane and on the line defined by P and E, and Z is the */
13776 /* depth associated with P. */
13777 
13778 /*   The projection plane is defined to be the plane that */
13779 /* contains O and has normal defined by O and E. */
13780 
13781 /*   The depth Z is defined in such a way that Z < 1, T maps */
13782 /* lines to lines (and planes to planes), and if two distinct */
13783 /* points have the same projection plane coordinates, then */
13784 /* the one closer to E has a smaller depth.  (Z increases */
13785 /* monotonically with orthogonal distance from P to the plane */
13786 /* that is parallel to the projection plane and contains E.) */
13787 /* This depth value facilitates depth sorting and depth buf- */
13788 /* fer methods. */
13789 
13790 
13791 /* On input: */
13792 
13793 /*       PX,PY,PZ = Cartesian coordinates of the point P to */
13794 /*                  be mapped onto the projection plane.  The */
13795 /*                  half line that contains P and has end- */
13796 /*                  point at E must intersect the plane. */
13797 
13798 /*       OX,OY,OZ = Coordinates of O (the origin of a coordi- */
13799 /*                  nate system in the projection plane).  A */
13800 /*                  reasonable value for O is a point near */
13801 /*                  the center of an object or scene to be */
13802 /*                  viewed. */
13803 
13804 /*       EX,EY,EZ = Coordinates of the eye-position E defin- */
13805 /*                  ing the normal to the plane and the line */
13806 /*                  of sight for the projection.  E must not */
13807 /*                  coincide with O or P, and the angle be- */
13808 /*                  tween the vectors O-E and P-E must be */
13809 /*                  less than 90 degrees.  Note that E and P */
13810 /*                  may lie on opposite sides of the projec- */
13811 /*                  tion plane. */
13812 
13813 /*       VX,VY,VZ = Coordinates of a point V which defines */
13814 /*                  the positive Y axis of an X-Y coordinate */
13815 /*                  system in the projection plane as the */
13816 /*                  half-line containing O and the projection */
13817 /*                  of O+V onto the plane.  The positive X */
13818 /*                  axis has direction defined by the cross */
13819 /*                  product V X (E-O). */
13820 
13821 /* The above parameters are not altered by this routine. */
13822 
13823 /*       INIT = long int switch which must be set to TRUE on */
13824 /*              the first call and when the values of O, E, */
13825 /*              or V have been altered since a previous call. */
13826 /*              If INIT = FALSE, it is assumed that only the */
13827 /*              coordinates of P have changed since a previ- */
13828 /*              ous call.  Previously stored quantities are */
13829 /*              used for increased efficiency in this case. */
13830 
13831 /* On output: */
13832 
13833 /*       INIT = Switch with value reset to FALSE if IER = 0. */
13834 
13835 /*       X,Y = Projection plane coordinates of the point */
13836 /*             that lies in the projection plane and on the */
13837 /*             line defined by E and P.  X and Y are not */
13838 /*             altered if IER .NE. 0. */
13839 
13840 /*       Z = Depth value defined above unless IER .NE. 0. */
13841 
13842 /*       IER = Error indicator. */
13843 /*             IER = 0 if no errors were encountered. */
13844 /*             IER = 1 if the inner product of O-E with P-E */
13845 /*                     is not positive, implying that E is */
13846 /*                     too close to the plane. */
13847 /*             IER = 2 if O, E, and O+V are collinear.  See */
13848 /*                     the description of VX,VY,VZ. */
13849 
13850 /* Modules required by PROJCT:  None */
13851 
13852 /* Intrinsic function called by PROJCT:  SQRT */
13853 
13854 /* *********************************************************** */
13855 
13856 
13857 /* Local parameters: */
13858 
13859 /* OES =         Norm squared of OE -- inner product (OE,OE) */
13860 /* S =           Scale factor for computing projections */
13861 /* SC =          Scale factor for normalizing VN and HN */
13862 /* XE,YE,ZE =    Local copies of EX, EY, EZ */
13863 /* XEP,YEP,ZEP = Components of the vector EP from E to P */
13864 /* XH,YH,ZH =    Components of a unit vector HN defining the */
13865 /*                 positive X-axis in the plane */
13866 /* XOE,YOE,ZOE = Components of the vector OE from O to E */
13867 /* XV,YV,ZV =    Components of a unit vector VN defining the */
13868 /*                 positive Y-axis in the plane */
13869 /* XW,YW,ZW =    Components of the vector W from O to the */
13870 /*                 projection of P onto the plane */
13871 
13872     if (*init) {
13873 
13874 /* Compute parameters defining the transformation: */
13875 /*   17 adds, 27 multiplies, 3 divides, 2 compares, and */
13876 /*   2 square roots. */
13877 
13878 /* Set the coordinates of E to local variables, compute */
13879 /*   OE = E-O and OES, and test for OE = 0. */
13880 
13881         xe = *ex;
13882         ye = *ey;
13883         ze = *ez;
13884         xoe = xe - *ox;
13885         yoe = ye - *oy;
13886         zoe = ze - *oz;
13887         oes = xoe * xoe + yoe * yoe + zoe * zoe;
13888         if (oes == 0.) {
13889             goto L1;
13890         }
13891 
13892 /* Compute S = (OE,V)/OES and VN = V - S*OE. */
13893 
13894         s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
13895         xv = *vx - s * xoe;
13896         yv = *vy - s * yoe;
13897         zv = *vz - s * zoe;
13898 
13899 /* Normalize VN to a unit vector. */
13900 
13901         sc = xv * xv + yv * yv + zv * zv;
13902         if (sc == 0.) {
13903             goto L2;
13904         }
13905         sc = 1. / sqrt(sc);
13906         xv = sc * xv;
13907         yv = sc * yv;
13908         zv = sc * zv;
13909 
13910 /* Compute HN = VN X OE (normalized). */
13911 
13912         xh = yv * zoe - yoe * zv;
13913         yh = xoe * zv - xv * zoe;
13914         zh = xv * yoe - xoe * yv;
13915         sc = sqrt(xh * xh + yh * yh + zh * zh);
13916         if (sc == 0.) {
13917             goto L2;
13918         }
13919         sc = 1. / sc;
13920         xh = sc * xh;
13921         yh = sc * yh;
13922         zh = sc * zh;
13923     }
13924 
13925 /* Apply the transformation:  13 adds, 12 multiplies, */
13926 /*                            1 divide, and 1 compare. */
13927 
13928 /* Compute EP = P-E, S = OES/(OE,EP), and W = OE - S*EP. */
13929 
13930     xep = *px - xe;
13931     yep = *py - ye;
13932     zep = *pz - ze;
13933     s = xoe * xep + yoe * yep + zoe * zep;
13934     if (s >= 0.) {
13935         goto L1;
13936     }
13937     s = oes / s;
13938     xw = xoe - s * xep;
13939     yw = yoe - s * yep;
13940     zw = zoe - s * zep;
13941 
13942 /* Map W into X = (W,HN), Y = (W,VN), compute Z = 1+S, and */
13943 /*   reset INIT. */
13944 
13945     *x = xw * xh + yw * yh + zw * zh;
13946     *y = xw * xv + yw * yv + zw * zv;
13947     *z__ = s + 1.;
13948     *init = FALSE_;
13949     *ier = 0;
13950     return 0;
13951 
13952 /* (OE,EP) .GE. 0. */
13953 
13954 L1:
13955     *ier = 1;
13956     return 0;
13957 
13958 /* O, E, and O+V are collinear. */
13959 
13960 L2:
13961     *ier = 2;
13962     return 0;
13963 } /* projct_ */
13964 
13965 /* Subroutine */ int scoord_(double *px, double *py, double *pz,
13966         double *plat, double *plon, double *pnrm)
13967 {
13968     /* Builtin functions */
13969     //double sqrt(double), atan2(double, double), asin(double);
13970 
13971 
13972 /* *********************************************************** */
13973 
13974 /*                                              From STRIPACK */
13975 /*                                            Robert J. Renka */
13976 /*                                  Dept. of Computer Science */
13977 /*                                       Univ. of North Texas */
13978 /*                                           renka@cs.unt.edu */
13979 /*                                                   08/27/90 */
13980 
13981 /*   This subroutine converts a point P from Cartesian coor- */
13982 /* dinates to spherical coordinates. */
13983 
13984 
13985 /* On input: */
13986 
13987 /*       PX,PY,PZ = Cartesian coordinates of P. */
13988 
13989 /* Input parameters are not altered by this routine. */
13990 
13991 /* On output: */
13992 
13993 /*       PLAT = Latitude of P in the range -PI/2 to PI/2, or */
13994 /*              0 if PNRM = 0.  PLAT should be scaled by */
13995 /*              180/PI to obtain the value in degrees. */
13996 
13997 /*       PLON = Longitude of P in the range -PI to PI, or 0 */
13998 /*              if P lies on the Z-axis.  PLON should be */
13999 /*              scaled by 180/PI to obtain the value in */
14000 /*              degrees. */
14001 
14002 /*       PNRM = Magnitude (Euclidean norm) of P. */
14003 
14004 /* Modules required by SCOORD:  None */
14005 
14006 /* Intrinsic functions called by SCOORD:  ASIN, ATAN2, SQRT */
14007 
14008 /* *********************************************************** */
14009 
14010     *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14011     if (*px != 0. || *py != 0.) {
14012         *plon = atan2(*py, *px);
14013     } else {
14014         *plon = 0.;
14015     }
14016     if (*pnrm != 0.) {
14017         *plat = asin(*pz / *pnrm);
14018     } else {
14019         *plat = 0.;
14020     }
14021     return 0;
14022 } /* scoord_ */
14023 
14024 double store_(double *x)
14025 {
14026     /* System generated locals */
14027     double ret_val;
14028 
14029 
14030 /* *********************************************************** */
14031 
14032 /*                                              From STRIPACK */
14033 /*                                            Robert J. Renka */
14034 /*                                  Dept. of Computer Science */
14035 /*                                       Univ. of North Texas */
14036 /*                                           renka@cs.unt.edu */
14037 /*                                                   05/09/92 */
14038 
14039 /*   This function forces its argument X to be stored in a */
14040 /* memory location, thus providing a means of determining */
14041 /* floating point number characteristics (such as the machine */
14042 /* precision) when it is necessary to avoid computation in */
14043 /* high precision registers. */
14044 
14045 
14046 /* On input: */
14047 
14048 /*       X = Value to be stored. */
14049 
14050 /* X is not altered by this function. */
14051 
14052 /* On output: */
14053 
14054 /*       STORE = Value of X after it has been stored and */
14055 /*               possibly truncated or rounded to the single */
14056 /*               precision word length. */
14057 
14058 /* Modules required by STORE:  None */
14059 
14060 /* *********************************************************** */
14061 
14062     stcom_1.y = *x;
14063     ret_val = stcom_1.y;
14064     return ret_val;
14065 } /* store_ */
14066 
14067 /* Subroutine */ int swap_(int *in1, int *in2, int *io1, int *
14068         io2, int *list, int *lptr, int *lend, int *lp21)
14069 {
14070     /* System generated locals */
14071     int i__1;
14072 
14073     /* Local variables */
14074     static int lp, lph, lpsav;
14075     extern int lstptr_(int *, int *, int *, int *);
14076 
14077 
14078 /* *********************************************************** */
14079 
14080 /*                                              From STRIPACK */
14081 /*                                            Robert J. Renka */
14082 /*                                  Dept. of Computer Science */
14083 /*                                       Univ. of North Texas */
14084 /*                                           renka@cs.unt.edu */
14085 /*                                                   06/22/98 */
14086 
14087 /*   Given a triangulation of a set of points on the unit */
14088 /* sphere, this subroutine replaces a diagonal arc in a */
14089 /* strictly convex quadrilateral (defined by a pair of adja- */
14090 /* cent triangles) with the other diagonal.  Equivalently, a */
14091 /* pair of adjacent triangles is replaced by another pair */
14092 /* having the same union. */
14093 
14094 
14095 /* On input: */
14096 
14097 /*       IN1,IN2,IO1,IO2 = Nodal indexes of the vertices of */
14098 /*                         the quadrilateral.  IO1-IO2 is re- */
14099 /*                         placed by IN1-IN2.  (IO1,IO2,IN1) */
14100 /*                         and (IO2,IO1,IN2) must be trian- */
14101 /*                         gles on input. */
14102 
14103 /* The above parameters are not altered by this routine. */
14104 
14105 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14106 /*                        gulation.  Refer to Subroutine */
14107 /*                        TRMESH. */
14108 
14109 /* On output: */
14110 
14111 /*       LIST,LPTR,LEND = Data structure updated with the */
14112 /*                        swap -- triangles (IO1,IO2,IN1) and */
14113 /*                        (IO2,IO1,IN2) are replaced by */
14114 /*                        (IN1,IN2,IO2) and (IN2,IN1,IO1) */
14115 /*                        unless LP21 = 0. */
14116 
14117 /*       LP21 = Index of IN1 as a neighbor of IN2 after the */
14118 /*              swap is performed unless IN1 and IN2 are */
14119 /*              adjacent on input, in which case LP21 = 0. */
14120 
14121 /* Module required by SWAP:  LSTPTR */
14122 
14123 /* Intrinsic function called by SWAP:  ABS */
14124 
14125 /* *********************************************************** */
14126 
14127 
14128 /* Local parameters: */
14129 
14130 /* LP,LPH,LPSAV = LIST pointers */
14131 
14132 
14133 /* Test for IN1 and IN2 adjacent. */
14134 
14135     /* Parameter adjustments */
14136     --lend;
14137     --lptr;
14138     --list;
14139 
14140     /* Function Body */
14141     lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14142     if ((i__1 = list[lp], abs(i__1)) == *in2) {
14143         *lp21 = 0;
14144         return 0;
14145     }
14146 
14147 /* Delete IO2 as a neighbor of IO1. */
14148 
14149     lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14150     lph = lptr[lp];
14151     lptr[lp] = lptr[lph];
14152 
14153 /* If IO2 is the last neighbor of IO1, make IN2 the */
14154 /*   last neighbor. */
14155 
14156     if (lend[*io1] == lph) {
14157         lend[*io1] = lp;
14158     }
14159 
14160 /* Insert IN2 as a neighbor of IN1 following IO1 */
14161 /*   using the hole created above. */
14162 
14163     lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14164     lpsav = lptr[lp];
14165     lptr[lp] = lph;
14166     list[lph] = *in2;
14167     lptr[lph] = lpsav;
14168 
14169 /* Delete IO1 as a neighbor of IO2. */
14170 
14171     lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14172     lph = lptr[lp];
14173     lptr[lp] = lptr[lph];
14174 
14175 /* If IO1 is the last neighbor of IO2, make IN1 the */
14176 /*   last neighbor. */
14177 
14178     if (lend[*io2] == lph) {
14179         lend[*io2] = lp;
14180     }
14181 
14182 /* Insert IN1 as a neighbor of IN2 following IO2. */
14183 
14184     lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14185     lpsav = lptr[lp];
14186     lptr[lp] = lph;
14187     list[lph] = *in1;
14188     lptr[lph] = lpsav;
14189     *lp21 = lph;
14190     return 0;
14191 } /* swap_ */
14192 
14193 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14194         double *x, double *y, double *z__)
14195 {
14196     /* System generated locals */
14197     long int ret_val;
14198 
14199     /* Local variables */
14200     static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14201 
14202 
14203 /* *********************************************************** */
14204 
14205 /*                                              From STRIPACK */
14206 /*                                            Robert J. Renka */
14207 /*                                  Dept. of Computer Science */
14208 /*                                       Univ. of North Texas */
14209 /*                                           renka@cs.unt.edu */
14210 /*                                                   03/29/91 */
14211 
14212 /*   This function decides whether or not to replace a */
14213 /* diagonal arc in a quadrilateral with the other diagonal. */
14214 /* The decision will be to swap (SWPTST = TRUE) if and only */
14215 /* if N4 lies above the plane (in the half-space not contain- */
14216 /* ing the origin) defined by (N1,N2,N3), or equivalently, if */
14217 /* the projection of N4 onto this plane is interior to the */
14218 /* circumcircle of (N1,N2,N3).  The decision will be for no */
14219 /* swap if the quadrilateral is not strictly convex. */
14220 
14221 
14222 /* On input: */
14223 
14224 /*       N1,N2,N3,N4 = Indexes of the four nodes defining the */
14225 /*                     quadrilateral with N1 adjacent to N2, */
14226 /*                     and (N1,N2,N3) in counterclockwise */
14227 /*                     order.  The arc connecting N1 to N2 */
14228 /*                     should be replaced by an arc connec- */
14229 /*                     ting N3 to N4 if SWPTST = TRUE.  Refer */
14230 /*                     to Subroutine SWAP. */
14231 
14232 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14233 /*               coordinates of the nodes.  (X(I),Y(I),Z(I)) */
14234 /*               define node I for I = N1, N2, N3, and N4. */
14235 
14236 /* Input parameters are not altered by this routine. */
14237 
14238 /* On output: */
14239 
14240 /*       SWPTST = TRUE if and only if the arc connecting N1 */
14241 /*                and N2 should be swapped for an arc con- */
14242 /*                necting N3 and N4. */
14243 
14244 /* Modules required by SWPTST:  None */
14245 
14246 /* *********************************************************** */
14247 
14248 
14249 /* Local parameters: */
14250 
14251 /* DX1,DY1,DZ1 = Coordinates of N4->N1 */
14252 /* DX2,DY2,DZ2 = Coordinates of N4->N2 */
14253 /* DX3,DY3,DZ3 = Coordinates of N4->N3 */
14254 /* X4,Y4,Z4 =    Coordinates of N4 */
14255 
14256     /* Parameter adjustments */
14257     --z__;
14258     --y;
14259     --x;
14260 
14261     /* Function Body */
14262     x4 = x[*n4];
14263     y4 = y[*n4];
14264     z4 = z__[*n4];
14265     dx1 = x[*n1] - x4;
14266     dx2 = x[*n2] - x4;
14267     dx3 = x[*n3] - x4;
14268     dy1 = y[*n1] - y4;
14269     dy2 = y[*n2] - y4;
14270     dy3 = y[*n3] - y4;
14271     dz1 = z__[*n1] - z4;
14272     dz2 = z__[*n2] - z4;
14273     dz3 = z__[*n3] - z4;
14274 
14275 /* N4 lies above the plane of (N1,N2,N3) iff N3 lies above */
14276 /*   the plane of (N2,N1,N4) iff Det(N3-N4,N2-N4,N1-N4) = */
14277 /*   (N3-N4,N2-N4 X N1-N4) > 0. */
14278 
14279     ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14280             dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14281     return ret_val;
14282 } /* swptst_ */
14283 
14284 /* Subroutine */ int trans_(int *n, double *rlat, double *rlon,
14285         double *x, double *y, double *z__)
14286 {
14287     /* System generated locals */
14288     int i__1;
14289 
14290     /* Builtin functions */
14291     //double cos(double), sin(double);
14292 
14293     /* Local variables */
14294     static int i__, nn;
14295     static double phi, theta, cosphi;
14296 
14297 
14298 /* *********************************************************** */
14299 
14300 /*                                              From STRIPACK */
14301 /*                                            Robert J. Renka */
14302 /*                                  Dept. of Computer Science */
14303 /*                                       Univ. of North Texas */
14304 /*                                           renka@cs.unt.edu */
14305 /*                                                   04/08/90 */
14306 
14307 /*   This subroutine transforms spherical coordinates into */
14308 /* Cartesian coordinates on the unit sphere for input to */
14309 /* Subroutine TRMESH.  Storage for X and Y may coincide with */
14310 /* storage for RLAT and RLON if the latter need not be saved. */
14311 
14312 
14313 /* On input: */
14314 
14315 /*       N = Number of nodes (points on the unit sphere) */
14316 /*           whose coordinates are to be transformed. */
14317 
14318 /*       RLAT = Array of length N containing latitudinal */
14319 /*              coordinates of the nodes in radians. */
14320 
14321 /*       RLON = Array of length N containing longitudinal */
14322 /*              coordinates of the nodes in radians. */
14323 
14324 /* The above parameters are not altered by this routine. */
14325 
14326 /*       X,Y,Z = Arrays of length at least N. */
14327 
14328 /* On output: */
14329 
14330 /*       X,Y,Z = Cartesian coordinates in the range -1 to 1. */
14331 /*               X(I)**2 + Y(I)**2 + Z(I)**2 = 1 for I = 1 */
14332 /*               to N. */
14333 
14334 /* Modules required by TRANS:  None */
14335 
14336 /* Intrinsic functions called by TRANS:  COS, SIN */
14337 
14338 /* *********************************************************** */
14339 
14340 
14341 /* Local parameters: */
14342 
14343 /* COSPHI = cos(PHI) */
14344 /* I =      DO-loop index */
14345 /* NN =     Local copy of N */
14346 /* PHI =    Latitude */
14347 /* THETA =  Longitude */
14348 
14349     /* Parameter adjustments */
14350     --z__;
14351     --y;
14352     --x;
14353     --rlon;
14354     --rlat;
14355 
14356     /* Function Body */
14357     nn = *n;
14358     i__1 = nn;
14359     for (i__ = 1; i__ <= i__1; ++i__) {
14360         phi = rlat[i__];
14361         theta = rlon[i__];
14362         cosphi = cos(phi);
14363         x[i__] = cosphi * cos(theta);
14364         y[i__] = cosphi * sin(theta);
14365         z__[i__] = sin(phi);
14366 /* L1: */
14367     }
14368     return 0;
14369 } /* trans_ */
14370 
14371 /* Subroutine */ int trfind_(int *nst, double *p, int *n,
14372         double *x, double *y, double *z__, int *list, int
14373         *lptr, int *lend, double *b1, double *b2, double *b3,
14374         int *i1, int *i2, int *i3)
14375 {
14376     /* Initialized data */
14377 
14378     static int ix = 1;
14379     static int iy = 2;
14380     static int iz = 3;
14381 
14382     /* System generated locals */
14383     int i__1;
14384     double d__1, d__2;
14385 
14386     /* Local variables */
14387     static double q[3];
14388     static int n0, n1, n2, n3, n4, nf;
14389     static double s12;
14390     static int nl, lp;
14391     static double xp, yp, zp;
14392     static int n1s, n2s;
14393     static double eps, tol, ptn1, ptn2;
14394     static int next;
14395     extern int jrand_(int *, int *, int *, int *);
14396     extern double store_(double *);
14397     extern int lstptr_(int *, int *, int *, int *);
14398 
14399 
14400 /* *********************************************************** */
14401 
14402 /*                                              From STRIPACK */
14403 /*                                            Robert J. Renka */
14404 /*                                  Dept. of Computer Science */
14405 /*                                       Univ. of North Texas */
14406 /*                                           renka@cs.unt.edu */
14407 /*                                                   11/30/99 */
14408 
14409 /*   This subroutine locates a point P relative to a triangu- */
14410 /* lation created by Subroutine TRMESH.  If P is contained in */
14411 /* a triangle, the three vertex indexes and barycentric coor- */
14412 /* dinates are returned.  Otherwise, the indexes of the */
14413 /* visible boundary nodes are returned. */
14414 
14415 
14416 /* On input: */
14417 
14418 /*       NST = Index of a node at which TRFIND begins its */
14419 /*             search.  Search time depends on the proximity */
14420 /*             of this node to P. */
14421 
14422 /*       P = Array of length 3 containing the x, y, and z */
14423 /*           coordinates (in that order) of the point P to be */
14424 /*           located. */
14425 
14426 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14427 
14428 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14429 /*               coordinates of the triangulation nodes (unit */
14430 /*               vectors).  (X(I),Y(I),Z(I)) defines node I */
14431 /*               for I = 1 to N. */
14432 
14433 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14434 /*                        gulation.  Refer to Subroutine */
14435 /*                        TRMESH. */
14436 
14437 /* Input parameters are not altered by this routine. */
14438 
14439 /* On output: */
14440 
14441 /*       B1,B2,B3 = Unnormalized barycentric coordinates of */
14442 /*                  the central projection of P onto the un- */
14443 /*                  derlying planar triangle if P is in the */
14444 /*                  convex hull of the nodes.  These parame- */
14445 /*                  ters are not altered if I1 = 0. */
14446 
14447 /*       I1,I2,I3 = Counterclockwise-ordered vertex indexes */
14448 /*                  of a triangle containing P if P is con- */
14449 /*                  tained in a triangle.  If P is not in the */
14450 /*                  convex hull of the nodes, I1 and I2 are */
14451 /*                  the rightmost and leftmost (boundary) */
14452 /*                  nodes that are visible from P, and */
14453 /*                  I3 = 0.  (If all boundary nodes are vis- */
14454 /*                  ible from P, then I1 and I2 coincide.) */
14455 /*                  I1 = I2 = I3 = 0 if P and all of the */
14456 /*                  nodes are coplanar (lie on a common great */
14457 /*                  circle. */
14458 
14459 /* Modules required by TRFIND:  JRAND, LSTPTR, STORE */
14460 
14461 /* Intrinsic function called by TRFIND:  ABS */
14462 
14463 /* *********************************************************** */
14464 
14465 
14466     /* Parameter adjustments */
14467     --p;
14468     --lend;
14469     --z__;
14470     --y;
14471     --x;
14472     --list;
14473     --lptr;
14474 
14475     /* Function Body */
14476 
14477 /* Local parameters: */
14478 
14479 /* EPS =      Machine precision */
14480 /* IX,IY,IZ = int seeds for JRAND */
14481 /* LP =       LIST pointer */
14482 /* N0,N1,N2 = Nodes in counterclockwise order defining a */
14483 /*              cone (with vertex N0) containing P, or end- */
14484 /*              points of a boundary edge such that P Right */
14485 /*              N1->N2 */
14486 /* N1S,N2S =  Initially-determined values of N1 and N2 */
14487 /* N3,N4 =    Nodes opposite N1->N2 and N2->N1, respectively */
14488 /* NEXT =     Candidate for I1 or I2 when P is exterior */
14489 /* NF,NL =    First and last neighbors of N0, or first */
14490 /*              (rightmost) and last (leftmost) nodes */
14491 /*              visible from P when P is exterior to the */
14492 /*              triangulation */
14493 /* PTN1 =     Scalar product <P,N1> */
14494 /* PTN2 =     Scalar product <P,N2> */
14495 /* Q =        (N2 X N1) X N2  or  N1 X (N2 X N1) -- used in */
14496 /*              the boundary traversal when P is exterior */
14497 /* S12 =      Scalar product <N1,N2> */
14498 /* TOL =      Tolerance (multiple of EPS) defining an upper */
14499 /*              bound on the magnitude of a negative bary- */
14500 /*              centric coordinate (B1 or B2) for P in a */
14501 /*              triangle -- used to avoid an infinite number */
14502 /*              of restarts with 0 <= B3 < EPS and B1 < 0 or */
14503 /*              B2 < 0 but small in magnitude */
14504 /* XP,YP,ZP = Local variables containing P(1), P(2), and P(3) */
14505 /* X0,Y0,Z0 = Dummy arguments for DET */
14506 /* X1,Y1,Z1 = Dummy arguments for DET */
14507 /* X2,Y2,Z2 = Dummy arguments for DET */
14508 
14509 /* Statement function: */
14510 
14511 /* DET(X1,...,Z0) .GE. 0 if and only if (X0,Y0,Z0) is in the */
14512 /*                       (closed) left hemisphere defined by */
14513 /*                       the plane containing (0,0,0), */
14514 /*                       (X1,Y1,Z1), and (X2,Y2,Z2), where */
14515 /*                       left is defined relative to an ob- */
14516 /*                       server at (X1,Y1,Z1) facing */
14517 /*                       (X2,Y2,Z2). */
14518 
14519 
14520 /* Initialize variables. */
14521 
14522     xp = p[1];
14523     yp = p[2];
14524     zp = p[3];
14525     n0 = *nst;
14526     if (n0 < 1 || n0 > *n) {
14527         n0 = jrand_(n, &ix, &iy, &iz);
14528     }
14529 
14530 /* Compute the relative machine precision EPS and TOL. */
14531 
14532     eps = 1.;
14533 L1:
14534     eps /= 2.;
14535     d__1 = eps + 1.;
14536     if (store_(&d__1) > 1.) {
14537         goto L1;
14538     }
14539     eps *= 2.;
14540     tol = eps * 4.;
14541 
14542 /* Set NF and NL to the first and last neighbors of N0, and */
14543 /*   initialize N1 = NF. */
14544 
14545 L2:
14546     lp = lend[n0];
14547     nl = list[lp];
14548     lp = lptr[lp];
14549     nf = list[lp];
14550     n1 = nf;
14551 
14552 /* Find a pair of adjacent neighbors N1,N2 of N0 that define */
14553 /*   a wedge containing P:  P LEFT N0->N1 and P RIGHT N0->N2. */
14554 
14555     if (nl > 0) {
14556 
14557 /*   N0 is an interior node.  Find N1. */
14558 
14559 L3:
14560         if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14561                 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14562                 -1e-10) {
14563             lp = lptr[lp];
14564             n1 = list[lp];
14565             if (n1 == nl) {
14566                 goto L6;
14567             }
14568             goto L3;
14569         }
14570     } else {
14571 
14572 /*   N0 is a boundary node.  Test for P exterior. */
14573 
14574         nl = -nl;
14575         if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14576                 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14577                 -1e-10) {
14578 
14579 /*   P is to the right of the boundary edge N0->NF. */
14580 
14581             n1 = n0;
14582             n2 = nf;
14583             goto L9;
14584         }
14585         if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14586                 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14587                 -1e-10) {
14588 
14589 /*   P is to the right of the boundary edge NL->N0. */
14590 
14591             n1 = nl;
14592             n2 = n0;
14593             goto L9;
14594         }
14595     }
14596 
14597 /* P is to the left of arcs N0->N1 and NL->N0.  Set N2 to the */
14598 /*   next neighbor of N0 (following N1). */
14599 
14600 L4:
14601     lp = lptr[lp];
14602     n2 = (i__1 = list[lp], abs(i__1));
14603     if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14604             n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14605         goto L7;
14606     }
14607     n1 = n2;
14608     if (n1 != nl) {
14609         goto L4;
14610     }
14611     if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14612             nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14613         goto L6;
14614     }
14615 
14616 /* P is left of or on arcs N0->NB for all neighbors NB */
14617 /*   of N0.  Test for P = +/-N0. */
14618 
14619     d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14620     if (store_(&d__2) < 1. - eps * 4.) {
14621 
14622 /*   All points are collinear iff P Left NB->N0 for all */
14623 /*     neighbors NB of N0.  Search the neighbors of N0. */
14624 /*     Note:  N1 = NL and LP points to NL. */
14625 
14626 L5:
14627         if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14628                 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14629                 -1e-10) {
14630             lp = lptr[lp];
14631             n1 = (i__1 = list[lp], abs(i__1));
14632             if (n1 == nl) {
14633                 goto L14;
14634             }
14635             goto L5;
14636         }
14637     }
14638 
14639 /* P is to the right of N1->N0, or P = +/-N0.  Set N0 to N1 */
14640 /*   and start over. */
14641 
14642     n0 = n1;
14643     goto L2;
14644 
14645 /* P is between arcs N0->N1 and N0->NF. */
14646 
14647 L6:
14648     n2 = nf;
14649 
14650 /* P is contained in a wedge defined by geodesics N0-N1 and */
14651 /*   N0-N2, where N1 is adjacent to N2.  Save N1 and N2 to */
14652 /*   test for cycling. */
14653 
14654 L7:
14655     n3 = n0;
14656     n1s = n1;
14657     n2s = n2;
14658 
14659 /* Top of edge-hopping loop: */
14660 
14661 L8:
14662 
14663     *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14664             x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14665      if (*b3 < -1e-10) {
14666 
14667 /*   Set N4 to the first neighbor of N2 following N1 (the */
14668 /*     node opposite N2->N1) unless N1->N2 is a boundary arc. */
14669 
14670         lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14671         if (list[lp] < 0) {
14672             goto L9;
14673         }
14674         lp = lptr[lp];
14675         n4 = (i__1 = list[lp], abs(i__1));
14676 
14677 /*   Define a new arc N1->N2 which intersects the geodesic */
14678 /*     N0-P. */
14679         if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14680                 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14681                 -1e-10) {
14682             n3 = n2;
14683             n2 = n4;
14684             n1s = n1;
14685             if (n2 != n2s && n2 != n0) {
14686                 goto L8;
14687             }
14688         } else {
14689             n3 = n1;
14690             n1 = n4;
14691             n2s = n2;
14692             if (n1 != n1s && n1 != n0) {
14693                 goto L8;
14694             }
14695         }
14696 
14697 /*   The starting node N0 or edge N1-N2 was encountered */
14698 /*     again, implying a cycle (infinite loop).  Restart */
14699 /*     with N0 randomly selected. */
14700 
14701         n0 = jrand_(n, &ix, &iy, &iz);
14702         goto L2;
14703     }
14704 
14705 /* P is in (N1,N2,N3) unless N0, N1, N2, and P are collinear */
14706 /*   or P is close to -N0. */
14707 
14708     if (*b3 >= eps) {
14709 
14710 /*   B3 .NE. 0. */
14711 
14712         *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14713                  - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14714         *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14715                  - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14716         if (*b1 < -tol || *b2 < -tol) {
14717 
14718 /*   Restart with N0 randomly selected. */
14719 
14720             n0 = jrand_(n, &ix, &iy, &iz);
14721             goto L2;
14722         }
14723     } else {
14724 
14725 /*   B3 = 0 and thus P lies on N1->N2. Compute */
14726 /*     B1 = Det(P,N2 X N1,N2) and B2 = Det(P,N1,N2 X N1). */
14727 
14728         *b3 = 0.;
14729         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14730         ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14731         ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14732         *b1 = ptn1 - s12 * ptn2;
14733         *b2 = ptn2 - s12 * ptn1;
14734         if (*b1 < -tol || *b2 < -tol) {
14735 
14736 /*   Restart with N0 randomly selected. */
14737 
14738             n0 = jrand_(n, &ix, &iy, &iz);
14739             goto L2;
14740         }
14741     }
14742 
14743 /* P is in (N1,N2,N3). */
14744 
14745     *i1 = n1;
14746     *i2 = n2;
14747     *i3 = n3;
14748     if (*b1 < 0.f) {
14749         *b1 = 0.f;
14750     }
14751     if (*b2 < 0.f) {
14752         *b2 = 0.f;
14753     }
14754     return 0;
14755 
14756 /* P Right N1->N2, where N1->N2 is a boundary edge. */
14757 /*   Save N1 and N2, and set NL = 0 to indicate that */
14758 /*   NL has not yet been found. */
14759 
14760 L9:
14761     n1s = n1;
14762     n2s = n2;
14763     nl = 0;
14764 
14765 /*           Counterclockwise Boundary Traversal: */
14766 
14767 L10:
14768 
14769     lp = lend[n2];
14770     lp = lptr[lp];
14771     next = list[lp];
14772      if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14773              - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14774             >= -1e-10) {
14775 
14776 /*   N2 is the rightmost visible node if P Forward N2->N1 */
14777 /*     or NEXT Forward N2->N1.  Set Q to (N2 X N1) X N2. */
14778 
14779         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14780         q[0] = x[n1] - s12 * x[n2];
14781         q[1] = y[n1] - s12 * y[n2];
14782         q[2] = z__[n1] - s12 * z__[n2];
14783         if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14784             goto L11;
14785         }
14786         if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14787             goto L11;
14788         }
14789 
14790 /*   N1, N2, NEXT, and P are nearly collinear, and N2 is */
14791 /*     the leftmost visible node. */
14792 
14793         nl = n2;
14794     }
14795 
14796 /* Bottom of counterclockwise loop: */
14797 
14798     n1 = n2;
14799     n2 = next;
14800     if (n2 != n1s) {
14801         goto L10;
14802     }
14803 
14804 /* All boundary nodes are visible from P. */
14805 
14806     *i1 = n1s;
14807     *i2 = n1s;
14808     *i3 = 0;
14809     return 0;
14810 
14811 /* N2 is the rightmost visible node. */
14812 
14813 L11:
14814     nf = n2;
14815     if (nl == 0) {
14816 
14817 /* Restore initial values of N1 and N2, and begin the search */
14818 /*   for the leftmost visible node. */
14819 
14820         n2 = n2s;
14821         n1 = n1s;
14822 
14823 /*           Clockwise Boundary Traversal: */
14824 
14825 L12:
14826         lp = lend[n1];
14827         next = -list[lp];
14828         if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14829                 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14830                  y[next]) >= -1e-10) {
14831 
14832 /*   N1 is the leftmost visible node if P or NEXT is */
14833 /*     forward of N1->N2.  Compute Q = N1 X (N2 X N1). */
14834 
14835             s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14836             q[0] = x[n2] - s12 * x[n1];
14837             q[1] = y[n2] - s12 * y[n1];
14838             q[2] = z__[n2] - s12 * z__[n1];
14839             if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14840                 goto L13;
14841             }
14842             if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14843                 goto L13;
14844             }
14845 
14846 /*   P, NEXT, N1, and N2 are nearly collinear and N1 is the */
14847 /*     rightmost visible node. */
14848 
14849             nf = n1;
14850         }
14851 
14852 /* Bottom of clockwise loop: */
14853 
14854         n2 = n1;
14855         n1 = next;
14856         if (n1 != n1s) {
14857             goto L12;
14858         }
14859 
14860 /* All boundary nodes are visible from P. */
14861 
14862         *i1 = n1;
14863         *i2 = n1;
14864         *i3 = 0;
14865         return 0;
14866 
14867 /* N1 is the leftmost visible node. */
14868 
14869 L13:
14870         nl = n1;
14871     }
14872 
14873 /* NF and NL have been found. */
14874 
14875     *i1 = nf;
14876     *i2 = nl;
14877     *i3 = 0;
14878     return 0;
14879 
14880 /* All points are collinear (coplanar). */
14881 
14882 L14:
14883     *i1 = 0;
14884     *i2 = 0;
14885     *i3 = 0;
14886     return 0;
14887 } /* trfind_ */
14888 
14889 /* Subroutine */ int trlist_(int *n, int *list, int *lptr,
14890         int *lend, int *nrow, int *nt, int *ltri, int *
14891         ier)
14892 {
14893     /* System generated locals */
14894     int ltri_dim1, ltri_offset, i__1, i__2;
14895 
14896     /* Local variables */
14897     static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
14898             lpl, isv;
14899     static long int arcs;
14900     static int lpln1;
14901 
14902 
14903 /* *********************************************************** */
14904 
14905 /*                                              From STRIPACK */
14906 /*                                            Robert J. Renka */
14907 /*                                  Dept. of Computer Science */
14908 /*                                       Univ. of North Texas */
14909 /*                                           renka@cs.unt.edu */
14910 /*                                                   07/20/96 */
14911 
14912 /*   This subroutine converts a triangulation data structure */
14913 /* from the linked list created by Subroutine TRMESH to a */
14914 /* triangle list. */
14915 
14916 /* On input: */
14917 
14918 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14919 
14920 /*       LIST,LPTR,LEND = Linked list data structure defin- */
14921 /*                        ing the triangulation.  Refer to */
14922 /*                        Subroutine TRMESH. */
14923 
14924 /*       NROW = Number of rows (entries per triangle) re- */
14925 /*              served for the triangle list LTRI.  The value */
14926 /*              must be 6 if only the vertex indexes and */
14927 /*              neighboring triangle indexes are to be */
14928 /*              stored, or 9 if arc indexes are also to be */
14929 /*              assigned and stored.  Refer to LTRI. */
14930 
14931 /* The above parameters are not altered by this routine. */
14932 
14933 /*       LTRI = int array of length at least NROW*NT, */
14934 /*              where NT is at most 2N-4.  (A sufficient */
14935 /*              length is 12N if NROW=6 or 18N if NROW=9.) */
14936 
14937 /* On output: */
14938 
14939 /*       NT = Number of triangles in the triangulation unless */
14940 /*            IER .NE. 0, in which case NT = 0.  NT = 2N-NB-2 */
14941 /*            if NB .GE. 3 or 2N-4 if NB = 0, where NB is the */
14942 /*            number of boundary nodes. */
14943 
14944 /*       LTRI = NROW by NT array whose J-th column contains */
14945 /*              the vertex nodal indexes (first three rows), */
14946 /*              neighboring triangle indexes (second three */
14947 /*              rows), and, if NROW = 9, arc indexes (last */
14948 /*              three rows) associated with triangle J for */
14949 /*              J = 1,...,NT.  The vertices are ordered */
14950 /*              counterclockwise with the first vertex taken */
14951 /*              to be the one with smallest index.  Thus, */
14952 /*              LTRI(2,J) and LTRI(3,J) are larger than */
14953 /*              LTRI(1,J) and index adjacent neighbors of */
14954 /*              node LTRI(1,J).  For I = 1,2,3, LTRI(I+3,J) */
14955 /*              and LTRI(I+6,J) index the triangle and arc, */
14956 /*              respectively, which are opposite (not shared */
14957 /*              by) node LTRI(I,J), with LTRI(I+3,J) = 0 if */
14958 /*              LTRI(I+6,J) indexes a boundary arc.  Vertex */
14959 /*              indexes range from 1 to N, triangle indexes */
14960 /*              from 0 to NT, and, if included, arc indexes */
14961 /*              from 1 to NA, where NA = 3N-NB-3 if NB .GE. 3 */
14962 /*              or 3N-6 if NB = 0.  The triangles are or- */
14963 /*              dered on first (smallest) vertex indexes. */
14964 
14965 /*       IER = Error indicator. */
14966 /*             IER = 0 if no errors were encountered. */
14967 /*             IER = 1 if N or NROW is outside its valid */
14968 /*                     range on input. */
14969 /*             IER = 2 if the triangulation data structure */
14970 /*                     (LIST,LPTR,LEND) is invalid.  Note, */
14971 /*                     however, that these arrays are not */
14972 /*                     completely tested for validity. */
14973 
14974 /* Modules required by TRLIST:  None */
14975 
14976 /* Intrinsic function called by TRLIST:  ABS */
14977 
14978 /* *********************************************************** */
14979 
14980 
14981 /* Local parameters: */
14982 
14983 /* ARCS =     long int variable with value TRUE iff are */
14984 /*              indexes are to be stored */
14985 /* I,J =      LTRI row indexes (1 to 3) associated with */
14986 /*              triangles KT and KN, respectively */
14987 /* I1,I2,I3 = Nodal indexes of triangle KN */
14988 /* ISV =      Variable used to permute indexes I1,I2,I3 */
14989 /* KA =       Arc index and number of currently stored arcs */
14990 /* KN =       Index of the triangle that shares arc I1-I2 */
14991 /*              with KT */
14992 /* KT =       Triangle index and number of currently stored */
14993 /*              triangles */
14994 /* LP =       LIST pointer */
14995 /* LP2 =      Pointer to N2 as a neighbor of N1 */
14996 /* LPL =      Pointer to the last neighbor of I1 */
14997 /* LPLN1 =    Pointer to the last neighbor of N1 */
14998 /* N1,N2,N3 = Nodal indexes of triangle KT */
14999 /* NM2 =      N-2 */
15000 
15001 
15002 /* Test for invalid input parameters. */
15003 
15004     /* Parameter adjustments */
15005     --lend;
15006     --list;
15007     --lptr;
15008     ltri_dim1 = *nrow;
15009     ltri_offset = 1 + ltri_dim1;
15010     ltri -= ltri_offset;
15011 
15012     /* Function Body */
15013     if (*n < 3 || *nrow != 6 && *nrow != 9) {
15014         goto L11;
15015     }
15016 
15017 /* Initialize parameters for loop on triangles KT = (N1,N2, */
15018 /*   N3), where N1 < N2 and N1 < N3. */
15019 
15020 /*   ARCS = TRUE iff arc indexes are to be stored. */
15021 /*   KA,KT = Numbers of currently stored arcs and triangles. */
15022 /*   NM2 = Upper bound on candidates for N1. */
15023 
15024     arcs = *nrow == 9;
15025     ka = 0;
15026     kt = 0;
15027     nm2 = *n - 2;
15028 
15029 /* Loop on nodes N1. */
15030 
15031     i__1 = nm2;
15032     for (n1 = 1; n1 <= i__1; ++n1) {
15033 
15034 /* Loop on pairs of adjacent neighbors (N2,N3).  LPLN1 points */
15035 /*   to the last neighbor of N1, and LP2 points to N2. */
15036 
15037         lpln1 = lend[n1];
15038         lp2 = lpln1;
15039 L1:
15040         lp2 = lptr[lp2];
15041         n2 = list[lp2];
15042         lp = lptr[lp2];
15043         n3 = (i__2 = list[lp], abs(i__2));
15044         if (n2 < n1 || n3 < n1) {
15045             goto L8;
15046         }
15047 
15048 /* Add a new triangle KT = (N1,N2,N3). */
15049 
15050         ++kt;
15051         ltri[kt * ltri_dim1 + 1] = n1;
15052         ltri[kt * ltri_dim1 + 2] = n2;
15053         ltri[kt * ltri_dim1 + 3] = n3;
15054 
15055 /* Loop on triangle sides (I2,I1) with neighboring triangles */
15056 /*   KN = (I1,I2,I3). */
15057 
15058         for (i__ = 1; i__ <= 3; ++i__) {
15059             if (i__ == 1) {
15060                 i1 = n3;
15061                 i2 = n2;
15062             } else if (i__ == 2) {
15063                 i1 = n1;
15064                 i2 = n3;
15065             } else {
15066                 i1 = n2;
15067                 i2 = n1;
15068             }
15069 
15070 /* Set I3 to the neighbor of I1 that follows I2 unless */
15071 /*   I2->I1 is a boundary arc. */
15072 
15073             lpl = lend[i1];
15074             lp = lptr[lpl];
15075 L2:
15076             if (list[lp] == i2) {
15077                 goto L3;
15078             }
15079             lp = lptr[lp];
15080             if (lp != lpl) {
15081                 goto L2;
15082             }
15083 
15084 /*   I2 is the last neighbor of I1 unless the data structure */
15085 /*     is invalid.  Bypass the search for a neighboring */
15086 /*     triangle if I2->I1 is a boundary arc. */
15087 
15088             if ((i__2 = list[lp], abs(i__2)) != i2) {
15089                 goto L12;
15090             }
15091             kn = 0;
15092             if (list[lp] < 0) {
15093                 goto L6;
15094             }
15095 
15096 /*   I2->I1 is not a boundary arc, and LP points to I2 as */
15097 /*     a neighbor of I1. */
15098 
15099 L3:
15100             lp = lptr[lp];
15101             i3 = (i__2 = list[lp], abs(i__2));
15102 
15103 /* Find J such that LTRI(J,KN) = I3 (not used if KN > KT), */
15104 /*   and permute the vertex indexes of KN so that I1 is */
15105 /*   smallest. */
15106 
15107             if (i1 < i2 && i1 < i3) {
15108                 j = 3;
15109             } else if (i2 < i3) {
15110                 j = 2;
15111                 isv = i1;
15112                 i1 = i2;
15113                 i2 = i3;
15114                 i3 = isv;
15115             } else {
15116                 j = 1;
15117                 isv = i1;
15118                 i1 = i3;
15119                 i3 = i2;
15120                 i2 = isv;
15121             }
15122 
15123 /* Test for KN > KT (triangle index not yet assigned). */
15124 
15125             if (i1 > n1) {
15126                 goto L7;
15127             }
15128 
15129 /* Find KN, if it exists, by searching the triangle list in */
15130 /*   reverse order. */
15131 
15132             for (kn = kt - 1; kn >= 1; --kn) {
15133                 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15134                          == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15135                     goto L5;
15136                 }
15137 /* L4: */
15138             }
15139             goto L7;
15140 
15141 /* Store KT as a neighbor of KN. */
15142 
15143 L5:
15144             ltri[j + 3 + kn * ltri_dim1] = kt;
15145 
15146 /* Store KN as a neighbor of KT, and add a new arc KA. */
15147 
15148 L6:
15149             ltri[i__ + 3 + kt * ltri_dim1] = kn;
15150             if (arcs) {
15151                 ++ka;
15152                 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15153                 if (kn != 0) {
15154                     ltri[j + 6 + kn * ltri_dim1] = ka;
15155                 }
15156             }
15157 L7:
15158             ;
15159         }
15160 
15161 /* Bottom of loop on triangles. */
15162 
15163 L8:
15164         if (lp2 != lpln1) {
15165             goto L1;
15166         }
15167 /* L9: */
15168     }
15169 
15170 /* No errors encountered. */
15171 
15172     *nt = kt;
15173     *ier = 0;
15174     return 0;
15175 
15176 /* Invalid input parameter. */
15177 
15178 L11:
15179     *nt = 0;
15180     *ier = 1;
15181     return 0;
15182 
15183 /* Invalid triangulation data structure:  I1 is a neighbor of */
15184 /*   I2, but I2 is not a neighbor of I1. */
15185 
15186 L12:
15187     *nt = 0;
15188     *ier = 2;
15189     return 0;
15190 } /* trlist_ */
15191 
15192 /* Subroutine */ int trlprt_(int *n, double *x, double *y,
15193         double *z__, int *iflag, int *nrow, int *nt, int *
15194         ltri, int *lout)
15195 {
15196     /* Initialized data */
15197 
15198     static int nmax = 9999;
15199     static int nlmax = 58;
15200 
15201     /* System generated locals */
15202     int ltri_dim1, ltri_offset, i__1;
15203 
15204     /* Local variables */
15205     static int i__, k, na, nb, nl, lun;
15206 
15207 
15208 /* *********************************************************** */
15209 
15210 /*                                              From STRIPACK */
15211 /*                                            Robert J. Renka */
15212 /*                                  Dept. of Computer Science */
15213 /*                                       Univ. of North Texas */
15214 /*                                           renka@cs.unt.edu */
15215 /*                                                   07/02/98 */
15216 
15217 /*   This subroutine prints the triangle list created by Sub- */
15218 /* routine TRLIST and, optionally, the nodal coordinates */
15219 /* (either latitude and longitude or Cartesian coordinates) */
15220 /* on long int unit LOUT.  The numbers of boundary nodes, */
15221 /* triangles, and arcs are also printed. */
15222 
15223 
15224 /* On input: */
15225 
15226 /*       N = Number of nodes in the triangulation. */
15227 /*           3 .LE. N .LE. 9999. */
15228 
15229 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15230 /*               coordinates of the nodes if IFLAG = 0, or */
15231 /*               (X and Y only) arrays of length N containing */
15232 /*               longitude and latitude, respectively, if */
15233 /*               IFLAG > 0, or unused dummy parameters if */
15234 /*               IFLAG < 0. */
15235 
15236 /*       IFLAG = Nodal coordinate option indicator: */
15237 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
15238 /*                         Cartesian coordinates) are to be */
15239 /*                         printed (to 6 decimal places). */
15240 /*               IFLAG > 0 if only X and Y (assumed to con- */
15241 /*                         tain longitude and latitude) are */
15242 /*                         to be printed (to 6 decimal */
15243 /*                         places). */
15244 /*               IFLAG < 0 if only the adjacency lists are to */
15245 /*                         be printed. */
15246 
15247 /*       NROW = Number of rows (entries per triangle) re- */
15248 /*              served for the triangle list LTRI.  The value */
15249 /*              must be 6 if only the vertex indexes and */
15250 /*              neighboring triangle indexes are stored, or 9 */
15251 /*              if arc indexes are also stored. */
15252 
15253 /*       NT = Number of triangles in the triangulation. */
15254 /*            1 .LE. NT .LE. 9999. */
15255 
15256 /*       LTRI = NROW by NT array whose J-th column contains */
15257 /*              the vertex nodal indexes (first three rows), */
15258 /*              neighboring triangle indexes (second three */
15259 /*              rows), and, if NROW = 9, arc indexes (last */
15260 /*              three rows) associated with triangle J for */
15261 /*              J = 1,...,NT. */
15262 
15263 /*       LOUT = long int unit number for output.  If LOUT is */
15264 /*              not in the range 0 to 99, output is written */
15265 /*              to unit 6. */
15266 
15267 /* Input parameters are not altered by this routine. */
15268 
15269 /* On output: */
15270 
15271 /*   The triangle list and nodal coordinates (as specified by */
15272 /* IFLAG) are written to unit LOUT. */
15273 
15274 /* Modules required by TRLPRT:  None */
15275 
15276 /* *********************************************************** */
15277 
15278     /* Parameter adjustments */
15279     --z__;
15280     --y;
15281     --x;
15282     ltri_dim1 = *nrow;
15283     ltri_offset = 1 + ltri_dim1;
15284     ltri -= ltri_offset;
15285 
15286     /* Function Body */
15287 
15288 /* Local parameters: */
15289 
15290 /* I =     DO-loop, nodal index, and row index for LTRI */
15291 /* K =     DO-loop and triangle index */
15292 /* LUN =   long int unit number for output */
15293 /* NA =    Number of triangulation arcs */
15294 /* NB =    Number of boundary nodes */
15295 /* NL =    Number of lines printed on the current page */
15296 /* NLMAX = Maximum number of print lines per page (except */
15297 /*           for the last page which may have two addi- */
15298 /*           tional lines) */
15299 /* NMAX =  Maximum value of N and NT (4-digit format) */
15300 
15301     lun = *lout;
15302     if (lun < 0 || lun > 99) {
15303         lun = 6;
15304     }
15305 
15306 /* Print a heading and test for invalid input. */
15307 
15308 /*      WRITE (LUN,100) N */
15309     nl = 3;
15310     if (*n < 3 || *n > nmax || *nrow != 6 && *nrow != 9 || *nt < 1 || *nt >
15311             nmax) {
15312 
15313 /* Print an error message and exit. */
15314 
15315 /*        WRITE (LUN,110) N, NROW, NT */
15316         return 0;
15317     }
15318     if (*iflag == 0) {
15319 
15320 /* Print X, Y, and Z. */
15321 
15322 /*        WRITE (LUN,101) */
15323         nl = 6;
15324         i__1 = *n;
15325         for (i__ = 1; i__ <= i__1; ++i__) {
15326             if (nl >= nlmax) {
15327 /*            WRITE (LUN,108) */
15328                 nl = 0;
15329             }
15330 /*          WRITE (LUN,103) I, X(I), Y(I), Z(I) */
15331             ++nl;
15332 /* L1: */
15333         }
15334     } else if (*iflag > 0) {
15335 
15336 /* Print X (longitude) and Y (latitude). */
15337 
15338 /*        WRITE (LUN,102) */
15339         nl = 6;
15340         i__1 = *n;
15341         for (i__ = 1; i__ <= i__1; ++i__) {
15342             if (nl >= nlmax) {
15343 /*            WRITE (LUN,108) */
15344                 nl = 0;
15345             }
15346 /*          WRITE (LUN,104) I, X(I), Y(I) */
15347             ++nl;
15348 /* L2: */
15349         }
15350     }
15351 
15352 /* Print the triangulation LTRI. */
15353 
15354     if (nl > nlmax / 2) {
15355 /*        WRITE (LUN,108) */
15356         nl = 0;
15357     }
15358     if (*nrow == 6) {
15359 /*        WRITE (LUN,105) */
15360     } else {
15361 /*        WRITE (LUN,106) */
15362     }
15363     nl += 5;
15364     i__1 = *nt;
15365     for (k = 1; k <= i__1; ++k) {
15366         if (nl >= nlmax) {
15367 /*          WRITE (LUN,108) */
15368             nl = 0;
15369         }
15370 /*        WRITE (LUN,107) K, (LTRI(I,K), I = 1,NROW) */
15371         ++nl;
15372 /* L3: */
15373     }
15374 
15375 /* Print NB, NA, and NT (boundary nodes, arcs, and */
15376 /*   triangles). */
15377 
15378     nb = (*n << 1) - *nt - 2;
15379     if (nb < 3) {
15380         nb = 0;
15381         na = *n * 3 - 6;
15382     } else {
15383         na = *nt + *n - 1;
15384     }
15385 /*      WRITE (LUN,109) NB, NA, NT */
15386     return 0;
15387 
15388 /* Print formats: */
15389 
15390 /*  100 FORMAT (///18X,'STRIPACK (TRLIST) Output,  N = ',I4) */
15391 /*  101 FORMAT (//8X,'Node',10X,'X(Node)',10X,'Y(Node)',10X, */
15392 /*     .        'Z(Node)'//) */
15393 /*  102 FORMAT (//16X,'Node',8X,'Longitude',9X,'Latitude'//) */
15394 /*  103 FORMAT (8X,I4,3D17.6) */
15395 /*  104 FORMAT (16X,I4,2D17.6) */
15396 /*  105 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors'/ */
15397 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15398 /*     .        'KT2',4X,'KT3'/) */
15399 /*  106 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors', */
15400 /*     .        14X,'Arcs'/ */
15401 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15402 /*     .        'KT2',4X,'KT3',4X,'KA1',4X,'KA2',4X,'KA3'/) */
15403 /*  107 FORMAT (2X,I4,2X,6(3X,I4),3(2X,I5)) */
15404 /*  108 FORMAT (///) */
15405 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
15406 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
15407 /*     .        ' Triangles') */
15408 /*  110 FORMAT (//1X,10X,'*** Invalid Parameter:  N =',I5, */
15409 /*     .        ', NROW =',I5,', NT =',I5,' ***') */
15410 } /* trlprt_ */
15411 
15412 /* Subroutine */ int trmesh_(int *n, double *x, double *y,
15413         double *z__, int *list, int *lptr, int *lend, int
15414         *lnew, int *near__, int *next, double *dist, int *ier)
15415 {
15416     /* System generated locals */
15417     int i__1, i__2;
15418 
15419     /* Local variables */
15420     static double d__;
15421     static int i__, j, k;
15422     static double d1, d2, d3;
15423     static int i0, lp, nn, lpl;
15424     extern long int left_(double *, double *, double *, double
15425             *, double *, double *, double *, double *,
15426             double *);
15427     static int nexti;
15428     extern /* Subroutine */ int addnod_(int *, int *, double *,
15429             double *, double *, int *, int *, int *,
15430             int *, int *);
15431 
15432 
15433 /* *********************************************************** */
15434 
15435 /*                                              From STRIPACK */
15436 /*                                            Robert J. Renka */
15437 /*                                  Dept. of Computer Science */
15438 /*                                       Univ. of North Texas */
15439 /*                                           renka@cs.unt.edu */
15440 /*                                                   03/04/03 */
15441 
15442 /*   This subroutine creates a Delaunay triangulation of a */
15443 /* set of N arbitrarily distributed points, referred to as */
15444 /* nodes, on the surface of the unit sphere.  The Delaunay */
15445 /* triangulation is defined as a set of (spherical) triangles */
15446 /* with the following five properties: */
15447 
15448 /*  1)  The triangle vertices are nodes. */
15449 /*  2)  No triangle contains a node other than its vertices. */
15450 /*  3)  The interiors of the triangles are pairwise disjoint. */
15451 /*  4)  The union of triangles is the convex hull of the set */
15452 /*        of nodes (the smallest convex set that contains */
15453 /*        the nodes).  If the nodes are not contained in a */
15454 /*        single hemisphere, their convex hull is the en- */
15455 /*        tire sphere and there are no boundary nodes. */
15456 /*        Otherwise, there are at least three boundary nodes. */
15457 /*  5)  The interior of the circumcircle of each triangle */
15458 /*        contains no node. */
15459 
15460 /* The first four properties define a triangulation, and the */
15461 /* last property results in a triangulation which is as close */
15462 /* as possible to equiangular in a certain sense and which is */
15463 /* uniquely defined unless four or more nodes lie in a common */
15464 /* plane.  This property makes the triangulation well-suited */
15465 /* for solving closest-point problems and for triangle-based */
15466 /* interpolation. */
15467 
15468 /*   The algorithm has expected time complexity O(N*log(N)) */
15469 /* for most nodal distributions. */
15470 
15471 /*   Spherical coordinates (latitude and longitude) may be */
15472 /* converted to Cartesian coordinates by Subroutine TRANS. */
15473 
15474 /*   The following is a list of the software package modules */
15475 /* which a user may wish to call directly: */
15476 
15477 /*  ADDNOD - Updates the triangulation by appending a new */
15478 /*             node. */
15479 
15480 /*  AREAS  - Returns the area of a spherical triangle. */
15481 
15482 /*  AREAV  - Returns the area of a Voronoi region associated */
15483 /*           with an interior node without requiring that the */
15484 /*           entire Voronoi diagram be computed and stored. */
15485 
15486 /*  BNODES - Returns an array containing the indexes of the */
15487 /*             boundary nodes (if any) in counterclockwise */
15488 /*             order.  Counts of boundary nodes, triangles, */
15489 /*             and arcs are also returned. */
15490 
15491 /*  CIRCLE - Computes the coordinates of a sequence of uni- */
15492 /*           formly spaced points on the unit circle centered */
15493 /*           at (0,0). */
15494 
15495 /*  CIRCUM - Returns the circumcenter of a spherical trian- */
15496 /*             gle. */
15497 
15498 /*  CRLIST - Returns the set of triangle circumcenters */
15499 /*             (Voronoi vertices) and circumradii associated */
15500 /*             with a triangulation. */
15501 
15502 /*  DELARC - Deletes a boundary arc from a triangulation. */
15503 
15504 /*  DELNOD - Updates the triangulation with a nodal deletion. */
15505 
15506 /*  EDGE   - Forces an arbitrary pair of nodes to be connec- */
15507 /*             ted by an arc in the triangulation. */
15508 
15509 /*  GETNP  - Determines the ordered sequence of L closest */
15510 /*             nodes to a given node, along with the associ- */
15511 /*             ated distances. */
15512 
15513 /*  INSIDE - Locates a point relative to a polygon on the */
15514 /*             surface of the sphere. */
15515 
15516 /*  INTRSC - Returns the point of intersection between a */
15517 /*             pair of great circle arcs. */
15518 
15519 /*  JRAND  - Generates a uniformly distributed pseudo-random */
15520 /*             int. */
15521 
15522 /*  LEFT   - Locates a point relative to a great circle. */
15523 
15524 /*  NEARND - Returns the index of the nearest node to an */
15525 /*             arbitrary point, along with its squared */
15526 /*             distance. */
15527 
15528 /*  PROJCT - Applies a perspective-depth projection to a */
15529 /*             point in 3-space. */
15530 
15531 /*  SCOORD - Converts a point from Cartesian coordinates to */
15532 /*             spherical coordinates. */
15533 
15534 /*  STORE  - Forces a value to be stored in main memory so */
15535 /*             that the precision of floating point numbers */
15536 /*             in memory locations rather than registers is */
15537 /*             computed. */
15538 
15539 /*  TRANS  - Transforms spherical coordinates into Cartesian */
15540 /*             coordinates on the unit sphere for input to */
15541 /*             Subroutine TRMESH. */
15542 
15543 /*  TRLIST - Converts the triangulation data structure to a */
15544 /*             triangle list more suitable for use in a fin- */
15545 /*             ite element code. */
15546 
15547 /*  TRLPRT - Prints the triangle list created by Subroutine */
15548 /*             TRLIST. */
15549 
15550 /*  TRMESH - Creates a Delaunay triangulation of a set of */
15551 /*             nodes. */
15552 
15553 /*  TRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15554 /*             file containing a triangulation plot. */
15555 
15556 /*  TRPRNT - Prints the triangulation data structure and, */
15557 /*             optionally, the nodal coordinates. */
15558 
15559 /*  VRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15560 /*             file containing a Voronoi diagram plot. */
15561 
15562 
15563 /* On input: */
15564 
15565 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15566 
15567 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15568 /*               coordinates of distinct nodes.  (X(K),Y(K), */
15569 /*               Z(K)) is referred to as node K, and K is re- */
15570 /*               ferred to as a nodal index.  It is required */
15571 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
15572 /*               K.  The first three nodes must not be col- */
15573 /*               linear (lie on a common great circle). */
15574 
15575 /* The above parameters are not altered by this routine. */
15576 
15577 /*       LIST,LPTR = Arrays of length at least 6N-12. */
15578 
15579 /*       LEND = Array of length at least N. */
15580 
15581 /*       NEAR,NEXT,DIST = Work space arrays of length at */
15582 /*                        least N.  The space is used to */
15583 /*                        efficiently determine the nearest */
15584 /*                        triangulation node to each un- */
15585 /*                        processed node for use by ADDNOD. */
15586 
15587 /* On output: */
15588 
15589 /*       LIST = Set of nodal indexes which, along with LPTR, */
15590 /*              LEND, and LNEW, define the triangulation as a */
15591 /*              set of N adjacency lists -- counterclockwise- */
15592 /*              ordered sequences of neighboring nodes such */
15593 /*              that the first and last neighbors of a bound- */
15594 /*              ary node are boundary nodes (the first neigh- */
15595 /*              bor of an interior node is arbitrary).  In */
15596 /*              order to distinguish between interior and */
15597 /*              boundary nodes, the last neighbor of each */
15598 /*              boundary node is represented by the negative */
15599 /*              of its index. */
15600 
15601 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
15602 /*              correspondence with the elements of LIST. */
15603 /*              LIST(LPTR(I)) indexes the node which follows */
15604 /*              LIST(I) in cyclical counterclockwise order */
15605 /*              (the first neighbor follows the last neigh- */
15606 /*              bor). */
15607 
15608 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
15609 /*              points to the last neighbor of node K for */
15610 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
15611 /*              only if K is a boundary node. */
15612 
15613 /*       LNEW = Pointer to the first empty location in LIST */
15614 /*              and LPTR (list length plus one).  LIST, LPTR, */
15615 /*              LEND, and LNEW are not altered if IER < 0, */
15616 /*              and are incomplete if IER > 0. */
15617 
15618 /*       NEAR,NEXT,DIST = Garbage. */
15619 
15620 /*       IER = Error indicator: */
15621 /*             IER =  0 if no errors were encountered. */
15622 /*             IER = -1 if N < 3 on input. */
15623 /*             IER = -2 if the first three nodes are */
15624 /*                      collinear. */
15625 /*             IER =  L if nodes L and M coincide for some */
15626 /*                      M > L.  The data structure represents */
15627 /*                      a triangulation of nodes 1 to M-1 in */
15628 /*                      this case. */
15629 
15630 /* Modules required by TRMESH:  ADDNOD, BDYADD, COVSPH, */
15631 /*                                INSERT, INTADD, JRAND, */
15632 /*                                LEFT, LSTPTR, STORE, SWAP, */
15633 /*                                SWPTST, TRFIND */
15634 
15635 /* Intrinsic function called by TRMESH:  ABS */
15636 
15637 /* *********************************************************** */
15638 
15639 
15640 /* Local parameters: */
15641 
15642 /* D =        (Negative cosine of) distance from node K to */
15643 /*              node I */
15644 /* D1,D2,D3 = Distances from node K to nodes 1, 2, and 3, */
15645 /*              respectively */
15646 /* I,J =      Nodal indexes */
15647 /* I0 =       Index of the node preceding I in a sequence of */
15648 /*              unprocessed nodes:  I = NEXT(I0) */
15649 /* K =        Index of node to be added and DO-loop index: */
15650 /*              K > 3 */
15651 /* LP =       LIST index (pointer) of a neighbor of K */
15652 /* LPL =      Pointer to the last neighbor of K */
15653 /* NEXTI =    NEXT(I) */
15654 /* NN =       Local copy of N */
15655 
15656     /* Parameter adjustments */
15657     --dist;
15658     --next;
15659     --near__;
15660     --lend;
15661     --z__;
15662     --y;
15663     --x;
15664     --list;
15665     --lptr;
15666 
15667     /* Function Body */
15668     nn = *n;
15669     if (nn < 3) {
15670         *ier = -1;
15671         return 0;
15672     }
15673 
15674 /* Store the first triangle in the linked list. */
15675 
15676     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15677             z__[3])) {
15678 
15679 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
15680 
15681         list[1] = 3;
15682         lptr[1] = 2;
15683         list[2] = -2;
15684         lptr[2] = 1;
15685         lend[1] = 2;
15686 
15687         list[3] = 1;
15688         lptr[3] = 4;
15689         list[4] = -3;
15690         lptr[4] = 3;
15691         lend[2] = 4;
15692 
15693         list[5] = 2;
15694         lptr[5] = 6;
15695         list[6] = -1;
15696         lptr[6] = 5;
15697         lend[3] = 6;
15698 
15699     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15700             y[3], &z__[3])) {
15701 
15702 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
15703 /*     i.e., node 3 lies in the left hemisphere defined by */
15704 /*     arc 1->2. */
15705 
15706         list[1] = 2;
15707         lptr[1] = 2;
15708         list[2] = -3;
15709         lptr[2] = 1;
15710         lend[1] = 2;
15711 
15712         list[3] = 3;
15713         lptr[3] = 4;
15714         list[4] = -1;
15715         lptr[4] = 3;
15716         lend[2] = 4;
15717 
15718         list[5] = 1;
15719         lptr[5] = 6;
15720         list[6] = -2;
15721         lptr[6] = 5;
15722         lend[3] = 6;
15723 
15724     } else {
15725 
15726 /*   The first three nodes are collinear. */
15727 
15728         *ier = -2;
15729         return 0;
15730     }
15731 
15732 /* Initialize LNEW and test for N = 3. */
15733 
15734     *lnew = 7;
15735     if (nn == 3) {
15736         *ier = 0;
15737         return 0;
15738     }
15739 
15740 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
15741 /*   used to obtain an expected-time (N*log(N)) incremental */
15742 /*   algorithm by enabling constant search time for locating */
15743 /*   each new node in the triangulation. */
15744 
15745 /* For each unprocessed node K, NEAR(K) is the index of the */
15746 /*   triangulation node closest to K (used as the starting */
15747 /*   point for the search in Subroutine TRFIND) and DIST(K) */
15748 /*   is an increasing function of the arc length (angular */
15749 /*   distance) between nodes K and NEAR(K):  -Cos(a) for arc */
15750 /*   length a. */
15751 
15752 /* Since it is necessary to efficiently find the subset of */
15753 /*   unprocessed nodes associated with each triangulation */
15754 /*   node J (those that have J as their NEAR entries), the */
15755 /*   subsets are stored in NEAR and NEXT as follows:  for */
15756 /*   each node J in the triangulation, I = NEAR(J) is the */
15757 /*   first unprocessed node in J's set (with I = 0 if the */
15758 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
15759 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
15760 /*   set are initially ordered by increasing indexes (which */
15761 /*   maximizes efficiency) but that ordering is not main- */
15762 /*   tained as the data structure is updated. */
15763 
15764 /* Initialize the data structure for the single triangle. */
15765 
15766     near__[1] = 0;
15767     near__[2] = 0;
15768     near__[3] = 0;
15769     for (k = nn; k >= 4; --k) {
15770         d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15771         d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15772         d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15773         if (d1 <= d2 && d1 <= d3) {
15774             near__[k] = 1;
15775             dist[k] = d1;
15776             next[k] = near__[1];
15777             near__[1] = k;
15778         } else if (d2 <= d1 && d2 <= d3) {
15779             near__[k] = 2;
15780             dist[k] = d2;
15781             next[k] = near__[2];
15782             near__[2] = k;
15783         } else {
15784             near__[k] = 3;
15785             dist[k] = d3;
15786             next[k] = near__[3];
15787             near__[3] = k;
15788         }
15789 /* L1: */
15790     }
15791 
15792 /* Add the remaining nodes */
15793 
15794     i__1 = nn;
15795     for (k = 4; k <= i__1; ++k) {
15796         addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15797                 lend[1], lnew, ier);
15798         if (*ier != 0) {
15799             return 0;
15800         }
15801 
15802 /* Remove K from the set of unprocessed nodes associated */
15803 /*   with NEAR(K). */
15804 
15805         i__ = near__[k];
15806         if (near__[i__] == k) {
15807             near__[i__] = next[k];
15808         } else {
15809             i__ = near__[i__];
15810 L2:
15811             i0 = i__;
15812             i__ = next[i0];
15813             if (i__ != k) {
15814                 goto L2;
15815             }
15816             next[i0] = next[k];
15817         }
15818         near__[k] = 0;
15819 
15820 /* Loop on neighbors J of node K. */
15821 
15822         lpl = lend[k];
15823         lp = lpl;
15824 L3:
15825         lp = lptr[lp];
15826         j = (i__2 = list[lp], abs(i__2));
15827 
15828 /* Loop on elements I in the sequence of unprocessed nodes */
15829 /*   associated with J:  K is a candidate for replacing J */
15830 /*   as the nearest triangulation node to I.  The next value */
15831 /*   of I in the sequence, NEXT(I), must be saved before I */
15832 /*   is moved because it is altered by adding I to K's set. */
15833 
15834         i__ = near__[j];
15835 L4:
15836         if (i__ == 0) {
15837             goto L5;
15838         }
15839         nexti = next[i__];
15840 
15841 /* Test for the distance from I to K less than the distance */
15842 /*   from I to J. */
15843 
15844         d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15845         if (d__ < dist[i__]) {
15846 
15847 /* Replace J by K as the nearest triangulation node to I: */
15848 /*   update NEAR(I) and DIST(I), and remove I from J's set */
15849 /*   of unprocessed nodes and add it to K's set. */
15850 
15851             near__[i__] = k;
15852             dist[i__] = d__;
15853             if (i__ == near__[j]) {
15854                 near__[j] = nexti;
15855             } else {
15856                 next[i0] = nexti;
15857             }
15858             next[i__] = near__[k];
15859             near__[k] = i__;
15860         } else {
15861             i0 = i__;
15862         }
15863 
15864 /* Bottom of loop on I. */
15865 
15866         i__ = nexti;
15867         goto L4;
15868 
15869 /* Bottom of loop on neighbors J. */
15870 
15871 L5:
15872         if (lp != lpl) {
15873             goto L3;
15874         }
15875 /* L6: */
15876     }
15877     return 0;
15878 } /* trmesh_ */
15879 
15880 /* Subroutine */ int trplot_(int *lun, double *pltsiz, double *
15881         elat, double *elon, double *a, int *n, double *x,
15882         double *y, double *z__, int *list, int *lptr, int
15883         *lend, char *, long int *numbr, int *ier, short )
15884 {
15885     /* Initialized data */
15886 
15887     static long int annot = TRUE_;
15888     static double fsizn = 10.;
15889     static double fsizt = 16.;
15890     static double tol = .5;
15891 
15892     /* System generated locals */
15893     int i__1, i__2;
15894     double d__1;
15895 
15896     /* Builtin functions */
15897     //double atan(double), sin(double);
15898     //int i_dnnt(double *);
15899     //double cos(double), sqrt(double);
15900 
15901     /* Local variables */
15902     static double t;
15903     static int n0, n1;
15904     static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
15905     static int ir, lp;
15906     static double ex, ey, ez, wr, tx, ty;
15907     static int lpl;
15908     static double wrs;
15909     static int ipx1, ipx2, ipy1, ipy2, nseg;
15910     extern /* Subroutine */ int drwarc_(int *, double *, double *,
15911              double *, int *);
15912 
15913 
15914 /* *********************************************************** */
15915 
15916 /*                                              From STRIPACK */
15917 /*                                            Robert J. Renka */
15918 /*                                  Dept. of Computer Science */
15919 /*                                       Univ. of North Texas */
15920 /*                                           renka@cs.unt.edu */
15921 /*                                                   03/04/03 */
15922 
15923 /*   This subroutine creates a level-2 Encapsulated Post- */
15924 /* script (EPS) file containing a graphical display of a */
15925 /* triangulation of a set of nodes on the surface of the unit */
15926 /* sphere.  The visible portion of the triangulation is */
15927 /* projected onto the plane that contains the origin and has */
15928 /* normal defined by a user-specified eye-position. */
15929 
15930 
15931 /* On input: */
15932 
15933 /*       LUN = long int unit number in the range 0 to 99. */
15934 /*             The unit should be opened with an appropriate */
15935 /*             file name before the call to this routine. */
15936 
15937 /*       PLTSIZ = Plot size in inches.  A circular window in */
15938 /*                the projection plane is mapped to a circu- */
15939 /*                lar viewport with diameter equal to .88* */
15940 /*                PLTSIZ (leaving room for labels outside the */
15941 /*                viewport).  The viewport is centered on the */
15942 /*                8.5 by 11 inch page, and its boundary is */
15943 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
15944 
15945 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
15946 /*                   the center of projection E (the center */
15947 /*                   of the plot).  The projection plane is */
15948 /*                   the plane that contains the origin and */
15949 /*                   has E as unit normal.  In a rotated */
15950 /*                   coordinate system for which E is the */
15951 /*                   north pole, the projection plane con- */
15952 /*                   tains the equator, and only northern */
15953 /*                   hemisphere nodes are visible (from the */
15954 /*                   point at infinity in the direction E). */
15955 /*                   These are projected orthogonally onto */
15956 /*                   the projection plane (by zeroing the z- */
15957 /*                   component in the rotated coordinate */
15958 /*                   system).  ELAT and ELON must be in the */
15959 /*                   range -90 to 90 and -180 to 180, respec- */
15960 /*                   tively. */
15961 
15962 /*       A = Angular distance in degrees from E to the boun- */
15963 /*           dary of a circular window against which the */
15964 /*           triangulation is clipped.  The projected window */
15965 /*           is a disk of radius r = Sin(A) centered at the */
15966 /*           origin, and only visible nodes whose projections */
15967 /*           are within distance r of the origin are included */
15968 /*           in the plot.  Thus, if A = 90, the plot includes */
15969 /*           the entire hemisphere centered at E.  0 .LT. A */
15970 /*           .LE. 90. */
15971 
15972 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15973 
15974 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15975 /*               coordinates of the nodes (unit vectors). */
15976 
15977 /*       LIST,LPTR,LEND = Data structure defining the trian- */
15978 /*                        gulation.  Refer to Subroutine */
15979 /*                        TRMESH. */
15980 
15981 /*       TITLE = Type CHARACTER variable or constant contain- */
15982 /*               ing a string to be centered above the plot. */
15983 /*               The string must be enclosed in parentheses; */
15984 /*               i.e., the first and last characters must be */
15985 /*               '(' and ')', respectively, but these are not */
15986 /*               displayed.  TITLE may have at most 80 char- */
15987 /*               acters including the parentheses. */
15988 
15989 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
15990 /*               nodal indexes are plotted next to the nodes. */
15991 
15992 /* Input parameters are not altered by this routine. */
15993 
15994 /* On output: */
15995 
15996 /*       IER = Error indicator: */
15997 /*             IER = 0 if no errors were encountered. */
15998 /*             IER = 1 if LUN, PLTSIZ, or N is outside its */
15999 /*                     valid range. */
16000 /*             IER = 2 if ELAT, ELON, or A is outside its */
16001 /*                     valid range. */
16002 /*             IER = 3 if an error was encountered in writing */
16003 /*                     to unit LUN. */
16004 
16005 /*   The values in the data statement below may be altered */
16006 /* in order to modify various plotting options. */
16007 
16008 /* Module required by TRPLOT:  DRWARC */
16009 
16010 /* Intrinsic functions called by TRPLOT:  ABS, ATAN, COS, */
16011 /*                                          DBLE, NINT, SIN, */
16012 /*                                          SQRT */
16013 
16014 /* *********************************************************** */
16015 
16016 
16017     /* Parameter adjustments */
16018     --lend;
16019     --z__;
16020     --y;
16021     --x;
16022     --list;
16023     --lptr;
16024 
16025     /* Function Body */
16026 
16027 /* Local parameters: */
16028 
16029 /* ANNOT =     long int variable with value TRUE iff the plot */
16030 /*               is to be annotated with the values of ELAT, */
16031 /*               ELON, and A */
16032 /* CF =        Conversion factor for degrees to radians */
16033 /* CT =        Cos(ELAT) */
16034 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16035 /* FSIZN =     Font size in points for labeling nodes with */
16036 /*               their indexes if NUMBR = TRUE */
16037 /* FSIZT =     Font size in points for the title (and */
16038 /*               annotation if ANNOT = TRUE) */
16039 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16040 /*               left corner of the bounding box or viewport */
16041 /*               box */
16042 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16043 /*               right corner of the bounding box or viewport */
16044 /*               box */
16045 /* IR =        Half the width (height) of the bounding box or */
16046 /*               viewport box in points -- viewport radius */
16047 /* LP =        LIST index (pointer) */
16048 /* LPL =       Pointer to the last neighbor of N0 */
16049 /* N0 =        Index of a node whose incident arcs are to be */
16050 /*               drawn */
16051 /* N1 =        Neighbor of N0 */
16052 /* NSEG =      Number of line segments used by DRWARC in a */
16053 /*               polygonal approximation to a projected edge */
16054 /* P0 =        Coordinates of N0 in the rotated coordinate */
16055 /*               system or label location (first two */
16056 /*               components) */
16057 /* P1 =        Coordinates of N1 in the rotated coordinate */
16058 /*               system or intersection of edge N0-N1 with */
16059 /*               the equator (in the rotated coordinate */
16060 /*               system) */
16061 /* R11...R23 = Components of the first two rows of a rotation */
16062 /*               that maps E to the north pole (0,0,1) */
16063 /* SF =        Scale factor for mapping world coordinates */
16064 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16065 /*               to viewport coordinates in [IPX1,IPX2] X */
16066 /*               [IPY1,IPY2] */
16067 /* T =         Temporary variable */
16068 /* TOL =       Maximum distance in points between a projected */
16069 /*               triangulation edge and its approximation by */
16070 /*               a polygonal curve */
16071 /* TX,TY =     Translation vector for mapping world coordi- */
16072 /*               nates to viewport coordinates */
16073 /* WR =        Window radius r = Sin(A) */
16074 /* WRS =       WR**2 */
16075 
16076 
16077 /* Test for invalid parameters. */
16078 
16079     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16080         goto L11;
16081     }
16082     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16083         goto L12;
16084     }
16085 
16086 /* Compute a conversion factor CF for degrees to radians */
16087 /*   and compute the window radius WR. */
16088 
16089     cf = atan(1.) / 45.;
16090     wr = sin(cf * *a);
16091     wrs = wr * wr;
16092 
16093 /* Compute the lower left (IPX1,IPY1) and upper right */
16094 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16095 /*   The coordinates, specified in default user space units */
16096 /*   (points, at 72 points/inch with origin at the lower */
16097 /*   left corner of the page), are chosen to preserve the */
16098 /*   square aspect ratio, and to center the plot on the 8.5 */
16099 /*   by 11 inch page.  The center of the page is (306,396), */
16100 /*   and IR = PLTSIZ/2 in points. */
16101 
16102     d__1 = *pltsiz * 36.;
16103     ir = i_dnnt(&d__1);
16104     ipx1 = 306 - ir;
16105     ipx2 = ir + 306;
16106     ipy1 = 396 - ir;
16107     ipy2 = ir + 396;
16108 
16109 /* Output header comments. */
16110 
16111 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16112 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16113 /*     .        '%%BoundingBox:',4I4/ */
16114 /*     .        '%%Title:  Triangulation'/ */
16115 /*     .        '%%Creator:  STRIPACK'/ */
16116 /*     .        '%%EndComments') */
16117 
16118 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16119 /*   of a viewport box obtained by shrinking the bounding box */
16120 /*   by 12% in each dimension. */
16121 
16122     d__1 = (double) ir * .88;
16123     ir = i_dnnt(&d__1);
16124     ipx1 = 306 - ir;
16125     ipx2 = ir + 306;
16126     ipy1 = 396 - ir;
16127     ipy2 = ir + 396;
16128 
16129 /* Set the line thickness to 2 points, and draw the */
16130 /*   viewport boundary. */
16131 
16132     t = 2.;
16133 /*      WRITE (LUN,110,ERR=13) T */
16134 /*      WRITE (LUN,120,ERR=13) IR */
16135 /*      WRITE (LUN,130,ERR=13) */
16136 /*  110 FORMAT (F12.6,' setlinewidth') */
16137 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16138 /*  130 FORMAT ('stroke') */
16139 
16140 /* Set up an affine mapping from the window box [-WR,WR] X */
16141 /*   [-WR,WR] to the viewport box. */
16142 
16143     sf = (double) ir / wr;
16144     tx = ipx1 + sf * wr;
16145     ty = ipy1 + sf * wr;
16146 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16147 /*  140 FORMAT (2F12.6,' translate'/ */
16148 /*    .        2F12.6,' scale') */
16149 
16150 /* The line thickness must be changed to reflect the new */
16151 /*   scaling which is applied to all subsequent output. */
16152 /*   Set it to 1.0 point. */
16153 
16154     t = 1. / sf;
16155 /*      WRITE (LUN,110,ERR=13) T */
16156 
16157 /* Save the current graphics state, and set the clip path to */
16158 /*   the boundary of the window. */
16159 
16160 /*      WRITE (LUN,150,ERR=13) */
16161 /*      WRITE (LUN,160,ERR=13) WR */
16162 /*      WRITE (LUN,170,ERR=13) */
16163 /*  150 FORMAT ('gsave') */
16164 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
16165 /*  170 FORMAT ('clip newpath') */
16166 
16167 /* Compute the Cartesian coordinates of E and the components */
16168 /*   of a rotation R which maps E to the north pole (0,0,1). */
16169 /*   R is taken to be a rotation about the z-axis (into the */
16170 /*   yz-plane) followed by a rotation about the x-axis chosen */
16171 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
16172 /*   E is the north or south pole. */
16173 
16174 /*           ( R11  R12  0   ) */
16175 /*       R = ( R21  R22  R23 ) */
16176 /*           ( EX   EY   EZ  ) */
16177 
16178     t = cf * *elon;
16179     ct = cos(cf * *elat);
16180     ex = ct * cos(t);
16181     ey = ct * sin(t);
16182     ez = sin(cf * *elat);
16183     if (ct != 0.) {
16184         r11 = -ey / ct;
16185         r12 = ex / ct;
16186     } else {
16187         r11 = 0.;
16188         r12 = 1.;
16189     }
16190     r21 = -ez * r12;
16191     r22 = ez * r11;
16192     r23 = ct;
16193 
16194 /* Loop on visible nodes N0 that project to points */
16195 /*   (P0(1),P0(2)) in the window. */
16196 
16197     i__1 = *n;
16198     for (n0 = 1; n0 <= i__1; ++n0) {
16199         p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16200         if (p0[2] < 0.) {
16201             goto L3;
16202         }
16203         p0[0] = r11 * x[n0] + r12 * y[n0];
16204         p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16205         if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16206             goto L3;
16207         }
16208         lpl = lend[n0];
16209         lp = lpl;
16210 
16211 /* Loop on neighbors N1 of N0.  LPL points to the last */
16212 /*   neighbor of N0.  Copy the components of N1 into P. */
16213 
16214 L1:
16215         lp = lptr[lp];
16216         n1 = (i__2 = list[lp], abs(i__2));
16217         p1[0] = r11 * x[n1] + r12 * y[n1];
16218         p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16219         p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16220         if (p1[2] < 0.) {
16221 
16222 /*   N1 is a 'southern hemisphere' point.  Move it to the */
16223 /*     intersection of edge N0-N1 with the equator so that */
16224 /*     the edge is clipped properly.  P1(3) is set to 0. */
16225 
16226             p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16227             p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16228             t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16229             p1[0] /= t;
16230             p1[1] /= t;
16231         }
16232 
16233 /*   If node N1 is in the window and N1 < N0, bypass edge */
16234 /*     N0->N1 (since edge N1->N0 has already been drawn). */
16235 
16236         if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16237             goto L2;
16238         }
16239 
16240 /*   Add the edge to the path.  (TOL is converted to world */
16241 /*     coordinates.) */
16242 
16243         if (p1[2] < 0.) {
16244             p1[2] = 0.;
16245         }
16246         d__1 = tol / sf;
16247         drwarc_(lun, p0, p1, &d__1, &nseg);
16248 
16249 /* Bottom of loops. */
16250 
16251 L2:
16252         if (lp != lpl) {
16253             goto L1;
16254         }
16255 L3:
16256         ;
16257     }
16258 
16259 /* Paint the path and restore the saved graphics state (with */
16260 /*   no clip path). */
16261 
16262 /*      WRITE (LUN,130,ERR=13) */
16263 /*      WRITE (LUN,190,ERR=13) */
16264 /*  190 FORMAT ('grestore') */
16265     if (*numbr) {
16266 
16267 /* Nodes in the window are to be labeled with their indexes. */
16268 /*   Convert FSIZN from points to world coordinates, and */
16269 /*   output the commands to select a font and scale it. */
16270 
16271         t = fsizn / sf;
16272 /*        WRITE (LUN,200,ERR=13) T */
16273 /*  200   FORMAT ('/Helvetica findfont'/ */
16274 /*     .          F12.6,' scalefont setfont') */
16275 
16276 /* Loop on visible nodes N0 that project to points */
16277 /*   P0 = (P0(1),P0(2)) in the window. */
16278 
16279         i__1 = *n;
16280         for (n0 = 1; n0 <= i__1; ++n0) {
16281             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16282                 goto L4;
16283             }
16284             p0[0] = r11 * x[n0] + r12 * y[n0];
16285             p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16286             if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16287                 goto L4;
16288             }
16289 
16290 /*   Move to P0 and draw the label N0.  The first character */
16291 /*     will will have its lower left corner about one */
16292 /*     character width to the right of the nodal position. */
16293 
16294 /*          WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16295 /*          WRITE (LUN,220,ERR=13) N0 */
16296 /*  210     FORMAT (2F12.6,' moveto') */
16297 /*  220     FORMAT ('(',I3,') show') */
16298 L4:
16299             ;
16300         }
16301     }
16302 
16303 /* Convert FSIZT from points to world coordinates, and output */
16304 /*   the commands to select a font and scale it. */
16305 
16306     t = fsizt / sf;
16307 /*      WRITE (LUN,200,ERR=13) T */
16308 
16309 /* Display TITLE centered above the plot: */
16310 
16311     p0[1] = wr + t * 3.;
16312 /*      WRITE (LUN,230,ERR=13) TITLE, P0(2) */
16313 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
16314 /*     .        ' moveto') */
16315 /*      WRITE (LUN,240,ERR=13) TITLE */
16316 /*  240 FORMAT (A80/'  show') */
16317     if (annot) {
16318 
16319 /* Display the window center and radius below the plot. */
16320 
16321         p0[0] = -wr;
16322         p0[1] = -wr - 50. / sf;
16323 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16324 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
16325         p0[1] -= t * 2.;
16326 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16327 /*        WRITE (LUN,260,ERR=13) A */
16328 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
16329 /*     .          ',  ELON = ',F8.2,') show') */
16330 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
16331     }
16332 
16333 /* Paint the path and output the showpage command and */
16334 /*   end-of-file indicator. */
16335 
16336 /*      WRITE (LUN,270,ERR=13) */
16337 /*  270 FORMAT ('stroke'/ */
16338 /*     .        'showpage'/ */
16339 /*     .        '%%EOF') */
16340 
16341 /* HP's interpreters require a one-byte End-of-PostScript-Job */
16342 /*   indicator (to eliminate a timeout error message): */
16343 /*   ASCII 4. */
16344 
16345 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
16346 /*  280 FORMAT (A1) */
16347 
16348 /* No error encountered. */
16349 
16350     *ier = 0;
16351     return 0;
16352 
16353 /* Invalid input parameter LUN, PLTSIZ, or N. */
16354 
16355 L11:
16356     *ier = 1;
16357     return 0;
16358 
16359 /* Invalid input parameter ELAT, ELON, or A. */
16360 
16361 L12:
16362     *ier = 2;
16363     return 0;
16364 
16365 /* Error writing to unit LUN. */
16366 
16367 /* L13: */
16368     *ier = 3;
16369     return 0;
16370 } /* trplot_ */
16371 
16372 /* Subroutine */ int trprnt_(int *n, double *x, double *y,
16373         double *z__, int *iflag, int *list, int *lptr,
16374         int *lend, int *lout)
16375 {
16376     /* Initialized data */
16377 
16378     static int nmax = 9999;
16379     static int nlmax = 58;
16380 
16381     /* System generated locals */
16382     int i__1;
16383 
16384     /* Local variables */
16385     static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16386             400];
16387 
16388 
16389 /* *********************************************************** */
16390 
16391 /*                                              From STRIPACK */
16392 /*                                            Robert J. Renka */
16393 /*                                  Dept. of Computer Science */
16394 /*                                       Univ. of North Texas */
16395 /*                                           renka@cs.unt.edu */
16396 /*                                                   07/25/98 */
16397 
16398 /*   This subroutine prints the triangulation adjacency lists */
16399 /* created by Subroutine TRMESH and, optionally, the nodal */
16400 /* coordinates (either latitude and longitude or Cartesian */
16401 /* coordinates) on long int unit LOUT.  The list of neighbors */
16402 /* of a boundary node is followed by index 0.  The numbers of */
16403 /* boundary nodes, triangles, and arcs are also printed. */
16404 
16405 
16406 /* On input: */
16407 
16408 /*       N = Number of nodes in the triangulation.  N .GE. 3 */
16409 /*           and N .LE. 9999. */
16410 
16411 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16412 /*               coordinates of the nodes if IFLAG = 0, or */
16413 /*               (X and Y only) arrays of length N containing */
16414 /*               longitude and latitude, respectively, if */
16415 /*               IFLAG > 0, or unused dummy parameters if */
16416 /*               IFLAG < 0. */
16417 
16418 /*       IFLAG = Nodal coordinate option indicator: */
16419 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
16420 /*                         Cartesian coordinates) are to be */
16421 /*                         printed (to 6 decimal places). */
16422 /*               IFLAG > 0 if only X and Y (assumed to con- */
16423 /*                         tain longitude and latitude) are */
16424 /*                         to be printed (to 6 decimal */
16425 /*                         places). */
16426 /*               IFLAG < 0 if only the adjacency lists are to */
16427 /*                         be printed. */
16428 
16429 /*       LIST,LPTR,LEND = Data structure defining the trian- */
16430 /*                        gulation.  Refer to Subroutine */
16431 /*                        TRMESH. */
16432 
16433 /*       LOUT = long int unit for output.  If LOUT is not in */
16434 /*              the range 0 to 99, output is written to */
16435 /*              long int unit 6. */
16436 
16437 /* Input parameters are not altered by this routine. */
16438 
16439 /* On output: */
16440 
16441 /*   The adjacency lists and nodal coordinates (as specified */
16442 /* by IFLAG) are written to unit LOUT. */
16443 
16444 /* Modules required by TRPRNT:  None */
16445 
16446 /* *********************************************************** */
16447 
16448     /* Parameter adjustments */
16449     --lend;
16450     --z__;
16451     --y;
16452     --x;
16453     --list;
16454     --lptr;
16455 
16456     /* Function Body */
16457 
16458 /* Local parameters: */
16459 
16460 /* I =     NABOR index (1 to K) */
16461 /* INC =   Increment for NL associated with an adjacency list */
16462 /* K =     Counter and number of neighbors of NODE */
16463 /* LP =    LIST pointer of a neighbor of NODE */
16464 /* LPL =   Pointer to the last neighbor of NODE */
16465 /* LUN =   long int unit for output (copy of LOUT) */
16466 /* NA =    Number of arcs in the triangulation */
16467 /* NABOR = Array containing the adjacency list associated */
16468 /*           with NODE, with zero appended if NODE is a */
16469 /*           boundary node */
16470 /* NB =    Number of boundary nodes encountered */
16471 /* ND =    Index of a neighbor of NODE (or negative index) */
16472 /* NL =    Number of lines that have been printed on the */
16473 /*           current page */
16474 /* NLMAX = Maximum number of print lines per page (except */
16475 /*           for the last page which may have two addi- */
16476 /*           tional lines) */
16477 /* NMAX =  Upper bound on N (allows 4-digit indexes) */
16478 /* NODE =  Index of a node and DO-loop index (1 to N) */
16479 /* NN =    Local copy of N */
16480 /* NT =    Number of triangles in the triangulation */
16481 
16482     nn = *n;
16483     lun = *lout;
16484     if (lun < 0 || lun > 99) {
16485         lun = 6;
16486     }
16487 
16488 /* Print a heading and test the range of N. */
16489 
16490 /*      WRITE (LUN,100) NN */
16491     if (nn < 3 || nn > nmax) {
16492 
16493 /* N is outside its valid range. */
16494 
16495 /*        WRITE (LUN,110) */
16496         return 0;
16497     }
16498 
16499 /* Initialize NL (the number of lines printed on the current */
16500 /*   page) and NB (the number of boundary nodes encountered). */
16501 
16502     nl = 6;
16503     nb = 0;
16504     if (*iflag < 0) {
16505 
16506 /* Print LIST only.  K is the number of neighbors of NODE */
16507 /*   that have been stored in NABOR. */
16508 
16509 /*        WRITE (LUN,101) */
16510         i__1 = nn;
16511         for (node = 1; node <= i__1; ++node) {
16512             lpl = lend[node];
16513             lp = lpl;
16514             k = 0;
16515 
16516 L1:
16517             ++k;
16518             lp = lptr[lp];
16519             nd = list[lp];
16520             nabor[k - 1] = nd;
16521             if (lp != lpl) {
16522                 goto L1;
16523             }
16524             if (nd <= 0) {
16525 
16526 /*   NODE is a boundary node.  Correct the sign of the last */
16527 /*     neighbor, add 0 to the end of the list, and increment */
16528 /*     NB. */
16529 
16530                 nabor[k - 1] = -nd;
16531                 ++k;
16532                 nabor[k - 1] = 0;
16533                 ++nb;
16534             }
16535 
16536 /*   Increment NL and print the list of neighbors. */
16537 
16538             inc = (k - 1) / 14 + 2;
16539             nl += inc;
16540             if (nl > nlmax) {
16541 /*            WRITE (LUN,108) */
16542                 nl = inc;
16543             }
16544 /*          WRITE (LUN,104) NODE, (NABOR(I), I = 1,K) */
16545 /*          IF (K .NE. 14) */
16546 /*           WRITE (LUN,107) */
16547 /* L2: */
16548         }
16549     } else if (*iflag > 0) {
16550 
16551 /* Print X (longitude), Y (latitude), and LIST. */
16552 
16553 /*        WRITE (LUN,102) */
16554         i__1 = nn;
16555         for (node = 1; node <= i__1; ++node) {
16556             lpl = lend[node];
16557             lp = lpl;
16558             k = 0;
16559 
16560 L3:
16561             ++k;
16562             lp = lptr[lp];
16563             nd = list[lp];
16564             nabor[k - 1] = nd;
16565             if (lp != lpl) {
16566                 goto L3;
16567             }
16568             if (nd <= 0) {
16569 
16570 /*   NODE is a boundary node. */
16571 
16572                 nabor[k - 1] = -nd;
16573                 ++k;
16574                 nabor[k - 1] = 0;
16575                 ++nb;
16576             }
16577 
16578 /*   Increment NL and print X, Y, and NABOR. */
16579 
16580             inc = (k - 1) / 8 + 2;
16581             nl += inc;
16582             if (nl > nlmax) {
16583 /*            WRITE (LUN,108) */
16584                 nl = inc;
16585             }
16586 /*          WRITE (LUN,105) NODE, X(NODE), Y(NODE), (NABOR(I), I = 1,K) */
16587 /*          IF (K .NE. 8) */
16588 /*           PRINT *,K */
16589 /*           WRITE (LUN,107) */
16590 /* L4: */
16591         }
16592     } else {
16593 
16594 /* Print X, Y, Z, and LIST. */
16595 
16596 /*        WRITE (LUN,103) */
16597         i__1 = nn;
16598         for (node = 1; node <= i__1; ++node) {
16599             lpl = lend[node];
16600             lp = lpl;
16601             k = 0;
16602 
16603 L5:
16604             ++k;
16605             lp = lptr[lp];
16606             nd = list[lp];
16607             nabor[k - 1] = nd;
16608             if (lp != lpl) {
16609                 goto L5;
16610             }
16611             if (nd <= 0) {
16612 
16613 /*   NODE is a boundary node. */
16614 
16615                 nabor[k - 1] = -nd;
16616                 ++k;
16617                 nabor[k - 1] = 0;
16618                 ++nb;
16619             }
16620 
16621 /*   Increment NL and print X, Y, Z, and NABOR. */
16622 
16623             inc = (k - 1) / 5 + 2;
16624             nl += inc;
16625             if (nl > nlmax) {
16626 /*            WRITE (LUN,108) */
16627                 nl = inc;
16628             }
16629 /*          WRITE (LUN,106) NODE, X(NODE), Y(NODE),Z(NODE), (NABOR(I), I = 1,K) */
16630 /*          IF (K .NE. 5) */
16631 /*           print *,K */
16632 /*           WRITE (LUN,107) */
16633 /* L6: */
16634         }
16635     }
16636 
16637 /* Print NB, NA, and NT (boundary nodes, arcs, and */
16638 /*   triangles). */
16639 
16640     if (nb != 0) {
16641         na = nn * 3 - nb - 3;
16642         nt = (nn << 1) - nb - 2;
16643     } else {
16644         na = nn * 3 - 6;
16645         nt = (nn << 1) - 4;
16646     }
16647 /*      WRITE (LUN,109) NB, NA, NT */
16648     return 0;
16649 
16650 /* Print formats: */
16651 
16652 /*  100 FORMAT (///15X,'STRIPACK Triangulation Data ', */
16653 /*     .        'Structure,  N = ',I5//) */
16654 /*  101 FORMAT (1X,'Node',31X,'Neighbors of Node'//) */
16655 /*  102 FORMAT (1X,'Node',5X,'Longitude',6X,'Latitude', */
16656 /*     .        18X,'Neighbors of Node'//) */
16657 /*  103 FORMAT (1X,'Node',5X,'X(Node)',8X,'Y(Node)',8X, */
16658 /*     .        'Z(Node)',11X,'Neighbors of Node'//) */
16659 /*  104 FORMAT (1X,I4,4X,14I5/(1X,8X,14I5)) */
16660 /*  105 FORMAT (1X,I4,2D15.6,4X,8I5/(1X,38X,8I5)) */
16661 /*  106 FORMAT (1X,I4,3D15.6,4X,5I5/(1X,53X,5I5)) */
16662 /*  107 FORMAT (1X) */
16663 /*  108 FORMAT (///) */
16664 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
16665 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
16666 /*     .        ' Triangles') */
16667 /*  110 FORMAT (1X,10X,'*** N is outside its valid', */
16668 /*     .        ' range ***') */
16669 } /* trprnt_ */
16670 
16671 /* Subroutine */ int vrplot_(int *lun, double *pltsiz, double *
16672         elat, double *elon, double *a, int *n, double *x,
16673         double *y, double *z__, int *nt, int *listc, int *
16674         lptr, int *lend, double *xc, double *yc, double *zc,
16675         char *, long int *numbr, int *ier, short)
16676 {
16677     /* Initialized data */
16678 
16679     static long int annot = TRUE_;
16680     static double fsizn = 10.;
16681     static double fsizt = 16.;
16682     static double tol = .5;
16683 
16684     /* System generated locals */
16685     int i__1;
16686     double d__1;
16687 
16688     /* Builtin functions */
16689     //double atan(double), sin(double);
16690     //int i_dnnt(double *);
16691     //double cos(double), sqrt(double);
16692 
16693     /* Local variables */
16694     static double t;
16695     static int n0;
16696     static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16697             sf;
16698     static int ir, lp;
16699     static double ex, ey, ez, wr, tx, ty;
16700     static long int in1, in2;
16701     static int kv1, kv2, lpl;
16702     static double wrs;
16703     static int ipx1, ipx2, ipy1, ipy2, nseg;
16704     extern /* Subroutine */ int drwarc_(int *, double *, double *,
16705              double *, int *);
16706 
16707 
16708 /* *********************************************************** */
16709 
16710 /*                                              From STRIPACK */
16711 /*                                            Robert J. Renka */
16712 /*                                  Dept. of Computer Science */
16713 /*                                       Univ. of North Texas */
16714 /*                                           renka@cs.unt.edu */
16715 /*                                                   03/04/03 */
16716 
16717 /*   This subroutine creates a level-2 Encapsulated Post- */
16718 /* script (EPS) file containing a graphical depiction of a */
16719 /* Voronoi diagram of a set of nodes on the unit sphere. */
16720 /* The visible portion of the diagram is projected orthog- */
16721 /* onally onto the plane that contains the origin and has */
16722 /* normal defined by a user-specified eye-position. */
16723 
16724 /*   The parameters defining the Voronoi diagram may be com- */
16725 /* puted by Subroutine CRLIST. */
16726 
16727 
16728 /* On input: */
16729 
16730 /*       LUN = long int unit number in the range 0 to 99. */
16731 /*             The unit should be opened with an appropriate */
16732 /*             file name before the call to this routine. */
16733 
16734 /*       PLTSIZ = Plot size in inches.  A circular window in */
16735 /*                the projection plane is mapped to a circu- */
16736 /*                lar viewport with diameter equal to .88* */
16737 /*                PLTSIZ (leaving room for labels outside the */
16738 /*                viewport).  The viewport is centered on the */
16739 /*                8.5 by 11 inch page, and its boundary is */
16740 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
16741 
16742 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
16743 /*                   the center of projection E (the center */
16744 /*                   of the plot).  The projection plane is */
16745 /*                   the plane that contains the origin and */
16746 /*                   has E as unit normal.  In a rotated */
16747 /*                   coordinate system for which E is the */
16748 /*                   north pole, the projection plane con- */
16749 /*                   tains the equator, and only northern */
16750 /*                   hemisphere points are visible (from the */
16751 /*                   point at infinity in the direction E). */
16752 /*                   These are projected orthogonally onto */
16753 /*                   the projection plane (by zeroing the z- */
16754 /*                   component in the rotated coordinate */
16755 /*                   system).  ELAT and ELON must be in the */
16756 /*                   range -90 to 90 and -180 to 180, respec- */
16757 /*                   tively. */
16758 
16759 /*       A = Angular distance in degrees from E to the boun- */
16760 /*           dary of a circular window against which the */
16761 /*           Voronoi diagram is clipped.  The projected win- */
16762 /*           dow is a disk of radius r = Sin(A) centered at */
16763 /*           the origin, and only visible vertices whose */
16764 /*           projections are within distance r of the origin */
16765 /*           are included in the plot.  Thus, if A = 90, the */
16766 /*           plot includes the entire hemisphere centered at */
16767 /*           E.  0 .LT. A .LE. 90. */
16768 
16769 /*       N = Number of nodes (Voronoi centers) and Voronoi */
16770 /*           regions.  N .GE. 3. */
16771 
16772 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16773 /*               coordinates of the nodes (unit vectors). */
16774 
16775 /*       NT = Number of Voronoi region vertices (triangles, */
16776 /*            including those in the extended triangulation */
16777 /*            if the number of boundary nodes NB is nonzero): */
16778 /*            NT = 2*N-4. */
16779 
16780 /*       LISTC = Array of length 3*NT containing triangle */
16781 /*               indexes (indexes to XC, YC, and ZC) stored */
16782 /*               in 1-1 correspondence with LIST/LPTR entries */
16783 /*               (or entries that would be stored in LIST for */
16784 /*               the extended triangulation):  the index of */
16785 /*               triangle (N1,N2,N3) is stored in LISTC(K), */
16786 /*               LISTC(L), and LISTC(M), where LIST(K), */
16787 /*               LIST(L), and LIST(M) are the indexes of N2 */
16788 /*               as a neighbor of N1, N3 as a neighbor of N2, */
16789 /*               and N1 as a neighbor of N3.  The Voronoi */
16790 /*               region associated with a node is defined by */
16791 /*               the CCW-ordered sequence of circumcenters in */
16792 /*               one-to-one correspondence with its adjacency */
16793 /*               list (in the extended triangulation). */
16794 
16795 /*       LPTR = Array of length 3*NT = 6*N-12 containing a */
16796 /*              set of pointers (LISTC indexes) in one-to-one */
16797 /*              correspondence with the elements of LISTC. */
16798 /*              LISTC(LPTR(I)) indexes the triangle which */
16799 /*              follows LISTC(I) in cyclical counterclockwise */
16800 /*              order (the first neighbor follows the last */
16801 /*              neighbor). */
16802 
16803 /*       LEND = Array of length N containing a set of */
16804 /*              pointers to triangle lists.  LP = LEND(K) */
16805 /*              points to a triangle (indexed by LISTC(LP)) */
16806 /*              containing node K for K = 1 to N. */
16807 
16808 /*       XC,YC,ZC = Arrays of length NT containing the */
16809 /*                  Cartesian coordinates of the triangle */
16810 /*                  circumcenters (Voronoi vertices). */
16811 /*                  XC(I)**2 + YC(I)**2 + ZC(I)**2 = 1. */
16812 
16813 /*       TITLE = Type CHARACTER variable or constant contain- */
16814 /*               ing a string to be centered above the plot. */
16815 /*               The string must be enclosed in parentheses; */
16816 /*               i.e., the first and last characters must be */
16817 /*               '(' and ')', respectively, but these are not */
16818 /*               displayed.  TITLE may have at most 80 char- */
16819 /*               acters including the parentheses. */
16820 
16821 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
16822 /*               nodal indexes are plotted at the Voronoi */
16823 /*               region centers. */
16824 
16825 /* Input parameters are not altered by this routine. */
16826 
16827 /* On output: */
16828 
16829 /*       IER = Error indicator: */
16830 /*             IER = 0 if no errors were encountered. */
16831 /*             IER = 1 if LUN, PLTSIZ, N, or NT is outside */
16832 /*                     its valid range. */
16833 /*             IER = 2 if ELAT, ELON, or A is outside its */
16834 /*                     valid range. */
16835 /*             IER = 3 if an error was encountered in writing */
16836 /*                     to unit LUN. */
16837 
16838 /* Module required by VRPLOT:  DRWARC */
16839 
16840 /* Intrinsic functions called by VRPLOT:  ABS, ATAN, COS, */
16841 /*                                          DBLE, NINT, SIN, */
16842 /*                                          SQRT */
16843 
16844 /* *********************************************************** */
16845 
16846 
16847     /* Parameter adjustments */
16848     --lend;
16849     --z__;
16850     --y;
16851     --x;
16852     --zc;
16853     --yc;
16854     --xc;
16855     --listc;
16856     --lptr;
16857 
16858     /* Function Body */
16859 
16860 /* Local parameters: */
16861 
16862 /* ANNOT =     long int variable with value TRUE iff the plot */
16863 /*               is to be annotated with the values of ELAT, */
16864 /*               ELON, and A */
16865 /* CF =        Conversion factor for degrees to radians */
16866 /* CT =        Cos(ELAT) */
16867 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16868 /* FSIZN =     Font size in points for labeling nodes with */
16869 /*               their indexes if NUMBR = TRUE */
16870 /* FSIZT =     Font size in points for the title (and */
16871 /*               annotation if ANNOT = TRUE) */
16872 /* IN1,IN2 =   long int variables with value TRUE iff the */
16873 /*               projections of vertices KV1 and KV2, respec- */
16874 /*               tively, are inside the window */
16875 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16876 /*               left corner of the bounding box or viewport */
16877 /*               box */
16878 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16879 /*               right corner of the bounding box or viewport */
16880 /*               box */
16881 /* IR =        Half the width (height) of the bounding box or */
16882 /*               viewport box in points -- viewport radius */
16883 /* KV1,KV2 =   Endpoint indexes of a Voronoi edge */
16884 /* LP =        LIST index (pointer) */
16885 /* LPL =       Pointer to the last neighbor of N0 */
16886 /* N0 =        Index of a node */
16887 /* NSEG =      Number of line segments used by DRWARC in a */
16888 /*               polygonal approximation to a projected edge */
16889 /* P1 =        Coordinates of vertex KV1 in the rotated */
16890 /*               coordinate system */
16891 /* P2 =        Coordinates of vertex KV2 in the rotated */
16892 /*               coordinate system or intersection of edge */
16893 /*               KV1-KV2 with the equator (in the rotated */
16894 /*               coordinate system) */
16895 /* R11...R23 = Components of the first two rows of a rotation */
16896 /*               that maps E to the north pole (0,0,1) */
16897 /* SF =        Scale factor for mapping world coordinates */
16898 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16899 /*               to viewport coordinates in [IPX1,IPX2] X */
16900 /*               [IPY1,IPY2] */
16901 /* T =         Temporary variable */
16902 /* TOL =       Maximum distance in points between a projected */
16903 /*               Voronoi edge and its approximation by a */
16904 /*               polygonal curve */
16905 /* TX,TY =     Translation vector for mapping world coordi- */
16906 /*               nates to viewport coordinates */
16907 /* WR =        Window radius r = Sin(A) */
16908 /* WRS =       WR**2 */
16909 /* X0,Y0 =     Projection plane coordinates of node N0 or */
16910 /*               label location */
16911 
16912 
16913 /* Test for invalid parameters. */
16914 
16915     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
16916             nt != 2 * *n - 4) {
16917         goto L11;
16918     }
16919     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16920         goto L12;
16921     }
16922 
16923 /* Compute a conversion factor CF for degrees to radians */
16924 /*   and compute the window radius WR. */
16925 
16926     cf = atan(1.) / 45.;
16927     wr = sin(cf * *a);
16928     wrs = wr * wr;
16929 
16930 /* Compute the lower left (IPX1,IPY1) and upper right */
16931 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16932 /*   The coordinates, specified in default user space units */
16933 /*   (points, at 72 points/inch with origin at the lower */
16934 /*   left corner of the page), are chosen to preserve the */
16935 /*   square aspect ratio, and to center the plot on the 8.5 */
16936 /*   by 11 inch page.  The center of the page is (306,396), */
16937 /*   and IR = PLTSIZ/2 in points. */
16938 
16939     d__1 = *pltsiz * 36.;
16940     ir = i_dnnt(&d__1);
16941     ipx1 = 306 - ir;
16942     ipx2 = ir + 306;
16943     ipy1 = 396 - ir;
16944     ipy2 = ir + 396;
16945 
16946 /* Output header comments. */
16947 
16948 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16949 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16950 /*     .        '%%BoundingBox:',4I4/ */
16951 /*     .        '%%Title:  Voronoi diagram'/ */
16952 /*     .        '%%Creator:  STRIPACK'/ */
16953 /*     .        '%%EndComments') */
16954 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16955 /*   of a viewport box obtained by shrinking the bounding box */
16956 /*   by 12% in each dimension. */
16957 
16958     d__1 = (double) ir * .88;
16959     ir = i_dnnt(&d__1);
16960     ipx1 = 306 - ir;
16961     ipx2 = ir + 306;
16962     ipy1 = 396 - ir;
16963     ipy2 = ir + 396;
16964 
16965 /* Set the line thickness to 2 points, and draw the */
16966 /*   viewport boundary. */
16967 
16968     t = 2.;
16969 /*      WRITE (LUN,110,ERR=13) T */
16970 /*      WRITE (LUN,120,ERR=13) IR */
16971 /*      WRITE (LUN,130,ERR=13) */
16972 /*  110 FORMAT (F12.6,' setlinewidth') */
16973 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16974 /*  130 FORMAT ('stroke') */
16975 
16976 /* Set up an affine mapping from the window box [-WR,WR] X */
16977 /*   [-WR,WR] to the viewport box. */
16978 
16979     sf = (double) ir / wr;
16980     tx = ipx1 + sf * wr;
16981     ty = ipy1 + sf * wr;
16982 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16983 /*  140 FORMAT (2F12.6,' translate'/ */
16984 /*     .        2F12.6,' scale') */
16985 
16986 /* The line thickness must be changed to reflect the new */
16987 /*   scaling which is applied to all subsequent output. */
16988 /*   Set it to 1.0 point. */
16989 
16990     t = 1. / sf;
16991 /*      WRITE (LUN,110,ERR=13) T */
16992 
16993 /* Save the current graphics state, and set the clip path to */
16994 /*   the boundary of the window. */
16995 
16996 /*      WRITE (LUN,150,ERR=13) */
16997 /*      WRITE (LUN,160,ERR=13) WR */
16998 /*      WRITE (LUN,170,ERR=13) */
16999 /*  150 FORMAT ('gsave') */
17000 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
17001 /*  170 FORMAT ('clip newpath') */
17002 
17003 /* Compute the Cartesian coordinates of E and the components */
17004 /*   of a rotation R which maps E to the north pole (0,0,1). */
17005 /*   R is taken to be a rotation about the z-axis (into the */
17006 /*   yz-plane) followed by a rotation about the x-axis chosen */
17007 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
17008 /*   E is the north or south pole. */
17009 
17010 /*           ( R11  R12  0   ) */
17011 /*       R = ( R21  R22  R23 ) */
17012 /*           ( EX   EY   EZ  ) */
17013 
17014     t = cf * *elon;
17015     ct = cos(cf * *elat);
17016     ex = ct * cos(t);
17017     ey = ct * sin(t);
17018     ez = sin(cf * *elat);
17019     if (ct != 0.) {
17020         r11 = -ey / ct;
17021         r12 = ex / ct;
17022     } else {
17023         r11 = 0.;
17024         r12 = 1.;
17025     }
17026     r21 = -ez * r12;
17027     r22 = ez * r11;
17028     r23 = ct;
17029 
17030 /* Loop on nodes (Voronoi centers) N0. */
17031 /*   LPL indexes the last neighbor of N0. */
17032 
17033     i__1 = *n;
17034     for (n0 = 1; n0 <= i__1; ++n0) {
17035         lpl = lend[n0];
17036 
17037 /* Set KV2 to the first (and last) vertex index and compute */
17038 /*   its coordinates P2 in the rotated coordinate system. */
17039 
17040         kv2 = listc[lpl];
17041         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17042         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17043         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17044 
17045 /*   IN2 = TRUE iff KV2 is in the window. */
17046 
17047         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17048 
17049 /* Loop on neighbors N1 of N0.  For each triangulation edge */
17050 /*   N0-N1, KV1-KV2 is the corresponding Voronoi edge. */
17051 
17052         lp = lpl;
17053 L1:
17054         lp = lptr[lp];
17055         kv1 = kv2;
17056         p1[0] = p2[0];
17057         p1[1] = p2[1];
17058         p1[2] = p2[2];
17059         in1 = in2;
17060         kv2 = listc[lp];
17061 
17062 /*   Compute the new values of P2 and IN2. */
17063 
17064         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17065         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17066         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17067         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17068 
17069 /* Add edge KV1-KV2 to the path iff both endpoints are inside */
17070 /*   the window and KV2 > KV1, or KV1 is inside and KV2 is */
17071 /*   outside (so that the edge is drawn only once). */
17072 
17073         if (! in1 || in2 && kv2 <= kv1) {
17074             goto L2;
17075         }
17076         if (p2[2] < 0.) {
17077 
17078 /*   KV2 is a 'southern hemisphere' point.  Move it to the */
17079 /*     intersection of edge KV1-KV2 with the equator so that */
17080 /*     the edge is clipped properly.  P2(3) is set to 0. */
17081 
17082             p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17083             p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17084             t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17085             p2[0] /= t;
17086             p2[1] /= t;
17087         }
17088 
17089 /*   Add the edge to the path.  (TOL is converted to world */
17090 /*     coordinates.) */
17091 
17092         if (p2[2] < 0.) {
17093             p2[2] = 0.f;
17094         }
17095         d__1 = tol / sf;
17096         drwarc_(lun, p1, p2, &d__1, &nseg);
17097 
17098 /* Bottom of loops. */
17099 
17100 L2:
17101         if (lp != lpl) {
17102             goto L1;
17103         }
17104 /* L3: */
17105     }
17106 
17107 /* Paint the path and restore the saved graphics state (with */
17108 /*   no clip path). */
17109 
17110 /*      WRITE (LUN,130,ERR=13) */
17111 /*      WRITE (LUN,190,ERR=13) */
17112 /*  190 FORMAT ('grestore') */
17113     if (*numbr) {
17114 
17115 /* Nodes in the window are to be labeled with their indexes. */
17116 /*   Convert FSIZN from points to world coordinates, and */
17117 /*   output the commands to select a font and scale it. */
17118 
17119         t = fsizn / sf;
17120 /*        WRITE (LUN,200,ERR=13) T */
17121 /*  200   FORMAT ('/Helvetica findfont'/ */
17122 /*     .          F12.6,' scalefont setfont') */
17123 
17124 /* Loop on visible nodes N0 that project to points (X0,Y0) in */
17125 /*   the window. */
17126 
17127         i__1 = *n;
17128         for (n0 = 1; n0 <= i__1; ++n0) {
17129             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17130                 goto L4;
17131             }
17132             x0 = r11 * x[n0] + r12 * y[n0];
17133             y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17134             if (x0 * x0 + y0 * y0 > wrs) {
17135                 goto L4;
17136             }
17137 
17138 /*   Move to (X0,Y0), and draw the label N0 with the origin */
17139 /*     of the first character at (X0,Y0). */
17140 
17141 /*          WRITE (LUN,210,ERR=13) X0, Y0 */
17142 /*          WRITE (LUN,220,ERR=13) N0 */
17143 /*  210     FORMAT (2F12.6,' moveto') */
17144 /*  220     FORMAT ('(',I3,') show') */
17145 L4:
17146             ;
17147         }
17148     }
17149 
17150 /* Convert FSIZT from points to world coordinates, and output */
17151 /*   the commands to select a font and scale it. */
17152 
17153     t = fsizt / sf;
17154 /*      WRITE (LUN,200,ERR=13) T */
17155 
17156 /* Display TITLE centered above the plot: */
17157 
17158     y0 = wr + t * 3.;
17159 /*      WRITE (LUN,230,ERR=13) TITLE, Y0 */
17160 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
17161 /*     .        ' moveto') */
17162 /*      WRITE (LUN,240,ERR=13) TITLE */
17163 /*  240 FORMAT (A80/'  show') */
17164     if (annot) {
17165 
17166 /* Display the window center and radius below the plot. */
17167 
17168         x0 = -wr;
17169         y0 = -wr - 50. / sf;
17170 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17171 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
17172         y0 -= t * 2.;
17173 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17174 /*        WRITE (LUN,260,ERR=13) A */
17175 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
17176 /*     .          ',  ELON = ',F8.2,') show') */
17177 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
17178     }
17179 
17180 /* Paint the path and output the showpage command and */
17181 /*   end-of-file indicator. */
17182 
17183 /*      WRITE (LUN,270,ERR=13) */
17184 /*  270 FORMAT ('stroke'/ */
17185 /*     .        'showpage'/ */
17186 /*     .        '%%EOF') */
17187 
17188 /* HP's interpreters require a one-byte End-of-PostScript-Job */
17189 /*   indicator (to eliminate a timeout error message): */
17190 /*   ASCII 4. */
17191 
17192 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
17193 /*  280 FORMAT (A1) */
17194 
17195 /* No error encountered. */
17196 
17197     *ier = 0;
17198     return 0;
17199 
17200 /* Invalid input parameter LUN, PLTSIZ, N, or NT. */
17201 
17202 L11:
17203     *ier = 1;
17204     return 0;
17205 
17206 /* Invalid input parameter ELAT, ELON, or A. */
17207 
17208 L12:
17209     *ier = 2;
17210     return 0;
17211 
17212 /* Error writing to unit LUN. */
17213 
17214 /* L13: */
17215     *ier = 3;
17216     return 0;
17217 } /* vrplot_ */
17218 
17219 /* Subroutine */ int random_(int *ix, int *iy, int *iz,
17220         double *rannum)
17221 {
17222     static double x;
17223 
17224 
17225 /*   This routine returns pseudo-random numbers uniformly */
17226 /* distributed in the interval (0,1).  int seeds IX, IY, */
17227 /* and IZ should be initialized to values in the range 1 to */
17228 /* 30,000 before the first call to RANDOM, and should not */
17229 /* be altered between subsequent calls (unless a sequence */
17230 /* of random numbers is to be repeated by reinitializing the */
17231 /* seeds). */
17232 
17233 /* Reference:  B. A. Wichmann and I. D. Hill, An Efficient */
17234 /*             and Portable Pseudo-random Number Generator, */
17235 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
17236 /*             pp. 188-190. */
17237 
17238     *ix = *ix * 171 % 30269;
17239     *iy = *iy * 172 % 30307;
17240     *iz = *iz * 170 % 30323;
17241     x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17242             double) (*iz) / 30323.;
17243     *rannum = x - (int) x;
17244     return 0;
17245 } /* random_ */
17246 
17247 #undef TRUE_
17248 #undef FALSE_
17249 #undef abs
17250 
17251 /*################################################################################################
17252 ##########  strid.f -- translated by f2c (version 20030320). ###################################
17253 ######   You must link the resulting object file with the libraries: #############################
17254 ####################    -lf2c -lm   (in that order)   ############################################
17255 ################################################################################################*/
17256 
17257 
17258 
17259 EMData* Util::mult_scalar(EMData* img, float scalar)
17260 {
17261         ENTERFUNC;
17262         /* Exception Handle */
17263         if (!img) {
17264                 throw NullPointerException("NULL input image");
17265         }
17266         /* ============  output = scalar*input  ================== */
17267 
17268         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17269         int size = nx*ny*nz;
17270         EMData * img2 = img->copy_head();
17271         float *img_ptr  =img->get_data();
17272         float *img2_ptr = img2->get_data();
17273         for (int i=0;i<size;i++)img2_ptr[i] = img_ptr[i]*scalar;
17274         img2->update();
17275 
17276         if(img->is_complex()) {
17277                 img2->set_complex(true);
17278                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17279         }
17280         EXITFUNC;
17281         return img2;
17282 }
17283 
17284 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17285 {
17286         ENTERFUNC;
17287         /* Exception Handle */
17288         if (!img) {
17289                 throw NullPointerException("NULL input image");
17290         }
17291         /* ==============   output = img + scalar*img1   ================ */
17292 
17293         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17294         int size = nx*ny*nz;
17295         EMData * img2 = img->copy_head();
17296         float *img_ptr  =img->get_data();
17297         float *img2_ptr = img2->get_data();
17298         float *img1_ptr = img1->get_data();
17299         for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17300         img2->update();
17301         if(img->is_complex()) {
17302                 img2->set_complex(true);
17303                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17304         }
17305 
17306         EXITFUNC;
17307         return img2;
17308 }
17309 
17310 EMData* Util::addn_img(EMData* img, EMData* img1)
17311 {
17312         ENTERFUNC;
17313         /* Exception Handle */
17314         if (!img) {
17315                 throw NullPointerException("NULL input image");
17316         }
17317         /* ==============   output = img + img1   ================ */
17318 
17319         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17320         int size = nx*ny*nz;
17321         EMData * img2 = img->copy_head();
17322         float *img_ptr  =img->get_data();
17323         float *img2_ptr = img2->get_data();
17324         float *img1_ptr = img1->get_data();
17325         for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17326         img2->update();
17327         if(img->is_complex()) {
17328                 img2->set_complex(true);
17329                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17330         }
17331 
17332         EXITFUNC;
17333         return img2;
17334 }
17335 
17336 EMData* Util::subn_img(EMData* img, EMData* img1)
17337 {
17338         ENTERFUNC;
17339         /* Exception Handle */
17340         if (!img) {
17341                 throw NullPointerException("NULL input image");
17342         }
17343         /* ==============   output = img - img1   ================ */
17344 
17345         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17346         int size = nx*ny*nz;
17347         EMData * img2 = img->copy_head();
17348         float *img_ptr  =img->get_data();
17349         float *img2_ptr = img2->get_data();
17350         float *img1_ptr = img1->get_data();
17351         for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17352         img2->update();
17353         if(img->is_complex()) {
17354                 img2->set_complex(true);
17355                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17356         }
17357 
17358         EXITFUNC;
17359         return img2;
17360 }
17361 
17362 EMData* Util::muln_img(EMData* img, EMData* img1)
17363 {
17364         ENTERFUNC;
17365         /* Exception Handle */
17366         if (!img) {
17367                 throw NullPointerException("NULL input image");
17368         }
17369         /* ==============   output = img * img1   ================ */
17370 
17371         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17372         int size = nx*ny*nz;
17373         EMData * img2 = img->copy_head();
17374         float *img_ptr  =img->get_data();
17375         float *img2_ptr = img2->get_data();
17376         float *img1_ptr = img1->get_data();
17377         if(img->is_complex()) {
17378                 for (int i=0; i<size; i+=2) {
17379                         img2_ptr[i]   = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17380                         img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17381                 }
17382                 img2->set_complex(true);
17383                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17384         } else {
17385                 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17386                 img2->update();
17387         }
17388 
17389         EXITFUNC;
17390         return img2;
17391 }
17392 
17393 EMData* Util::divn_img(EMData* img, EMData* img1)
17394 {
17395         ENTERFUNC;
17396         /* Exception Handle */
17397         if (!img) {
17398                 throw NullPointerException("NULL input image");
17399         }
17400         /* ==============   output = img / img1   ================ */
17401 
17402         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17403         int size = nx*ny*nz;
17404         EMData * img2 = img->copy_head();
17405         float *img_ptr  =img->get_data();
17406         float *img2_ptr = img2->get_data();
17407         float *img1_ptr = img1->get_data();
17408         if(img->is_complex()) {
17409                 float  sq2;
17410                 for (int i=0; i<size; i+=2) {
17411                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17412                         img2_ptr[i]   = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17413                         img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17414                 }
17415                 img2->set_complex(true);
17416                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17417         } else {
17418                 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17419                 img2->update();
17420         }
17421 
17422         EXITFUNC;
17423         return img2;
17424 }
17425 
17426 EMData* Util::divn_filter(EMData* img, EMData* img1)
17427 {
17428         ENTERFUNC;
17429         /* Exception Handle */
17430         if (!img) {
17431                 throw NullPointerException("NULL input image");
17432         }
17433         /* ========= img /= img1 ===================== */
17434 
17435         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17436         int size = nx*ny*nz;
17437         EMData * img2 = img->copy_head();
17438         float *img_ptr  =img->get_data();
17439         float *img1_ptr = img1->get_data();
17440         float *img2_ptr = img2->get_data();
17441         if(img->is_complex()) {
17442                 for (int i=0; i<size; i+=2) {
17443                         if(img1_ptr[i] > 1.e-10f) {
17444                         img2_ptr[i]   = img_ptr[i]  /img1_ptr[i];
17445                         img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17446                         } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17447                 }
17448         } else  throw ImageFormatException("Only Fourier image allowed");
17449 
17450         img->update();
17451 
17452         EXITFUNC;
17453         return img2;
17454 }
17455 
17456 void Util::mul_scalar(EMData* img, float scalar)
17457 {
17458         ENTERFUNC;
17459         /* Exception Handle */
17460         if (!img) {
17461                 throw NullPointerException("NULL input image");
17462         }
17463         /* ============  output = scalar*input  ================== */
17464 
17465         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17466         int size = nx*ny*nz;
17467         float *img_ptr  =img->get_data();
17468         for (int i=0;i<size;i++) img_ptr[i] *= scalar;
17469         img->update();
17470 
17471         EXITFUNC;
17472 }
17473 
17474 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17475 {
17476         ENTERFUNC;
17477         /* Exception Handle */
17478         if (!img) {
17479                 throw NullPointerException("NULL input image");
17480         }
17481         /* ==============   img += scalar*img1   ================ */
17482 
17483         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17484         int size = nx*ny*nz;
17485         float *img_ptr  =img->get_data();
17486         float *img1_ptr = img1->get_data();
17487         for (int i=0;i<size;i++)img_ptr[i] += img1_ptr[i]*scalar;
17488         img1->update();
17489 
17490         EXITFUNC;
17491 }
17492 
17493 void Util::add_img(EMData* img, EMData* img1)
17494 {
17495         ENTERFUNC;
17496         /* Exception Handle */
17497         if (!img) {
17498                 throw NullPointerException("NULL input image");
17499         }
17500         /* ========= img += img1 ===================== */
17501 
17502         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17503         int size = nx*ny*nz;
17504         float *img_ptr  = img->get_data();
17505         float *img1_ptr = img1->get_data();
17506         for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i];
17507         img->update();
17508 
17509         EXITFUNC;
17510 }
17511 
17512 void Util::add_img_abs(EMData* img, EMData* img1)
17513 {
17514         ENTERFUNC;
17515         /* Exception Handle */
17516         if (!img) {
17517                 throw NullPointerException("NULL input image");
17518         }
17519         /* ========= img += img1 ===================== */
17520 
17521         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17522         int size = nx*ny*nz;
17523         float *img_ptr  = img->get_data();
17524         float *img1_ptr = img1->get_data();
17525         for (int i=0;i<size;i++) img_ptr[i] += abs(img1_ptr[i]);
17526         img->update();
17527 
17528         EXITFUNC;
17529 }
17530 
17531 void Util::add_img2(EMData* img, EMData* img1)
17532 {
17533         ENTERFUNC;
17534         /* Exception Handle */
17535         if (!img) {
17536                 throw NullPointerException("NULL input image");
17537         }
17538         /* ========= img += img1**2 ===================== */
17539 
17540         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17541         int size = nx*ny*nz;
17542         float *img_ptr  = img->get_data();
17543         float *img1_ptr = img1->get_data();
17544         if(img->is_complex()) {
17545                 for (int i=0; i<size; i+=2) img_ptr[i] += img1_ptr[i] * img1_ptr[i] + img1_ptr[i+1] * img1_ptr[i+1] ;
17546         } else {
17547                 for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17548         }
17549         img->update();
17550 
17551         EXITFUNC;
17552 }
17553 
17554 void Util::sub_img(EMData* img, EMData* img1)
17555 {
17556         ENTERFUNC;
17557         /* Exception Handle */
17558         if (!img) {
17559                 throw NullPointerException("NULL input image");
17560         }
17561         /* ========= img -= img1 ===================== */
17562 
17563         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17564         int size = nx*ny*nz;
17565         float *img_ptr  = img->get_data();
17566         float *img1_ptr = img1->get_data();
17567         for (int i=0;i<size;i++) img_ptr[i] -= img1_ptr[i];
17568         img->update();
17569 
17570         EXITFUNC;
17571 }
17572 
17573 void Util::mul_img(EMData* img, EMData* img1)
17574 {
17575         ENTERFUNC;
17576         /* Exception Handle */
17577         if (!img) {
17578                 throw NullPointerException("NULL input image");
17579         }
17580         /* ========= img *= img1 ===================== */
17581 
17582         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17583         int size = nx*ny*nz;
17584         float *img_ptr  = img->get_data();
17585         float *img1_ptr = img1->get_data();
17586         if(img->is_complex()) {
17587                 for (int i=0; i<size; i+=2) {
17588                         float tmp     = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17589                         img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17590                         img_ptr[i]   = tmp;
17591 
17592                 }
17593         } else {
17594                 for (int i=0;i<size;i++) img_ptr[i] *= img1_ptr[i];
17595         }
17596         img->update();
17597 
17598         EXITFUNC;
17599 }
17600 
17601 void Util::div_img(EMData* img, EMData* img1)
17602 {
17603         ENTERFUNC;
17604         /* Exception Handle */
17605         if (!img) {
17606                 throw NullPointerException("NULL input image");
17607         }
17608         /* ========= img /= img1 ===================== */
17609 
17610         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17611         int size = nx*ny*nz;
17612         float *img_ptr  = img->get_data();
17613         float *img1_ptr = img1->get_data();
17614         if(img->is_complex()) {
17615                 float  sq2;
17616                 for (int i=0; i<size; i+=2) {
17617                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17618                         float tmp    = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17619                         img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17620                         img_ptr[i]   = tmp;
17621                 }
17622         } else {
17623                 for (int i=0; i<size; i++) img_ptr[i] /= img1_ptr[i];
17624         }
17625         img->update();
17626 
17627         EXITFUNC;
17628 }
17629 
17630 void Util::div_filter(EMData* img, EMData* img1)
17631 {
17632         ENTERFUNC;
17633         /* Exception Handle */
17634         if (!img) {
17635                 throw NullPointerException("NULL input image");
17636         }
17637         /* ========= img /= img1 ===================== */
17638 
17639         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17640         int size = nx*ny*nz;
17641         float *img_ptr  = img->get_data();
17642         float *img1_ptr = img1->get_data();
17643         if(img->is_complex()) {
17644                 for (int i=0; i<size; i+=2) {
17645                         if(img1_ptr[i] > 1.e-10f) {
17646                         img_ptr[i]   /= img1_ptr[i];
17647                         img_ptr[i+1] /= img1_ptr[i];
17648                         } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17649                 }
17650         } else throw ImageFormatException("Only Fourier image allowed");
17651 
17652         img->update();
17653 
17654         EXITFUNC;
17655 }
17656 
17657 #define img_ptr(i,j,k)  img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*nxo]
17658 
17659 EMData* Util::pack_complex_to_real(EMData* img)
17660 {
17661         ENTERFUNC;
17662         /* Exception Handle */
17663         if (!img) {
17664                 throw NullPointerException("NULL input image");
17665         }
17666         /* ==============   img is modulus of a complex image in FFT format (so its imaginary parts are zero),
17667                               output is img packed into real image with Friedel part added,   ================ */
17668 
17669         int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17670         int nx = nxo - 2 + img->is_fftodd();
17671         int lsd2 = (nx + 2 - nx%2) / 2; // Extended x-dimension of the complex image
17672         int nyt, nzt;
17673         int nx2 = nx/2;
17674         int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17675         int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17676         int nx2p = nx2+nx%2;
17677         int ny2p = ny2+ny%2;
17678         int nz2p = nz2+nz%2;
17679         EMData& power = *(new EMData()); // output image
17680         power.set_size(nx, ny, nz);
17681         power.set_array_offsets(-nx2,-ny2,-nz2);
17682         //img->set_array_offsets(1,1,1);
17683         float *img_ptr  = img->get_data();
17684         for (int iz = 1; iz <= nz; iz++) {
17685                 int jz=iz-1;
17686                 if(jz>=nz2p) jz=jz-nzt;
17687                 for (int iy = 1; iy <= ny; iy++) {
17688                         int jy=iy-1;
17689                         if(jy>=ny2p) jy=jy-nyt;
17690                         for (int ix = 1; ix <= lsd2; ix++) {
17691                                 int jx=ix-1;
17692                                 if(jx>=nx2p) jx=jx-nx;
17693                                 power(jx,jy,jz) = img_ptr(ix,iy,iz); //real(img->cmplx(ix,iy,iz));
17694                         }
17695                 }
17696         }
17697 //  Create the Friedel related half
17698         int  nzb, nze, nyb, nye, nxb, nxe;
17699         nxb =-nx2+(nx+1)%2;
17700         nxe = nx2-(nx+1)%2;
17701         if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17702         if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17703         for (int iz = nzb; iz <= nze; iz++) {
17704                 for (int iy = nyb; iy <= nye; iy++) {
17705                         for (int ix = 1; ix <= nxe; ix++) { // Note this loop begins with 1 - FFT should create correct Friedel related 0 plane
17706                                 power(-ix,-iy,-iz) = power(ix,iy,iz);
17707                         }
17708                 }
17709         }
17710         if(ny2 != 0)  {
17711                 if(nz2 != 0)  {
17712                         if(nz%2 == 0) {  //if nz even, fix the first slice
17713                                 for (int iy = nyb; iy <= nye; iy++) {
17714                                         for (int ix = nxb; ix <= -1; ix++) {
17715                                                 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17716                                         }
17717                                 }
17718                                 if(ny%2 == 0) {  //if ny even, fix the first line
17719                                         for (int ix = nxb; ix <= -1; ix++) {
17720                                                 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17721                                         }
17722                                 }
17723                         }
17724                 }
17725                 if(ny%2 == 0) {  //if ny even, fix the first column
17726                         for (int iz = nzb; iz <= nze; iz++) {
17727                                 for (int ix = nxb; ix <= -1; ix++) {
17728                                         power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17729                                 }
17730                         }
17731                 }
17732 
17733         }
17734         power.update();
17735         power.set_array_offsets(0,0,0);
17736         return &power;
17737 }
17738 #undef  img_ptr
17739 
17740 float Util::ang_n(float peakp, string mode, int maxrin)
17741 {
17742     if (mode == "f" || mode == "F")
17743         return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17744     else
17745         return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17746 }
17747 
17748 
17749 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17750 {
17751     float* data = ring->get_data();
17752     float av=0.0;
17753     float sq=0.0;
17754     float nn=0.0;
17755     int nring = numr.size()/3;
17756     for( int i=0; i < nring; ++i )
17757     {
17758         int numr3i = numr[3*i+2];
17759         int numr2i = numr[3*i+1]-1;
17760         float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17761         for( int j=0; j < numr3i; ++j )
17762         {
17763             int jc = numr2i+j;
17764             av += data[jc] * w;
17765             sq += data[jc] * data[jc] * w;
17766             nn += w;
17767         }
17768     }
17769 
17770     float avg = av/nn;
17771     float sgm = sqrt( (sq-av*av/nn)/nn );
17772     int n = ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17773     for( int i=0; i < n; ++i )
17774     {
17775         data[i] -= avg;
17776         data[i] /= sgm;
17777     }
17778 
17779     ring->update();
17780 }
17781 
17782 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17783                 float xrng, float yrng, float step, string mode,
17784                 vector<int>numr, float cnx, float cny) {
17785 
17786     // Manually extract.
17787 /*    vector< EMAN::EMData* > crefim;
17788     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17789     crefim.reserve(crefim_len);
17790 
17791     for(std::size_t i=0;i<crefim_len;i++) {
17792         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17793         crefim.push_back(proxy());
17794     }
17795 */
17796 
17797         size_t crefim_len = crefim.size();
17798 
17799         int   ky = int(2*yrng/step+0.5)/2;
17800         int   kx = int(2*xrng/step+0.5)/2;
17801         int   iref, nref=0, mirror=0;
17802         float iy, ix, sx=0, sy=0;
17803         float peak = -1.0E23f;
17804         float ang=0.0f;
17805         for (int i = -ky; i <= ky; i++) {
17806                 iy = i * step ;
17807                 for (int j = -kx; j <= kx; j++) {
17808                         ix = j*step ;
17809                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17810 
17811                         Normalize_ring( cimage, numr );
17812 
17813                         Frngs(cimage, numr);
17814                         //  compare with all reference images
17815                         // for iref in xrange(len(crefim)):
17816                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17817                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17818                                 double qn = retvals["qn"];
17819                                 double qm = retvals["qm"];
17820                                 if(qn >= peak || qm >= peak) {
17821                                         sx = -ix;
17822                                         sy = -iy;
17823                                         nref = iref;
17824                                         if (qn >= qm) {
17825                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17826                                                 peak = static_cast<float>(qn);
17827                                                 mirror = 0;
17828                                         } else {
17829                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17830                                                 peak = static_cast<float>(qm);
17831                                                 mirror = 1;
17832                                         }
17833                                 }
17834                         }  delete cimage; cimage = 0;
17835                 }
17836         }
17837         float co, so, sxs, sys;
17838         co = static_cast<float>( cos(ang*pi/180.0) );
17839         so = static_cast<float>( -sin(ang*pi/180.0) );
17840         sxs = sx*co - sy*so;
17841         sys = sx*so + sy*co;
17842         vector<float> res;
17843         res.push_back(ang);
17844         res.push_back(sxs);
17845         res.push_back(sys);
17846         res.push_back(static_cast<float>(mirror));
17847         res.push_back(static_cast<float>(nref));
17848         res.push_back(peak);
17849         return res;
17850 }
17851 
17852 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
17853                 float xrng, float yrng, float step, string mode,
17854                 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
17855 
17856     // Manually extract.
17857 /*    vector< EMAN::EMData* > crefim;
17858     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17859     crefim.reserve(crefim_len);
17860 
17861     for(std::size_t i=0;i<crefim_len;i++) {
17862         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17863         crefim.push_back(proxy());
17864     }
17865 */
17866 
17867         size_t crefim_len = crefim.size();
17868 
17869         int   ky = int(2*yrng/step+0.5)/2;
17870         int   kx = int(2*xrng/step+0.5)/2;
17871         int   iref, nref=0, mirror=0;
17872         float iy, ix, sx=0, sy=0;
17873         float peak = -1.0E23f;
17874         float ang=0.0f;
17875         for (int i = -ky; i <= ky; i++) {
17876                 iy = i * step ;
17877                 for (int j = -kx; j <= kx; j++) {
17878                         ix = j*step ;
17879                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17880 
17881                         Normalize_ring( cimage, numr );
17882 
17883                         Frngs(cimage, numr);
17884                         //  compare with all reference images
17885                         // for iref in xrange(len(crefim)):
17886                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17887                                 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
17888                                 double qn = retvals["qn"];
17889                                 double qm = retvals["qm"];
17890                                 if(qn >= peak || qm >= peak) {
17891                                         sx = -ix;
17892                                         sy = -iy;
17893                                         nref = iref;
17894                                         if (qn >= qm) {
17895                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17896                                                 peak = static_cast<float>(qn);
17897                                                 mirror = 0;
17898                                         } else {
17899                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17900                                                 peak = static_cast<float>(qm);
17901                                                 mirror = 1;
17902                                         }
17903                                 }
17904                         }  delete cimage; cimage = 0;
17905                 }
17906         }
17907         float co, so, sxs, sys;
17908         co = static_cast<float>( cos(ang*pi/180.0) );
17909         so = static_cast<float>( -sin(ang*pi/180.0) );
17910         sxs = sx*co - sy*so;
17911         sys = sx*so + sy*co;
17912         vector<float> res;
17913         res.push_back(ang);
17914         res.push_back(sxs);
17915         res.push_back(sys);
17916         res.push_back(static_cast<float>(mirror));
17917         res.push_back(static_cast<float>(nref));
17918         res.push_back(peak);
17919         return res;
17920 }
17921 
17922 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
17923                 float xrng, float yrng, float step, string mode,
17924                 vector< int >numr, float cnx, float cny) {
17925 
17926     // Manually extract.
17927 /*    vector< EMAN::EMData* > crefim;
17928     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17929     crefim.reserve(crefim_len);
17930 
17931     for(std::size_t i=0;i<crefim_len;i++) {
17932         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17933         crefim.push_back(proxy());
17934     }
17935 */
17936         size_t crefim_len = crefim.size();
17937 
17938         int   ky = int(2*yrng/step+0.5)/2;
17939         int   kx = int(2*xrng/step+0.5)/2;
17940         int   iref, nref=0;
17941         float iy, ix, sx=0, sy=0;
17942         float peak = -1.0E23f;
17943         float ang=0.0f;
17944         for (int i = -ky; i <= ky; i++) {
17945                 iy = i * step ;
17946                 for (int j = -kx; j <= kx; j++) {
17947                         ix = j*step ;
17948                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17949                         Frngs(cimage, numr);
17950                         //  compare with all reference images
17951                         // for iref in xrange(len(crefim)):
17952                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17953                                 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
17954                                 double qn = retvals["qn"];
17955                                 if(qn >= peak) {
17956                                         sx = -ix;
17957                                         sy = -iy;
17958                                         nref = iref;
17959                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17960                                         peak = static_cast<float>(qn);
17961                                 }
17962                         }  delete cimage; cimage = 0;
17963                 }
17964         }
17965         float co, so, sxs, sys;
17966         co = static_cast<float>( cos(ang*pi/180.0) );
17967         so = static_cast<float>( -sin(ang*pi/180.0) );
17968         sxs = sx*co - sy*so;
17969         sys = sx*so + sy*co;
17970         vector<float> res;
17971         res.push_back(ang);
17972         res.push_back(sxs);
17973         res.push_back(sys);
17974         res.push_back(static_cast<float>(nref));
17975         res.push_back(peak);
17976         return res;
17977 }
17978 
17979 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
17980                 float xrng, float yrng, float step, float ant, string mode,
17981                 vector<int>numr, float cnx, float cny) {
17982 
17983     // Manually extract.
17984 /*    vector< EMAN::EMData* > crefim;
17985     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17986     crefim.reserve(crefim_len);
17987 
17988     for(std::size_t i=0;i<crefim_len;i++) {
17989         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17990         crefim.push_back(proxy());
17991     }
17992 */
17993         size_t crefim_len = crefim.size();
17994         const float qv = static_cast<float>( pi/180.0 );
17995 
17996         Transform * t = image->get_attr("xform.projection");
17997         Dict d = t->get_params("spider");
17998         if(t) {delete t; t=0;}
17999         float phi = d["phi"];
18000         float theta = d["theta"];
18001         int   ky = int(2*yrng/step+0.5)/2;
18002         int   kx = int(2*xrng/step+0.5)/2;
18003         int   iref, nref=0, mirror=0;
18004         float iy, ix, sx=0, sy=0;
18005         float peak = -1.0E23f;
18006         float ang=0.0f;
18007         float imn1 = sin(theta*qv)*cos(phi*qv);
18008         float imn2 = sin(theta*qv)*sin(phi*qv);
18009         float imn3 = cos(theta*qv);
18010         vector<float> n1(crefim_len);
18011         vector<float> n2(crefim_len);
18012         vector<float> n3(crefim_len);
18013         for ( iref = 0; iref < (int)crefim_len; iref++) {
18014                         n1[iref] = crefim[iref]->get_attr("n1");
18015                         n2[iref] = crefim[iref]->get_attr("n2");
18016                         n3[iref] = crefim[iref]->get_attr("n3");
18017         }
18018         for (int i = -ky; i <= ky; i++) {
18019             iy = i * step ;
18020             for (int j = -kx; j <= kx; j++) {
18021                 ix = j*step;
18022                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18023 
18024                 Normalize_ring( cimage, numr );
18025 
18026                 Frngs(cimage, numr);
18027                 //  compare with all reference images
18028                 // for iref in xrange(len(crefim)):
18029                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18030                         if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18031                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18032                                 double qn = retvals["qn"];
18033                                 double qm = retvals["qm"];
18034                                 if(qn >= peak || qm >= peak) {
18035                                         sx = -ix;
18036                                         sy = -iy;
18037                                         nref = iref;
18038                                         if (qn >= qm) {
18039                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18040                                                 peak = static_cast<float>( qn );
18041                                                 mirror = 0;
18042                                         } else {
18043                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18044                                                 peak = static_cast<float>( qm );
18045                                                 mirror = 1;
18046                                         }
18047                                 }
18048                         }
18049                 }  delete cimage; cimage = 0;
18050             }
18051         }
18052         float co, so, sxs, sys;
18053         if(peak == -1.0E23) {
18054                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18055                 nref = -1;
18056         } else {
18057                 co =  cos(ang*qv);
18058                 so = -sin(ang*qv);
18059                 sxs = sx*co - sy*so;
18060                 sys = sx*so + sy*co;
18061         }
18062         vector<float> res;
18063         res.push_back(ang);
18064         res.push_back(sxs);
18065         res.push_back(sys);
18066         res.push_back(static_cast<float>(mirror));
18067         res.push_back(static_cast<float>(nref));
18068         res.push_back(peak);
18069         return res;
18070 }
18071 
18072 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18073                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18074                 vector<int>numr, float cnx, float cny) {
18075 
18076     // Manually extract.
18077 /*    vector< EMAN::EMData* > crefim;
18078     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18079     crefim.reserve(crefim_len);
18080 
18081     for(std::size_t i=0;i<crefim_len;i++) {
18082         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18083         crefim.push_back(proxy());
18084     }
18085 */
18086         size_t crefim_len = crefim.size();
18087         const float qv = static_cast<float>(pi/180.0);
18088 
18089         Transform* t = image->get_attr("xform.projection");
18090         Dict d = t->get_params("spider");
18091         if(t) {delete t; t=0;}
18092         float phi = d["phi"];
18093         float theta = d["theta"];
18094         float psi = d["psi"];
18095         int ky = int(2*yrng/step+0.5)/2;
18096         int kx = int(2*xrng/step+0.5)/2;
18097         int iref, nref = 0, mirror = 0;
18098         float iy, ix, sx = 0, sy = 0;
18099         float peak = -1.0E23f;
18100         float ang = 0.0f;
18101         float imn1 = sin(theta*qv)*cos(phi*qv);
18102         float imn2 = sin(theta*qv)*sin(phi*qv);
18103         float imn3 = cos(theta*qv);
18104         vector<float> n1(crefim_len);
18105         vector<float> n2(crefim_len);
18106         vector<float> n3(crefim_len);
18107         for (iref = 0; iref < (int)crefim_len; iref++) {
18108                         n1[iref] = crefim[iref]->get_attr("n1");
18109                         n2[iref] = crefim[iref]->get_attr("n2");
18110                         n3[iref] = crefim[iref]->get_attr("n3");
18111         }
18112         bool nomirror = (theta<90.0) || (theta==90.0) && (psi<psi_max);
18113         if (!nomirror) {
18114                 phi = fmod(phi+540.0f, 360.0f);
18115                 theta = 180-theta;
18116                 psi = fmod(540.0f-psi, 360.0f);
18117         }
18118         for (int i = -ky; i <= ky; i++) {
18119             iy = i * step ;
18120             for (int j = -kx; j <= kx; j++) {
18121                 ix = j*step;
18122                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18123 
18124                 Normalize_ring(cimage, numr);
18125 
18126                 Frngs(cimage, numr);
18127                 //  compare with all reference images
18128                 // for iref in xrange(len(crefim)):
18129                 for (iref = 0; iref < (int)crefim_len; iref++) {
18130                         if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18131                                 if (nomirror) {
18132                                         Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 0);
18133                                         double qn = retvals["qn"];
18134                                         if (qn >= peak) {
18135                                                 sx = -ix;
18136                                                 sy = -iy;
18137                                                 nref = iref;
18138                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18139                                                 peak = static_cast<float>(qn);
18140                                                 mirror = 0;
18141                                         }
18142                                 } else {
18143                                         Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 1);
18144                                         double qn = retvals["qn"];
18145                                         if (qn >= peak) {
18146                                                 sx = -ix;
18147                                                 sy = -iy;
18148                                                 nref = iref;
18149                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18150                                                 peak = static_cast<float>(qn);
18151                                                 mirror = 1;
18152                                         }
18153                                 }
18154                         }
18155                 }  delete cimage; cimage = 0;
18156             }
18157         }
18158         float co, so, sxs, sys;
18159         if(peak == -1.0E23) {
18160                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18161                 nref = -1;
18162         } else {
18163                 co =  cos(ang*qv);
18164                 so = -sin(ang*qv);
18165                 sxs = sx*co - sy*so;
18166                 sys = sx*so + sy*co;
18167         }
18168         vector<float> res;
18169         res.push_back(ang);
18170         res.push_back(sxs);
18171         res.push_back(sys);
18172         res.push_back(static_cast<float>(mirror));
18173         res.push_back(static_cast<float>(nref));
18174         res.push_back(peak);
18175         return res;
18176 }
18177 
18178 
18179 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18180                 float xrng, float yrng, float step, float psi_max, string mode,
18181                 vector<int>numr, float cnx, float cny) {
18182 
18183         size_t crefim_len = crefim.size();
18184 
18185         int   ky = int(2*yrng/step+0.5)/2;
18186         int   kx = int(2*xrng/step+0.5)/2;
18187         int   iref, nref=0, mirror=0;
18188         float iy, ix, sx=0, sy=0;
18189         float peak = -1.0E23f;
18190         float ang=0.0f;
18191         for (int i = -ky; i <= ky; i++) {
18192                 iy = i * step ;
18193                 for (int j = -kx; j <= kx; j++) {
18194                         ix = j*step ;
18195                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18196 
18197                         Normalize_ring( cimage, numr );
18198 
18199                         Frngs(cimage, numr);
18200                         //  compare with all reference images
18201                         // for iref in xrange(len(crefim)):
18202                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18203                                 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18204                                 double qn = retvals["qn"];
18205                                 double qm = retvals["qm"];
18206                                 if(qn >= peak || qm >= peak) {
18207                                         sx = -ix;
18208                                         sy = -iy;
18209                                         nref = iref;
18210                                         if (qn >= qm) {
18211                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18212                                                 peak = static_cast<float>(qn);
18213                                                 mirror = 0;
18214                                         } else {
18215                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18216                                                 peak = static_cast<float>(qm);
18217                                                 mirror = 1;
18218                                         }
18219                                 }
18220                         }  delete cimage; cimage = 0;
18221                 }
18222         }
18223         float co, so, sxs, sys;
18224         co = static_cast<float>( cos(ang*pi/180.0) );
18225         so = static_cast<float>( -sin(ang*pi/180.0) );
18226         sxs = sx*co - sy*so;
18227         sys = sx*so + sy*co;
18228         vector<float> res;
18229         res.push_back(ang);
18230         res.push_back(sxs);
18231         res.push_back(sys);
18232         res.push_back(static_cast<float>(mirror));
18233         res.push_back(static_cast<float>(nref));
18234         res.push_back(peak);
18235         return res;
18236 }
18237 
18238 void  Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
18239                         float xrng, float yrng, float step, string mode,
18240                         vector< int >numr, float cnx, float cny,
18241                         EMData *peaks, EMData *peakm) {
18242 
18243         int   maxrin = numr[numr.size()-1];
18244 
18245         int   ky = int(2*yrng/step+0.5)/2;
18246         int   kx = int(2*xrng/step+0.5)/2;
18247 
18248         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18249         float *p_ccf1ds = peaks->get_data();
18250 
18251         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18252         float *p_ccf1dm = peakm->get_data();
18253 
18254         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18255                 p_ccf1ds[i] = -1.e20f;
18256                 p_ccf1dm[i] = -1.e20f;
18257         }
18258 
18259         for (int i = -ky; i <= ky; i++) {
18260                 float iy = i * step;
18261                 for (int j = -kx; j <= kx; j++) {
18262                         float ix = j*step;
18263                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18264                         Frngs(cimage, numr);
18265                         Crosrng_msg_vec(crefim, cimage, numr,
18266                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18267                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18268                         delete cimage; cimage = 0;
18269                 }
18270         }
18271         return;
18272 }
18273 
18274 void  Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
18275      float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
18276      EMData *peaks_compress, EMData *peakm_compress) {
18277 
18278         int   maxrin = numr[numr.size()-1];
18279 
18280         int   ky = int(2*yrng/step+0.5)/2;
18281         int   kx = int(2*xrng/step+0.5)/2;
18282 
18283         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18284         float *p_ccf1ds = peaks->get_data();
18285 
18286         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18287         float *p_ccf1dm = peakm->get_data();
18288 
18289         peaks_compress->set_size(maxrin, 1, 1);
18290         float *p_ccf1ds_compress = peaks_compress->get_data();
18291 
18292         peakm_compress->set_size(maxrin, 1, 1);
18293         float *p_ccf1dm_compress = peakm_compress->get_data();
18294 
18295         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18296                 p_ccf1ds[i] = -1.e20f;
18297                 p_ccf1dm[i] = -1.e20f;
18298         }
18299 
18300         for (int i = -ky; i <= ky; i++) {
18301                 float iy = i * step;
18302                 for (int j = -kx; j <= kx; j++) {
18303                         float ix = j*step;
18304                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18305                         Frngs(cimage, numr);
18306                         Crosrng_msg_vec(crefim, cimage, numr,
18307                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18308                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18309                         delete cimage; cimage = 0;
18310                 }
18311         }
18312         for (int x=0; x<maxrin; x++) {
18313                 float maxs = -1.0e22f;
18314                 float maxm = -1.0e22f;
18315                 for (int i=1; i<=2*ky+1; i++) {
18316                         for (int j=1; j<=2*kx+1; j++) {
18317                                 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
18318                                 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
18319                         }
18320                 }
18321                 p_ccf1ds_compress[x] = maxs;
18322                 p_ccf1dm_compress[x] = maxm;
18323         }
18324         return;
18325 }
18326 
18327 struct ccf_point
18328 {
18329     float value;
18330     int i;
18331     int j;
18332     int k;
18333     int mirror;
18334 };
18335 
18336 
18337 struct ccf_value
18338 {
18339     bool operator()( const ccf_point& a, const ccf_point& b )
18340     {
18341         return a.value > b.value;
18342     }
18343 };
18344 
18345 
18346 vector<float>  Util::ali2d_ccf_list(EMData* image, EMData* crefim,
18347                         float xrng, float yrng, float step, string mode,
18348                         vector< int >numr, float cnx, float cny, double T) {
18349 
18350         int   maxrin = numr[numr.size()-1];
18351 
18352         int   ky = int(2*yrng/step+0.5)/2;
18353         int   kx = int(2*xrng/step+0.5)/2;
18354 
18355         float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
18356         float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
18357         int vol = maxrin*(2*kx+1)*(2*ky+1);
18358         vector<ccf_point> ccf(2*vol);
18359         ccf_point temp;
18360 
18361         int index = 0;
18362         for (int i = -ky; i <= ky; i++) {
18363                 float iy = i * step;
18364                 for (int j = -kx; j <= kx; j++) {
18365                         float ix = j*step;
18366                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18367                         Frngs(cimage, numr);
18368                         Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
18369                         for (int k=0; k<maxrin; k++) {
18370                                 temp.value = p_ccf1ds[k];
18371                                 temp.i = k;
18372                                 temp.j = j;
18373                                 temp.k = i;
18374                                 temp.mirror = 0;
18375                                 ccf[index] = temp;
18376                                 index++;
18377                                 temp.value = p_ccf1dm[k];
18378                                 temp.mirror = 1;
18379                                 ccf[index] = temp;
18380                                 index++;
18381                         }
18382                         delete cimage; cimage = 0;
18383                 }
18384         }
18385 
18386         delete p_ccf1ds;
18387         delete p_ccf1dm;
18388         std::sort(ccf.begin(), ccf.end(), ccf_value());
18389 
18390         double qt = (double)ccf[0].value;
18391         vector <double> p(2*vol), cp(2*vol);
18392 
18393         double sump = 0.0;
18394         for (int i=0; i<2*vol; i++) {
18395                 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
18396                 sump += p[i];
18397         }
18398         for (int i=0; i<2*vol; i++) {
18399                 p[i] /= sump;
18400         }
18401         for (int i=1; i<2*vol; i++) {
18402                 p[i] += p[i-1];
18403         }
18404         p[2*vol-1] = 2.0;
18405 
18406         float t = get_frand(0.0f, 1.0f);
18407         int select = 0;
18408         while (p[select] < t)   select += 1;
18409 
18410         vector<float> a(6);
18411         a[0] = ccf[select].value;
18412         a[1] = (float)ccf[select].i;
18413         a[2] = (float)ccf[select].j;
18414         a[3] = (float)ccf[select].k;
18415         a[4] = (float)ccf[select].mirror;
18416         a[5] = (float)select;
18417         return a;
18418 }
18419 
18420 
18421 /*
18422 void Util::multiref_peaks_ali(EMData* image, const vector< EMData* >& crefim,
18423                         float xrng, float yrng, float step, string mode,
18424                         vector< int >numr, float cnx, float cny,
18425                         EMData *peaks, EMData *peakm, int nphi, int ntheta) {
18426 
18427 // formerly known as apmq
18428     // Determine shift and rotation between image and many reference
18429     // images (crefim, weights have to be applied) quadratic
18430     // interpolation
18431 
18432 
18433     // Manually extract.
18434 *//*    vector< EMAN::EMData* > crefim;
18435     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18436     crefim.reserve(crefim_len);
18437 
18438     for(std::size_t i=0;i<crefim_len;i++) {
18439         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18440         crefim.push_back(proxy());
18441     }
18442 */
18443 /*
18444         int   maxrin = numr[numr.size()-1];
18445 
18446         size_t crefim_len = crefim.size();
18447 
18448         int   iref;
18449         int   ky = int(2*yrng/step+0.5)/2;
18450         int   kx = int(2*xrng/step+0.5)/2;
18451         int   tkx = 2*kx+3;
18452         int   tky = 2*ky+3;
18453 
18454         peaks->set_size(maxrin, nphi, ntheta, tkx, tky);
18455         float *p_ccf1ds = peaks->get_data();
18456 
18457 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k+(l*nz))*ny))*nx]
18458 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k*ny))*nx]
18459         peakm->set_size(maxrin, nphi, ntheta, tkx, tky);
18460         float *p_ccf1dm = peakm->get_data();
18461 
18462         for ( int i = 0; i<maxrin*(int)crefim_len*(2*kx+3)*(2*ky+3); i++) {
18463                 p_ccf1ds[i] = -1.e20f;
18464                 p_ccf1dm[i] = -1.e20f;
18465         }
18466 
18467         float  iy, ix;
18468         for (int i = -ky; i <= ky; i++) {
18469                 iy = i * step ;
18470                 for (int j = -kx; j <= kx; j++) {
18471                         ix = j*step ;
18472                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18473                         Frngs(cimage, numr);
18474                         //  compare with all reference images
18475                         // for iref in xrange(len(crefim)):
18476                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18477                                 Crosrng_msg_vec(crefim[iref], cimage, numr,
18478                                         p_ccf1ds+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin,
18479                                         p_ccf1dm+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin);
18480                         }
18481                         delete cimage; cimage = 0;
18482                 }
18483         }
18484         return;
18485 }
18486 */
18487 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18488 
18489         EMData *rot;
18490 
18491         const int nmax=3, mmax=3;
18492         char task[60], csave[60];
18493         long int lsave[4];
18494         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18495         double f, f1, f2, f3, factr, pgtol, x[nmax], l[nmax], u[nmax], g[nmax], dsave[29], wa[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
18496         long int SIXTY=60;
18497 
18498         //     We wish to have no output.
18499         iprint = -1;
18500 
18501         //c     We specify the tolerances in the stopping criteria.
18502         factr=1.0e1;
18503         pgtol=1.0e-5;
18504 
18505         //     We specify the dimension n of the sample problem and the number
18506         //        m of limited memory corrections stored.  (n and m should not
18507         //        exceed the limits nmax and mmax respectively.)
18508         n=3;
18509         m=3;
18510 
18511         //     We now provide nbd which defines the bounds on the variables:
18512         //                    l   specifies the lower bounds,
18513         //                    u   specifies the upper bounds.
18514         //                    x   specifies the initial guess
18515         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18516         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18517         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18518 
18519 
18520         //     We start the iteration by initializing task.
18521         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18522         strcpy(task,"START");
18523         for (int i=5;i<60;i++)  task[i]=' ';
18524 
18525         //     This is the call to the L-BFGS-B code.
18526         // (* call the L-BFGS-B routine with task='START' once before loop *)
18527         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18528         //int step = 1;
18529 
18530         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18531         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18532 
18533                 if (strncmp(task,"FG",2)==0) {
18534                 //   the minimization routine has returned to request the
18535                 //   function f and gradient g values at the current x
18536 
18537                 //        Compute function value f for the sample problem.
18538                 rot = new EMData();
18539                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
18540                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18541                 //f = -f;
18542                 delete rot;
18543 
18544                 //        Compute gradient g for the sample problem.
18545                 float dt = 1.0e-3f;
18546                 rot = new EMData();
18547                 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
18548                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18549                 //f1 = -f1;
18550                 g[0] = (f1-f)/dt;
18551                 delete rot;
18552 
18553                 dt = 1.0e-2f;
18554                 rot = new EMData();
18555                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
18556                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18557                 //f2 = -f2;
18558                 g[1] = (f2-f)/dt;
18559                 delete rot;
18560 
18561                 rot = new EMData();
18562                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
18563                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18564                 //f3 = -f3;
18565                 g[2] = (f3-f)/dt;
18566                 delete rot;
18567                 }
18568 
18569                 //c          go back to the minimization routine.
18570                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18571                 //step++;
18572         }
18573 
18574         //printf("Total step is %d\n", step);
18575         vector<float> res;
18576         res.push_back(static_cast<float>(x[0]));
18577         res.push_back(static_cast<float>(x[1]));
18578         res.push_back(static_cast<float>(x[2]));
18579         //res.push_back(step);
18580         return res;
18581 }
18582 
18583 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
18584 
18585         EMData *rot;
18586 
18587         const int nmax=3, mmax=3;
18588         char task[60], csave[60];
18589         long int lsave[4];
18590         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18591         double f, f1, f2, f3, factr, pgtol, x[nmax], l[nmax], u[nmax], g[nmax], dsave[29], wa[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
18592         long int SIXTY=60;
18593 
18594         //     We wish to have no output.
18595         iprint = -1;
18596 
18597         //c     We specify the tolerances in the stopping criteria.
18598         factr=1.0e1;
18599         pgtol=1.0e-5;
18600 
18601         //     We specify the dimension n of the sample problem and the number
18602         //        m of limited memory corrections stored.  (n and m should not
18603         //        exceed the limits nmax and mmax respectively.)
18604         n=3;
18605         m=3;
18606 
18607         //     We now provide nbd which defines the bounds on the variables:
18608         //                    l   specifies the lower bounds,
18609         //                    u   specifies the upper bounds.
18610         //                    x   specifies the initial guess
18611         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18612         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18613         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18614 
18615 
18616         //     We start the iteration by initializing task.
18617         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18618         strcpy(task,"START");
18619         for (int i=5;i<60;i++)  task[i]=' ';
18620 
18621         //     This is the call to the L-BFGS-B code.
18622         // (* call the L-BFGS-B routine with task='START' once before loop *)
18623         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18624         //int step = 1;
18625 
18626         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18627         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18628 
18629                 if (strncmp(task,"FG",2)==0) {
18630                 //   the minimization routine has returned to request the
18631                 //   function f and gradient g values at the current x
18632 
18633                 //        Compute function value f for the sample problem.
18634                 rot = new EMData();
18635                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18636                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18637                 //f = -f;
18638                 delete rot;
18639 
18640                 //        Compute gradient g for the sample problem.
18641                 float dt = 1.0e-3f;
18642                 rot = new EMData();
18643                 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18644                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18645                 //f1 = -f1;
18646                 g[0] = (f1-f)/dt;
18647                 delete rot;
18648 
18649                 rot = new EMData();
18650                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
18651                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18652                 //f2 = -f2;
18653                 g[1] = (f2-f)/dt;
18654                 delete rot;
18655 
18656                 rot = new EMData();
18657                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
18658                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18659                 //f3 = -f3;
18660                 g[2] = (f3-f)/dt;
18661                 delete rot;
18662                 }
18663 
18664                 //c          go back to the minimization routine.
18665                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18666                 //step++;
18667         }
18668 
18669         //printf("Total step is %d\n", step);
18670         vector<float> res;
18671         res.push_back(static_cast<float>(x[0]));
18672         res.push_back(static_cast<float>(x[1]));
18673         res.push_back(static_cast<float>(x[2]));
18674         //res.push_back(step);
18675         return res;
18676 }
18677 
18678 vector<float> Util::twoD_to_3D_ali(EMData* volft, Util::KaiserBessel& kb, EMData *refim, EMData* mask, float phi, float theta, float psi, float sxs, float sys) {
18679 
18680         EMData *proj, *proj2;
18681 
18682         const int nmax=5, mmax=5;
18683         char task[60], csave[60];
18684         long int lsave[4];
18685         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18686         double f, ft, factr, pgtol, x[nmax], l[nmax], u[nmax], g[nmax], dsave[29], wa[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
18687         long int SIXTY=60;
18688 
18689         //     We wish to have no output.
18690         iprint = -1;
18691 
18692         //c     We specify the tolerances in the stopping criteria.
18693         factr=1.0e1;
18694         pgtol=1.0e-5;
18695 
18696         //     We specify the dimension n of the sample problem and the number
18697         //        m of limited memory corrections stored.  (n and m should not
18698         //        exceed the limits nmax and mmax respectively.)
18699         n=5;
18700         m=5;
18701 
18702         //     We now provide nbd which defines the bounds on the variables:
18703         //                    l   specifies the lower bounds,
18704         //                    u   specifies the upper bounds.
18705         //                    x   specifies the initial guess
18706         x[0] = phi;     nbd[0] = 2;     l[0] = phi-2.0;         u[0] = phi+2.0;
18707         x[1] = theta;   nbd[1] = 2;     l[1] = theta-2.0;       u[1] = theta+2.0;
18708         x[2] = psi;     nbd[2] = 2;     l[2] = psi-2.0;         u[2] = psi+2.0;
18709         x[3] = sxs;     nbd[3] = 2;     l[3] = sxs-2.0;         u[3] = sxs+2.0;
18710         x[4] = sys;     nbd[4] = 2;     l[4] = sys-2.0;         u[4] = sys+2.0;
18711 
18712 
18713         //     We start the iteration by initializing task.
18714         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18715         strcpy(task,"START");
18716         for (int i=5;i<60;i++)  task[i]=' ';
18717 
18718         //     This is the call to the L-BFGS-B code.
18719         // (* call the L-BFGS-B routine with task='START' once before loop *)
18720         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18721         int step = 1;
18722 
18723         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18724         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18725 
18726                 if (strncmp(task,"FG",2)==0) {
18727                 //   the minimization routine has returned to request the
18728                 //   function f and gradient g values at the current x
18729 
18730                 //        Compute function value f for the sample problem.
18731                 proj = new EMData();
18732                 proj2 = new EMData();
18733                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18734                 proj->fft_shuffle();
18735                 proj->center_origin_fft();
18736                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18737                 proj->do_ift_inplace();
18738                 int M = proj->get_ysize()/2;
18739                 proj2 = proj->window_center(M);
18740                 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18741                 //f = -f;
18742                 delete proj;
18743                 delete proj2;
18744 
18745                 //        Compute gradient g for the sample problem.
18746                 float dt = 1.0e-3f;
18747                 proj = new EMData();
18748                 proj2 = new EMData();
18749                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
18750                 proj->fft_shuffle();
18751                 proj->center_origin_fft();
18752                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18753                 proj->do_ift_inplace();
18754                 proj2 = proj->window_center(M);
18755                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18756                 //ft = -ft;
18757                 delete proj;
18758                 delete proj2;
18759                 g[0] = (ft-f)/dt;
18760 
18761                 proj = new EMData();
18762                 proj2 = new EMData();
18763                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
18764                 proj->fft_shuffle();
18765                 proj->center_origin_fft();
18766                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18767                 proj->do_ift_inplace();
18768                 proj2 = proj->window_center(M);
18769                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18770                 //ft = -ft;
18771                 delete proj;
18772                 delete proj2;
18773                 g[1] = (ft-f)/dt;
18774 
18775                 proj = new EMData();
18776                 proj2 = new EMData();
18777                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
18778                 proj->fft_shuffle();
18779                 proj->center_origin_fft();
18780                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18781                 proj->do_ift_inplace();
18782                 proj2 = proj->window_center(M);
18783                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18784                 //ft = -ft;
18785                 delete proj;
18786                 delete proj2;
18787                 g[2] = (ft-f)/dt;
18788 
18789                 proj = new EMData();
18790                 proj2 = new EMData();
18791                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18792                 proj->fft_shuffle();
18793                 proj->center_origin_fft();
18794                 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
18795                 proj->do_ift_inplace();
18796                 proj2 = proj->window_center(M);
18797                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18798                 //ft = -ft;
18799                 delete proj;
18800                 delete proj2;
18801                 g[3] = (ft-f)/dt;
18802 
18803                 proj = new EMData();
18804                 proj2 = new EMData();
18805                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18806                 proj->fft_shuffle();
18807                 proj->center_origin_fft();
18808                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
18809                 proj->do_ift_inplace();
18810                 proj2 = proj->window_center(M);
18811                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18812                 //ft = -ft;
18813                 delete proj;
18814                 delete proj2;
18815                 g[4] = (ft-f)/dt;
18816                 }
18817 
18818                 //c          go back to the minimization routine.
18819                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18820                 step++;
18821         }
18822 
18823         //printf("Total step is %d\n", step);
18824         vector<float> res;
18825         res.push_back(static_cast<float>(x[0]));
18826         res.push_back(static_cast<float>(x[1]));
18827         res.push_back(static_cast<float>(x[2]));
18828         res.push_back(static_cast<float>(x[3]));
18829         res.push_back(static_cast<float>(x[4]));
18830         //res.push_back(step);
18831         return res;
18832 }
18833 
18834 
18835 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18836 
18837         double  x[4];
18838         int n;
18839         int l = 3;
18840         int m = 200;
18841         double e = 1e-9;
18842         double step = 0.01;
18843         float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
18844 
18845         x[1] = ang;
18846         x[2] = sxs;
18847         x[3] = sys;
18848 
18849         Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);   // Call steepest descent optimization subroutine
18850         //printf("Took %d steps\n", n);
18851 
18852         vector<float> res;
18853         res.push_back(static_cast<float>(x[1]));
18854         res.push_back(static_cast<float>(x[2]));
18855         res.push_back(static_cast<float>(x[3]));
18856         res.push_back(static_cast<float>(n));
18857         return res;
18858 }
18859 
18860 
18861 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
18862 
18863         EMData *rot= new EMData();
18864         float ccc;
18865 
18866         rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
18867         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18868         delete rot;
18869         return ccc;
18870 }
18871 
18872 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
18873 
18874         double  x[4];
18875         int n;
18876         int l = 3;
18877         int m = 200;
18878         double e = 1e-9;
18879         double step = 0.001;
18880         float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
18881 
18882         x[1] = ang;
18883         x[2] = sxs;
18884         x[3] = sys;
18885 
18886         Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);   // Call steepest descent optimization subroutine
18887         //printf("Took %d steps\n", n);
18888 
18889         vector<float> res;
18890         res.push_back(static_cast<float>(x[1]));
18891         res.push_back(static_cast<float>(x[2]));
18892         res.push_back(static_cast<float>(x[3]));
18893         res.push_back(static_cast<float>(n));
18894         return res;
18895 }
18896 
18897 
18898 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
18899 
18900         EMData *rot= new EMData();
18901         float ccc;
18902 
18903         rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
18904         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18905         delete rot;
18906         return ccc;
18907 }
18908 
18909 #define img_ptr(i,j,k)  img_ptr[i+(j+(k*ny))*nx]
18910 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*nx]
18911 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
18912 {
18913         ENTERFUNC;
18914         /* Exception Handle */
18915         if (!img) {
18916                 throw NullPointerException("NULL input image");
18917         }
18918 
18919         int newx, newy, newz;
18920         bool  keep_going;
18921         cout << " entered   " <<endl;
18922         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
18923         //int size = nx*ny*nz;
18924         EMData * img2 = new EMData();
18925         img2->set_size(nx,ny,nz);
18926         img2->to_zero();
18927         float *img_ptr  =img->get_data();
18928         float *img2_ptr = img2->get_data();
18929         int r2 = ro*ro;
18930         int r3 = r2*ro;
18931         int ri2 = ri*ri;
18932         int ri3 = ri2*ri;
18933 
18934         int n2 = nx/2;
18935 
18936         for (int k=-n2; k<=n2; k++) {           //cout << " k   "<<k <<endl;
18937                 float z2 = static_cast<float>(k*k);
18938                 for (int j=-n2; j<=n2; j++) {
18939                         float y2 = z2 + j*j;
18940                         if(y2 <= r2) {
18941                                                                                         //cout << "  j  "<<j <<endl;
18942 
18943                                 for (int i=-n2; i<=n2; i++) {
18944                                         float x2 = y2 + i*i;
18945                                         if(x2 <= r3) {
18946                                                                                         //cout << "  i  "<<i <<endl;
18947                                                 int ib = i+n2; int jb = j+n2; int kb = k+n2;
18948                                                 if(x2 >= ri3) {
18949                                                         //  this is the outer shell, here points can only vanish
18950                                                         if(img_ptr(ib,jb,kb) == 1.0f) {
18951                                                                 //cout << "  1  "<<ib <<endl;
18952                                                                 if(Util::get_frand(0.0f, 1.0f) > qprob){
18953                                                                         img2_ptr(ib,jb,kb) = 0.0f;
18954                                                                         keep_going = true;
18955                                                                 //cout << "  try  "<<ib <<endl;
18956                                                                         while(keep_going) {
18957                                                                                 newx = Util::get_irand(-ro,ro);
18958                                                                                 newy = Util::get_irand(-ro,ro);
18959                                                                                 newz = Util::get_irand(-ro,ro);
18960                                                                                 if(newx*newx+newy*newy+newz*newz <= r3) {
18961                                                                                         newx += n2; newy += n2; newz += n2;
18962                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
18963                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
18964                                                                                                 keep_going = false; }
18965                                                                                 }
18966                                                                         }
18967                                                                 }   else  img2_ptr(ib,jb,kb) = 1.0f;
18968                                                         }
18969                                                 }  else  {
18970                                                         // this is inner shell, the point can only move (or vanish, if all neighbors exist)
18971                                                         if(img_ptr(ib,jb,kb) == 1.0) {
18972                                                                 if(Util::get_frand(0.0f,1.0f) > qprob) {
18973                                                                         //  find out the number of neighbors
18974                                                                         float  numn = -1.0f;  // we already know the central one is 1
18975                                                                         for (newz = -1; newz <= 1; newz++)
18976                                                                                 for (newy = -1; newy <= 1; newy++)
18977                                                                                         for (newx = -1; newx <= 1; newx++)
18978                                                                                                 numn += img_ptr(ib+newx,jb+newy,kb+newz);
18979                                                                         img2_ptr(ib,jb,kb) = 0.0;
18980                                                                         if(numn == 26.0f) {
18981                                                                                 //  all neighbors exist, it has to vanish
18982                                                                                 keep_going = true;
18983                                                                                 while(keep_going) {
18984                                                                                         newx = Util::get_irand(-ro,ro);
18985                                                                                         newy = Util::get_irand(-ro,ro);
18986                                                                                         newz = Util::get_irand(-ro,ro);
18987                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
18988                                                                                                 newx += n2; newy += n2; newz += n2;
18989                                                                                                 if( img_ptr(newx,newy,newz) == 0.0f) {
18990                                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
18991                                                                                                                 if(newx*newx+newy*newy+newz*newz < r3) {
18992                                                                                                                         newx += n2; newy += n2; newz += n2;
18993                                                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
18994                                                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
18995                                                                                                                                 keep_going = false; }
18996                                                                                                                 }
18997                                                                                                         }
18998                                                                                                 }
18999                                                                                         }
19000                                                                                 }
19001                                                                         }  else if(numn == 25.0f) {
19002                                                                                 // there is only one empty neighbor, move there
19003                                                                                 for (newz = -1; newz <= 1; newz++) {
19004                                                                                         for (newy = -1; newy <= 1; newy++) {
19005                                                                                                 for (newx = -1; newx <= 1; newx++) {
19006                                                                                                         if( newx != 0 && newy != 0 && newz != 0) {
19007                                                                                                                 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
19008                                                                                                                         img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
19009                                                                                                                         }
19010                                                                                                         }
19011                                                                                                 }
19012                                                                                         }
19013                                                                                 }
19014                                                                         }  else {
19015                                                                                 //  more than one neighbor is zero, select randomly one and move there
19016                                                                                 keep_going = true;
19017                                                                                 while(keep_going) {
19018                                                                                         newx = Util::get_irand(-1,1);
19019                                                                                         newy = Util::get_irand(-1,1);
19020                                                                                         newz = Util::get_irand(-1,1);
19021                                                                                         if(newx != 0 && newy != 0 && newz != 0)  {
19022                                                                                                 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
19023                                                                                                         img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;//?????
19024                                                                                                         keep_going = false;
19025                                                                                                 }
19026                                                                                         }
19027                                                                                 }
19028                                                                         }
19029                                                                 }  else img2_ptr(ib,jb,kb) = 1.0f;
19030                                                         }
19031                                                 }
19032                                         }
19033                                 }
19034                         }
19035                 }
19036         }
19037         //for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + Util::get_irand(1,7);
19038         img2->update();
19039 
19040         EXITFUNC;
19041         return img2;
19042 }
19043 #undef img_ptr
19044 #undef img2_ptr
19045 
19046 struct point3d_t
19047 {
19048         point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
19049 
19050         int x;
19051         int y;
19052         int z;
19053 };
19054 
19055 
19056 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
19057 {
19058         int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
19059         int noff = 6;
19060 
19061         int nx = visited->get_xsize();
19062         int ny = visited->get_ysize();
19063         int nz = visited->get_zsize();
19064 
19065         vector< point3d_t > pts;
19066         pts.push_back( point3d_t(ix, iy, iz) );
19067         visited->set_value_at( ix, iy, iz, (float)grpid );
19068 
19069         int start = 0;
19070         int end = pts.size();
19071 
19072         while( end > start ) {
19073                 for(int i=start; i < end; ++i ) {
19074                         int ix = pts[i].x;
19075                         int iy = pts[i].y;
19076                         int iz = pts[i].z;
19077 
19078                         for( int j=0; j < noff; ++j ) {
19079                                 int jx = ix + offs[j][0];
19080                                 int jy = iy + offs[j][1];
19081                                 int jz = iz + offs[j][2];
19082 
19083                                 if( jx < 0 || jx >= nx ) continue;
19084                                 if( jy < 0 || jy >= ny ) continue;
19085                                 if( jz < 0 || jz >= nz ) continue;
19086 
19087 
19088                                 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
19089                                     pts.push_back( point3d_t(jx, jy, jz) );
19090                                     visited->set_value_at( jx, jy, jz, (float)grpid );
19091                                 }
19092 
19093                         }
19094                 }
19095 
19096                 start = end;
19097                 end = pts.size();
19098         }
19099         return pts.size();
19100 }
19101 
19102 
19103 EMData* Util::get_biggest_cluster( EMData* mg )
19104 {
19105         int nx = mg->get_xsize();
19106         int ny = mg->get_ysize();
19107         int nz = mg->get_zsize();
19108 
19109         EMData* visited = new EMData();
19110         visited->set_size( nx, ny, nz );
19111         visited->to_zero();
19112         int grpid = 0;
19113         int maxgrp = 0;
19114         int maxsize = 0;
19115         for( int iz=0; iz < nz; ++iz ) {
19116                 for( int iy=0; iy < ny; ++iy ) {
19117                         for( int ix=0; ix < nx; ++ix ) {
19118                                 if( (*mg)(ix, iy, iz)==0.0 ) continue;
19119 
19120                                 if( (*visited)(ix, iy, iz) > 0.0 ) {
19121                                         // visited before, must be in other group.
19122                                         continue;
19123                                 }
19124 
19125                                 grpid++;
19126                                 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
19127                                 if( grpsize > maxsize ) {
19128                                         maxsize = grpsize;
19129                                         maxgrp = grpid;
19130                                 }
19131                         }
19132                 }
19133         }
19134 
19135         Assert( maxgrp > 0 );
19136 
19137         int npoint = 0;
19138         EMData* result = new EMData();
19139         result->set_size( nx, ny, nz );
19140         result->to_zero();
19141 
19142         for( int iz=0; iz < nz; ++iz ) {
19143                 for( int iy=0; iy < ny; ++iy ) {
19144                         for( int ix=0; ix < nx; ++ix ) {
19145                                 if( (*visited)(ix, iy, iz)==maxgrp ) {
19146                                         (*result)(ix,iy,iz) = 1.0;
19147                                         npoint++;
19148                                 }
19149                         }
19150                 }
19151         }
19152 
19153         Assert( npoint==maxsize );
19154         delete visited;
19155         return result;
19156 
19157 }
19158 
19159 EMData* Util::ctf_img(int nx, int ny, int nz, float dz,float ps,float voltage,float cs, float wgh,float b_factor,float dza, float azz, float sign)
19160 {
19161         int   ix, iy, iz;
19162         int   i,  j, k;
19163         int   nr2, nl2;
19164         float  dzz, az, ak;
19165         float  scx, scy, scz;
19166         int offset = 2 - nx%2;
19167         int lsm = nx + offset;
19168         EMData* ctf_img1 = new EMData();
19169         ctf_img1->set_size(lsm, ny, nz);
19170         float freq = 1.0f/(2.0f*ps);
19171         scx = 2.0f/float(nx);
19172         if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
19173         if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
19174         nr2 = ny/2 ;
19175         nl2 = nz/2 ;
19176         for ( k=0; k<nz;k++) {
19177                 iz = k;  if(k>nl2) iz=k-nz;
19178                 for ( j=0; j<ny;j++) {
19179                         iy = j;  if(j>nr2) iy=j - ny;
19180                         for ( i=0; i<lsm/2; i++) {
19181                                 ix=i;
19182                                 ak=pow(ix*ix*scx*scx+iy*scy*iy*scy+iz*scz*iz*scz, 0.5f)*freq;
19183                                 if(ak!=0) az=0.0; else az=M_PI;
19184                                 dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f));
19185                                 (*ctf_img1) (i*2,j,k)   = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
19186                                 (*ctf_img1) (i*2+1,j,k) = 0.0f;
19187                         }
19188                 }
19189         }
19190         ctf_img1->update();
19191         ctf_img1->set_complex(true);
19192         ctf_img1->set_ri(true);
19193         //ctf_img1->attr_dict["is_complex"] = 1;
19194         //ctf_img1->attr_dict["is_ri"] = 1;
19195         if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
19196         return ctf_img1;
19197 }
19198 /*
19199 #define  cent(i)     out[i+N]
19200 #define  assign(i)   out[i]
19201 vector<float> Util::cluster_pairwise(EMData* d, int K) {
19202 
19203         int nx = d->get_xsize();
19204         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19205         vector<float> out(N+K+2);
19206         if(N*(N-1)/2 != nx) {
19207                 //print  "  incorrect dimension"
19208                 return out;}
19209         //  assign random objects as centers
19210         for(int i=0; i<N; i++) assign(i) = float(i);
19211         // shuffle
19212         for(int i=0; i<N; i++) {
19213                 int j = Util::get_irand(0,N-1);
19214                 float temp = assign(i);
19215                 assign(i) = assign(j);
19216                 assign(j) = temp;
19217         }
19218         for(int k=0; k<K; k++) cent(k) = float(assign(k));
19219         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
19220         //
19221         for(int i=0; i<N; i++) assign(i) = 0.0f;
19222         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
19223         bool change = true;
19224         int it = -1;
19225         while(change && disp < dispold) {
19226                 change = false;
19227                 dispold = disp;
19228                 it++;
19229                 //cout<<"Iteration:  "<<it<<endl;
19230                 // dispersion is a sum of distance from objects to object center
19231                 disp = 0.0f;
19232                 for(int i=0; i<N; i++) {
19233                         qm = 1.0e23f;
19234                         for(int k=0; k<K; k++) {
19235                                 if(float(i) == cent(k)) {
19236                                         qm = 0.0f;
19237                                         na = (float)k;
19238                                 } else {
19239                                         float dt = (*d)(mono(i,int(cent(k))));
19240                                         if(dt < qm) {
19241                                                 qm = dt;
19242                                                 na = (float)k;
19243                                         }
19244                                 }
19245                         }
19246                         disp += qm;
19247                         if(na != assign(i)) {
19248                                 assign(i) = na;
19249                                 change = true;
19250                         }
19251                 }
19252         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
19253                 //print disp
19254                 //print  assign
19255                 // find centers
19256                 for(int k=0; k<K; k++) {
19257                         qm = 1.0e23f;
19258                         for(int i=0; i<N; i++) {
19259                                 if(assign(i) == float(k)) {
19260                                         float q = 0.0;
19261                                         for(int j=0; j<N; j++) {
19262                                                 if(assign(j) == float(k)) {
19263                                                                 //it cannot be the same object
19264                                                         if(i != j)  q += (*d)(mono(i,j));
19265                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
19266                                                 }
19267                                         }
19268                                         if(q < qm) {
19269                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
19270                                                 qm = q;
19271                                                 cent(k) = float(i);
19272                                         }
19273                                 }
19274                         }
19275                 }
19276         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
19277         }
19278         out[N+K] = disp;
19279         out[N+K+1] = float(it);
19280         return  out;
19281 }
19282 #undef  cent
19283 #undef  assign
19284 */
19285 #define  cent(i)     out[i+N]
19286 #define  assign(i)   out[i]
19287 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
19288         int nx = d->get_xsize();
19289         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19290         vector<float> out(N+K+2);
19291         if(N*(N-1)/2 != nx) {
19292                 //print  "  incorrect dimension"
19293                 return out;}
19294         //  assign random objects as centers
19295         for(int i=0; i<N; i++) assign(i) = float(i);
19296         // shuffle
19297         for(int i=0; i<N; i++) {
19298                 int j = Util::get_irand(0,N-1);
19299                 float temp = assign(i);
19300                 assign(i) = assign(j);
19301                 assign(j) = temp;
19302         }
19303         for(int k=0; k<K; k++) cent(k) = float(assign(k));
19304         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
19305         //
19306         for(int i=0; i<N; i++) assign(i) = 0.0f;
19307         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
19308         bool change = true;
19309         int it = -1;
19310         int ct = -1;
19311         while(change && disp < dispold || ct > 0) {
19312 
19313                 change = false;
19314                 dispold = disp;
19315                 it++;
19316 
19317                 // dispersion is a sum of distance from objects to object center
19318                 disp = 0.0f;
19319                 ct = 0;
19320                 for(int i=0; i<N; i++) {
19321                         qm = 1.0e23f;
19322                         for(int k=0; k<K; k++) {
19323                                 if(float(i) == cent(k)) {
19324                                         qm = 0.0f;
19325                                         na = (float)k;
19326                                 } else {
19327                                         float dt = (*d)(mono(i,int(cent(k))));
19328                                         if(dt < qm) {
19329                                                 qm = dt;
19330                                                 na = (float)k;
19331                                         }
19332                                 }
19333                         }
19334 
19335 
19336                         // Simulated annealing
19337                         if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
19338                             na = (float)(Util::get_irand(0, K));
19339                             qm = (*d)(mono(i,int(na)));
19340                             ct++;
19341                         }
19342 
19343                         disp += qm;
19344 
19345                         if(na != assign(i)) {
19346                                 assign(i) = na;
19347                                 change = true;
19348                         }
19349                 }
19350 
19351                 //cout<<"Iteration:  "<<it<< " disp " <<disp << " T " << T << " disturb " << ct << endl;
19352                 T = T*F;
19353 
19354         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
19355                 //print disp
19356                 //print  assign
19357                 // find centers
19358                 for(int k=0; k<K; k++) {
19359                         qm = 1.0e23f;
19360                         for(int i=0; i<N; i++) {
19361                                 if(assign(i) == float(k)) {
19362                                         float q = 0.0;
19363                                         for(int j=0; j<N; j++) {
19364                                                 if(assign(j) == float(k)) {
19365                                                                 //it cannot be the same object
19366                                                         if(i != j)  q += (*d)(mono(i,j));
19367                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
19368                                                 }
19369                                         }
19370                                         if(q < qm) {
19371                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
19372                                                 qm = q;
19373                                                 cent(k) = float(i);
19374                                         }
19375                                 }
19376                         }
19377                 }
19378         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
19379         }
19380         out[N+K] = disp;
19381         out[N+K+1] = float(it);
19382         return  out;
19383 }
19384 #undef  cent
19385 #undef  assign
19386 /*
19387 #define  groupping(i,k)   group[i + k*m]
19388 vector<float> Util::cluster_equalsize(EMData* d, int m) {
19389         int nx = d->get_xsize();
19390         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19391         int K = N/m;
19392         //cout<<"  K  "<<K<<endl;
19393         vector<float> group(N+1);
19394         if(N*(N-1)/2 != nx) {
19395                 //print  "  incorrect dimension"
19396                 return group;}
19397         bool active[N];
19398         for(int i=0; i<N; i++) active[i] = true;
19399 
19400         float dm, qd;
19401         int   ppi, ppj;
19402         for(int k=0; k<K; k++) {
19403                 // find two most similiar objects among active
19404                 cout<<"  k  "<<k<<endl;
19405                 dm = 1.0e23;
19406                 for(int i=1; i<N; i++) {
19407                         if(active[i]) {
19408                                 for(int j=0; j<i; j++) {
19409                                         if(active[j]) {
19410                                                 qd = (*d)(mono(i,j));
19411                                                 if(qd < dm) {
19412                                                         dm = qd;
19413                                                         ppi = i;
19414                                                         ppj = j;
19415                                                 }
19416                                         }
19417                                 }
19418                         }
19419                 }
19420                 groupping(0,k) = float(ppi);
19421                 groupping(1,k) = float(ppj);
19422                 active[ppi] = false;
19423                 active[ppj] = false;
19424 
19425                 // find progressively objects most similar to those in the current list
19426                 for(int l=2; l<m; l++) {
19427                         //cout<<"  l  "<<l<<endl;
19428                         dm = 1.0e23;
19429                         for(int i=0; i<N; i++) {
19430                                 if(active[i]) {
19431                                         qd = 0.0;
19432                                         for(int j=0; j<l; j++) { //j in groupping[k]:
19433                         //cout<<"  groupping(j,k)  "<<groupping(j,k)<<"   "<<j<<endl;
19434                                                 int jj = int(groupping(j,k));
19435                         //cout<<"   "<<jj<<endl;
19436                                                 qd += (*d)(mono(i,jj));
19437                                         }
19438                                         if(qd < dm) {
19439                                                 dm = qd;
19440                                                 ppi = i;
19441                                         }
19442                                 }
19443                         }
19444                         groupping(l,k) = float(ppi);
19445                         active[ppi] = false;
19446                         //cout<<"  k  "<<k<<"  l  "<<l<<"  ppi  "<<ppi<<"  dm  "<<dm<<endl;
19447                 }
19448                 //for(int j=k*m; j<(k+1)*m; j++) cout<<group[j]<<endl;
19449         }
19450         // there might be remaining objects when N is not divisible by m, simply put them in one group
19451         if(N%m != 0) {
19452                 int j = K*m;
19453                 K++;
19454                 //cout<<"  N%m  "<<N%m<<"   "<<K<<endl;
19455                 for(int i=0; i<N; i++) {
19456                         if(active[i]) {
19457                                 group[j] = float(i);
19458                 //cout<<"  j,i  "<<j<<"   "<<i<<"   "<<K-1 <<"   "<< group[j]<<endl;
19459                                 j++;
19460                         }
19461                 }
19462         }
19463 
19464         int  cent[K];
19465          // find centers
19466         for(int k=0; k<K; k++) {
19467                 float qm = 1.0e23f;
19468                 for(int i=0; i<N; i++) {
19469                         if(group[i] == float(k)) {
19470                                 qd = 0.0;
19471                                 for(int j=0; j<N; j++) {
19472                                         if(group[j] == float(k)) {
19473                                                 //it cannot be the same object
19474                                                 if(i != j)  qd += (*d)(mono(i,j));
19475                                         }
19476                                 }
19477                                 if(qd < qm) {
19478                                         qm = qd;
19479                                         cent[k] = i;
19480                                 }
19481                         }
19482                 }
19483         }
19484         // dispersion is a sum of distances from objects to object center
19485         float disp = 0.0f;
19486         for(int i=0; i<N; i++) {
19487                 for(int k=0; k<K; k++) if(i == cent[k]) disp += (*d)(mono(i,cent[k]));
19488         }
19489         group[N] = disp;
19490         return  group;
19491 }
19492 #undef  groupping
19493 */
19494 
19495 vector<float> Util::cluster_equalsize(EMData* d) {
19496         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
19497         int nx = d->get_xsize();
19498         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19499         int K = N/2;
19500         vector<float> group(N);
19501         if(N*(N-1)/2 != nx) {
19502                 //print  "  incorrect dimension"
19503                 return group;}
19504         //bool active[N];       //this does not compile in VS2005. --Grant Tang
19505         bool * active = new bool[N];
19506         for(int i=0; i<N; i++) active[i] = true;
19507 
19508         float dm, qd;
19509         int   ppi = 0, ppj = 0;
19510         for(int k=0; k<K; k++) {
19511                 // find pairs of most similiar objects among active
19512                 //cout<<"  k  "<<k<<endl;
19513                 dm = 1.0e23f;
19514                 for(int i=1; i<N; i++) {
19515                         if(active[i]) {
19516                                 for(int j=0; j<i; j++) {
19517                                         if(active[j]) {
19518                                                 qd = (*d)(i*(i - 1)/2 + j);
19519                                                 if(qd < dm) {
19520                                                         dm = qd;
19521                                                         ppi = i;
19522                                                         ppj = j;
19523                                                 }
19524                                         }
19525                                 }
19526                         }
19527                 }
19528                 group[2*k] = float(ppi);
19529                 group[1+2*k] = float(ppj);
19530                 active[ppi] = false;
19531                 active[ppj] = false;
19532         }
19533 
19534         delete [] active;
19535         active = NULL;
19536         return  group;
19537 }
19538 /*
19539 #define son(i,j)=i*(i-1)/2+j
19540 vector<float> Util::cluster_equalsize(EMData* d) {
19541         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
19542         int nx = d->get_xsize();
19543         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19544         int K = N/2;
19545         vector<float> group(N);
19546         if(N*(N-1)/2 != nx) {
19547                 //print  "  incorrect dimension"
19548                 return group;}
19549         //bool active[N];
19550         int  active[N];
19551         for(int i=0; i<N; i++) active[i] = i;
19552 
19553         float dm, qd;
19554         int   ppi = 0, ppj = 0, ln = N;
19555         for(int k=0; k<K; k++) {
19556                 // find pairs of most similiar objects among active
19557                 //cout<<"  k:  "<<k<<endl;
19558                 dm = 1.0e23;
19559                 for(int i=1; i<ln; i++) {
19560                         for(int j=0; j<i; j++) {
19561                                 //qd = (*d)(mono(active[i],active[j]));
19562                                 qd = (*d)(active[i]*(active[i] - 1)/2 + active[j]);
19563                                 if(qd < dm) {
19564                                         dm = qd;
19565                                         ppi = i;
19566                                         ppj = j;
19567                                 }
19568                         }
19569                 }
19570                 group[2*k]   = float(active[ppi]);
19571                 group[1+2*k] = float(active[ppj]);
19572                 //  Shorten the list
19573                 if(ppi > ln-3 || ppj > ln - 3) {
19574                         if(ppi > ln-3 && ppj > ln - 3) {
19575                         } else if(ppi > ln-3) {
19576                                 if(ppi == ln -1) active[ppj] = active[ln-2];
19577                                 else             active[ppj] = active[ln-1];
19578                         } else { // ppj>ln-3
19579                                 if(ppj == ln -1) active[ppi] = active[ln-2];
19580                                 else             active[ppi] = active[ln-1];
19581                         }
19582                 } else {
19583                         active[ppi] = active[ln-1];
19584                         active[ppj] = active[ln-2];
19585                 }
19586                 ln = ln - 2;
19587         }
19588         return  group;
19589 }
19590 
19591 */
19592 #define data(i,j) group[i*ny+j]
19593 vector<float> Util::vareas(EMData* d) {
19594         const float step=0.001f;
19595         int ny = d->get_ysize();
19596         //  input emdata should have size 2xN, where N is number of points
19597         //  output vector should be 2xN, first element is the number of elements
19598         //  associated with this point, second is 0 is the element is touching the border, 1 if it is interior
19599         vector<float> group(2*ny);
19600         for(int i=0; i<2*ny; i++) group[i] = 0.0f;
19601         int K = int(1.0f/step) +1;
19602         int hit = 0;
19603         for(int kx=0; kx<=K; kx++) {
19604                 float tx = kx*step;
19605                 for(int ky=0; ky<=K; ky++) {
19606                         float ty = ky*step;
19607                         float dm = 1.0e23f;
19608                         for(int i=0; i<ny; i++) {
19609                                 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
19610                                 if( qd < dm) {
19611                                         dm = qd;
19612                                         hit = i;
19613                                 }
19614                         }
19615                         data(0,hit) += 1.0f;
19616                         if(kx == 0 || ky == 0 || kx == K || ky == K)  data(1,hit) = 1.0f;
19617                 }
19618         }
19619         return  group;
19620 }
19621 #undef data
19622 
19623 EMData* Util::get_slice(EMData *vol, int dim, int index) {
19624 
19625         int nx = vol->get_xsize();
19626         int ny = vol->get_ysize();
19627         int nz = vol->get_zsize();
19628         float *vol_data = vol->get_data();
19629         int new_nx, new_ny;
19630 
19631         if (nz == 1)
19632                 throw ImageDimensionException("Error: Input must be a 3-D object");
19633         if ((dim < 1) || (dim > 3))
19634                 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
19635         if (((dim == 1) && (index < 0 || index > nx-1)) ||
19636           ((dim == 1) && (index < 0 || index > nx-1)) ||
19637           ((dim == 1) && (index < 0 || index > nx-1)))
19638                 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
19639 
19640         if (dim == 1) {
19641                 new_nx = ny;
19642                 new_ny = nz;
19643         } else if (dim == 2) {
19644                 new_nx = nx;
19645                 new_ny = nz;
19646         } else {
19647                 new_nx = nx;
19648                 new_ny = ny;
19649         }
19650 
19651         EMData *slice = new EMData();
19652         slice->set_size(new_nx, new_ny, 1);
19653         float *slice_data = slice->get_data();
19654 
19655         if (dim == 1) {
19656                 for (int x=0; x<new_nx; x++)
19657                         for (int y=0; y<new_ny; y++)
19658                                 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
19659         } else if (dim == 2) {
19660                 for (int x=0; x<new_nx; x++)
19661                         for (int y=0; y<new_ny; y++)
19662                                 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
19663         } else {
19664                 for (int x=0; x<new_nx; x++)
19665                         for (int y=0; y<new_ny; y++)
19666                                 slice_data[y*new_nx+x] = vol_data[(index*ny+y)*nx+x];
19667         }
19668 
19669         return slice;
19670 }
19671 
19672 void Util::image_mutation(EMData *img, float mutation_rate) {
19673         int nx = img->get_xsize();
19674         float min = img->get_attr("minimum");
19675         float max = img->get_attr("maximum");
19676         float* img_data = img->get_data();
19677         array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
19678         return;
19679 }
19680 
19681 
19682 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
19683 
19684         if (is_mirror != 0) {
19685                 for (int i=0; i<len_list; i++) {
19686                         int r = rand()%10000;
19687                         float f = r/10000.0f;
19688                         if (f < mutation_rate) list[i] = 1-list[i];
19689                 }
19690         } else {
19691                 map<int, vector<int> >  graycode;
19692                 map<vector<int>, int> rev_graycode;
19693                 vector <int> gray;
19694 
19695                 int K=1;
19696                 for (int i=0; i<L; i++) K*=2;
19697 
19698                 for (int k=0; k<K; k++) {
19699                         int shift = 0;
19700                         vector <int> gray;
19701                         for (int i=L-1; i>-1; i--) {
19702                                 int t = ((k>>i)%2-shift)%2;
19703                                 gray.push_back(t);
19704                                 shift += t-2;
19705                         }
19706                         graycode[k] = gray;
19707                         rev_graycode[gray] = k;
19708                 }
19709 
19710                 float gap = (K-1)/(max_val-min_val);
19711                 for (int i=0; i<len_list; i++) {
19712                         float val = list[i];
19713                         if (val < min_val) { val = min_val; }
19714                         else if  (val > max_val) { val = max_val; }
19715                         int k = int((val-min_val)*gap+0.5);
19716                         vector<int> gray = graycode[k];
19717                         bool changed = false;
19718                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
19719                                 int r = rand()%10000;
19720                                 float f = r/10000.0f;
19721                                 if (f < mutation_rate) {
19722                                         *p = 1-*p;
19723                                         changed = true;
19724                                 }
19725                         }
19726                         if (changed) {
19727                                 k = rev_graycode[gray];
19728                                 list[i] = k/gap+min_val;
19729                         }
19730                 }
19731         }
19732 
19733 }
19734 
19735 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
19736 
19737         if (is_mirror != 0) {
19738                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
19739                         int r = rand()%10000;
19740                         float f = r/10000.0f;
19741                         if (f < mutation_rate) *q = 1-*q;
19742                 }
19743         } else {
19744                 map<int, vector<int> >  graycode;
19745                 map<vector<int>, int> rev_graycode;
19746                 vector <int> gray;
19747 
19748                 int K=1;
19749                 for (int i=0; i<L; i++) K*=2;
19750 
19751                 for (int k=0; k<K; k++) {
19752                         int shift = 0;
19753                         vector <int> gray;
19754                         for (int i=L-1; i>-1; i--) {
19755                                 int t = ((k>>i)%2-shift)%2;
19756                                 gray.push_back(t);
19757                                 shift += t-2;
19758                         }
19759                         graycode[k] = gray;
19760                         rev_graycode[gray] = k;
19761                 }
19762 
19763                 float gap = (K-1)/(max_val-min_val);
19764                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
19765                         float val = *q;
19766                         if (val < min_val) { val = min_val; }
19767                         else if  (val > max_val) { val = max_val; }
19768                         int k = int((val-min_val)*gap+0.5);
19769                         vector<int> gray = graycode[k];
19770                         bool changed = false;
19771                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
19772                                 int r = rand()%10000;
19773                                 float f = r/10000.0f;
19774                                 if (f < mutation_rate) {
19775                                         *p = 1-*p;
19776                                         changed = true;
19777                                 }
19778                         }
19779                         if (changed) {
19780                                 k = rev_graycode[gray];
19781                                 *q = k/gap+min_val;
19782                         }
19783                 }
19784         }
19785         return list;
19786 }
19787 
19788 
19789 void Util::bb_enumerate_(int* argParts, int* dimClasses, int nParts, int K, int T,int n_guesses, int* Levels) {
19790 
19791         int* Indices = new int[nParts*K];
19792         // Make a vector of nParts vectors of K int* each
19793         vector <vector <int*> > Parts(nParts,vector<int*>(K));
19794         int ind_c = 0;
19795 
19796         for (int i=0; i < nParts; i++){
19797                 for(int j = 0; j < K; j++){
19798                         Parts[i][j]=argParts + ind_c;
19799                         Indices[i*K + j] = ind_c;
19800                         ind_c = ind_c + *(dimClasses+i*K + j);
19801 
19802                 }
19803         }
19804 
19805         // make a copy of vector Parts for debugging purposes.
19806         // comment out if not debugging
19807         vector <vector <int*> > oldParts = Parts;
19808 
19809         // in the following we call initial_prune with Parts which is a vector. This is not the most
19810         // efficient since vector does not allow for direct addressing. But since initial_prune doesn't have very high complexity, and
19811         // the running time for 7 partitions with 288 classes per partition is on the order of a couple of minutes at most, i'll just leave it for now.....
19812         Util::initial_prune(Parts, dimClasses, nParts, K,T);
19813         //**********************************************************************************************************************************************
19814 
19815         // figure out the partition with the smallest number of classes. that will be the MAXIMUM number of matches we can find
19816         unsigned int numLevels = Parts[0].size();// initialize to number of classes in the first partition
19817         for (int i=1; i < nParts; i++){
19818                 if (Parts[i].size() < numLevels) numLevels = Parts[i].size();
19819         }
19820 
19821         // To maintain feasibility there can be at most
19822         // numLevel matches in the optimal solution.
19823 
19824         // int* Levels = new int[numLevels]; // Levels[i] corresponds to the number of possibilities we consider for the i-th match, and this
19825                                           // determines how many branches occur at that level.
19826         // numLevels is at most K. Since Levels is pre-allocated in python code with K elements, it should be fine.
19827         //for(int i =0; i < numLevels; i++)
19828         //      Levels[i] = 1;
19829         // modify argParts so the dummy variable of each class is set to 1 if the class is not removed by initial_prune, and -1 otherwise.
19830         // since the branch function is extremely computationally intensive, we have to pass it argParts (the 1-dimensional array) instead
19831         // of the vector Parts (which doesn't allow for direct access).
19832 
19833         // Indices[i*K + j] is the number of offsets from the beginning of argParts of the first element of the j-th class of hte i-th partition
19834 
19835         for(int i = 0; i < nParts; i++){
19836                 for(int j=0; j < K; j++){
19837                         *(argParts + Indices[i*K + j]+1) = -1;
19838                 }
19839         }
19840 
19841         int num_classes;
19842         int old_index;
19843         for(int i=0; i<nParts; i++){
19844                 num_classes = Parts[i].size();// number of classes in partition i after pruning
19845                 //cout<<"num_classes: "<< num_classes<<"\n";
19846                 for (int j=0; j < num_classes; j++){
19847                         old_index = *(Parts[i][j]);
19848                         //cout << "old_index: " << old_index<<"\n";
19849                         *(argParts + Indices[i*K + old_index]+1) = 1;
19850                 }
19851         }
19852 
19853         // 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
19854         // and the rest is the list of matches
19855         // in one dimensional form.
19856         cout <<"begin partition matching\n";
19857         int* output = Util::branch(argParts, Indices,dimClasses, nParts, K, T,Levels, numLevels,0,n_guesses);
19858         cout <<"done with partition matching \n";
19859         cout<<"total cost: "<<*output<<"\n";
19860         cout<<"number of matches: "<<*(output+1)<<"\n";
19861         // 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
19862         //bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
19863 }
19864 
19865 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
19866         //cout<<"sanitycheck called\n";
19867         int total_cost = *output;
19868         int num_matches = *(output+1);
19869 
19870         int cost=0;
19871         int* intx;
19872         int intx_size;
19873         int* intx_next(0);
19874         int intx_next_size = 0;
19875         int curclass;
19876         int curclass_size;
19877         //cout<<"cost by match: [";
19878         for(int i = 0; i < num_matches; i++){
19879                 curclass = *(output+2+ i*nParts);
19880                 // check feasibility
19881                 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
19882                 *(argParts + Indices[curclass]+1) = -5;
19883                 // initialize intx to the number of elements in class curclass of partition 0 of the i-th match
19884                 curclass_size = *(dimClasses+curclass)-2;
19885                 intx = new int[curclass_size];
19886                 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
19887                 intx_size = curclass_size;
19888 
19889                 for (int j=1; j < nParts; j++){
19890                       curclass = *(output+2+ i*nParts+j);
19891                       if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
19892                       *(argParts + Indices[j*K+curclass]+1)=-5;
19893                       // compute the intersection of intx and class curclass of partition j of the i-th match
19894                       intx_next_size = Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,0);
19895                       intx_next = new int[intx_next_size];
19896                       Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
19897                       delete[] intx;
19898                       intx=intx_next;
19899                       intx_size= intx_next_size;
19900                       if (j==nParts-1) delete[] intx_next;
19901                 }
19902 
19903                 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
19904                 //cout <<intx_next_size<<",";
19905                 cost = cost + intx_next_size;
19906         }
19907         //cout<<"]\n";
19908         if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
19909 
19910         return 1;
19911 
19912 }
19913 
19914 int* Util::branch(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int curlevel,int n_guesses) {
19915         // Base Case: we're at a leaf, no more feasible matches possible
19916 
19917         if (curlevel > nLevels-1){
19918                 int* res = new int[2];
19919                 *res = 0;
19920                 *(res+1)=0;
19921                 return res;
19922         }
19923 
19924         // We may still find more feasible matchings with cost gt T, so explore level curlevel
19925         int nBranches = *(Levels + curlevel);
19926 
19927         // call findTopLargest to get the nBranches feasible matchings with the largest weight (gt T) over all other feasible matches
19928         // matchlist is in one dimensional array form......
19929 
19930         int* matchlist = new int[nBranches*nParts];
19931         int* costlist = new int[nBranches];// cost of each of the nBranches matches. If cost[i] < T then that means findTopLargest found less than i+1 matches
19932                                            // with cost > T
19933 
19934         // initialize elements of costlist to 0
19935         for (int i=0; i < nBranches; i++) *(costlist+i)=0;
19936 
19937         // each class in the matches found by findTopLargest is encoded by the original index of the first element of the class in argPart
19938         // each match contains nParts classes, with the i-th class belonging to the i-th partition.
19939 
19940         Util::findTopLargest(argParts,Indices, dimClasses, nParts, K,  T, matchlist, nBranches,costlist,n_guesses);
19941 
19942         // if there are no feasible matches with cost gt T, then return 0
19943         if (costlist[0]<= T){
19944                 int* res = new int[2];
19945                 *res = 0;
19946                 *(res+1)=0;
19947                 return res;
19948         }
19949 
19950         int* maxreturn = new int[2];//initialize to placeholder
19951         *maxreturn=0;
19952         *(maxreturn+1)=0;
19953 
19954         // some temporary variables
19955         int old_index;
19956         int totalcost;
19957         int nmatches;
19958         //int offset;
19959 
19960         for(int i=0; i < nBranches ; i++){
19961 
19962                 // consider the i-th match returned by findTopLargest
19963 
19964                 if (costlist[i] <= T) break;
19965 
19966                 // 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.
19967                 // After branch returns, compute overall cost, unmark  the classes just marked as 1 again in preparation for next loop.
19968 
19969                 for(int j=0; j < nParts; j++){
19970                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
19971                         old_index=matchlist[i*nParts + j];
19972                         *(argParts + Indices[j*K+old_index] + 1) = -2;
19973                 }
19974 
19975 
19976                 int* ret = Util::branch(argParts, Indices, dimClasses, nParts, K, T, Levels, nLevels, curlevel+1,n_guesses);
19977 
19978                 // first element of ret is the total cost of all the matches in ret, and second element is the number of matches in ret
19979                 totalcost = costlist[i] + *ret;
19980 
19981                 // *************************************************************************************
19982                 // for debugging purposes
19983 
19984                 // debug 1: for multi-branching in levels i less some pre-specified maxLevel. Assume maxLevel is pretty small else way too many print outs to be useful
19985                    bool debug1 = 0;
19986                    if (debug1){
19987                        int maxLevel = 2;
19988                         if (curlevel < maxLevel) cout<<"total cost level" << curlevel<<": "<<totalcost<<"\n";
19989                    }
19990 
19991                 // debug 2: for multi-branching in ALL (or many ...) levels. This is data specific so it's all hard coded
19992                    bool debug2 = 0;
19993                    if (debug2){
19994                         int skip1 = 5;
19995                         int max1=20;
19996                          if ((curlevel <= max1) && (curlevel%skip1 == 0)) cout << "total cost level "<< curlevel<<": "<<totalcost<<"\n";
19997 
19998                         int skip2 = 10;
19999                         int max2 = 70;
20000                         if ((curlevel > max1 )&&(curlevel <= max2) && (curlevel%skip2 == 0)) cout << "total cost level "<< curlevel<<": "<<totalcost<<"\n";
20001                    }
20002                 // *************************************************************************************
20003 
20004 
20005                 // if totalcost > maxcost, or some variatio thereof, then copy the stuff over to maxreturn.
20006                 // There are several possibilities here:
20007                 //    Option 1: Simply compare costs and take the largest one.
20008                 //    Option 2: At each level, if two costs are equal, then take the one which contains fewer matches, and thus
20009                 //              ensuring matches with larger weights. The motivation for this is largely the (possibly naive) assumption that
20010                 //              if we take the average of a larger number of images, then the averaged image will be "better".
20011                 //    Option 3: Do option 2 only on the highest level, i.e., curlevel=0
20012 
20013                  if (totalcost > *maxreturn) // option 1
20014                 // if ((totalcost > *maxreturn) || ( (curlevel==0) && (totalcost == *maxreturn) && (*(ret+1)+1 < *(maxreturn+1)) )) // option 3
20015 
20016                 //if ((totalcost > *maxreturn) || ( (totalcost == *maxreturn) && (*(ret+1)+1 < *(maxreturn+1)) )) // option 2
20017                 {
20018                         nmatches = 1 + *(ret+1);
20019                         delete[] maxreturn; // get rid of the old maxreturn
20020                         maxreturn = new int[2+nmatches*nParts];
20021                         *maxreturn = totalcost;
20022                         *(maxreturn + 1)= nmatches;
20023                         int nret = 2+(nmatches-1)*nParts;
20024                         for(int iret=2; iret <nret;iret++) *(maxreturn+iret)=*(ret+iret);
20025                         for(int imax=0; imax<nParts;imax++) *(maxreturn+nret+imax)=matchlist[i*nParts + imax];
20026                 }
20027 
20028 
20029                 delete[] ret;
20030 
20031                 // unmark the marked classes in preparation for the next iteration
20032 
20033                 for(int j=0; j < nParts; j++){
20034                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
20035                         old_index=matchlist[i*nParts + j];
20036                         *(argParts + Indices[j*K+old_index] + 1) = 1;
20037                 }
20038 
20039         }
20040 
20041         delete[] matchlist;
20042         delete[] costlist;
20043         return maxreturn;
20044 
20045 }
20046 
20047 int Util::findTopLargest(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int max_num_matches, int* costlist, int n_guesses){
20048         int guess;
20049         int* curmax = new int[nParts+1]; // first element is the max weight and the subsequent elements is the match with the weight.
20050         int newT=T;
20051         int num_found=0;
20052 
20053         for(int i=0; i < max_num_matches; i++){
20054                 guess = Util::generatesubmax(argParts, Indices,dimClasses,nParts, K, T,  n_guesses);
20055 
20056                 if (T < guess) newT = guess -1;
20057                 // find the feasible match with the largest weight and put results in curmax
20058                 Util::search2(argParts, Indices,dimClasses,nParts, K, newT,curmax);
20059                 if (*curmax <= T){
20060                         max_num_matches=i;
20061                         break;
20062                 }
20063                 else {
20064                         *(costlist+i) = *curmax;
20065 
20066                         for (int j=0; j<nParts; j++){
20067                                 *(matchlist+i*nParts+j) = *(curmax+1+j);
20068                                 *(argParts + Indices[j*K+*(curmax+1+j)] + 1) = -3;// mark the classes in curmax as unavailable using -3 (remember to change it back)
20069 
20070                         }
20071                         num_found = num_found+1;
20072                 }
20073 
20074         }
20075 
20076 
20077         delete[] curmax;
20078         // go through the selected classes (in matchlist) and reset to 1
20079 
20080         for (int i=0 ; i < max_num_matches; i++){
20081                 for (int j = 0; j < nParts; j++){
20082                         *(argParts + Indices[j*K+*(matchlist+i*nParts +j)] + 1) = 1;
20083                 }
20084 
20085         }
20086 
20087 
20088         return num_found;
20089 }
20090 
20091 
20092 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int newT, int* curmax){
20093         // initialize the current max weight to 0
20094         *curmax= 0;
20095         // some temp variables
20096         bool flag = 0;
20097         int nintx;
20098         int* dummy(0);
20099         int* ret;
20100 
20101         for(int a=0; a<K; a++)
20102         {
20103                 // check that class a of partition 0 is active and has greater than newT elements. If not the case, then skip to the next class
20104                 if (*(argParts + Indices[a] + 1) < 1) continue;
20105                 if (*(dimClasses + a)-2 <= newT) continue;
20106 
20107                 // 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
20108 
20109                 for( int i=1; i < nParts; i++){
20110                         flag = 0; // if flag stays 0 then no class in this partition is active, which implies no feasible match with class a of part 0
20111                         for(int j=0; j < K; j++){
20112                                 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
20113                                 if (*(dimClasses + i*K+j)-2 <= newT) {*(argParts + Indices[i*K+j] + 1) =-4; continue;}
20114                                 nintx = Util::k_means_cont_table_(argParts + Indices[a]+2,argParts + Indices[i*K+j]+2, dummy, *(dimClasses + a)-2, *(dimClasses + i*K+j)-2,0);
20115                                 if (nintx > newT) flag=1;
20116                                 else *(argParts + Indices[i*K+j] + 1) =-4;
20117                         }
20118                         if (flag==0) {break;}
20119                 }
20120 
20121                 // explore determines the feasible match with the largest weight greater than newT
20122                 if (flag > 0){ // Each partition has one or more active class
20123                         ret=Util::explore2(argParts, Indices, dimClasses, nParts, K, newT, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2, *(dimClasses+a)-2,0);
20124 
20125                         if (*ret > *curmax){
20126                                 *curmax = *ret;
20127                                 *(curmax+1)=a;
20128                                 for (int cp =0; cp < nParts-1; cp++) *(curmax+2+cp) = *(ret+1+cp);
20129 
20130                         }
20131                         delete[] ret;
20132                 }
20133                 // take all the classes marked as -4 and remark it as 1 in preparation for next round
20134                 for( int i=1; i < nParts; i++){
20135                         for(int j=0; j < K; j++){
20136                                 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
20137 
20138                         }
20139                 }
20140 
20141 
20142 
20143         }
20144 
20145 
20146 }
20147 
20148 
20149 int* Util::explore2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int newT, int* curintx, int size_curintx, int* next, int size_next, int depth){
20150 // depth is the level which is going to be explored in the current iteration
20151         int* curintx2(0);
20152 
20153         int nintx = size_curintx;
20154 
20155         // take the intx of next and cur
20156         if (depth >0){
20157                 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
20158                 if (nintx <= newT) {curintx2 = new int[1]; *curintx2=0;return curintx2;}
20159         }
20160 
20161         // we're at a leaf so return.
20162         if (depth == (nParts-1)) { curintx2 = new int[1]; *curintx2 = nintx; return curintx2;}
20163 
20164 
20165         // have not yet reached a leaf, and current weight is still greather than T, so keep on going.
20166 
20167         if (depth > 0){
20168                 curintx2 = new int[nintx]; // put the intersection set in here
20169                 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
20170         }
20171 
20172         if (depth == 0){
20173                 // set curintx2 to curintx
20174                 curintx2 = new int[size_curintx];
20175                 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
20176         }
20177 
20178 
20179         // recursion (non-leaf case)
20180         depth=depth+1;
20181         int* curmax = new int[nParts-depth+1];
20182         *curmax=0;
20183         int* ret;
20184         // we now consider each of the classes in partition depth in turn
20185         for (int i=0; i < K; i++){
20186 
20187                 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue; // class is not active so move on
20188                 size_next = (*(dimClasses + depth*K+i ))-2;
20189                 if (size_next <= newT) continue;
20190                 ret = Util::explore2(argParts,Indices, dimClasses, nParts, K, newT, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth);
20191                 if (*ret > *curmax && *ret > newT){
20192                         *curmax = *ret;
20193                         *(curmax+1)=i;
20194                         for (int j=0; j<nParts-depth-1; j++) { *(curmax+2 + j) = *(ret+1+j);}
20195                 }
20196                 delete[] ret;
20197         }
20198 
20199         delete[] curintx2;
20200         return curmax;
20201 }
20202 
20203 
20204 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
20205         //cout<<"initial_prune\n";
20206         // simple initial pruning. For class indClass of partition indPart:
20207         // 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
20208         // If for some partition (not equal to indPart) for which there is no such class, then return 0 immediately.
20209 
20210         // 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
20211 
20212         // remember when calling k_means_cont_table, the first element of each class is an index, and actual number of elements in the class
20213         // (as stored in dimClasses) should be decremented by 1 accordingly, and second is dummy variable
20214 
20215         int* dummy(0);
20216         int* cref;
20217         int cref_size;
20218         int* ccomp;
20219         int ccomp_size;
20220         int nintx;
20221         for (int i=0; i < nParts; i++){
20222                 for (int j =0; j < K; j++){
20223 
20224                         // consider class Parts[i][j]
20225                         cref = Parts[i][j];//incr by 1 since first element is index and second is dummy
20226                         cref_size = (*(dimClasses+i*K+(*cref)))-2;
20227 
20228 
20229                         if (cref_size <= T){
20230 
20231                                 *cref = -1;
20232                                 continue;
20233                         }
20234                         bool done = 0;
20235                         for (int a = 0; a < nParts; a++){
20236                                 if (a == i) continue; //consider all classes not in partition i and set to inactive all those classes whose intx with cref is not gt T
20237                                 bool hasActive=0;
20238                                 for (unsigned int b=0; b < Parts[a].size(); b++){
20239                                         // get the card of the intx between Parts[i][j] and Parts[a][b] using k_means_cont_table
20240                                         // remember first element of each class is the index of the class
20241                                         ccomp = Parts[a][b];
20242                                         ccomp_size= (*(dimClasses+a*K+(*ccomp)))-2;
20243                                         nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
20244 
20245 
20246                                         if (nintx <= T)
20247                                                 *(ccomp+1) = 0; // class Parts[a][b] is 'inactive' for cref
20248                                         else{
20249                                                 *(ccomp+1)=1; // class Parts[a][b] is 'active' for cref
20250                                                 hasActive=1;
20251                                         }
20252                                 }
20253                                 // see if partition a has at least one active class.if not then we're done with cref
20254                                 if (hasActive < 1){
20255                                    done=1;
20256                                    break;
20257                                 }
20258 
20259                         }
20260 
20261                         if (done > 0){
20262                                 // remove class j from partition i
20263 
20264                                 *cref = -1; // mark for deletion later
20265                                 continue; // move on to class Parts[i][j+1]
20266                         }
20267 
20268                         // now we adopt more aggressive measures. we find the match with the largest weight which has class Parts[i][j] for partition i.
20269                         // 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.
20270 
20271                         // (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.
20272                         // Note that if there are ALOT of partitions, then sorting the partititions may actually take longer than the time saved by ordering hte
20273                         // partitions. However, we do not have a lot of partitions, (usually around 10), so sorting time will be trivial compared to exploring time.
20274 
20275                         // explore returns one if there is a feasible matching containing class cref which has weight gt T, and otherwise returns 0
20276                         //bool found = 1;
20277                         bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
20278 
20279                         if (found<1){ // There is NO feasible matching with class j (cref)  with weight greater than T, so delete this class from Parts
20280                                 // Parts[i].erase(Parts[i].begin()+j);
20281                                 *cref = -1;
20282                         }
20283                 }
20284 
20285                 // Erase from Parts[i] all the classes that's being designated for erasure
20286 
20287                 for (int d = K-1; d > -1; d--){
20288                         if (*(Parts[i][d]) < 0) Parts[i].erase(Parts[i].begin()+d);
20289                 }
20290 
20291         }
20292 
20293         // Print out how many classes are left in each partition
20294         //for (int i =0; i < nParts; i++)
20295         //      cout << Parts[i].size()<<", ";
20296         //cout << "\n";
20297 }
20298 
20299 
20300 bool Util::explore(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T, int partref, int* curintx, int size_curintx, int* next,  int size_next, int depth) {
20301 
20302 
20303         if (size_next <= T) return 0;
20304 
20305         // take the intx of next and cur
20306         int* curintx2(0);
20307         int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
20308         if (nintx <= T) return 0;
20309 
20310         int old_depth=depth;
20311         if (depth == partref) depth = depth + 1; // we skip classes in partref
20312         if (depth == (nParts)) { if (old_depth>0) return 1;}
20313 
20314         // have not yet reached a leaf, and current weight is still greather than T, so keep on going.
20315 
20316         curintx2 = new int[nintx]; // put the intersection set in here
20317         Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
20318 
20319         // if (old_depth > 0) {delete[] curintx;} // don't delete when depth == 0 because curintx in that case is a pointer into Parts
20320 
20321         // we now consider each of the classes in partition (depth+1) in turn
20322         bool gt_thresh;
20323         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
20324 
20325         for (int i=0; i < num_classes; i++){
20326                 if (*(Parts[depth][i]+1) < 1) continue; // class is not active so move on
20327                 size_next = (*(dimClasses + depth*K+(*(Parts[depth][i])) ))-2;
20328                 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
20329                 if (gt_thresh) return 1;
20330         }
20331         delete[] curintx2;
20332         return 0;
20333 }
20334 
20335 
20336 int Util::generatesubmax(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int n_guesses){
20337         int guess=0;
20338 
20339         int* perm = new int[nParts];
20340         for(int i=0; i<nParts; i++) perm[i]=i;
20341 
20342         // some temporary variables
20343         int* intx(0);
20344         int* intx_next(0);
20345         int nintx;
20346         int nintxmax=0;
20347         int class_max = 0, class_max_next = 0;
20348         int intx_size = 0, part, part_next;
20349         int ipold,indsw;
20350 
20351         for(int i=0; i< n_guesses; i++){
20352                 // shuffle perm array
20353                 for(int ip = 0; ip<nParts; ip++){
20354                         indsw = (rand())%nParts;
20355                         // swap ip(th) element with the (indsw)th element
20356                         ipold = perm[ip];
20357                         perm[ip]=perm[indsw];
20358                         perm[indsw]=ipold;
20359                 }
20360 
20361 
20362                 // find the two classes in partitions perm[0] and perm[1] that yield the largest intersection
20363                 part=*perm;
20364                 part_next=*(perm+1);
20365                 for (int a=0; a < K; a++)
20366                 {
20367                         if (*(argParts + Indices[part*K+a] + 1) < 1) continue;
20368                         for (int b=0; b < K; b++)
20369                         {
20370                                 if (*(argParts + Indices[part_next*K + b] + 1) < 1) continue;
20371                                 nintx = Util::k_means_cont_table_(argParts + Indices[part*K+a]+2,argParts + Indices[part_next*K + b]+2, intx, *(dimClasses + part*K+a)-2,  *(dimClasses + part_next*K + b)-2,0);
20372                                 if (nintx <= nintxmax) continue;
20373                                 nintxmax = nintx;
20374                                 class_max = a;
20375                                 class_max_next = b;
20376                         }
20377                 }
20378 
20379                 // no more....
20380                 if (nintxmax < 1) {continue;}
20381 
20382                 if (nParts > 2){
20383                         intx = new int[nintxmax];
20384                         intx_size = nintxmax;
20385                         Util::k_means_cont_table_(argParts + Indices[part*K+class_max]+2,argParts + Indices[part_next*K + class_max_next]+2, intx, *(dimClasses + part*K+class_max)-2, *(dimClasses+part_next*K+class_max_next)-2,1); // get intx
20386                 }
20387 
20388                 // for each subsequent partition perm[i], i>=2, find the partition that yields the largest weight with the current intx
20389                 for(int j = 2; j < nParts; j++){
20390                         part = *(perm+j);
20391                         nintxmax=0;
20392                         for(int a = 0; a < K; a++){
20393                                 if (*(argParts + Indices[part*K+a] + 1) < 1) continue; // skip inactive classes
20394                                 nintx =  Util::k_means_cont_table_(intx, argParts + Indices[part*K + a]+2, intx_next, intx_size,  (*(dimClasses + part*K+a))-2,0);
20395                                 if (nintx <= nintxmax) continue;
20396                                 nintxmax = nintx;
20397                                 class_max = a;
20398                         }
20399 
20400                         // no more stuff....
20401                         if (nintxmax < 1) {
20402 
20403                                 delete[] intx;
20404                                 break;
20405                         }
20406 
20407 
20408                         intx_next = new int[nintxmax];
20409                         Util::k_means_cont_table_(intx, argParts + Indices[part*K + class_max]+2, intx_next, intx_size,  *(dimClasses + part*K+class_max)-2,1);
20410                         delete[] intx;
20411                         intx = intx_next;
20412                         intx_size = nintxmax;
20413                         if (j==nParts - 1) delete[] intx_next;
20414 
20415                 }
20416 
20417                 if (nintxmax > guess) guess = nintxmax;
20418 
20419         }
20420         delete[] perm;
20421         return guess;
20422 }
20423 
20424 
20425 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int nTop, int n_guesses, bool doMPI, int* Levels) {
20426 
20427         // 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
20428         // 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
20429         // Make a vector of nParts vectors of K int* each
20430          int* Indices = new int[nParts*K];
20431          int ind_c = 0;
20432          for (int i=0; i < nParts; i++){
20433                  for(int j = 0; j < K; j++){
20434                          Indices[i*K + j] = ind_c;
20435                          ind_c = ind_c + *(dimClasses+i*K + j);
20436 
20437                  }
20438          }
20439 
20440         // return top weighted matches for mpi version
20441         if (nTop > 0 && doMPI > 0){
20442                  // find the nTop largest matches (not required to be mutually feasible)
20443                 int* matchlist = new int[nTop*nParts];
20444                 int* costlist=new int[nTop];
20445                 for (int i=0; i< nTop; i++) {*(costlist+i) = 0;}
20446                 int matchesFound = Util::findTopLargest(argParts,Indices, dimClasses, nParts, K,  T, matchlist, nTop,costlist,n_guesses);
20447                 vector<int> ret(nTop*(nParts+1) + 1);
20448                 ret[0] = matchesFound;
20449                 int m = nParts + 1;
20450                 // For each match in matchlist and its corresponding cost in costlist, put them in ret
20451                 for(int i=0; i < nTop; i++){
20452                         ret[1+i*m] = *(costlist+i);
20453                         for (int j=0; j < nParts; j++){
20454                                 ret[1+i*m + 1 + j] = matchlist[i*nParts + j];
20455                         }
20456                 }
20457 
20458                 return ret;
20459 
20460         }
20461 
20462         // do initial pruning on argParts and return the pruned partitions
20463 
20464         // Make a vector of nParts vectors of K int* each
20465         vector <vector <int*> > Parts(nParts,vector<int*>(K));
20466         ind_c = 0;
20467         int argParts_size=0;
20468         for (int i=0; i < nParts; i++){
20469                 for(int j = 0; j < K; j++){
20470                         Parts[i][j]=argParts + ind_c;
20471                         ind_c = ind_c + *(dimClasses+i*K + j);
20472                         argParts_size = argParts_size + *(dimClasses+i*K + j);
20473 
20474                 }
20475         }
20476 
20477         // in the following we call initial_prune with Parts which is a vector. This is not the most
20478         // efficient since vector does not allow for direct addressing. But since initial_prune doesn't have very high complexity, and
20479         // 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.....
20480 
20481         Util::initial_prune(Parts, dimClasses, nParts, K,T);
20482         for(int i = 0; i < nParts; i++){
20483                 for(int j=0; j < K; j++){
20484                         *(argParts + Indices[i*K + j]+1) = -1;
20485                 }
20486         }
20487 
20488         int num_classes;
20489         int old_index;
20490         for(int i=0; i<nParts; i++){
20491                 num_classes = Parts[i].size();// number of classes in partition i after pruning
20492                 for (int j=0; j < num_classes; j++){
20493                         old_index = *(Parts[i][j]);
20494                         //cout << "old_index: " << old_index<<"\n";
20495                         *(argParts + Indices[i*K + old_index]+1) = 1;
20496                 }
20497         }
20498 
20499 
20500         if (doMPI > 0){
20501                 // turn argParts into vector ret and return ret
20502                 vector<int> ret(argParts_size);
20503                 for(int i=0; i < argParts_size; i++)
20504                         ret[i]= (*(argParts+i));
20505 
20506                 return ret;
20507         }
20508 
20509         // if we're not doing mpi then keep going and call branchMPI and return the output
20510         //cout <<"begin partition matching\n";
20511         int* dummy(0);
20512         int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T,Levels, K,0,n_guesses,-1, dummy);
20513         //cout <<"done with partition matching \n";
20514         //cout<<"total cost: "<<*output<<"\n";
20515         //cout<<"number of matches: "<<*(output+1)<<"\n";
20516         // 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
20517         bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
20518 
20519         // something is wrong with output of branchMPI!
20520         if (correct < 1){
20521                 cout << "something is wrong with output of branchMPI!\n";
20522                 vector<int> ret(1);
20523                 ret[0]=-1;
20524                 return ret;
20525         }
20526 
20527         // output is not nonsense, so now put it into a single dimension vector and return
20528         // 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
20529         // and the rest is the list of matches. output is one dimensional
20530 
20531         int output_size = 2+ *(output+1) * nParts;
20532         vector<int> ret(output_size);
20533         for (int i = 0; i < output_size; i++){
20534                 ret[i]=*(output+i);
20535         }
20536         return ret;
20537 
20538 }
20539 
20540 vector<int> Util::branchMPIpy_(int* argParts, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int n_guesses, int nFirst, int* firstmatches){
20541         //cout<<"branchMPIpy_ called\n";
20542         // if nLevels == K, then we  compute nLevels - which is the number of active classes of the partition with the smallest number of active classes
20543         // this is not really necessary but would save a call to findTopLargest in branchMPI
20544         int num_active;
20545         int* Indices = new int[nParts*K];
20546         // Make a vector of nParts vectors of K int* each
20547         int ind_c = 0;
20548         for (int i=0; i < nParts; i++){
20549                 num_active = 0;
20550                 for(int j = 0; j < K; j++){
20551                         Indices[i*K + j] = ind_c; // offset from argParts of the first element of the jth class of the i-th partition
20552                         if (*(argParts+ind_c + 1) == 1) num_active = num_active + 1;
20553                         ind_c = ind_c + *(dimClasses+i*K + j);
20554                 }
20555 
20556                 if (num_active < nLevels) {nLevels = num_active;}
20557         }
20558 
20559 
20560         //add in code for dynamically changing levels
20561 
20562         //cout<<"num levels "<<nLevels<<"\n";
20563         //cout<<"calling branchMPI\n";
20564 
20565         int* output = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T,  Levels,  nLevels, 0,n_guesses, nFirst,firstmatches);
20566 
20567         // call sanity check on outupt to make sure the returned matches are feasible with cost over the threshold T
20568         //cout<<"total cost: "<<*output<<"\n";
20569         //cout<<"number of matches: "<<*(output+1)<<"\n";
20570         // 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
20571         bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
20572 
20573         // something is wrong with output of branchMPI!
20574         if (correct < 1){
20575                 cout << "something is wrong with output of branchMPI!\n";
20576                 vector<int> ret(1);
20577                 ret[0]=-1;
20578                 return ret;
20579         }
20580 
20581         // output is not nonsense, so now put it into a single dimension vector and return
20582         // 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
20583         // and the rest is the list of matches. output is one dimensional
20584 
20585         int output_size = 2+ *(output+1) * nParts;
20586         vector<int> ret(output_size);
20587         for (int i = 0; i < output_size; i++){
20588                 ret[i]=*(output+i);
20589         }
20590         return ret;
20591 }
20592 
20593 
20594 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* Levels, int nLevels, int curlevel,int n_guesses, int nFirst, int* firstmatches) {
20595 
20596         // Base Case: we're at a leaf, no more feasible matches possible
20597         if (curlevel > nLevels-1){
20598                 int* res = new int[2];
20599                 *res = 0;
20600                 *(res+1)=0;
20601                 return res;
20602         }
20603 
20604 
20605         // We may still find more feasible matchings with cost gt T, so explore level curlevel
20606         int nBranches = *(Levels + curlevel);
20607 
20608         // MPI: the first match is already chosen in MPI version, so we are going to branch only once at level 0
20609         if (curlevel==0 && nFirst > 0)
20610         {
20611                 nBranches = nFirst;
20612         }
20613 
20614         // call findTopLargest to get the nBranches feasible matchings with the largest weight (gt T) over all other feasible matches
20615 
20616         int* matchlist = new int[nBranches*nParts];
20617         int* costlist = new int[nBranches];// cost of each of the nBranches matches. If cost[i] < T then that means findTopLargest found less than i+1 matches
20618                                            // with cost > T
20619 
20620         for (int i=0; i < nBranches; i++)
20621                 *(costlist+i)=0;
20622 
20623         // each class in the matches found by findTopLargest is encoded by the original index of the first element of the class in argPart
20624         // each match contains nParts classes, with the i-th class belonging to the i-th partition.
20625 
20626         // MPI: first match is already chosen, so copy the match in first match over to matchlist, compute weight of match, and set costlist to the weight
20627         if (curlevel == 0 && nFirst > 0){
20628                 for(int i = 0; i < nBranches; i++){
20629                         *(costlist+i) = *(firstmatches +i*(nParts+1));
20630                         for (int j=0; j< nParts; j++)
20631                                 *(matchlist + i*nParts +j) = *(firstmatches +i*(nParts+1) + 1 + j);
20632                 }
20633         }
20634         else
20635                 Util::findTopLargest(argParts,Indices, dimClasses, nParts, K,  T, matchlist, nBranches,costlist,n_guesses);
20636 
20637         // if there are no feasible matches with cost gt T, then return 0
20638         if (costlist[0]<= T){
20639                 int* res = new int[2];
20640                 *res = 0;
20641                 *(res+1)=0;
20642                 return res;
20643         }
20644 
20645         int* maxreturn = new int[2];//initialize to placeholder
20646         *maxreturn=0;
20647         *(maxreturn+1)=0;
20648 
20649         // some temporary variables
20650         int old_index;
20651         int totalcost;
20652         int nmatches;
20653         //int offset;
20654 
20655         for(int i=0; i < nBranches ; i++){
20656 
20657                 // consider the i-th match returned by findTopLargest
20658 
20659                 if (costlist[i] <= T) break;
20660 
20661                 // 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.
20662                 // After branch returns, compute overall cost, unmark  the classes just marked as 1 again in preparation for next loop.
20663 
20664                 for(int j=0; j < nParts; j++){
20665                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
20666                         old_index=matchlist[i*nParts + j];
20667                         *(argParts + Indices[j*K+old_index] + 1) = -2;
20668                 }
20669 
20670 
20671                 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T, Levels, nLevels, curlevel+1,n_guesses, nFirst, firstmatches);
20672 
20673                 // first element of ret is the total cost of all the matches in ret, and second element is the number of matches in ret
20674                 totalcost = costlist[i] + *ret;
20675 
20676 
20677                  if (totalcost > *maxreturn) // option 1
20678                 {
20679                         nmatches = 1 + *(ret+1);
20680                         delete[] maxreturn; // get rid of the old maxreturn
20681                         maxreturn = new int[2+nmatches*nParts];
20682                         *maxreturn = totalcost;
20683                         *(maxreturn + 1)= nmatches;
20684                         int nret = 2+(nmatches-1)*nParts;
20685                         for(int iret=2; iret <nret;iret++) *(maxreturn+iret)=*(ret+iret);
20686                         for(int imax=0; imax<nParts;imax++) *(maxreturn+nret+imax)=matchlist[i*nParts + imax];
20687                 }
20688 
20689 
20690                 delete[] ret;
20691 
20692                 // unmark the marked classes in preparation for the next iteration
20693 
20694                 for(int j=0; j < nParts; j++){
20695                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
20696                         old_index=matchlist[i*nParts + j];
20697                         *(argParts + Indices[j*K+old_index] + 1) = 1;
20698                 }
20699 
20700         }
20701 
20702         delete[] matchlist;
20703         delete[] costlist;
20704 
20705         return maxreturn;
20706 
20707 }

Generated on Mon Jul 19 12:40:14 2010 for EMAN2 by  doxygen 1.4.7