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

util_sparx.cpp

Go to the documentation of this file.
00001 
00005 /*
00006  * Author: Pawel A.Penczek, 09/09/2006 (Pawel.A.Penczek@uth.tmc.edu)
00007  * Copyright (c) 2000-2006 The University of Texas - Houston Medical School
00008  *
00009  * This software is issued under a joint BSD/GNU license. You may use the
00010  * source code in this file under either license. However, note that the
00011  * complete EMAN2 and SPARX software packages have some GPL dependencies,
00012  * so you are responsible for compliance with the licenses of these packages
00013  * if you opt to use BSD licensing. The warranty disclaimer below holds
00014  * in either instance.
00015  *
00016  * This complete copyright notice must be included in any revised version of the
00017  * source code. Additional authorship citations may be added, but existing
00018  * author citations must be preserved.
00019  *
00020  * This program is free software; you can redistribute it and/or modify
00021  * it under the terms of the GNU General Public License as published by
00022  * the Free Software Foundation; either version 2 of the License, or
00023  * (at your option) any later version.
00024  *
00025  * This program is distributed in the hope that it will be useful,
00026  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00028  * GNU General Public License for more details.
00029  *
00030  * You should have received a copy of the GNU General Public License
00031  * along with this program; if not, write to the Free Software
00032  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00033  *
00034  */
00035 
00036 #ifdef _WIN32
00037 #pragma warning(disable:4819)
00038 #endif  //_WIN32
00039 
00040 #include <cstring>
00041 #include <ctime>
00042 #include <iostream>
00043 #include <cstdio>
00044 #include <cstdlib>
00045 #include <boost/format.hpp>
00046 #include "emdata.h"
00047 #include "util.h"
00048 #include "fundamentals.h"
00049 #include "lapackblas.h"
00050 #include "lbfgsb.h"
00051 using namespace EMAN;
00052 #include "steepest.h"
00053 #include "emassert.h"
00054 #include "randnum.h"
00055 
00056 #include <gsl/gsl_sf_bessel.h>
00057 #include <gsl/gsl_sf_bessel.h>
00058 #include <cmath>
00059 using namespace std;
00060 using std::complex;
00061 
00062 vector<float> Util::infomask(EMData* Vol, EMData* mask, bool flip = false)
00063 //  flip true:  find statistics under the mask (mask >0.5)
00064 //  flip false: find statistics ourside the mask (mask <0.5)
00065 {
00066         ENTERFUNC;
00067         vector<float> stats;
00068         float *Volptr, *maskptr,MAX,MIN;
00069         long double Sum1,Sum2;
00070         long count;
00071 
00072         MAX = -FLT_MAX;
00073         MIN =  FLT_MAX;
00074         count = 0L;
00075         Sum1  = 0.0L;
00076         Sum2  = 0.0L;
00077 
00078         if (mask == NULL) {
00079                 //Vol->update_stat();
00080                 stats.push_back(Vol->get_attr("mean"));
00081                 stats.push_back(Vol->get_attr("sigma"));
00082                 stats.push_back(Vol->get_attr("minimum"));
00083                 stats.push_back(Vol->get_attr("maximum"));
00084                 return stats;
00085         }
00086 
00087         /* Check if the sizes of the mask and image are same */
00088 
00089         size_t nx = Vol->get_xsize();
00090         size_t ny = Vol->get_ysize();
00091         size_t nz = Vol->get_zsize();
00092 
00093         size_t mask_nx = mask->get_xsize();
00094         size_t mask_ny = mask->get_ysize();
00095         size_t mask_nz = mask->get_zsize();
00096 
00097         if  (nx != mask_nx || ny != mask_ny || nz != mask_nz )
00098                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
00099 
00100  /*       if (nx != mask_nx ||
00101             ny != mask_ny ||
00102             nz != mask_nz  ) {
00103            // should throw an exception here!!! (will clean it up later CY)
00104            fprintf(stderr, "The dimension of the image does not match the dimension of the mask!\n");
00105            fprintf(stderr, " nx = %d, mask_nx = %d\n", nx, mask_nx);
00106            fprintf(stderr, " ny = %d, mask_ny = %d\n", ny, mask_ny);
00107            fprintf(stderr, " nz = %d, mask_nz = %d\n", nz, mask_nz);
00108            exit(1);
00109         }
00110  */
00111         Volptr = Vol->get_data();
00112         maskptr = mask->get_data();
00113 
00114         for (size_t i = 0; i < nx*ny*nz; i++) {
00115                 if ((maskptr[i]>0.5f) == flip) {
00116                         Sum1 += Volptr[i];
00117                         Sum2 += Volptr[i]*double(Volptr[i]);
00118                         MAX = (MAX < Volptr[i])?Volptr[i]:MAX;
00119                         MIN = (MIN > Volptr[i])?Volptr[i]:MIN;
00120                         count++;
00121                 }
00122         }
00123 
00124         if (count == 0) {
00125                 LOGERR("Invalid mask");
00126                 throw ImageFormatException( "Invalid mask");
00127         }
00128 
00129         float avg = static_cast<float>(Sum1/count);
00130         float sig = static_cast<float>(sqrt((Sum2 - Sum1*Sum1/count)/(count-1)));
00131 
00132         stats.push_back(avg);
00133         stats.push_back(sig);
00134         stats.push_back(MIN);
00135         stats.push_back(MAX);
00136 
00137         return stats;
00138 }
00139 
00140 
00141 //----------------------------------------------------------------------------------------------------------
00142 
00143 Dict Util::im_diff(EMData* V1, EMData* V2, EMData* mask)
00144 {
00145         ENTERFUNC;
00146 
00147         if (!EMUtil::is_same_size(V1, V2)) {
00148                 LOGERR("images not same size");
00149                 throw ImageFormatException( "images not same size");
00150         }
00151 
00152         size_t nx = V1->get_xsize();
00153         size_t ny = V1->get_ysize();
00154         size_t nz = V1->get_zsize();
00155         size_t size = nx*ny*nz;
00156 
00157         EMData *BD = new EMData();
00158         BD->set_size(nx, ny, nz);
00159 
00160         float *params = new float[2];
00161 
00162         float *V1ptr, *V2ptr, *MASKptr, *BDptr, A, B;
00163         long double S1=0.L,S2=0.L,S3=0.L,S4=0.L;
00164         int nvox = 0L;
00165 
00166         V1ptr = V1->get_data();
00167         V2ptr = V2->get_data();
00168         BDptr = BD->get_data();
00169 
00170 
00171         if(!mask){
00172                 EMData * Mask = new EMData();
00173                 Mask->set_size(nx,ny,nz);
00174                 Mask->to_one();
00175                 MASKptr = Mask->get_data();
00176         } else {
00177                 if (!EMUtil::is_same_size(V1, mask)) {
00178                         LOGERR("mask not same size");
00179                         throw ImageFormatException( "mask not same size");
00180                 }
00181 
00182                 MASKptr = mask->get_data();
00183         }
00184 
00185 
00186 
00187 //       calculation of S1,S2,S3,S3,nvox
00188 
00189         for (size_t i = 0L;i < size; i++) {
00190               if (MASKptr[i]>0.5f) {
00191                S1 += V1ptr[i]*V2ptr[i];
00192                S2 += V1ptr[i]*V1ptr[i];
00193                S3 += V2ptr[i];
00194                S4 += V1ptr[i];
00195                nvox ++;
00196               }
00197         }
00198 
00199         if ((nvox*S1 - S3*S4) == 0. || (nvox*S2 - S4*S4) == 0) {
00200                 A =1.0f ;
00201         } else {
00202                 A = static_cast<float>( (nvox*S1 - S3*S4)/(nvox*S2 - S4*S4) );
00203         }
00204         B = static_cast<float> (A*S4  -  S3)/nvox;
00205 
00206         // calculation of the difference image
00207 
00208         for (size_t i = 0L;i < size; i++) {
00209              if (MASKptr[i]>0.5f) {
00210                BDptr[i] = A*V1ptr[i] -  B  - V2ptr[i];
00211              }  else  {
00212                BDptr[i] = 0.f;
00213              }
00214         }
00215 
00216         BD->update();
00217 
00218         params[0] = A;
00219         params[1] = B;
00220 
00221         Dict BDnParams;
00222         BDnParams["imdiff"] = BD;
00223         BDnParams["A"] = params[0];
00224         BDnParams["B"] = params[1];
00225 
00226         EXITFUNC;
00227         return BDnParams;
00228  }
00229 
00230 //----------------------------------------------------------------------------------------------------------
00231 
00232 
00233 
00234 EMData *Util::TwoDTestFunc(int Size, float p, float q,  float a, float b, int flag, float alphaDeg) //PRB
00235 {
00236         ENTERFUNC;
00237         int Mid= (Size+1)/2;
00238 
00239         if (flag==0) { // This is the real function
00240                 EMData* ImBW = new EMData();
00241                 ImBW->set_size(Size,Size,1);
00242                 ImBW->to_zero();
00243 
00244                 float tempIm;
00245                 float x,y;
00246 
00247                 for (int ix=(1-Mid);  ix<Mid; ix++){
00248                         for (int iy=(1-Mid);  iy<Mid; iy++){
00249                                 x = (float)ix;
00250                                 y = (float)iy;
00251                         tempIm= static_cast<float>( (1/(2*M_PI)) * cos(p*x)* cos(q*y) * exp(-.5*x*x/(a*a))* exp(-.5*y*y/(b*b)) );
00252                                 (*ImBW)(ix+Mid-1,iy+Mid-1) = tempIm * exp(.5f*p*p*a*a)* exp(.5f*q*q*b*b);
00253                         }
00254                 }
00255                 ImBW->update();
00256                 ImBW->set_complex(false);
00257                 ImBW->set_ri(true);
00258 
00259                 return ImBW;
00260         }
00261         else if (flag==1) {  // This is the Fourier Transform
00262                 EMData* ImBWFFT = new EMData();
00263                 ImBWFFT ->set_size(2*Size,Size,1);
00264                 ImBWFFT ->to_zero();
00265 
00266                 float r,s;
00267 
00268                 for (int ir=(1-Mid);  ir<Mid; ir++){
00269                         for (int is=(1-Mid);  is<Mid; is++){
00270                                 r = (float)ir;
00271                                 s = (float)is;
00272                         (*ImBWFFT)(2*(ir+Mid-1),is+Mid-1)= cosh(p*r*a*a) * cosh(q*s*b*b) *
00273                                 exp(-.5f*r*r*a*a)* exp(-.5f*s*s*b*b);
00274                         }
00275                 }
00276                 ImBWFFT->update();
00277                 ImBWFFT->set_complex(true);
00278                 ImBWFFT->set_ri(true);
00279                 ImBWFFT->set_shuffled(true);
00280                 ImBWFFT->set_fftodd(true);
00281 
00282                 return ImBWFFT;
00283         }
00284         else if (flag==2 || flag==3) { //   This is the projection in Real Space
00285                 float alpha = static_cast<float>( alphaDeg*M_PI/180.0 );
00286                 float C=cos(alpha);
00287                 float S=sin(alpha);
00288                 float D= sqrt(S*S*b*b + C*C*a*a);
00289                 //float D2 = D*D;   PAP - to get rid of warning
00290 
00291                 float P = p * C *a*a/D ;
00292                 float Q = q * S *b*b/D ;
00293 
00294                 if (flag==2) {
00295                         EMData* pofalpha = new EMData();
00296                         pofalpha ->set_size(Size,1,1);
00297                         pofalpha ->to_zero();
00298 
00299                         float Norm0 =  D*(float)sqrt(2*pi);
00300                         float Norm1 =  exp( .5f*(P+Q)*(P+Q)) / Norm0 ;
00301                         float Norm2 =  exp( .5f*(P-Q)*(P-Q)) / Norm0 ;
00302                         float sD;
00303 
00304                         for (int is=(1-Mid);  is<Mid; is++){
00305                                 sD = is/D ;
00306                                 (*pofalpha)(is+Mid-1) =  Norm1 * exp(-.5f*sD*sD)*cos(sD*(P+Q))
00307                          + Norm2 * exp(-.5f*sD*sD)*cos(sD*(P-Q));
00308                         }
00309                         pofalpha-> update();
00310                         pofalpha-> set_complex(false);
00311                         pofalpha-> set_ri(true);
00312 
00313                         return pofalpha;
00314                 }
00315                 if (flag==3) { // This is the projection in Fourier Space
00316                         float vD;
00317 
00318                         EMData* pofalphak = new EMData();
00319                         pofalphak ->set_size(2*Size,1,1);
00320                         pofalphak ->to_zero();
00321 
00322                         for (int iv=(1-Mid);  iv<Mid; iv++){
00323                                 vD = iv*D ;
00324                                 (*pofalphak)(2*(iv+Mid-1)) =  exp(-.5f*vD*vD)*(cosh(vD*(P+Q)) + cosh(vD*(P-Q)) );
00325                         }
00326                         pofalphak-> update();
00327                         pofalphak-> set_complex(false);
00328                         pofalphak-> set_ri(true);
00329 
00330                         return pofalphak;
00331                 }
00332         }
00333         else if (flag==4) {
00334                 cout <<" FH under construction";
00335                 EMData* OutFT= TwoDTestFunc(Size, p, q, a, b, 1);
00336                 EMData* TryFH= OutFT -> real2FH(4.0);
00337                 return TryFH;
00338         } else {
00339                 cout <<" flag must be 0,1,2,3, or 4";
00340         }
00341 
00342         EXITFUNC;
00343         return 0;
00344 }
00345 
00346 
00347 void Util::spline_mat(float *x, float *y, int n,  float *xq, float *yq, int m) //PRB
00348 {
00349 
00350         float x0= x[0];
00351         float x1= x[1];
00352         float x2= x[2];
00353         float y0= y[0];
00354         float y1= y[1];
00355         float y2= y[2];
00356         float yp1 =  (y1-y0)/(x1-x0) +  (y2-y0)/(x2-x0) - (y2-y1)/(x2-x1)  ;
00357         float xn  = x[n];
00358         float xnm1= x[n-1];
00359         float xnm2= x[n-2];
00360         float yn  = y[n];
00361         float ynm1= y[n-1];
00362         float ynm2= y[n-2];
00363         float ypn=  (yn-ynm1)/(xn-xnm1) +  (yn-ynm2)/(xn-xnm2) - (ynm1-ynm2)/(xnm1-xnm2) ;
00364         float *y2d = new float[n];
00365         Util::spline(x,y,n,yp1,ypn,y2d);
00366         Util::splint(x,y,y2d,n,xq,yq,m); //PRB
00367         delete [] y2d;
00368         return;
00369 }
00370 
00371 
00372 void Util::spline(float *x, float *y, int n, float yp1, float ypn, float *y2) //PRB
00373 {
00374         int i,k;
00375         float p, qn, sig, un, *u;
00376         u = new float[n-1];
00377 
00378         if (yp1 > .99e30){
00379                 y2[0]=u[0]=0.0;
00380         } else {
00381                 y2[0]=-.5f;
00382                 u[0] =(3.0f/ (x[1] -x[0]))*( (y[1]-y[0])/(x[1]-x[0]) -yp1);
00383         }
00384 
00385         for (i=1; i < n-1; i++) {
00386                 sig= (x[i] - x[i-1])/(x[i+1] - x[i-1]);
00387                 p = sig*y2[i-1] + 2.0f;
00388                 y2[i]  = (sig-1.0f)/p;
00389                 u[i] = (y[i+1] - y[i] )/(x[i+1]-x[i] ) -  (y[i] - y[i-1] )/(x[i] -x[i-1]);
00390                 u[i] = (6.0f*u[i]/ (x[i+1]-x[i-1]) - sig*u[i-1])/p;
00391         }
00392 
00393         if (ypn>.99e30){
00394                 qn=0; un=0;
00395         } else {
00396                 qn= .5f;
00397                 un= (3.0f/(x[n-1] -x[n-2])) * (ypn -  (y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
00398         }
00399         y2[n-1]= (un - qn*u[n-2])/(qn*y2[n-2]+1.0f);
00400         for (k=n-2; k>=0; k--){
00401                 y2[k]=y2[k]*y2[k+1]+u[k];
00402         }
00403         delete [] u;
00404 }
00405 
00406 
00407 void Util::splint( float *xa, float *ya, float *y2a, int n,  float *xq, float *yq, int m) //PRB
00408 {
00409         int klo, khi, k;
00410         float h, b, a;
00411 
00412 //      klo=0; // can try to put here
00413         for (int j=0; j<m;j++){
00414                 klo=0;
00415                 khi=n-1;
00416                 while (khi-klo >1) {
00417                         k=(khi+klo) >>1;
00418                         if  (xa[k]>xq[j]){ khi=k;}
00419                         else { klo=k;}
00420                 }
00421                 h=xa[khi]- xa[klo];
00422                 if (h==0.0) printf("Bad XA input to routine SPLINT \n");
00423                 a =(xa[khi]-xq[j])/h;
00424                 b=(xq[j]-xa[klo])/h;
00425                 yq[j]=a*ya[klo] + b*ya[khi]
00426                         + ((a*a*a-a)*y2a[klo]
00427                              +(b*b*b-b)*y2a[khi]) *(h*h)/6.0f;
00428         }
00429 //      printf("h=%f, a = %f, b=%f, ya[klo]=%f, ya[khi]=%f , yq=%f\n",h, a, b, ya[klo], ya[khi],yq[0]);
00430 }
00431 
00432 
00433 void Util::Radialize(int *PermMatTr, float *kValsSorted,   // PRB
00434                float *weightofkValsSorted, int Size, int *SizeReturned)
00435 {
00436         int iMax = (int) floor( (Size-1.0)/2 +.01);
00437         int CountMax = (iMax+2)*(iMax+1)/2;
00438         int Count=-1;
00439         float *kVals     = new float[CountMax];
00440         float *weightMat = new float[CountMax];
00441         int *PermMat     = new   int[CountMax];
00442         SizeReturned[0] = CountMax;
00443 
00444 //      printf("Aa \n");        fflush(stdout);
00445         for (int jkx=0; jkx< iMax+1; jkx++) {
00446                 for (int jky=0; jky< jkx+1; jky++) {
00447                         Count++;
00448                         kVals[Count] = sqrtf((float) (jkx*jkx +jky*jky));
00449                         weightMat[Count]=  1.0;
00450                         if (jkx!=0)  { weightMat[Count] *=2;}
00451                         if (jky!=0)  { weightMat[Count] *=2;}
00452                         if (jkx!=jky){ weightMat[Count] *=2;}
00453                         PermMat[Count]=Count+1;
00454                 }
00455         }
00456 
00457         int lkVals = Count+1;
00458 //      printf("Cc \n");fflush(stdout);
00459 
00460         sort_mat(&kVals[0],&kVals[Count],
00461              &PermMat[0],  &PermMat[Count]);  //PermMat is
00462                                 //also returned as well as kValsSorted
00463         fflush(stdout);
00464 
00465         int newInd;
00466 
00467         for (int iP=0; iP < lkVals ; iP++ ) {
00468                 newInd =  PermMat[iP];
00469                 PermMatTr[newInd-1] = iP+1;
00470         }
00471 
00472 //      printf("Ee \n"); fflush(stdout);
00473 
00474         int CountA=-1;
00475         int CountB=-1;
00476 
00477         while (CountB< (CountMax-1)) {
00478                 CountA++;
00479                 CountB++;
00480 //              printf("CountA=%d ; CountB=%d \n", CountA,CountB);fflush(stdout);
00481                 kValsSorted[CountA] = kVals[CountB] ;
00482                 if (CountB<(CountMax-1) ) {
00483                         while (fabs(kVals[CountB] -kVals[CountB+1])<.0000001  ) {
00484                                 SizeReturned[0]--;
00485                                 for (int iP=0; iP < lkVals; iP++){
00486 //                                      printf("iP=%d \n", iP);fflush(stdout);
00487                                         if  (PermMatTr[iP]>CountA+1) {
00488                                                 PermMatTr[iP]--;
00489                                         }
00490                                 }
00491                                 CountB++;
00492                         }
00493                 }
00494         }
00495 
00496 
00497         for (int CountD=0; CountD < CountMax; CountD++) {
00498             newInd = PermMatTr[CountD];
00499             weightofkValsSorted[newInd-1] += weightMat[CountD];
00500         }
00501 
00502 }
00503 
00504 
00505 vector<float>
00506 Util::even_angles(float delta, float t1, float t2, float p1, float p2)
00507 {
00508         vector<float> angles;
00509         float psi = 0.0;
00510         if ((0.0 == t1 && 0.0 == t2)||(t1 >= t2)) {
00511                 t1 = 0.0f;
00512                 t2 = 90.0f;
00513         }
00514         if ((0.0 == p1 && 0.0 == p2)||(p1 >= p2)) {
00515                 p1 = 0.0f;
00516                 p2 = 359.9f;
00517         }
00518         bool skip = ((t1 < 90.0)&&(90.0 == t2)&&(0.0 == p1)&&(p2 > 180.0));
00519         for (float theta = t1; theta <= t2; theta += delta) {
00520                 float detphi;
00521                 int lt;
00522                 if ((0.0 == theta)||(180.0 == theta)) {
00523                         detphi = 360.0f;
00524                         lt = 1;
00525                 } else {
00526                         detphi = delta/sin(theta*static_cast<float>(dgr_to_rad));
00527                         lt = int((p2 - p1)/detphi)-1;
00528                         if (lt < 1) lt = 1;
00529                         detphi = (p2 - p1)/lt;
00530                 }
00531                 for (int i = 0; i < lt; i++) {
00532                         float phi = p1 + i*detphi;
00533                         if (skip&&(90.0 == theta)&&(phi > 180.0)) continue;
00534                         angles.push_back(phi);
00535                         angles.push_back(theta);
00536                         angles.push_back(psi);
00537                 }
00538         }
00539         return angles;
00540 }
00541 
00542 
00543 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00544 /*float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00545 {
00546 
00547 //  purpose: quadratic interpolation
00548 //
00549 //  parameters:       xx,yy treated as circularly closed.
00550 //                    fdata - image 1..nxdata, 1..nydata
00551 //
00552 //                    f3    fc       f0, f1, f2, f3 are the values
00553 //                     +             at the grid points.  x is the
00554 //                     + x           point at which the function
00555 //              f2++++f0++++f1       is to be estimated. (it need
00556 //                     +             not be in the first quadrant).
00557 //                     +             fc - the outer corner point
00558 //                    f4             nearest x.
00559 c
00560 //                                   f0 is the value of the fdata at
00561 //                                   fdata(i,j), it is the interior mesh
00562 //                                   point nearest  x.
00563 //                                   the coordinates of f0 are (x0,y0),
00564 //                                   the coordinates of f1 are (xb,y0),
00565 //                                   the coordinates of f2 are (xa,y0),
00566 //                                   the coordinates of f3 are (x0,yb),
00567 //                                   the coordinates of f4 are (x0,ya),
00568 //                                   the coordinates of fc are (xc,yc),
00569 c
00570 //                   o               hxa, hxb are the mesh spacings
00571 //                   +               in the x-direction to the left
00572 //                  hyb              and right of the center point.
00573 //                   +
00574 //            ++hxa++o++hxb++o       hyb, hya are the mesh spacings
00575 //                   +               in the y-direction.
00576 //                  hya
00577 //                   +               hxc equals either  hxb  or  hxa
00578 //                   o               depending on where the corner
00579 //                                   point is located.
00580 c
00581 //                                   construct the interpolant
00582 //                                   f = f0 + c1*(x-x0) +
00583 //                                       c2*(x-x0)*(x-x1) +
00584 //                                       c3*(y-y0) + c4*(y-y0)*(y-y1)
00585 //                                       + c5*(x-x0)*(y-y0)
00586 //
00587 //
00588 
00589     float x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00590     float quadri;
00591     int   i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00592 
00593     x = xx;
00594     y = yy;
00595 
00596     // circular closure
00597         while ( x < 1.0 ) x += nxdata;
00598         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00599         while ( y < 1.0 ) y += nydata;
00600         while ( y >= (float)(nydata+1) )  y -= nydata;
00601 
00602 
00603     i   = (int) x;
00604     j   = (int) y;
00605 
00606     dx0 = x - i;
00607     dy0 = y - j;
00608 
00609     ip1 = i + 1;
00610     im1 = i - 1;
00611     jp1 = j + 1;
00612     jm1 = j - 1;
00613 
00614     if (ip1 > nxdata) ip1 = ip1 - nxdata;
00615     if (im1 < 1)      im1 = im1 + nxdata;
00616     if (jp1 > nydata) jp1 = jp1 - nydata;
00617     if (jm1 < 1)      jm1 = jm1 + nydata;
00618 
00619     f0  = fdata(i,j);
00620     c1  = fdata(ip1,j) - f0;
00621     c2  = (c1 - f0 + fdata(im1,j)) * 0.5;
00622     c3  = fdata(i,jp1) - f0;
00623     c4  = (c3 - f0 + fdata(i,jm1)) * 0.5;
00624 
00625     dxb = dx0 - 1;
00626     dyb = dy0 - 1;
00627 
00628     // hxc & hyc are either 1 or -1
00629     if (dx0 >= 0) { hxc = 1; } else { hxc = -1; }
00630     if (dy0 >= 0) { hyc = 1; } else { hyc = -1; }
00631 
00632     ic  = i + hxc;
00633     jc  = j + hyc;
00634 
00635     if (ic > nxdata) { ic = ic - nxdata; }  else if (ic < 1) { ic = ic + nxdata; }
00636     if (jc > nydata) { jc = jc - nydata; } else if (jc < 1) { jc = jc + nydata; }
00637 
00638     c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0)) * c2
00639             - hyc * c3 - (hyc * (hyc - 1.0)) * c4) * (hxc * hyc));
00640 
00641     quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00642 
00643     return quadri;
00644 }*/
00645 float Util::quadri(float xx, float yy, int nxdata, int nydata, float* fdata)
00646 {
00647 //  purpose: quadratic interpolation
00648 //  Optimized for speed, circular closer removed, checking of ranges removed
00649         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00650         float  quadri;
00651         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00652 
00653         x = xx;
00654         y = yy;
00655 
00656         //     any xx and yy
00657         while ( x < 1.0 )                 x += nxdata;
00658         while ( x >= (float)(nxdata+1) )  x -= nxdata;
00659         while ( y < 1.0 )                 y += nydata;
00660         while ( y >= (float)(nydata+1) )  y -= nydata;
00661 
00662         i   = (int) x;
00663         j   = (int) y;
00664 
00665         dx0 = x - i;
00666         dy0 = y - j;
00667 
00668         ip1 = i + 1;
00669         im1 = i - 1;
00670         jp1 = j + 1;
00671         jm1 = j - 1;
00672 
00673         if (ip1 > nxdata) ip1 -= nxdata;
00674         if (im1 < 1)      im1 += nxdata;
00675         if (jp1 > nydata) jp1 -= nydata;
00676         if (jm1 < 1)      jm1 += nydata;
00677 
00678         f0  = fdata(i,j);
00679         c1  = fdata(ip1,j) - f0;
00680         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00681         c3  = fdata(i,jp1) - f0;
00682         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00683 
00684         dxb = dx0 - 1;
00685         dyb = dy0 - 1;
00686 
00687         // hxc & hyc are either 1 or -1
00688         if (dx0 >= 0) hxc = 1; else hxc = -1;
00689         if (dy0 >= 0) hyc = 1; else hyc = -1;
00690 
00691         ic  = i + hxc;
00692         jc  = j + hyc;
00693 
00694         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00695         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00696 
00697         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00698                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00699 
00700 
00701         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00702 
00703         return quadri;
00704 }
00705 
00706 #undef fdata
00707 
00708 #define  fdata(i,j)      fdata[ i-1 + (j-1)*nxdata ]
00709 float Util::quadri_background(float xx, float yy, int nxdata, int nydata, float* fdata, int xnew, int ynew)
00710 {
00711 //  purpose: quadratic interpolation
00712 //  Optimized for speed, circular closer removed, checking of ranges removed
00713         float  x, y, dx0, dy0, f0, c1, c2, c3, c4, c5, dxb, dyb;
00714         float  quadri;
00715         int    i, j, ip1, im1, jp1, jm1, ic, jc, hxc, hyc;
00716 
00717         x = xx;
00718         y = yy;
00719 
00720         // wrap around is not done circulantly; if (x,y) is not in the image, then x = xnew and y = ynew
00721         if ( (x < 1.0) || ( x >= (float)(nxdata+1) ) || ( y < 1.0 ) || ( y >= (float)(nydata+1) )){
00722               x = (float)xnew;
00723                   y = (float)ynew;
00724      }
00725 
00726 
00727         i   = (int) x;
00728         j   = (int) y;
00729 
00730         dx0 = x - i;
00731         dy0 = y - j;
00732 
00733         ip1 = i + 1;
00734         im1 = i - 1;
00735         jp1 = j + 1;
00736         jm1 = j - 1;
00737 
00738         if (ip1 > nxdata) ip1 -= nxdata;
00739         if (im1 < 1)      im1 += nxdata;
00740         if (jp1 > nydata) jp1 -= nydata;
00741         if (jm1 < 1)      jm1 += nydata;
00742 
00743         f0  = fdata(i,j);
00744         c1  = fdata(ip1,j) - f0;
00745         c2  = (c1 - f0 + fdata(im1,j)) * 0.5f;
00746         c3  = fdata(i,jp1) - f0;
00747         c4  = (c3 - f0 + fdata(i,jm1)) * 0.5f;
00748 
00749         dxb = dx0 - 1;
00750         dyb = dy0 - 1;
00751 
00752         // hxc & hyc are either 1 or -1
00753         if (dx0 >= 0) hxc = 1; else hxc = -1;
00754         if (dy0 >= 0) hyc = 1; else hyc = -1;
00755 
00756         ic  = i + hxc;
00757         jc  = j + hyc;
00758 
00759         if (ic > nxdata) ic -= nxdata;  else if (ic < 1) ic += nxdata;
00760         if (jc > nydata) jc -= nydata;  else if (jc < 1) jc += nydata;
00761 
00762         c5  =  ( (fdata(ic,jc) - f0 - hxc * c1 - (hxc * (hxc - 1.0f)) * c2
00763                 - hyc * c3 - (hyc * (hyc - 1.0f)) * c4) * (hxc * hyc));
00764 
00765 
00766         quadri = f0 + dx0 * (c1 + dxb * c2 + dy0 * c5) + dy0 * (c3 + dyb * c4);
00767 
00768         return quadri;
00769 }
00770 
00771 #undef fdata
00772 
00773 
00774 float  Util::get_pixel_conv_new(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb) {
00775         int K = kb.get_window_size();
00776         int kbmin = -K/2;
00777         int kbmax = -kbmin;
00778         int kbc = kbmax+1;
00779 
00780         float pixel =0.0f;
00781         float w=0.0f;
00782 
00783         delx = restrict1(delx, nx);
00784         int inxold = int(round(delx));
00785         if ( ny < 2 ) {  //1D
00786                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00787                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00788                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00789                 float tablex4 = kb.i0win_tab(delx-inxold);
00790                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00791                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00792                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00793 
00794                 int x1, x2, x3, x4, x5, x6, x7;
00795 
00796                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
00797                         x1 = (inxold-3+nx)%nx;
00798                         x2 = (inxold-2+nx)%nx;
00799                         x3 = (inxold-1+nx)%nx;
00800                         x4 = (inxold  +nx)%nx;
00801                         x5 = (inxold+1+nx)%nx;
00802                         x6 = (inxold+2+nx)%nx;
00803                         x7 = (inxold+3+nx)%nx;
00804                 } else {
00805                         x1 = inxold-3;
00806                         x2 = inxold-2;
00807                         x3 = inxold-1;
00808                         x4 = inxold;
00809                         x5 = inxold+1;
00810                         x6 = inxold+2;
00811                         x7 = inxold+3;
00812                 }
00813 
00814                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
00815                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
00816                         data[x7]*tablex7 ;
00817 
00818                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
00819         } else if ( nz < 2 ) {  // 2D
00820                 dely = restrict1(dely, ny);
00821                 int inyold = int(round(dely));
00822                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00823                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00824                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00825                 float tablex4 = kb.i0win_tab(delx-inxold);
00826                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00827                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00828                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00829 
00830                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00831                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00832                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00833                 float tabley4 = kb.i0win_tab(dely-inyold);
00834                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00835                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00836                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00837 
00838                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
00839 
00840                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
00841                         x1 = (inxold-3+nx)%nx;
00842                         x2 = (inxold-2+nx)%nx;
00843                         x3 = (inxold-1+nx)%nx;
00844                         x4 = (inxold  +nx)%nx;
00845                         x5 = (inxold+1+nx)%nx;
00846                         x6 = (inxold+2+nx)%nx;
00847                         x7 = (inxold+3+nx)%nx;
00848 
00849                         y1 = ((inyold-3+ny)%ny)*nx;
00850                         y2 = ((inyold-2+ny)%ny)*nx;
00851                         y3 = ((inyold-1+ny)%ny)*nx;
00852                         y4 = ((inyold  +ny)%ny)*nx;
00853                         y5 = ((inyold+1+ny)%ny)*nx;
00854                         y6 = ((inyold+2+ny)%ny)*nx;
00855                         y7 = ((inyold+3+ny)%ny)*nx;
00856                 } else {
00857                         x1 = inxold-3;
00858                         x2 = inxold-2;
00859                         x3 = inxold-1;
00860                         x4 = inxold;
00861                         x5 = inxold+1;
00862                         x6 = inxold+2;
00863                         x7 = inxold+3;
00864 
00865                         y1 = (inyold-3)*nx;
00866                         y2 = (inyold-2)*nx;
00867                         y3 = (inyold-1)*nx;
00868                         y4 = inyold*nx;
00869                         y5 = (inyold+1)*nx;
00870                         y6 = (inyold+2)*nx;
00871                         y7 = (inyold+3)*nx;
00872                 }
00873 
00874                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
00875                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
00876                              data[x7+y1]*tablex7 ) * tabley1 +
00877                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
00878                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
00879                              data[x7+y2]*tablex7 ) * tabley2 +
00880                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
00881                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
00882                              data[x7+y3]*tablex7 ) * tabley3 +
00883                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
00884                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
00885                              data[x7+y4]*tablex7 ) * tabley4 +
00886                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
00887                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
00888                              data[x7+y5]*tablex7 ) * tabley5 +
00889                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
00890                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
00891                              data[x7+y6]*tablex7 ) * tabley6 +
00892                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
00893                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
00894                              data[x7+y7]*tablex7 ) * tabley7;
00895 
00896                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
00897                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
00898         } else {  //  3D
00899                 dely = restrict1(dely, ny);
00900                 int inyold = int(Util::round(dely));
00901                 delz = restrict1(delz, nz);
00902                 int inzold = int(Util::round(delz));
00903 
00904                 float tablex1 = kb.i0win_tab(delx-inxold+3);
00905                 float tablex2 = kb.i0win_tab(delx-inxold+2);
00906                 float tablex3 = kb.i0win_tab(delx-inxold+1);
00907                 float tablex4 = kb.i0win_tab(delx-inxold);
00908                 float tablex5 = kb.i0win_tab(delx-inxold-1);
00909                 float tablex6 = kb.i0win_tab(delx-inxold-2);
00910                 float tablex7 = kb.i0win_tab(delx-inxold-3);
00911 
00912                 float tabley1 = kb.i0win_tab(dely-inyold+3);
00913                 float tabley2 = kb.i0win_tab(dely-inyold+2);
00914                 float tabley3 = kb.i0win_tab(dely-inyold+1);
00915                 float tabley4 = kb.i0win_tab(dely-inyold);
00916                 float tabley5 = kb.i0win_tab(dely-inyold-1);
00917                 float tabley6 = kb.i0win_tab(dely-inyold-2);
00918                 float tabley7 = kb.i0win_tab(dely-inyold-3);
00919 
00920                 float tablez1 = kb.i0win_tab(delz-inzold+3);
00921                 float tablez2 = kb.i0win_tab(delz-inzold+2);
00922                 float tablez3 = kb.i0win_tab(delz-inzold+1);
00923                 float tablez4 = kb.i0win_tab(delz-inzold);
00924                 float tablez5 = kb.i0win_tab(delz-inzold-1);
00925                 float tablez6 = kb.i0win_tab(delz-inzold-2);
00926                 float tablez7 = kb.i0win_tab(delz-inzold-3);
00927 
00928                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
00929 
00930                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
00931                         x1 = (inxold-3+nx)%nx;
00932                         x2 = (inxold-2+nx)%nx;
00933                         x3 = (inxold-1+nx)%nx;
00934                         x4 = (inxold  +nx)%nx;
00935                         x5 = (inxold+1+nx)%nx;
00936                         x6 = (inxold+2+nx)%nx;
00937                         x7 = (inxold+3+nx)%nx;
00938 
00939                         y1 = ((inyold-3+ny)%ny)*nx;
00940                         y2 = ((inyold-2+ny)%ny)*nx;
00941                         y3 = ((inyold-1+ny)%ny)*nx;
00942                         y4 = ((inyold  +ny)%ny)*nx;
00943                         y5 = ((inyold+1+ny)%ny)*nx;
00944                         y6 = ((inyold+2+ny)%ny)*nx;
00945                         y7 = ((inyold+3+ny)%ny)*nx;
00946 
00947                         z1 = ((inzold-3+nz)%nz)*nx*ny;
00948                         z2 = ((inzold-2+nz)%nz)*nx*ny;
00949                         z3 = ((inzold-1+nz)%nz)*nx*ny;
00950                         z4 = ((inzold  +nz)%nz)*nx*ny;
00951                         z5 = ((inzold+1+nz)%nz)*nx*ny;
00952                         z6 = ((inzold+2+nz)%nz)*nx*ny;
00953                         z7 = ((inzold+3+nz)%nz)*nx*ny;
00954                 } else {
00955                         x1 = inxold-3;
00956                         x2 = inxold-2;
00957                         x3 = inxold-1;
00958                         x4 = inxold;
00959                         x5 = inxold+1;
00960                         x6 = inxold+2;
00961                         x7 = inxold+3;
00962 
00963                         y1 = (inyold-3)*nx;
00964                         y2 = (inyold-2)*nx;
00965                         y3 = (inyold-1)*nx;
00966                         y4 = inyold*nx;
00967                         y5 = (inyold+1)*nx;
00968                         y6 = (inyold+2)*nx;
00969                         y7 = (inyold+3)*nx;
00970 
00971                         z1 = (inzold-3)*nx*ny;
00972                         z2 = (inzold-2)*nx*ny;
00973                         z3 = (inzold-1)*nx*ny;
00974                         z4 = inzold*nx*ny;
00975                         z5 = (inzold+1)*nx*ny;
00976                         z6 = (inzold+2)*nx*ny;
00977                         z7 = (inzold+3)*nx*ny;
00978                 }
00979 
00980                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
00981                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
00982                              data[x7+y1+z1]*tablex7 ) * tabley1 +
00983                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
00984                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
00985                              data[x7+y2+z1]*tablex7 ) * tabley2 +
00986                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
00987                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
00988                              data[x7+y3+z1]*tablex7 ) * tabley3 +
00989                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
00990                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
00991                              data[x7+y4+z1]*tablex7 ) * tabley4 +
00992                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
00993                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
00994                              data[x7+y5+z1]*tablex7 ) * tabley5 +
00995                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
00996                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
00997                              data[x7+y6+z1]*tablex7 ) * tabley6 +
00998                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
00999                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01000                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01001                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01002                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01003                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01004                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01005                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01006                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01007                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01008                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01009                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01010                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01011                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01012                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01013                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01014                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01015                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01016                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01017                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01018                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01019                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01020                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01021                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01022                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01023                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01024                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01025                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01026                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01027                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01028                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01029                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01030                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01031                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01032                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01033                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01034                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01035                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01036                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01037                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01038                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01039                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01040                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01041                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01042                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01043                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01044                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01045                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01046                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01047                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01048                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01049                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01050                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01051                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01052                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01053                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01054                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01055                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01056                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01057                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01058                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01059                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01060                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01061                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01062                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01063                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01064                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01065                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01066                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01067                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01068                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01069                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01070                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01071                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01072                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01073                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01074                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01075                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01076                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01077                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01078                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01079                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01080                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01081                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01082                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01083                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01084                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01085                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01086                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01087                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01088                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01089                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01090                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01091                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01092                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01093                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01094                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01095                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01096                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01097                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01098                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01099                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01100                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01101                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01102                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01103                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01104                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01105                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01106                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01107                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01108                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01109                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01110                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01111                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01112                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01113                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01114                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01115                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01116                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01117                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01118                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01119                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01120                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01121                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01122                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01123                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01124                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01125                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01126                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01127 
01128                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01129                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01130                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01131         }
01132         return pixel/w;
01133 }
01134 
01135 float  Util::get_pixel_conv_new_background(int nx, int ny, int nz, float delx, float dely, float delz, float* data, Util::KaiserBessel& kb, int xnew, int ynew) {
01136         int K = kb.get_window_size();
01137         int kbmin = -K/2;
01138         int kbmax = -kbmin;
01139         int kbc = kbmax+1;
01140 
01141         float pixel =0.0f;
01142         float w=0.0f;
01143 
01144     float argdelx = delx; // adding this for 2D case where the wrap around is not done circulantly using restrict1.
01145         delx = restrict1(delx, nx);
01146         int inxold = int(round(delx));
01147         if ( ny < 2 ) {  //1D
01148                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01149                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01150                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01151                 float tablex4 = kb.i0win_tab(delx-inxold);
01152                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01153                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01154                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01155 
01156                 int x1, x2, x3, x4, x5, x6, x7;
01157 
01158                 if ( inxold <= kbc || inxold >=nx-kbc-2 )  {
01159                         x1 = (inxold-3+nx)%nx;
01160                         x2 = (inxold-2+nx)%nx;
01161                         x3 = (inxold-1+nx)%nx;
01162                         x4 = (inxold  +nx)%nx;
01163                         x5 = (inxold+1+nx)%nx;
01164                         x6 = (inxold+2+nx)%nx;
01165                         x7 = (inxold+3+nx)%nx;
01166                 } else {
01167                         x1 = inxold-3;
01168                         x2 = inxold-2;
01169                         x3 = inxold-1;
01170                         x4 = inxold;
01171                         x5 = inxold+1;
01172                         x6 = inxold+2;
01173                         x7 = inxold+3;
01174                 }
01175 
01176                 pixel = data[x1]*tablex1 + data[x2]*tablex2 + data[x3]*tablex3 +
01177                         data[x4]*tablex4 + data[x5]*tablex5 + data[x6]*tablex6 +
01178                         data[x7]*tablex7 ;
01179 
01180                 w = tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7;
01181         } else if ( nz < 2 ) {  // 2D
01182 
01183                 delx = argdelx;
01184                 // the wrap around is not done circulantly for 2D case; if (argdelx, argdely) is not in the image, then make them (xnew, ynew) which is definitely in the image
01185                 if ((delx < 0.0f) || (delx >= (float) (nx)) || (dely < 0.0f) || (dely >= (float) (ny)) ){
01186                 delx = (float)xnew*2.0f;
01187                 dely = (float)ynew*2.0f;
01188                 }
01189 
01190                 int inxold = int(round(delx));
01191                 int inyold = int(round(dely));
01192 
01193                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01194                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01195                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01196                 float tablex4 = kb.i0win_tab(delx-inxold);
01197                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01198                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01199                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01200 
01201                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01202                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01203                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01204                 float tabley4 = kb.i0win_tab(dely-inyold);
01205                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01206                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01207                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01208 
01209                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7;
01210 
01211                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 )  {
01212                         x1 = (inxold-3+nx)%nx;
01213                         x2 = (inxold-2+nx)%nx;
01214                         x3 = (inxold-1+nx)%nx;
01215                         x4 = (inxold  +nx)%nx;
01216                         x5 = (inxold+1+nx)%nx;
01217                         x6 = (inxold+2+nx)%nx;
01218                         x7 = (inxold+3+nx)%nx;
01219 
01220                         y1 = ((inyold-3+ny)%ny)*nx;
01221                         y2 = ((inyold-2+ny)%ny)*nx;
01222                         y3 = ((inyold-1+ny)%ny)*nx;
01223                         y4 = ((inyold  +ny)%ny)*nx;
01224                         y5 = ((inyold+1+ny)%ny)*nx;
01225                         y6 = ((inyold+2+ny)%ny)*nx;
01226                         y7 = ((inyold+3+ny)%ny)*nx;
01227                 } else {
01228                         x1 = inxold-3;
01229                         x2 = inxold-2;
01230                         x3 = inxold-1;
01231                         x4 = inxold;
01232                         x5 = inxold+1;
01233                         x6 = inxold+2;
01234                         x7 = inxold+3;
01235 
01236                         y1 = (inyold-3)*nx;
01237                         y2 = (inyold-2)*nx;
01238                         y3 = (inyold-1)*nx;
01239                         y4 = inyold*nx;
01240                         y5 = (inyold+1)*nx;
01241                         y6 = (inyold+2)*nx;
01242                         y7 = (inyold+3)*nx;
01243                 }
01244 
01245                 pixel    = ( data[x1+y1]*tablex1 + data[x2+y1]*tablex2 + data[x3+y1]*tablex3 +
01246                              data[x4+y1]*tablex4 + data[x5+y1]*tablex5 + data[x6+y1]*tablex6 +
01247                              data[x7+y1]*tablex7 ) * tabley1 +
01248                            ( data[x1+y2]*tablex1 + data[x2+y2]*tablex2 + data[x3+y2]*tablex3 +
01249                              data[x4+y2]*tablex4 + data[x5+y2]*tablex5 + data[x6+y2]*tablex6 +
01250                              data[x7+y2]*tablex7 ) * tabley2 +
01251                            ( data[x1+y3]*tablex1 + data[x2+y3]*tablex2 + data[x3+y3]*tablex3 +
01252                              data[x4+y3]*tablex4 + data[x5+y3]*tablex5 + data[x6+y3]*tablex6 +
01253                              data[x7+y3]*tablex7 ) * tabley3 +
01254                            ( data[x1+y4]*tablex1 + data[x2+y4]*tablex2 + data[x3+y4]*tablex3 +
01255                              data[x4+y4]*tablex4 + data[x5+y4]*tablex5 + data[x6+y4]*tablex6 +
01256                              data[x7+y4]*tablex7 ) * tabley4 +
01257                            ( data[x1+y5]*tablex1 + data[x2+y5]*tablex2 + data[x3+y5]*tablex3 +
01258                              data[x4+y5]*tablex4 + data[x5+y5]*tablex5 + data[x6+y5]*tablex6 +
01259                              data[x7+y5]*tablex7 ) * tabley5 +
01260                            ( data[x1+y6]*tablex1 + data[x2+y6]*tablex2 + data[x3+y6]*tablex3 +
01261                              data[x4+y6]*tablex4 + data[x5+y6]*tablex5 + data[x6+y6]*tablex6 +
01262                              data[x7+y6]*tablex7 ) * tabley6 +
01263                            ( data[x1+y7]*tablex1 + data[x2+y7]*tablex2 + data[x3+y7]*tablex3 +
01264                              data[x4+y7]*tablex4 + data[x5+y7]*tablex5 + data[x6+y7]*tablex6 +
01265                              data[x7+y7]*tablex7 ) * tabley7;
01266 
01267                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01268                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7);
01269         } else {  //  3D
01270                 dely = restrict1(dely, ny);
01271                 int inyold = int(Util::round(dely));
01272                 delz = restrict1(delz, nz);
01273                 int inzold = int(Util::round(delz));
01274 
01275                 float tablex1 = kb.i0win_tab(delx-inxold+3);
01276                 float tablex2 = kb.i0win_tab(delx-inxold+2);
01277                 float tablex3 = kb.i0win_tab(delx-inxold+1);
01278                 float tablex4 = kb.i0win_tab(delx-inxold);
01279                 float tablex5 = kb.i0win_tab(delx-inxold-1);
01280                 float tablex6 = kb.i0win_tab(delx-inxold-2);
01281                 float tablex7 = kb.i0win_tab(delx-inxold-3);
01282 
01283                 float tabley1 = kb.i0win_tab(dely-inyold+3);
01284                 float tabley2 = kb.i0win_tab(dely-inyold+2);
01285                 float tabley3 = kb.i0win_tab(dely-inyold+1);
01286                 float tabley4 = kb.i0win_tab(dely-inyold);
01287                 float tabley5 = kb.i0win_tab(dely-inyold-1);
01288                 float tabley6 = kb.i0win_tab(dely-inyold-2);
01289                 float tabley7 = kb.i0win_tab(dely-inyold-3);
01290 
01291                 float tablez1 = kb.i0win_tab(delz-inzold+3);
01292                 float tablez2 = kb.i0win_tab(delz-inzold+2);
01293                 float tablez3 = kb.i0win_tab(delz-inzold+1);
01294                 float tablez4 = kb.i0win_tab(delz-inzold);
01295                 float tablez5 = kb.i0win_tab(delz-inzold-1);
01296                 float tablez6 = kb.i0win_tab(delz-inzold-2);
01297                 float tablez7 = kb.i0win_tab(delz-inzold-3);
01298 
01299                 int x1, x2, x3, x4, x5, x6, x7, y1, y2, y3, y4, y5, y6, y7, z1, z2, z3, z4, z5, z6, z7;
01300 
01301                 if ( inxold <= kbc || inxold >=nx-kbc-2 || inyold <= kbc || inyold >=ny-kbc-2 || inzold <= kbc || inzold >= nz-kbc-2 )  {
01302                         x1 = (inxold-3+nx)%nx;
01303                         x2 = (inxold-2+nx)%nx;
01304                         x3 = (inxold-1+nx)%nx;
01305                         x4 = (inxold  +nx)%nx;
01306                         x5 = (inxold+1+nx)%nx;
01307                         x6 = (inxold+2+nx)%nx;
01308                         x7 = (inxold+3+nx)%nx;
01309 
01310                         y1 = ((inyold-3+ny)%ny)*nx;
01311                         y2 = ((inyold-2+ny)%ny)*nx;
01312                         y3 = ((inyold-1+ny)%ny)*nx;
01313                         y4 = ((inyold  +ny)%ny)*nx;
01314                         y5 = ((inyold+1+ny)%ny)*nx;
01315                         y6 = ((inyold+2+ny)%ny)*nx;
01316                         y7 = ((inyold+3+ny)%ny)*nx;
01317 
01318                         z1 = ((inzold-3+nz)%nz)*nx*ny;
01319                         z2 = ((inzold-2+nz)%nz)*nx*ny;
01320                         z3 = ((inzold-1+nz)%nz)*nx*ny;
01321                         z4 = ((inzold  +nz)%nz)*nx*ny;
01322                         z5 = ((inzold+1+nz)%nz)*nx*ny;
01323                         z6 = ((inzold+2+nz)%nz)*nx*ny;
01324                         z7 = ((inzold+3+nz)%nz)*nx*ny;
01325                 } else {
01326                         x1 = inxold-3;
01327                         x2 = inxold-2;
01328                         x3 = inxold-1;
01329                         x4 = inxold;
01330                         x5 = inxold+1;
01331                         x6 = inxold+2;
01332                         x7 = inxold+3;
01333 
01334                         y1 = (inyold-3)*nx;
01335                         y2 = (inyold-2)*nx;
01336                         y3 = (inyold-1)*nx;
01337                         y4 = inyold*nx;
01338                         y5 = (inyold+1)*nx;
01339                         y6 = (inyold+2)*nx;
01340                         y7 = (inyold+3)*nx;
01341 
01342                         z1 = (inzold-3)*nx*ny;
01343                         z2 = (inzold-2)*nx*ny;
01344                         z3 = (inzold-1)*nx*ny;
01345                         z4 = inzold*nx*ny;
01346                         z5 = (inzold+1)*nx*ny;
01347                         z6 = (inzold+2)*nx*ny;
01348                         z7 = (inzold+3)*nx*ny;
01349                 }
01350 
01351                 pixel  = ( ( data[x1+y1+z1]*tablex1 + data[x2+y1+z1]*tablex2 + data[x3+y1+z1]*tablex3 +
01352                              data[x4+y1+z1]*tablex4 + data[x5+y1+z1]*tablex5 + data[x6+y1+z1]*tablex6 +
01353                              data[x7+y1+z1]*tablex7 ) * tabley1 +
01354                            ( data[x1+y2+z1]*tablex1 + data[x2+y2+z1]*tablex2 + data[x3+y2+z1]*tablex3 +
01355                              data[x4+y2+z1]*tablex4 + data[x5+y2+z1]*tablex5 + data[x6+y2+z1]*tablex6 +
01356                              data[x7+y2+z1]*tablex7 ) * tabley2 +
01357                            ( data[x1+y3+z1]*tablex1 + data[x2+y3+z1]*tablex2 + data[x3+y3+z1]*tablex3 +
01358                              data[x4+y3+z1]*tablex4 + data[x5+y3+z1]*tablex5 + data[x6+y3+z1]*tablex6 +
01359                              data[x7+y3+z1]*tablex7 ) * tabley3 +
01360                            ( data[x1+y4+z1]*tablex1 + data[x2+y4+z1]*tablex2 + data[x3+y4+z1]*tablex3 +
01361                              data[x4+y4+z1]*tablex4 + data[x5+y4+z1]*tablex5 + data[x6+y4+z1]*tablex6 +
01362                              data[x7+y4+z1]*tablex7 ) * tabley4 +
01363                            ( data[x1+y5+z1]*tablex1 + data[x2+y5+z1]*tablex2 + data[x3+y5+z1]*tablex3 +
01364                              data[x4+y5+z1]*tablex4 + data[x5+y5+z1]*tablex5 + data[x6+y5+z1]*tablex6 +
01365                              data[x7+y5+z1]*tablex7 ) * tabley5 +
01366                            ( data[x1+y6+z1]*tablex1 + data[x2+y6+z1]*tablex2 + data[x3+y6+z1]*tablex3 +
01367                              data[x4+y6+z1]*tablex4 + data[x5+y6+z1]*tablex5 + data[x6+y6+z1]*tablex6 +
01368                              data[x7+y6+z1]*tablex7 ) * tabley6 +
01369                            ( data[x1+y7+z1]*tablex1 + data[x2+y7+z1]*tablex2 + data[x3+y7+z1]*tablex3 +
01370                              data[x4+y7+z1]*tablex4 + data[x5+y7+z1]*tablex5 + data[x6+y7+z1]*tablex6 +
01371                              data[x7+y7+z1]*tablex7 ) * tabley7 ) *tablez1 +
01372                          ( ( data[x1+y1+z2]*tablex1 + data[x2+y1+z2]*tablex2 + data[x3+y1+z2]*tablex3 +
01373                              data[x4+y1+z2]*tablex4 + data[x5+y1+z2]*tablex5 + data[x6+y1+z2]*tablex6 +
01374                              data[x7+y1+z2]*tablex7 ) * tabley1 +
01375                            ( data[x1+y2+z2]*tablex1 + data[x2+y2+z2]*tablex2 + data[x3+y2+z2]*tablex3 +
01376                              data[x4+y2+z2]*tablex4 + data[x5+y2+z2]*tablex5 + data[x6+y2+z2]*tablex6 +
01377                              data[x7+y2+z2]*tablex7 ) * tabley2 +
01378                            ( data[x1+y3+z2]*tablex1 + data[x2+y3+z2]*tablex2 + data[x3+y3+z2]*tablex3 +
01379                              data[x4+y3+z2]*tablex4 + data[x5+y3+z2]*tablex5 + data[x6+y3+z2]*tablex6 +
01380                              data[x7+y3+z2]*tablex7 ) * tabley3 +
01381                            ( data[x1+y4+z2]*tablex1 + data[x2+y4+z2]*tablex2 + data[x3+y4+z2]*tablex3 +
01382                              data[x4+y4+z2]*tablex4 + data[x5+y4+z2]*tablex5 + data[x6+y4+z2]*tablex6 +
01383                              data[x7+y4+z2]*tablex7 ) * tabley4 +
01384                            ( data[x1+y5+z2]*tablex1 + data[x2+y5+z2]*tablex2 + data[x3+y5+z2]*tablex3 +
01385                              data[x4+y5+z2]*tablex4 + data[x5+y5+z2]*tablex5 + data[x6+y5+z2]*tablex6 +
01386                              data[x7+y5+z2]*tablex7 ) * tabley5 +
01387                            ( data[x1+y6+z2]*tablex1 + data[x2+y6+z2]*tablex2 + data[x3+y6+z2]*tablex3 +
01388                              data[x4+y6+z2]*tablex4 + data[x5+y6+z2]*tablex5 + data[x6+y6+z2]*tablex6 +
01389                              data[x7+y6+z2]*tablex7 ) * tabley6 +
01390                            ( data[x1+y7+z2]*tablex1 + data[x2+y7+z2]*tablex2 + data[x3+y7+z2]*tablex3 +
01391                              data[x4+y7+z2]*tablex4 + data[x5+y7+z2]*tablex5 + data[x6+y7+z2]*tablex6 +
01392                              data[x7+y7+z2]*tablex7 ) * tabley7 ) *tablez2 +
01393                          ( ( data[x1+y1+z3]*tablex1 + data[x2+y1+z3]*tablex2 + data[x3+y1+z3]*tablex3 +
01394                              data[x4+y1+z3]*tablex4 + data[x5+y1+z3]*tablex5 + data[x6+y1+z3]*tablex6 +
01395                              data[x7+y1+z3]*tablex7 ) * tabley1 +
01396                            ( data[x1+y2+z3]*tablex1 + data[x2+y2+z3]*tablex2 + data[x3+y2+z3]*tablex3 +
01397                              data[x4+y2+z3]*tablex4 + data[x5+y2+z3]*tablex5 + data[x6+y2+z3]*tablex6 +
01398                              data[x7+y2+z3]*tablex7 ) * tabley2 +
01399                            ( data[x1+y3+z3]*tablex1 + data[x2+y3+z3]*tablex2 + data[x3+y3+z3]*tablex3 +
01400                              data[x4+y3+z3]*tablex4 + data[x5+y3+z3]*tablex5 + data[x6+y3+z3]*tablex6 +
01401                              data[x7+y3+z3]*tablex7 ) * tabley3 +
01402                            ( data[x1+y4+z3]*tablex1 + data[x2+y4+z3]*tablex2 + data[x3+y4+z3]*tablex3 +
01403                              data[x4+y4+z3]*tablex4 + data[x5+y4+z3]*tablex5 + data[x6+y4+z3]*tablex6 +
01404                              data[x7+y4+z3]*tablex7 ) * tabley4 +
01405                            ( data[x1+y5+z3]*tablex1 + data[x2+y5+z3]*tablex2 + data[x3+y5+z3]*tablex3 +
01406                              data[x4+y5+z3]*tablex4 + data[x5+y5+z3]*tablex5 + data[x6+y5+z3]*tablex6 +
01407                              data[x7+y5+z3]*tablex7 ) * tabley5 +
01408                            ( data[x1+y6+z3]*tablex1 + data[x2+y6+z3]*tablex2 + data[x3+y6+z3]*tablex3 +
01409                              data[x4+y6+z3]*tablex4 + data[x5+y6+z3]*tablex5 + data[x6+y6+z3]*tablex6 +
01410                              data[x7+y6+z3]*tablex7 ) * tabley6 +
01411                            ( data[x1+y7+z3]*tablex1 + data[x2+y7+z3]*tablex2 + data[x3+y7+z3]*tablex3 +
01412                              data[x4+y7+z3]*tablex4 + data[x5+y7+z3]*tablex5 + data[x6+y7+z3]*tablex6 +
01413                              data[x7+y7+z3]*tablex7 ) * tabley7 ) *tablez3 +
01414                          ( ( data[x1+y1+z4]*tablex1 + data[x2+y1+z4]*tablex2 + data[x3+y1+z4]*tablex3 +
01415                              data[x4+y1+z4]*tablex4 + data[x5+y1+z4]*tablex5 + data[x6+y1+z4]*tablex6 +
01416                              data[x7+y1+z4]*tablex7 ) * tabley1 +
01417                            ( data[x1+y2+z4]*tablex1 + data[x2+y2+z4]*tablex2 + data[x3+y2+z4]*tablex3 +
01418                              data[x4+y2+z4]*tablex4 + data[x5+y2+z4]*tablex5 + data[x6+y2+z4]*tablex6 +
01419                              data[x7+y2+z4]*tablex7 ) * tabley2 +
01420                            ( data[x1+y3+z4]*tablex1 + data[x2+y3+z4]*tablex2 + data[x3+y3+z4]*tablex3 +
01421                              data[x4+y3+z4]*tablex4 + data[x5+y3+z4]*tablex5 + data[x6+y3+z4]*tablex6 +
01422                              data[x7+y3+z4]*tablex7 ) * tabley3 +
01423                            ( data[x1+y4+z4]*tablex1 + data[x2+y4+z4]*tablex2 + data[x3+y4+z4]*tablex3 +
01424                              data[x4+y4+z4]*tablex4 + data[x5+y4+z4]*tablex5 + data[x6+y4+z4]*tablex6 +
01425                              data[x7+y4+z4]*tablex7 ) * tabley4 +
01426                            ( data[x1+y5+z4]*tablex1 + data[x2+y5+z4]*tablex2 + data[x3+y5+z4]*tablex3 +
01427                              data[x4+y5+z4]*tablex4 + data[x5+y5+z4]*tablex5 + data[x6+y5+z4]*tablex6 +
01428                              data[x7+y5+z4]*tablex7 ) * tabley5 +
01429                            ( data[x1+y6+z4]*tablex1 + data[x2+y6+z4]*tablex2 + data[x3+y6+z4]*tablex3 +
01430                              data[x4+y6+z4]*tablex4 + data[x5+y6+z4]*tablex5 + data[x6+y6+z4]*tablex6 +
01431                              data[x7+y6+z4]*tablex7 ) * tabley6 +
01432                            ( data[x1+y7+z4]*tablex1 + data[x2+y7+z4]*tablex2 + data[x3+y7+z4]*tablex3 +
01433                              data[x4+y7+z4]*tablex4 + data[x5+y7+z4]*tablex5 + data[x6+y7+z4]*tablex6 +
01434                              data[x7+y7+z4]*tablex7 ) * tabley7 ) *tablez4 +
01435                          ( ( data[x1+y1+z5]*tablex1 + data[x2+y1+z5]*tablex2 + data[x3+y1+z5]*tablex3 +
01436                              data[x4+y1+z5]*tablex4 + data[x5+y1+z5]*tablex5 + data[x6+y1+z5]*tablex6 +
01437                              data[x7+y1+z5]*tablex7 ) * tabley1 +
01438                            ( data[x1+y2+z5]*tablex1 + data[x2+y2+z5]*tablex2 + data[x3+y2+z5]*tablex3 +
01439                              data[x4+y2+z5]*tablex4 + data[x5+y2+z5]*tablex5 + data[x6+y2+z5]*tablex6 +
01440                              data[x7+y2+z5]*tablex7 ) * tabley2 +
01441                            ( data[x1+y3+z5]*tablex1 + data[x2+y3+z5]*tablex2 + data[x3+y3+z5]*tablex3 +
01442                              data[x4+y3+z5]*tablex4 + data[x5+y3+z5]*tablex5 + data[x6+y3+z5]*tablex6 +
01443                              data[x7+y3+z5]*tablex7 ) * tabley3 +
01444                            ( data[x1+y4+z5]*tablex1 + data[x2+y4+z5]*tablex2 + data[x3+y4+z5]*tablex3 +
01445                              data[x4+y4+z5]*tablex4 + data[x5+y4+z5]*tablex5 + data[x6+y4+z5]*tablex6 +
01446                              data[x7+y4+z5]*tablex7 ) * tabley4 +
01447                            ( data[x1+y5+z5]*tablex1 + data[x2+y5+z5]*tablex2 + data[x3+y5+z5]*tablex3 +
01448                              data[x4+y5+z5]*tablex4 + data[x5+y5+z5]*tablex5 + data[x6+y5+z5]*tablex6 +
01449                              data[x7+y5+z5]*tablex7 ) * tabley5 +
01450                            ( data[x1+y6+z5]*tablex1 + data[x2+y6+z5]*tablex2 + data[x3+y6+z5]*tablex3 +
01451                              data[x4+y6+z5]*tablex4 + data[x5+y6+z5]*tablex5 + data[x6+y6+z5]*tablex6 +
01452                              data[x7+y6+z5]*tablex7 ) * tabley6 +
01453                            ( data[x1+y7+z5]*tablex1 + data[x2+y7+z5]*tablex2 + data[x3+y7+z5]*tablex3 +
01454                              data[x4+y7+z5]*tablex4 + data[x5+y7+z5]*tablex5 + data[x6+y7+z5]*tablex6 +
01455                              data[x7+y7+z5]*tablex7 ) * tabley7 ) *tablez5 +
01456                          ( ( data[x1+y1+z6]*tablex1 + data[x2+y1+z6]*tablex2 + data[x3+y1+z6]*tablex3 +
01457                              data[x4+y1+z6]*tablex4 + data[x5+y1+z6]*tablex5 + data[x6+y1+z6]*tablex6 +
01458                              data[x7+y1+z6]*tablex7 ) * tabley1 +
01459                            ( data[x1+y2+z6]*tablex1 + data[x2+y2+z6]*tablex2 + data[x3+y2+z6]*tablex3 +
01460                              data[x4+y2+z6]*tablex4 + data[x5+y2+z6]*tablex5 + data[x6+y2+z6]*tablex6 +
01461                              data[x7+y2+z6]*tablex7 ) * tabley2 +
01462                            ( data[x1+y3+z6]*tablex1 + data[x2+y3+z6]*tablex2 + data[x3+y3+z6]*tablex3 +
01463                              data[x4+y3+z6]*tablex4 + data[x5+y3+z6]*tablex5 + data[x6+y3+z6]*tablex6 +
01464                              data[x7+y3+z6]*tablex7 ) * tabley3 +
01465                            ( data[x1+y4+z6]*tablex1 + data[x2+y4+z6]*tablex2 + data[x3+y4+z6]*tablex3 +
01466                              data[x4+y4+z6]*tablex4 + data[x5+y4+z6]*tablex5 + data[x6+y4+z6]*tablex6 +
01467                              data[x7+y4+z6]*tablex7 ) * tabley4 +
01468                            ( data[x1+y5+z6]*tablex1 + data[x2+y5+z6]*tablex2 + data[x3+y5+z6]*tablex3 +
01469                              data[x4+y5+z6]*tablex4 + data[x5+y5+z6]*tablex5 + data[x6+y5+z6]*tablex6 +
01470                              data[x7+y5+z6]*tablex7 ) * tabley5 +
01471                            ( data[x1+y6+z6]*tablex1 + data[x2+y6+z6]*tablex2 + data[x3+y6+z6]*tablex3 +
01472                              data[x4+y6+z6]*tablex4 + data[x5+y6+z6]*tablex5 + data[x6+y6+z6]*tablex6 +
01473                              data[x7+y6+z6]*tablex7 ) * tabley6 +
01474                            ( data[x1+y7+z6]*tablex1 + data[x2+y7+z6]*tablex2 + data[x3+y7+z6]*tablex3 +
01475                              data[x4+y7+z6]*tablex4 + data[x5+y7+z6]*tablex5 + data[x6+y7+z6]*tablex6 +
01476                              data[x7+y7+z6]*tablex7 ) * tabley7 ) *tablez6 +
01477                          ( ( data[x1+y1+z7]*tablex1 + data[x2+y1+z7]*tablex2 + data[x3+y1+z7]*tablex3 +
01478                              data[x4+y1+z7]*tablex4 + data[x5+y1+z7]*tablex5 + data[x6+y1+z7]*tablex6 +
01479                              data[x7+y1+z7]*tablex7 ) * tabley1 +
01480                            ( data[x1+y2+z7]*tablex1 + data[x2+y2+z7]*tablex2 + data[x3+y2+z7]*tablex3 +
01481                              data[x4+y2+z7]*tablex4 + data[x5+y2+z7]*tablex5 + data[x6+y2+z7]*tablex6 +
01482                              data[x7+y2+z7]*tablex7 ) * tabley2 +
01483                            ( data[x1+y3+z7]*tablex1 + data[x2+y3+z7]*tablex2 + data[x3+y3+z7]*tablex3 +
01484                              data[x4+y3+z7]*tablex4 + data[x5+y3+z7]*tablex5 + data[x6+y3+z7]*tablex6 +
01485                              data[x7+y3+z7]*tablex7 ) * tabley3 +
01486                            ( data[x1+y4+z7]*tablex1 + data[x2+y4+z7]*tablex2 + data[x3+y4+z7]*tablex3 +
01487                              data[x4+y4+z7]*tablex4 + data[x5+y4+z7]*tablex5 + data[x6+y4+z7]*tablex6 +
01488                              data[x7+y4+z7]*tablex7 ) * tabley4 +
01489                            ( data[x1+y5+z7]*tablex1 + data[x2+y5+z7]*tablex2 + data[x3+y5+z7]*tablex3 +
01490                              data[x4+y5+z7]*tablex4 + data[x5+y5+z7]*tablex5 + data[x6+y5+z7]*tablex6 +
01491                              data[x7+y5+z7]*tablex7 ) * tabley5 +
01492                            ( data[x1+y6+z7]*tablex1 + data[x2+y6+z7]*tablex2 + data[x3+y6+z7]*tablex3 +
01493                              data[x4+y6+z7]*tablex4 + data[x5+y6+z7]*tablex5 + data[x6+y6+z7]*tablex6 +
01494                              data[x7+y6+z7]*tablex7 ) * tabley6 +
01495                            ( data[x1+y7+z7]*tablex1 + data[x2+y7+z7]*tablex2 + data[x3+y7+z7]*tablex3 +
01496                              data[x4+y7+z7]*tablex4 + data[x5+y7+z7]*tablex5 + data[x6+y7+z7]*tablex6 +
01497                              data[x7+y7+z7]*tablex7 ) * tabley7 ) *tablez7;
01498 
01499                 w = (tablex1+tablex2+tablex3+tablex4+tablex5+tablex6+tablex7) *
01500                     (tabley1+tabley2+tabley3+tabley4+tabley5+tabley6+tabley7) *
01501                     (tablez1+tablez2+tablez3+tablez4+tablez5+tablez6+tablez7);
01502         }
01503         return pixel/w;
01504 }
01505 
01506 /*
01507 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01508 
01509         int nxreal = nx - 2;
01510         if (nxreal != ny)
01511                 throw ImageDimensionException("extractpoint requires ny == nx");
01512         int nhalf = nxreal/2;
01513         int kbsize = kb.get_window_size();
01514         int kbmin = -kbsize/2;
01515         int kbmax = -kbmin;
01516         bool flip = (nuxnew < 0.f);
01517         if (flip) {
01518                 nuxnew *= -1;
01519                 nuynew *= -1;
01520         }
01521         // put (xnew,ynew) on a grid.  The indices will be wrong for
01522         // the Fourier elements in the image, but the grid sizing will
01523         // be correct.
01524         int ixn = int(Util::round(nuxnew));
01525         int iyn = int(Util::round(nuynew));
01526         // set up some temporary weighting arrays
01527         float* wy0 = new float[kbmax - kbmin + 1];
01528         float* wy = wy0 - kbmin; // wy[kbmin:kbmax]
01529         float* wx0 = new float[kbmax - kbmin + 1];
01530         float* wx = wx0 - kbmin;
01531         for (int i = kbmin; i <= kbmax; i++) {
01532                         int iyp = iyn + i;
01533                         wy[i] = kb.i0win_tab(nuynew - iyp);
01534                         int ixp = ixn + i;
01535                         wx[i] = kb.i0win_tab(nuxnew - ixp);
01536         }
01537         // restrict loops to non-zero elements
01538         int iymin = 0;
01539         for (int iy = kbmin; iy <= -1; iy++) {
01540                 if (wy[iy] != 0.f) {
01541                         iymin = iy;
01542                         break;
01543                 }
01544         }
01545         int iymax = 0;
01546         for (int iy = kbmax; iy >= 1; iy--) {
01547                 if (wy[iy] != 0.f) {
01548                         iymax = iy;
01549                         break;
01550                 }
01551         }
01552         int ixmin = 0;
01553         for (int ix = kbmin; ix <= -1; ix++) {
01554                 if (wx[ix] != 0.f) {
01555                         ixmin = ix;
01556                         break;
01557                 }
01558         }
01559         int ixmax = 0;
01560         for (int ix = kbmax; ix >= 1; ix--) {
01561                 if (wx[ix] != 0.f) {
01562                         ixmax = ix;
01563                         break;
01564                 }
01565         }
01566         float wsum = 0.0f;
01567         for (int iy = iymin; iy <= iymax; iy++)
01568                 for (int ix = ixmin; ix <= ixmax; ix++)
01569                         wsum += wx[ix]*wy[iy];
01570 
01571         complex<float> result(0.f,0.f);
01572         if ((ixn >= -kbmin) && (ixn <= nhalf-1-kbmax) && (iyn >= -nhalf-kbmin) && (iyn <= nhalf-1-kbmax)) {
01573                 // (xin,yin) not within window border from the edge
01574                 for (int iy = iymin; iy <= iymax; iy++) {
01575                         int iyp = iyn + iy;
01576                         for (int ix = ixmin; ix <= ixmax; ix++) {
01577                                 int ixp = ixn + ix;
01578                                 float w = wx[ix]*wy[iy];
01579                                 complex<float> val = fimage->cmplx(ixp,iyp);
01580                                 result += val*w;
01581                         }
01582                 }
01583         } else {
01584                 // points that "stick out"
01585                 for (int iy = iymin; iy <= iymax; iy++) {
01586                         int iyp = iyn + iy;
01587                         for (int ix = ixmin; ix <= ixmax; ix++) {
01588                                 int ixp = ixn + ix;
01589                                 bool mirror = false;
01590                                 int ixt= ixp, iyt= iyp;
01591                                 if (ixt < 0) {
01592                                         ixt = -ixt;
01593                                         iyt = -iyt;
01594                                         mirror = !mirror;
01595                                 }
01596                                 if (ixt > nhalf) {
01597                                         ixt = nxreal - ixt;
01598                                         iyt = -iyt;
01599                                         mirror = !mirror;
01600                                 }
01601                                 if (iyt > nhalf-1)  iyt -= nxreal;
01602                                 if (iyt < -nhalf)   iyt += nxreal;
01603                                 float w = wx[ix]*wy[iy];
01604                                 complex<float> val = fimage->cmplx(ixt,iyt);
01605                                 if (mirror)  result += conj(val)*w;
01606                                 else         result += val*w;
01607                         }
01608                 }
01609         }
01610         if (flip)  result = conj(result)/wsum;
01611         else result /= wsum;
01612         delete [] wx0;
01613         delete [] wy0;
01614         return result;
01615 }*/
01616 
01617 /*
01618 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01619 
01620         int nxreal = nx - 2;
01621         if (nxreal != ny)
01622                 throw ImageDimensionException("extractpoint requires ny == nx");
01623         int nhalf = nxreal/2;
01624         bool flip = false;
01625         if (nuxnew < 0.f) {
01626                 nuxnew *= -1;
01627                 nuynew *= -1;
01628                 flip = true;
01629         }
01630         if (nuynew >= nhalf-0.5)  {
01631                 nuynew -= nxreal;
01632         } else if (nuynew < -nhalf-0.5) {
01633                 nuynew += nxreal;
01634         }
01635 
01636         // put (xnew,ynew) on a grid.  The indices will be wrong for
01637         // the Fourier elements in the image, but the grid sizing will
01638         // be correct.
01639         int ixn = int(Util::round(nuxnew));
01640         int iyn = int(Util::round(nuynew));
01641 
01642         // set up some temporary weighting arrays
01643         static float wy[7];
01644         static float wx[7];
01645 
01646         float iynn = nuynew - iyn;
01647         wy[0] = kb.i0win_tab(iynn+3);
01648         wy[1] = kb.i0win_tab(iynn+2);
01649         wy[2] = kb.i0win_tab(iynn+1);
01650         wy[3] = kb.i0win_tab(iynn);
01651         wy[4] = kb.i0win_tab(iynn-1);
01652         wy[5] = kb.i0win_tab(iynn-2);
01653         wy[6] = kb.i0win_tab(iynn-3);
01654 
01655         float ixnn = nuxnew - ixn;
01656         wx[0] = kb.i0win_tab(ixnn+3);
01657         wx[1] = kb.i0win_tab(ixnn+2);
01658         wx[2] = kb.i0win_tab(ixnn+1);
01659         wx[3] = kb.i0win_tab(ixnn);
01660         wx[4] = kb.i0win_tab(ixnn-1);
01661         wx[5] = kb.i0win_tab(ixnn-2);
01662         wx[6] = kb.i0win_tab(ixnn-3);
01663 
01664         float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01665 
01666         complex<float> result(0.f,0.f);
01667         for (int iy = 0; iy < 7; iy++) {
01668                 int iyp = iyn + iy - 3 ;
01669                 for (int ix = 0; ix < 7; ix++) {
01670                         int ixp = ixn + ix - 3;
01671                         float w = wx[ix]*wy[iy];
01672                         complex<float> val = fimage->cmplx(ixp,iyp);
01673                         result += val*w;
01674                 }
01675         }
01676 
01677         if (flip)  result = conj(result)/wsum;
01678         else result /= wsum;
01679 
01680         return result;
01681 }*/
01682 
01683 
01684 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01685 
01686         int nxreal = nx - 2;
01687         if (nxreal != ny)
01688                 throw ImageDimensionException("extractpoint requires ny == nx");
01689         int nhalf = nxreal/2;
01690         bool flip = (nuxnew < 0.f);
01691         if (flip) {
01692                 nuxnew *= -1;
01693                 nuynew *= -1;
01694         }
01695         if (nuynew >= nhalf-0.5)  {
01696                 nuynew -= nxreal;
01697         } else if (nuynew < -nhalf-0.5) {
01698                 nuynew += nxreal;
01699         }
01700 
01701         // put (xnew,ynew) on a grid.  The indices will be wrong for
01702         // the Fourier elements in the image, but the grid sizing will
01703         // be correct.
01704         int ixn = int(Util::round(nuxnew));
01705         int iyn = int(Util::round(nuynew));
01706 
01707         // set up some temporary weighting arrays
01708         static float wy[7];
01709         static float wx[7];
01710 
01711         float iynn = nuynew - iyn;
01712         wy[0] = kb.i0win_tab(iynn+3);
01713         wy[1] = kb.i0win_tab(iynn+2);
01714         wy[2] = kb.i0win_tab(iynn+1);
01715         wy[3] = kb.i0win_tab(iynn);
01716         wy[4] = kb.i0win_tab(iynn-1);
01717         wy[5] = kb.i0win_tab(iynn-2);
01718         wy[6] = kb.i0win_tab(iynn-3);
01719 
01720         float ixnn = nuxnew - ixn;
01721         wx[0] = kb.i0win_tab(ixnn+3);
01722         wx[1] = kb.i0win_tab(ixnn+2);
01723         wx[2] = kb.i0win_tab(ixnn+1);
01724         wx[3] = kb.i0win_tab(ixnn);
01725         wx[4] = kb.i0win_tab(ixnn-1);
01726         wx[5] = kb.i0win_tab(ixnn-2);
01727         wx[6] = kb.i0win_tab(ixnn-3);
01728 
01729         float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01730 
01731         complex<float> result(0.f,0.f);
01732         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01733                 // (xin,yin) not within window border from the edge
01734                 for (int iy = 0; iy < 7; iy++) {
01735                         int iyp = iyn + iy - 3 ;
01736                         for (int ix = 0; ix < 7; ix++) {
01737                                 int ixp = ixn + ix - 3;
01738                                 float w = wx[ix]*wy[iy];
01739                                 complex<float> val = fimage->cmplx(ixp,iyp);
01740                                 result += val*w;
01741                         }
01742                 }
01743         } else {
01744                 // points that "stick out"
01745                 for (int iy = 0; iy < 7; iy++) {
01746                         int iyp = iyn + iy - 3;
01747                         for (int ix = 0; ix < 7; ix++) {
01748                                 int ixp = ixn + ix - 3;
01749                                 bool mirror = false;
01750                                 int ixt = ixp, iyt = iyp;
01751                                 if (ixt < 0) {
01752                                         ixt = -ixt;
01753                                         iyt = -iyt;
01754                                         mirror = !mirror;
01755                                 }
01756                                 if (ixt > nhalf) {
01757                                         ixt = nxreal - ixt;
01758                                         iyt = -iyt;
01759                                         mirror = !mirror;
01760                                 }
01761                                 if (iyt > nhalf-1)  iyt -= nxreal;
01762                                 if (iyt < -nhalf)   iyt += nxreal;
01763                                 float w = wx[ix]*wy[iy];
01764                                 complex<float> val = fimage->cmplx(ixt,iyt);
01765                                 if (mirror)  result += conj(val)*w;
01766                                 else         result += val*w;
01767                         }
01768                 }
01769         }
01770         if (flip)  result = conj(result)/wsum;
01771         else result /= wsum;
01772         return result;
01773 }
01774 
01775 /*
01776 complex<float> Util::extractpoint2(int nx, int ny, float nuxnew, float nuynew, EMData *fimage, Util::KaiserBessel& kb) {
01777 
01778         int nxreal = nx - 2;
01779         if (nxreal != ny)
01780                 throw ImageDimensionException("extractpoint requires ny == nx");
01781         int nhalf = nxreal/2;
01782         bool flip = (nuxnew < 0.f);
01783         if (flip) {
01784                 nuxnew *= -1;
01785                 nuynew *= -1;
01786         }
01787         // put (xnew,ynew) on a grid.  The indices will be wrong for
01788         // the Fourier elements in the image, but the grid sizing will
01789         // be correct.
01790         int ixn = int(Util::round(nuxnew));
01791         int iyn = int(Util::round(nuynew));
01792         // set up some temporary weighting arrays
01793         static float wy[7];
01794         static float wx[7];
01795 
01796         float iynn = nuynew - iyn;
01797         wy[0] = kb.i0win_tab(iynn+3);
01798         wy[1] = kb.i0win_tab(iynn+2);
01799         wy[2] = kb.i0win_tab(iynn+1);
01800         wy[3] = kb.i0win_tab(iynn);
01801         wy[4] = kb.i0win_tab(iynn-1);
01802         wy[5] = kb.i0win_tab(iynn-2);
01803         wy[6] = kb.i0win_tab(iynn-3);
01804 
01805         float ixnn = nuxnew - ixn;
01806         wx[0] = kb.i0win_tab(ixnn+3);
01807         wx[1] = kb.i0win_tab(ixnn+2);
01808         wx[2] = kb.i0win_tab(ixnn+1);
01809         wx[3] = kb.i0win_tab(ixnn);
01810         wx[4] = kb.i0win_tab(ixnn-1);
01811         wx[5] = kb.i0win_tab(ixnn-2);
01812         wx[6] = kb.i0win_tab(ixnn-3);
01813 
01814         float wsum = (wx[0]+wx[1]+wx[2]+wx[3]+wx[4]+wx[5]+wx[6])*(wy[0]+wy[1]+wy[2]+wy[3]+wy[4]+wy[5]+wy[6]);
01815 
01816         complex<float> result(0.f,0.f);
01817 
01818         if ((ixn >= 3) && (ixn <= nhalf-3) && (iyn >= -nhalf+3) && (iyn <= nhalf-4)) {
01819                 // (xin,yin) not within window border from the edge
01820                 result = ( fimage->cmplx(ixn-3,iyn-3)*wx[0] +
01821                            fimage->cmplx(ixn-2,iyn-3)*wx[1] +
01822                            fimage->cmplx(ixn-1,iyn-3)*wx[2] +
01823                            fimage->cmplx(ixn+0,iyn-3)*wx[3] +
01824                            fimage->cmplx(ixn+1,iyn-3)*wx[4] +
01825                            fimage->cmplx(ixn+2,iyn-3)*wx[5] +
01826                            fimage->cmplx(ixn+3,iyn-3)*wx[6] )*wy[0] +
01827                            ( fimage->cmplx(ixn-3,iyn-2)*wx[0] +
01828                            fimage->cmplx(ixn-2,iyn-2)*wx[1] +
01829                            fimage->cmplx(ixn-1,iyn-2)*wx[2] +
01830                            fimage->cmplx(ixn+0,iyn-2)*wx[3] +
01831                            fimage->cmplx(ixn+1,iyn-2)*wx[4] +
01832                            fimage->cmplx(ixn+2,iyn-2)*wx[5] +
01833                            fimage->cmplx(ixn+3,iyn-2)*wx[6] )*wy[1] +
01834                            ( fimage->cmplx(ixn-3,iyn-1)*wx[0] +
01835                            fimage->cmplx(ixn-2,iyn-1)*wx[1] +
01836                            fimage->cmplx(ixn-1,iyn-1)*wx[2] +
01837                            fimage->cmplx(ixn+0,iyn-1)*wx[3] +
01838                            fimage->cmplx(ixn+1,iyn-1)*wx[4] +
01839                            fimage->cmplx(ixn+2,iyn-1)*wx[5] +
01840                            fimage->cmplx(ixn+3,iyn-1)*wx[6] )*wy[2] +
01841                            ( fimage->cmplx(ixn-3,iyn+0)*wx[0] +
01842                            fimage->cmplx(ixn-2,iyn+0)*wx[1] +
01843                            fimage->cmplx(ixn-1,iyn+0)*wx[2] +
01844                            fimage->cmplx(ixn+0,iyn+0)*wx[3] +
01845                            fimage->cmplx(ixn+1,iyn+0)*wx[4] +
01846                            fimage->cmplx(ixn+2,iyn+0)*wx[5] +
01847                            fimage->cmplx(ixn+3,iyn+0)*wx[6] )*wy[3] +
01848                            ( fimage->cmplx(ixn-3,iyn+1)*wx[0] +
01849                            fimage->cmplx(ixn-2,iyn+1)*wx[1] +
01850                            fimage->cmplx(ixn-1,iyn+1)*wx[2] +
01851                            fimage->cmplx(ixn+0,iyn+1)*wx[3] +
01852                            fimage->cmplx(ixn+1,iyn+1)*wx[4] +
01853                            fimage->cmplx(ixn+2,iyn+1)*wx[5] +
01854                            fimage->cmplx(ixn+3,iyn+1)*wx[6] )*wy[4] +
01855                            ( fimage->cmplx(ixn-3,iyn+2)*wx[0] +
01856                            fimage->cmplx(ixn-2,iyn+2)*wx[1] +
01857                            fimage->cmplx(ixn-1,iyn+2)*wx[2] +
01858                            fimage->cmplx(ixn+0,iyn+2)*wx[3] +
01859                            fimage->cmplx(ixn+1,iyn+2)*wx[4] +
01860                            fimage->cmplx(ixn+2,iyn+2)*wx[5] +
01861                            fimage->cmplx(ixn+3,iyn+2)*wx[6] )*wy[5] +
01862                            ( fimage->cmplx(ixn-3,iyn+3)*wx[0] +
01863                            fimage->cmplx(ixn-2,iyn+3)*wx[1] +
01864                            fimage->cmplx(ixn-1,iyn+3)*wx[2] +
01865                            fimage->cmplx(ixn+0,iyn+3)*wx[3] +
01866                            fimage->cmplx(ixn+1,iyn+3)*wx[4] +
01867                            fimage->cmplx(ixn+2,iyn+3)*wx[5] +
01868                            fimage->cmplx(ixn+3,iyn+3)*wx[6] )*wy[6];
01869 
01870         } else {
01871                 // points that "stick out"
01872                 for (int iy = 0; iy < 7; iy++) {
01873                         int iyp = iyn + iy - 3;
01874                         for (int ix = 0; ix < 7; ix++) {
01875                                 int ixp = ixn + ix - 3;
01876                                 bool mirror = false;
01877                                 int ixt= ixp, iyt= iyp;
01878                                 if (ixt < 0) {
01879                                         ixt = -ixt;
01880                                         iyt = -iyt;
01881                                         mirror = !mirror;
01882                                 }
01883                                 if (ixt > nhalf) {
01884                                         ixt = nxreal - ixt;
01885                                         iyt = -iyt;
01886                                         mirror = !mirror;
01887                                 }
01888                                 if (iyt > nhalf-1)  iyt -= nxreal;
01889                                 if (iyt < -nhalf)   iyt += nxreal;
01890                                 float w = wx[ix]*wy[iy];
01891                                 complex<float> val = fimage->cmplx(ixt,iyt);
01892                                 if (mirror)  result += conj(val)*w;
01893                                 else         result += val*w;
01894                         }
01895                 }
01896         }
01897         if (flip)  result = conj(result)/wsum;
01898         else result /= wsum;
01899         return result;
01900 }*/
01901 
01902 
01903 float Util::triquad(float R, float S, float T, float* fdata)
01904 {
01905 
01906     const float C2 = 0.5f;    //1.0 / 2.0;
01907     const float C4 = 0.25f;   //1.0 / 4.0;
01908     const float C8 = 0.125f;  //1.0 / 8.0;
01909 
01910     float  RS   = R * S;
01911     float  ST   = S * T;
01912     float  RT   = R * T;
01913     float  RST  = R * ST;
01914 
01915     float  RSQ  = 1-R*R;
01916     float  SSQ  = 1-S*S;
01917     float  TSQ  = 1-T*T;
01918 
01919     float  RM1  = (1-R);
01920     float  SM1  = (1-S);
01921     float  TM1  = (1-T);
01922 
01923     float  RP1  = (1+R);
01924     float  SP1  = (1+S);
01925     float  TP1  = (1+T);
01926 
01927     float triquad =
01928     (-C8) * RST * RM1  * SM1  * TM1 * fdata[0] +
01929         ( C4) * ST  * RSQ  * SM1  * TM1 * fdata[1] +
01930         ( C8) * RST * RP1  * SM1  * TM1 * fdata[2] +
01931         ( C4) * RT  * RM1  * SSQ  * TM1 * fdata[3] +
01932         (-C2) * T   * RSQ  * SSQ  * TM1 * fdata[4] +
01933         (-C4) * RT  * RP1  * SSQ  * TM1 * fdata[5] +
01934         ( C8) * RST * RM1  * SP1  * TM1 * fdata[6] +
01935         (-C4) * ST  * RSQ  * SP1  * TM1 * fdata[7] +
01936         (-C8) * RST * RP1  * SP1  * TM1 * fdata[8] +
01937 //
01938         ( C4) * RS  * RM1  * SM1  * TSQ * fdata[9]  +
01939         (-C2) * S   * RSQ  * SM1  * TSQ * fdata[10] +
01940         (-C4) * RS  * RP1  * SM1  * TSQ * fdata[11] +
01941         (-C2) * R   * RM1  * SSQ  * TSQ * fdata[12] +
01942                       RSQ  * SSQ  * TSQ * fdata[13] +
01943         ( C2) * R   * RP1  * SSQ  * TSQ * fdata[14] +
01944         (-C4) * RS  * RM1  * SP1  * TSQ * fdata[15] +
01945         ( C2) * S   * RSQ  * SP1  * TSQ * fdata[16] +
01946         ( C4) * RS  * RP1  * SP1  * TSQ * fdata[17] +
01947  //
01948         ( C8) * RST * RM1  * SM1  * TP1 * fdata[18] +
01949         (-C4) * ST  * RSQ  * SM1  * TP1 * fdata[19] +
01950         (-C8) * RST * RP1  * SM1  * TP1 * fdata[20] +
01951         (-C4) * RT  * RM1  * SSQ  * TP1 * fdata[21] +
01952         ( C2) * T   * RSQ  * SSQ  * TP1 * fdata[22] +
01953         ( C4) * RT  * RP1  * SSQ  * TP1 * fdata[23] +
01954         (-C8) * RST * RM1  * SP1  * TP1 * fdata[24] +
01955         ( C4) * ST  * RSQ  * SP1  * TP1 * fdata[25] +
01956         ( C8) * RST * RP1  * SP1  * TP1 * fdata[26]   ;
01957      return triquad;
01958 }
01959 
01960 Util::sincBlackman::sincBlackman(int M_, float fc_, int ntable_)
01961                 : M(M_), fc(fc_), ntable(ntable_) {
01962         // Sinc-Blackman kernel
01963         build_sBtable();
01964 }
01965 
01966 void Util::sincBlackman::build_sBtable() {
01967         sBtable.resize(ntable+1);
01968         int ltab = int(round(float(ntable)/1.25f));
01969         int M2 = M/2;
01970         fltb = float(ltab)/M2;
01971         for (int i=ltab+1; i <= ntable; i++) sBtable[i] = 0.0f;
01972         float x = 1.0e-7f;
01973         sBtable[0] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01974         for (int i=1; i <= ltab; i++) {
01975                 x = float(i)/fltb;
01976                 sBtable[i] = (float)(sin(twopi*fc*x)/x*(0.52-0.5*cos(twopi*(x-M2)/M)+0.08*cos(2*twopi*(x-M2)/M)));
01977                 //cout << "  "<<x<<"  "<<sBtable[i] <<endl;
01978         }
01979 }
01980 
01981 Util::KaiserBessel::KaiserBessel(float alpha_, int K_, float r_, float v_,
01982                                          int N_, float vtable_, int ntable_)
01983                 : alpha(alpha_), v(v_), r(r_), N(N_), K(K_), vtable(vtable_),
01984                   ntable(ntable_) {
01985         // Default values are alpha=1.25, K=6, r=0.5, v = K/2
01986         if (0.f == v) v = float(K)/2;
01987         if (0.f == vtable) vtable = v;
01988         alphar = alpha*r;
01989         fac = static_cast<float>(twopi)*alphar*v;
01990         vadjust = 1.0f*v;
01991         facadj = static_cast<float>(twopi)*alphar*vadjust;
01992         build_I0table();
01993 }
01994 
01995 float Util::KaiserBessel::i0win(float x) const {
01996         float val0 = float(gsl_sf_bessel_I0(facadj));
01997         float absx = fabs(x);
01998         if (absx > vadjust) return 0.f;
01999         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02000         float res = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02001         return res;
02002 }
02003 
02004 void Util::KaiserBessel::build_I0table() {
02005         i0table.resize(ntable+1); // i0table[0:ntable]
02006         int ltab = int(round(float(ntable)/1.25f));
02007         fltb = float(ltab)/(K/2);
02008         float val0 = static_cast<float>(gsl_sf_bessel_I0(facadj));
02009         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02010         for (int i=0; i <= ltab; i++) {
02011                 float s = float(i)/fltb/N;
02012                 if (s < vadjust) {
02013                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02014                         i0table[i] = static_cast<float>(gsl_sf_bessel_I0(facadj*rt))/val0;
02015                 } else {
02016                         i0table[i] = 0.f;
02017                 }
02018 //              cout << "  "<<s*N<<"  "<<i0table[i] <<endl;
02019         }
02020 }
02021 
02022 float Util::KaiserBessel::I0table_maxerror() {
02023         float maxdiff = 0.f;
02024         for (int i = 1; i <= ntable; i++) {
02025                 float diff = fabs(i0table[i] - i0table[i-1]);
02026                 if (diff > maxdiff) maxdiff = diff;
02027         }
02028         return maxdiff;
02029 }
02030 
02031 float Util::KaiserBessel::sinhwin(float x) const {
02032         float val0 = sinh(fac)/fac;
02033         float absx = fabs(x);
02034         if (0.0 == x) {
02035                 float res = 1.0f;
02036                 return res;
02037         } else if (absx == alphar) {
02038                 return 1.0f/val0;
02039         } else if (absx < alphar) {
02040                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02041                 float facrt = fac*rt;
02042                 float res = (sinh(facrt)/facrt)/val0;
02043                 return res;
02044         } else {
02045                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02046                 float facrt = fac*rt;
02047                 float res = (sin(facrt)/facrt)/val0;
02048                 return res;
02049         }
02050 }
02051 
02052 float Util::FakeKaiserBessel::i0win(float x) const {
02053         float val0 = sqrt(facadj)*float(gsl_sf_bessel_I1(facadj));
02054         float absx = fabs(x);
02055         if (absx > vadjust) return 0.f;
02056         float rt = sqrt(1.f - pow(absx/vadjust, 2));
02057         float res = sqrt(facadj*rt)*float(gsl_sf_bessel_I1(facadj*rt))/val0;
02058         return res;
02059 }
02060 
02061 void Util::FakeKaiserBessel::build_I0table() {
02062         i0table.resize(ntable+1); // i0table[0:ntable]
02063         int ltab = int(round(float(ntable)/1.1f));
02064         fltb = float(ltab)/(K/2);
02065         float val0 = sqrt(facadj)*static_cast<float>(gsl_sf_bessel_I1(facadj));
02066         for (int i=ltab+1; i <= ntable; i++) i0table[i] = 0.f;
02067         for (int i=0; i <= ltab; i++) {
02068                 float s = float(i)/fltb/N;
02069                 if (s < vadjust) {
02070                         float rt = sqrt(1.f - pow(s/vadjust, 2));
02071                         i0table[i] = sqrt(facadj*rt)*static_cast<float>(gsl_sf_bessel_I1(facadj*rt))/val0;
02072                 } else {
02073                         i0table[i] = 0.f;
02074                 }
02075         }
02076 }
02077 
02078 float Util::FakeKaiserBessel::sinhwin(float x) const {
02079         float val0 = sinh(fac)/fac;
02080         float absx = fabs(x);
02081         if (0.0 == x) {
02082                 float res = 1.0f;
02083                 return res;
02084         } else if (absx == alphar) {
02085                 return 1.0f/val0;
02086         } else if (absx < alphar) {
02087                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02088                 float facrt = fac*rt;
02089                 float res = (sinh(facrt)/facrt)/val0;
02090                 return res;
02091         } else {
02092                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02093                 float facrt = fac*rt;
02094                 float res = (sin(facrt)/facrt)/val0;
02095                 return res;
02096         }
02097 }
02098 
02099 #if 0 // 1-st order KB window
02100 float Util::FakeKaiserBessel::sinhwin(float x) const {
02101         //float val0 = sinh(fac)/fac;
02102         float prefix = 2*facadj*vadjust/float(gsl_sf_bessel_I1(facadj));
02103         float val0 = prefix*(cosh(facadj) - sinh(facadj)/facadj);
02104         float absx = fabs(x);
02105         if (0.0 == x) {
02106                 //float res = 1.0f;
02107                 float res = val0;
02108                 return res;
02109         } else if (absx == alphar) {
02110                 //return 1.0f/val0;
02111                 return prefix;
02112         } else if (absx < alphar) {
02113                 float rt = sqrt(1.0f - pow((x/alphar), 2));
02114                 //float facrt = fac*rt;
02115                 float facrt = facadj*rt;
02116                 //float res = (sinh(facrt)/facrt)/val0;
02117                 float res = prefix*(cosh(facrt) - sinh(facrt)/facrt);
02118                 return res;
02119         } else {
02120                 float rt = sqrt(pow((x/alphar),2) - 1.f);
02121                 //float facrt = fac*rt;
02122                 float facrt = facadj*rt;
02123                 //float res = (sin(facrt)/facrt)/val0;
02124                 float res = prefix*(sin(facrt)/facrt - cos(facrt));
02125                 return res;
02126         }
02127 }
02128 #endif // 0
02129 
02130 
02131 
02132 #define  circ(i)         circ[i-1]
02133 #define  numr(i,j)       numr[(j-1)*3 + i-1]
02134 #define  xim(i,j)        xim[(j-1)*nsam + i-1]
02135 
02136 EMData* Util::Polar2D(EMData* image, vector<int> numr, string cmode){
02137         int nsam = image->get_xsize();
02138         int nrow = image->get_ysize();
02139         int nring = numr.size()/3;
02140         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02141         EMData* out = new EMData();
02142         out->set_size(lcirc,1,1);
02143         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02144         float *xim  = image->get_data();
02145         float *circ = out->get_data();
02146 /*   alrq(image->get_data(), nsam, nrow, &numr[0], out->get_data(), lcirc, nring, cmode);
02147    return out;
02148 }
02149 void Util::alrq(float *xim,  int nsam , int nrow , int *numr,
02150           float *circ, int lcirc, int nring, char mode)
02151 {*/
02152 /*
02153 c
02154 c  purpose:
02155 c
02156 c  resmaple to polar coordinates
02157 c
02158 */
02159         //  dimension         xim(nsam,nrow),circ(lcirc)
02160         //  integer           numr(3,nring)
02161 
02162         double dfi, dpi;
02163         int    ns2, nr2, i, inr, l, nsim, kcirc, lt, j;
02164         float  yq, xold, yold, fi, x, y;
02165 
02166         ns2 = nsam/2+1;
02167         nr2 = nrow/2+1;
02168         dpi = 2.0*atan(1.0);
02169 
02170         for (i=1;i<=nring;i++) {
02171                 // radius of the ring
02172                 inr = numr(1,i);
02173                 yq  = static_cast<float>(inr);
02174                 l   = numr(3,i);
02175                 if (mode == 'h' || mode == 'H')  lt = l/2;
02176                 else                             lt = l/4;
02177 
02178                 nsim           = lt-1;
02179                 dfi            = dpi/(nsim+1);
02180                 kcirc          = numr(2,i);
02181                 xold           = 0.0f;
02182                 yold           = static_cast<float>(inr);
02183                 circ(kcirc)    = quadri(xold+(float)ns2,yold+(float)nr2,nsam,nrow,xim);
02184                 xold           = static_cast<float>(inr);
02185                 yold           = 0.0f;
02186                 circ(lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02187 
02188                 if (mode == 'f' || mode == 'F') {
02189                         xold              = 0.0f;
02190                         yold              = static_cast<float>(-inr);
02191                         circ(lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02192                         xold              = static_cast<float>(-inr);
02193                         yold              = 0.0f;
02194                         circ(lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02195                 }
02196 
02197                 for (j=1;j<=nsim;j++) {
02198                         fi               = static_cast<float>(dfi*j);
02199                         x                = sin(fi)*yq;
02200                         y                = cos(fi)*yq;
02201                         xold             = x;
02202                         yold             = y;
02203                         circ(j+kcirc)    = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02204                         xold             =  y;
02205                         yold             = -x;
02206                         circ(j+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02207 
02208                         if (mode == 'f' || mode == 'F')  {
02209                                 xold                = -x;
02210                                 yold                = -y;
02211                                 circ(j+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02212                                 xold                = -y;
02213                                 yold                =  x;
02214                                 circ(j+lt+lt+lt+kcirc) = quadri(xold+ns2,yold+nr2,nsam,nrow,xim);
02215                         }
02216                 }
02217         }
02218         return  out;
02219 }
02220 
02221 EMData* Util::Polar2Dm(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode){
02222         int nsam = image->get_xsize();
02223         int nrow = image->get_ysize();
02224         int nring = numr.size()/3;
02225         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02226         EMData* out = new EMData();
02227         out->set_size(lcirc,1,1);
02228         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02229         float *xim  = image->get_data();
02230         float *circ = out->get_data();
02231         double dpi, dfi;
02232         int    it, jt, inr, l, nsim, kcirc, lt;
02233         float  xold, yold, fi, x, y;
02234 
02235         //     cns2 and cnr2 are predefined centers
02236         //     no need to set to zero, all elements are defined
02237         dpi = 2*atan(1.0);
02238         for (it=1; it<=nring; it++) {
02239                 // radius of the ring
02240                 inr = numr(1,it);
02241 
02242                 // "F" means a full circle interpolation
02243                 // "H" means a half circle interpolation
02244 
02245                 l = numr(3,it);
02246                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02247                 else                              lt = l / 4;
02248 
02249                 nsim  = lt - 1;
02250                 dfi   = dpi / (nsim+1);
02251                 kcirc = numr(2,it);
02252                 xold  = 0.0f+cns2;
02253                 yold  = inr+cnr2;
02254 
02255                 Assert( kcirc <= lcirc );
02256                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on 90 degree
02257 
02258                 xold  = inr+cns2;
02259                 yold  = 0.0f+cnr2;
02260                 Assert( lt+kcirc <= lcirc );
02261                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 0 degree
02262 
02263                 if ( mode == 'f' || mode == 'F' ) {
02264                         xold = 0.0f+cns2;
02265                         yold = -inr+cnr2;
02266                         Assert( lt+lt+kcirc <= lcirc );
02267                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on 270 degree
02268 
02269                         xold = -inr+cns2;
02270                         yold = 0.0f+cnr2;
02271                         Assert(lt+lt+lt+kcirc <= lcirc );
02272                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on 180 degree
02273                 }
02274 
02275                 for (jt=1; jt<=nsim; jt++) {
02276                         fi   = static_cast<float>(dfi * jt);
02277                         x    = sin(fi) * inr;
02278                         y    = cos(fi) * inr;
02279 
02280                         xold = x+cns2;
02281                         yold = y+cnr2;
02282 
02283                         Assert( jt+kcirc <= lcirc );
02284                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);      // Sampling on the first quadrant
02285 
02286                         xold = y+cns2;
02287                         yold = -x+cnr2;
02288 
02289                         Assert( jt+lt+kcirc <= lcirc );
02290                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);    // Sampling on the fourth quadrant
02291 
02292                         if ( mode == 'f' || mode == 'F' ) {
02293                                 xold = -x+cns2;
02294                                 yold = -y+cnr2;
02295 
02296                                 Assert( jt+lt+lt+kcirc <= lcirc );
02297                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim); // Sampling on the third quadrant
02298 
02299                                 xold = -y+cns2;
02300                                 yold = x+cnr2;
02301 
02302                                 Assert( jt+lt+lt+lt+kcirc <= lcirc );
02303                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);  // Sampling on the second quadrant
02304                         }
02305                 } // end for jt
02306         } //end for it
02307         return out;
02308 }
02309 
02310 float Util::bilinear(float xold, float yold, int nsam, int nrow, float* xim)
02311 {
02312 /*
02313 c  purpose: linear interpolation
02314   Optimized for speed, circular closer removed, checking of ranges removed
02315 */
02316     float bilinear;
02317     int   ixold, iyold;
02318 
02319 /*
02320         float xdif, ydif, xrem, yrem;
02321         ixold   = (int) floor(xold);
02322         iyold   = (int) floor(yold);
02323         ydif = yold - iyold;
02324         yrem = 1.0f - ydif;
02325 
02326         //  May want to insert if?
02327 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02328 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02329 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02330         xdif = xold - ixold;
02331         xrem = 1.0f- xdif;
02332 //                 RBUF(K) = YDIF*(BUF(NADDR+NSAM)*XREM
02333 //     &                    +BUF(NADDR+NSAM+1)*XDIF)
02334 //     &                    +YREM*(BUF(NADDR)*XREM + BUF(NADDR+1)*XDIF)
02335         bilinear = ydif*(xim(ixold,iyold+1)*xrem + xim(ixold+1,iyold+1)*xdif) +
02336                                         yrem*(xim(ixold,iyold)*xrem+xim(ixold+1,iyold)*xdif);
02337 
02338     return bilinear;
02339 }
02340 */
02341         float xdif, ydif;
02342 
02343         ixold   = (int) xold;
02344         iyold   = (int) yold;
02345         ydif = yold - iyold;
02346 
02347         //  May want to insert it?
02348 //              IF ((IYOLD .GE. 1 .AND. IYOLD .LE. NROW-1) .AND.
02349 //     &            (IXOLD .GE. 1 .AND. IXOLD .LE. NSAM-1)) THEN
02350 //c                INSIDE BOUNDARIES OF OUTPUT IMAGE
02351         xdif = xold - ixold;
02352         bilinear = xim(ixold, iyold) + ydif* (xim(ixold, iyold+1) - xim(ixold, iyold)) +
02353                    xdif* (xim(ixold+1, iyold) - xim(ixold, iyold) +
02354                            ydif* (xim(ixold+1, iyold+1) - xim(ixold+1, iyold) - xim(ixold, iyold+1) + xim(ixold, iyold)) );
02355 
02356         return bilinear;
02357 }
02358 
02359 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02360              int  *numr, float *circ, int , int  nring, char  mode) {
02361         double dpi, dfi;
02362         int    it, jt, inr, l, nsim, kcirc, lt;
02363         float   xold, yold, fi, x, y;
02364 
02365         //     cns2 and cnr2 are predefined centers
02366         //     no need to set to zero, all elements are defined
02367 
02368         dpi = 2*atan(1.0);
02369         for (it=1; it<=nring; it++) {
02370                 // radius of the ring
02371                 inr = numr(1,it);
02372 
02373                 l = numr(3,it);
02374                 if ( mode == 'h' || mode == 'H' ) lt = l / 2;
02375                 else                              lt = l / 4;
02376 
02377                 nsim  = lt - 1;
02378                 dfi   = dpi / (nsim+1);
02379                 kcirc = numr(2,it);
02380 
02381 
02382                 xold  = 0.0f+cns2;
02383                 yold  = inr+cnr2;
02384 
02385                 circ(kcirc) = quadri(xold,yold,nsam,nrow,xim);
02386 
02387                 xold  = inr+cns2;
02388                 yold  = 0.0f+cnr2;
02389                 circ(lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02390 
02391                 if ( mode == 'f' || mode == 'F' ) {
02392                         xold = 0.0f+cns2;
02393                         yold = -inr+cnr2;
02394                         circ(lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02395 
02396                         xold = -inr+cns2;
02397                         yold = 0.0f+cnr2;
02398                         circ(lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02399                 }
02400 
02401                 for (jt=1; jt<=nsim; jt++) {
02402                         fi   = static_cast<float>(dfi * jt);
02403                         x    = sin(fi) * inr;
02404                         y    = cos(fi) * inr;
02405 
02406                         xold = x+cns2;
02407                         yold = y+cnr2;
02408                         circ(jt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02409 
02410                         xold = y+cns2;
02411                         yold = -x+cnr2;
02412                         circ(jt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02413 
02414                         if ( mode == 'f' || mode == 'F' ) {
02415                                 xold = -x+cns2;
02416                                 yold = -y+cnr2;
02417                                 circ(jt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02418 
02419                                 xold = -y+cns2;
02420                                 yold = x+cnr2;
02421                                 circ(jt+lt+lt+lt+kcirc) = quadri(xold,yold,nsam,nrow,xim);
02422                         }
02423                 } // end for jt
02424         } //end for it
02425 }
02426 /*
02427 void Util::alrl_ms(float *xim, int    nsam, int  nrow, float cns2, float cnr2,
02428              int  *numr, float *circ, int lcirc, int  nring, char  mode)
02429 {
02430    double dpi, dfi;
02431    int    it, jt, inr, l, nsim, kcirc, lt, xold, yold;
02432    float  yq, fi, x, y;
02433 
02434    //     cns2 and cnr2 are predefined centers
02435    //     no need to set to zero, all elements are defined
02436 
02437    dpi = 2*atan(1.0);
02438    for (it=1; it<=nring; it++) {
02439       // radius of the ring
02440       inr = numr(1,it);
02441       yq  = inr;
02442 
02443       l = numr(3,it);
02444       if ( mode == 'h' || mode == 'H' ) {
02445          lt = l / 2;
02446       }
02447       else { // if ( mode == 'f' || mode == 'F' )
02448          lt = l / 4;
02449       }
02450 
02451       nsim  = lt - 1;
02452       dfi   = dpi / (nsim+1);
02453       kcirc = numr(2,it);
02454 
02455 
02456         xold = (int) (0.0+cns2);
02457         yold = (int) (inr+cnr2);
02458 
02459         circ(kcirc) = xim(xold, yold);
02460 
02461       xold = (int) (inr+cns2);
02462       yold = (int) (0.0+cnr2);
02463       circ(lt+kcirc) = xim(xold, yold);
02464 
02465       if ( mode == 'f' || mode == 'F' ) {
02466          xold  = (int) (0.0+cns2);
02467          yold = (int) (-inr+cnr2);
02468          circ(lt+lt+kcirc) = xim(xold, yold);
02469 
02470          xold  = (int) (-inr+cns2);
02471          yold = (int) (0.0+cnr2);
02472          circ(lt+lt+lt+kcirc) = xim(xold, yold);
02473       }
02474 
02475       for (jt=1; jt<=nsim; jt++) {
02476          fi   = dfi * jt;
02477          x    = sin(fi) * yq;
02478          y    = cos(fi) * yq;
02479 
02480          xold  = (int) (x+cns2);
02481          yold = (int) (y+cnr2);
02482          circ(jt+kcirc) = xim(xold, yold);
02483 
02484          xold  = (int) (y+cns2);
02485          yold = (int) (-x+cnr2);
02486          circ(jt+lt+kcirc) = xim(xold, yold);
02487 
02488          if ( mode == 'f' || mode == 'F' ) {
02489             xold  = (int) (-x+cns2);
02490             yold = (int) (-y+cnr2);
02491             circ(jt+lt+lt+kcirc) = xim(xold, yold);
02492 
02493             xold  = (int) (-y+cns2);
02494             yold = (int) (x+cnr2);
02495             circ(jt+lt+lt+lt+kcirc) = xim(xold, yold);
02496          }
02497       } // end for jt
02498    } //end for it
02499 }
02500 */
02501 //xim((int) floor(xold), (int) floor(yold))
02502 #undef  xim
02503 
02504 EMData* Util::Polar2Dmi(EMData* image, float cns2, float cnr2, vector<int> numr, string cmode, Util::KaiserBessel& kb){
02505 // input image is twice the size of the original image
02506         int nring = numr.size()/3;
02507         int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
02508         EMData* out = new EMData();
02509         out->set_size(lcirc,1,1);
02510         char mode = (cmode == "F" || cmode == "f") ? 'f' : 'h';
02511         float *circ = out->get_data();
02512         float *fimage = image->get_data();
02513         int nx = image->get_xsize();
02514         int ny = image->get_ysize();
02515         int nz = image->get_zsize();
02516         double dpi, dfi;
02517         int    it, jt, inr, l, nsim, kcirc, lt;
02518         float  yq, xold, yold, fi, x, y;
02519 
02520         //     cns2 and cnr2 are predefined centers
02521         //     no need to set to zero, all elements are defined
02522 
02523         dpi = 2*atan(1.0);
02524         for (it=1;it<=nring;it++) {
02525                 // radius of the ring
02526                 inr = numr(1,it);
02527                 yq  = static_cast<float>(inr);
02528 
02529                 l = numr(3,it);
02530                 if ( mode == 'h' || mode == 'H' )  lt = l / 2;
02531                 else                               lt = l / 4;
02532 
02533                 nsim  = lt - 1;
02534                 dfi   = dpi / (nsim+1);
02535                 kcirc = numr(2,it);
02536                 xold  = 0.0f;
02537                 yold  = static_cast<float>(inr);
02538                 circ(kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02539 //      circ(kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02540 
02541                 xold  = static_cast<float>(inr);
02542                 yold  = 0.0f;
02543                 circ(lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02544 //      circ(lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02545 
02546         if ( mode == 'f' || mode == 'F' ) {
02547                 xold = 0.0f;
02548                 yold = static_cast<float>(-inr);
02549                 circ(lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02550 //         circ(lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02551 
02552                 xold = static_cast<float>(-inr);
02553                 yold = 0.0f;
02554                 circ(lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02555 //         circ(lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02556         }
02557 
02558         for (jt=1;jt<=nsim;jt++) {
02559                 fi   = static_cast<float>(dfi * jt);
02560                 x    = sin(fi) * yq;
02561                 y    = cos(fi) * yq;
02562 
02563                 xold = x;
02564                 yold = y;
02565                 circ(jt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02566 //         circ(jt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02567 
02568                 xold = y;
02569                 yold = -x;
02570                 circ(jt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02571 //         circ(jt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02572 
02573         if ( mode == 'f' || mode == 'F' ) {
02574                 xold = -x;
02575                 yold = -y;
02576                 circ(jt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02577 //            circ(jt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02578 
02579                 xold = -y;
02580                 yold = x;
02581                 circ(jt+lt+lt+lt+kcirc) = get_pixel_conv_new(nx,ny,nz,2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,fimage,kb);
02582 //            circ(jt+lt+lt+lt+kcirc) = image->get_pixel_conv(2*(xold+cns2-1.0f),2*(yold+cnr2-1.0f),0,kb);
02583         }
02584         } // end for jt
02585         } //end for it
02586         return  out;
02587 }
02588 
02589 /*
02590 
02591         A set of 1-D power-of-two FFTs
02592         Pawel & Chao 01/20/06
02593 
02594 fftr_q(xcmplx,nv)
02595   single precision
02596 
02597  dimension xcmplx(2,iabs(nv)/2);
02598  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02599  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02600 
02601 
02602 fftr_d(xcmplx,nv)
02603   double precision
02604 
02605  dimension xcmplx(2,iabs(nv)/2);
02606  xcmplx(1,1) --- R(0), xcmplx(2,1) --- R(NV/2)
02607  xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02608 
02609 
02610 
02611 */
02612 #define  tab1(i)      tab1[i-1]
02613 #define  xcmplx(i,j)  xcmplx [(j-1)*2 + i-1]
02614 #define  br(i)        br[i-1]
02615 #define  bi(i)        bi[i-1]
02616 //-----------------------------------------
02617 void Util::fftc_d(double *br, double *bi, int ln, int ks)
02618 {
02619         double rni,sgn,tr1,tr2,ti1,ti2;
02620         double cc,c,ss,s,t,x2,x3,x4,x5;
02621         int    b3,b4,b5,b6,b7,b56;
02622         int    n, k, l, j, i, ix0, ix1, status=0;
02623 
02624         const double tab1[] = {
02625                 9.58737990959775e-5,
02626                 1.91747597310703e-4,
02627                 3.83495187571395e-4,
02628                 7.66990318742704e-4,
02629                 1.53398018628476e-3,
02630                 3.06795676296598e-3,
02631                 6.13588464915449e-3,
02632                 1.22715382857199e-2,
02633                 2.45412285229123e-2,
02634                 4.90676743274181e-2,
02635                 9.80171403295604e-2,
02636                 1.95090322016128e-1,
02637                 3.82683432365090e-1,
02638                 7.07106781186546e-1,
02639                 1.00000000000000,
02640         };
02641 
02642         n=(int)pow(2.0f,ln);
02643 
02644         k=abs(ks);
02645         l=16-ln;
02646         b3=n*k;
02647         b6=b3;
02648         b7=k;
02649         if (ks > 0) {
02650                 sgn=1.0f;
02651         } else {
02652                 sgn=-1.0f;
02653                 rni=1.0f/(float)(n);
02654                 j=1;
02655                 for (i=1; i<=n; i++) {
02656                         br(j)=br(j)*rni;
02657                         bi(j)=bi(j)*rni;
02658                         j=j+k;
02659                 }
02660         }
02661 
02662 L12:
02663    b6=b6/2;
02664    b5=b6;
02665    b4=2*b6;
02666    b56=b5-b6;
02667 
02668 L14:
02669    tr1=br(b5+1);
02670    ti1=bi(b5+1);
02671    tr2=br(b56+1);
02672    ti2=bi(b56+1);
02673 
02674    br(b5+1)=tr2-tr1;
02675    bi(b5+1)=ti2-ti1;
02676    br(b56+1)=tr1+tr2;
02677    bi(b56+1)=ti1+ti2;
02678 
02679    b5=b5+b4;
02680    b56=b5-b6;
02681    if ( b5 <= b3 )  goto  L14;
02682    if ( b6 == b7 )  goto  L20;
02683 
02684    b4=b7;
02685    cc=2.0f*pow(tab1(l),2);
02686    c=1.0f-cc;
02687    l++;
02688    ss=sgn*tab1(l);
02689    s=ss;
02690 
02691 L16:
02692    b5=b6+b4;
02693    b4=2*b6;
02694    b56=b5-b6;
02695 
02696 L18:
02697    tr1=br(b5+1);
02698    ti1=bi(b5+1);
02699    tr2=br(b56+1);
02700    ti2=bi(b56+1);
02701    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02702    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02703    br(b56+1)=tr1+tr2;
02704    bi(b56+1)=ti1+ti2;
02705 
02706    b5=b5+b4;
02707    b56=b5-b6;
02708    if ( b5 <= b3 )  goto  L18;
02709    b4=b5-b6;
02710    b5=b4-b3;
02711    c=-c;
02712    b4=b6-b5;
02713    if ( b5 < b4 )  goto  L16;
02714    b4=b4+b7;
02715    if ( b4 >= b5 ) goto  L12;
02716 
02717    t=c-cc*c-ss*s;
02718    s=s+ss*c-cc*s;
02719    c=t;
02720    goto  L16;
02721 
02722 L20:
02723    ix0=b3/2;
02724    b3=b3-b7;
02725    b4=0;
02726    b5=0;
02727    b6=ix0;
02728    ix1=0;
02729    if (b6 == b7) goto EXIT;
02730 
02731 L22:
02732    b4=b3-b4;
02733    b5=b3-b5;
02734    x2=br(b4+1);
02735    x3=br(b5+1);
02736    x4=bi(b4+1);
02737    x5=bi(b5+1);
02738    br(b4+1)=x3;
02739    br(b5+1)=x2;
02740    bi(b4+1)=x5;
02741    bi(b5+1)=x4;
02742    if(b6 < b4)  goto  L22;
02743 
02744 L24:
02745    b4=b4+b7;
02746    b5=b6+b5;
02747    x2=br(b4+1);
02748    x3=br(b5+1);
02749    x4=bi(b4+1);
02750    x5=bi(b5+1);
02751    br(b4+1)=x3;
02752    br(b5+1)=x2;
02753    bi(b4+1)=x5;
02754    bi(b5+1)=x4;
02755    ix0=b6;
02756 
02757 L26:
02758    ix0=ix0/2;
02759    ix1=ix1-ix0;
02760    if( ix1 >= 0)  goto L26;
02761 
02762    ix0=2*ix0;
02763    b4=b4+b7;
02764    ix1=ix1+ix0;
02765    b5=ix1;
02766    if ( b5 >= b4)  goto  L22;
02767    if ( b4 < b6)   goto  L24;
02768 
02769 EXIT:
02770    status = 0;
02771 }
02772 
02773 // -----------------------------------------------------------------
02774 void Util::fftc_q(float *br, float *bi, int ln, int ks)
02775 {
02776         //  dimension  br(1),bi(1)
02777 
02778         int b3,b4,b5,b6,b7,b56;
02779         int n, k, l, j, i, ix0, ix1;
02780         float rni, tr1, ti1, tr2, ti2, cc, c, ss, s, t, x2, x3, x4, x5, sgn;
02781         int status=0;
02782 
02783         const float tab1[] = {
02784                 9.58737990959775e-5f,
02785                 1.91747597310703e-4f,
02786                 3.83495187571395e-4f,
02787                 7.66990318742704e-4f,
02788                 1.53398018628476e-3f,
02789                 3.06795676296598e-3f,
02790                 6.13588464915449e-3f,
02791                 1.22715382857199e-2f,
02792                 2.45412285229123e-2f,
02793                 4.90676743274181e-2f,
02794                 9.80171403295604e-2f,
02795                 1.95090322016128e-1f,
02796                 3.82683432365090e-1f,
02797                 7.07106781186546e-1f,
02798                 1.00000000000000f,
02799         };
02800 
02801         n=(int)pow(2.0f,ln);
02802 
02803         k=abs(ks);
02804         l=16-ln;
02805         b3=n*k;
02806         b6=b3;
02807         b7=k;
02808         if( ks > 0 ) {
02809                 sgn=1.0f;
02810         } else {
02811                 sgn=-1.0f;
02812                 rni=1.0f/(float)n;
02813                 j=1;
02814                 for (i=1; i<=n; i++) {
02815                         br(j)=br(j)*rni;
02816                         bi(j)=bi(j)*rni;
02817                         j=j+k;
02818                 }
02819         }
02820 L12:
02821    b6=b6/2;
02822    b5=b6;
02823    b4=2*b6;
02824    b56=b5-b6;
02825 L14:
02826    tr1=br(b5+1);
02827    ti1=bi(b5+1);
02828 
02829    tr2=br(b56+1);
02830    ti2=bi(b56+1);
02831 
02832    br(b5+1)=tr2-tr1;
02833    bi(b5+1)=ti2-ti1;
02834    br(b56+1)=tr1+tr2;
02835    bi(b56+1)=ti1+ti2;
02836 
02837    b5=b5+b4;
02838    b56=b5-b6;
02839    if ( b5 <= b3 )  goto  L14;
02840    if ( b6 == b7 )  goto  L20;
02841 
02842    b4=b7;
02843    cc=2.0f*pow(tab1(l),2);
02844    c=1.0f-cc;
02845    l++;
02846    ss=sgn*tab1(l);
02847    s=ss;
02848 L16:
02849    b5=b6+b4;
02850    b4=2*b6;
02851    b56=b5-b6;
02852 L18:
02853    tr1=br(b5+1);
02854    ti1=bi(b5+1);
02855    tr2=br(b56+1);
02856    ti2=bi(b56+1);
02857    br(b5+1)=c*(tr2-tr1)-s*(ti2-ti1);
02858    bi(b5+1)=s*(tr2-tr1)+c*(ti2-ti1);
02859    br(b56+1)=tr1+tr2;
02860    bi(b56+1)=ti1+ti2;
02861 
02862    b5=b5+b4;
02863    b56=b5-b6;
02864    if(b5 <= b3)  goto L18;
02865    b4=b5-b6;
02866    b5=b4-b3;
02867    c=-c;
02868    b4=b6-b5;
02869    if(b5 < b4)  goto  L16;
02870    b4=b4+b7;
02871    if(b4 >= b5) goto  L12;
02872 
02873    t=c-cc*c-ss*s;
02874    s=s+ss*c-cc*s;
02875    c=t;
02876    goto  L16;
02877 L20:
02878    ix0=b3/2;
02879    b3=b3-b7;
02880    b4=0;
02881    b5=0;
02882    b6=ix0;
02883    ix1=0;
02884    if ( b6 == b7) goto EXIT;
02885 L22:
02886    b4=b3-b4;
02887    b5=b3-b5;
02888    x2=br(b4+1);
02889    x3=br(b5+1);
02890    x4=bi(b4+1);
02891    x5=bi(b5+1);
02892    br(b4+1)=x3;
02893    br(b5+1)=x2;
02894    bi(b4+1)=x5;
02895    bi(b5+1)=x4;
02896    if (b6 < b4) goto  L22;
02897 L24:
02898    b4=b4+b7;
02899    b5=b6+b5;
02900    x2=br(b4+1);
02901    x3=br(b5+1);
02902    x4=bi(b4+1);
02903    x5=bi(b5+1);
02904    br(b4+1)=x3;
02905    br(b5+1)=x2;
02906    bi(b4+1)=x5;
02907    bi(b5+1)=x4;
02908    ix0=b6;
02909 L26:
02910    ix0=ix0/2;
02911    ix1=ix1-ix0;
02912    if(ix1 >= 0)  goto  L26;
02913 
02914    ix0=2*ix0;
02915    b4=b4+b7;
02916    ix1=ix1+ix0;
02917    b5=ix1;
02918    if (b5 >= b4)  goto  L22;
02919    if (b4 < b6)   goto  L24;
02920 EXIT:
02921    status = 0;
02922 }
02923 
02924 void  Util::fftr_q(float *xcmplx, int nv)
02925 {
02926    // dimension xcmplx(2,1); xcmplx(1,i) --- real, xcmplx(2,i) --- imaginary
02927 
02928         int nu, inv, nu1, n, isub, n2, i1, i2, i;
02929         float ss, cc, c, s, tr, ti, tr1, tr2, ti1, ti2, t;
02930 
02931         const float tab1[] = {
02932                 9.58737990959775e-5f,
02933                 1.91747597310703e-4f,
02934                 3.83495187571395e-4f,
02935                 7.66990318742704e-4f,
02936                 1.53398018628476e-3f,
02937                 3.06795676296598e-3f,
02938                 6.13588464915449e-3f,
02939                 1.22715382857199e-2f,
02940                 2.45412285229123e-2f,
02941                 4.90676743274181e-2f,
02942                 9.80171403295604e-2f,
02943                 1.95090322016128e-1f,
02944                 3.82683432365090e-1f,
02945                 7.07106781186546e-1f,
02946                 1.00000000000000f,
02947         };
02948 
02949         nu=abs(nv);
02950         inv=nv/nu;
02951         nu1=nu-1;
02952         n=(int)pow(2.f,nu1);
02953         isub=16-nu1;
02954 
02955         ss=-tab1(isub);
02956         cc=-2.0f*pow(tab1(isub-1),2.f);
02957         c=1.0f;
02958         s=0.0f;
02959         n2=n/2;
02960         if ( inv > 0) {
02961                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
02962                 tr=xcmplx(1,1);
02963                 ti=xcmplx(2,1);
02964                 xcmplx(1,1)=tr+ti;
02965                 xcmplx(2,1)=tr-ti;
02966                 for (i=1;i<=n2;i++) {
02967                         i1=i+1;
02968                         i2=n-i+1;
02969                         tr1=xcmplx(1,i1);
02970                         tr2=xcmplx(1,i2);
02971                         ti1=xcmplx(2,i1);
02972                         ti2=xcmplx(2,i2);
02973                         t=(cc*c-ss*s)+c;
02974                         s=(cc*s+ss*c)+s;
02975                         c=t;
02976                         xcmplx(1,i1)=0.5f*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
02977                         xcmplx(1,i2)=0.5f*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
02978                         xcmplx(2,i1)=0.5f*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02979                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
02980                 }
02981         } else {
02982                 tr=xcmplx(1,1);
02983                 ti=xcmplx(2,1);
02984                 xcmplx(1,1)=0.5f*(tr+ti);
02985                 xcmplx(2,1)=0.5f*(tr-ti);
02986                 for (i=1; i<=n2; i++) {
02987                         i1=i+1;
02988                         i2=n-i+1;
02989                         tr1=xcmplx(1,i1);
02990                         tr2=xcmplx(1,i2);
02991                         ti1=xcmplx(2,i1);
02992                         ti2=xcmplx(2,i2);
02993                         t=(cc*c-ss*s)+c;
02994                         s=(cc*s+ss*c)+s;
02995                         c=t;
02996                         xcmplx(1,i1)=0.5f*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
02997                         xcmplx(1,i2)=0.5f*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
02998                         xcmplx(2,i1)=0.5f*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
02999                         xcmplx(2,i2)=0.5f*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03000                 }
03001                 fftc_q(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03002         }
03003 }
03004 
03005 // -------------------------------------------
03006 void  Util::fftr_d(double *xcmplx, int nv)
03007 {
03008         // double precision  x(2,1)
03009         int    i1, i2,  nu, inv, nu1, n, isub, n2, i;
03010         double tr1,tr2,ti1,ti2,tr,ti;
03011         double cc,c,ss,s,t;
03012         const double tab1[] = {
03013                 9.58737990959775e-5,
03014                 1.91747597310703e-4,
03015                 3.83495187571395e-4,
03016                 7.66990318742704e-4,
03017                 1.53398018628476e-3,
03018                 3.06795676296598e-3,
03019                 6.13588464915449e-3,
03020                 1.22715382857199e-2,
03021                 2.45412285229123e-2,
03022                 4.90676743274181e-2,
03023                 9.80171403295604e-2,
03024                 1.95090322016128e-1,
03025                 3.82683432365090e-1,
03026                 7.07106781186546e-1,
03027                 1.00000000000000,
03028         };
03029 
03030         nu=abs(nv);
03031         inv=nv/nu;
03032         nu1=nu-1;
03033         n=(int)pow(2.0f,nu1);
03034         isub=16-nu1;
03035         ss=-tab1(isub);
03036         cc=-2.0*pow(tab1(isub-1),2);
03037         c=1.0f;
03038         s=0.0f;
03039         n2=n/2;
03040 
03041         if ( inv > 0 ) {
03042                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,2);
03043                 tr=xcmplx(1,1);
03044                 ti=xcmplx(2,1);
03045                 xcmplx(1,1)=tr+ti;
03046                 xcmplx(2,1)=tr-ti;
03047                 for (i=1;i<=n2;i++) {
03048                         i1=i+1;
03049                         i2=n-i+1;
03050                         tr1=xcmplx(1,i1);
03051                         tr2=xcmplx(1,i2);
03052                         ti1=xcmplx(2,i1);
03053                         ti2=xcmplx(2,i2);
03054                         t=(cc*c-ss*s)+c;
03055                         s=(cc*s+ss*c)+s;
03056                         c=t;
03057                         xcmplx(1,i1)=0.5*((tr1+tr2)+(ti1+ti2)*c-(tr1-tr2)*s);
03058                         xcmplx(1,i2)=0.5*((tr1+tr2)-(ti1+ti2)*c+(tr1-tr2)*s);
03059                         xcmplx(2,i1)=0.5*((ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03060                         xcmplx(2,i2)=0.5*(-(ti1-ti2)-(ti1+ti2)*s-(tr1-tr2)*c);
03061                 }
03062         } else {
03063                 tr=xcmplx(1,1);
03064                 ti=xcmplx(2,1);
03065                 xcmplx(1,1)=0.5*(tr+ti);
03066                 xcmplx(2,1)=0.5*(tr-ti);
03067                 for (i=1; i<=n2; i++) {
03068                         i1=i+1;
03069                         i2=n-i+1;
03070                         tr1=xcmplx(1,i1);
03071                         tr2=xcmplx(1,i2);
03072                         ti1=xcmplx(2,i1);
03073                         ti2=xcmplx(2,i2);
03074                         t=(cc*c-ss*s)+c;
03075                         s=(cc*s+ss*c)+s;
03076                         c=t;
03077                         xcmplx(1,i1)=0.5*((tr1+tr2)-(tr1-tr2)*s-(ti1+ti2)*c);
03078                         xcmplx(1,i2)=0.5*((tr1+tr2)+(tr1-tr2)*s+(ti1+ti2)*c);
03079                         xcmplx(2,i1)=0.5*((ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03080                         xcmplx(2,i2)=0.5*(-(ti1-ti2)+(tr1-tr2)*c-(ti1+ti2)*s);
03081                 }
03082                 fftc_d(&xcmplx(1,1),&xcmplx(2,1),nu1,-2);
03083         }
03084 }
03085 #undef  tab1
03086 #undef  xcmplx
03087 #undef  br
03088 #undef  bi
03089 
03090 
03091 void Util::Frngs(EMData* circp, vector<int> numr){
03092         int nring = numr.size()/3;
03093         float *circ = circp->get_data();
03094         int i, l;
03095         for (i=1; i<=nring;i++) {
03096 
03097 #ifdef _WIN32
03098                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03099 #else
03100                 l=(int)(log2(numr(3,i)));
03101 #endif  //_WIN32
03102 
03103                 fftr_q(&circ(numr(2,i)),l);
03104         }
03105 }
03106 
03107 void Util::Frngs_inv(EMData* circp, vector<int> numr){
03108         int nring = numr.size()/3;
03109         float *circ = circp->get_data();
03110         int i, l;
03111         for (i=1; i<=nring;i++) {
03112 
03113 #ifdef _WIN32
03114                 l = (int)( log((float)numr(3,i))/log(2.0f) );
03115 #else
03116                 l=(int)(log2(numr(3,i)));
03117 #endif  //_WIN32
03118 
03119                 fftr_q(&circ(numr(2,i)),-l);
03120         }
03121 }
03122 #undef  circ
03123 
03124 #define  b(i)            b[i-1]
03125 void Util::prb1d(double *b, int npoint, float *pos) {
03126         double  c2,c3;
03127         int     nhalf;
03128 
03129         nhalf = npoint/2 + 1;
03130         *pos  = 0.0;
03131 
03132         if (npoint == 7) {
03133                 c2 = 49.*b(1) + 6.*b(2) - 21.*b(3) - 32.*b(4) - 27.*b(5)
03134                      - 6.*b(6) + 31.*b(7);
03135                 c3 = 5.*b(1) - 3.*b(3) - 4.*b(4) - 3.*b(5) + 5.*b(7);
03136         }
03137         else if (npoint == 5) {
03138                 c2 = (74.*b(1) - 23.*b(2) - 60.*b(3) - 37.*b(4)
03139                    + 46.*b(5) ) / (-70.);
03140                 c3 = (2.*b(1) - b(2) - 2.*b(3) - b(4) + 2.*b(5) ) / 14.0;
03141         }
03142         else if (npoint == 3) {
03143                 c2 = (5.*b(1) - 8.*b(2) + 3.*b(3) ) / (-2.0);
03144                 c3 = (b(1) - 2.*b(2) + b(3) ) / 2.0;
03145         }
03146         //else if (npoint == 9) {
03147         else  { // at least one has to be true!!
03148                 c2 = (1708.*b(1) + 581.*b(2) - 246.*b(3) - 773.*b(4)
03149                      - 1000.*b(5) - 927.*b(6) - 554.*b(7) + 119.*b(8)
03150                      + 1092.*b(9) ) / (-4620.);
03151                 c3 = (28.*b(1) + 7.*b(2) - 8.*b(3) - 17.*b(4) - 20.*b(5)
03152                      - 17.*b(6) - 8.*b(7) + 7.*b(8) + 28.*b(9) ) / 924.0;
03153         }
03154         if (c3 != 0.0)  *pos = static_cast<float>(c2/(2.0*c3) - nhalf);
03155 }
03156 #undef  b
03157 
03158 #define  circ1(i)        circ1[i-1]
03159 #define  circ2(i)        circ2[i-1]
03160 #define  t(i)            t[i-1]
03161 #define  q(i)            q[i-1]
03162 #define  b(i)            b[i-1]
03163 #define  t7(i)           t7[i-1]
03164 Dict Util::Crosrng_e(EMData*  circ1p, EMData* circ2p, vector<int> numr, int neg) {
03165         //  neg = 0 straight,  neg = 1 mirrored
03166         int nring = numr.size()/3;
03167         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03168         int maxrin = numr[numr.size()-1];
03169         double qn;   float  tot;
03170         float *circ1 = circ1p->get_data();
03171         float *circ2 = circ2p->get_data();
03172 /*
03173 c checks single position, neg is flag for checking mirrored position
03174 c
03175 c  input - fourier transforms of rings!
03176 c  first set is conjugated (mirrored) if neg
03177 c  circ1 already multiplied by weights!
03178 c       automatic arrays
03179         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03180         double precision  q(maxrin)
03181         double precision  t7(-3:3)
03182 */
03183         float *t;
03184         double t7[7], *q;
03185         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03186         float  pos;
03187 
03188 #ifdef _WIN32
03189         ip = -(int)(log((float)maxrin)/log(2.0f));
03190 #else
03191         ip = -(int) (log2(maxrin));
03192 #endif  //_WIN32
03193 
03194         q = (double*)calloc(maxrin, sizeof(double));
03195         t = (float*)calloc(maxrin, sizeof(float));
03196 
03197 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03198         for (i=1; i<=nring; i++) {
03199                 numr3i = numr(3,i);
03200                 numr2i = numr(2,i);
03201 
03202                 t(1) = (circ1(numr2i)) * circ2(numr2i);
03203 
03204                 if (numr3i != maxrin) {
03205                          // test .ne. first for speed on some compilers
03206                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03207                         t(2)            = 0.0;
03208 
03209                         if (neg) {
03210                                 // first set is conjugated (mirrored)
03211                                 for (j=3;j<=numr3i;j=j+2) {
03212                                         jc = j+numr2i-1;
03213                                         t(j) =(circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03214                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03215                                 }
03216                         } else {
03217                                 for (j=3;j<=numr3i;j=j+2) {
03218                                         jc = j+numr2i-1;
03219                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03220                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03221                                 }
03222                         }
03223                         for (j=1;j<=numr3i+1;j++) q(j) = q(j) + t(j);
03224                 } else {
03225                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03226                         if (neg) {
03227                                 // first set is conjugated (mirrored)
03228                                 for (j=3;j<=maxrin;j=j+2) {
03229                                         jc = j+numr2i-1;
03230                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03231                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03232                                 }
03233                         } else {
03234                                 for (j=3;j<=maxrin;j=j+2) {
03235                                         jc = j+numr2i-1;
03236                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03237                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03238                                 }
03239                         }
03240                         for (j = 1; j <= maxrin; j++) q(j) += t(j);
03241                 }
03242         }
03243 
03244         fftr_d(q,ip);
03245 
03246         qn = -1.0e20;
03247         for (j=1;j<=maxrin;j++) {
03248            if (q(j) >= qn) {
03249                   qn = q(j); jtot = j;
03250            }
03251         }
03252 
03253         for (k=-3; k<=3; k++) {
03254                 j = (jtot+k+maxrin-1)%maxrin + 1;
03255                 t7(k+4) = q(j);
03256         }
03257 
03258         prb1d(t7,7,&pos);
03259 
03260         tot = (float)jtot + pos;
03261 
03262         if (q) free(q);
03263         if (t) free(t);
03264 
03265         Dict retvals;
03266         retvals["qn"] = qn;
03267         retvals["tot"] = tot;
03268         return  retvals;
03269 }
03270 
03271 Dict Util::Crosrng_ew(EMData*  circ1p, EMData* circ2p, vector<int> numr, vector<float> w, int neg) {
03272    //  neg = 0 straight,  neg = 1 mirrored
03273         int nring = numr.size()/3;
03274         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03275         int maxrin = numr[numr.size()-1];
03276         double qn;   float  tot;
03277         float *circ1 = circ1p->get_data();
03278         float *circ2 = circ2p->get_data();
03279 /*
03280 c checks single position, neg is flag for checking mirrored position
03281 c
03282 c  input - fourier transforms of rings!
03283 c  first set is conjugated (mirrored) if neg
03284 c  multiplication by weights!
03285 c       automatic arrays
03286         dimension         t(maxrin)  removed +2 as it is only needed for other ffts
03287         double precision  q(maxrin)
03288         double precision  t7(-3:3)
03289 */
03290         float *t;
03291         double t7[7], *q;
03292         int    i, j, k, ip, jc, numr3i, numr2i, jtot = 0;
03293         float  pos;
03294 
03295 #ifdef _WIN32
03296         ip = -(int)(log((float)maxrin)/log(2.0f));
03297 #else
03298         ip = -(int) (log2(maxrin));
03299 #endif  //_WIN32
03300 
03301         q = (double*)calloc(maxrin, sizeof(double));
03302         t = (float*)calloc(maxrin, sizeof(float));
03303 
03304 //   cout << *qn <<"  " <<*tot<<"  "<<ip<<endl;
03305         for (i=1;i<=nring;i++) {
03306                 numr3i = numr(3,i);
03307                 numr2i = numr(2,i);
03308 
03309                 t(1) = circ1(numr2i) * circ2(numr2i);
03310 
03311                 if (numr3i != maxrin) {
03312                         // test .ne. first for speed on some compilers
03313                         t(numr3i+1) = circ1(numr2i+1) * circ2(numr2i+1);
03314                         t(2)      = 0.0;
03315 
03316                         if (neg) {
03317                                 // first set is conjugated (mirrored)
03318                                 for (j=3; j<=numr3i; j=j+2) {
03319                                         jc = j+numr2i-1;
03320                                         t(j)   =  (circ1(jc))*circ2(jc)-(circ1(jc+1))*circ2(jc+1);
03321                                         t(j+1) = -(circ1(jc))*circ2(jc+1)-(circ1(jc+1))*circ2(jc);
03322                                 }
03323                         } else {
03324                                 for (j=3; j<=numr3i; j=j+2) {
03325                                         jc = j+numr2i-1;
03326                                         t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03327                                         t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03328                                 }
03329                         }
03330                         for (j=1;j<=numr3i+1;j++) q(j) += t(j)*w[i-1];
03331                 } else {
03332                         t(2) = circ1(numr2i+1) * circ2(numr2i+1);
03333                         if (neg) {
03334                                 // first set is conjugated (mirrored)
03335                                 for (j=3; j<=maxrin; j=j+2) {
03336                                         jc = j+numr2i-1;
03337                                         t(j) = (circ1(jc))*circ2(jc) - (circ1(jc+1))*circ2(jc+1);
03338                                         t(j+1) = -(circ1(jc))*circ2(jc+1) - (circ1(jc+1))*circ2(jc);
03339                                 }
03340                         } else {
03341                                 for (j=3; j<=maxrin; j=j+2) {
03342                                 jc = j+numr2i-1;
03343                                 t(j) = (circ1(jc))*circ2(jc) + (circ1(jc+1))*circ2(jc+1);
03344                                 t(j+1) = -(circ1(jc))*circ2(jc+1) + (circ1(jc+1))*circ2(jc);
03345                                 }
03346                         }
03347                         for (j = 1; j <= maxrin; j++) q(j) += t(j)*w[i-1];
03348                 }
03349         }
03350 
03351         fftr_d(q,ip);
03352 
03353         qn = -1.0e20;
03354         for (j=1;j<=maxrin;j++) {
03355                 //cout << j << "  " << q(j) << endl;
03356                 if (q(j) >= qn) {
03357                         qn = q(j);
03358                         jtot = j;
03359                 }
03360         }
03361 
03362         for (k=-3; k<=3; k++) {
03363                 j = (jtot+k+maxrin-1)%maxrin + 1;
03364                 t7(k+4) = q(j);
03365         }
03366 
03367         prb1d(t7,7,&pos);
03368 
03369         tot = (float)jtot + pos;
03370 
03371         //if (q) free(q);
03372         if (t) free(t);
03373 
03374         Dict retvals;
03375         //tot = 1;
03376         //qn = q(1);
03377         retvals["qn"] = qn;
03378         retvals["tot"] = tot;
03379 
03380         if (q) free(q);
03381 
03382         return  retvals;
03383 }
03384 
03385 Dict Util::Crosrng_ms(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03386         int nring = numr.size()/3;
03387         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03388         int maxrin = numr[numr.size()-1];
03389         double qn; float tot; double qm; float tmt;
03390         float *circ1 = circ1p->get_data();
03391         float *circ2 = circ2p->get_data();
03392 /*
03393 c
03394 c  checks both straight & mirrored positions
03395 c
03396 c  input - fourier transforms of rings!!
03397 c  circ1 already multiplied by weights!
03398 c
03399 */
03400 
03401         // dimension             circ1(lcirc),circ2(lcirc)
03402 
03403         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03404         double *t, *q, t7[7];
03405 
03406         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03407         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03408 
03409         qn  = 0.0f;
03410         qm  = 0.0f;
03411         tot = 0.0f;
03412         tmt = 0.0f;
03413 #ifdef _WIN32
03414         ip = -(int)(log((float)maxrin)/log(2.0f));
03415 #else
03416         ip = -(int)(log2(maxrin));
03417 #endif  //_WIN32
03418   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03419 
03420         //  c - straight  = circ1 * conjg(circ2)
03421         //  zero q array
03422 
03423         q = (double*)calloc(maxrin,sizeof(double));
03424 
03425         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03426         //   zero t array
03427         t = (double*)calloc(maxrin,sizeof(double));
03428 
03429    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03430         for (i=1; i<=nring; i++) {
03431 
03432                 numr3i = numr(3,i);   // Number of samples of this ring
03433                 numr2i = numr(2,i);   // The beginning point of this ring
03434 
03435                 t1   = circ1(numr2i) * circ2(numr2i);
03436                 q(1) += t1;
03437                 t(1) += t1;
03438 
03439                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03440                 if (numr3i == maxrin)  {
03441                         q(2) += t1;
03442                         t(2) += t1;
03443                 } else {
03444                         q(numr3i+1) += t1;
03445                         t(numr3i+1) += t1;
03446                 }
03447 
03448                 for (j=3; j<=numr3i; j += 2) {
03449                         jc     = j+numr2i-1;
03450 
03451 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03452 //                                ----- -----    ----- -----
03453 //                                 t1     t2      t3    t4
03454 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03455 //                                    ----- -----    ----- -----
03456 //                                     t1    t2       t3    t4
03457 
03458                         c1     = circ1(jc);
03459                         c2     = circ1(jc+1);
03460                         d1     = circ2(jc);
03461                         d2     = circ2(jc+1);
03462 
03463                         t1     = c1 * d1;
03464                         t2     = c2 * d2;
03465                         t3     = c1 * d2;
03466                         t4     = c2 * d1;
03467 
03468                         q(j)   += t1 + t2;
03469                         q(j+1) += -t3 + t4;
03470                         t(j)   += t1 - t2;
03471                         t(j+1) += -t3 - t4;
03472                 }
03473         }
03474         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03475         fftr_d(q,ip);
03476 
03477         qn  = -1.0e20;
03478         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03479                 if (q(j) >= qn) {
03480                         qn  = q(j);
03481                         jtot = j;
03482                 }
03483         }
03484 
03485         for (k=-3; k<=3; k++) {
03486                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03487                 t7(k+4) = q(j);
03488         }
03489 
03490         // interpolate
03491         prb1d(t7,7,&pos);
03492         tot = (float)(jtot)+pos;
03493         // Do not interpolate
03494         //tot = (float)(jtot);
03495 
03496         // mirrored
03497         fftr_d(t,ip);
03498 
03499         // find angle
03500         qm = -1.0e20;
03501         for (j=1; j<=maxrin;j++) {//cout <<"  "<<j<<"   "<<t(j) <<endl;
03502                 if ( t(j) >= qm ) {
03503                         qm   = t(j);
03504                         jtot = j;
03505                 }
03506         }
03507 
03508         for (k=-3; k<=3; k++)  {
03509                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03510                 t7(k+4) = t(j);
03511         }
03512 
03513         // interpolate
03514 
03515         prb1d(t7,7,&pos);
03516         tmt = float(jtot) + pos;
03517         // Do not interpolate
03518         //tmt = float(jtot);
03519 
03520         free(t);
03521         free(q);
03522 
03523         Dict retvals;
03524         retvals["qn"] = qn;
03525         retvals["tot"] = tot;
03526         retvals["qm"] = qm;
03527         retvals["tmt"] = tmt;
03528         return retvals;
03529 }
03530 
03531 Dict Util::Crosrng_ms_delta(EMData* circ1p, EMData* circ2p, vector<int> numr, float delta_start, float delta) {
03532         int nring = numr.size()/3;
03533         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03534         int maxrin = numr[numr.size()-1];
03535         double qn; float tot; double qm; float tmt;
03536         float *circ1 = circ1p->get_data();
03537         float *circ2 = circ2p->get_data();
03538 /*
03539 c
03540 c  checks both straight & mirrored positions
03541 c
03542 c  input - fourier transforms of rings!!
03543 c  circ1 already multiplied by weights!
03544 c
03545 */
03546 
03547         // dimension             circ1(lcirc),circ2(lcirc)
03548 
03549         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03550         double *t, *q;
03551 
03552         int   ip, jc, numr3i, numr2i, i, j, jtot = 0;
03553         float t1, t2, t3, t4, c1, c2, d1, d2;
03554 
03555         qn  = 0.0f;
03556         qm  = 0.0f;
03557         tot = 0.0f;
03558         tmt = 0.0f;
03559 #ifdef _WIN32
03560         ip = -(int)(log((float)maxrin)/log(2.0f));
03561 #else
03562         ip = -(int)(log2(maxrin));
03563 #endif  //_WIN32
03564   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03565 
03566         //  c - straight  = circ1 * conjg(circ2)
03567         //  zero q array
03568 
03569         q = (double*)calloc(maxrin,sizeof(double));
03570 
03571         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03572         //   zero t array
03573         t = (double*)calloc(maxrin,sizeof(double));
03574 
03575    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03576         for (i=1; i<=nring; i++) {
03577 
03578                 numr3i = numr(3,i);   // Number of samples of this ring
03579                 numr2i = numr(2,i);   // The beginning point of this ring
03580 
03581                 t1   = circ1(numr2i) * circ2(numr2i);
03582                 q(1) += t1;
03583                 t(1) += t1;
03584 
03585                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03586                 if (numr3i == maxrin)  {
03587                         q(2) += t1;
03588                         t(2) += t1;
03589                 } else {
03590                         q(numr3i+1) += t1;
03591                         t(numr3i+1) += t1;
03592                 }
03593 
03594                 for (j=3; j<=numr3i; j += 2) {
03595                         jc     = j+numr2i-1;
03596 
03597 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03598 //                                ----- -----    ----- -----
03599 //                                 t1     t2      t3    t4
03600 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03601 //                                    ----- -----    ----- -----
03602 //                                     t1    t2       t3    t4
03603 
03604                         c1     = circ1(jc);
03605                         c2     = circ1(jc+1);
03606                         d1     = circ2(jc);
03607                         d2     = circ2(jc+1);
03608 
03609                         t1     = c1 * d1;
03610                         t2     = c2 * d2;
03611                         t3     = c1 * d2;
03612                         t4     = c2 * d1;
03613 
03614                         q(j)   += t1 + t2;
03615                         q(j+1) += -t3 + t4;
03616                         t(j)   += t1 - t2;
03617                         t(j+1) += -t3 - t4;
03618                 }
03619         }
03620         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03621         fftr_d(q,ip);
03622 
03623         qn  = -1.0e20;
03624 
03625         int jstart = 1+static_cast<int>(delta_start/360.0*maxrin);
03626         int jstep = static_cast<int>(delta/360.0*maxrin);
03627         if (jstep < 1) { jstep = 1; }
03628 
03629         for (j=jstart; j<=maxrin; j+=jstep) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
03630                 if (q(j) >= qn) {
03631                         qn  = q(j);
03632                         jtot = j;
03633                 }
03634         }
03635 
03636         //for (k=-3; k<=3; k++) {
03637         //      j = ((jtot+k+maxrin-1)%maxrin)+1;
03638         //      t7(k+4) = q(j);
03639         //}
03640 
03641         // interpolate
03642         //prb1d(t7,7,&pos);
03643         //tot = (float)(jtot)+pos;
03644         // Do not interpolate
03645         tot = (float)(jtot);
03646 
03647         // mirrored
03648         fftr_d(t,ip);
03649 
03650         // find angle
03651         qm = -1.0e20;
03652         for (j=jstart; j<=maxrin;j+=jstep) {//cout <<"  "<<j<<" "<<t(j) <<endl;
03653                 if ( t(j) >= qm ) {
03654                         qm   = t(j);
03655                         jtot = j;
03656                 }
03657         }
03658 
03659         //for (k=-3; k<=3; k++)  {
03660         //      j = ((jtot+k+maxrin-1)%maxrin) + 1;
03661         //      t7(k+4) = t(j);
03662         //}
03663 
03664         // interpolate
03665 
03666         //prb1d(t7,7,&pos);
03667         //tmt = float(jtot) + pos;
03668         // Do not interpolate
03669         tmt = float(jtot);
03670 
03671         free(t);
03672         free(q);
03673 
03674         Dict retvals;
03675         retvals["qn"] = qn;
03676         retvals["tot"] = tot;
03677         retvals["qm"] = qm;
03678         retvals["tmt"] = tmt;
03679         return retvals;
03680 }
03681 
03682 
03683 Dict Util::Crosrng_psi_0_180(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi_max) {
03684         int nring = numr.size()/3;
03685         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03686         int maxrin = numr[numr.size()-1];
03687         double qn; float tot; double qm; float tmt;
03688         float *circ1 = circ1p->get_data();
03689         float *circ2 = circ2p->get_data();
03690 
03691         // dimension             circ1(lcirc),circ2(lcirc)
03692 
03693         // t(maxrin), q(maxrin), t7(-3:3)  //maxrin+2 removed
03694         double *t, *q, t7[7];
03695 
03696         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03697         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03698 
03699         qn  = 0.0f;
03700         qm  = 0.0f;
03701         tot = 0.0f;
03702         tmt = 0.0f;
03703 #ifdef _WIN32
03704         ip = -(int)(log((float)maxrin)/log(2.0f));
03705 #else
03706         ip = -(int)(log2(maxrin));
03707 #endif  //_WIN32
03708   //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
03709 
03710         //  c - straight  = circ1 * conjg(circ2)
03711         //  zero q array
03712 
03713         q = (double*)calloc(maxrin,sizeof(double));
03714 
03715         //   t - mirrored  = conjg(circ1) * conjg(circ2)
03716         //   zero t array
03717         t = (double*)calloc(maxrin,sizeof(double));
03718 
03719    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03720         for (i=1; i<=nring; i++) {
03721 
03722                 numr3i = numr(3,i);   // Number of samples of this ring
03723                 numr2i = numr(2,i);   // The beginning point of this ring
03724 
03725                 t1   = circ1(numr2i) * circ2(numr2i);
03726                 q(1) += t1;
03727                 t(1) += t1;
03728 
03729                 t1   = circ1(numr2i+1) * circ2(numr2i+1);
03730                 if (numr3i == maxrin)  {
03731                         q(2) += t1;
03732                         t(2) += t1;
03733                 } else {
03734                         q(numr3i+1) += t1;
03735                         t(numr3i+1) += t1;
03736                 }
03737 
03738                 for (j=3; j<=numr3i; j += 2) {
03739                         jc     = j+numr2i-1;
03740 
03741 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03742 //                                ----- -----    ----- -----
03743 //                                 t1     t2      t3    t4
03744 // Here, conj(c1+c2i)*conj(d1+d2i) = (c1*d1-c2*d2)+(-c1*d2-c2*d1)i
03745 //                                    ----- -----    ----- -----
03746 //                                     t1    t2       t3    t4
03747 
03748                         c1     = circ1(jc);
03749                         c2     = circ1(jc+1);
03750                         d1     = circ2(jc);
03751                         d2     = circ2(jc+1);
03752 
03753                         t1     = c1 * d1;
03754                         t2     = c2 * d2;
03755                         t3     = c1 * d2;
03756                         t4     = c2 * d1;
03757 
03758                         q(j)   += t1 + t2;
03759                         q(j+1) += -t3 + t4;
03760                         t(j)   += t1 - t2;
03761                         t(j+1) += -t3 - t4;
03762                 }
03763         }
03764         //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<"   "<<t(j) <<endl;
03765         fftr_d(q,ip);
03766 
03767         int psi_range  = int(psi_max/360.0*maxrin+0.5);
03768         const int psi_0 = 0;
03769         int psi_180    = int(  180.0/360.0*maxrin+0.5);
03770 
03771         qn  = -1.0e20;
03772         for (k=-psi_range; k<=psi_range; k++) {
03773                 j = (k+psi_0+maxrin-1)%maxrin+1;//string modemo = "f";cout <<" 90   "<<j<<"   "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03774                 if (q(j) >= qn) {
03775                         qn  = q(j);
03776                         jtot = j;
03777                 }
03778         }
03779 
03780         for (k=-psi_range; k<=psi_range; k++) {
03781                 j = (k+psi_180+maxrin-1)%maxrin+1; //cout <<" 270  "<<j<<"  "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03782                 if (q(j) >= qn) {
03783                         qn  = q(j);
03784                         jtot = j;
03785                 }
03786         }
03787 
03788         for (k=-3; k<=3; k++) {
03789                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03790                 t7(k+4) = q(j);
03791         }
03792 
03793         // interpolate
03794         prb1d(t7,7,&pos);
03795         tot = (float)(jtot)+pos;
03796         // Do not interpolate
03797         //tot = (float)(jtot);
03798 
03799         // mirrored
03800         fftr_d(t,ip);
03801 
03802         // find angle
03803         qm = -1.0e20;
03804         for (k=-psi_range; k<=psi_range; k++) {
03805                 j = (k+psi_0+maxrin-1)%maxrin+1;//string modemo = "f";cout <<" 90   "<<j<<"   "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03806                 if (t(j) >= qm) {
03807                         qm  = t(j);
03808                         jtot = j;
03809                 }
03810         }
03811 
03812         for (k=-psi_range; k<=psi_range; k++) {
03813                 j = (k+psi_180+maxrin-1)%maxrin+1; //cout <<" 270  "<<j<<"  "<<ang_n(j,modemo,maxrin) <<"  "<<float(j)/maxrin*360.0<<" "<<q(j) <<endl;
03814                 if (t(j) >= qm) {
03815                         qm  = t(j);
03816                         jtot = j;
03817                 }
03818         }
03819 
03820         for (k=-3; k<=3; k++)  {
03821                 j = ((jtot+k+maxrin-1)%maxrin) + 1;
03822                 t7(k+4) = t(j);
03823         }
03824 
03825         // interpolate
03826 
03827         prb1d(t7,7,&pos);
03828         tmt = float(jtot) + pos;
03829         // Do not interpolate
03830         //tmt = float(jtot);
03831 
03832         free(t);
03833         free(q);
03834 
03835         Dict retvals;
03836         retvals["qn"] = qn;
03837         retvals["tot"] = tot;
03838         retvals["qm"] = qm;
03839         retvals["tmt"] = tmt;
03840         return retvals;
03841 }
03842 
03843 
03844 Dict Util::Crosrng_sm_psi(EMData* circ1p, EMData* circ2p, vector<int> numr, float psi, int flag) {
03845 // flag 0 - straignt, 1 - mirror
03846 
03847         int nring = numr.size()/3;
03848         int maxrin = numr[numr.size()-1];
03849         double qn; float tot; double qm; float tmt;
03850         float *circ1 = circ1p->get_data();
03851         float *circ2 = circ2p->get_data();
03852 
03853         double *q, t7[7];
03854 
03855         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
03856         float t1, t2, t3, t4, c1, c2, d1, d2, pos;
03857 
03858         qn  = 0.0f;
03859         qm  = 0.0f;
03860         tot = 0.0f;
03861         tmt = 0.0f;
03862 #ifdef _WIN32
03863         ip = -(int)(log((float)maxrin)/log(2.0f));
03864 #else
03865         ip = -(int)(log2(maxrin));
03866 #endif  //_WIN32
03867 
03868         //  c - straight  = circ1 * conjg(circ2)
03869         //  zero q array
03870 
03871         q = (double*)calloc(maxrin,sizeof(double));
03872 
03873    //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
03874         if (flag==0) {
03875                 for (i=1; i<=nring; i++) {
03876 
03877                         numr3i = numr(3,i);   // Number of samples of this ring
03878                         numr2i = numr(2,i);   // The beginning point of this ring
03879 
03880                         t1   = circ1(numr2i) * circ2(numr2i);
03881                         q(1) += t1;
03882 
03883                         t1   = circ1(numr2i+1) * circ2(numr2i+1);
03884                         if (numr3i == maxrin)  {
03885                                 q(2) += t1;
03886                         } else {
03887                                 q(numr3i+1) += t1;
03888                         }
03889 
03890                         for (j=3; j<=numr3i; j += 2) {
03891                                 jc     = j+numr2i-1;
03892 
03893         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03894         //                                ----- -----    ----- -----
03895         //                                 t1     t2      t3    t4
03896 
03897                                 c1     = circ1(jc);
03898                                 c2     = circ1(jc+1);
03899                                 d1     = circ2(jc);
03900                                 d2     = circ2(jc+1);
03901 
03902                                 t1     = c1 * d1;
03903                                 t3     = c1 * d2;
03904                                 t2     = c2 * d2;
03905                                 t4     = c2 * d1;
03906 
03907                                 q(j)   += t1 + t2;
03908                                 q(j+1) += -t3 + t4;
03909                         }
03910                 }
03911         } else {
03912                 for (i=1; i<=nring; i++) {
03913 
03914                         numr3i = numr(3,i);   // Number of samples of this ring
03915                         numr2i = numr(2,i);   // The beginning point of this ring
03916 
03917                         t1   = circ1(numr2i) * circ2(numr2i);
03918                         q(1) += t1;
03919 
03920                         t1   = circ1(numr2i+1) * circ2(numr2i+1);
03921                         if (numr3i == maxrin)  {
03922                                 q(2) += t1;
03923                         } else {
03924                                 q(numr3i+1) += t1;
03925                         }
03926 
03927                         for (j=3; j<=numr3i; j += 2) {
03928                                 jc     = j+numr2i-1;
03929 
03930         // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
03931         //                                ----- -----    ----- -----
03932         //                                 t1     t2      t3    t4
03933 
03934                                 c1     = circ1(jc);
03935                                 c2     = circ1(jc+1);
03936                                 d1     = circ2(jc);
03937                                 d2     = circ2(jc+1);
03938 
03939                                 t1     = c1 * d1;
03940                                 t3     = c1 * d2;
03941                                 t2     = c2 * d2;
03942                                 t4     = c2 * d1;
03943 
03944                                 q(j)   += t1 - t2;
03945                                 q(j+1) += -t3 - t4;
03946                         }
03947                 }
03948         }
03949         fftr_d(q,ip);
03950 
03951         qn  = -1.0e20;
03952         int psi_pos = int(psi/360.0*maxrin+0.5);
03953 
03954         for (k=-5; k<=5; k++) {
03955                 j = (psi_pos+maxrin-1)%maxrin+1;
03956                 if (q(j) >= qn) {
03957                         qn  = q(j);
03958                         jtot = j;
03959                 }
03960         }
03961 
03962         for (k=-3; k<=3; k++) {
03963                 j = ((jtot+k+maxrin-1)%maxrin)+1;
03964                 t7(k+4) = q(j);
03965         }
03966 
03967         // interpolate
03968         prb1d(t7,7,&pos);
03969         tot = (float)(jtot)+pos;
03970         free(q);
03971 
03972         Dict retvals;
03973         retvals["qn"] = qn;
03974         retvals["tot"] = tot;
03975         return retvals;
03976 }
03977 
03978 Dict Util::Crosrng_ns(EMData* circ1p, EMData* circ2p, vector<int> numr) {
03979         int nring = numr.size()/3;
03980         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
03981         int maxrin = numr[numr.size()-1];
03982         double qn; float tot;
03983         float *circ1 = circ1p->get_data();
03984         float *circ2 = circ2p->get_data();
03985 /*
03986 c
03987 c  checks only straight position
03988 c
03989 c  input - fourier transforms of rings!!
03990 c  circ1 already multiplied by weights!
03991 c
03992 */
03993 
03994         // dimension             circ1(lcirc),circ2(lcirc)
03995 
03996         // q(maxrin), t7(-3:3)  //maxrin+2 removed
03997         double *q, t7[7];
03998 
03999         int   ip, jc, numr3i, numr2i, i, j, k, jtot = 0;
04000         float c1, c2, d1, d2, pos;
04001 
04002         qn  = 0.0;
04003         tot = 0.0;
04004 #ifdef _WIN32
04005         ip = -(int)(log((float)maxrin)/log(2.0f));
04006 #else
04007    ip = -(int)(log2(maxrin));
04008 #endif  //_WIN32
04009         //for (j=1; j<=maxrin;j++) cout <<"  "<<j<<"   "<<circ1(j)<<"   "<<circ2(j) <<endl;
04010 
04011         //  c - straight  = circ1 * conjg(circ2)
04012         //  zero q array
04013 
04014         q = (double*)calloc(maxrin,sizeof(double));
04015 
04016                         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04017         for (i=1; i<=nring; i++) {
04018 
04019                 numr3i = numr(3,i);   // Number of samples of this ring
04020                 numr2i = numr(2,i);   // The beginning point of this ring
04021 
04022                 q(1) += circ1(numr2i) * circ2(numr2i);
04023 
04024                 if (numr3i == maxrin)   q(2) += circ1(numr2i+1) * circ2(numr2i+1);
04025                 else  q(numr3i+1) += circ1(numr2i+1) * circ2(numr2i+1);
04026 
04027                 for (j=3; j<=numr3i; j += 2) {
04028                         jc     = j+numr2i-1;
04029 
04030 // Here, (c1+c2i)*conj(d1+d2i) = (c1*d1+c2*d2)+(-c1*d2+c2*d1)i
04031 //                                ----- -----    ----- -----
04032 //                                 t1     t2      t3    t4
04033 
04034                         c1     = circ1(jc);
04035                         c2     = circ1(jc+1);
04036                         d1     = circ2(jc);
04037                         d2     = circ2(jc+1);
04038 
04039                         q(j)   += c1 * d1 + c2 * d2;
04040                         q(j+1) += -c1 * d2 + c2 * d1;
04041                 }
04042         }
04043 //for (j=1; j<=maxrin; j++) cout <<"  "<<j<<"   "<<q(j) <<endl;
04044         fftr_d(q,ip);
04045 
04046         qn  = -1.0e20;
04047         for (j=1; j<=maxrin; j++) {//cout <<"  "<<j<<"   "<<q(j) <<endl;
04048                 if (q(j) >= qn) {
04049                         qn  = q(j);
04050                         jtot = j;
04051                 }
04052         }
04053 
04054         for (k=-3; k<=3; k++)  {
04055                 j = ((jtot+k+maxrin-1)%maxrin)+1;
04056                 t7(k+4) = q(j);
04057         }
04058 
04059         // interpolate
04060         prb1d(t7,7,&pos);
04061         tot = (float)(jtot)+pos;
04062         // Do not interpolate
04063         //*tot = (float)(jtot);
04064 
04065         free(q);
04066 
04067         Dict retvals;
04068         retvals["qn"] = qn;
04069         retvals["tot"] = tot;
04070         return retvals;
04071 }
04072 
04073 #define  dout(i,j)        dout[i+maxrin*j]
04074 #define  circ1b(i)        circ1b[i-1]
04075 #define  circ2b(i)        circ2b[i-1]
04076 
04077 EMData* Util::Crosrng_msg(EMData* circ1, EMData* circ2, vector<int> numr) {
04078 
04079    // dimension         circ1(lcirc),circ2(lcirc)
04080 
04081         int   ip, jc, numr3i, numr2i, i, j;
04082         float t1, t2, t3, t4, c1, c2, d1, d2;
04083 
04084         int nring = numr.size()/3;
04085         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04086         int maxrin = numr[numr.size()-1];
04087 
04088         float* circ1b = circ1->get_data();
04089         float* circ2b = circ2->get_data();
04090 
04091         // t(maxrin), q(maxrin)  // removed +2
04092         double *t, *q;
04093 
04094         q = (double*)calloc(maxrin,sizeof(double));
04095         t = (double*)calloc(maxrin,sizeof(double));
04096 
04097 #ifdef _WIN32
04098         ip = -(int)(log((float)maxrin)/log(2.0f));
04099 #else
04100         ip = -(int)(log2(maxrin));
04101 #endif  //_WIN32
04102 
04103         //  q - straight  = circ1 * conjg(circ2)
04104 
04105         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04106 
04107         //   premultiply  arrays ie( circ12 = circ1 * circ2) much slower
04108 
04109         for (i=1; i<=nring; i++) {
04110 
04111                 numr3i = numr(3,i);
04112                 numr2i = numr(2,i);
04113 
04114                 t1   = circ1b(numr2i) * circ2b(numr2i);
04115                 q(1) = q(1)+t1;
04116                 t(1) = t(1)+t1;
04117 
04118                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04119                 if (numr3i == maxrin)  {
04120                         q(2) += t1;
04121                         t(2) += t1;
04122                 } else {
04123                         q(numr3i+1) += t1;
04124                         t(numr3i+1) += t1;
04125                 }
04126 
04127                 for (j=3; j<=numr3i; j=j+2) {
04128                         jc     = j+numr2i-1;
04129 
04130                         c1     = circ1b(jc);
04131                         c2     = circ1b(jc+1);
04132                         d1     = circ2b(jc);
04133                         d2     = circ2b(jc+1);
04134 
04135                         t1     = c1 * d1;
04136                         t3     = c1 * d2;
04137                         t2     = c2 * d2;
04138                         t4     = c2 * d1;
04139 
04140                         q(j)   += t1 + t2;
04141                         q(j+1) += - t3 + t4;
04142                         t(j)   += t1 - t2;
04143                         t(j+1) += - t3 - t4;
04144                 }
04145         }
04146 
04147         // straight
04148         fftr_d(q,ip);
04149 
04150         // mirrored
04151         fftr_d(t,ip);
04152 
04153         EMData* out = new EMData();
04154         out->set_size(maxrin,2,1);
04155         float *dout = out->get_data();
04156         for (int i=0; i<maxrin; i++) {dout(i,0)=static_cast<float>(q[i]); dout(i,1)=static_cast<float>(t[i]);}
04157         //out->set_size(maxrin,1,1);
04158         //float *dout = out->get_data();
04159         //for (int i=0; i<maxrin; i++) {dout(i,0)=q[i];}
04160         free(t);
04161         free(q);
04162         return out;
04163 }
04164 
04165 
04166 vector<float> Util::Crosrng_msg_vec_p(EMData* circ1, EMData* circ2, vector<int> numr ) {
04167 
04168         int maxrin = numr[numr.size()-1];
04169 
04170         vector<float> r(2*maxrin);
04171 
04172         Crosrng_msg_vec( circ1, circ2, numr, &r[0], &r[maxrin] );
04173 
04174         return r;
04175 }
04176 
04177 #define  dout(i,j)        dout[i+maxrin*j]
04178 #define  circ1b(i)        circ1b[i-1]
04179 #define  circ2b(i)        circ2b[i-1]
04180 
04181 void Util::Crosrng_msg_vec(EMData* circ1, EMData* circ2, vector<int> numr, float *q, float *t) {
04182 
04183    // dimension         circ1(lcirc),circ2(lcirc)
04184 
04185         int   ip, jc, numr3i, numr2i, i, j;
04186         float t1, t2, t3, t4, c1, c2, d1, d2;
04187 
04188         int nring = numr.size()/3;
04189         //int lcirc = numr[3*nring-2]+numr[3*nring-1]-1;
04190         int maxrin = numr[numr.size()-1];
04191 
04192         float* circ1b = circ1->get_data();
04193         float* circ2b = circ2->get_data();
04194 
04195 #ifdef _WIN32
04196         ip = -(int)(log((float)maxrin)/log(2.0f));
04197 #else
04198         ip = -(int)(log2(maxrin));
04199 #endif  //_WIN32
04200         for (int i=1; i<=maxrin; i++)  {q(i) = 0.0f; t(i) = 0.0f;}
04201 
04202         //  q - straight  = circ1 * conjg(circ2)
04203 
04204         //   t - mirrored  = conjg(circ1) * conjg(circ2)
04205 
04206         for (i=1; i<=nring; i++) {
04207 
04208                 numr3i = numr(3,i);
04209                 numr2i = numr(2,i);
04210 
04211                 t1   = circ1b(numr2i) * circ2b(numr2i);
04212                 q(1) += t1;
04213                 t(1) += t1;
04214 
04215                 t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04216                 if (numr3i == maxrin)  {
04217                         q(2) += t1;
04218                         t(2) += t1;
04219                 } else {
04220                         q(numr3i+1) += t1;
04221                         t(numr3i+1) += t1;
04222                 }
04223 
04224                 for (j=3; j<=numr3i; j=j+2) {
04225                         jc     = j+numr2i-1;
04226 
04227                         c1     = circ1b(jc);
04228                         c2     = circ1b(jc+1);
04229                         d1     = circ2b(jc);
04230                         d2     = circ2b(jc+1);
04231 
04232                         t1     = c1 * d1;
04233                         t3     = c1 * d2;
04234                         t2     = c2 * d2;
04235                         t4     = c2 * d1;
04236 
04237                         q(j)   += t1 + t2;
04238                         q(j+1) += -t3 + t4;
04239                         t(j)   += t1 - t2;
04240                         t(j+1) += -t3 - t4;
04241                 }
04242         }
04243         // straight
04244         fftr_q(q,ip);
04245         //for (int i=0; i<maxrin; i++) cout<<i<<"  B    "<<q[i]<<"       "<<t[i]<<endl;
04246 
04247         // mirrored
04248         fftr_q(t,ip);
04249 }
04250 
04251 
04252 
04253 EMData* Util::Crosrng_msg_s(EMData* circ1, EMData* circ2, vector<int> numr)
04254 {
04255 
04256         int   ip, jc, numr3i, numr2i, i, j;
04257         float t1, t2, t3, t4, c1, c2, d1, d2;
04258 
04259         int nring = numr.size()/3;
04260         int maxrin = numr[numr.size()-1];
04261 
04262         float* circ1b = circ1->get_data();
04263         float* circ2b = circ2->get_data();
04264 
04265         double *q;
04266 
04267         q = (double*)calloc(maxrin,sizeof(double));
04268 
04269 #ifdef _WIN32
04270         ip = -(int)(log((float)maxrin)/log(2.0f));
04271 #else
04272         ip = -(int)(log2(maxrin));
04273 #endif  //_WIN32
04274 
04275          //  q - straight  = circ1 * conjg(circ2)
04276 
04277         for (i=1;i<=nring;i++) {
04278 
04279                 numr3i = numr(3,i);
04280                 numr2i = numr(2,i);
04281 
04282                 t1   = circ1b(numr2i) * circ2b(numr2i);
04283                 q(1) = q(1)+t1;
04284 
04285                 if (numr3i == maxrin)  {
04286                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04287                         q(2) = q(2)+t1;
04288                 } else {
04289                         t1              = circ1b(numr2i+1) * circ2b(numr2i+1);
04290                         q(numr3i+1) = q(numr3i+1)+t1;
04291                 }
04292 
04293                 for (j=3;j<=numr3i;j=j+2) {
04294                         jc     = j+numr2i-1;
04295 
04296                         c1     = circ1b(jc);
04297                         c2     = circ1b(jc+1);
04298                         d1     = circ2b(jc);
04299                         d2     = circ2b(jc+1);
04300 
04301                         t1     = c1 * d1;
04302                         t3     = c1 * d2;
04303                         t2     = c2 * d2;
04304                         t4     = c2 * d1;
04305 
04306                         q(j)   = q(j)   + t1 + t2;
04307                         q(j+1) = q(j+1) - t3 + t4;
04308                 }
04309         }
04310 
04311         // straight
04312         fftr_d(q,ip);
04313 
04314         EMData* out = new EMData();
04315         out->set_size(maxrin,1,1);
04316         float *dout = out->get_data();
04317         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(q[i]);
04318         free(q);
04319         return out;
04320 
04321 }
04322 
04323 
04324 EMData* Util::Crosrng_msg_m(EMData* circ1, EMData* circ2, vector<int> numr)
04325 {
04326 
04327         int   ip, jc, numr3i, numr2i, i, j;
04328         float t1, t2, t3, t4, c1, c2, d1, d2;
04329 
04330         int nring = numr.size()/3;
04331         int maxrin = numr[numr.size()-1];
04332 
04333         float* circ1b = circ1->get_data();
04334         float* circ2b = circ2->get_data();
04335 
04336         double *t;
04337 
04338         t = (double*)calloc(maxrin,sizeof(double));
04339 
04340 #ifdef _WIN32
04341         ip = -(int)(log((float)maxrin)/log(2.0f));
04342 #else
04343         ip = -(int)(log2(maxrin));
04344 #endif  //_WIN32
04345 
04346          //   t - mirrored  = conjg(circ1) * conjg(circ2)
04347 
04348         for (i=1;i<=nring;i++) {
04349 
04350                 numr3i = numr(3,i);
04351                 numr2i = numr(2,i);
04352 
04353                 t1   = circ1b(numr2i) * circ2b(numr2i);
04354                 t(1) = t(1)+t1;
04355 
04356                 if (numr3i == maxrin)  {
04357                         t1   = circ1b(numr2i+1) * circ2b(numr2i+1);
04358                         t(2) = t(2)+t1;
04359                 }
04360 
04361                 for (j=3;j<=numr3i;j=j+2) {
04362                         jc     = j+numr2i-1;
04363 
04364                         c1     = circ1b(jc);
04365                         c2     = circ1b(jc+1);
04366                         d1     = circ2b(jc);
04367                         d2     = circ2b(jc+1);
04368 
04369                         t1     = c1 * d1;
04370                         t3     = c1 * d2;
04371                         t2     = c2 * d2;
04372                         t4     = c2 * d1;
04373 
04374                         t(j)   = t(j)   + t1 - t2;
04375                         t(j+1) = t(j+1) - t3 - t4;
04376                 }
04377         }
04378 
04379         // mirrored
04380         fftr_d(t,ip);
04381 
04382         EMData* out = new EMData();
04383         out->set_size(maxrin,1,1);
04384         float *dout = out->get_data();
04385         for (int i=0; i<maxrin; i++) dout[i]=static_cast<float>(t[i]);
04386         free(t);
04387         return out;
04388 
04389 }
04390 
04391 #undef circ1b
04392 #undef circ2b
04393 #undef dout
04394 
04395 #undef  circ1
04396 #undef  circ2
04397 #undef  t
04398 #undef  q
04399 #undef  b
04400 #undef  t7
04401 
04402 
04403 #define    QUADPI                   3.141592653589793238462643383279502884197
04404 #define    PI2                      2*QUADPI
04405 
04406 float Util::ener(EMData* ave, vector<int> numr) {
04407         ENTERFUNC;
04408         long double ener,en;
04409 
04410         int nring = numr.size()/3;
04411         float *aveptr = ave->get_data();
04412 
04413         ener = 0.0;
04414         for (int i=1; i<=nring; i++) {
04415                 int numr3i = numr(3,i);
04416                 int np     = numr(2,i)-1;
04417                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04418                 en = tq*(aveptr[np]*aveptr[np]+aveptr[np+1]*aveptr[np+1])*0.5;
04419                 for (int j=np+2; j<np+numr3i-1; j++) en += tq*aveptr[j]*aveptr[j];
04420                 ener += en/numr3i;
04421         }
04422         EXITFUNC;
04423         return static_cast<float>(ener);
04424 }
04425 
04426 float Util::ener_tot(const vector<EMData*>& data, vector<int> numr, vector<float> tot) {
04427         ENTERFUNC;
04428         long double ener, en;
04429         float arg, cs, si;
04430 
04431         int nima = data.size();
04432         int nring = numr.size()/3;
04433         int maxrin = numr(3,nring);
04434 
04435         ener = 0.0;
04436         for (int i=1; i<=nring; i++) {
04437                 int numr3i = numr(3,i);
04438                 int np     = numr(2,i)-1;
04439                 float tq = static_cast<float>(PI2*numr(1,i)/numr3i);
04440                 float temp1 = 0.0, temp2 = 0.0;
04441                 for (int kk=0; kk<nima; kk++) {
04442                         float *ptr = data[kk]->get_data();
04443                         temp1 += ptr[np];
04444                         temp2 += static_cast<float>(ptr[np+1]*cos(PI2*(tot[kk]-1.0f)/2.0f*numr3i/maxrin));
04445                 }
04446                 en = tq*(temp1*temp1+temp2*temp2)*0.5;
04447                 for (int j=2; j<numr3i; j+=2) {
04448                         float tempr = 0.0, tempi = 0.0;
04449                         for (int kk=0; kk<nima; kk++) {
04450                                 float *ptr = data[kk]->get_data();
04451                                 arg = static_cast<float>( PI2*(tot[kk]-1.0)*(j/2)/maxrin );
04452                                 cs = cos(arg);
04453                                 si = sin(arg);
04454                                 tempr += ptr[np + j]*cs - ptr[np + j +1]*si;
04455                                 tempi += ptr[np + j]*si + ptr[np + j +1]*cs;
04456                         }
04457                         en += tq*(tempr*tempr+tempi*tempi);
04458                 }
04459                 ener += en/numr3i;
04460         }
04461         EXITFUNC;
04462         return static_cast<float>(ener);
04463 }
04464 
04465 void Util::update_fav (EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04466         int nring = numr.size()/3;
04467         float *ave = avep->get_data();
04468         float *dat = datp->get_data();
04469         int i, j, numr3i, np;
04470         float  arg, cs, si;
04471         int maxrin = numr(3,nring);
04472         if(mirror == 1) { //for mirrored data has to be conjugated
04473                 for (i=1; i<=nring; i++) {
04474                         numr3i = numr(3,i);
04475                         np     = numr(2,i)-1;
04476                         ave[np]   += dat[np];
04477                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04478                         for (j=2; j<numr3i; j=j+2) {
04479                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04480                                 cs = cos(arg);
04481                                 si = sin(arg);
04482                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04483                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04484                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04485                         }
04486                 }
04487         } else {
04488                 for (i=1; i<=nring; i++) {
04489                         numr3i = numr(3,i);
04490                         np     = numr(2,i)-1;
04491                         ave[np]   += dat[np];
04492                         ave[np+1] += static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04493                         for (j=2; j<numr3i; j=j+2) {
04494                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04495                                 cs = cos(arg);
04496                                 si = sin(arg);
04497                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04498                                 ave[np + j]    += dat[np + j]*cs - dat[np + j +1]*si;
04499                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04500                         }
04501                 }
04502         }
04503         avep->update();
04504         EXITFUNC;
04505 }
04506 
04507 void Util::sub_fav(EMData* avep, EMData* datp, float tot, int mirror, vector<int> numr) {
04508         int nring = numr.size()/3;
04509         float *ave = avep->get_data();
04510         float *dat = datp->get_data();
04511         int i, j, numr3i, np;
04512         float  arg, cs, si;
04513         int maxrin = numr(3,nring);
04514         if(mirror == 1) { //for mirrored data has to be conjugated
04515                 for (i=1; i<=nring; i++) {
04516                         numr3i = numr(3,i);
04517                         np     = numr(2,i)-1;
04518                         ave[np]   -= dat[np];
04519                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04520                         for (j=2; j<numr3i; j=j+2) {
04521                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04522                                 cs = cos(arg);
04523                                 si = sin(arg);
04524                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04525                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04526                                 ave[np + j +1] += dat[np + j]*si + dat[np + j +1]*cs;
04527                         }
04528                 }
04529         } else {
04530                 for (i=1; i<=nring; i++) {
04531                         numr3i = numr(3,i);
04532                         np     = numr(2,i)-1;
04533                         ave[np]   -= dat[np];
04534                         ave[np+1] -= static_cast<float>( dat[np+1]*cos(PI2*(tot-1.0f)/2.0f*numr3i/maxrin) );
04535                         for (j=2; j<numr3i; j=j+2) {
04536                                 arg = static_cast<float>( PI2*(tot-1.)*(j/2)/maxrin );
04537                                 cs = cos(arg);
04538                                 si = sin(arg);
04539                                 //complex(data[np + j],data[np + j +1])*complex(cos(arg),sin(arg))
04540                                 ave[np + j]    -= dat[np + j]*cs - dat[np + j +1]*si;
04541                                 ave[np + j +1] -= dat[np + j]*si + dat[np + j +1]*cs;
04542                         }
04543                 }
04544         }
04545         avep->update();
04546         EXITFUNC;
04547 }
04548 
04549 
04550 #undef    QUADPI
04551 #undef    PI2
04552 
04553 #undef  numr
04554 #undef  circ
04555 
04556 
04557 #define QUADPI   3.141592653589793238462643383279502884197
04558 #define PI2      QUADPI*2
04559 #define deg_rad  QUADPI/180.0
04560 #define rad_deg  180.0/QUADPI
04561 
04562 struct ori_t
04563 {
04564     int iphi;
04565     int itht;
04566     int id;
04567 };
04568 
04569 
04570 struct cmpang
04571 {
04572     bool operator()( const ori_t& a, const ori_t& b )
04573     {
04574         if( a.itht != b.itht )
04575         {
04576             return a.itht < b.itht;
04577         }
04578 
04579         return a.iphi < b.iphi;
04580     }
04581 };
04582 
04583 
04584 vector<double> Util::cml_weights(const vector<float>& cml){
04585         static const int NBIN = 100;
04586         int nline=cml.size()/2;
04587         vector<double> weights(nline);
04588 
04589         vector<ori_t> angs(nline);
04590         for( int i=0; i < nline; ++i ) {
04591                 angs[i].iphi = int( NBIN*cml[2*i] );
04592                 angs[i].itht = int( NBIN*cml[2*i+1] );
04593                 if( angs[i].itht == 180*NBIN ) angs[i].itht = 0;
04594                 angs[i].id = i;
04595         }
04596 
04597         //std::cout << "# of angs: " << angs.size() << std::endl;
04598 
04599         std::sort( angs.begin(), angs.end(), cmpang() );
04600 
04601         vector<float> newphi;
04602         vector<float> newtht;
04603         vector< vector<int> > indices;
04604 
04605         int curt_iphi = -1;
04606         int curt_itht = -1;
04607         for(unsigned int i=0 ;i < angs.size(); ++i ) {
04608                 if( angs[i].iphi==curt_iphi && angs[i].itht==curt_itht ) {
04609                         Assert( indices.size() > 0 );
04610                         indices.back().push_back(angs[i].id);
04611                 } else {
04612                         curt_iphi = angs[i].iphi;
04613                         curt_itht = angs[i].itht;
04614 
04615                         newphi.push_back( float(curt_iphi)/NBIN );
04616                         newtht.push_back( float(curt_itht)/NBIN );
04617                         indices.push_back( vector<int>(1,angs[i].id) );
04618                 }
04619         }
04620 
04621         //std::cout << "# of indpendent ang: " << newphi.size() << std::endl;
04622 
04623 
04624         int num_agl = newphi.size();
04625 
04626         if(num_agl>2) {
04627                 vector<double> w=Util::vrdg(newphi, newtht);
04628 
04629                 Assert( w.size()==newphi.size() );
04630                 Assert( indices.size()==newphi.size() );
04631 
04632                 for(unsigned int i=0; i < newphi.size(); ++i ) {
04633                     /*
04634                     std::cout << "phi,tht,w,n: ";
04635                     std::cout << boost::format( "%10.3f" ) % newphi[i] << " ";
04636                     std::cout << boost::format( "%10.3f" ) % newtht[i] << " ";
04637                     std::cout << boost::format( "%8.6f"  ) % w[i] << " ";
04638                     std::cout << indices[i].size() << "(";
04639                     */
04640 
04641                     for(unsigned int j=0; j < indices[i].size(); ++j ) {
04642                             int id = indices[i][j];
04643                             weights[id] = w[i]/indices[i].size();
04644                             //std::cout << id << " ";
04645                     }
04646 
04647                     //std::cout << ")" << std::endl;
04648 
04649                 }
04650         } else {
04651                 cout<<"warning in Util.cml_weights"<<endl;
04652                 double val = PI2/float(nline);
04653                 for(int i=0; i<nline; i++)  weights[i]=val;
04654         }
04655 
04656         return weights;
04657 
04658 }
04659 
04660 /****************************************************
04661  * New code for common-lines
04662  ****************************************************/
04663 
04664 void Util::set_line(EMData* img, int posline, EMData* line, int offset, int length)
04665 {
04666         int i;
04667         int nx=img->get_xsize();
04668         float *img_ptr  = img->get_data();
04669         float *line_ptr = line->get_data();
04670         for (i=0;i<length;i++) img_ptr[nx*posline + i] = line_ptr[offset + i];
04671         img->update();
04672 }
04673 
04674 void Util::cml_prepare_line(EMData* sino, EMData* line, int ilf, int ihf, int pos_line, int nblines){
04675     int j;
04676     int nx = sino->get_xsize();
04677     int i = nx * pos_line;
04678     float r1, r2;
04679     float *line_ptr = line->get_data();
04680     float *sino_ptr = sino->get_data();
04681     for (j=ilf;j<=ihf; j += 2) {
04682         r1 = line_ptr[j];
04683         r2 = line_ptr[j + 1];
04684         sino_ptr[i + j - ilf] = r1;
04685         sino_ptr[i + j - ilf + 1] = r2;
04686         sino_ptr[i + nx * nblines + j - ilf] = r1;
04687         sino_ptr[i + nx * nblines + j - ilf + 1] = -r2;
04688     }
04689     sino->update();
04690 }
04691 
04692 vector<double> Util::cml_init_rot(vector<float> Ori){
04693     int nb_ori = Ori.size() / 4;
04694     int i, ind;
04695     float ph, th, ps;
04696     double cph, cth, cps, sph, sth, sps;
04697     vector<double> Rot(nb_ori*9);
04698     for (i=0; i<nb_ori; ++i){
04699         ind = i*4;
04700         // spider convention phi=psi-90, psi=phi+90
04701         ph = Ori[ind+2]-90;
04702         th = Ori[ind+1];
04703         ps = Ori[ind]+90;
04704         ph *= deg_rad;
04705         th *= deg_rad;
04706         ps *= deg_rad;
04707         // pre-calculate some trigo stuffs
04708         cph = cos(ph);
04709         cth = cos(th);
04710         cps = cos(ps);
04711         sph = sin(ph);
04712         sth = sin(th);
04713         sps = sin(ps);
04714         // fill rotation matrix
04715         ind = i*9;
04716         Rot[ind] = cph*cps-cth*sps*sph;
04717         Rot[ind+1] = cph*sps+cth*cps*sph;
04718         Rot[ind+2] = sth*sph;
04719         Rot[ind+3] = -sph*cps-cth*sps*cph;
04720         Rot[ind+4] = -sph*sps+cth*cps*cph;
04721         Rot[ind+5] = sth*cph;
04722         Rot[ind+6] = sth*sps;
04723         Rot[ind+7] = -sth*cps;
04724         Rot[ind+8] = cth;
04725     }
04726 
04727     return Rot;
04728 }
04729 
04730 vector<float> Util::cml_update_rot(vector<float> Rot, int iprj, float nph, float th, float nps){
04731     float ph, ps;
04732     double cph, cth, cps, sph, sth, sps;
04733     int ind = iprj*9;
04734     // spider convention phi=psi-90, psi=phi+90
04735     ph = nps-90;
04736     ps = nph+90;
04737     ph *= deg_rad;
04738     th *= deg_rad;
04739     ps *= deg_rad;
04740     // pre-calculate some trigo stuffs
04741     cph = cos(ph);
04742     cth = cos(th);
04743     cps = cos(ps);
04744     sph = sin(ph);
04745     sth = sin(th);
04746     sps = sin(ps);
04747     // fill rotation matrix
04748     Rot[ind] = (float)(cph*cps-cth*sps*sph);
04749     Rot[ind+1] = (float)(cph*sps+cth*cps*sph);
04750     Rot[ind+2] = (float)(sth*sph);
04751     Rot[ind+3] = (float)(-sph*cps-cth*sps*cph);
04752     Rot[ind+4] = (float)(-sph*sps+cth*cps*cph);
04753     Rot[ind+5] = (float)(sth*cph);
04754     Rot[ind+6] = (float)(sth*sps);
04755     Rot[ind+7] = (float)(-sth*cps);
04756     Rot[ind+8] = (float)(cth);
04757 
04758     return Rot;
04759 }
04760 
04761 vector<int> Util::cml_line_insino(vector<float> Rot, int i_prj, int n_prj){
04762     vector<int> com(2*(n_prj - 1));
04763     int a = i_prj*9;
04764     int i, b, c;
04765     int n1=0, n2=0;
04766     float vmax = 1 - 1.0e-6f;
04767     double r11, r12, r13, r23, r31, r32, r33;
04768 
04769     c = 0;
04770     for (i=0; i<n_prj; ++i){
04771         if (i!=i_prj){
04772             b = i*9;
04773             // this is equivalent to R = A*B'
04774             r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04775             r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04776             r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04777             r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04778             r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04779             r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04780             r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04781             if (r33 > vmax) {
04782                 n2 = 270;
04783                 n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04784             }
04785             else if (r33 < -vmax) {
04786                 n2 = 270;
04787                 n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04788             } else {
04789                 n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04790                 n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04791                 if (n1 < 0) {n1 += 360;}
04792                 if (n2 <= 0) {n2 = abs(n2);}
04793                 else {n2 = 360 - n2;}
04794             }
04795 
04796             if (n1 >= 360){n1 = n1 % 360;}
04797             if (n2 >= 360){n2 = n2 % 360;}
04798 
04799             // store common-lines
04800             b = c*2;
04801             com[b] = n1;
04802             com[b+1] = n2;
04803             ++c;
04804         }
04805     }
04806 
04807     return com;
04808 
04809 }
04810 
04811 vector<int> Util::cml_line_insino_all(vector<float> Rot, vector<int> seq, int n_prj, int n_lines) {
04812     vector<int> com(2*n_lines);
04813     int a=0, b, c, l;
04814     int n1=0, n2=0, mem=-1;
04815     float vmax = 1 - 1.0e-6f;
04816     double r11, r12, r13, r23, r31, r32, r33;
04817     c = 0;
04818     for (l=0; l<n_lines; ++l){
04819         c = 2*l;
04820         if (seq[c]!=mem){
04821             mem = seq[c];
04822             a = seq[c]*9;
04823         }
04824         b = seq[c+1]*9;
04825 
04826         // this is equivalent to R = A*B'
04827         r11 = Rot[a]*Rot[b]+Rot[a+1]*Rot[b+1]+Rot[a+2]*Rot[b+2];
04828         r12 = Rot[a]*Rot[b+3]+Rot[a+1]*Rot[b+4]+Rot[a+2]*Rot[b+5];
04829         r13 = Rot[a]*Rot[b+6]+Rot[a+1]*Rot[b+7]+Rot[a+2]*Rot[b+8];
04830         r23 = Rot[a+3]*Rot[b+6]+Rot[a+4]*Rot[b+7]+Rot[a+5]*Rot[b+8];
04831         r31 = Rot[a+6]*Rot[b]+Rot[a+7]*Rot[b+1]+Rot[a+8]*Rot[b+2];
04832         r32 = Rot[a+6]*Rot[b+3]+Rot[a+7]*Rot[b+4]+Rot[a+8]*Rot[b+5];
04833         r33 = Rot[a+6]*Rot[b+6]+Rot[a+7]*Rot[b+7]+Rot[a+8]*Rot[b+8];
04834         if (r33 > vmax) {
04835             n2 = 270;
04836             n1 = 270 + nint180((float)(rad_deg*atan2(r12, r11)));
04837         }
04838         else if (r33 < -vmax) {
04839             n2 = 270;
04840             n1 = 270 - nint180((float)(rad_deg*atan2(r12, r11)));
04841         } else {
04842             n2 = nint180((float)(rad_deg*atan2(r31, -r32)));
04843             n1 = nint180((float)(rad_deg*atan2(r13, r23)));
04844             if (n1 < 0) {n1 += 360;}
04845             if (n2 <= 0) {n2 = abs(n2);}
04846             else {n2 = 360 - n2;}
04847         }
04848         if (n1 >= 360){n1 = n1 % 360;}
04849         if (n2 >= 360){n2 = n2 % 360;}
04850 
04851         // store common-lines
04852         com[c] = n1;
04853         com[c+1] = n2;
04854     }
04855 
04856     return com;
04857 
04858 }
04859 
04860 vector<double> Util::cml_line_in3d(vector<float> Ori, vector<int> seq, int nprj, int nlines){
04861     // seq is the pairwise index ij: 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04862     vector<double> cml(2*nlines); // [phi, theta] / line
04863     float ph1, th1;
04864     float ph2, th2;
04865     double nx, ny, nz;
04866     double norm;
04867     double sth1=0, sph1=0, cth1=0, cph1=0;
04868     double sth2, sph2, cth2, cph2;
04869     int l, ind, c;
04870     int mem = -1;
04871     for (l=0; l<nlines; ++l){
04872         c = 2*l;
04873         if (seq[c]!=mem){
04874             mem = seq[c];
04875             ind = 4*seq[c];
04876             ph1 = Ori[ind]*deg_rad;
04877             th1 = Ori[ind+1]*deg_rad;
04878             sth1 = sin(th1);
04879             sph1 = sin(ph1);
04880             cth1 = cos(th1);
04881             cph1 = cos(ph1);
04882         }
04883         ind = 4*seq[c+1];
04884         ph2 = Ori[ind]*deg_rad;
04885         th2 = Ori[ind+1]*deg_rad;
04886         sth2 = sin(th2);
04887         cth2 = cos(th2);
04888         sph2 = sin(ph2);
04889         cph2 = cos(ph2);
04890         // cross product
04891         nx = sth1*cph1*cth2 - cth1*sth2*cph2;
04892         ny = cth1*sth2*sph2 - cth2*sth1*sph1;
04893         nz = sth1*sph1*sth2*cph2 - sth1*cph1*sth2*sph2;
04894         norm = sqrt(nx*nx+ny*ny+nz*nz);
04895         nx /= norm;
04896         ny /= norm;
04897         nz /= norm;
04898         // apply mirror if need
04899         if (nz<0) {nx=-nx; ny=-ny; nz=-nz;}
04900         // compute theta and phi
04901         cml[c+1] = acos(nz);
04902         if (cml[c+1] == 0) {cml[c] = 0;}
04903         else {
04904             cml[c+1] *= rad_deg;
04905             if (cml[c+1] > 89.99) {cml[c+1] = 89.99;} // this fix some pb in Voronoi
04906             cml[c] = rad_deg * atan2(nx, ny);
04907             cml[c] = fmod(360 + cml[c], 360);
04908 
04909         }
04910     }
04911 
04912     return cml;
04913 }
04914 
04915 double Util::cml_disc(const vector<EMData*>& data, vector<int> com, vector<int> seq, vector<float> weights, int n_lines) {
04916     double res = 0;
04917     double buf = 0;
04918     float* line_1;
04919     float* line_2;
04920     int i, n, ind;
04921     int lnlen = data[0]->get_xsize();
04922     for (n=0; n<n_lines; ++n) {
04923         ind = n*2;
04924         line_1 = data[seq[ind]]->get_data() + com[ind] * lnlen;
04925         line_2 = data[seq[ind+1]]->get_data() + com[ind+1] *lnlen;
04926         buf = 0;
04927         for (i=0; i<lnlen; ++i) {
04928             buf += (line_1[i]-line_2[i])*(line_1[i]-line_2[i]);
04929         }
04930         res += buf * weights[n];
04931     }
04932 
04933     return res;
04934 
04935 }
04936 
04937 vector<double> Util::cml_spin_psi(const vector<EMData*>& data, vector<int> com, vector<float> weights, \
04938                                  int iprj, vector<int> iw, int n_psi, int d_psi, int n_prj){
04939     // res: [best_disc, best_ipsi]
04940     // seq: pairwise indexes ij, 0, 1, 0, 2, 0, 3, 1, 2, 1, 3, 2, 3
04941     // iw : index to know where is the weight for the common-lines on the current projection in the all weights, [12, 4, 2, 7]
04942     vector<double> res(2);
04943     int lnlen = data[0]->get_xsize();
04944     int end = 2*(n_prj-1);
04945     double disc, buf, bdisc, tmp;
04946     int n, i, ipsi, ind, bipsi, c;
04947     float* line_1;
04948     float* line_2;
04949     bdisc = 1.0e6;
04950     bipsi = -1;
04951     // loop psi
04952     for(ipsi=0; ipsi<n_psi; ipsi += d_psi) {
04953         // discrepancy
04954         disc = 0;
04955         c = 0;
04956         for (n=0; n<n_prj; ++n) {
04957             if(n!=iprj) {
04958                 ind = 2*c;
04959                 line_1 = data[iprj]->get_data() + com[ind] * lnlen;
04960                 line_2 = data[n]->get_data() + com[ind+1] * lnlen;
04961                 buf = 0;
04962                 for (i=0; i<lnlen; ++i) {
04963                     tmp = line_1[i]-line_2[i];
04964                     buf += tmp*tmp;
04965                 }
04966                 disc += buf * weights[iw[c]];
04967                 ++c;
04968             }
04969         }
04970         // select the best value
04971         if (disc <= bdisc) {
04972             bdisc = disc;
04973             bipsi = ipsi;
04974         }
04975         // update common-lines
04976         for (i=0; i<end; i+=2){
04977             com[i] += d_psi;
04978             if (com[i] >= n_psi) {com[i] = com[i] % n_psi;}
04979         }
04980     }
04981     res[0] = bdisc;
04982     res[1] = float(bipsi);
04983 
04984     return res;
04985 }
04986 
04987 #undef  QUADPI
04988 #undef  PI2
04989 #undef  deg_rad
04990 #undef  rad_deg
04991 
04992 /****************************************************
04993  * END OF NEW CODE FOR COMMON-LINES
04994  ****************************************************/
04995 
04996 // helper function for k-means
04997 Dict Util::min_dist_real(EMData* image, const vector<EMData*>& data) {
04998         ENTERFUNC;
04999 
05000         int nima = data.size();
05001         vector<float> res(nima);
05002         double result = 0.;
05003         double valmin = 1.0e20;
05004         int valpos = -1;
05005 
05006         for (int kk=0; kk<nima; kk++){
05007         result = 0;
05008 
05009         float *y_data = data[kk]->get_data();
05010         float *x_data = image->get_data();
05011         long totsize = image->get_xsize()*image->get_ysize();
05012         for (long i = 0; i < totsize; i++) {
05013             double temp = x_data[i]- y_data[i];
05014             result += temp*temp;
05015         }
05016         result /= totsize;
05017         res[kk] = (float)result;
05018 
05019         if(result<valmin) {valmin = result; valpos = kk;}
05020 
05021         }
05022 
05023         Dict retvals;
05024         retvals["dist"] = res;
05025         retvals["pos"]  = valpos;
05026 
05027         EXITFUNC;
05028         return retvals;
05029 
05030 }
05031 
05032 Dict Util::min_dist_four(EMData* image, const vector<EMData*>& data) {
05033         ENTERFUNC;
05034 
05035         int nima = data.size();
05036         vector<float> res(nima);
05037         double result = 0.;
05038         double valmin = 1.0e20;
05039         int valpos = -1;
05040 
05041         for (int kk=0; kk<nima; kk++){
05042         result = 0;
05043         //validate_input_args(image, data[kk]);
05044 
05045         float *y_data = data[kk]->get_data();
05046         float *x_data = image->get_data();
05047 
05048         // Implemented by PAP  01/09/06 - please do not change.  If in doubts, write/call me.
05049         int nx  = data[kk]->get_xsize();
05050         int ny  = data[kk]->get_ysize();
05051         nx = (nx - 2 + data[kk]->is_fftodd()); // nx is the real-space size of the input image
05052         int lsd2 = (nx + 2 - nx%2) ; // Extended x-dimension of the complex image
05053 
05054         int ixb = 2*((nx+1)%2);
05055         int iyb = ny%2;
05056         int iz = 0;
05057 
05058         for ( int iy = 0; iy <= ny-1; iy++) {
05059             for ( int ix = 2; ix <= lsd2 - 1 - ixb; ix++) {
05060                 int ii = ix + (iy  + iz * ny)* lsd2;
05061                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05062             }
05063         }
05064         for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05065             int ii = (iy  + iz * ny)* lsd2;
05066             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05067             result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05068         }
05069         if(nx%2 == 0) {
05070             for ( int iy = 1; iy <= ny/2-1 + iyb; iy++) {
05071                 int ii = lsd2 - 2 + (iy  + iz * ny)* lsd2;
05072                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05073                 result += (x_data[ii+1] - y_data[ii+1])*double(x_data[ii+1] - y_data[ii+1]);
05074             }
05075 
05076         }
05077         result *= 2;
05078         result += (x_data[0] - y_data[0])*double(x_data[0] - y_data[0]);
05079         if(ny%2 == 0) {
05080             int ii = (ny/2  + iz * ny)* lsd2;
05081             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05082         }
05083         if(nx%2 == 0) {
05084             int ii = lsd2 - 2 + (0  + iz * ny)* lsd2;
05085             result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05086             if(ny%2 == 0) {
05087                 int ii = lsd2 - 2 +(ny/2  + iz * ny)* lsd2;
05088                 result += (x_data[ii] - y_data[ii])*double(x_data[ii] - y_data[ii]);
05089             }
05090         }
05091 
05092         result /= (long int)nx*(long int)ny*(long int)nx*(long int)ny;
05093         res[kk] = (float)result;
05094 
05095         if(result<valmin) {valmin = result; valpos = kk;}
05096 
05097         }
05098 
05099         Dict retvals;
05100         retvals["dist"] = res;
05101         retvals["pos"]  = valpos;
05102 
05103         EXITFUNC;
05104         return retvals;
05105 }
05106 
05107 int Util::k_means_cont_table_(int* group1, int* group2, int* stb, long int s1, long int s2, int flag) {
05108     long int d2 = group2[s2 - 1] - group2[0];
05109     long int p2 = 0;
05110     long int i1 = 0;
05111     long int i2 = 0;
05112     long int max = 0;
05113     long int cont = 0;
05114     long int i = 0;
05115     int stop1 = 0;
05116     int stop2 = 0;
05117 
05118     for (i=0; i<s1; i++) {
05119         p2 = (long int)(s2 * (double)group1[i] / (double)d2);
05120         if (p2 >= s2) {p2 = s2 - 1;}
05121         i1 = p2;
05122         i2 = p2;
05123         max = s2;
05124         if (group1[i] < group2[0] || group1[i] > group2[s2 - 1]) {continue;}
05125 
05126         stop1 = 0;
05127         stop2 = 0;
05128         while (max--) {
05129             if (group1[i] == group2[i1]) {
05130                 if (flag) {stb[cont] = group1[i];}
05131                 cont++;
05132                 break;
05133             }
05134             if (group2[i1] < group1[i]) {stop1=1;}
05135             if (group1[i] == group2[i2]) {
05136                 if (flag) {stb[cont] = group1[i];}
05137                 cont++;
05138                 break;
05139             }
05140             if (group2[i2] > group1[i]) {stop2=1;}
05141             //printf("i1 %li i2 %li    v2 %i v2 %i   stop1 %i stop2 %i\n", i1, i2, group2[i1], group2[i2], stop1, stop2);
05142 
05143             if (stop1 & stop2) {break;}
05144             i1--;
05145             i2++;
05146             if (i1 < 0) {i1 = 0;}
05147             if (i2 >= s2) {i2 = s2 - 1;}
05148         }
05149         //printf("v1: %i    ite: %li   cont: %li\n", group1[i], s2-max, cont);
05150     }
05151 
05152     return cont;
05153 }
05154 
05155 
05156 
05157 #define old_ptr(i,j,k)          old_ptr[i+(j+(k*ny))*nx]
05158 #define new_ptr(iptr,jptr,kptr) new_ptr[iptr+(jptr+(kptr*new_ny))*new_nx]
05159 EMData* Util::decimate(EMData* img, int x_step, int y_step, int z_step)
05160 {
05161         /* Exception Handle */
05162         if (!img) {
05163                 throw NullPointerException("NULL input image");
05164         }
05165         /* ============================== */
05166 
05167         // Get the size of the input image
05168         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05169         /* ============================== */
05170 
05171 
05172         /* Exception Handle */
05173         if ((x_step-1 > nx/2 || y_step-1 > ny/2 || z_step-1 > nz/2) || (x_step-1)<0 || (y_step-1)<0 || (z_step-1)<0)
05174         {
05175                 LOGERR("Parameters for decimation cannot exceed the center of the image.");
05176                 throw ImageDimensionException("Parameters for decimation cannot exceed the center of the image.");
05177         }
05178         /* ============================== */
05179 
05180 
05181         /*    Calculation of the start point */
05182         int new_st_x=(nx/2)%x_step, new_st_y=(ny/2)%y_step, new_st_z=(nz/2)%z_step;
05183         /* ============================*/
05184 
05185 
05186         /* Calculation of the size of the decimated image */
05187         int rx=2*(nx/(2*x_step)), ry=2*(ny/(2*y_step)), rz=2*(nz/(2*z_step));
05188         int r1=int(ceil((nx-(x_step*rx))/(1.f*x_step))), r2=int(ceil((ny-(y_step*ry))/(1.f*y_step)));
05189         int r3=int(ceil((nz-(z_step*rz))/(1.f*z_step)));
05190         if(r1>1){r1=1;}
05191         if(r2>1){r2=1;}
05192         if(r3>1){r3=1;}
05193         int new_nx=rx+r1, new_ny=ry+r2, new_nz=rz+r3;
05194         /* ===========================================*/
05195 
05196 
05197         EMData* img2 = new EMData();
05198         img2->set_size(new_nx,new_ny,new_nz);
05199         float *new_ptr = img2->get_data();
05200         float *old_ptr = img->get_data();
05201         int iptr, jptr, kptr = 0;
05202         for (int k=new_st_z; k<nz; k+=z_step) {jptr=0;
05203                 for (int j=new_st_y; j<ny; j+=y_step) {iptr=0;
05204                         for (int i=new_st_x; i<nx; i+=x_step) {
05205                                 new_ptr(iptr,jptr,kptr) = old_ptr(i,j,k);
05206                         iptr++;}
05207                 jptr++;}
05208         kptr++;}
05209         img2->update();
05210         return img2;
05211 }
05212 #undef old_ptr
05213 #undef new_ptr
05214 
05215 #define inp(i,j,k)  inp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*ny))*nx]
05216 #define outp(i,j,k) outp[i+(j+(k*new_ny))*new_nx]
05217 EMData* Util::window(EMData* img,int new_nx,int new_ny, int new_nz, int x_offset, int y_offset, int z_offset)
05218 {
05219         /* Exception Handle */
05220         if (!img) throw NullPointerException("NULL input image");
05221         /* ============================== */
05222 
05223         // Get the size of the input image
05224         int nx=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
05225         /* ============================== */
05226 
05227         /* Exception Handle */
05228         if(new_nx>nx || new_ny>ny || new_nz>nz)
05229                 throw ImageDimensionException("The size of the windowed image cannot exceed the input image size.");
05230         if((nx/2)-(new_nx/2)+x_offset<0 || (ny/2)-(new_ny/2)+y_offset<0 || (nz/2)-(new_nz/2)+z_offset<0)
05231                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05232         if(x_offset>((nx-(nx/2))-(new_nx-(new_nx/2))) || y_offset>((ny-(ny/2))-(new_ny-(new_ny/2))) || z_offset>((nz-(nz/2))-(new_nz-(new_nz/2))))
05233                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05234         /* ============================== */
05235 
05236         /*    Calculation of the start point */
05237         int  new_st_x = nx/2-new_nx/2 + x_offset,
05238              new_st_y = ny/2-new_ny/2 + y_offset,
05239              new_st_z = nz/2-new_nz/2 + z_offset;
05240         /* ============================== */
05241 
05242         /* Exception Handle */
05243         if (new_st_x<0 || new_st_y<0 || new_st_z<0)   //  WHAT HAPPENS WITH THE END POINT CHECK??  PAP
05244                 throw ImageDimensionException("The offset inconsistent with the input image size.");
05245         /* ============================== */
05246 
05247         EMData* wind = img->copy_head();
05248         wind->set_size(new_nx, new_ny, new_nz);
05249         float *outp=wind->get_data();
05250         float *inp=img->get_data();
05251 
05252         for (int k=0; k<new_nz; k++)
05253                 for(int j=0; j<new_ny; j++)
05254                         for(int i=0; i<new_nx; i++)
05255                                 outp(i,j,k) = inp(i,j,k);
05256         wind->update();
05257         return wind;
05258 }
05259 #undef inp
05260 #undef outp
05261 
05262 #define inp(i,j,k) inp[i+(j+(k*ny))*nx]
05263 #define outp(i,j,k) outp[(i+new_st_x)+((j+new_st_y)+((k+new_st_z)*new_ny))*new_nx]
05264 EMData *Util::pad(EMData* img,int new_nx, int new_ny, int new_nz, int x_offset, int y_offset, int z_offset,char *params)
05265 {
05266         /* Exception Handle */
05267         if (!img)  throw NullPointerException("NULL input image");
05268         /* ============================== */
05269 
05270         // Get the size of the input image
05271         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
05272         /* ============================== */
05273 
05274         /* Exception Handle */
05275         if(new_nx<nx || new_ny<ny || new_nz<nz)
05276                 throw ImageDimensionException("The size of the padded image cannot be lower than the input image size.");
05277         if((new_nx/2)-(nx/2)+x_offset<0 || (new_ny/2)-(ny/2)+y_offset<0 || (new_nz/2)-(nz/2)+z_offset<0)
05278                 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05279         if(x_offset>((new_nx-(new_nx/2))-(nx-(nx/2))) || y_offset>((new_ny-(new_ny/2))-(ny-(ny/2))) || z_offset>((new_nz-(new_nz/2))-(nz-(nz/2))))
05280                 throw ImageDimensionException("The offset imconsistent with the input image size. Solution: Change the offset parameters");
05281         /* ============================== */
05282 
05283         EMData* pading = img->copy_head();
05284         pading->set_size(new_nx, new_ny, new_nz);
05285         float *inp  = img->get_data();
05286         float *outp = pading->get_data();
05287 
05288 
05289         /* Calculation of the average and the circumference values for background substitution
05290         =======================================================================================*/
05291         float background;
05292 
05293         if (strcmp(params,"average")==0) background = img->get_attr("mean");
05294         else if (strcmp(params,"circumference")==0) {
05295                 float sum1=0.0f;
05296                 int cnt=0;
05297                 for(int i=0;i<nx;i++) {
05298                         sum1 += inp(i,0,0) + inp(i,ny-1,nz-1);
05299                         cnt+=2;
05300                 }
05301                 if(nz-1 == 0) {
05302                         for (int j=1;j<ny-1;j++) {
05303                                 sum1 += inp(1,j,0) + inp(nx-1,j,0);
05304                                 cnt+=2;
05305                         }
05306                 } else {
05307                         for (int k=1;k<nz-1;k++) {
05308                                 for (int j=1;j<ny-1;j++) {
05309                                         sum1 += inp(1,j,0) + inp(nx-1,j,0);
05310                                         cnt+=2;
05311                                 }
05312                         }
05313                 }
05314                 background = sum1/cnt;
05315         } else {
05316                 background = static_cast<float>( atof( params ) );
05317         }
05318         /*=====================================================================================*/
05319 
05320          /*Initial Padding */
05321         int new_st_x=0,new_st_y=0,new_st_z=0;
05322         for (int k=0;k<new_nz;k++)
05323                 for(int j=0;j<new_ny;j++)
05324                         for (int i=0;i<new_nx;i++)
05325                                 outp(i,j,k)=background;
05326         /*============================== */
05327 
05328         /*    Calculation of the start point */
05329         new_st_x=int((new_nx/2-nx/2)  + x_offset);
05330         new_st_y=int((new_ny/2-ny/2)  + y_offset);
05331         new_st_z=int((new_nz/2-nz/2)  + z_offset);
05332         /* ============================== */
05333 
05334         for (int k=0;k<nz;k++)
05335                 for(int j=0;j<ny;j++)
05336                         for(int i=0;i<nx;i++)
05337                                 outp(i,j,k)=inp(i,j,k);
05338         pading->update();
05339         return pading;
05340 }
05341 #undef inp
05342 #undef outp
05343 //-------------------------------------------------------------------------------------------------------------------------------------------------------------
05344 
05345 void Util::colreverse(float* beg, float* end, int nx) {
05346         float* tmp = new float[nx];
05347         int n = (end - beg)/nx;
05348         int nhalf = n/2;
05349         for (int i = 0; i < nhalf; i++) {
05350                 // swap col i and col n-1-i
05351                 memcpy(tmp, beg+i*nx, nx*sizeof(float));
05352                 memcpy(beg+i*nx, beg+(n-1-i)*nx, nx*sizeof(float));
05353                 memcpy(beg+(n-1-i)*nx, tmp, nx*sizeof(float));
05354         }
05355         delete[] tmp;
05356 }
05357 
05358 void Util::slicereverse(float *beg, float *end, int nx,int ny)
05359 {
05360         int nxy = nx*ny;
05361         colreverse(beg, end, nxy);
05362 }
05363 
05364 
05365 void Util::cyclicshift(EMData *image, Dict params) {
05366 
05367         if (image->is_complex()) throw ImageFormatException("Real image required for IntegerCyclicShift2DProcessor");
05368 
05369         int dx = params["dx"];
05370         int dy = params["dy"];
05371         int dz = params["dz"];
05372 
05373         // The reverse trick we're using shifts to the left (a negative shift)
05374         int nx = image->get_xsize();
05375         dx %= nx;
05376         if (dx < 0) dx += nx;
05377         int ny = image->get_ysize();
05378         dy %= ny;
05379         if (dy < 0) dy += ny;
05380         int nz = image->get_zsize();
05381         dz %= nz;
05382         if (dz < 0) dz += nz;
05383 
05384         int mx = -(dx - nx);
05385         int my = -(dy - ny);
05386         int mz = -(dz - nz);
05387 
05388         float* data = image->get_data();
05389         // x-reverses
05390         if (mx != 0) {
05391                 for (int iz = 0; iz < nz; iz++)
05392                        for (int iy = 0; iy < ny; iy++) {
05393                                 // reverses for column iy
05394                                 int offset = nx*iy + nx*ny*iz; // starting location for column iy in slice iz
05395                                 reverse(&data[offset],&data[offset+mx]);
05396                                 reverse(&data[offset+mx],&data[offset+nx]);
05397                                 reverse(&data[offset],&data[offset+nx]);
05398                         }
05399         }
05400         // y-reverses
05401         if (my != 0) {
05402                 for (int iz = 0; iz < nz; iz++) {
05403                         int offset = nx*ny*iz;
05404                         colreverse(&data[offset], &data[offset + my*nx], nx);
05405                         colreverse(&data[offset + my*nx], &data[offset + ny*nx], nx);
05406                         colreverse(&data[offset], &data[offset + ny*nx], nx);
05407                 }
05408         }
05409         if (mz != 0) {
05410                 slicereverse(&data[0], &data[mz*ny*nx], nx, ny);
05411                 slicereverse(&data[mz*ny*nx], &data[nz*ny*nx], nx, ny);
05412                 slicereverse(&data[0], &data[nz*ny*nx], nx ,ny);
05413         }
05414         image->update();
05415 }
05416 
05417 //-----------------------------------------------------------------------------------------------------------------------
05418 
05419 
05420 vector<float> Util::histogram(EMData* image, EMData* mask, int nbins, float hmin, float hmax)
05421 {
05422         if (image->is_complex())
05423                 throw ImageFormatException("Cannot do histogram on Fourier image");
05424         //float hmax, hmin;
05425         float *imageptr=0, *maskptr=0;
05426         int nx=image->get_xsize();
05427         int ny=image->get_ysize();
05428         int nz=image->get_zsize();
05429 
05430         if(mask != NULL){
05431                 if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
05432                         throw ImageDimensionException("The size of mask image should be of same size as the input image");
05433                 maskptr =mask->get_data();
05434         }
05435         if( nbins == 0) nbins = nx;
05436         vector <float> freq(2*nbins, 0.0);
05437 
05438         imageptr=image->get_data();
05439         if( hmin == hmax ) {
05440                 if(mask == NULL) {
05441                         hmax = image->get_attr("maximum");
05442                         hmin = image->get_attr("minimum");
05443                 } else {
05444                         bool  First = true;
05445                         for (int i = 0;i < nx*ny*nz; i++) {
05446                         if (maskptr[i]>=0.5f) {
05447                                         if(First) {
05448                                                 hmax = imageptr[i];
05449                                                 hmin = imageptr[i];
05450                                                 First = false;
05451                                         } else {
05452                                                 hmax = (hmax < imageptr[i])?imageptr[i]:hmax;
05453                                                 hmin = (hmin > imageptr[i])?imageptr[i]:hmin;
05454                                         }
05455                                 }
05456                         }
05457                 }
05458         }
05459         float hdiff = hmax - hmin;
05460         float ff = (nbins-1)/hdiff;
05461         for (int i = 0; i < nbins; i++) freq[nbins+i] = hmin + (float(i)+0.5f)/ff;
05462         if(mask == NULL) {
05463                 for(int i = 0; i < nx*ny*nz; i++) {
05464                         int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05465                         if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05466                 }
05467         } else {
05468                 for(int i = 0; i < nx*ny*nz; i++) {
05469                         if(maskptr[i] >= 0.5) {
05470                                 int jbin = static_cast<int>((imageptr[i]-hmin)*ff + 1.5);
05471                                 if(jbin >= 1 && jbin <= nbins)  freq[jbin-1] += 1.0;
05472                         }
05473                 }
05474         }
05475         return freq;
05476 }
05477 
05478 Dict Util::histc(EMData *ref,EMData *img, EMData *mask)
05479 {
05480         /* Exception Handle */
05481         if (img->is_complex() || ref->is_complex())
05482                 throw ImageFormatException("Cannot do Histogram on Fourier Image");
05483 
05484         if(mask != NULL){
05485                 if(img->get_xsize() != mask->get_xsize() || img->get_ysize() != mask->get_ysize() || img->get_zsize() != mask->get_zsize())
05486                         throw ImageDimensionException("The size of mask image should be of same size as the input image"); }
05487         /* ===================================================== */
05488 
05489         /* Image size calculation */
05490         int size_ref = ((ref->get_xsize())*(ref->get_ysize())*(ref->get_zsize()));
05491         int size_img = ((img->get_xsize())*(img->get_ysize())*(img->get_zsize()));
05492         /* ===================================================== */
05493 
05494         /* The reference image attributes */
05495         float *ref_ptr = ref->get_data();
05496         float ref_h_min = ref->get_attr("minimum");
05497         float ref_h_max = ref->get_attr("maximum");
05498         float ref_h_avg = ref->get_attr("mean");
05499         float ref_h_sig = ref->get_attr("sigma");
05500         /* ===================================================== */
05501 
05502         /* Input image under mask attributes */
05503         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05504 
05505         vector<float> img_data = Util::infomask(img, mask);
05506         float img_avg = img_data[0];
05507         float img_sig = img_data[1];
05508 
05509         /* The image under mask -- size calculation */
05510         int cnt=0;
05511         for(int i=0;i<size_img;i++)
05512                 if (mask_ptr[i]>0.5f)
05513                                 cnt++;
05514         /* ===================================================== */
05515 
05516         /* Histogram of reference image calculation */
05517         float ref_h_diff = ref_h_max - ref_h_min;
05518 
05519         #ifdef _WIN32
05520                 int hist_len = _cpp_min((int)size_ref/16,_cpp_min((int)size_img/16,256));
05521         #else
05522                 int hist_len = std::min((int)size_ref/16,std::min((int)size_img/16,256));
05523         #endif  //_WIN32
05524 
05525         float *ref_freq_bin = new float[3*hist_len];
05526 
05527         //initialize value in each bin to zero
05528         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] = 0.f;
05529 
05530         for (int i = 0;i < size_ref;i++) {
05531                 int L = static_cast<int>(((ref_ptr[i] - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05532                 ref_freq_bin[L]++;
05533         }
05534         for (int i = 0;i < (3*hist_len);i++) ref_freq_bin[i] *= static_cast<float>(cnt)/static_cast<float>(size_ref);
05535 
05536         //Parameters Calculation (i.e) 'A' x + 'B'
05537         float A = ref_h_sig/img_sig;
05538         float B = ref_h_avg - (A*img_avg);
05539 
05540         vector<float> args;
05541         args.push_back(A);
05542         args.push_back(B);
05543 
05544         vector<float> scale;
05545         scale.push_back(1.e-7f*A);
05546         scale.push_back(-1.e-7f*B);
05547 
05548         vector<float> ref_freq_hist;
05549         for(int i = 0;i < (3*hist_len);i++) ref_freq_hist.push_back((int)ref_freq_bin[i]);
05550 
05551         vector<float> data;
05552         data.push_back(ref_h_diff);
05553         data.push_back(ref_h_min);
05554 
05555         Dict parameter;
05556 
05557         /* Parameters displaying the arguments A & B, and the scaling function and the data's */
05558         parameter["args"] = args;
05559         parameter["scale"]= scale;
05560         parameter["data"] = data;
05561         parameter["ref_freq_bin"] = ref_freq_hist;
05562         parameter["size_img"]=size_img;
05563         parameter["hist_len"]=hist_len;
05564         /* ===================================================== */
05565 
05566         return parameter;
05567 }
05568 
05569 
05570 float Util::hist_comp_freq(float PA,float PB,int size_img, int hist_len, EMData *img, vector<float> ref_freq_hist, EMData *mask, float ref_h_diff, float ref_h_min)
05571 {
05572         float *img_ptr = img->get_data();
05573         float *mask_ptr = (mask == NULL)?img->get_data():mask->get_data();
05574 
05575         int *img_freq_bin = new int[3*hist_len];
05576         for(int i = 0;i < (3*hist_len);i++) img_freq_bin[i] = 0;
05577         for(int i = 0;i < size_img;i++) {
05578                 if(mask_ptr[i] > 0.5f) {
05579                         float img_xn = img_ptr[i]*PA + PB;
05580                         int L = static_cast<int>(((img_xn - ref_h_min)/ref_h_diff) * (hist_len-1) + hist_len+1);
05581                         if(L >= 0 && L < (3*hist_len)) img_freq_bin[L]++;
05582                 }
05583         }
05584         int freq_hist = 0;
05585 
05586         for(int i = 0;i < (3*hist_len);i++) freq_hist += (int)pow((float)((int)ref_freq_hist[i] - (int)img_freq_bin[i]),2.f);
05587         freq_hist = (-freq_hist);
05588         return static_cast<float>(freq_hist);
05589 }
05590 //------------------------------------------------------------------------------------------------------------------------------------------------------------------
05591 #define    QUADPI                       3.141592653589793238462643383279502884197
05592 #define    DGR_TO_RAD                   QUADPI/180
05593 #define    DM(I)                        DM          [I-1]
05594 #define    SS(I)                        SS          [I-1]
05595 Dict Util::CANG(float PHI,float THETA,float PSI)
05596 {
05597         double CPHI,SPHI,CTHE,STHE,CPSI,SPSI;
05598         vector<float>   DM,SS;
05599 
05600         for(int i =0;i<9;i++) DM.push_back(0);
05601 
05602         for(int i =0;i<6;i++) SS.push_back(0);
05603 
05604         CPHI = cos(double(PHI)*DGR_TO_RAD);
05605         SPHI = sin(double(PHI)*DGR_TO_RAD);
05606         CTHE = cos(double(THETA)*DGR_TO_RAD);
05607         STHE = sin(double(THETA)*DGR_TO_RAD);
05608         CPSI = cos(double(PSI)*DGR_TO_RAD);
05609         SPSI = sin(double(PSI)*DGR_TO_RAD);
05610 
05611         SS(1) = float(CPHI);
05612         SS(2) = float(SPHI);
05613         SS(3) = float(CTHE);
05614         SS(4) = float(STHE);
05615         SS(5) = float(CPSI);
05616         SS(6) = float(SPSI);
05617 
05618         DM(1) = float(CPHI*CTHE*CPSI-SPHI*SPSI);
05619         DM(2) = float(SPHI*CTHE*CPSI+CPHI*SPSI);
05620         DM(3) = float(-STHE*CPSI);
05621         DM(4) = float(-CPHI*CTHE*SPSI-SPHI*CPSI);
05622         DM(5) = float(-SPHI*CTHE*SPSI+CPHI*CPSI);
05623         DM(6) = float(STHE*SPSI);
05624         DM(7) = float(STHE*CPHI);
05625         DM(8) = float(STHE*SPHI);
05626         DM(9) = float(CTHE);
05627 
05628         Dict DMnSS;
05629         DMnSS["DM"] = DM;
05630         DMnSS["SS"] = SS;
05631 
05632         return(DMnSS);
05633 }
05634 #undef SS
05635 #undef DM
05636 #undef QUADPI
05637 #undef DGR_TO_RAD
05638 //-----------------------------------------------------------------------------------------------------------------------
05639 #define    DM(I)                        DM[I-1]
05640 #define    B(i,j)                       Bptr[i-1+((j-1)*NSAM)]
05641 #define    CUBE(i,j,k)                  CUBEptr[(i-1)+((j-1)+((k-1)*NY3D))*NX3D]
05642 
05643 void Util::BPCQ(EMData *B,EMData *CUBE, vector<float> DM)
05644 {
05645 
05646         float  *Bptr = B->get_data();
05647         float  *CUBEptr = CUBE->get_data();
05648 
05649         int NSAM,NROW,NX3D,NY3D,NZC,KZ,IQX,IQY,LDPX,LDPY,LDPZ,LDPNMX,LDPNMY,NZ1;
05650         float DIPX,DIPY,XB,YB,XBB,YBB;
05651 
05652         Transform * t = B->get_attr("xform.projection");
05653         Dict d = t->get_params("spider");
05654         if(t) {delete t; t=0;}
05655         //  Unsure about sign of shifts, check later PAP 06/28/09
05656         float x_shift = d[ "tx" ];
05657         float y_shift = d[ "ty" ];
05658         x_shift = -x_shift;
05659         y_shift = -y_shift;
05660 
05661         NSAM = B->get_xsize();
05662         NROW = B->get_ysize();
05663         NX3D = CUBE->get_xsize();
05664         NY3D = CUBE->get_ysize();
05665         NZC  = CUBE->get_zsize();
05666 
05667 
05668         LDPX   = NX3D/2 +1;
05669         LDPY   = NY3D/2 +1;
05670         LDPZ   = NZC/2 +1;
05671         LDPNMX = NSAM/2 +1;
05672         LDPNMY = NROW/2 +1;
05673         NZ1    = 1;
05674 
05675         for(int K=1;K<=NZC;K++) {
05676                 KZ=K-1+NZ1;
05677                 for(int J=1;J<=NY3D;J++) {
05678                         XBB = (1-LDPX)*DM(1)+(J-LDPY)*DM(2)+(KZ-LDPZ)*DM(3);
05679                         YBB = (1-LDPX)*DM(4)+(J-LDPY)*DM(5)+(KZ-LDPZ)*DM(6);
05680                         for(int I=1;I<=NX3D;I++) {
05681                                 XB  = (I-1)*DM(1)+XBB-x_shift;
05682                                 IQX = int(XB+float(LDPNMX));
05683                                 if (IQX <1 || IQX >= NSAM) continue;
05684                                 YB  = (I-1)*DM(4)+YBB-y_shift;
05685                                 IQY = int(YB+float(LDPNMY));
05686                                 if (IQY<1 || IQY>=NROW)  continue;
05687                                 DIPX = XB+LDPNMX-IQX;
05688                                 DIPY = YB+LDPNMY-IQY;
05689 
05690                                 CUBE(I,J,K) = CUBE(I,J,K)+B(IQX,IQY)+DIPY*(B(IQX,IQY+1)-B(IQX,IQY))+DIPX*(B(IQX+1,IQY)-B(IQX,IQY)+DIPY*(B(IQX+1,IQY+1)-B(IQX+1,IQY)-B(IQX,IQY+1)+B(IQX,IQY)));
05691                         }
05692                 }
05693         }
05694 }
05695 
05696 #undef DM
05697 #undef B
05698 #undef CUBE
05699 
05700 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05701 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
05702 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
05703 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
05704 
05705 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05706 {
05707         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05708         float WW,OX,OY;
05709 
05710         NSAM = PROJ->get_xsize();
05711         NROW = PROJ->get_ysize();
05712         int ntotal = NSAM*NROW;
05713         float q = 2.0f;
05714         float qt = 8.0f/q;
05715         //  Fix for padding 2x
05716         int ipad = 1;
05717         NSAM *= ipad;
05718         NROW *= ipad;
05719         NNNN = NSAM+2-(NSAM%2);
05720         int NX2 = NSAM/2;
05721         NR2  = NROW/2;
05722 
05723         NANG = int(SS.size())/6;
05724 
05725         EMData* W = new EMData();
05726         int Wnx = NNNN/2;
05727         W->set_size(Wnx,NROW,1);
05728         W->to_zero();
05729         float *Wptr = W->get_data();
05730         float *PROJptr = PROJ->get_data();
05731         for (L=1; L<=NANG; L++) {
05732                 float  tmp1 = SS(3,K)*SS(4,L)*(SS(1,K)*SS(1,L) + SS(2,K)*SS(2,L)) - SS(3,L)*SS(4,K);
05733                 float  tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) ); 
05734                 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05735                 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05736                 if(OX < 0.0f) {
05737                         OX = -OX;
05738                         OY = -OY;
05739                 }
05740 
05741                 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f ) {
05742                         for(int J=1;J<=NROW;J++) {
05743                                 JY = (J-1);
05744                                 if(JY > NR2) JY -= NROW;
05745 #ifdef _WIN32
05746                                 int xma = _cpp_min(int(0.5f+(q-JY*OY)/OX),NX2);
05747                                 int xmi = _cpp_max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05748 #else
05749                                 int xma = std::min(int(0.5f+(q-JY*OY)/OX),NX2);
05750                                 int xmi = std::max(int((-q-JY*OY)/OX+0.5+NSAM)-NSAM,0);
05751 #endif  //_WIN32
05752                                 if( xmi <= xma) {
05753                                         for(int I=xmi;I<=xma;I++) {
05754                                                 float Y = fabs(OX*I + OY*JY);
05755                                                 W(I+1,J) += exp(-qt*Y*Y);
05756         //cout << " L   "<<L << " I   "<<I << " JY   "<<JY << " ARG   "<<qt*Y*Y <<endl;
05757                                         }
05758                                 }
05759                         }
05760                 } else {
05761                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
05762                 }
05763         }
05764         EMData* proj_in = PROJ;
05765 
05766         PROJ = PROJ->norm_pad( false, ipad);
05767         PROJ->do_fft_inplace();
05768         PROJ->update();
05769         //cout << " x   "<<PROJ->get_xsize() << " y   "<<PROJ->get_ysize() <<endl;
05770         PROJptr = PROJ->get_data();
05771 
05772         float WNRMinv,temp;
05773         float osnr = 1.0f/SNR;
05774         WNRMinv = 1.0f/W(1,1);
05775         for(int J=1;J<=NROW;J++)  {
05776                 JY = J-1;
05777                 if( JY > NR2)  JY -= NROW;
05778                 float sy = JY;
05779                 sy /= NROW;
05780                 sy *= sy;
05781                 for(int I=1;I<=NNNN;I+=2) {
05782                         KX           = (I+1)/2;
05783                         temp         = W(KX,J)*WNRMinv;
05784                         WW           = temp/(temp*temp + osnr);
05785                         // This is supposed to fix fall-off due to Gaussian function in the weighting function
05786                         float sx = KX-1;
05787                         sx /= NSAM;
05788                         WW *= exp(qt*(sy + sx*sx));
05789                         PROJ(I,J)   *= WW;
05790                         PROJ(I+1,J) *= WW;
05791                 }
05792         }
05793         delete W; W = 0;
05794         PROJ->do_ift_inplace();
05795         PROJ->depad();
05796 
05797         float* data_src = PROJ->get_data();
05798         float* data_dst = proj_in->get_data();
05799 
05800         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
05801 
05802         proj_in->update();
05803 
05804         delete PROJ;
05805 }
05806 /*
05807 void Util::WTF(EMData* PROJ,vector<float> SS,float SNR,int K)
05808 {
05809         int NSAM,NROW,NNNN,NR2,L,JY,KX,NANG;
05810         float WW,OX,OY,Y;
05811 
05812         NSAM = PROJ->get_xsize();
05813         NROW = PROJ->get_ysize();
05814         //  Fix for padding 2x
05815         int ntotal = NSAM*NROW;
05816         int ipad = 1;
05817         NSAM *= ipad;
05818         NROW *= ipad;
05819         NNNN = NSAM+2-(NSAM%2);
05820         NR2  = NROW/2;
05821 
05822         NANG = int(SS.size())/6;
05823 
05824         EMData* W = new EMData();
05825         int Wnx = NNNN/2;
05826         W->set_size(Wnx,NROW,1);
05827         W->to_zero();
05828         float *Wptr = W->get_data();
05829         float *PROJptr = PROJ->get_data();
05830         for (L=1; L<=NANG; L++) {
05831                 float  tmp1 = SS(3,K)*SS(4,L)*(SS(1,K)*SS(1,L) + SS(2,K)*SS(2,L)) - SS(3,L)*SS(4,K);
05832                 float  tmp2 = SS(4,L)*( SS(1,K)*SS(2,L) - SS(1,L)*SS(2,K) ); 
05833                 OX = SS(6,K)*tmp2 + SS(5,K)*tmp1;
05834                 OY = SS(5,K)*tmp2 - SS(6,K)*tmp1;
05835         //cout << " OX   "<<OX << " OY   "<<OY <<endl;
05836 
05837                 if( fabs(OX) > 1.0e-6f || fabs(OY) > 1.0e6f) {
05838                         for(int J=1;J<=NROW;J++) {
05839                                 JY = (J-1);
05840                                 if(JY > NR2) JY=JY-NROW;
05841                                 for(int I=1;I<=NNNN/2;I++) {
05842                                         Y =  fabs(OX * (I-1) + OY * JY);
05843                                         if(Y < 2.0f) {
05844                                         W(I,J) += exp(-4*Y*Y);
05845         cout << " L   "<<L << " I   "<<I-1 << " JY   "<<JY << " ARG   "<<4*Y*Y<<endl;}
05846                                 }
05847                         }
05848                 } else {
05849                         for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++)  W(I,J) += 1.0f;
05850                 }
05851         }
05852         EMData* proj_in = PROJ;
05853 
05854         PROJ = PROJ->norm_pad( false, ipad);
05855         PROJ->do_fft_inplace();
05856         PROJ->update();
05857         //cout << " x   "<<PROJ->get_xsize() << " y   "<<PROJ->get_ysize() <<endl;
05858         PROJptr = PROJ->get_data();
05859 
05860         float WNRMinv,temp;
05861         float osnr = 1.0f/SNR;
05862         WNRMinv = 1.0f/W(1,1);
05863         for(int J=1;J<=NROW;J++)
05864                 for(int I=1;I<=NNNN;I+=2) {
05865                         KX           = (I+1)/2;
05866                         temp         = W(KX,J)*WNRMinv;
05867                         WW           = temp/(temp*temp + osnr);
05868                         PROJ(I,J)   *= WW;
05869                         PROJ(I+1,J) *= WW;
05870                 }
05871         delete W; W = 0;
05872         PROJ->do_ift_inplace();
05873         PROJ->depad();
05874 
05875         float* data_src = PROJ->get_data();
05876         float* data_dst = proj_in->get_data();
05877 
05878         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
05879 
05880         proj_in->update();
05881 
05882         delete PROJ;
05883 }
05884 */
05885 #undef PROJ
05886 #undef W
05887 #undef SS
05888 //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
05889 #define    W(i,j)                       Wptr        [i-1+((j-1)*Wnx)]
05890 #define    PROJ(i,j)                    PROJptr     [i-1+((j-1)*NNNN)]
05891 #define    SS(I,J)                      SS          [I-1 + (J-1)*6]
05892 #define    RI(i,j)                      RI          [(i-1) + ((j-1)*3)]
05893 #define    CC(i)                        CC          [i-1]
05894 #define    CP(i)                        CP          [i-1]
05895 #define    VP(i)                        VP          [i-1]
05896 #define    VV(i)                        VV          [i-1]
05897 #define    AMAX1(i,j)                   i>j?i:j
05898 #define    AMIN1(i,j)                   i<j?i:j
05899 void Util::WTM(EMData *PROJ,vector<float>SS, int DIAMETER,int NUMP)
05900 {
05901         float rad2deg =(180.0f/3.1415926f);
05902         float deg2rad = (3.1415926f/180.0f);
05903 
05904         int NSAM,NROW,NNNN,NR2,NANG,L,JY;
05905 
05906         NSAM = PROJ->get_xsize();
05907         NROW = PROJ->get_ysize();
05908         NNNN = NSAM+2-(NSAM%2);
05909         NR2  = NROW/2;
05910         NANG = int(SS.size())/6;
05911 
05912         float RI[9];
05913         RI(1,1)=SS(1,NUMP)*SS(3,NUMP)*SS(5,NUMP)-SS(2,NUMP)*SS(6,NUMP);
05914         RI(2,1)=-SS(1,NUMP)*SS(3,NUMP)*SS(6,NUMP)-SS(2,NUMP)*SS(5,NUMP);
05915         RI(3,1)=SS(1,NUMP)*SS(4,NUMP);
05916         RI(1,2)=SS(2,NUMP)*SS(3,NUMP)*SS(5,NUMP)+SS(1,NUMP)*SS(6,NUMP);
05917         RI(2,2)=-SS(2,NUMP)*SS(3,NUMP)*SS(6,NUMP)+SS(1,NUMP)*SS(5,NUMP);
05918         RI(3,2)=SS(2,NUMP)*SS(4,NUMP);
05919         RI(1,3)=-SS(4,NUMP)*SS(5,NUMP);
05920         RI(2,3)=SS(4,NUMP)*SS(6,NUMP);
05921         RI(3,3)=SS(3,NUMP);
05922 
05923         float THICK=static_cast<float>( NSAM)/DIAMETER/2.0f ;
05924 
05925         EMData* W = new EMData();
05926         int Wnx = NNNN/2;
05927         W->set_size(NNNN/2,NROW,1);
05928         W->to_one();
05929         float *Wptr = W->get_data();
05930 
05931         float ALPHA,TMP,FV,RT,FM,CCN,CC[3],CP[2],VP[2],VV[3];
05932 
05933         for (L=1; L<=NANG; L++) {
05934                 if (L != NUMP) {
05935                         CC(1)=SS(2,L)*SS(4,L)*SS(3,NUMP)-SS(3,L)*SS(2,NUMP)*SS(4,NUMP);
05936                         CC(2)=SS(3,L)*SS(1,NUMP)*SS(4,NUMP)-SS(1,L)*SS(4,L)*SS(3,NUMP);
05937                         CC(3)=SS(1,L)*SS(4,L)*SS(2,NUMP)*SS(4,NUMP)-SS(2,L)*SS(4,L)*SS(1,NUMP)*SS(4,NUMP);
05938 
05939                         TMP = sqrt(CC(1)*CC(1) +  CC(2)*CC(2) + CC(3)*CC(3));
05940                         CCN=static_cast<float>( AMAX1( AMIN1(TMP,1.0) ,-1.0) );
05941                         ALPHA=rad2deg*float(asin(CCN));
05942                         if (ALPHA>180.0f) ALPHA=ALPHA-180.0f;
05943                         if (ALPHA>90.0f) ALPHA=180.0f-ALPHA;
05944                         if(ALPHA<1.0E-6) {
05945                                 for(int J=1;J<=NROW;J++) for(int I=1;I<=NNNN/2;I++) W(I,J)+=1.0;
05946                         } else {
05947                                 FM=THICK/(fabs(sin(ALPHA*deg2rad)));
05948                                 CC(1)   = CC(1)/CCN;CC(2)   = CC(2)/CCN;CC(3)   = CC(3)/CCN;
05949                                 VV(1)= SS(2,L)*SS(4,L)*CC(3)-SS(3,L)*CC(2);
05950                                 VV(2)= SS(3,L)*CC(1)-SS(1,L)*SS(4,L)*CC(3);
05951                                 VV(3)= SS(1,L)*SS(4,L)*CC(2)-SS(2,L)*SS(4,L)*CC(1);
05952                                 CP(1)   = 0.0;CP(2) = 0.0;
05953                                 VP(1)   = 0.0;VP(2) = 0.0;
05954 
05955                                 CP(1) = CP(1) + RI(1,1)*CC(1) + RI(1,2)*CC(2) + RI(1,3)*CC(3);
05956                                 CP(2) = CP(2) + RI(2,1)*CC(1) + RI(2,2)*CC(2) + RI(2,3)*CC(3);
05957                                 VP(1) = VP(1) + RI(1,1)*VV(1) + RI(1,2)*VV(2) + RI(1,3)*VV(3);
05958                                 VP(2) = VP(2) + RI(2,1)*VV(1) + RI(2,2)*VV(2) + RI(2,3)*VV(3);
05959 
05960                                 TMP = CP(1)*VP(2)-CP(2)*VP(1);
05961 
05962                                 //     PREVENT TMP TO BE TOO SMALL, SIGN IS IRRELEVANT
05963                                 TMP = AMAX1(1.0E-4f,fabs(TMP));
05964                                 float tmpinv = 1.0f/TMP;
05965                                 for(int J=1;J<=NROW;J++) {
05966                                         JY = (J-1);
05967                                         if (JY>NR2)  JY=JY-NROW;
05968                                         for(int I=1;I<=NNNN/2;I++) {
05969                                                 FV     = fabs((JY*CP(1)-(I-1)*CP(2))*tmpinv);
05970                                                 RT     = 1.0f-FV/FM;
05971                                                 W(I,J) += ((RT>0.0f)*RT);
05972                                         }
05973                                 }
05974                         }
05975                 }
05976         }
05977 
05978         EMData* proj_in = PROJ;
05979 
05980         PROJ = PROJ->norm_pad( false, 1);
05981         PROJ->do_fft_inplace();
05982         PROJ->update();
05983         float *PROJptr = PROJ->get_data();
05984 
05985         int KX;
05986         float WW;
05987         for(int J=1; J<=NROW; J++)
05988                 for(int I=1; I<=NNNN; I+=2) {
05989                         KX          =  (I+1)/2;
05990                         WW          =  1.0f/W(KX,J);
05991                         PROJ(I,J)   = PROJ(I,J)*WW;
05992                         PROJ(I+1,J) = PROJ(I+1,J)*WW;
05993                 }
05994         delete W; W = 0;
05995         PROJ->do_ift_inplace();
05996         PROJ->depad();
05997 
05998         float* data_src = PROJ->get_data();
05999         float* data_dst = proj_in->get_data();
06000 
06001         int ntotal = NSAM*NROW;
06002         for( int i=0; i < ntotal; ++i )  data_dst[i] = data_src[i];
06003 
06004         proj_in->update();
06005         delete PROJ;
06006 }
06007 #undef   AMAX1
06008 #undef   AMIN1
06009 #undef   RI
06010 #undef   CC
06011 #undef   CP
06012 #undef   VV
06013 #undef   VP
06014 
06015 
06016 #undef   W
06017 #undef   SS
06018 #undef   PROJ
06019 
06020 float Util::tf(float dzz, float ak, float voltage, float cs, float wgh, float b_factor, float sign)
06021 {
06022         float cst  = cs*1.0e7f;
06023 
06024         wgh /= 100.0;
06025         float phase = atan(wgh/sqrt(1.0f-wgh*wgh));
06026         float lambda=12.398f/sqrt(voltage*(1022.0f+voltage));
06027         float ak2 = ak*ak;
06028         float g1 = dzz*1.0e4f*lambda*ak2;
06029         float g2 = cst*lambda*lambda*lambda*ak2*ak2/2.0f;
06030 
06031         float ctfv = static_cast<float>( sin(M_PI*(g1-g2)+phase)*sign );
06032         if(b_factor != 0.0f)  ctfv *= exp(-b_factor*ak2/4.0f);
06033 
06034         return ctfv;
06035 }
06036 
06037 EMData* Util::compress_image_mask(EMData* image, EMData* mask)
06038 {
06039         /***********
06040         ***get the size of the image for validation purpose
06041         **************/
06042         int nx = image->get_xsize(),ny = image->get_ysize(),nz = image->get_zsize();  //Aren't  these  implied?  Please check and let me know, PAP.
06043         /********
06044         ***Exception Handle
06045         *************/
06046         if(nx != mask->get_xsize() || ny != mask->get_ysize() || nz != mask->get_zsize())
06047                 throw ImageDimensionException("The dimension of the image does not match the dimension of the mask!");
06048 
06049         int i, size = nx*ny*nz;
06050 
06051         float* img_ptr = image->get_data();
06052         float* mask_ptr = mask->get_data();
06053 
06054         int ln=0;  //length of the output image = number of points under the mask.
06055         for(i = 0;i < size;i++) if(mask_ptr[i] > 0.5f) ln++;
06056 
06057         EMData* new_image = new EMData();
06058         new_image->set_size(ln,1,1); /* set size of the new image */
06059         float *new_ptr    = new_image->get_data();
06060 
06061         ln=-1;
06062         for(i = 0;i < size;i++){
06063                 if(mask_ptr[i] > 0.5f) {
06064                         ln++;
06065                         new_ptr[ln]=img_ptr[i];
06066                 }
06067         }
06068 
06069         return new_image;
06070 }
06071 
06072 EMData *Util::reconstitute_image_mask(EMData* image, EMData *mask )
06073 {
06074         /********
06075         ***Exception Handle
06076         *************/
06077         if(mask == NULL)
06078                 throw ImageDimensionException("The mask cannot be an null image");
06079 
06080         /***********
06081         ***get the size of the mask
06082         **************/
06083         int nx = mask->get_xsize(),ny = mask->get_ysize(),nz = mask->get_zsize();
06084 
06085         int i,size = nx*ny*nz;                   /* loop counters */
06086         /* new image declaration */
06087         EMData *new_image = new EMData();
06088         new_image->set_size(nx,ny,nz);           /* set the size of new image */
06089         float *new_ptr  = new_image->get_data(); /* set size of the new image */
06090         float *mask_ptr = mask->get_data();      /* assign a pointer to the mask image */
06091         float *img_ptr  = image->get_data();     /* assign a pointer to the 1D image */
06092         int count = 0;
06093         float sum_under_mask = 0.0 ;
06094         for(i = 0;i < size;i++){
06095                         if(mask_ptr[i] > 0.5f){
06096                                 new_ptr[i] = img_ptr[count];
06097                                 sum_under_mask += img_ptr[count];
06098                                 count++;
06099                                 if( count > image->get_xsize() ) {
06100                                     throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too large");
06101                                 }
06102                         }
06103         }
06104 
06105         if( count > image->get_xsize() ) {
06106             throw ImageDimensionException("Error: in reconstitute_image_mask, the mask doesn't match the image, it is too small");
06107         }
06108 
06109         float avg_under_mask = sum_under_mask / count;
06110         for(i = 0;i < size;i++) {
06111                 if(mask_ptr[i] <= 0.5f)  new_ptr[i] = avg_under_mask;
06112         }
06113         new_image->update();
06114         return new_image;
06115 }
06116 
06117 
06118 
06119 vector<float> Util::merge_peaks(vector<float> peak1, vector<float> peak2,float p_size)
06120 {
06121         vector<float>new_peak;
06122         int n1=peak1.size()/3;
06123         float p_size2=p_size*p_size;
06124         for (int i=0;i<n1;++i) {
06125                 vector<float>::iterator it2= peak1.begin()+3*i;
06126                 bool push_back1=true;
06127                 int n2=peak2.size()/3;
06128                 /*cout<<"peak2 size==="<<n2<<"i====="<<i<<endl;
06129                        cout<<"new peak size==="<<new_peak.size()/3<<endl;*/
06130                 if(n2 ==0) {
06131                         new_peak.push_back(*it2);
06132                         new_peak.push_back(*(it2+1));
06133                         new_peak.push_back(*(it2+2));
06134                 } else  {
06135                         int j=0;
06136                         while (j< n2-1 ) {
06137                                 vector<float>::iterator it3= peak2.begin()+3*j;
06138                                 float d2=((*(it2+1))-(*(it3+1)))*((*(it2+1))-(*(it3+1)))+((*(it2+2))-(*(it3+2)))*((*(it2+2))-(*(it3+2)));
06139                                 if(d2< p_size2 ) {
06140                                         if( (*it2)<(*it3) ) {
06141                                                 new_peak.push_back(*it3);
06142                                                 new_peak.push_back(*(it3+1));
06143                                                 new_peak.push_back(*(it3+2));
06144                                                 peak2.erase(it3);
06145                                                 peak2.erase(it3);
06146                                                 peak2.erase(it3);
06147                                                 push_back1=false;
06148                                         } else {
06149                                                 peak2.erase(it3);
06150                                                 peak2.erase(it3);
06151                                                 peak2.erase(it3);
06152                                         }
06153                                 } else  j=j+1;
06154                                 n2=peak2.size()/3;
06155                         }
06156                         if(push_back1) {
06157                                 new_peak.push_back(*it2);
06158                                 new_peak.push_back(*(it2+1));
06159                                 new_peak.push_back(*(it2+2));
06160                         }
06161                 }
06162         }
06163         return new_peak;
06164 }
06165 
06166 int Util::coveig(int n, float *covmat, float *eigval, float *eigvec)
06167 {
06168         // n size of the covariance/correlation matrix
06169         // covmat --- covariance/correlation matrix (n by n)
06170         // eigval --- returns eigenvalues
06171         // eigvec --- returns eigenvectors
06172 
06173         ENTERFUNC;
06174 
06175         int i;
06176 
06177         // make a copy of covmat so that it will not be overwritten
06178         for ( i = 0 ; i < n * n ; i++ )   eigvec[i] = covmat[i];
06179 
06180         char NEEDV = 'V';
06181         char UPLO = 'U';
06182         int lwork = -1;
06183         int info = 0;
06184         float *work, wsize;
06185 
06186         //  query to get optimal workspace
06187         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, &wsize, &lwork, &info);
06188         lwork = (int)wsize;
06189 
06190         work = (float *)calloc(lwork, sizeof(float));
06191         //  calculate eigs
06192         ssyev_(&NEEDV, &UPLO, &n, eigvec, &n, eigval, work, &lwork, &info);
06193         free(work);
06194         EXITFUNC;
06195         return info;
06196 }
06197 
06198 Dict Util::coveig_for_py(int ncov, const vector<float>& covmatpy)
06199 {
06200 
06201         ENTERFUNC;
06202         int len = covmatpy.size();
06203         float *eigvec;
06204         float *eigval;
06205         float *covmat;
06206         int status = 0;
06207         eigval = (float*)calloc(ncov,sizeof(float));
06208         eigvec = (float*)calloc(ncov*ncov,sizeof(float));
06209         covmat = (float*)calloc(ncov*ncov, sizeof(float));
06210 
06211         const float *covmat_ptr;
06212         covmat_ptr = &covmatpy[0];
06213         for(int i=0;i<len;i++){
06214             covmat[i] = covmat_ptr[i];
06215         }
06216 
06217         status = Util::coveig(ncov, covmat, eigval, eigvec);
06218 
06219         vector<float> eigval_py(ncov);
06220         const float *eigval_ptr;
06221         eigval_ptr = &eigval[0];
06222         for(int i=0;i<ncov;i++){
06223             eigval_py[i] = eigval_ptr[i];
06224         }
06225 
06226         vector<float> eigvec_py(ncov*ncov);
06227         const float *eigvec_ptr;
06228         eigvec_ptr = &eigvec[0];
06229         for(int i=0;i<ncov*ncov;i++){
06230             eigvec_py[i] = eigvec_ptr[i];
06231         }
06232 
06233         Dict res;
06234         res["eigval"] = eigval_py;
06235         res["eigvec"] = eigvec_py;
06236 
06237         EXITFUNC;
06238         return res;
06239 }
06240 
06241 vector<float> Util::pw_extract(vector<float>pw, int n, int iswi, float ps)
06242 {
06243         int k,m,n1,klmd,klm2d,nklmd,n2d,n_larg,l, n2;
06244 
06245         k=(int)pw.size();
06246         l=0;
06247         m=k;
06248         n2=n+2;
06249         n1=n+1;
06250         klmd=k+l+m;
06251         klm2d= k+l+m+2;
06252         nklmd=k+l+m+n;
06253         n2d=n+2;
06254         /*size has to be increased when N is large*/
06255         n_larg=klmd*2;
06256         klm2d=n_larg+klm2d;
06257         klmd=n_larg+klmd;
06258         nklmd=n_larg+nklmd;
06259         int size_q=klm2d*n2d;
06260         int size_cu=nklmd*2;
06261         static int i__;
06262 
06263          double *q ;
06264          double *x ;
06265          double *res;
06266          double *cu;
06267          float *q2;
06268          float *pw_;
06269          long int *iu;
06270          double *s;
06271          q = (double*)calloc(size_q,sizeof(double));
06272          x = (double*)calloc(n2d,sizeof(double));
06273          res = (double*)calloc(klmd,sizeof(double));
06274          cu =(double*)calloc(size_cu,sizeof(double));
06275          s = (double*)calloc(klmd,sizeof(double));
06276          q2 = (float*)calloc(size_q,sizeof(float));
06277          iu = (long int*)calloc(size_cu,sizeof(long int));
06278          pw_ = (float*)calloc(k,sizeof(float));
06279 
06280         for( i__ =0;i__<k;++i__)
06281                 {
06282                 pw_[i__]=log(pw[i__]); }
06283         long int l_k=k;
06284         long int l_n=n;
06285         long int l_iswi=iswi;
06286         vector<float> cl1_res;
06287         cl1_res=Util::call_cl1(&l_k, &l_n, &ps, &l_iswi, pw_, q2, q, x, res, cu, s, iu);
06288         free(q);
06289         free(x);
06290         free(res);
06291         free(s);
06292         free(cu);
06293         free(q2);
06294         free(iu);
06295         free(pw_);
06296         return cl1_res;
06297 }
06298 vector<float> Util::call_cl1(long int *k, long int *n, float *ps, long int *iswi, float *pw, float *q2,double *q, double *x, double *res, double *cu, double *s, long int *iu)
06299 {
06300     long int q2_dim1, q2_offset, q_dim1, q_offset, i__1, i__2;
06301     float r__1;
06302     int tmp__i;
06303     long int i__, j;
06304     --s;
06305     --res;
06306     iu -= 3;
06307     cu -= 3;
06308     --x;
06309     long int klm2d;
06310     klm2d= *k+*k+2;
06311     klm2d=klm2d+klm2d;
06312     q_dim1 = klm2d;
06313     q_offset = 1 + q_dim1;
06314     q -= q_offset;
06315     q2_dim1 = klm2d;
06316     q2_offset = 1 + q2_dim1;
06317     q2 -= q2_offset;
06318     i__2=0;
06319     i__1 = *n - 1;
06320     tmp__i=0;
06321     for (j = 1; j <= i__1; ++j) {
06322         i__2 = *k;
06323         tmp__i+=1;
06324         for (i__ = 1; i__ <= i__2; ++i__) {
06325             r__1 = float(i__ - 1) /(float) *k / (*ps * 2);
06326             q2[i__ + j * q2_dim1] = pow(r__1, tmp__i);
06327         }
06328     }
06329     for  (i__ = 1; i__ <= i__2; ++i__)
06330       { q2[i__ + *n * q2_dim1] = 1.f;
06331             q2[i__ + (*n + 1) * q2_dim1] = pw[i__-1];
06332         }
06333    vector<float> fit_res;
06334    fit_res=Util::lsfit(k, n, &klm2d, iswi, &q2[q2_offset], &q[q_offset], &x[1], &res[1], &cu[3], &s[1], &iu[3]);
06335    return fit_res;
06336 }
06337 vector<float> Util::lsfit(long int *ks,long int *n, long int *klm2d, long int *iswi,float *q1,double *q, double *x, double *res, double *cu, double *s, long int *iu)
06338 {
06339     /* System generated locals */
06340     long int q_dim1, q_offset, q1_dim1, q1_offset, i__1, i__2;
06341 
06342     /* Local variables */
06343     long int i__, j, m, n1, ii, jj;
06344     double tmp;
06345     vector<float> p;
06346     --x;
06347     q_dim1 = *klm2d;
06348     q_offset = 1 + q_dim1;
06349     q -= q_offset;
06350     q1_dim1 = *klm2d;
06351     q1_offset = 1 + q1_dim1;
06352     q1 -= q1_offset;
06353     --s;
06354     --res;
06355     iu -= 3;
06356     cu -= 3;
06357 
06358     /* Function Body */
06359     long int l = 0;
06360 
06361 /* C==ZHONG HUANG,JULY,12,02;L=0,1,2,3,4,5,6 correspond to different equality constraints */
06362     m = *ks;
06363     n1 = *n + 1;
06364     if (*iswi == 1) {
06365         i__1 = n1;
06366         for (jj = 1; jj <= i__1; ++jj) {
06367             i__2 = *ks;
06368             for (ii = 1; ii <= i__2; ++ii) {
06369         /*      q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];*/
06370 
06371                 q[*ks + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1]
06372                         ;
06373             }
06374         }
06375     } else if (*iswi == 2) {
06376         i__1 = *ks;
06377         for (ii = 1; ii <= i__1; ++ii) {
06378             i__2 = n1;
06379             for (jj = 1; jj <= i__2; ++jj) {
06380                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06381                 q[*ks + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06382             }
06383         }
06384     } else if (*iswi == 3) {
06385         l = 2;
06386         i__1 = n1;
06387         for (jj = 1; jj <= i__1; ++jj) {
06388             i__2 = *ks + 2;
06389             for (ii = 1; ii <= i__2; ++ii) {
06390                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06391             }
06392             i__2 = *ks;
06393             for (ii = 1; ii <= i__2; ++ii) {
06394                 q[*ks + 2 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06395             }
06396         }
06397     } else if (*iswi == 4) {
06398         l = 2;
06399         i__1 = n1;
06400         for (jj = 1; jj <= i__1; ++jj) {
06401             i__2 = *ks + 2;
06402             for (ii = 1; ii <= i__2; ++ii) {
06403                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06404             }
06405             i__2 = *ks;
06406             for (ii = 1; ii <= i__2; ++ii) {
06407                 q[*ks + 2 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06408             }
06409         }
06410     } else if (*iswi == 5) {
06411         l = 1;
06412         i__1 = n1;
06413         for (jj = 1; jj <= i__1; ++jj) {
06414             i__2 = *ks + 1;
06415             for (ii = 1; ii <= i__2; ++ii) {
06416                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06417             }
06418             i__2 = *ks;
06419             for (ii = 1; ii <= i__2; ++ii) {
06420                 q[*ks + 1 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06421             }
06422         }
06423     } else if (*iswi == 6) {
06424         l = 1;
06425         i__1 = n1;
06426         for (jj = 1; jj <= i__1; ++jj) {
06427             i__2 = *ks + 1;
06428             for (ii = 1; ii <= i__2; ++ii) {
06429                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06430             }
06431             i__2 = *ks;
06432             for (ii = 1; ii <= i__2; ++ii) {
06433                 q[*ks + 1 + ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06434             }
06435         }
06436     } else if (*iswi == 7) {
06437         l = 3;
06438         i__1 = n1;
06439         for (jj = 1; jj <= i__1; ++jj) {
06440             i__2 = *ks + 3;
06441             for (ii = 1; ii <= i__2; ++ii) {
06442                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06443             }
06444             i__2 = *ks;
06445             for (ii = 1; ii <= i__2; ++ii) {
06446                 q[*ks + 3 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06447             }
06448         }
06449     } else if (*iswi == 8) {
06450         l = 4;
06451         i__1 = n1;
06452         for (jj = 1; jj <= i__1; ++jj) {
06453             i__2 = *ks + 4;
06454             for (ii = 1; ii <= i__2; ++ii) {
06455                 q[ii + jj * q_dim1] = (double) q1[ii + jj * q1_dim1];
06456             }
06457             i__2 = *ks;
06458             for (ii = 1; ii <= i__2; ++ii) {
06459                 q[*ks + 4 + ii + jj * q_dim1] = -((double) q1[ii + jj * q1_dim1]);
06460             }
06461         }
06462     }
06463 
06464     Util::cl1(ks, &l, &m, n, klm2d, &q[q_offset], &x[1], &res[1], &cu[3], &iu[3], &s[1]);
06465     i__1 = *ks;
06466     int tmp__j=0;
06467     for (i__ = 1; i__ <= i__1; ++i__) {
06468         tmp = 0.f;
06469         i__2 = *n - 1;
06470         for (j = 1; j <= i__2; ++j) {
06471         tmp__j=j;
06472             tmp += pow(q1[i__ + q1_dim1], tmp__j) * x[j];
06473         }
06474         tmp += x[*n];
06475         p.push_back(static_cast<float>(exp(tmp)));
06476         p.push_back(q1[i__ + q1_dim1]);
06477     }
06478     i__2=*n;
06479     for (i__=1;i__<=i__2;++i__)
06480         { p.push_back(static_cast<float>(x[i__]));}
06481     return p;
06482 }
06483 void Util::cl1(long int *k, long int *l, long int *m, long int *n, long int *klm2d,
06484         double *q, double *x, double *res, double *cu, long int *iu, double *s)
06485 {
06486 
06487     long int q_dim1, q_offset, i__1, i__2;
06488     double d__1;
06489 
06490     static long int i__, j;
06491     static double z__;
06492     static long int n1, n2, ia, ii, kk, in, nk, js;
06493     static double sn, zu, zv;
06494     static long int nk1, klm, nkl, jmn, jpn;
06495     static double cuv;
06496     static long int klm1, nkl1, klm2, kode, iimn, nklm, iter;
06497     static float xmin;
06498     static double xmax;
06499     static long int iout;
06500     static double xsum;
06501     static long int iineg, maxit;
06502     static double toler;
06503     static float error;
06504     static double pivot;
06505     static long int kforce, iphase;
06506     static double tpivot;
06507 
06508     --s;
06509     --res;
06510     iu -= 3;
06511     cu -= 3;
06512     --x;
06513     q_dim1 = *klm2d;
06514     q_offset = 1 + q_dim1;
06515     q -= q_offset;
06516 
06517     /* Function Body */
06518     maxit = 500;
06519     kode = 0;
06520     toler = 1e-4f;
06521     iter = 0;
06522     n1 = *n + 1;
06523     n2 = *n + 2;
06524     nk = *n + *k;
06525     nk1 = nk + 1;
06526     nkl = nk + *l;
06527     nkl1 = nkl + 1;
06528     klm = *k + *l + *m;
06529     klm1 = klm + 1;
06530     klm2 = klm + 2;
06531     nklm = *n + klm;
06532     kforce = 1;
06533     iter = 0;
06534     js = 1;
06535     ia = 0;
06536 /* SET UP LABELS IN Q. */
06537     i__1 = *n;
06538     for (j = 1; j <= i__1; ++j) {
06539         q[klm2 + j * q_dim1] = (double) j;
06540 /* L10: */
06541     }
06542     i__1 = klm;
06543     for (i__ = 1; i__ <= i__1; ++i__) {
06544         q[i__ + n2 * q_dim1] = (double) (*n + i__);
06545         if (q[i__ + n1 * q_dim1] >= 0.f) {
06546             goto L30;
06547         }
06548         i__2 = n2;
06549         for (j = 1; j <= i__2; ++j) {
06550             q[i__ + j * q_dim1] = -q[i__ + j * q_dim1];
06551 /* L20: */
06552         }
06553 L30:
06554         ;
06555     }
06556 /* SET UP PHASE 1 COSTS. */
06557     iphase = 2;
06558     i__1 = nklm;
06559     for (j = 1; j <= i__1; ++j) {
06560         cu[(j << 1) + 1] = 0.f;
06561         cu[(j << 1) + 2] = 0.f;
06562         iu[(j << 1) + 1] = 0;
06563         iu[(j << 1) + 2] = 0;
06564 /* L40: */
06565     }
06566     if (*l == 0) {
06567         goto L60;
06568     }
06569     i__1 = nkl;
06570     for (j = nk1; j <= i__1; ++j) {
06571         cu[(j << 1) + 1] = 1.f;
06572         cu[(j << 1) + 2] = 1.f;
06573         iu[(j << 1) + 1] = 1;
06574         iu[(j << 1) + 2] = 1;
06575 /* L50: */
06576     }
06577     iphase = 1;
06578 L60:
06579     if (*m == 0) {
06580         goto L80;
06581     }
06582     i__1 = nklm;
06583     for (j = nkl1; j <= i__1; ++j) {
06584         cu[(j << 1) + 2] = 1.f;
06585         iu[(j << 1) + 2] = 1;
06586         jmn = j - *n;
06587         if (q[jmn + n2 * q_dim1] < 0.f) {
06588             iphase = 1;
06589         }
06590 /* L70: */
06591     }
06592 L80:
06593     if (kode == 0) {
06594         goto L150;
06595     }
06596     i__1 = *n;
06597     for (j = 1; j <= i__1; ++j) {
06598         if ((d__1 = x[j]) < 0.) {
06599             goto L90;
06600         } else if (d__1 == 0) {
06601             goto L110;
06602         } else {
06603             goto L100;
06604         }
06605 L90:
06606         cu[(j << 1) + 1] = 1.f;
06607         iu[(j << 1) + 1] = 1;
06608         goto L110;
06609 L100:
06610         cu[(j << 1) + 2] = 1.f;
06611         iu[(j << 1) + 2] = 1;
06612 L110:
06613         ;
06614     }
06615     i__1 = *k;
06616     for (j = 1; j <= i__1; ++j) {
06617         jpn = j + *n;
06618         if ((d__1 = res[j]) < 0.) {
06619             goto L120;
06620         } else if (d__1 == 0) {
06621             goto L140;
06622         } else {
06623             goto L130;
06624         }
06625 L120:
06626         cu[(jpn << 1) + 1] = 1.f;
06627         iu[(jpn << 1) + 1] = 1;
06628         if (q[j + n2 * q_dim1] > 0.f) {
06629             iphase = 1;
06630         }
06631         goto L140;
06632 L130:
06633         cu[(jpn << 1) + 2] = 1.f;
06634         iu[(jpn << 1) + 2] = 1;
06635         if (q[j + n2 * q_dim1] < 0.f) {
06636             iphase = 1;
06637         }
06638 L140:
06639         ;
06640     }
06641 L150:
06642     if (iphase == 2) {
06643         goto L500;
06644     }
06645 /* COMPUTE THE MARGINAL COSTS. */
06646 L160:
06647     i__1 = n1;
06648     for (j = js; j <= i__1; ++j) {
06649         xsum = 0.;
06650         i__2 = klm;
06651         for (i__ = 1; i__ <= i__2; ++i__) {
06652             ii = (long int) q[i__ + n2 * q_dim1];
06653             if (ii < 0) {
06654                 goto L170;
06655             }
06656             z__ = cu[(ii << 1) + 1];
06657             goto L180;
06658 L170:
06659             iineg = -ii;
06660             z__ = cu[(iineg << 1) + 2];
06661 L180:
06662             xsum += q[i__ + j * q_dim1] * z__;
06663 /*  180       XSUM = XSUM + Q(I,J)*Z */
06664 /* L190: */
06665         }
06666         q[klm1 + j * q_dim1] = xsum;
06667 /* L200: */
06668     }
06669     i__1 = *n;
06670     for (j = js; j <= i__1; ++j) {
06671         ii = (long int) q[klm2 + j * q_dim1];
06672         if (ii < 0) {
06673             goto L210;
06674         }
06675         z__ = cu[(ii << 1) + 1];
06676         goto L220;
06677 L210:
06678         iineg = -ii;
06679         z__ = cu[(iineg << 1) + 2];
06680 L220:
06681         q[klm1 + j * q_dim1] -= z__;
06682 /* L230: */
06683     }
06684 /* DETERMINE THE VECTOR TO ENTER THE BASIS. */
06685 L240:
06686     xmax = 0.f;
06687     if (js > *n) {
06688         goto L490;
06689     }
06690     i__1 = *n;
06691     for (j = js; j <= i__1; ++j) {
06692         zu = q[klm1 + j * q_dim1];
06693         ii = (long int) q[klm2 + j * q_dim1];
06694         if (ii > 0) {
06695             goto L250;
06696         }
06697         ii = -ii;
06698         zv = zu;
06699         zu = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06700         goto L260;
06701 L250:
06702         zv = -zu - cu[(ii << 1) + 1] - cu[(ii << 1) + 2];
06703 L260:
06704         if (kforce == 1 && ii > *n) {
06705             goto L280;
06706         }
06707         if (iu[(ii << 1) + 1] == 1) {
06708             goto L270;
06709         }
06710         if (zu <= xmax) {
06711             goto L270;
06712         }
06713         xmax = zu;
06714         in = j;
06715 L270:
06716         if (iu[(ii << 1) + 2] == 1) {
06717             goto L280;
06718         }
06719         if (zv <= xmax) {
06720             goto L280;
06721         }
06722         xmax = zv;
06723         in = j;
06724 L280:
06725         ;
06726     }
06727     if (xmax <= toler) {
06728         goto L490;
06729     }
06730     if (q[klm1 + in * q_dim1] == xmax) {
06731         goto L300;
06732     }
06733     i__1 = klm2;
06734     for (i__ = 1; i__ <= i__1; ++i__) {
06735         q[i__ + in * q_dim1] = -q[i__ + in * q_dim1];
06736 /* L290: */
06737     }
06738     q[klm1 + in * q_dim1] = xmax;
06739 /* DETERMINE THE VECTOR TO LEAVE THE BASIS. */
06740 L300:
06741     if (iphase == 1 || ia == 0) {
06742         goto L330;
06743     }
06744     xmax = 0.f;
06745     i__1 = ia;
06746     for (i__ = 1; i__ <= i__1; ++i__) {
06747         z__ = (d__1 = q[i__ + in * q_dim1], abs(d__1));
06748         if (z__ <= xmax) {
06749             goto L310;
06750         }
06751         xmax = z__;
06752         iout = i__;
06753 L310:
06754         ;
06755     }
06756     if (xmax <= toler) {
06757         goto L330;
06758     }
06759     i__1 = n2;
06760     for (j = 1; j <= i__1; ++j) {
06761         z__ = q[ia + j * q_dim1];
06762         q[ia + j * q_dim1] = q[iout + j * q_dim1];
06763         q[iout + j * q_dim1] = z__;
06764 /* L320: */
06765     }
06766     iout = ia;
06767     --ia;
06768     pivot = q[iout + in * q_dim1];
06769     goto L420;
06770 L330:
06771     kk = 0;
06772     i__1 = klm;
06773     for (i__ = 1; i__ <= i__1; ++i__) {
06774         z__ = q[i__ + in * q_dim1];
06775         if (z__ <= toler) {
06776             goto L340;
06777         }
06778         ++kk;
06779         res[kk] = q[i__ + n1 * q_dim1] / z__;
06780         s[kk] = (double) i__;
06781 L340:
06782         ;
06783     }
06784 L350:
06785     if (kk > 0) {
06786         goto L360;
06787     }
06788     kode = 2;
06789     goto L590;
06790 L360:
06791     xmin = static_cast<float>( res[1] );
06792     iout = (long int) s[1];
06793     j = 1;
06794     if (kk == 1) {
06795         goto L380;
06796     }
06797     i__1 = kk;
06798     for (i__ = 2; i__ <= i__1; ++i__) {
06799         if (res[i__] >= xmin) {
06800             goto L370;
06801         }
06802         j = i__;
06803         xmin = static_cast<float>( res[i__] );
06804         iout = (long int) s[i__];
06805 L370:
06806         ;
06807     }
06808     res[j] = res[kk];
06809     s[j] = s[kk];
06810 L380:
06811     --kk;
06812     pivot = q[iout + in * q_dim1];
06813     ii = (long int) q[iout + n2 * q_dim1];
06814     if (iphase == 1) {
06815         goto L400;
06816     }
06817     if (ii < 0) {
06818         goto L390;
06819     }
06820     if (iu[(ii << 1) + 2] == 1) {
06821         goto L420;
06822     }
06823     goto L400;
06824 L390:
06825     iineg = -ii;
06826     if (iu[(iineg << 1) + 1] == 1) {
06827         goto L420;
06828     }
06829 /* 400 II = IABS(II) */
06830 L400:
06831     ii = abs(ii);
06832     cuv = cu[(ii << 1) + 1] + cu[(ii << 1) + 2];
06833     if (q[klm1 + in * q_dim1] - pivot * cuv <= toler) {
06834         goto L420;
06835     }
06836 /* BYPASS INTERMEDIATE VERTICES. */
06837     i__1 = n1;
06838     for (j = js; j <= i__1; ++j) {
06839         z__ = q[iout + j * q_dim1];
06840         q[klm1 + j * q_dim1] -= z__ * cuv;
06841         q[iout + j * q_dim1] = -z__;
06842 /* L410: */
06843     }
06844     q[iout + n2 * q_dim1] = -q[iout + n2 * q_dim1];
06845     goto L350;
06846 /* GAUSS-JORDAN ELIMINATION. */
06847 L420:
06848     if (iter < maxit) {
06849         goto L430;
06850     }
06851     kode = 3;
06852     goto L590;
06853 L430:
06854     ++iter;
06855     i__1 = n1;
06856     for (j = js; j <= i__1; ++j) {
06857         if (j != in) {
06858             q[iout + j * q_dim1] /= pivot;
06859         }
06860 /* L440: */
06861     }
06862 /* IF PERMITTED, USE SUBROUTINE COL OF THE DESCRIPTION */
06863 /* SECTION AND REPLACE THE FOLLOWING SEVEN STATEMENTS DOWN */
06864 /* TO AND INCLUDING STATEMENT NUMBER 460 BY.. */
06865 /*     DO 460 J=JS,N1 */
06866 /*        IF(J .EQ. IN) GO TO 460 */
06867 /*        Z = -Q(IOUT,J) */
06868 /*        CALL COL(Q(1,J), Q(1,IN), Z, IOUT, KLM1) */
06869 /* 460 CONTINUE */
06870     i__1 = n1;
06871     for (j = js; j <= i__1; ++j) {
06872         if (j == in) {
06873             goto L460;
06874         }
06875         z__ = -q[iout + j * q_dim1];
06876         i__2 = klm1;
06877         for (i__ = 1; i__ <= i__2; ++i__) {
06878             if (i__ != iout) {
06879                 q[i__ + j * q_dim1] += z__ * q[i__ + in * q_dim1];
06880             }
06881 /* L450: */
06882         }
06883 L460:
06884         ;
06885     }
06886     tpivot = -pivot;
06887     i__1 = klm1;
06888     for (i__ = 1; i__ <= i__1; ++i__) {
06889         if (i__ != iout) {
06890             q[i__ + in * q_dim1] /= tpivot;
06891         }
06892 /* L470: */
06893     }
06894     q[iout + in * q_dim1] = 1.f / pivot;
06895     z__ = q[iout + n2 * q_dim1];
06896     q[iout + n2 * q_dim1] = q[klm2 + in * q_dim1];
06897     q[klm2 + in * q_dim1] = z__;
06898     ii = (long int) abs(z__);
06899     if (iu[(ii << 1) + 1] == 0 || iu[(ii << 1) + 2] == 0) {
06900         goto L240;
06901     }
06902     i__1 = klm2;
06903     for (i__ = 1; i__ <= i__1; ++i__) {
06904         z__ = q[i__ + in * q_dim1];
06905         q[i__ + in * q_dim1] = q[i__ + js * q_dim1];
06906         q[i__ + js * q_dim1] = z__;
06907 /* L480: */
06908     }
06909     ++js;
06910     goto L240;
06911 /* TEST FOR OPTIMALITY. */
06912 L490:
06913     if (kforce == 0) {
06914         goto L580;
06915     }
06916     if (iphase == 1 && q[klm1 + n1 * q_dim1] <= toler) {
06917         goto L500;
06918     }
06919     kforce = 0;
06920     goto L240;
06921 /* SET UP PHASE 2 COSTS. */
06922 L500:
06923     iphase = 2;
06924     i__1 = nklm;
06925     for (j = 1; j <= i__1; ++j) {
06926         cu[(j << 1) + 1] = 0.f;
06927         cu[(j << 1) + 2] = 0.f;
06928 /* L510: */
06929     }
06930     i__1 = nk;
06931     for (j = n1; j <= i__1; ++j) {
06932         cu[(j << 1) + 1] = 1.f;
06933         cu[(j << 1) + 2] = 1.f;
06934 /* L520: */
06935     }
06936     i__1 = klm;
06937     for (i__ = 1; i__ <= i__1; ++i__) {
06938         ii = (long int) q[i__ + n2 * q_dim1];
06939         if (ii > 0) {
06940             goto L530;
06941         }
06942         ii = -ii;
06943         if (iu[(ii << 1) + 2] == 0) {
06944             goto L560;
06945         }
06946         cu[(ii << 1) + 2] = 0.f;
06947         goto L540;
06948 L530:
06949         if (iu[(ii << 1) + 1] == 0) {
06950             goto L560;
06951         }
06952         cu[(ii << 1) + 1] = 0.f;
06953 L540:
06954         ++ia;
06955         i__2 = n2;
06956         for (j = 1; j <= i__2; ++j) {
06957             z__ = q[ia + j * q_dim1];
06958             q[ia + j * q_dim1] = q[i__ + j * q_dim1];
06959             q[i__ + j * q_dim1] = z__;
06960 /* L550: */
06961         }
06962 L560:
06963         ;
06964     }
06965     goto L160;
06966 L570:
06967     if (q[klm1 + n1 * q_dim1] <= toler) {
06968         goto L500;
06969     }
06970     kode = 1;
06971     goto L590;
06972 L580:
06973     if (iphase == 1) {
06974         goto L570;
06975     }
06976 /* PREPARE OUTPUT. */
06977     kode = 0;
06978 L590:
06979     xsum = 0.;
06980     i__1 = *n;
06981     for (j = 1; j <= i__1; ++j) {
06982         x[j] = 0.f;
06983 /* L600: */
06984     }
06985     i__1 = klm;
06986     for (i__ = 1; i__ <= i__1; ++i__) {
06987         res[i__] = 0.f;
06988 /* L610: */
06989     }
06990     i__1 = klm;
06991     for (i__ = 1; i__ <= i__1; ++i__) {
06992         ii = (long int) q[i__ + n2 * q_dim1];
06993         sn = 1.f;
06994         if (ii > 0) {
06995             goto L620;
06996         }
06997         ii = -ii;
06998         sn = -1.f;
06999 L620:
07000         if (ii > *n) {
07001             goto L630;
07002         }
07003         x[ii] = sn * q[i__ + n1 * q_dim1];
07004         goto L640;
07005 L630:
07006         iimn = ii - *n;
07007         res[iimn] = sn * q[i__ + n1 * q_dim1];
07008         if (ii >= n1 && ii <= nk) {
07009             xsum += q[i__ + n1 * q_dim1];
07010         }
07011 L640:
07012         ;
07013     }
07014     error = (float)xsum;
07015     return;
07016 }
07017 
07018 float Util::eval(char * images,EMData * img, vector<int> S,int N, int ,int size)
07019 {
07020         int j,d;
07021         EMData * e = new EMData();
07022         float *eptr, *imgptr;
07023         imgptr = img->get_data();
07024         float SSE = 0.f;
07025         for (j = 0 ; j < N ; j++) {
07026                 e->read_image(images,S[j]);
07027                 eptr = e->get_data();
07028                 for (d = 0; d < size; d++) {
07029                         SSE += ((eptr[d] - imgptr[d])*(eptr[d] - imgptr[d]));}
07030                 }
07031         delete e;
07032         return SSE;
07033 }
07034 
07035 
07036 #define         mymax(x,y)              (((x)>(y))?(x):(y))
07037 #define         mymin(x,y)              (((x)<(y))?(x):(y))
07038 #define         sign(x,y)               (((((y)>0)?(1):(-1))*(y!=0))*(x))
07039 
07040 
07041 #define         quadpi                  3.141592653589793238462643383279502884197
07042 #define         dgr_to_rad              quadpi/180
07043 #define         deg_to_rad              quadpi/180
07044 #define         rad_to_deg              180/quadpi
07045 #define         rad_to_dgr              180/quadpi
07046 #define         TRUE                    1
07047 #define         FALSE                   0
07048 
07049 
07050 #define theta(i)                theta   [i-1]
07051 #define phi(i)                  phi     [i-1]
07052 #define weight(i)               weight  [i-1]
07053 #define lband(i)                lband   [i-1]
07054 #define ts(i)                   ts      [i-1]
07055 #define thetast(i)              thetast [i-1]
07056 #define key(i)                  key     [i-1]
07057 
07058 
07059 vector<double> Util::vrdg(const vector<float>& ph, const vector<float>& th)
07060 {
07061 
07062         ENTERFUNC;
07063 
07064         if ( th.size() != ph.size() ) {
07065                 LOGERR("images not same size");
07066                 throw ImageFormatException( "images not same size");
07067         }
07068 
07069         // rand_seed
07070         srand(10);
07071 
07072         int i,*key;
07073         int len = th.size();
07074         double *theta,*phi,*weight;
07075         theta   =       (double*) calloc(len,sizeof(double));
07076         phi     =       (double*) calloc(len,sizeof(double));
07077         weight  =       (double*) calloc(len,sizeof(double));
07078         key     =       (int*) calloc(len,sizeof(int));
07079         const float *thptr, *phptr;
07080 
07081         thptr = &th[0];
07082         phptr = &ph[0];
07083         for(i=1;i<=len;i++){
07084                 key(i) = i;
07085                 weight(i) = 0.0;
07086         }
07087 
07088         for(i = 0;i<len;i++){
07089                 theta[i] = thptr[i];
07090                 phi[i]   = phptr[i];
07091         }
07092 
07093         //  sort by theta
07094         Util::hsortd(theta, phi, key, len, 1);
07095 
07096         //Util::voronoidiag(theta,phi, weight, len);
07097         Util::voronoi(phi, theta, weight, len);
07098 
07099         //sort by key
07100         Util::hsortd(weight, weight, key, len, 2);
07101 
07102         free(theta);
07103         free(phi);
07104         free(key);
07105         vector<double> wt;
07106         double count = 0;
07107         for(i=1; i<= len; i++)
07108         {
07109                 wt.push_back(weight(i));
07110                 count += weight(i);
07111         }
07112 
07113         //if( abs(count-6.28) > 0.1 )
07114         //{
07115         //    printf("Warning: SUM OF VORONOI CELLS AREAS IS %lf, should 2*PI\n", count);
07116         //}
07117 
07118         free(weight);
07119 
07120         EXITFUNC;
07121         return wt;
07122 
07123 }
07124 
07125 struct  tmpstruct{
07126         double theta1,phi1;
07127         int key1;
07128         };
07129 
07130 void Util::hsortd(double *theta,double *phi,int *key,int len,int option)
07131 {
07132         ENTERFUNC;
07133         vector<tmpstruct> tmp(len);
07134         int i;
07135         for(i = 1;i<=len;i++)
07136         {
07137                 tmp[i-1].theta1 = theta(i);
07138                 tmp[i-1].phi1 = phi(i);
07139                 tmp[i-1].key1 = key(i);
07140         }
07141 
07142         if (option == 1) sort(tmp.begin(),tmp.end(),Util::cmp1);
07143         if (option == 2) sort(tmp.begin(),tmp.end(),Util::cmp2);
07144 
07145         for(i = 1;i<=len;i++)
07146         {
07147                 theta(i) = tmp[i-1].theta1;
07148                 phi(i)   = tmp[i-1].phi1;
07149                 key(i)   = tmp[i-1].key1;
07150         }
07151         EXITFUNC;
07152 }
07153 
07154 bool Util::cmp1(tmpstruct tmp1,tmpstruct tmp2)
07155 {
07156         return(tmp1.theta1 < tmp2.theta1);
07157 }
07158 
07159 bool Util::cmp2(tmpstruct tmp1,tmpstruct tmp2)
07160 {
07161         return(tmp1.key1 < tmp2.key1);
07162 }
07163 
07164 /******************  VORONOI DIAGRAM **********************************/
07165 /*
07166 void Util::voronoidiag(double *theta,double *phi,double* weight,int n)
07167 {
07168         ENTERFUNC;
07169 
07170         int     *lband;
07171         double  aat=0.0f,*ts;
07172         double  aa,acum,area;
07173         int     last;
07174         int numth       =       1;
07175         int nbt         =       1;//mymax((int)(sqrt((n/500.0))) , 3);
07176 
07177         int i,it,l,k;
07178         int nband,lb,low,medium,lhigh,lbw,lenw;
07179 
07180 
07181         lband   =       (int*)calloc(nbt,sizeof(int));
07182         ts      =       (double*)calloc(nbt,sizeof(double));
07183 
07184         if(lband == NULL || ts == NULL ){
07185                 fprintf(stderr,"memory allocation failure!\n");
07186                 exit(1);
07187         }
07188 
07189         nband=nbt;
07190 
07191         while(nband>0){
07192                 Util::angstep(ts,nband);
07193 
07194                 l=1;
07195                 for(i=1;i<=n;i++){
07196                         if(theta(i)>ts(l)){
07197                                 lband(l)=i;
07198                                 l=l+1;
07199                                 if(l>nband)  exit(1);
07200                         }
07201                 }
07202 
07203                 l=1;
07204                 for(i=1;i<=n;i++){
07205                         if(theta(i)>ts(l)){
07206                                 lband(l)=i;
07207                                 l=l+1;
07208                                 if(l>nband)  exit(1);
07209                         }
07210                 }
07211 
07212                 lband(l)=n+1;
07213                 acum=0.0;
07214                 for(it=l;it>=1;it-=numth){
07215                         for(i=it;i>=mymax(1,it-numth+1);i--){
07216                         if(i==l) last   =        TRUE;
07217                         else     last   =        FALSE;
07218 
07219                         if(l==1){
07220                                 lb=1;
07221                                 low=1;
07222                                 medium=n+1;
07223                                 lhigh=n-lb+1;
07224                                 lbw=1;
07225                         }
07226                         else if(i==1){
07227                                 lb=1;
07228                                 low=1;
07229                                 medium=lband(1);
07230                                 lhigh=lband(2)-1;
07231                                 lbw=1;
07232                         }
07233                         else if(i==l){
07234                                 if(l==2)        lb=1;
07235                                 else            lb=lband(l-2);
07236                                 low=lband(l-1)-lb+1;
07237                                 medium=lband(l)-lb+1;
07238                                 lhigh=n-lb+1;
07239                                 lbw=lband(i-1);
07240                         }
07241                         else{
07242                                 if(i==2)        lb=1;
07243                                 else            lb=lband(i-2);
07244                                 low=lband(i-1)-lb+1;
07245                                 medium=lband(i)-lb+1;
07246                                 lhigh=lband(i+1)-1-lb+1;
07247                                 lbw=lband(i-1);
07248                         }
07249                         lenw=medium-low;
07250 
07251 
07252                         Util::voronoi(&phi(lb),&theta(lb),&weight(lbw),lenw,low,medium,lhigh,last);
07253 
07254 
07255                         if(nband>1){
07256                                 if(i==1)        area=quadpi*2.0*(1.0-cos(ts(1)*dgr_to_rad));
07257                                 else            area=quadpi*2.0*(cos(ts(i-1)*dgr_to_rad)-cos(ts(i)*dgr_to_rad));
07258 
07259                                 aa = 0.0;
07260                                 for(k = lbw;k<=lbw+lenw-1;k++)
07261                                         aa = aa+weight(k);
07262 
07263                                 acum=acum+aa;
07264                                 aat=aa/area;
07265                                 }
07266 
07267                         }
07268                         for(i=it;mymax(1,it-numth+1);i--){
07269                         if(fabs(aat-1.0)>0.02){
07270                                 nband=mymax(0,mymin( (int)(((float)nband) * 0.75) ,nband-1) );
07271                                 goto  label2;
07272                                 }
07273                         }
07274                 acum=acum/quadpi/2.0;
07275                 exit(1);
07276 label2:
07277 
07278                 continue;
07279                 }
07280 
07281         free(ts);
07282         free(lband);
07283 
07284         }
07285 
07286         EXITFUNC;
07287 }
07288 
07289 
07290 void Util::angstep(double* thetast,int len){
07291 
07292         ENTERFUNC;
07293 
07294         double t1,t2,tmp;
07295         int i;
07296         if(len>1){
07297                 t1=0;
07298                 for(i=1;i<=len-1;i++){
07299                         tmp=cos(t1)-1.0/((float)len);
07300                         t2=acos(sign(mymin(1.0,fabs(tmp)),tmp));
07301                         thetast(i)=t2 * rad_to_deg;
07302                         t1=t2;
07303                 }
07304         }
07305         thetast(len)=90.0;
07306 
07307         EXITFUNC;
07308 }
07309 */
07310 /*
07311 void Util::voronoi(double *phi, double *theta, double *weight, int lenw, int low, int medium, int nt, int last)
07312 {
07313 
07314         ENTERFUNC;
07315         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07316         int nt6, n, ier,nout,lnew,mdup,nd;
07317         int i,k,mt,status;
07318 
07319 
07320         double *ds, *x, *y, *z;
07321         double tol=1.0e-8;
07322         double a;
07323 
07324         if(last){
07325                 if(medium>nt)  n = nt+nt;
07326                 else           n = nt+nt-medium+1;
07327         }
07328         else{
07329                 n=nt;
07330         }
07331 
07332         nt6 = n*6;
07333 
07334         list = (int*)calloc(nt6,sizeof(int));
07335         lptr = (int*)calloc(nt6,sizeof(int));
07336         lend = (int*)calloc(n  ,sizeof(int));
07337         iwk  = (int*)calloc(n  ,sizeof(int));
07338         good = (int*)calloc(n  ,sizeof(int));
07339         key  = (int*)calloc(n  ,sizeof(int));
07340         indx = (int*)calloc(n  ,sizeof(int));
07341         lcnt = (int*)calloc(n  ,sizeof(int));
07342 
07343         ds      =       (double*) calloc(n,sizeof(double));
07344         x       =       (double*) calloc(n,sizeof(double));
07345         y       =       (double*) calloc(n,sizeof(double));
07346         z       =       (double*) calloc(n,sizeof(double));
07347 
07348         if (list == NULL ||
07349         lptr == NULL ||
07350         lend == NULL ||
07351         iwk  == NULL ||
07352         good == NULL ||
07353         key  == NULL ||
07354         indx == NULL ||
07355         lcnt == NULL ||
07356         x    == NULL ||
07357         y    == NULL ||
07358         z    == NULL ||
07359         ds   == NULL) {
07360                 printf("memory allocation failure!\n");
07361                 exit(1);
07362         }
07363 
07364 
07365 
07366         for(i = 1;i<=nt;i++){
07367                 x[i-1] = theta(i);
07368                 y[i-1] = phi(i);
07369         }
07370 
07371 
07372 
07373         if (last) {
07374                 for(i=nt+1;i<=n;i++){
07375                         x[i-1]=180.0-x[2*nt-i];
07376                         y[i-1]=180.0+y[2*nt-i];
07377                 }
07378         }
07379 
07380 
07381         Util::disorder2(x,y,key,n);
07382 
07383         Util::ang_to_xyz(x,y,z,n);
07384 
07385 
07386         //  Make sure that first three are no colinear
07387         label1:
07388         for(k=0; k<2; k++){
07389                 for(i=k+1; i<3; i++){
07390                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol){
07391                                 Util::flip23(x, y, z, key, k, n);
07392                                 goto label1;
07393                         }
07394                 }
07395         }
07396 
07397 
07398         status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew,indx,lcnt, iwk, good, ds, &ier);
07399 
07400 
07401         if (status != 0) {
07402                 printf(" error in trmsh3 \n");
07403                 exit(1);
07404         }
07405 
07406 
07407         mdup=n-nout;
07408         if (ier == -2) {
07409                 printf("*** Error in TRMESH:the first three nodes are collinear***\n");
07410                 exit(1);
07411         }
07412         else if (ier > 0) {
07413                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07414                 exit(1);
07415         }
07416 
07417         nd=0;
07418         for (k=1;k<=n;k++){
07419                 if (indx[k-1]>0){
07420                         nd++;
07421                         good[nd-1]=k;
07422                 }
07423         }
07424 
07425 
07426         for(i = 1;i<=nout;i++) {
07427                 k=good[i-1];
07428                 if (key[k-1] >= low && key[k-1]<medium){
07429                         a = Util::areav_(&i,&nout,x,y,z,list,lptr,lend,&ier);
07430                         if (ier != 0){
07431                                 weight[key[k-1]-low] =-1.0;
07432                         }
07433                         else {
07434                                 weight[key[k-1]-low]=a/lcnt[i-1];
07435                         }
07436                 }
07437         }
07438 
07439 // Fill out the duplicated weights
07440         for(i = 1;i<=n;i++){
07441                 mt=-indx[i-1];
07442                 if (mt>0){
07443                         k=good[mt-1];
07444 //  This is a duplicated entry, get the already calculated
07445 //   weight and assign it.
07446                         if (key[i-1]>=low && key[i-1]<medium){
07447 //  Is it already calculated weight??
07448                                 if(key[k-1]>=low && key[k-1]<medium){
07449                                         weight[key[i-1]-low]=weight[key[k-1]-low];
07450                                 }
07451                                 else{
07452 //  No, the weight is from the outside of valid region, calculate it anyway
07453                                         a = Util::areav_(&mt, &nout, x, y, z, list, lptr, lend, &ier);
07454                                         if (ier != 0){
07455                                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07456                                                 weight[key[i-1]-low] =-1.0;
07457                                         }
07458                                         else {
07459                                                 weight[key[i-1]-low] = a/lcnt[mt-1];
07460                                         }
07461                                 }
07462                         }
07463                 }
07464         }
07465 
07466 
07467         free(list);
07468         free(lend);
07469         free(iwk);
07470         free(good);
07471         free(key);
07472 
07473         free(indx);
07474         free(lcnt);
07475         free(ds);
07476         free(x);
07477         free(y);
07478         free(z);
07479         EXITFUNC;
07480 }
07481 */
07482 void Util::voronoi(double *phi, double *theta, double *weight, int nt)
07483 {
07484 
07485         ENTERFUNC;
07486 
07487         int *list, *lptr, *lend, *iwk, *key,*lcnt,*indx,*good;
07488         int nt6, n, ier, nout, lnew, mdup, nd;
07489         int i,k,mt,status;
07490 
07491 
07492         double *ds, *x, *y, *z;
07493         double tol  = 1.0e-8;
07494         double dtol = 15;
07495         double a;
07496 
07497         /*if(last){
07498                 if(medium>nt)  n = nt+nt;
07499                 else           n = nt+nt-medium+1;
07500         }
07501         else{
07502                 n=nt;
07503         }*/
07504 
07505         n = nt + nt;
07506 
07507         nt6 = n*6;
07508 
07509         list = (int*)calloc(nt6,sizeof(int));
07510         lptr = (int*)calloc(nt6,sizeof(int));
07511         lend = (int*)calloc(n  ,sizeof(int));
07512         iwk  = (int*)calloc(n  ,sizeof(int));
07513         good = (int*)calloc(n  ,sizeof(int));
07514         key  = (int*)calloc(n  ,sizeof(int));
07515         indx = (int*)calloc(n  ,sizeof(int));
07516         lcnt = (int*)calloc(n  ,sizeof(int));
07517 
07518         ds      =       (double*) calloc(n,sizeof(double));
07519         x       =       (double*) calloc(n,sizeof(double));
07520         y       =       (double*) calloc(n,sizeof(double));
07521         z       =       (double*) calloc(n,sizeof(double));
07522 
07523         if (list == NULL ||
07524         lptr == NULL ||
07525         lend == NULL ||
07526         iwk  == NULL ||
07527         good == NULL ||
07528         key  == NULL ||
07529         indx == NULL ||
07530         lcnt == NULL ||
07531         x    == NULL ||
07532         y    == NULL ||
07533         z    == NULL ||
07534         ds   == NULL) {
07535                 printf("memory allocation failure!\n");
07536                 exit(1);
07537         }
07538 
07539         bool colinear=true;
07540         while(colinear)
07541         {
07542 
07543         L1:
07544             for(i = 0; i<nt; i++){
07545                 x[i] = theta[i];
07546                 y[i] = phi[i];
07547                 x[nt+i] = 180.0 - x[i];
07548                 y[nt+i] = 180.0 + y[i];
07549             }
07550 
07551             Util::disorder2(x, y, key, n);
07552 
07553             // check if the first three angles are not close, else shuffle
07554             double val;
07555             for(k=0; k<2; k++){
07556                 for(i=k+1; i<3; i++){
07557                     val = (x[i]-x[k])*(x[i]-x[k]) + (y[i]-y[k])*(y[i]-y[k]);
07558                     if( val  < dtol) {
07559                         goto L1;
07560                     }
07561                 }
07562             }
07563 
07564             Util::ang_to_xyz(x, y, z, n);
07565 
07566             //  Make sure that first three has no duplication
07567             bool dupnode=true;
07568             dupnode=true;
07569             while(dupnode)
07570             {
07571                 for(k=0; k<2; k++){
07572                     for(i=k+1; i<3; i++){
07573                         if(  x[i]*x[k]+y[i]*y[k]+z[i]*z[k] > 1.0-tol) {
07574                                 Util::flip23(x, y, z, key, k, n);
07575                                 continue;
07576                         }
07577                     }
07578                 }
07579                 dupnode = false;
07580             }
07581 
07582 
07583             ier = 0;
07584 
07585             status = Util::trmsh3_(&n,&tol,x,y,z,&nout,list,lptr,lend,&lnew, indx, lcnt, iwk, good, ds, &ier);
07586 
07587             if (status != 0) {
07588                 printf(" error in trmsh3 \n");
07589                 exit(1);
07590             }
07591 
07592             if (ier > 0) {
07593                 printf("*** Error in TRMESH:  duplicate nodes encountered ***\n");
07594                 exit(1);
07595             }
07596 
07597             mdup=n-nout;
07598             if (ier == -2) {
07599                 //printf("in TRMESH:the first three nodes are colinear*** disorder again\n");
07600             }
07601             else
07602             {
07603                 colinear=false;
07604             }
07605         }
07606 
07607 
07608         Assert( ier != -2 );
07609 //  Create a list of unique nodes GOOD, the numbers refer to locations on the full list
07610 //  INDX contains node numbers from the squeezed list
07611         nd=0;
07612         for (k=1; k<=n; k++){
07613                 if (indx[k-1]>0) {
07614                         nd++;
07615                         good[nd-1]=k;
07616                 }
07617         }
07618 
07619 //
07620 // *** Compute the Voronoi region areas.
07621 //
07622         for(i = 1; i<=nout; i++) {
07623                 k=good[i-1];
07624                 //  We only need n weights from hemisphere
07625                 if (key[k-1] <= nt) {
07626 //  CALCULATE THE AREA
07627                         a = Util::areav_(&i, &nout, x, y, z, list, lptr, lend, &ier);
07628                         if (ier != 0){
07629 //  We set the weight to -1, this will signal the error in the calling
07630 //   program, as the area will turn out incorrect
07631                                 printf("    *** error in areav:  ier = %d ***\n", ier);
07632                                 weight[key[k-1]-1] =-1.0;
07633                         } else {
07634 //  Assign the weight
07635                                 weight[key[k-1]-1]=a/lcnt[i-1];
07636                         }
07637                 }
07638         }
07639 
07640 
07641 // Fill out the duplicated weights
07642         for(i = 1; i<=n; i++){
07643                 mt =- indx[i-1];
07644                 if (mt>0){
07645                         k = good[mt-1];
07646 //  This is a duplicated entry, get the already calculated
07647 //   weight and assign it.
07648                 //  We only need n weights from hemisphere
07649                         if (key[i-1] <= nt && key[k-1] <= nt) { weight[key[i-1]-1] = weight[key[k-1]-1];}
07650                         }
07651         }
07652 
07653         free(list);
07654         free(lend);
07655         free(iwk);
07656         free(good);
07657         free(key);
07658         free(lptr);
07659         free(indx);
07660         free(lcnt);
07661         free(ds);
07662         free(x);
07663         free(y);
07664         free(z);
07665 
07666 
07667         EXITFUNC;
07668 }
07669 
07670 void Util::disorder2(double *x,double *y, int *key, int len)
07671 {
07672         ENTERFUNC;
07673         int k, i;
07674         for(i=0; i<len; i++) key[i]=i+1;
07675 
07676         for(i = 0; i<len;i++){
07677                 k = rand()%len;
07678                 std::swap(key[k], key[i]);
07679                 std::swap(x[k], x[i]);
07680                 std::swap(y[k], y[i]);
07681         }
07682         EXITFUNC;
07683 }
07684 
07685 void Util::ang_to_xyz(double *x,double *y,double *z,int len)
07686 {
07687         ENTERFUNC;
07688         double costheta,sintheta,cosphi,sinphi;
07689         for(int i = 0;  i<len;  i++)
07690         {
07691                 cosphi = cos(y[i]*dgr_to_rad);
07692                 sinphi = sin(y[i]*dgr_to_rad);
07693                 if(fabs(x[i]-90.0)< 1.0e-5){
07694                         x[i] = cosphi;
07695                         y[i] = sinphi;
07696                         z[i] = 0.0;
07697                 }
07698                 else{
07699                         costheta = cos(x[i]*dgr_to_rad);
07700                         sintheta = sin(x[i]*dgr_to_rad);
07701                         x[i] = cosphi*sintheta;
07702                         y[i] = sinphi*sintheta;
07703                         z[i] = costheta;
07704                 }
07705         }
07706         EXITFUNC;
07707 }
07708 
07709 void Util::flip23(double *x,double *y,double *z,int *key, int k, int len)
07710 {
07711         ENTERFUNC;
07712         int i = k;
07713         while( i == k )  i = rand()%len;
07714         std::swap(key[i], key[k]);
07715         std::swap(x[i], x[k]);
07716         std::swap(y[i], y[k]);
07717         std::swap(z[i], z[k]);
07718         EXITFUNC;
07719 }
07720 
07721 
07722 #undef  mymax
07723 #undef  mymin
07724 #undef  sign
07725 #undef  quadpi
07726 #undef  dgr_to_rad
07727 #undef  deg_to_rad
07728 #undef  rad_to_deg
07729 #undef  rad_to_dgr
07730 #undef  TRUE
07731 #undef  FALSE
07732 #undef  theta
07733 #undef  phi
07734 #undef  weight
07735 #undef  lband
07736 #undef  ts
07737 #undef  thetast
07738 #undef  key
07739 
07740 
07741 /*################################################################################################
07742 ##########  strid.f -- translated by f2c (version 20030320). ###################################
07743 ######   You must link the resulting object file with the libraries: #############################
07744 ####################    -lf2c -lm   (in that order)   ############################################
07745 ################################################################################################*/
07746 
07747 /* Common Block Declarations */
07748 
07749 
07750 #define TRUE_ (1)
07751 #define FALSE_ (0)
07752 #define abs(x) ((x) >= 0 ? (x) : -(x))
07753 
07754 struct stcom_{
07755     double y;
07756 };
07757 stcom_ stcom_1;
07758 #ifdef KR_headers
07759 double floor();
07760 int i_dnnt(x) double *x;
07761 #else
07762 int i_dnnt(double *x)
07763 #endif
07764 {
07765         return (int)(*x >= 0. ? floor(*x + .5) : -floor(.5 - *x));
07766 }
07767 
07768 
07769 
07770 
07771 /* ____________________STRID______________________________________ */
07772 /* Subroutine */ int Util::trmsh3_(int *n0, double *tol, double *x,
07773         double *y, double *z__, int *n, int *list, int *
07774         lptr, int *lend, int *lnew, int *indx, int *lcnt,
07775         int *near__, int *next, double *dist, int *ier)
07776 {
07777     /* System generated locals */
07778     int i__1, i__2;
07779 
07780     /* Local variables */
07781     static double d__;
07782     static int i__, j;
07783     static double d1, d2, d3;
07784     static int i0, lp, kt, ku, lpl, nku;
07785     extern long int left_(double *, double *, double *, double
07786             *, double *, double *, double *, double *,
07787             double *);
07788     static int nexti;
07789     extern /* Subroutine */ int addnod_(int *, int *, double *,
07790             double *, double *, int *, int *, int *,
07791             int *, int *);
07792 
07793 
07794 /* *********************************************************** */
07795 
07796 /*                                              From STRIPACK */
07797 /*                                            Robert J. Renka */
07798 /*                                  Dept. of Computer Science */
07799 /*                                       Univ. of North Texas */
07800 /*                                           renka@cs.unt.edu */
07801 /*                                                   01/20/03 */
07802 
07803 /*   This is an alternative to TRMESH with the inclusion of */
07804 /* an efficient means of removing duplicate or nearly dupli- */
07805 /* cate nodes. */
07806 
07807 /*   This subroutine creates a Delaunay triangulation of a */
07808 /* set of N arbitrarily distributed points, referred to as */
07809 /* nodes, on the surface of the unit sphere.  Refer to Sub- */
07810 /* routine TRMESH for definitions and a list of additional */
07811 /* subroutines.  This routine is an alternative to TRMESH */
07812 /* with the inclusion of an efficient means of removing dup- */
07813 /* licate or nearly duplicate nodes. */
07814 
07815 /*   The algorithm has expected time complexity O(N*log(N)) */
07816 /* for random nodal distributions. */
07817 
07818 
07819 /* On input: */
07820 
07821 /*       N0 = Number of nodes, possibly including duplicates. */
07822 /*            N0 .GE. 3. */
07823 
07824 /*       TOL = Tolerance defining a pair of duplicate nodes: */
07825 /*             bound on the deviation from 1 of the cosine of */
07826 /*             the angle between the nodes.  Note that */
07827 /*             |1-cos(A)| is approximately A*A/2. */
07828 
07829 /* The above parameters are not altered by this routine. */
07830 
07831 /*       X,Y,Z = Arrays of length at least N0 containing the */
07832 /*               Cartesian coordinates of nodes.  (X(K),Y(K), */
07833 /*               Z(K)) is referred to as node K, and K is re- */
07834 /*               ferred to as a nodal index.  It is required */
07835 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
07836 /*               K.  The first three nodes must not be col- */
07837 /*               linear (lie on a common great circle). */
07838 
07839 /*       LIST,LPTR = Arrays of length at least 6*N0-12. */
07840 
07841 /*       LEND = Array of length at least N0. */
07842 
07843 /*       INDX = Array of length at least N0. */
07844 
07845 /*       LCNT = Array of length at least N0 (length N is */
07846 /*              sufficient). */
07847 
07848 /*       NEAR,NEXT,DIST = Work space arrays of length at */
07849 /*                        least N0.  The space is used to */
07850 /*                        efficiently determine the nearest */
07851 /*                        triangulation node to each un- */
07852 /*                        processed node for use by ADDNOD. */
07853 
07854 /* On output: */
07855 
07856 /*       N = Number of nodes in the triangulation.  3 .LE. N */
07857 /*           .LE. N0, or N = 0 if IER < 0. */
07858 
07859 /*       X,Y,Z = Arrays containing the Cartesian coordinates */
07860 /*               of the triangulation nodes in the first N */
07861 /*               locations.  The original array elements are */
07862 /*               shifted down as necessary to eliminate dup- */
07863 /*               licate nodes. */
07864 
07865 /*       LIST = Set of nodal indexes which, along with LPTR, */
07866 /*              LEND, and LNEW, define the triangulation as a */
07867 /*              set of N adjacency lists -- counterclockwise- */
07868 /*              ordered sequences of neighboring nodes such */
07869 /*              that the first and last neighbors of a bound- */
07870 /*              ary node are boundary nodes (the first neigh- */
07871 /*              bor of an interior node is arbitrary).  In */
07872 /*              order to distinguish between interior and */
07873 /*              boundary nodes, the last neighbor of each */
07874 /*              boundary node is represented by the negative */
07875 /*              of its index. */
07876 
07877 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
07878 /*              correspondence with the elements of LIST. */
07879 /*              LIST(LPTR(I)) indexes the node which follows */
07880 /*              LIST(I) in cyclical counterclockwise order */
07881 /*              (the first neighbor follows the last neigh- */
07882 /*              bor). */
07883 
07884 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
07885 /*              points to the last neighbor of node K for */
07886 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
07887 /*              only if K is a boundary node. */
07888 
07889 /*       LNEW = Pointer to the first empty location in LIST */
07890 /*              and LPTR (list length plus one).  LIST, LPTR, */
07891 /*              LEND, and LNEW are not altered if IER < 0, */
07892 /*              and are incomplete if IER > 0. */
07893 
07894 /*       INDX = Array of output (triangulation) nodal indexes */
07895 /*              associated with input nodes.  For I = 1 to */
07896 /*              N0, INDX(I) is the index (for X, Y, and Z) of */
07897 /*              the triangulation node with the same (or */
07898 /*              nearly the same) coordinates as input node I. */
07899 
07900 /*       LCNT = Array of int weights (counts) associated */
07901 /*              with the triangulation nodes.  For I = 1 to */
07902 /*              N, LCNT(I) is the number of occurrences of */
07903 /*              node I in the input node set, and thus the */
07904 /*              number of duplicates is LCNT(I)-1. */
07905 
07906 /*       NEAR,NEXT,DIST = Garbage. */
07907 
07908 /*       IER = Error indicator: */
07909 /*             IER =  0 if no errors were encountered. */
07910 /*             IER = -1 if N0 < 3 on input. */
07911 /*             IER = -2 if the first three nodes are */
07912 /*                      collinear. */
07913 /*             IER = -3 if Subroutine ADDNOD returns an error */
07914 /*                      flag.  This should not occur. */
07915 
07916 /* Modules required by TRMSH3:  ADDNOD, BDYADD, COVSPH, */
07917 /*                                INSERT, INTADD, JRAND, */
07918 /*                                LEFT, LSTPTR, STORE, SWAP, */
07919 /*                                SWPTST, TRFIND */
07920 
07921 /* Intrinsic function called by TRMSH3:  ABS */
07922 
07923 /* *********************************************************** */
07924 
07925 
07926 /* Local parameters: */
07927 
07928 /* D =        (Negative cosine of) distance from node KT to */
07929 /*              node I */
07930 /* D1,D2,D3 = Distances from node KU to nodes 1, 2, and 3, */
07931 /*              respectively */
07932 /* I,J =      Nodal indexes */
07933 /* I0 =       Index of the node preceding I in a sequence of */
07934 /*              unprocessed nodes:  I = NEXT(I0) */
07935 /* KT =       Index of a triangulation node */
07936 /* KU =       Index of an unprocessed node and DO-loop index */
07937 /* LP =       LIST index (pointer) of a neighbor of KT */
07938 /* LPL =      Pointer to the last neighbor of KT */
07939 /* NEXTI =    NEXT(I) */
07940 /* NKU =      NEAR(KU) */
07941 
07942     /* Parameter adjustments */
07943     --dist;
07944     --next;
07945     --near__;
07946     --indx;
07947     --lend;
07948     --z__;
07949     --y;
07950     --x;
07951     --list;
07952     --lptr;
07953     --lcnt;
07954 
07955     /* Function Body */
07956     if (*n0 < 3) {
07957         *n = 0;
07958         *ier = -1;
07959         return 0;
07960     }
07961 
07962 /* Store the first triangle in the linked list. */
07963 
07964     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
07965             z__[3])) {
07966 
07967 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
07968 
07969         list[1] = 3;
07970         lptr[1] = 2;
07971         list[2] = -2;
07972         lptr[2] = 1;
07973         lend[1] = 2;
07974 
07975         list[3] = 1;
07976         lptr[3] = 4;
07977         list[4] = -3;
07978         lptr[4] = 3;
07979         lend[2] = 4;
07980 
07981         list[5] = 2;
07982         lptr[5] = 6;
07983         list[6] = -1;
07984         lptr[6] = 5;
07985         lend[3] = 6;
07986 
07987     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
07988             y[3], &z__[3])) {
07989 
07990 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
07991 /*     i.e., node 3 lies in the left hemisphere defined by */
07992 /*     arc 1->2. */
07993 
07994         list[1] = 2;
07995         lptr[1] = 2;
07996         list[2] = -3;
07997         lptr[2] = 1;
07998         lend[1] = 2;
07999 
08000         list[3] = 3;
08001         lptr[3] = 4;
08002         list[4] = -1;
08003         lptr[4] = 3;
08004         lend[2] = 4;
08005 
08006         list[5] = 1;
08007         lptr[5] = 6;
08008         list[6] = -2;
08009         lptr[6] = 5;
08010         lend[3] = 6;
08011 
08012 
08013     } else {
08014 
08015 /*   The first three nodes are collinear. */
08016 
08017         *n = 0;
08018         *ier = -2;
08019         return 0;
08020     }
08021 
08022     //printf("pass check colinear\n");
08023 
08024 /* Initialize LNEW, INDX, and LCNT, and test for N = 3. */
08025 
08026     *lnew = 7;
08027     indx[1] = 1;
08028     indx[2] = 2;
08029     indx[3] = 3;
08030     lcnt[1] = 1;
08031     lcnt[2] = 1;
08032     lcnt[3] = 1;
08033     if (*n0 == 3) {
08034         *n = 3;
08035         *ier = 0;
08036         return 0;
08037     }
08038 
08039 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
08040 /*   used to obtain an expected-time (N*log(N)) incremental */
08041 /*   algorithm by enabling constant search time for locating */
08042 /*   each new node in the triangulation. */
08043 
08044 /* For each unprocessed node KU, NEAR(KU) is the index of the */
08045 /*   triangulation node closest to KU (used as the starting */
08046 /*   point for the search in Subroutine TRFIND) and DIST(KU) */
08047 /*   is an increasing function of the arc length (angular */
08048 /*   distance) between nodes KU and NEAR(KU):  -Cos(a) for */
08049 /*   arc length a. */
08050 
08051 /* Since it is necessary to efficiently find the subset of */
08052 /*   unprocessed nodes associated with each triangulation */
08053 /*   node J (those that have J as their NEAR entries), the */
08054 /*   subsets are stored in NEAR and NEXT as follows:  for */
08055 /*   each node J in the triangulation, I = NEAR(J) is the */
08056 /*   first unprocessed node in J's set (with I = 0 if the */
08057 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
08058 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
08059 /*   set are initially ordered by increasing indexes (which */
08060 /*   maximizes efficiency) but that ordering is not main- */
08061 /*   tained as the data structure is updated. */
08062 
08063 /* Initialize the data structure for the single triangle. */
08064 
08065     near__[1] = 0;
08066     near__[2] = 0;
08067     near__[3] = 0;
08068     for (ku = *n0; ku >= 4; --ku) {
08069         d1 = -(x[ku] * x[1] + y[ku] * y[1] + z__[ku] * z__[1]);
08070         d2 = -(x[ku] * x[2] + y[ku] * y[2] + z__[ku] * z__[2]);
08071         d3 = -(x[ku] * x[3] + y[ku] * y[3] + z__[ku] * z__[3]);
08072         if (d1 <= d2 && d1 <= d3) {
08073             near__[ku] = 1;
08074             dist[ku] = d1;
08075             next[ku] = near__[1];
08076             near__[1] = ku;
08077         } else if (d2 <= d1 && d2 <= d3) {
08078             near__[ku] = 2;
08079             dist[ku] = d2;
08080             next[ku] = near__[2];
08081             near__[2] = ku;
08082         } else {
08083             near__[ku] = 3;
08084             dist[ku] = d3;
08085             next[ku] = near__[3];
08086             near__[3] = ku;
08087         }
08088 /* L1: */
08089     }
08090 
08091 /* Loop on unprocessed nodes KU.  KT is the number of nodes */
08092 /*   in the triangulation, and NKU = NEAR(KU). */
08093 
08094     kt = 3;
08095     i__1 = *n0;
08096     for (ku = 4; ku <= i__1; ++ku) {
08097         nku = near__[ku];
08098 
08099 /* Remove KU from the set of unprocessed nodes associated */
08100 /*   with NEAR(KU). */
08101         i__ = nku;
08102         if (near__[i__] == ku) {
08103             near__[i__] = next[ku];
08104         } else {
08105             i__ = near__[i__];
08106 L2:
08107             i0 = i__;
08108             i__ = next[i0];
08109             if (i__ != ku) {
08110                 goto L2;
08111             }
08112             next[i0] = next[ku];
08113         }
08114         near__[ku] = 0;
08115 
08116 /* Bypass duplicate nodes. */
08117 
08118         if (dist[ku] <= *tol - 1.) {
08119             indx[ku] = -nku;
08120             ++lcnt[nku];
08121             goto L6;
08122         }
08123 
08124 
08125 /* Add a new triangulation node KT with LCNT(KT) = 1. */
08126         ++kt;
08127         x[kt] = x[ku];
08128         y[kt] = y[ku];
08129         z__[kt] = z__[ku];
08130         indx[ku] = kt;
08131         lcnt[kt] = 1;
08132         addnod_(&nku, &kt, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08133                 , lnew, ier);
08134         if (*ier != 0) {
08135             *n = 0;
08136             *ier = -3;
08137             return 0;
08138         }
08139 
08140 /* Loop on neighbors J of node KT. */
08141 
08142         lpl = lend[kt];
08143         lp = lpl;
08144 L3:
08145         lp = lptr[lp];
08146         j = (i__2 = list[lp], abs(i__2));
08147 
08148 /* Loop on elements I in the sequence of unprocessed nodes */
08149 /*   associated with J:  KT is a candidate for replacing J */
08150 /*   as the nearest triangulation node to I.  The next value */
08151 /*   of I in the sequence, NEXT(I), must be saved before I */
08152 /*   is moved because it is altered by adding I to KT's set. */
08153 
08154         i__ = near__[j];
08155 L4:
08156         if (i__ == 0) {
08157             goto L5;
08158         }
08159         nexti = next[i__];
08160 
08161 /* Test for the distance from I to KT less than the distance */
08162 /*   from I to J. */
08163 
08164         d__ = -(x[i__] * x[kt] + y[i__] * y[kt] + z__[i__] * z__[kt]);
08165         if (d__ < dist[i__]) {
08166 
08167 /* Replace J by KT as the nearest triangulation node to I: */
08168 /*   update NEAR(I) and DIST(I), and remove I from J's set */
08169 /*   of unprocessed nodes and add it to KT's set. */
08170 
08171             near__[i__] = kt;
08172             dist[i__] = d__;
08173             if (i__ == near__[j]) {
08174                 near__[j] = nexti;
08175             } else {
08176                 next[i0] = nexti;
08177             }
08178             next[i__] = near__[kt];
08179             near__[kt] = i__;
08180         } else {
08181             i0 = i__;
08182         }
08183 
08184 /* Bottom of loop on I. */
08185 
08186         i__ = nexti;
08187         goto L4;
08188 
08189 /* Bottom of loop on neighbors J. */
08190 
08191 L5:
08192         if (lp != lpl) {
08193             goto L3;
08194         }
08195 L6:
08196         ;
08197     }
08198     *n = kt;
08199     *ier = 0;
08200     return 0;
08201 } /* trmsh3_ */
08202 
08203 /* stripack.dbl sent by Robert on 06/03/03 */
08204 /* Subroutine */ int addnod_(int *nst, int *k, double *x,
08205         double *y, double *z__, int *list, int *lptr, int
08206         *lend, int *lnew, int *ier)
08207 {
08208     /* Initialized data */
08209 
08210     static double tol = 0.;
08211 
08212     /* System generated locals */
08213     int i__1;
08214 
08215     /* Local variables */
08216     static int l;
08217     static double p[3], b1, b2, b3;
08218     static int i1, i2, i3, kk, lp, in1, io1, io2, km1, lpf, ist, lpo1;
08219     extern /* Subroutine */ int swap_(int *, int *, int *,
08220             int *, int *, int *, int *, int *);
08221     static int lpo1s;
08222     extern /* Subroutine */ int bdyadd_(int *, int *, int *,
08223             int *, int *, int *, int *), intadd_(int *,
08224             int *, int *, int *, int *, int *, int *,
08225             int *), trfind_(int *, double *, int *,
08226             double *, double *, double *, int *, int *,
08227             int *, double *, double *, double *, int *,
08228             int *, int *), covsph_(int *, int *, int *,
08229             int *, int *, int *);
08230     extern int lstptr_(int *, int *, int *, int *);
08231     extern long int swptst_(int *, int *, int *, int *,
08232             double *, double *, double *);
08233 
08234 
08235 /* *********************************************************** */
08236 
08237 /*                                              From STRIPACK */
08238 /*                                            Robert J. Renka */
08239 /*                                  Dept. of Computer Science */
08240 /*                                       Univ. of North Texas */
08241 /*                                           renka@cs.unt.edu */
08242 /*                                                   01/08/03 */
08243 
08244 /*   This subroutine adds node K to a triangulation of the */
08245 /* convex hull of nodes 1,...,K-1, producing a triangulation */
08246 /* of the convex hull of nodes 1,...,K. */
08247 
08248 /*   The algorithm consists of the following steps:  node K */
08249 /* is located relative to the triangulation (TRFIND), its */
08250 /* index is added to the data structure (INTADD or BDYADD), */
08251 /* and a sequence of swaps (SWPTST and SWAP) are applied to */
08252 /* the arcs opposite K so that all arcs incident on node K */
08253 /* and opposite node K are locally optimal (satisfy the cir- */
08254 /* cumcircle test).  Thus, if a Delaunay triangulation is */
08255 /* input, a Delaunay triangulation will result. */
08256 
08257 
08258 /* On input: */
08259 
08260 /*       NST = Index of a node at which TRFIND begins its */
08261 /*             search.  Search time depends on the proximity */
08262 /*             of this node to K.  If NST < 1, the search is */
08263 /*             begun at node K-1. */
08264 
08265 /*       K = Nodal index (index for X, Y, Z, and LEND) of the */
08266 /*           new node to be added.  K .GE. 4. */
08267 
08268 /*       X,Y,Z = Arrays of length .GE. K containing Car- */
08269 /*               tesian coordinates of the nodes. */
08270 /*               (X(I),Y(I),Z(I)) defines node I for */
08271 /*               I = 1,...,K. */
08272 
08273 /* The above parameters are not altered by this routine. */
08274 
08275 /*       LIST,LPTR,LEND,LNEW = Data structure associated with */
08276 /*                             the triangulation of nodes 1 */
08277 /*                             to K-1.  The array lengths are */
08278 /*                             assumed to be large enough to */
08279 /*                             add node K.  Refer to Subrou- */
08280 /*                             tine TRMESH. */
08281 
08282 /* On output: */
08283 
08284 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
08285 /*                             the addition of node K as the */
08286 /*                             last entry unless IER .NE. 0 */
08287 /*                             and IER .NE. -3, in which case */
08288 /*                             the arrays are not altered. */
08289 
08290 /*       IER = Error indicator: */
08291 /*             IER =  0 if no errors were encountered. */
08292 /*             IER = -1 if K is outside its valid range */
08293 /*                      on input. */
08294 /*             IER = -2 if all nodes (including K) are col- */
08295 /*                      linear (lie on a common geodesic). */
08296 /*             IER =  L if nodes L and K coincide for some */
08297 /*                      L < K.  Refer to TOL below. */
08298 
08299 /* Modules required by ADDNOD:  BDYADD, COVSPH, INSERT, */
08300 /*                                INTADD, JRAND, LSTPTR, */
08301 /*                                STORE, SWAP, SWPTST, */
08302 /*                                TRFIND */
08303 
08304 /* Intrinsic function called by ADDNOD:  ABS */
08305 
08306 /* *********************************************************** */
08307 
08308 
08309 /* Local parameters: */
08310 
08311 /* B1,B2,B3 = Unnormalized barycentric coordinates returned */
08312 /*              by TRFIND. */
08313 /* I1,I2,I3 = Vertex indexes of a triangle containing K */
08314 /* IN1 =      Vertex opposite K:  first neighbor of IO2 */
08315 /*              that precedes IO1.  IN1,IO1,IO2 are in */
08316 /*              counterclockwise order. */
08317 /* IO1,IO2 =  Adjacent neighbors of K defining an arc to */
08318 /*              be tested for a swap */
08319 /* IST =      Index of node at which TRFIND begins its search */
08320 /* KK =       Local copy of K */
08321 /* KM1 =      K-1 */
08322 /* L =        Vertex index (I1, I2, or I3) returned in IER */
08323 /*              if node K coincides with a vertex */
08324 /* LP =       LIST pointer */
08325 /* LPF =      LIST pointer to the first neighbor of K */
08326 /* LPO1 =     LIST pointer to IO1 */
08327 /* LPO1S =    Saved value of LPO1 */
08328 /* P =        Cartesian coordinates of node K */
08329 /* TOL =      Tolerance defining coincident nodes:  bound on */
08330 /*              the deviation from 1 of the cosine of the */
08331 /*              angle between the nodes. */
08332 /*              Note that |1-cos(A)| is approximately A*A/2. */
08333 
08334     /* Parameter adjustments */
08335     --lend;
08336     --z__;
08337     --y;
08338     --x;
08339     --list;
08340     --lptr;
08341 
08342     /* Function Body */
08343 
08344     kk = *k;
08345     if (kk < 4) {
08346         goto L3;
08347     }
08348 
08349 /* Initialization: */
08350     km1 = kk - 1;
08351     ist = *nst;
08352     if (ist < 1) {
08353         ist = km1;
08354     }
08355     p[0] = x[kk];
08356     p[1] = y[kk];
08357     p[2] = z__[kk];
08358 
08359 /* Find a triangle (I1,I2,I3) containing K or the rightmost */
08360 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
08361 /*   from node K. */
08362     trfind_(&ist, p, &km1, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[1]
08363             , &b1, &b2, &b3, &i1, &i2, &i3);
08364 
08365 /*   Test for collinear or (nearly) duplicate nodes. */
08366 
08367     if (i1 == 0) {
08368         goto L4;
08369     }
08370     l = i1;
08371     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08372         goto L5;
08373     }
08374     l = i2;
08375     if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08376         goto L5;
08377     }
08378     if (i3 != 0) {
08379         l = i3;
08380         if (p[0] * x[l] + p[1] * y[l] + p[2] * z__[l] >= 1. - tol) {
08381             goto L5;
08382         }
08383         intadd_(&kk, &i1, &i2, &i3, &list[1], &lptr[1], &lend[1], lnew);
08384     } else {
08385         if (i1 != i2) {
08386             bdyadd_(&kk, &i1, &i2, &list[1], &lptr[1], &lend[1], lnew);
08387         } else {
08388             covsph_(&kk, &i1, &list[1], &lptr[1], &lend[1], lnew);
08389         }
08390     }
08391     *ier = 0;
08392 
08393 /* Initialize variables for optimization of the */
08394 /*   triangulation. */
08395     lp = lend[kk];
08396     lpf = lptr[lp];
08397     io2 = list[lpf];
08398     lpo1 = lptr[lpf];
08399     io1 = (i__1 = list[lpo1], abs(i__1));
08400 
08401 /* Begin loop:  find the node opposite K. */
08402 
08403 L1:
08404     lp = lstptr_(&lend[io1], &io2, &list[1], &lptr[1]);
08405     if (list[lp] < 0) {
08406         goto L2;
08407     }
08408     lp = lptr[lp];
08409     in1 = (i__1 = list[lp], abs(i__1));
08410 
08411 /* Swap test:  if a swap occurs, two new arcs are */
08412 /*             opposite K and must be tested. */
08413 
08414     lpo1s = lpo1;
08415     if (! swptst_(&in1, &kk, &io1, &io2, &x[1], &y[1], &z__[1])) {
08416         goto L2;
08417     }
08418     swap_(&in1, &kk, &io1, &io2, &list[1], &lptr[1], &lend[1], &lpo1);
08419     if (lpo1 == 0) {
08420 
08421 /*   A swap is not possible because KK and IN1 are already */
08422 /*     adjacent.  This error in SWPTST only occurs in the */
08423 /*     neutral case and when there are nearly duplicate */
08424 /*     nodes. */
08425 
08426         lpo1 = lpo1s;
08427         goto L2;
08428     }
08429     io1 = in1;
08430     goto L1;
08431 
08432 /* No swap occurred.  Test for termination and reset */
08433 /*   IO2 and IO1. */
08434 
08435 L2:
08436     if (lpo1 == lpf || list[lpo1] < 0) {
08437         return 0;
08438     }
08439     io2 = io1;
08440     lpo1 = lptr[lpo1];
08441     io1 = (i__1 = list[lpo1], abs(i__1));
08442     goto L1;
08443 
08444 /* KK < 4. */
08445 
08446 L3:
08447     *ier = -1;
08448     return 0;
08449 
08450 /* All nodes are collinear. */
08451 
08452 L4:
08453     *ier = -2;
08454     return 0;
08455 
08456 /* Nodes L and K coincide. */
08457 
08458 L5:
08459     *ier = l;
08460     return 0;
08461 } /* addnod_ */
08462 
08463 double angle_(double *v1, double *v2, double *v3)
08464 {
08465     /* System generated locals */
08466     double ret_val;
08467 
08468     /* Builtin functions */
08469     //double sqrt(double), acos(double);
08470 
08471     /* Local variables */
08472     static double a;
08473     static int i__;
08474     static double ca, s21, s23, u21[3], u23[3];
08475     extern long int left_(double *, double *, double *, double
08476             *, double *, double *, double *, double *,
08477             double *);
08478 
08479 
08480 /* *********************************************************** */
08481 
08482 /*                                              From STRIPACK */
08483 /*                                            Robert J. Renka */
08484 /*                                  Dept. of Computer Science */
08485 /*                                       Univ. of North Texas */
08486 /*                                           renka@cs.unt.edu */
08487 /*                                                   06/03/03 */
08488 
08489 /*   Given a sequence of three nodes (V1,V2,V3) on the sur- */
08490 /* face of the unit sphere, this function returns the */
08491 /* interior angle at V2 -- the dihedral angle between the */
08492 /* plane defined by V2 and V3 (and the origin) and the plane */
08493 /* defined by V2 and V1 or, equivalently, the angle between */
08494 /* the normals V2 X V3 and V2 X V1.  Note that the angle is */
08495 /* in the range 0 to Pi if V3 Left V1->V2, Pi to 2*Pi other- */
08496 /* wise.  The surface area of a spherical polygon with CCW- */
08497 /* ordered vertices V1, V2, ..., Vm is Asum - (m-2)*Pi, where */
08498 /* Asum is the sum of the m interior angles computed from the */
08499 /* sequences (Vm,V1,V2), (V1,V2,V3), (V2,V3,V4), ..., */
08500 /* (Vm-1,Vm,V1). */
08501 
08502 
08503 /* On input: */
08504 
08505 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08506 /*                  sian coordinates of unit vectors.  These */
08507 /*                  vectors, if nonzero, are implicitly */
08508 /*                  scaled to have length 1. */
08509 
08510 /* Input parameters are not altered by this function. */
08511 
08512 /* On output: */
08513 
08514 /*       ANGLE = Angle defined above, or 0 if V2 X V1 = 0 or */
08515 /*               V2 X V3 = 0. */
08516 
08517 /* Module required by ANGLE:  LEFT */
08518 
08519 /* Intrinsic functions called by ANGLE:  ACOS, SQRT */
08520 
08521 /* *********************************************************** */
08522 
08523 
08524 /* Local parameters: */
08525 
08526 /* A =       Interior angle at V2 */
08527 /* CA =      cos(A) */
08528 /* I =       DO-loop index and index for U21 and U23 */
08529 /* S21,S23 = Sum of squared components of U21 and U23 */
08530 /* U21,U23 = Unit normal vectors to the planes defined by */
08531 /*             pairs of triangle vertices */
08532 
08533 
08534 /* Compute cross products U21 = V2 X V1 and U23 = V2 X V3. */
08535 
08536     /* Parameter adjustments */
08537     --v3;
08538     --v2;
08539     --v1;
08540 
08541     /* Function Body */
08542     u21[0] = v2[2] * v1[3] - v2[3] * v1[2];
08543     u21[1] = v2[3] * v1[1] - v2[1] * v1[3];
08544     u21[2] = v2[1] * v1[2] - v2[2] * v1[1];
08545 
08546     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08547     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08548     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08549 
08550 /* Normalize U21 and U23 to unit vectors. */
08551 
08552     s21 = 0.;
08553     s23 = 0.;
08554     for (i__ = 1; i__ <= 3; ++i__) {
08555         s21 += u21[i__ - 1] * u21[i__ - 1];
08556         s23 += u23[i__ - 1] * u23[i__ - 1];
08557 /* L1: */
08558     }
08559 
08560 /* Test for a degenerate triangle associated with collinear */
08561 /*   vertices. */
08562 
08563     if (s21 == 0. || s23 == 0.) {
08564         ret_val = 0.;
08565         return ret_val;
08566     }
08567     s21 = sqrt(s21);
08568     s23 = sqrt(s23);
08569     for (i__ = 1; i__ <= 3; ++i__) {
08570         u21[i__ - 1] /= s21;
08571         u23[i__ - 1] /= s23;
08572 /* L2: */
08573     }
08574 
08575 /* Compute the angle A between normals: */
08576 
08577 /*   CA = cos(A) = <U21,U23> */
08578 
08579     ca = u21[0] * u23[0] + u21[1] * u23[1] + u21[2] * u23[2];
08580     if (ca < -1.) {
08581         ca = -1.;
08582     }
08583     if (ca > 1.) {
08584         ca = 1.;
08585     }
08586     a = acos(ca);
08587 
08588 /* Adjust A to the interior angle:  A > Pi iff */
08589 /*   V3 Right V1->V2. */
08590 
08591     if (! left_(&v1[1], &v1[2], &v1[3], &v2[1], &v2[2], &v2[3], &v3[1], &v3[2]
08592             , &v3[3])) {
08593         a = acos(-1.) * 2. - a;
08594     }
08595     ret_val = a;
08596     return ret_val;
08597 } /* angle_ */
08598 
08599 double areas_(double *v1, double *v2, double *v3)
08600 {
08601     /* System generated locals */
08602     double ret_val;
08603 
08604     /* Builtin functions */
08605     //double sqrt(double), acos(double);
08606 
08607     /* Local variables */
08608     static int i__;
08609     static double a1, a2, a3, s12, s31, s23, u12[3], u23[3], u31[3], ca1,
08610             ca2, ca3;
08611 
08612 
08613 /* *********************************************************** */
08614 
08615 /*                                              From STRIPACK */
08616 /*                                            Robert J. Renka */
08617 /*                                  Dept. of Computer Science */
08618 /*                                       Univ. of North Texas */
08619 /*                                           renka@cs.unt.edu */
08620 /*                                                   06/22/98 */
08621 
08622 /*   This function returns the area of a spherical triangle */
08623 /* on the unit sphere. */
08624 
08625 
08626 /* On input: */
08627 
08628 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
08629 /*                  sian coordinates of unit vectors (the */
08630 /*                  three triangle vertices in any order). */
08631 /*                  These vectors, if nonzero, are implicitly */
08632 /*                  scaled to have length 1. */
08633 
08634 /* Input parameters are not altered by this function. */
08635 
08636 /* On output: */
08637 
08638 /*       AREAS = Area of the spherical triangle defined by */
08639 /*               V1, V2, and V3 in the range 0 to 2*PI (the */
08640 /*               area of a hemisphere).  AREAS = 0 (or 2*PI) */
08641 /*               if and only if V1, V2, and V3 lie in (or */
08642 /*               close to) a plane containing the origin. */
08643 
08644 /* Modules required by AREAS:  None */
08645 
08646 /* Intrinsic functions called by AREAS:  ACOS, SQRT */
08647 
08648 /* *********************************************************** */
08649 
08650 
08651 /* Local parameters: */
08652 
08653 /* A1,A2,A3 =    Interior angles of the spherical triangle */
08654 /* CA1,CA2,CA3 = cos(A1), cos(A2), and cos(A3), respectively */
08655 /* I =           DO-loop index and index for Uij */
08656 /* S12,S23,S31 = Sum of squared components of U12, U23, U31 */
08657 /* U12,U23,U31 = Unit normal vectors to the planes defined by */
08658 /*                 pairs of triangle vertices */
08659 
08660 
08661 /* Compute cross products Uij = Vi X Vj. */
08662 
08663     /* Parameter adjustments */
08664     --v3;
08665     --v2;
08666     --v1;
08667 
08668     /* Function Body */
08669     u12[0] = v1[2] * v2[3] - v1[3] * v2[2];
08670     u12[1] = v1[3] * v2[1] - v1[1] * v2[3];
08671     u12[2] = v1[1] * v2[2] - v1[2] * v2[1];
08672 
08673     u23[0] = v2[2] * v3[3] - v2[3] * v3[2];
08674     u23[1] = v2[3] * v3[1] - v2[1] * v3[3];
08675     u23[2] = v2[1] * v3[2] - v2[2] * v3[1];
08676 
08677     u31[0] = v3[2] * v1[3] - v3[3] * v1[2];
08678     u31[1] = v3[3] * v1[1] - v3[1] * v1[3];
08679     u31[2] = v3[1] * v1[2] - v3[2] * v1[1];
08680 
08681 /* Normalize Uij to unit vectors. */
08682 
08683     s12 = 0.;
08684     s23 = 0.;
08685     s31 = 0.;
08686     for (i__ = 1; i__ <= 3; ++i__) {
08687         s12 += u12[i__ - 1] * u12[i__ - 1];
08688         s23 += u23[i__ - 1] * u23[i__ - 1];
08689         s31 += u31[i__ - 1] * u31[i__ - 1];
08690 /* L2: */
08691     }
08692 
08693 /* Test for a degenerate triangle associated with collinear */
08694 /*   vertices. */
08695 
08696     if (s12 == 0. || s23 == 0. || s31 == 0.) {
08697         ret_val = 0.;
08698         return ret_val;
08699     }
08700     s12 = sqrt(s12);
08701     s23 = sqrt(s23);
08702     s31 = sqrt(s31);
08703     for (i__ = 1; i__ <= 3; ++i__) {
08704         u12[i__ - 1] /= s12;
08705         u23[i__ - 1] /= s23;
08706         u31[i__ - 1] /= s31;
08707 /* L3: */
08708     }
08709 
08710 /* Compute interior angles Ai as the dihedral angles between */
08711 /*   planes: */
08712 /*           CA1 = cos(A1) = -<U12,U31> */
08713 /*           CA2 = cos(A2) = -<U23,U12> */
08714 /*           CA3 = cos(A3) = -<U31,U23> */
08715 
08716     ca1 = -u12[0] * u31[0] - u12[1] * u31[1] - u12[2] * u31[2];
08717     ca2 = -u23[0] * u12[0] - u23[1] * u12[1] - u23[2] * u12[2];
08718     ca3 = -u31[0] * u23[0] - u31[1] * u23[1] - u31[2] * u23[2];
08719     if (ca1 < -1.) {
08720         ca1 = -1.;
08721     }
08722     if (ca1 > 1.) {
08723         ca1 = 1.;
08724     }
08725     if (ca2 < -1.) {
08726         ca2 = -1.;
08727     }
08728     if (ca2 > 1.) {
08729         ca2 = 1.;
08730     }
08731     if (ca3 < -1.) {
08732         ca3 = -1.;
08733     }
08734     if (ca3 > 1.) {
08735         ca3 = 1.;
08736     }
08737     a1 = acos(ca1);
08738     a2 = acos(ca2);
08739     a3 = acos(ca3);
08740 
08741 /* Compute AREAS = A1 + A2 + A3 - PI. */
08742 
08743     ret_val = a1 + a2 + a3 - acos(-1.);
08744     if (ret_val < 0.) {
08745         ret_val = 0.;
08746     }
08747     return ret_val;
08748 } /* areas_ */
08749 
08750 double Util::areav_(int *k, int *n, double *x, double *y,
08751         double *z__, int *list, int *lptr, int *lend, int
08752         *ier)
08753 {
08754     /* Initialized data */
08755 
08756     static double amax = 6.28;
08757 
08758     /* System generated locals */
08759     double ret_val;
08760 
08761     /* Local variables */
08762     static double a, c0[3], c2[3], c3[3];
08763     static int n1, n2, n3;
08764     static double v1[3], v2[3], v3[3];
08765     static int lp, lpl, ierr;
08766     static double asum;
08767     extern double areas_(double *, double *, double *);
08768     static long int first;
08769     extern /* Subroutine */ int circum_(double *, double *,
08770             double *, double *, int *);
08771 
08772 
08773 /* *********************************************************** */
08774 
08775 /*                                            Robert J. Renka */
08776 /*                                  Dept. of Computer Science */
08777 /*                                       Univ. of North Texas */
08778 /*                                           renka@cs.unt.edu */
08779 /*                                                   10/25/02 */
08780 
08781 /*   Given a Delaunay triangulation and the index K of an */
08782 /* interior node, this subroutine returns the (surface) area */
08783 /* of the Voronoi region associated with node K.  The Voronoi */
08784 /* region is the polygon whose vertices are the circumcenters */
08785 /* of the triangles that contain node K, where a triangle */
08786 /* circumcenter is the point (unit vector) lying at the same */
08787 /* angular distance from the three vertices and contained in */
08788 /* the same hemisphere as the vertices. */
08789 
08790 
08791 /* On input: */
08792 
08793 /*       K = Nodal index in the range 1 to N. */
08794 
08795 /*       N = Number of nodes in the triangulation.  N > 3. */
08796 
08797 /*       X,Y,Z = Arrays of length N containing the Cartesian */
08798 /*               coordinates of the nodes (unit vectors). */
08799 
08800 /*       LIST,LPTR,LEND = Data structure defining the trian- */
08801 /*                        gulation.  Refer to Subroutine */
08802 /*                        TRMESH. */
08803 
08804 /* Input parameters are not altered by this function. */
08805 
08806 /* On output: */
08807 
08808 /*       AREAV = Area of Voronoi region K unless IER > 0, */
08809 /*               in which case AREAV = 0. */
08810 
08811 /*       IER = Error indicator: */
08812 /*             IER = 0 if no errors were encountered. */
08813 /*             IER = 1 if K or N is outside its valid range */
08814 /*                     on input. */
08815 /*             IER = 2 if K indexes a boundary node. */
08816 /*             IER = 3 if an error flag is returned by CIRCUM */
08817 /*                     (null triangle). */
08818 /*             IER = 4 if AREAS returns a value greater than */
08819 /*                     AMAX (defined below). */
08820 
08821 /* Modules required by AREAV:  AREAS, CIRCUM */
08822 
08823 /* *********************************************************** */
08824 
08825 
08826 /* Maximum valid triangle area is less than 2*Pi: */
08827 
08828     /* Parameter adjustments */
08829     --lend;
08830     --z__;
08831     --y;
08832     --x;
08833     --list;
08834     --lptr;
08835 
08836     /* Function Body */
08837 
08838 /* Test for invalid input. */
08839 
08840     if (*k < 1 || *k > *n || *n <= 3) {
08841         goto L11;
08842     }
08843 
08844 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
08845 /*   FIRST = TRUE only for the first triangle. */
08846 /*   The Voronoi region area is accumulated in ASUM. */
08847 
08848     n1 = *k;
08849     v1[0] = x[n1];
08850     v1[1] = y[n1];
08851     v1[2] = z__[n1];
08852     lpl = lend[n1];
08853     n3 = list[lpl];
08854     if (n3 < 0) {
08855         goto L12;
08856     }
08857     lp = lpl;
08858     first = TRUE_;
08859     asum = 0.;
08860 
08861 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
08862 
08863 L1:
08864     n2 = n3;
08865     lp = lptr[lp];
08866     n3 = list[lp];
08867     v2[0] = x[n2];
08868     v2[1] = y[n2];
08869     v2[2] = z__[n2];
08870     v3[0] = x[n3];
08871     v3[1] = y[n3];
08872     v3[2] = z__[n3];
08873     if (first) {
08874 
08875 /* First triangle:  compute the circumcenter C3 and save a */
08876 /*   copy in C0. */
08877 
08878         circum_(v1, v2, v3, c3, &ierr);
08879         if (ierr != 0) {
08880             goto L13;
08881         }
08882         c0[0] = c3[0];
08883         c0[1] = c3[1];
08884         c0[2] = c3[2];
08885         first = FALSE_;
08886     } else {
08887 
08888 /* Set C2 to C3, compute the new circumcenter C3, and compute */
08889 /*   the area A of triangle (V1,C2,C3). */
08890 
08891         c2[0] = c3[0];
08892         c2[1] = c3[1];
08893         c2[2] = c3[2];
08894         circum_(v1, v2, v3, c3, &ierr);
08895         if (ierr != 0) {
08896             goto L13;
08897         }
08898         a = areas_(v1, c2, c3);
08899         if (a > amax) {
08900             goto L14;
08901         }
08902         asum += a;
08903     }
08904 
08905 /* Bottom on loop on neighbors of K. */
08906 
08907     if (lp != lpl) {
08908         goto L1;
08909     }
08910 
08911 /* Compute the area of triangle (V1,C3,C0). */
08912 
08913     a = areas_(v1, c3, c0);
08914     if (a > amax) {
08915         goto L14;
08916     }
08917     asum += a;
08918 
08919 /* No error encountered. */
08920 
08921     *ier = 0;
08922     ret_val = asum;
08923     return ret_val;
08924 
08925 /* Invalid input. */
08926 
08927 L11:
08928     *ier = 1;
08929     ret_val = 0.;
08930     return ret_val;
08931 
08932 /* K indexes a boundary node. */
08933 
08934 L12:
08935     *ier = 2;
08936     ret_val = 0.;
08937     return ret_val;
08938 
08939 /* Error in CIRCUM. */
08940 
08941 L13:
08942     *ier = 3;
08943     ret_val = 0.;
08944     return ret_val;
08945 
08946 /* AREAS value larger than AMAX. */
08947 
08948 L14:
08949     *ier = 4;
08950     ret_val = 0.;
08951     return ret_val;
08952 } /* areav_ */
08953 
08954 double areav_new__(int *k, int *n, double *x, double *y,
08955         double *z__, int *list, int *lptr, int *lend, int
08956         *ier)
08957 {
08958     /* System generated locals */
08959     double ret_val = 0;
08960 
08961     /* Builtin functions */
08962     //double acos(double);
08963 
08964     /* Local variables */
08965     static int m;
08966     static double c1[3], c2[3], c3[3];
08967     static int n1, n2, n3;
08968     static double v1[3], v2[3], v3[3];
08969     static int lp;
08970     static double c1s[3], c2s[3];
08971     static int lpl, ierr;
08972     static double asum;
08973     extern double angle_(double *, double *, double *);
08974     static float areav;
08975     extern /* Subroutine */ int circum_(double *, double *,
08976             double *, double *, int *);
08977 
08978 
08979 /* *********************************************************** */
08980 
08981 /*                                            Robert J. Renka */
08982 /*                                  Dept. of Computer Science */
08983 /*                                       Univ. of North Texas */
08984 /*                                           renka@cs.unt.edu */
08985 /*                                                   06/03/03 */
08986 
08987 /*   Given a Delaunay triangulation and the index K of an */
08988 /* interior node, this subroutine returns the (surface) area */
08989 /* of the Voronoi region associated with node K.  The Voronoi */
08990 /* region is the polygon whose vertices are the circumcenters */
08991 /* of the triangles that contain node K, where a triangle */
08992 /* circumcenter is the point (unit vector) lying at the same */
08993 /* angular distance from the three vertices and contained in */
08994 /* the same hemisphere as the vertices.  The Voronoi region */
08995 /* area is computed as Asum-(m-2)*Pi, where m is the number */
08996 /* of Voronoi vertices (neighbors of K) and Asum is the sum */
08997 /* of interior angles at the vertices. */
08998 
08999 
09000 /* On input: */
09001 
09002 /*       K = Nodal index in the range 1 to N. */
09003 
09004 /*       N = Number of nodes in the triangulation.  N > 3. */
09005 
09006 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09007 /*               coordinates of the nodes (unit vectors). */
09008 
09009 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09010 /*                        gulation.  Refer to Subroutine */
09011 /*                        TRMESH. */
09012 
09013 /* Input parameters are not altered by this function. */
09014 
09015 /* On output: */
09016 
09017 /*       AREAV = Area of Voronoi region K unless IER > 0, */
09018 /*               in which case AREAV = 0. */
09019 
09020 /*       IER = Error indicator: */
09021 /*             IER = 0 if no errors were encountered. */
09022 /*             IER = 1 if K or N is outside its valid range */
09023 /*                     on input. */
09024 /*             IER = 2 if K indexes a boundary node. */
09025 /*             IER = 3 if an error flag is returned by CIRCUM */
09026 /*                     (null triangle). */
09027 
09028 /* Modules required by AREAV:  ANGLE, CIRCUM */
09029 
09030 /* Intrinsic functions called by AREAV:  ACOS, DBLE */
09031 
09032 /* *********************************************************** */
09033 
09034 
09035 /* Test for invalid input. */
09036 
09037     /* Parameter adjustments */
09038     --lend;
09039     --z__;
09040     --y;
09041     --x;
09042     --list;
09043     --lptr;
09044 
09045     /* Function Body */
09046     if (*k < 1 || *k > *n || *n <= 3) {
09047         goto L11;
09048     }
09049 
09050 /* Initialization:  Set N3 to the last neighbor of N1 = K. */
09051 /*   The number of neighbors and the sum of interior angles */
09052 /*   are accumulated in M and ASUM, respectively. */
09053 
09054     n1 = *k;
09055     v1[0] = x[n1];
09056     v1[1] = y[n1];
09057     v1[2] = z__[n1];
09058     lpl = lend[n1];
09059     n3 = list[lpl];
09060     if (n3 < 0) {
09061         goto L12;
09062     }
09063     lp = lpl;
09064     m = 0;
09065     asum = 0.;
09066 
09067 /* Loop on triangles (N1,N2,N3) containing N1 = K. */
09068 
09069 L1:
09070     ++m;
09071     n2 = n3;
09072     lp = lptr[lp];
09073     n3 = list[lp];
09074     v2[0] = x[n2];
09075     v2[1] = y[n2];
09076     v2[2] = z__[n2];
09077     v3[0] = x[n3];
09078     v3[1] = y[n3];
09079     v3[2] = z__[n3];
09080     if (m == 1) {
09081 
09082 /* First triangle:  compute the circumcenter C2 and save a */
09083 /*   copy in C1S. */
09084 
09085         circum_(v1, v2, v3, c2, &ierr);
09086         if (ierr != 0) {
09087             goto L13;
09088         }
09089         c1s[0] = c2[0];
09090         c1s[1] = c2[1];
09091         c1s[2] = c2[2];
09092     } else if (m == 2) {
09093 
09094 /* Second triangle:  compute the circumcenter C3 and save a */
09095 /*   copy in C2S. */
09096 
09097         circum_(v1, v2, v3, c3, &ierr);
09098         if (ierr != 0) {
09099             goto L13;
09100         }
09101         c2s[0] = c3[0];
09102         c2s[1] = c3[1];
09103         c2s[2] = c3[2];
09104     } else {
09105 
09106 /* Set C1 to C2, set C2 to C3, compute the new circumcenter */
09107 /*   C3, and compute the interior angle at C2 from the */
09108 /*   sequence of vertices (C1,C2,C3). */
09109 
09110         c1[0] = c2[0];
09111         c1[1] = c2[1];
09112         c1[2] = c2[2];
09113         c2[0] = c3[0];
09114         c2[1] = c3[1];
09115         c2[2] = c3[2];
09116         circum_(v1, v2, v3, c3, &ierr);
09117         if (ierr != 0) {
09118             goto L13;
09119         }
09120         asum += angle_(c1, c2, c3);
09121     }
09122 
09123 /* Bottom on loop on neighbors of K. */
09124 
09125     if (lp != lpl) {
09126         goto L1;
09127     }
09128 
09129 /* C3 is the last vertex.  Compute its interior angle from */
09130 /*   the sequence (C2,C3,C1S). */
09131 
09132     asum += angle_(c2, c3, c1s);
09133 
09134 /* Compute the interior angle at C1S from */
09135 /*   the sequence (C3,C1S,C2S). */
09136 
09137     asum += angle_(c3, c1s, c2s);
09138 
09139 /* No error encountered. */
09140 
09141     *ier = 0;
09142     ret_val = asum - (double) (m - 2) * acos(-1.);
09143     return ret_val;
09144 
09145 /* Invalid input. */
09146 
09147 L11:
09148     *ier = 1;
09149     areav = 0.f;
09150     return ret_val;
09151 
09152 /* K indexes a boundary node. */
09153 
09154 L12:
09155     *ier = 2;
09156     areav = 0.f;
09157     return ret_val;
09158 
09159 /* Error in CIRCUM. */
09160 
09161 L13:
09162     *ier = 3;
09163     areav = 0.f;
09164     return ret_val;
09165 } /* areav_new__ */
09166 
09167 /* Subroutine */ int bdyadd_(int *kk, int *i1, int *i2, int *
09168         list, int *lptr, int *lend, int *lnew)
09169 {
09170     static int k, n1, n2, lp, lsav, nsav, next;
09171     extern /* Subroutine */ int insert_(int *, int *, int *,
09172             int *, int *);
09173 
09174 
09175 /* *********************************************************** */
09176 
09177 /*                                              From STRIPACK */
09178 /*                                            Robert J. Renka */
09179 /*                                  Dept. of Computer Science */
09180 /*                                       Univ. of North Texas */
09181 /*                                           renka@cs.unt.edu */
09182 /*                                                   07/11/96 */
09183 
09184 /*   This subroutine adds a boundary node to a triangulation */
09185 /* of a set of KK-1 points on the unit sphere.  The data */
09186 /* structure is updated with the insertion of node KK, but no */
09187 /* optimization is performed. */
09188 
09189 /*   This routine is identical to the similarly named routine */
09190 /* in TRIPACK. */
09191 
09192 
09193 /* On input: */
09194 
09195 /*       KK = Index of a node to be connected to the sequence */
09196 /*            of all visible boundary nodes.  KK .GE. 1 and */
09197 /*            KK must not be equal to I1 or I2. */
09198 
09199 /*       I1 = First (rightmost as viewed from KK) boundary */
09200 /*            node in the triangulation that is visible from */
09201 /*            node KK (the line segment KK-I1 intersects no */
09202 /*            arcs. */
09203 
09204 /*       I2 = Last (leftmost) boundary node that is visible */
09205 /*            from node KK.  I1 and I2 may be determined by */
09206 /*            Subroutine TRFIND. */
09207 
09208 /* The above parameters are not altered by this routine. */
09209 
09210 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09211 /*                             created by Subroutine TRMESH. */
09212 /*                             Nodes I1 and I2 must be in- */
09213 /*                             cluded in the triangulation. */
09214 
09215 /* On output: */
09216 
09217 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09218 /*                             the addition of node KK.  Node */
09219 /*                             KK is connected to I1, I2, and */
09220 /*                             all boundary nodes in between. */
09221 
09222 /* Module required by BDYADD:  INSERT */
09223 
09224 /* *********************************************************** */
09225 
09226 
09227 /* Local parameters: */
09228 
09229 /* K =     Local copy of KK */
09230 /* LP =    LIST pointer */
09231 /* LSAV =  LIST pointer */
09232 /* N1,N2 = Local copies of I1 and I2, respectively */
09233 /* NEXT =  Boundary node visible from K */
09234 /* NSAV =  Boundary node visible from K */
09235 
09236     /* Parameter adjustments */
09237     --lend;
09238     --lptr;
09239     --list;
09240 
09241     /* Function Body */
09242     k = *kk;
09243     n1 = *i1;
09244     n2 = *i2;
09245 
09246 /* Add K as the last neighbor of N1. */
09247 
09248     lp = lend[n1];
09249     lsav = lptr[lp];
09250     lptr[lp] = *lnew;
09251     list[*lnew] = -k;
09252     lptr[*lnew] = lsav;
09253     lend[n1] = *lnew;
09254     ++(*lnew);
09255     next = -list[lp];
09256     list[lp] = next;
09257     nsav = next;
09258 
09259 /* Loop on the remaining boundary nodes between N1 and N2, */
09260 /*   adding K as the first neighbor. */
09261 
09262 L1:
09263     lp = lend[next];
09264     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09265     if (next == n2) {
09266         goto L2;
09267     }
09268     next = -list[lp];
09269     list[lp] = next;
09270     goto L1;
09271 
09272 /* Add the boundary nodes between N1 and N2 as neighbors */
09273 /*   of node K. */
09274 
09275 L2:
09276     lsav = *lnew;
09277     list[*lnew] = n1;
09278     lptr[*lnew] = *lnew + 1;
09279     ++(*lnew);
09280     next = nsav;
09281 
09282 L3:
09283     if (next == n2) {
09284         goto L4;
09285     }
09286     list[*lnew] = next;
09287     lptr[*lnew] = *lnew + 1;
09288     ++(*lnew);
09289     lp = lend[next];
09290     next = list[lp];
09291     goto L3;
09292 
09293 L4:
09294     list[*lnew] = -n2;
09295     lptr[*lnew] = lsav;
09296     lend[k] = *lnew;
09297     ++(*lnew);
09298     return 0;
09299 } /* bdyadd_ */
09300 
09301 /* Subroutine */ int bnodes_(int *n, int *list, int *lptr,
09302         int *lend, int *nodes, int *nb, int *na, int *nt)
09303 {
09304     /* System generated locals */
09305     int i__1;
09306 
09307     /* Local variables */
09308     static int k, n0, lp, nn, nst;
09309 
09310 
09311 /* *********************************************************** */
09312 
09313 /*                                              From STRIPACK */
09314 /*                                            Robert J. Renka */
09315 /*                                  Dept. of Computer Science */
09316 /*                                       Univ. of North Texas */
09317 /*                                           renka@cs.unt.edu */
09318 /*                                                   06/26/96 */
09319 
09320 /*   Given a triangulation of N nodes on the unit sphere */
09321 /* created by Subroutine TRMESH, this subroutine returns an */
09322 /* array containing the indexes (if any) of the counterclock- */
09323 /* wise-ordered sequence of boundary nodes -- the nodes on */
09324 /* the boundary of the convex hull of the set of nodes.  (The */
09325 /* boundary is empty if the nodes do not lie in a single */
09326 /* hemisphere.)  The numbers of boundary nodes, arcs, and */
09327 /* triangles are also returned. */
09328 
09329 
09330 /* On input: */
09331 
09332 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09333 
09334 /*       LIST,LPTR,LEND = Data structure defining the trian- */
09335 /*                        gulation.  Refer to Subroutine */
09336 /*                        TRMESH. */
09337 
09338 /* The above parameters are not altered by this routine. */
09339 
09340 /*       NODES = int array of length at least NB */
09341 /*               (NB .LE. N). */
09342 
09343 /* On output: */
09344 
09345 /*       NODES = Ordered sequence of boundary node indexes */
09346 /*               in the range 1 to N (in the first NB loca- */
09347 /*               tions). */
09348 
09349 /*       NB = Number of boundary nodes. */
09350 
09351 /*       NA,NT = Number of arcs and triangles, respectively, */
09352 /*               in the triangulation. */
09353 
09354 /* Modules required by BNODES:  None */
09355 
09356 /* *********************************************************** */
09357 
09358 
09359 /* Local parameters: */
09360 
09361 /* K =   NODES index */
09362 /* LP =  LIST pointer */
09363 /* N0 =  Boundary node to be added to NODES */
09364 /* NN =  Local copy of N */
09365 /* NST = First element of nodes (arbitrarily chosen to be */
09366 /*         the one with smallest index) */
09367 
09368     /* Parameter adjustments */
09369     --lend;
09370     --list;
09371     --lptr;
09372     --nodes;
09373 
09374     /* Function Body */
09375     nn = *n;
09376 
09377 /* Search for a boundary node. */
09378 
09379     i__1 = nn;
09380     for (nst = 1; nst <= i__1; ++nst) {
09381         lp = lend[nst];
09382         if (list[lp] < 0) {
09383             goto L2;
09384         }
09385 /* L1: */
09386     }
09387 
09388 /* The triangulation contains no boundary nodes. */
09389 
09390     *nb = 0;
09391     *na = (nn - 2) * 3;
09392     *nt = nn - (2<<1);
09393     return 0;
09394 
09395 /* NST is the first boundary node encountered.  Initialize */
09396 /*   for traversal of the boundary. */
09397 
09398 L2:
09399     nodes[1] = nst;
09400     k = 1;
09401     n0 = nst;
09402 
09403 /* Traverse the boundary in counterclockwise order. */
09404 
09405 L3:
09406     lp = lend[n0];
09407     lp = lptr[lp];
09408     n0 = list[lp];
09409     if (n0 == nst) {
09410         goto L4;
09411     }
09412     ++k;
09413     nodes[k] = n0;
09414     goto L3;
09415 
09416 /* Store the counts. */
09417 
09418 L4:
09419     *nb = k;
09420     *nt = (*n << 1) - *nb - 2;
09421     *na = *nt + *n - 1;
09422     return 0;
09423 } /* bnodes_ */
09424 
09425 /* Subroutine */ int circle_(int *k, double *xc, double *yc,
09426         int *ier)
09427 {
09428     /* System generated locals */
09429     int i__1;
09430 
09431     /* Builtin functions */
09432     //double atan(double), cos(double), sin(double);
09433 
09434     /* Local variables */
09435     static double a, c__;
09436     static int i__;
09437     static double s;
09438     static int k2, k3;
09439     static double x0, y0;
09440     static int kk, np1;
09441 
09442 
09443 /* *********************************************************** */
09444 
09445 /*                                              From STRIPACK */
09446 /*                                            Robert J. Renka */
09447 /*                                  Dept. of Computer Science */
09448 /*                                       Univ. of North Texas */
09449 /*                                           renka@cs.unt.edu */
09450 /*                                                   04/06/90 */
09451 
09452 /*   This subroutine computes the coordinates of a sequence */
09453 /* of N equally spaced points on the unit circle centered at */
09454 /* (0,0).  An N-sided polygonal approximation to the circle */
09455 /* may be plotted by connecting (XC(I),YC(I)) to (XC(I+1), */
09456 /* YC(I+1)) for I = 1,...,N, where XC(N+1) = XC(1) and */
09457 /* YC(N+1) = YC(1).  A reasonable value for N in this case */
09458 /* is 2*PI*R, where R is the radius of the circle in device */
09459 /* coordinates. */
09460 
09461 
09462 /* On input: */
09463 
09464 /*       K = Number of points in each quadrant, defining N as */
09465 /*           4K.  K .GE. 1. */
09466 
09467 /*       XC,YC = Arrays of length at least N+1 = 4K+1. */
09468 
09469 /* K is not altered by this routine. */
09470 
09471 /* On output: */
09472 
09473 /*       XC,YC = Cartesian coordinates of the points on the */
09474 /*               unit circle in the first N+1 locations. */
09475 /*               XC(I) = cos(A*(I-1)), YC(I) = sin(A*(I-1)), */
09476 /*               where A = 2*PI/N.  Note that XC(N+1) = XC(1) */
09477 /*               and YC(N+1) = YC(1). */
09478 
09479 /*       IER = Error indicator: */
09480 /*             IER = 0 if no errors were encountered. */
09481 /*             IER = 1 if K < 1 on input. */
09482 
09483 /* Modules required by CIRCLE:  None */
09484 
09485 /* Intrinsic functions called by CIRCLE:  ATAN, COS, DBLE, */
09486 /*                                          SIN */
09487 
09488 /* *********************************************************** */
09489 
09490 
09491 /* Local parameters: */
09492 
09493 /* I =     DO-loop index and index for XC and YC */
09494 /* KK =    Local copy of K */
09495 /* K2 =    K*2 */
09496 /* K3 =    K*3 */
09497 /* NP1 =   N+1 = 4*K + 1 */
09498 /* A =     Angular separation between adjacent points */
09499 /* C,S =   Cos(A) and sin(A), respectively, defining a */
09500 /*           rotation through angle A */
09501 /* X0,Y0 = Cartesian coordinates of a point on the unit */
09502 /*           circle in the first quadrant */
09503 
09504     /* Parameter adjustments */
09505     --yc;
09506     --xc;
09507 
09508     /* Function Body */
09509     kk = *k;
09510     k2 = kk << 1;
09511     k3 = kk * 3;
09512     np1 = (kk << 2) + 1;
09513 
09514 /* Test for invalid input, compute A, C, and S, and */
09515 /*   initialize (X0,Y0) to (1,0). */
09516 
09517     if (kk < 1) {
09518         goto L2;
09519     }
09520     a = atan(1.) * 2. / (double) kk;
09521     c__ = cos(a);
09522     s = sin(a);
09523     x0 = 1.;
09524     y0 = 0.;
09525 
09526 /* Loop on points (X0,Y0) in the first quadrant, storing */
09527 /*   the point and its reflections about the x axis, the */
09528 /*   y axis, and the line y = -x. */
09529 
09530     i__1 = kk;
09531     for (i__ = 1; i__ <= i__1; ++i__) {
09532         xc[i__] = x0;
09533         yc[i__] = y0;
09534         xc[i__ + kk] = -y0;
09535         yc[i__ + kk] = x0;
09536         xc[i__ + k2] = -x0;
09537         yc[i__ + k2] = -y0;
09538         xc[i__ + k3] = y0;
09539         yc[i__ + k3] = -x0;
09540 
09541 /*   Rotate (X0,Y0) counterclockwise through angle A. */
09542 
09543         x0 = c__ * x0 - s * y0;
09544         y0 = s * x0 + c__ * y0;
09545 /* L1: */
09546     }
09547 
09548 /* Store the coordinates of the first point as the last */
09549 /*   point. */
09550 
09551     xc[np1] = xc[1];
09552     yc[np1] = yc[1];
09553     *ier = 0;
09554     return 0;
09555 
09556 /* K < 1. */
09557 
09558 L2:
09559     *ier = 1;
09560     return 0;
09561 } /* circle_ */
09562 
09563 /* Subroutine */ int circum_(double *v1, double *v2, double *v3,
09564         double *c__, int *ier)
09565 {
09566     /* Builtin functions */
09567     //double sqrt(double);
09568 
09569     /* Local variables */
09570     static int i__;
09571     static double e1[3], e2[3], cu[3], cnorm;
09572 
09573 
09574 /* *********************************************************** */
09575 
09576 /*                                              From STRIPACK */
09577 /*                                            Robert J. Renka */
09578 /*                                  Dept. of Computer Science */
09579 /*                                       Univ. of North Texas */
09580 /*                                           renka@cs.unt.edu */
09581 /*                                                   10/27/02 */
09582 
09583 /*   This subroutine returns the circumcenter of a spherical */
09584 /* triangle on the unit sphere:  the point on the sphere sur- */
09585 /* face that is equally distant from the three triangle */
09586 /* vertices and lies in the same hemisphere, where distance */
09587 /* is taken to be arc-length on the sphere surface. */
09588 
09589 
09590 /* On input: */
09591 
09592 /*       V1,V2,V3 = Arrays of length 3 containing the Carte- */
09593 /*                  sian coordinates of the three triangle */
09594 /*                  vertices (unit vectors) in CCW order. */
09595 
09596 /* The above parameters are not altered by this routine. */
09597 
09598 /*       C = Array of length 3. */
09599 
09600 /* On output: */
09601 
09602 /*       C = Cartesian coordinates of the circumcenter unless */
09603 /*           IER > 0, in which case C is not defined.  C = */
09604 /*           (V2-V1) X (V3-V1) normalized to a unit vector. */
09605 
09606 /*       IER = Error indicator: */
09607 /*             IER = 0 if no errors were encountered. */
09608 /*             IER = 1 if V1, V2, and V3 lie on a common */
09609 /*                     line:  (V2-V1) X (V3-V1) = 0. */
09610 /*             (The vertices are not tested for validity.) */
09611 
09612 /* Modules required by CIRCUM:  None */
09613 
09614 /* Intrinsic function called by CIRCUM:  SQRT */
09615 
09616 /* *********************************************************** */
09617 
09618 
09619 /* Local parameters: */
09620 
09621 /* CNORM = Norm of CU:  used to compute C */
09622 /* CU =    Scalar multiple of C:  E1 X E2 */
09623 /* E1,E2 = Edges of the underlying planar triangle: */
09624 /*           V2-V1 and V3-V1, respectively */
09625 /* I =     DO-loop index */
09626 
09627     /* Parameter adjustments */
09628     --c__;
09629     --v3;
09630     --v2;
09631     --v1;
09632 
09633     /* Function Body */
09634     for (i__ = 1; i__ <= 3; ++i__) {
09635         e1[i__ - 1] = v2[i__] - v1[i__];
09636         e2[i__ - 1] = v3[i__] - v1[i__];
09637 /* L1: */
09638     }
09639 
09640 /* Compute CU = E1 X E2 and CNORM**2. */
09641 
09642     cu[0] = e1[1] * e2[2] - e1[2] * e2[1];
09643     cu[1] = e1[2] * e2[0] - e1[0] * e2[2];
09644     cu[2] = e1[0] * e2[1] - e1[1] * e2[0];
09645     cnorm = cu[0] * cu[0] + cu[1] * cu[1] + cu[2] * cu[2];
09646 
09647 /* The vertices lie on a common line if and only if CU is */
09648 /*   the zero vector. */
09649 
09650     if (cnorm != 0.) {
09651 
09652 /*   No error:  compute C. */
09653 
09654         cnorm = sqrt(cnorm);
09655         for (i__ = 1; i__ <= 3; ++i__) {
09656             c__[i__] = cu[i__ - 1] / cnorm;
09657 /* L2: */
09658         }
09659 
09660 /* If the vertices are nearly identical, the problem is */
09661 /*   ill-conditioned and it is possible for the computed */
09662 /*   value of C to be 180 degrees off:  <C,V1> near -1 */
09663 /*   when it should be positive. */
09664 
09665         if (c__[1] * v1[1] + c__[2] * v1[2] + c__[3] * v1[3] < -.5) {
09666             c__[1] = -c__[1];
09667             c__[2] = -c__[2];
09668             c__[3] = -c__[3];
09669         }
09670         *ier = 0;
09671     } else {
09672 
09673 /*   CU = 0. */
09674 
09675         *ier = 1;
09676     }
09677     return 0;
09678 } /* circum_ */
09679 
09680 /* Subroutine */ int covsph_(int *kk, int *n0, int *list, int
09681         *lptr, int *lend, int *lnew)
09682 {
09683     static int k, lp, nst, lsav, next;
09684     extern /* Subroutine */ int insert_(int *, int *, int *,
09685             int *, int *);
09686 
09687 
09688 /* *********************************************************** */
09689 
09690 /*                                              From STRIPACK */
09691 /*                                            Robert J. Renka */
09692 /*                                  Dept. of Computer Science */
09693 /*                                       Univ. of North Texas */
09694 /*                                           renka@cs.unt.edu */
09695 /*                                                   07/17/96 */
09696 
09697 /*   This subroutine connects an exterior node KK to all */
09698 /* boundary nodes of a triangulation of KK-1 points on the */
09699 /* unit sphere, producing a triangulation that covers the */
09700 /* sphere.  The data structure is updated with the addition */
09701 /* of node KK, but no optimization is performed.  All boun- */
09702 /* dary nodes must be visible from node KK. */
09703 
09704 
09705 /* On input: */
09706 
09707 /*       KK = Index of the node to be connected to the set of */
09708 /*            all boundary nodes.  KK .GE. 4. */
09709 
09710 /*       N0 = Index of a boundary node (in the range 1 to */
09711 /*            KK-1).  N0 may be determined by Subroutine */
09712 /*            TRFIND. */
09713 
09714 /* The above parameters are not altered by this routine. */
09715 
09716 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
09717 /*                             created by Subroutine TRMESH. */
09718 /*                             Node N0 must be included in */
09719 /*                             the triangulation. */
09720 
09721 /* On output: */
09722 
09723 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
09724 /*                             the addition of node KK as the */
09725 /*                             last entry.  The updated */
09726 /*                             triangulation contains no */
09727 /*                             boundary nodes. */
09728 
09729 /* Module required by COVSPH:  INSERT */
09730 
09731 /* *********************************************************** */
09732 
09733 
09734 /* Local parameters: */
09735 
09736 /* K =     Local copy of KK */
09737 /* LP =    LIST pointer */
09738 /* LSAV =  LIST pointer */
09739 /* NEXT =  Boundary node visible from K */
09740 /* NST =   Local copy of N0 */
09741 
09742     /* Parameter adjustments */
09743     --lend;
09744     --lptr;
09745     --list;
09746 
09747     /* Function Body */
09748     k = *kk;
09749     nst = *n0;
09750 
09751 /* Traverse the boundary in clockwise order, inserting K as */
09752 /*   the first neighbor of each boundary node, and converting */
09753 /*   the boundary node to an interior node. */
09754 
09755     next = nst;
09756 L1:
09757     lp = lend[next];
09758     insert_(&k, &lp, &list[1], &lptr[1], lnew);
09759     next = -list[lp];
09760     list[lp] = next;
09761     if (next != nst) {
09762         goto L1;
09763     }
09764 
09765 /* Traverse the boundary again, adding each node to K's */
09766 /*   adjacency list. */
09767 
09768     lsav = *lnew;
09769 L2:
09770     lp = lend[next];
09771     list[*lnew] = next;
09772     lptr[*lnew] = *lnew + 1;
09773     ++(*lnew);
09774     next = list[lp];
09775     if (next != nst) {
09776         goto L2;
09777     }
09778 
09779     lptr[*lnew - 1] = lsav;
09780     lend[k] = *lnew - 1;
09781     return 0;
09782 } /* covsph_ */
09783 
09784 /* Subroutine */ int crlist_(int *n, int *ncol, double *x,
09785         double *y, double *z__, int *list, int *lend, int
09786         *lptr, int *lnew, int *ltri, int *listc, int *nb,
09787         double *xc, double *yc, double *zc, double *rc,
09788         int *ier)
09789 {
09790     /* System generated locals */
09791     int i__1, i__2;
09792 
09793     /* Builtin functions */
09794     //double acos(double);
09795 
09796     /* Local variables */
09797     static double c__[3], t;
09798     static int i1, i2, i3, i4, n0, n1, n2, n3, n4;
09799     static double v1[3], v2[3], v3[3];
09800     static int lp, kt, nn, nt, nm2, kt1, kt2, kt11, kt12, kt21, kt22, lpl,
09801              lpn;
09802     static long int swp;
09803     static int ierr;
09804     extern /* Subroutine */ int circum_(double *, double *,
09805             double *, double *, int *);
09806     extern int lstptr_(int *, int *, int *, int *);
09807     extern long int swptst_(int *, int *, int *, int *,
09808             double *, double *, double *);
09809 
09810 
09811 /* *********************************************************** */
09812 
09813 /*                                              From STRIPACK */
09814 /*                                            Robert J. Renka */
09815 /*                                  Dept. of Computer Science */
09816 /*                                       Univ. of North Texas */
09817 /*                                           renka@cs.unt.edu */
09818 /*                                                   03/05/03 */
09819 
09820 /*   Given a Delaunay triangulation of nodes on the surface */
09821 /* of the unit sphere, this subroutine returns the set of */
09822 /* triangle circumcenters corresponding to Voronoi vertices, */
09823 /* along with the circumradii and a list of triangle indexes */
09824 /* LISTC stored in one-to-one correspondence with LIST/LPTR */
09825 /* entries. */
09826 
09827 /*   A triangle circumcenter is the point (unit vector) lying */
09828 /* at the same angular distance from the three vertices and */
09829 /* contained in the same hemisphere as the vertices.  (Note */
09830 /* that the negative of a circumcenter is also equidistant */
09831 /* from the vertices.)  If the triangulation covers the sur- */
09832 /* face, the Voronoi vertices are the circumcenters of the */
09833 /* triangles in the Delaunay triangulation.  LPTR, LEND, and */
09834 /* LNEW are not altered in this case. */
09835 
09836 /*   On the other hand, if the nodes are contained in a sin- */
09837 /* gle hemisphere, the triangulation is implicitly extended */
09838 /* to the entire surface by adding pseudo-arcs (of length */
09839 /* greater than 180 degrees) between boundary nodes forming */
09840 /* pseudo-triangles whose 'circumcenters' are included in the */
09841 /* list.  This extension to the triangulation actually con- */
09842 /* sists of a triangulation of the set of boundary nodes in */
09843 /* which the swap test is reversed (a non-empty circumcircle */
09844 /* test).  The negative circumcenters are stored as the */
09845 /* pseudo-triangle 'circumcenters'.  LISTC, LPTR, LEND, and */
09846 /* LNEW contain a data structure corresponding to the ex- */
09847 /* tended triangulation (Voronoi diagram), but LIST is not */
09848 /* altered in this case.  Thus, if it is necessary to retain */
09849 /* the original (unextended) triangulation data structure, */
09850 /* copies of LPTR and LNEW must be saved before calling this */
09851 /* routine. */
09852 
09853 
09854 /* On input: */
09855 
09856 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
09857 /*           Note that, if N = 3, there are only two Voronoi */
09858 /*           vertices separated by 180 degrees, and the */
09859 /*           Voronoi regions are not well defined. */
09860 
09861 /*       NCOL = Number of columns reserved for LTRI.  This */
09862 /*              must be at least NB-2, where NB is the number */
09863 /*              of boundary nodes. */
09864 
09865 /*       X,Y,Z = Arrays of length N containing the Cartesian */
09866 /*               coordinates of the nodes (unit vectors). */
09867 
09868 /*       LIST = int array containing the set of adjacency */
09869 /*              lists.  Refer to Subroutine TRMESH. */
09870 
09871 /*       LEND = Set of pointers to ends of adjacency lists. */
09872 /*              Refer to Subroutine TRMESH. */
09873 
09874 /* The above parameters are not altered by this routine. */
09875 
09876 /*       LPTR = Array of pointers associated with LIST.  Re- */
09877 /*              fer to Subroutine TRMESH. */
09878 
09879 /*       LNEW = Pointer to the first empty location in LIST */
09880 /*              and LPTR (list length plus one). */
09881 
09882 /*       LTRI = int work space array dimensioned 6 by */
09883 /*              NCOL, or unused dummy parameter if NB = 0. */
09884 
09885 /*       LISTC = int array of length at least 3*NT, where */
09886 /*               NT = 2*N-4 is the number of triangles in the */
09887 /*               triangulation (after extending it to cover */
09888 /*               the entire surface if necessary). */
09889 
09890 /*       XC,YC,ZC,RC = Arrays of length NT = 2*N-4. */
09891 
09892 /* On output: */
09893 
09894 /*       LPTR = Array of pointers associated with LISTC: */
09895 /*              updated for the addition of pseudo-triangles */
09896 /*              if the original triangulation contains */
09897 /*              boundary nodes (NB > 0). */
09898 
09899 /*       LNEW = Pointer to the first empty location in LISTC */
09900 /*              and LPTR (list length plus one).  LNEW is not */
09901 /*              altered if NB = 0. */
09902 
09903 /*       LTRI = Triangle list whose first NB-2 columns con- */
09904 /*              tain the indexes of a clockwise-ordered */
09905 /*              sequence of vertices (first three rows) */
09906 /*              followed by the LTRI column indexes of the */
09907 /*              triangles opposite the vertices (or 0 */
09908 /*              denoting the exterior region) in the last */
09909 /*              three rows.  This array is not generally of */
09910 /*              any use. */
09911 
09912 /*       LISTC = Array containing triangle indexes (indexes */
09913 /*               to XC, YC, ZC, and RC) stored in 1-1 corres- */
09914 /*               pondence with LIST/LPTR entries (or entries */
09915 /*               that would be stored in LIST for the */
09916 /*               extended triangulation):  the index of tri- */
09917 /*               angle (N1,N2,N3) is stored in LISTC(K), */
09918 /*               LISTC(L), and LISTC(M), where LIST(K), */
09919 /*               LIST(L), and LIST(M) are the indexes of N2 */
09920 /*               as a neighbor of N1, N3 as a neighbor of N2, */
09921 /*               and N1 as a neighbor of N3.  The Voronoi */
09922 /*               region associated with a node is defined by */
09923 /*               the CCW-ordered sequence of circumcenters in */
09924 /*               one-to-one correspondence with its adjacency */
09925 /*               list (in the extended triangulation). */
09926 
09927 /*       NB = Number of boundary nodes unless IER = 1. */
09928 
09929 /*       XC,YC,ZC = Arrays containing the Cartesian coordi- */
09930 /*                  nates of the triangle circumcenters */
09931 /*                  (Voronoi vertices).  XC(I)**2 + YC(I)**2 */
09932 /*                  + ZC(I)**2 = 1.  The first NB-2 entries */
09933 /*                  correspond to pseudo-triangles if NB > 0. */
09934 
09935 /*       RC = Array containing circumradii (the arc lengths */
09936 /*            or angles between the circumcenters and associ- */
09937 /*            ated triangle vertices) in 1-1 correspondence */
09938 /*            with circumcenters. */
09939 
09940 /*       IER = Error indicator: */
09941 /*             IER = 0 if no errors were encountered. */
09942 /*             IER = 1 if N < 3. */
09943 /*             IER = 2 if NCOL < NB-2. */
09944 /*             IER = 3 if a triangle is degenerate (has ver- */
09945 /*                     tices lying on a common geodesic). */
09946 
09947 /* Modules required by CRLIST:  CIRCUM, LSTPTR, SWPTST */
09948 
09949 /* Intrinsic functions called by CRLIST:  ABS, ACOS */
09950 
09951 /* *********************************************************** */
09952 
09953 
09954 /* Local parameters: */
09955 
09956 /* C =         Circumcenter returned by Subroutine CIRCUM */
09957 /* I1,I2,I3 =  Permutation of (1,2,3):  LTRI row indexes */
09958 /* I4 =        LTRI row index in the range 1 to 3 */
09959 /* IERR =      Error flag for calls to CIRCUM */
09960 /* KT =        Triangle index */
09961 /* KT1,KT2 =   Indexes of a pair of adjacent pseudo-triangles */
09962 /* KT11,KT12 = Indexes of the pseudo-triangles opposite N1 */
09963 /*               and N2 as vertices of KT1 */
09964 /* KT21,KT22 = Indexes of the pseudo-triangles opposite N1 */
09965 /*               and N2 as vertices of KT2 */
09966 /* LP,LPN =    LIST pointers */
09967 /* LPL =       LIST pointer of the last neighbor of N1 */
09968 /* N0 =        Index of the first boundary node (initial */
09969 /*               value of N1) in the loop on boundary nodes */
09970 /*               used to store the pseudo-triangle indexes */
09971 /*               in LISTC */
09972 /* N1,N2,N3 =  Nodal indexes defining a triangle (CCW order) */
09973 /*               or pseudo-triangle (clockwise order) */
09974 /* N4 =        Index of the node opposite N2 -> N1 */
09975 /* NM2 =       N-2 */
09976 /* NN =        Local copy of N */
09977 /* NT =        Number of pseudo-triangles:  NB-2 */
09978 /* SWP =       long int variable set to TRUE in each optimiza- */
09979 /*               tion loop (loop on pseudo-arcs) iff a swap */
09980 /*               is performed */
09981 /* V1,V2,V3 =  Vertices of triangle KT = (N1,N2,N3) sent to */
09982 /*               Subroutine CIRCUM */
09983 
09984     /* Parameter adjustments */
09985     --lend;
09986     --z__;
09987     --y;
09988     --x;
09989     ltri -= 7;
09990     --list;
09991     --lptr;
09992     --listc;
09993     --xc;
09994     --yc;
09995     --zc;
09996     --rc;
09997 
09998     /* Function Body */
09999     nn = *n;
10000     *nb = 0;
10001     nt = 0;
10002     if (nn < 3) {
10003         goto L21;
10004     }
10005 
10006 /* Search for a boundary node N1. */
10007 
10008     i__1 = nn;
10009     for (n1 = 1; n1 <= i__1; ++n1) {
10010         lp = lend[n1];
10011         if (list[lp] < 0) {
10012             goto L2;
10013         }
10014 /* L1: */
10015     }
10016 
10017 /* The triangulation already covers the sphere. */
10018 
10019     goto L9;
10020 
10021 /* There are NB .GE. 3 boundary nodes.  Add NB-2 pseudo- */
10022 /*   triangles (N1,N2,N3) by connecting N3 to the NB-3 */
10023 /*   boundary nodes to which it is not already adjacent. */
10024 
10025 /*   Set N3 and N2 to the first and last neighbors, */
10026 /*     respectively, of N1. */
10027 
10028 L2:
10029     n2 = -list[lp];
10030     lp = lptr[lp];
10031     n3 = list[lp];
10032 
10033 /*   Loop on boundary arcs N1 -> N2 in clockwise order, */
10034 /*     storing triangles (N1,N2,N3) in column NT of LTRI */
10035 /*     along with the indexes of the triangles opposite */
10036 /*     the vertices. */
10037 
10038 L3:
10039     ++nt;
10040     if (nt <= *ncol) {
10041         ltri[nt * 6 + 1] = n1;
10042         ltri[nt * 6 + 2] = n2;
10043         ltri[nt * 6 + 3] = n3;
10044         ltri[nt * 6 + 4] = nt + 1;
10045         ltri[nt * 6 + 5] = nt - 1;
10046         ltri[nt * 6 + 6] = 0;
10047     }
10048     n1 = n2;
10049     lp = lend[n1];
10050     n2 = -list[lp];
10051     if (n2 != n3) {
10052         goto L3;
10053     }
10054 
10055     *nb = nt + 2;
10056     if (*ncol < nt) {
10057         goto L22;
10058     }
10059     ltri[nt * 6 + 4] = 0;
10060     if (nt == 1) {
10061         goto L7;
10062     }
10063 
10064 /* Optimize the exterior triangulation (set of pseudo- */
10065 /*   triangles) by applying swaps to the pseudo-arcs N1-N2 */
10066 /*   (pairs of adjacent pseudo-triangles KT1 and KT2 > KT1). */
10067 /*   The loop on pseudo-arcs is repeated until no swaps are */
10068 /*   performed. */
10069 
10070 L4:
10071     swp = FALSE_;
10072     i__1 = nt - 1;
10073     for (kt1 = 1; kt1 <= i__1; ++kt1) {
10074         for (i3 = 1; i3 <= 3; ++i3) {
10075             kt2 = ltri[i3 + 3 + kt1 * 6];
10076             if (kt2 <= kt1) {
10077                 goto L5;
10078             }
10079 
10080 /*   The LTRI row indexes (I1,I2,I3) of triangle KT1 = */
10081 /*     (N1,N2,N3) are a cyclical permutation of (1,2,3). */
10082 
10083             if (i3 == 1) {
10084                 i1 = 2;
10085                 i2 = 3;
10086             } else if (i3 == 2) {
10087                 i1 = 3;
10088                 i2 = 1;
10089             } else {
10090                 i1 = 1;
10091                 i2 = 2;
10092             }
10093             n1 = ltri[i1 + kt1 * 6];
10094             n2 = ltri[i2 + kt1 * 6];
10095             n3 = ltri[i3 + kt1 * 6];
10096 
10097 /*   KT2 = (N2,N1,N4) for N4 = LTRI(I,KT2), where */
10098 /*     LTRI(I+3,KT2) = KT1. */
10099 
10100             if (ltri[kt2 * 6 + 4] == kt1) {
10101                 i4 = 1;
10102             } else if (ltri[kt2 * 6 + 5] == kt1) {
10103                 i4 = 2;
10104             } else {
10105                 i4 = 3;
10106             }
10107             n4 = ltri[i4 + kt2 * 6];
10108 
10109 /*   The empty circumcircle test is reversed for the pseudo- */
10110 /*     triangles.  The reversal is implicit in the clockwise */
10111 /*     ordering of the vertices. */
10112 
10113             if (! swptst_(&n1, &n2, &n3, &n4, &x[1], &y[1], &z__[1])) {
10114                 goto L5;
10115             }
10116 
10117 /*   Swap arc N1-N2 for N3-N4.  KTij is the triangle opposite */
10118 /*     Nj as a vertex of KTi. */
10119 
10120             swp = TRUE_;
10121             kt11 = ltri[i1 + 3 + kt1 * 6];
10122             kt12 = ltri[i2 + 3 + kt1 * 6];
10123             if (i4 == 1) {
10124                 i2 = 2;
10125                 i1 = 3;
10126             } else if (i4 == 2) {
10127                 i2 = 3;
10128                 i1 = 1;
10129             } else {
10130                 i2 = 1;
10131                 i1 = 2;
10132             }
10133             kt21 = ltri[i1 + 3 + kt2 * 6];
10134             kt22 = ltri[i2 + 3 + kt2 * 6];
10135             ltri[kt1 * 6 + 1] = n4;
10136             ltri[kt1 * 6 + 2] = n3;
10137             ltri[kt1 * 6 + 3] = n1;
10138             ltri[kt1 * 6 + 4] = kt12;
10139             ltri[kt1 * 6 + 5] = kt22;
10140             ltri[kt1 * 6 + 6] = kt2;
10141             ltri[kt2 * 6 + 1] = n3;
10142             ltri[kt2 * 6 + 2] = n4;
10143             ltri[kt2 * 6 + 3] = n2;
10144             ltri[kt2 * 6 + 4] = kt21;
10145             ltri[kt2 * 6 + 5] = kt11;
10146             ltri[kt2 * 6 + 6] = kt1;
10147 
10148 /*   Correct the KT11 and KT22 entries that changed. */
10149 
10150             if (kt11 != 0) {
10151                 i4 = 4;
10152                 if (ltri[kt11 * 6 + 4] != kt1) {
10153                     i4 = 5;
10154                     if (ltri[kt11 * 6 + 5] != kt1) {
10155                         i4 = 6;
10156                     }
10157                 }
10158                 ltri[i4 + kt11 * 6] = kt2;
10159             }
10160             if (kt22 != 0) {
10161                 i4 = 4;
10162                 if (ltri[kt22 * 6 + 4] != kt2) {
10163                     i4 = 5;
10164                     if (ltri[kt22 * 6 + 5] != kt2) {
10165                         i4 = 6;
10166                     }
10167                 }
10168                 ltri[i4 + kt22 * 6] = kt1;
10169             }
10170 L5:
10171             ;
10172         }
10173 /* L6: */
10174     }
10175     if (swp) {
10176         goto L4;
10177     }
10178 
10179 /* Compute and store the negative circumcenters and radii of */
10180 /*   the pseudo-triangles in the first NT positions. */
10181 
10182 L7:
10183     i__1 = nt;
10184     for (kt = 1; kt <= i__1; ++kt) {
10185         n1 = ltri[kt * 6 + 1];
10186         n2 = ltri[kt * 6 + 2];
10187         n3 = ltri[kt * 6 + 3];
10188         v1[0] = x[n1];
10189         v1[1] = y[n1];
10190         v1[2] = z__[n1];
10191         v2[0] = x[n2];
10192         v2[1] = y[n2];
10193         v2[2] = z__[n2];
10194         v3[0] = x[n3];
10195         v3[1] = y[n3];
10196         v3[2] = z__[n3];
10197         circum_(v2, v1, v3, c__, &ierr);
10198         if (ierr != 0) {
10199             goto L23;
10200         }
10201 
10202 /*   Store the negative circumcenter and radius (computed */
10203 /*     from <V1,C>). */
10204 
10205         xc[kt] = -c__[0];
10206         yc[kt] = -c__[1];
10207         zc[kt] = -c__[2];
10208         t = -(v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2]);
10209         if (t < -1.) {
10210             t = -1.;
10211         }
10212         if (t > 1.) {
10213             t = 1.;
10214         }
10215         rc[kt] = acos(t);
10216 /* L8: */
10217     }
10218 
10219 /* Compute and store the circumcenters and radii of the */
10220 /*   actual triangles in positions KT = NT+1, NT+2, ... */
10221 /*   Also, store the triangle indexes KT in the appropriate */
10222 /*   LISTC positions. */
10223 
10224 L9:
10225     kt = nt;
10226 
10227 /*   Loop on nodes N1. */
10228 
10229     nm2 = nn - 2;
10230     i__1 = nm2;
10231     for (n1 = 1; n1 <= i__1; ++n1) {
10232         lpl = lend[n1];
10233         lp = lpl;
10234         n3 = list[lp];
10235 
10236 /*   Loop on adjacent neighbors N2,N3 of N1 for which N2 > N1 */
10237 /*     and N3 > N1. */
10238 
10239 L10:
10240         lp = lptr[lp];
10241         n2 = n3;
10242         n3 = (i__2 = list[lp], abs(i__2));
10243         if (n2 <= n1 || n3 <= n1) {
10244             goto L11;
10245         }
10246         ++kt;
10247 
10248 /*   Compute the circumcenter C of triangle KT = (N1,N2,N3). */
10249 
10250         v1[0] = x[n1];
10251         v1[1] = y[n1];
10252         v1[2] = z__[n1];
10253         v2[0] = x[n2];
10254         v2[1] = y[n2];
10255         v2[2] = z__[n2];
10256         v3[0] = x[n3];
10257         v3[1] = y[n3];
10258         v3[2] = z__[n3];
10259         circum_(v1, v2, v3, c__, &ierr);
10260         if (ierr != 0) {
10261             goto L23;
10262         }
10263 
10264 /*   Store the circumcenter, radius and triangle index. */
10265 
10266         xc[kt] = c__[0];
10267         yc[kt] = c__[1];
10268         zc[kt] = c__[2];
10269         t = v1[0] * c__[0] + v1[1] * c__[1] + v1[2] * c__[2];
10270         if (t < -1.) {
10271             t = -1.;
10272         }
10273         if (t > 1.) {
10274             t = 1.;
10275         }
10276         rc[kt] = acos(t);
10277 
10278 /*   Store KT in LISTC(LPN), where Abs(LIST(LPN)) is the */
10279 /*     index of N2 as a neighbor of N1, N3 as a neighbor */
10280 /*     of N2, and N1 as a neighbor of N3. */
10281 
10282         lpn = lstptr_(&lpl, &n2, &list[1], &lptr[1]);
10283         listc[lpn] = kt;
10284         lpn = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
10285         listc[lpn] = kt;
10286         lpn = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10287         listc[lpn] = kt;
10288 L11:
10289         if (lp != lpl) {
10290             goto L10;
10291         }
10292 /* L12: */
10293     }
10294     if (nt == 0) {
10295         goto L20;
10296     }
10297 
10298 /* Store the first NT triangle indexes in LISTC. */
10299 
10300 /*   Find a boundary triangle KT1 = (N1,N2,N3) with a */
10301 /*     boundary arc opposite N3. */
10302 
10303     kt1 = 0;
10304 L13:
10305     ++kt1;
10306     if (ltri[kt1 * 6 + 4] == 0) {
10307         i1 = 2;
10308         i2 = 3;
10309         i3 = 1;
10310         goto L14;
10311     } else if (ltri[kt1 * 6 + 5] == 0) {
10312         i1 = 3;
10313         i2 = 1;
10314         i3 = 2;
10315         goto L14;
10316     } else if (ltri[kt1 * 6 + 6] == 0) {
10317         i1 = 1;
10318         i2 = 2;
10319         i3 = 3;
10320         goto L14;
10321     }
10322     goto L13;
10323 L14:
10324     n1 = ltri[i1 + kt1 * 6];
10325     n0 = n1;
10326 
10327 /*   Loop on boundary nodes N1 in CCW order, storing the */
10328 /*     indexes of the clockwise-ordered sequence of triangles */
10329 /*     that contain N1.  The first triangle overwrites the */
10330 /*     last neighbor position, and the remaining triangles, */
10331 /*     if any, are appended to N1's adjacency list. */
10332 
10333 /*   A pointer to the first neighbor of N1 is saved in LPN. */
10334 
10335 L15:
10336     lp = lend[n1];
10337     lpn = lptr[lp];
10338     listc[lp] = kt1;
10339 
10340 /*   Loop on triangles KT2 containing N1. */
10341 
10342 L16:
10343     kt2 = ltri[i2 + 3 + kt1 * 6];
10344     if (kt2 != 0) {
10345 
10346 /*   Append KT2 to N1's triangle list. */
10347 
10348         lptr[lp] = *lnew;
10349         lp = *lnew;
10350         listc[lp] = kt2;
10351         ++(*lnew);
10352 
10353 /*   Set KT1 to KT2 and update (I1,I2,I3) such that */
10354 /*     LTRI(I1,KT1) = N1. */
10355 
10356         kt1 = kt2;
10357         if (ltri[kt1 * 6 + 1] == n1) {
10358             i1 = 1;
10359             i2 = 2;
10360             i3 = 3;
10361         } else if (ltri[kt1 * 6 + 2] == n1) {
10362             i1 = 2;
10363             i2 = 3;
10364             i3 = 1;
10365         } else {
10366             i1 = 3;
10367             i2 = 1;
10368             i3 = 2;
10369         }
10370         goto L16;
10371     }
10372 
10373 /*   Store the saved first-triangle pointer in LPTR(LP), set */
10374 /*     N1 to the next boundary node, test for termination, */
10375 /*     and permute the indexes:  the last triangle containing */
10376 /*     a boundary node is the first triangle containing the */
10377 /*     next boundary node. */
10378 
10379     lptr[lp] = lpn;
10380     n1 = ltri[i3 + kt1 * 6];
10381     if (n1 != n0) {
10382         i4 = i3;
10383         i3 = i2;
10384         i2 = i1;
10385         i1 = i4;
10386         goto L15;
10387     }
10388 
10389 /* No errors encountered. */
10390 
10391 L20:
10392     *ier = 0;
10393     return 0;
10394 
10395 /* N < 3. */
10396 
10397 L21:
10398     *ier = 1;
10399     return 0;
10400 
10401 /* Insufficient space reserved for LTRI. */
10402 
10403 L22:
10404     *ier = 2;
10405     return 0;
10406 
10407 /* Error flag returned by CIRCUM: KT indexes a null triangle. */
10408 
10409 L23:
10410     *ier = 3;
10411     return 0;
10412 } /* crlist_ */
10413 
10414 /* Subroutine */ int delarc_(int *n, int *io1, int *io2, int *
10415         list, int *lptr, int *lend, int *lnew, int *ier)
10416 {
10417     /* System generated locals */
10418     int i__1;
10419 
10420     /* Local variables */
10421     static int n1, n2, n3, lp, lph, lpl;
10422     extern /* Subroutine */ int delnb_(int *, int *, int *,
10423             int *, int *, int *, int *, int *);
10424     extern int lstptr_(int *, int *, int *, int *);
10425 
10426 
10427 /* *********************************************************** */
10428 
10429 /*                                              From STRIPACK */
10430 /*                                            Robert J. Renka */
10431 /*                                  Dept. of Computer Science */
10432 /*                                       Univ. of North Texas */
10433 /*                                           renka@cs.unt.edu */
10434 /*                                                   07/17/96 */
10435 
10436 /*   This subroutine deletes a boundary arc from a triangula- */
10437 /* tion.  It may be used to remove a null triangle from the */
10438 /* convex hull boundary.  Note, however, that if the union of */
10439 /* triangles is rendered nonconvex, Subroutines DELNOD, EDGE, */
10440 /* and TRFIND (and hence ADDNOD) may fail.  Also, Function */
10441 /* NEARND should not be called following an arc deletion. */
10442 
10443 /*   This routine is identical to the similarly named routine */
10444 /* in TRIPACK. */
10445 
10446 
10447 /* On input: */
10448 
10449 /*       N = Number of nodes in the triangulation.  N .GE. 4. */
10450 
10451 /*       IO1,IO2 = Indexes (in the range 1 to N) of a pair of */
10452 /*                 adjacent boundary nodes defining the arc */
10453 /*                 to be removed. */
10454 
10455 /* The above parameters are not altered by this routine. */
10456 
10457 /*       LIST,LPTR,LEND,LNEW = Triangulation data structure */
10458 /*                             created by Subroutine TRMESH. */
10459 
10460 /* On output: */
10461 
10462 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10463 /*                             the removal of arc IO1-IO2 */
10464 /*                             unless IER > 0. */
10465 
10466 /*       IER = Error indicator: */
10467 /*             IER = 0 if no errors were encountered. */
10468 /*             IER = 1 if N, IO1, or IO2 is outside its valid */
10469 /*                     range, or IO1 = IO2. */
10470 /*             IER = 2 if IO1-IO2 is not a boundary arc. */
10471 /*             IER = 3 if the node opposite IO1-IO2 is al- */
10472 /*                     ready a boundary node, and thus IO1 */
10473 /*                     or IO2 has only two neighbors or a */
10474 /*                     deletion would result in two triangu- */
10475 /*                     lations sharing a single node. */
10476 /*             IER = 4 if one of the nodes is a neighbor of */
10477 /*                     the other, but not vice versa, imply- */
10478 /*                     ing an invalid triangulation data */
10479 /*                     structure. */
10480 
10481 /* Module required by DELARC:  DELNB, LSTPTR */
10482 
10483 /* Intrinsic function called by DELARC:  ABS */
10484 
10485 /* *********************************************************** */
10486 
10487 
10488 /* Local parameters: */
10489 
10490 /* LP =       LIST pointer */
10491 /* LPH =      LIST pointer or flag returned by DELNB */
10492 /* LPL =      Pointer to the last neighbor of N1, N2, or N3 */
10493 /* N1,N2,N3 = Nodal indexes of a triangle such that N1->N2 */
10494 /*              is the directed boundary edge associated */
10495 /*              with IO1-IO2 */
10496 
10497     /* Parameter adjustments */
10498     --lend;
10499     --list;
10500     --lptr;
10501 
10502     /* Function Body */
10503     n1 = *io1;
10504     n2 = *io2;
10505 
10506 /* Test for errors, and set N1->N2 to the directed boundary */
10507 /*   edge associated with IO1-IO2:  (N1,N2,N3) is a triangle */
10508 /*   for some N3. */
10509 
10510     if (*n < 4 || n1 < 1 || n1 > *n || n2 < 1 || n2 > *n || n1 == n2) {
10511         *ier = 1;
10512         return 0;
10513     }
10514 
10515     lpl = lend[n2];
10516     if (-list[lpl] != n1) {
10517         n1 = n2;
10518         n2 = *io1;
10519         lpl = lend[n2];
10520         if (-list[lpl] != n1) {
10521             *ier = 2;
10522             return 0;
10523         }
10524     }
10525 
10526 /* Set N3 to the node opposite N1->N2 (the second neighbor */
10527 /*   of N1), and test for error 3 (N3 already a boundary */
10528 /*   node). */
10529 
10530     lpl = lend[n1];
10531     lp = lptr[lpl];
10532     lp = lptr[lp];
10533     n3 = (i__1 = list[lp], abs(i__1));
10534     lpl = lend[n3];
10535     if (list[lpl] <= 0) {
10536         *ier = 3;
10537         return 0;
10538     }
10539 
10540 /* Delete N2 as a neighbor of N1, making N3 the first */
10541 /*   neighbor, and test for error 4 (N2 not a neighbor */
10542 /*   of N1).  Note that previously computed pointers may */
10543 /*   no longer be valid following the call to DELNB. */
10544 
10545     delnb_(&n1, &n2, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10546     if (lph < 0) {
10547         *ier = 4;
10548         return 0;
10549     }
10550 
10551 /* Delete N1 as a neighbor of N2, making N3 the new last */
10552 /*   neighbor. */
10553 
10554     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], lnew, &lph);
10555 
10556 /* Make N3 a boundary node with first neighbor N2 and last */
10557 /*   neighbor N1. */
10558 
10559     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
10560     lend[n3] = lp;
10561     list[lp] = -n1;
10562 
10563 /* No errors encountered. */
10564 
10565     *ier = 0;
10566     return 0;
10567 } /* delarc_ */
10568 
10569 /* Subroutine */ int delnb_(int *n0, int *nb, int *n, int *
10570         list, int *lptr, int *lend, int *lnew, int *lph)
10571 {
10572     /* System generated locals */
10573     int i__1;
10574 
10575     /* Local variables */
10576     static int i__, lp, nn, lpb, lpl, lpp, lnw;
10577 
10578 
10579 /* *********************************************************** */
10580 
10581 /*                                              From STRIPACK */
10582 /*                                            Robert J. Renka */
10583 /*                                  Dept. of Computer Science */
10584 /*                                       Univ. of North Texas */
10585 /*                                           renka@cs.unt.edu */
10586 /*                                                   07/29/98 */
10587 
10588 /*   This subroutine deletes a neighbor NB from the adjacency */
10589 /* list of node N0 (but N0 is not deleted from the adjacency */
10590 /* list of NB) and, if NB is a boundary node, makes N0 a */
10591 /* boundary node.  For pointer (LIST index) LPH to NB as a */
10592 /* neighbor of N0, the empty LIST,LPTR location LPH is filled */
10593 /* in with the values at LNEW-1, pointer LNEW-1 (in LPTR and */
10594 /* possibly in LEND) is changed to LPH, and LNEW is decremen- */
10595 /* ted.  This requires a search of LEND and LPTR entailing an */
10596 /* expected operation count of O(N). */
10597 
10598 /*   This routine is identical to the similarly named routine */
10599 /* in TRIPACK. */
10600 
10601 
10602 /* On input: */
10603 
10604 /*       N0,NB = Indexes, in the range 1 to N, of a pair of */
10605 /*               nodes such that NB is a neighbor of N0. */
10606 /*               (N0 need not be a neighbor of NB.) */
10607 
10608 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
10609 
10610 /* The above parameters are not altered by this routine. */
10611 
10612 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10613 /*                             triangulation. */
10614 
10615 /* On output: */
10616 
10617 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
10618 /*                             the removal of NB from the ad- */
10619 /*                             jacency list of N0 unless */
10620 /*                             LPH < 0. */
10621 
10622 /*       LPH = List pointer to the hole (NB as a neighbor of */
10623 /*             N0) filled in by the values at LNEW-1 or error */
10624 /*             indicator: */
10625 /*             LPH > 0 if no errors were encountered. */
10626 /*             LPH = -1 if N0, NB, or N is outside its valid */
10627 /*                      range. */
10628 /*             LPH = -2 if NB is not a neighbor of N0. */
10629 
10630 /* Modules required by DELNB:  None */
10631 
10632 /* Intrinsic function called by DELNB:  ABS */
10633 
10634 /* *********************************************************** */
10635 
10636 
10637 /* Local parameters: */
10638 
10639 /* I =   DO-loop index */
10640 /* LNW = LNEW-1 (output value of LNEW) */
10641 /* LP =  LIST pointer of the last neighbor of NB */
10642 /* LPB = Pointer to NB as a neighbor of N0 */
10643 /* LPL = Pointer to the last neighbor of N0 */
10644 /* LPP = Pointer to the neighbor of N0 that precedes NB */
10645 /* NN =  Local copy of N */
10646 
10647     /* Parameter adjustments */
10648     --lend;
10649     --list;
10650     --lptr;
10651 
10652     /* Function Body */
10653     nn = *n;
10654 
10655 /* Test for error 1. */
10656 
10657     if (*n0 < 1 || *n0 > nn || *nb < 1 || *nb > nn || nn < 3) {
10658         *lph = -1;
10659         return 0;
10660     }
10661 
10662 /*   Find pointers to neighbors of N0: */
10663 
10664 /*     LPL points to the last neighbor, */
10665 /*     LPP points to the neighbor NP preceding NB, and */
10666 /*     LPB points to NB. */
10667 
10668     lpl = lend[*n0];
10669     lpp = lpl;
10670     lpb = lptr[lpp];
10671 L1:
10672     if (list[lpb] == *nb) {
10673         goto L2;
10674     }
10675     lpp = lpb;
10676     lpb = lptr[lpp];
10677     if (lpb != lpl) {
10678         goto L1;
10679     }
10680 
10681 /*   Test for error 2 (NB not found). */
10682 
10683     if ((i__1 = list[lpb], abs(i__1)) != *nb) {
10684         *lph = -2;
10685         return 0;
10686     }
10687 
10688 /*   NB is the last neighbor of N0.  Make NP the new last */
10689 /*     neighbor and, if NB is a boundary node, then make N0 */
10690 /*     a boundary node. */
10691 
10692     lend[*n0] = lpp;
10693     lp = lend[*nb];
10694     if (list[lp] < 0) {
10695         list[lpp] = -list[lpp];
10696     }
10697     goto L3;
10698 
10699 /*   NB is not the last neighbor of N0.  If NB is a boundary */
10700 /*     node and N0 is not, then make N0 a boundary node with */
10701 /*     last neighbor NP. */
10702 
10703 L2:
10704     lp = lend[*nb];
10705     if (list[lp] < 0 && list[lpl] > 0) {
10706         lend[*n0] = lpp;
10707         list[lpp] = -list[lpp];
10708     }
10709 
10710 /*   Update LPTR so that the neighbor following NB now fol- */
10711 /*     lows NP, and fill in the hole at location LPB. */
10712 
10713 L3:
10714     lptr[lpp] = lptr[lpb];
10715     lnw = *lnew - 1;
10716     list[lpb] = list[lnw];
10717     lptr[lpb] = lptr[lnw];
10718     for (i__ = nn; i__ >= 1; --i__) {
10719         if (lend[i__] == lnw) {
10720             lend[i__] = lpb;
10721             goto L5;
10722         }
10723 /* L4: */
10724     }
10725 
10726 L5:
10727     i__1 = lnw - 1;
10728     for (i__ = 1; i__ <= i__1; ++i__) {
10729         if (lptr[i__] == lnw) {
10730             lptr[i__] = lpb;
10731         }
10732 /* L6: */
10733     }
10734 
10735 /* No errors encountered. */
10736 
10737     *lnew = lnw;
10738     *lph = lpb;
10739     return 0;
10740 } /* delnb_ */
10741 
10742 /* Subroutine */ int delnod_(int *k, int *n, double *x,
10743         double *y, double *z__, int *list, int *lptr, int
10744         *lend, int *lnew, int *lwk, int *iwk, int *ier)
10745 {
10746     /* System generated locals */
10747     int i__1;
10748 
10749     /* Local variables */
10750     static int i__, j, n1, n2;
10751     static double x1, x2, y1, y2, z1, z2;
10752     static int nl, lp, nn, nr;
10753     static double xl, yl, zl, xr, yr, zr;
10754     static int nnb, lp21, lpf, lph, lpl, lpn, iwl, nit, lnw, lpl2;
10755     extern long int left_(double *, double *, double *, double
10756             *, double *, double *, double *, double *,
10757             double *);
10758     static long int bdry;
10759     static int ierr, lwkl;
10760     extern /* Subroutine */ int swap_(int *, int *, int *,
10761             int *, int *, int *, int *, int *), delnb_(
10762             int *, int *, int *, int *, int *, int *,
10763             int *, int *);
10764     extern int nbcnt_(int *, int *);
10765     extern /* Subroutine */ int optim_(double *, double *, double
10766             *, int *, int *, int *, int *, int *, int
10767             *, int *);
10768     static int nfrst;
10769     extern int lstptr_(int *, int *, int *, int *);
10770 
10771 
10772 /* *********************************************************** */
10773 
10774 /*                                              From STRIPACK */
10775 /*                                            Robert J. Renka */
10776 /*                                  Dept. of Computer Science */
10777 /*                                       Univ. of North Texas */
10778 /*                                           renka@cs.unt.edu */
10779 /*                                                   11/30/99 */
10780 
10781 /*   This subroutine deletes node K (along with all arcs */
10782 /* incident on node K) from a triangulation of N nodes on the */
10783 /* unit sphere, and inserts arcs as necessary to produce a */
10784 /* triangulation of the remaining N-1 nodes.  If a Delaunay */
10785 /* triangulation is input, a Delaunay triangulation will */
10786 /* result, and thus, DELNOD reverses the effect of a call to */
10787 /* Subroutine ADDNOD. */
10788 
10789 
10790 /* On input: */
10791 
10792 /*       K = Index (for X, Y, and Z) of the node to be */
10793 /*           deleted.  1 .LE. K .LE. N. */
10794 
10795 /* K is not altered by this routine. */
10796 
10797 /*       N = Number of nodes in the triangulation on input. */
10798 /*           N .GE. 4.  Note that N will be decremented */
10799 /*           following the deletion. */
10800 
10801 /*       X,Y,Z = Arrays of length N containing the Cartesian */
10802 /*               coordinates of the nodes in the triangula- */
10803 /*               tion. */
10804 
10805 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
10806 /*                             triangulation.  Refer to Sub- */
10807 /*                             routine TRMESH. */
10808 
10809 /*       LWK = Number of columns reserved for IWK.  LWK must */
10810 /*             be at least NNB-3, where NNB is the number of */
10811 /*             neighbors of node K, including an extra */
10812 /*             pseudo-node if K is a boundary node. */
10813 
10814 /*       IWK = int work array dimensioned 2 by LWK (or */
10815 /*             array of length .GE. 2*LWK). */
10816 
10817 /* On output: */
10818 
10819 /*       N = Number of nodes in the triangulation on output. */
10820 /*           The input value is decremented unless 1 .LE. IER */
10821 /*           .LE. 4. */
10822 
10823 /*       X,Y,Z = Updated arrays containing nodal coordinates */
10824 /*               (with elements K+1,...,N+1 shifted up one */
10825 /*               position, thus overwriting element K) unless */
10826 /*               1 .LE. IER .LE. 4. */
10827 
10828 /*       LIST,LPTR,LEND,LNEW = Updated triangulation data */
10829 /*                             structure reflecting the dele- */
10830 /*                             tion unless 1 .LE. IER .LE. 4. */
10831 /*                             Note that the data structure */
10832 /*                             may have been altered if IER > */
10833 /*                             3. */
10834 
10835 /*       LWK = Number of IWK columns required unless IER = 1 */
10836 /*             or IER = 3. */
10837 
10838 /*       IWK = Indexes of the endpoints of the new arcs added */
10839 /*             unless LWK = 0 or 1 .LE. IER .LE. 4.  (Arcs */
10840 /*             are associated with columns, or pairs of */
10841 /*             adjacent elements if IWK is declared as a */
10842 /*             singly-subscripted array.) */
10843 
10844 /*       IER = Error indicator: */
10845 /*             IER = 0 if no errors were encountered. */
10846 /*             IER = 1 if K or N is outside its valid range */
10847 /*                     or LWK < 0 on input. */
10848 /*             IER = 2 if more space is required in IWK. */
10849 /*                     Refer to LWK. */
10850 /*             IER = 3 if the triangulation data structure is */
10851 /*                     invalid on input. */
10852 /*             IER = 4 if K indexes an interior node with */
10853 /*                     four or more neighbors, none of which */
10854 /*                     can be swapped out due to collineari- */
10855 /*                     ty, and K cannot therefore be deleted. */
10856 /*             IER = 5 if an error flag (other than IER = 1) */
10857 /*                     was returned by OPTIM.  An error */
10858 /*                     message is written to the standard */
10859 /*                     output unit in this case. */
10860 /*             IER = 6 if error flag 1 was returned by OPTIM. */
10861 /*                     This is not necessarily an error, but */
10862 /*                     the arcs may not be optimal. */
10863 
10864 /*   Note that the deletion may result in all remaining nodes */
10865 /* being collinear.  This situation is not flagged. */
10866 
10867 /* Modules required by DELNOD:  DELNB, LEFT, LSTPTR, NBCNT, */
10868 /*                                OPTIM, SWAP, SWPTST */
10869 
10870 /* Intrinsic function called by DELNOD:  ABS */
10871 
10872 /* *********************************************************** */
10873 
10874 
10875 /* Local parameters: */
10876 
10877 /* BDRY =    long int variable with value TRUE iff N1 is a */
10878 /*             boundary node */
10879 /* I,J =     DO-loop indexes */
10880 /* IERR =    Error flag returned by OPTIM */
10881 /* IWL =     Number of IWK columns containing arcs */
10882 /* LNW =     Local copy of LNEW */
10883 /* LP =      LIST pointer */
10884 /* LP21 =    LIST pointer returned by SWAP */
10885 /* LPF,LPL = Pointers to the first and last neighbors of N1 */
10886 /* LPH =     Pointer (or flag) returned by DELNB */
10887 /* LPL2 =    Pointer to the last neighbor of N2 */
10888 /* LPN =     Pointer to a neighbor of N1 */
10889 /* LWKL =    Input value of LWK */
10890 /* N1 =      Local copy of K */
10891 /* N2 =      Neighbor of N1 */
10892 /* NFRST =   First neighbor of N1:  LIST(LPF) */
10893 /* NIT =     Number of iterations in OPTIM */
10894 /* NR,NL =   Neighbors of N1 preceding (to the right of) and */
10895 /*             following (to the left of) N2, respectively */
10896 /* NN =      Number of nodes in the triangulation */
10897 /* NNB =     Number of neighbors of N1 (including a pseudo- */
10898 /*             node representing the boundary if N1 is a */
10899 /*             boundary node) */
10900 /* X1,Y1,Z1 = Coordinates of N1 */
10901 /* X2,Y2,Z2 = Coordinates of N2 */
10902 /* XL,YL,ZL = Coordinates of NL */
10903 /* XR,YR,ZR = Coordinates of NR */
10904 
10905 
10906 /* Set N1 to K and NNB to the number of neighbors of N1 (plus */
10907 /*   one if N1 is a boundary node), and test for errors.  LPF */
10908 /*   and LPL are LIST indexes of the first and last neighbors */
10909 /*   of N1, IWL is the number of IWK columns containing arcs, */
10910 /*   and BDRY is TRUE iff N1 is a boundary node. */
10911 
10912     /* Parameter adjustments */
10913     iwk -= 3;
10914     --lend;
10915     --lptr;
10916     --list;
10917     --z__;
10918     --y;
10919     --x;
10920 
10921     /* Function Body */
10922     n1 = *k;
10923     nn = *n;
10924     if (n1 < 1 || n1 > nn || nn < 4 || *lwk < 0) {
10925         goto L21;
10926     }
10927     lpl = lend[n1];
10928     lpf = lptr[lpl];
10929     nnb = nbcnt_(&lpl, &lptr[1]);
10930     bdry = list[lpl] < 0;
10931     if (bdry) {
10932         ++nnb;
10933     }
10934     if (nnb < 3) {
10935         goto L23;
10936     }
10937     lwkl = *lwk;
10938     *lwk = nnb - 3;
10939     if (lwkl < *lwk) {
10940         goto L22;
10941     }
10942     iwl = 0;
10943     if (nnb == 3) {
10944         goto L3;
10945     }
10946 
10947 /* Initialize for loop on arcs N1-N2 for neighbors N2 of N1, */
10948 /*   beginning with the second neighbor.  NR and NL are the */
10949 /*   neighbors preceding and following N2, respectively, and */
10950 /*   LP indexes NL.  The loop is exited when all possible */
10951 /*   swaps have been applied to arcs incident on N1. */
10952 
10953     x1 = x[n1];
10954     y1 = y[n1];
10955     z1 = z__[n1];
10956     nfrst = list[lpf];
10957     nr = nfrst;
10958     xr = x[nr];
10959     yr = y[nr];
10960     zr = z__[nr];
10961     lp = lptr[lpf];
10962     n2 = list[lp];
10963     x2 = x[n2];
10964     y2 = y[n2];
10965     z2 = z__[n2];
10966     lp = lptr[lp];
10967 
10968 /* Top of loop:  set NL to the neighbor following N2. */
10969 
10970 L1:
10971     nl = (i__1 = list[lp], abs(i__1));
10972     if (nl == nfrst && bdry) {
10973         goto L3;
10974     }
10975     xl = x[nl];
10976     yl = y[nl];
10977     zl = z__[nl];
10978 
10979 /*   Test for a convex quadrilateral.  To avoid an incorrect */
10980 /*     test caused by collinearity, use the fact that if N1 */
10981 /*     is a boundary node, then N1 LEFT NR->NL and if N2 is */
10982 /*     a boundary node, then N2 LEFT NL->NR. */
10983 
10984     lpl2 = lend[n2];
10985     if (! ((bdry || left_(&xr, &yr, &zr, &xl, &yl, &zl, &x1, &y1, &z1)) && (
10986             list[lpl2] < 0 || left_(&xl, &yl, &zl, &xr, &yr, &zr, &x2, &y2, &
10987             z2)))) {
10988 
10989 /*   Nonconvex quadrilateral -- no swap is possible. */
10990 
10991         nr = n2;
10992         xr = x2;
10993         yr = y2;
10994         zr = z2;
10995         goto L2;
10996     }
10997 
10998 /*   The quadrilateral defined by adjacent triangles */
10999 /*     (N1,N2,NL) and (N2,N1,NR) is convex.  Swap in */
11000 /*     NL-NR and store it in IWK unless NL and NR are */
11001 /*     already adjacent, in which case the swap is not */
11002 /*     possible.  Indexes larger than N1 must be decremented */
11003 /*     since N1 will be deleted from X, Y, and Z. */
11004 
11005     swap_(&nl, &nr, &n1, &n2, &list[1], &lptr[1], &lend[1], &lp21);
11006     if (lp21 == 0) {
11007         nr = n2;
11008         xr = x2;
11009         yr = y2;
11010         zr = z2;
11011         goto L2;
11012     }
11013     ++iwl;
11014     if (nl <= n1) {
11015         iwk[(iwl << 1) + 1] = nl;
11016     } else {
11017         iwk[(iwl << 1) + 1] = nl - 1;
11018     }
11019     if (nr <= n1) {
11020         iwk[(iwl << 1) + 2] = nr;
11021     } else {
11022         iwk[(iwl << 1) + 2] = nr - 1;
11023     }
11024 
11025 /*   Recompute the LIST indexes and NFRST, and decrement NNB. */
11026 
11027     lpl = lend[n1];
11028     --nnb;
11029     if (nnb == 3) {
11030         goto L3;
11031     }
11032     lpf = lptr[lpl];
11033     nfrst = list[lpf];
11034     lp = lstptr_(&lpl, &nl, &list[1], &lptr[1]);
11035     if (nr == nfrst) {
11036         goto L2;
11037     }
11038 
11039 /*   NR is not the first neighbor of N1. */
11040 /*     Back up and test N1-NR for a swap again:  Set N2 to */
11041 /*     NR and NR to the previous neighbor of N1 -- the */
11042 /*     neighbor of NR which follows N1.  LP21 points to NL */
11043 /*     as a neighbor of NR. */
11044 
11045     n2 = nr;
11046     x2 = xr;
11047     y2 = yr;
11048     z2 = zr;
11049     lp21 = lptr[lp21];
11050     lp21 = lptr[lp21];
11051     nr = (i__1 = list[lp21], abs(i__1));
11052     xr = x[nr];
11053     yr = y[nr];
11054     zr = z__[nr];
11055     goto L1;
11056 
11057 /*   Bottom of loop -- test for termination of loop. */
11058 
11059 L2:
11060     if (n2 == nfrst) {
11061         goto L3;
11062     }
11063     n2 = nl;
11064     x2 = xl;
11065     y2 = yl;
11066     z2 = zl;
11067     lp = lptr[lp];
11068     goto L1;
11069 
11070 /* Delete N1 and all its incident arcs.  If N1 is an interior */
11071 /*   node and either NNB > 3 or NNB = 3 and N2 LEFT NR->NL, */
11072 /*   then N1 must be separated from its neighbors by a plane */
11073 /*   containing the origin -- its removal reverses the effect */
11074 /*   of a call to COVSPH, and all its neighbors become */
11075 /*   boundary nodes.  This is achieved by treating it as if */
11076 /*   it were a boundary node (setting BDRY to TRUE, changing */
11077 /*   a sign in LIST, and incrementing NNB). */
11078 
11079 L3:
11080     if (! bdry) {
11081         if (nnb > 3) {
11082             bdry = TRUE_;
11083         } else {
11084             lpf = lptr[lpl];
11085             nr = list[lpf];
11086             lp = lptr[lpf];
11087             n2 = list[lp];
11088             nl = list[lpl];
11089             bdry = left_(&x[nr], &y[nr], &z__[nr], &x[nl], &y[nl], &z__[nl], &
11090                     x[n2], &y[n2], &z__[n2]);
11091         }
11092         if (bdry) {
11093 
11094 /*   IF a boundary node already exists, then N1 and its */
11095 /*     neighbors cannot be converted to boundary nodes. */
11096 /*     (They must be collinear.)  This is a problem if */
11097 /*     NNB > 3. */
11098 
11099             i__1 = nn;
11100             for (i__ = 1; i__ <= i__1; ++i__) {
11101                 if (list[lend[i__]] < 0) {
11102                     bdry = FALSE_;
11103                     goto L5;
11104                 }
11105 /* L4: */
11106             }
11107             list[lpl] = -list[lpl];
11108             ++nnb;
11109         }
11110     }
11111 L5:
11112     if (! bdry && nnb > 3) {
11113         goto L24;
11114     }
11115 
11116 /* Initialize for loop on neighbors.  LPL points to the last */
11117 /*   neighbor of N1.  LNEW is stored in local variable LNW. */
11118 
11119     lp = lpl;
11120     lnw = *lnew;
11121 
11122 /* Loop on neighbors N2 of N1, beginning with the first. */
11123 
11124 L6:
11125     lp = lptr[lp];
11126     n2 = (i__1 = list[lp], abs(i__1));
11127     delnb_(&n2, &n1, n, &list[1], &lptr[1], &lend[1], &lnw, &lph);
11128     if (lph < 0) {
11129         goto L23;
11130     }
11131 
11132 /*   LP and LPL may require alteration. */
11133 
11134     if (lpl == lnw) {
11135         lpl = lph;
11136     }
11137     if (lp == lnw) {
11138         lp = lph;
11139     }
11140     if (lp != lpl) {
11141         goto L6;
11142     }
11143 
11144 /* Delete N1 from X, Y, Z, and LEND, and remove its adjacency */
11145 /*   list from LIST and LPTR.  LIST entries (nodal indexes) */
11146 /*   which are larger than N1 must be decremented. */
11147 
11148     --nn;
11149     if (n1 > nn) {
11150         goto L9;
11151     }
11152     i__1 = nn;
11153     for (i__ = n1; i__ <= i__1; ++i__) {
11154         x[i__] = x[i__ + 1];
11155         y[i__] = y[i__ + 1];
11156         z__[i__] = z__[i__ + 1];
11157         lend[i__] = lend[i__ + 1];
11158 /* L7: */
11159     }
11160 
11161     i__1 = lnw - 1;
11162     for (i__ = 1; i__ <= i__1; ++i__) {
11163         if (list[i__] > n1) {
11164             --list[i__];
11165         }
11166         if (list[i__] < -n1) {
11167             ++list[i__];
11168         }
11169 /* L8: */
11170     }
11171 
11172 /*   For LPN = first to last neighbors of N1, delete the */
11173 /*     preceding neighbor (indexed by LP). */
11174 
11175 /*   Each empty LIST,LPTR location LP is filled in with the */
11176 /*     values at LNW-1, and LNW is decremented.  All pointers */
11177 /*     (including those in LPTR and LEND) with value LNW-1 */
11178 /*     must be changed to LP. */
11179 
11180 /*  LPL points to the last neighbor of N1. */
11181 
11182 L9:
11183     if (bdry) {
11184         --nnb;
11185     }
11186     lpn = lpl;
11187     i__1 = nnb;
11188     for (j = 1; j <= i__1; ++j) {
11189         --lnw;
11190         lp = lpn;
11191         lpn = lptr[lp];
11192         list[lp] = list[lnw];
11193         lptr[lp] = lptr[lnw];
11194         if (lptr[lpn] == lnw) {
11195             lptr[lpn] = lp;
11196         }
11197         if (lpn == lnw) {
11198             lpn = lp;
11199         }
11200         for (i__ = nn; i__ >= 1; --i__) {
11201             if (lend[i__] == lnw) {
11202                 lend[i__] = lp;
11203                 goto L11;
11204             }
11205 /* L10: */
11206         }
11207 
11208 L11:
11209         for (i__ = lnw - 1; i__ >= 1; --i__) {
11210             if (lptr[i__] == lnw) {
11211                 lptr[i__] = lp;
11212             }
11213 /* L12: */
11214         }
11215 /* L13: */
11216     }
11217 
11218 /* Update N and LNEW, and optimize the patch of triangles */
11219 /*   containing K (on input) by applying swaps to the arcs */
11220 /*   in IWK. */
11221 
11222     *n = nn;
11223     *lnew = lnw;
11224     if (iwl > 0) {
11225         nit = iwl << 2;
11226         optim_(&x[1], &y[1], &z__[1], &iwl, &list[1], &lptr[1], &lend[1], &
11227                 nit, &iwk[3], &ierr);
11228         if (ierr != 0 && ierr != 1) {
11229             goto L25;
11230         }
11231         if (ierr == 1) {
11232             goto L26;
11233         }
11234     }
11235 
11236 /* Successful termination. */
11237 
11238     *ier = 0;
11239     return 0;
11240 
11241 /* Invalid input parameter. */
11242 
11243 L21:
11244     *ier = 1;
11245     return 0;
11246 
11247 /* Insufficient space reserved for IWK. */
11248 
11249 L22:
11250     *ier = 2;
11251     return 0;
11252 
11253 /* Invalid triangulation data structure.  NNB < 3 on input or */
11254 /*   N2 is a neighbor of N1 but N1 is not a neighbor of N2. */
11255 
11256 L23:
11257     *ier = 3;
11258     return 0;
11259 
11260 /* N1 is interior but NNB could not be reduced to 3. */
11261 
11262 L24:
11263     *ier = 4;
11264     return 0;
11265 
11266 /* Error flag (other than 1) returned by OPTIM. */
11267 
11268 L25:
11269     *ier = 5;
11270 /*      WRITE (*,100) NIT, IERR */
11271 /*  100 FORMAT (//5X,'*** Error in OPTIM (called from ', */
11272 /*     .        'DELNOD):  NIT = ',I4,', IER = ',I1,' ***'/) */
11273     return 0;
11274 
11275 /* Error flag 1 returned by OPTIM. */
11276 
11277 L26:
11278     *ier = 6;
11279     return 0;
11280 } /* delnod_ */
11281 
11282 /* Subroutine */ int drwarc_(int *, double *p, double *q,
11283         double *tol, int *nseg)
11284 {
11285     /* System generated locals */
11286     int i__1;
11287     double d__1;
11288 
11289     /* Builtin functions */
11290     //double sqrt(double);
11291 
11292     /* Local variables */
11293     static int i__, k;
11294     static double s, p1[3], p2[3], u1, u2, v1, v2;
11295     static int na;
11296     static double dp[3], du, dv, pm[3], um, vm, err, enrm;
11297 
11298 
11299 /* *********************************************************** */
11300 
11301 /*                                              From STRIPACK */
11302 /*                                            Robert J. Renka */
11303 /*                                  Dept. of Computer Science */
11304 /*                                       Univ. of North Texas */
11305 /*                                           renka@cs.unt.edu */
11306 /*                                                   03/04/03 */
11307 
11308 /*   Given unit vectors P and Q corresponding to northern */
11309 /* hemisphere points (with positive third components), this */
11310 /* subroutine draws a polygonal line which approximates the */
11311 /* projection of arc P-Q onto the plane containing the */
11312 /* equator. */
11313 
11314 /*   The line segment is drawn by writing a sequence of */
11315 /* 'moveto' and 'lineto' Postscript commands to unit LUN.  It */
11316 /* is assumed that an open file is attached to the unit, */
11317 /* header comments have been written to the file, a window- */
11318 /* to-viewport mapping has been established, etc. */
11319 
11320 /* On input: */
11321 
11322 /*       LUN = long int unit number in the range 0 to 99. */
11323 
11324 /*       P,Q = Arrays of length 3 containing the endpoints of */
11325 /*             the arc to be drawn. */
11326 
11327 /*       TOL = Maximum distance in world coordinates between */
11328 /*             the projected arc and polygonal line. */
11329 
11330 /* Input parameters are not altered by this routine. */
11331 
11332 /* On output: */
11333 
11334 /*       NSEG = Number of line segments in the polygonal */
11335 /*              approximation to the projected arc.  This is */
11336 /*              a decreasing function of TOL.  NSEG = 0 and */
11337 /*              no drawing is performed if P = Q or P = -Q */
11338 /*              or an error is encountered in writing to unit */
11339 /*              LUN. */
11340 
11341 /* STRIPACK modules required by DRWARC:  None */
11342 
11343 /* Intrinsic functions called by DRWARC:  ABS, DBLE, SQRT */
11344 
11345 /* *********************************************************** */
11346 
11347 
11348 /* Local parameters: */
11349 
11350 /* DP =    (Q-P)/NSEG */
11351 /* DU,DV = Components of the projection Q'-P' of arc P->Q */
11352 /*           onto the projection plane */
11353 /* ENRM =  Euclidean norm (or squared norm) of Q'-P' or PM */
11354 /* ERR =   Orthogonal distance from the projected midpoint */
11355 /*           PM' to the line defined by P' and Q': */
11356 /*           |Q'-P' X PM'-P'|/|Q'-P'| */
11357 /* I,K =   DO-loop indexes */
11358 /* NA =    Number of arcs (segments) in the partition of P-Q */
11359 /* P1,P2 = Pairs of adjacent points in a uniform partition of */
11360 /*           arc P-Q into NSEG segments; obtained by normal- */
11361 /*           izing PM values */
11362 /* PM =    Midpoint of arc P-Q or a point P + k*DP in a */
11363 /*           uniform partition of the line segment P-Q into */
11364 /*           NSEG segments */
11365 /* S =     Scale factor 1/NA */
11366 /* U1,V1 = Components of P' */
11367 /* U2,V2 = Components of Q' */
11368 /* UM,VM = Components of the midpoint PM' */
11369 
11370 
11371 /* Compute the midpoint PM of arc P-Q. */
11372 
11373     /* Parameter adjustments */
11374     --q;
11375     --p;
11376 
11377     /* Function Body */
11378     enrm = 0.;
11379     for (i__ = 1; i__ <= 3; ++i__) {
11380         pm[i__ - 1] = p[i__] + q[i__];
11381         enrm += pm[i__ - 1] * pm[i__ - 1];
11382 /* L1: */
11383     }
11384     if (enrm == 0.) {
11385         goto L5;
11386     }
11387     enrm = sqrt(enrm);
11388     pm[0] /= enrm;
11389     pm[1] /= enrm;
11390     pm[2] /= enrm;
11391 
11392 /* Project P, Q, and PM to P' = (U1,V1), Q' = (U2,V2), and */
11393 /*   PM' = (UM,VM), respectively. */
11394 
11395     u1 = p[1];
11396     v1 = p[2];
11397     u2 = q[1];
11398     v2 = q[2];
11399     um = pm[0];
11400     vm = pm[1];
11401 
11402 /* Compute the orthogonal distance ERR from PM' to the line */
11403 /*   defined by P' and Q'.  This is the maximum deviation */
11404 /*   between the projected arc and the line segment.  It is */
11405 /*   undefined if P' = Q'. */
11406 
11407     du = u2 - u1;
11408     dv = v2 - v1;
11409     enrm = du * du + dv * dv;
11410     if (enrm == 0.) {
11411         goto L5;
11412     }
11413     err = (d__1 = du * (vm - v1) - (um - u1) * dv, abs(d__1)) / sqrt(enrm);
11414 
11415 /* Compute the number of arcs into which P-Q will be parti- */
11416 /*   tioned (the number of line segments to be drawn): */
11417 /*   NA = ERR/TOL. */
11418 
11419     na = (int) (err / *tol + 1.);
11420 
11421 /* Initialize for loop on arcs P1-P2, where the intermediate */
11422 /*   points are obtained by normalizing PM = P + k*DP for */
11423 /*   DP = (Q-P)/NA */
11424 
11425     s = 1. / (double) na;
11426     for (i__ = 1; i__ <= 3; ++i__) {
11427         dp[i__ - 1] = s * (q[i__] - p[i__]);
11428         pm[i__ - 1] = p[i__];
11429         p1[i__ - 1] = p[i__];
11430 /* L2: */
11431     }
11432 
11433 /* Loop on arcs P1-P2, drawing the line segments associated */
11434 /*   with the projected endpoints. */
11435 
11436     i__1 = na - 1;
11437     for (k = 1; k <= i__1; ++k) {
11438         enrm = 0.;
11439         for (i__ = 1; i__ <= 3; ++i__) {
11440             pm[i__ - 1] += dp[i__ - 1];
11441             enrm += pm[i__ - 1] * pm[i__ - 1];
11442 /* L3: */
11443         }
11444         if (enrm == 0.) {
11445             goto L5;
11446         }
11447         enrm = sqrt(enrm);
11448         p2[0] = pm[0] / enrm;
11449         p2[1] = pm[1] / enrm;
11450         p2[2] = pm[2] / enrm;
11451 /*        WRITE (LUN,100,ERR=5) P1(1), P1(2), P2(1), P2(2) */
11452 /*  100   FORMAT (2F12.6,' moveto',2F12.6,' lineto') */
11453         p1[0] = p2[0];
11454         p1[1] = p2[1];
11455         p1[2] = p2[2];
11456 /* L4: */
11457     }
11458 /*      WRITE (LUN,100,ERR=5) P1(1), P1(2), Q(1), Q(2) */
11459 
11460 /* No error encountered. */
11461 
11462     *nseg = na;
11463     return 0;
11464 
11465 /* Invalid input value of P or Q. */
11466 
11467 L5:
11468     *nseg = 0;
11469     return 0;
11470 } /* drwarc_ */
11471 
11472 /* Subroutine */ int edge_(int *in1, int *in2, double *x,
11473         double *y, double *z__, int *lwk, int *iwk, int *
11474         list, int *lptr, int *lend, int *ier)
11475 {
11476     /* System generated locals */
11477     int i__1;
11478 
11479     /* Local variables */
11480     static int i__, n0, n1, n2;
11481     static double x0, x1, x2, y0, y1, y2, z0, z1, z2;
11482     static int nl, lp, nr;
11483     static double dp12;
11484     static int lp21, iwc, iwf, lft, lpl, iwl, nit;
11485     static double dp1l, dp2l, dp1r, dp2r;
11486     extern long int left_(double *, double *, double *, double
11487             *, double *, double *, double *, double *,
11488             double *);
11489     static int ierr;
11490     extern /* Subroutine */ int swap_(int *, int *, int *,
11491             int *, int *, int *, int *, int *);
11492     static int next, iwcp1, n1lst, iwend;
11493     extern /* Subroutine */ int optim_(double *, double *, double
11494             *, int *, int *, int *, int *, int *, int
11495             *, int *);
11496     static int n1frst;
11497 
11498 
11499 /* *********************************************************** */
11500 
11501 /*                                              From STRIPACK */
11502 /*                                            Robert J. Renka */
11503 /*                                  Dept. of Computer Science */
11504 /*                                       Univ. of North Texas */
11505 /*                                           renka@cs.unt.edu */
11506 /*                                                   07/30/98 */
11507 
11508 /*   Given a triangulation of N nodes and a pair of nodal */
11509 /* indexes IN1 and IN2, this routine swaps arcs as necessary */
11510 /* to force IN1 and IN2 to be adjacent.  Only arcs which */
11511 /* intersect IN1-IN2 are swapped out.  If a Delaunay triangu- */
11512 /* lation is input, the resulting triangulation is as close */
11513 /* as possible to a Delaunay triangulation in the sense that */
11514 /* all arcs other than IN1-IN2 are locally optimal. */
11515 
11516 /*   A sequence of calls to EDGE may be used to force the */
11517 /* presence of a set of edges defining the boundary of a non- */
11518 /* convex and/or multiply connected region, or to introduce */
11519 /* barriers into the triangulation.  Note that Subroutine */
11520 /* GETNP will not necessarily return closest nodes if the */
11521 /* triangulation has been constrained by a call to EDGE. */
11522 /* However, this is appropriate in some applications, such */
11523 /* as triangle-based interpolation on a nonconvex domain. */
11524 
11525 
11526 /* On input: */
11527 
11528 /*       IN1,IN2 = Indexes (of X, Y, and Z) in the range 1 to */
11529 /*                 N defining a pair of nodes to be connected */
11530 /*                 by an arc. */
11531 
11532 /*       X,Y,Z = Arrays of length N containing the Cartesian */
11533 /*               coordinates of the nodes. */
11534 
11535 /* The above parameters are not altered by this routine. */
11536 
11537 /*       LWK = Number of columns reserved for IWK.  This must */
11538 /*             be at least NI -- the number of arcs that */
11539 /*             intersect IN1-IN2.  (NI is bounded by N-3.) */
11540 
11541 /*       IWK = int work array of length at least 2*LWK. */
11542 
11543 /*       LIST,LPTR,LEND = Data structure defining the trian- */
11544 /*                        gulation.  Refer to Subroutine */
11545 /*                        TRMESH. */
11546 
11547 /* On output: */
11548 
11549 /*       LWK = Number of arcs which intersect IN1-IN2 (but */
11550 /*             not more than the input value of LWK) unless */
11551 /*             IER = 1 or IER = 3.  LWK = 0 if and only if */
11552 /*             IN1 and IN2 were adjacent (or LWK=0) on input. */
11553 
11554 /*       IWK = Array containing the indexes of the endpoints */
11555 /*             of the new arcs other than IN1-IN2 unless */
11556 /*             IER > 0 or LWK = 0.  New arcs to the left of */
11557 /*             IN1->IN2 are stored in the first K-1 columns */
11558 /*             (left portion of IWK), column K contains */
11559 /*             zeros, and new arcs to the right of IN1->IN2 */
11560 /*             occupy columns K+1,...,LWK.  (K can be deter- */
11561 /*             mined by searching IWK for the zeros.) */
11562 
11563 /*       LIST,LPTR,LEND = Data structure updated if necessary */
11564 /*                        to reflect the presence of an arc */
11565 /*                        connecting IN1 and IN2 unless IER > */
11566 /*                        0.  The data structure has been */
11567 /*                        altered if IER >= 4. */
11568 
11569 /*       IER = Error indicator: */
11570 /*             IER = 0 if no errors were encountered. */
11571 /*             IER = 1 if IN1 < 1, IN2 < 1, IN1 = IN2, */
11572 /*                     or LWK < 0 on input. */
11573 /*             IER = 2 if more space is required in IWK. */
11574 /*                     Refer to LWK. */
11575 /*             IER = 3 if IN1 and IN2 could not be connected */
11576 /*                     due to either an invalid data struc- */
11577 /*                     ture or collinear nodes (and floating */
11578 /*                     point error). */
11579 /*             IER = 4 if an error flag other than IER = 1 */
11580 /*                     was returned by OPTIM. */
11581 /*             IER = 5 if error flag 1 was returned by OPTIM. */
11582 /*                     This is not necessarily an error, but */
11583 /*                     the arcs other than IN1-IN2 may not */
11584 /*                     be optimal. */
11585 
11586 /*   An error message is written to the standard output unit */
11587 /* in the case of IER = 3 or IER = 4. */
11588 
11589 /* Modules required by EDGE:  LEFT, LSTPTR, OPTIM, SWAP, */
11590 /*                              SWPTST */
11591 
11592 /* Intrinsic function called by EDGE:  ABS */
11593 
11594 /* *********************************************************** */
11595 
11596 
11597 /* Local parameters: */
11598 
11599 /* DPij =     Dot product <Ni,Nj> */
11600 /* I =        DO-loop index and column index for IWK */
11601 /* IERR =     Error flag returned by Subroutine OPTIM */
11602 /* IWC =      IWK index between IWF and IWL -- NL->NR is */
11603 /*              stored in IWK(1,IWC)->IWK(2,IWC) */
11604 /* IWCP1 =    IWC + 1 */
11605 /* IWEND =    Input or output value of LWK */
11606 /* IWF =      IWK (column) index of the first (leftmost) arc */
11607 /*              which intersects IN1->IN2 */
11608 /* IWL =      IWK (column) index of the last (rightmost) are */
11609 /*              which intersects IN1->IN2 */
11610 /* LFT =      Flag used to determine if a swap results in the */
11611 /*              new arc intersecting IN1-IN2 -- LFT = 0 iff */
11612 /*              N0 = IN1, LFT = -1 implies N0 LEFT IN1->IN2, */
11613 /*              and LFT = 1 implies N0 LEFT IN2->IN1 */
11614 /* LP =       List pointer (index for LIST and LPTR) */
11615 /* LP21 =     Unused parameter returned by SWAP */
11616 /* LPL =      Pointer to the last neighbor of IN1 or NL */
11617 /* N0 =       Neighbor of N1 or node opposite NR->NL */
11618 /* N1,N2 =    Local copies of IN1 and IN2 */
11619 /* N1FRST =   First neighbor of IN1 */
11620 /* N1LST =    (Signed) last neighbor of IN1 */
11621 /* NEXT =     Node opposite NL->NR */
11622 /* NIT =      Flag or number of iterations employed by OPTIM */
11623 /* NL,NR =    Endpoints of an arc which intersects IN1-IN2 */
11624 /*              with NL LEFT IN1->IN2 */
11625 /* X0,Y0,Z0 = Coordinates of N0 */
11626 /* X1,Y1,Z1 = Coordinates of IN1 */
11627 /* X2,Y2,Z2 = Coordinates of IN2 */
11628 
11629 
11630 /* Store IN1, IN2, and LWK in local variables and test for */
11631 /*   errors. */
11632 
11633     /* Parameter adjustments */
11634     --lend;
11635     --lptr;
11636     --list;
11637     iwk -= 3;
11638     --z__;
11639     --y;
11640     --x;
11641 
11642     /* Function Body */
11643     n1 = *in1;
11644     n2 = *in2;
11645     iwend = *lwk;
11646     if (n1 < 1 || n2 < 1 || n1 == n2 || iwend < 0) {
11647         goto L31;
11648     }
11649 
11650 /* Test for N2 as a neighbor of N1.  LPL points to the last */
11651 /*   neighbor of N1. */
11652 
11653     lpl = lend[n1];
11654     n0 = (i__1 = list[lpl], abs(i__1));
11655     lp = lpl;
11656 L1:
11657     if (n0 == n2) {
11658         goto L30;
11659     }
11660     lp = lptr[lp];
11661     n0 = list[lp];
11662     if (lp != lpl) {
11663         goto L1;
11664     }
11665 
11666 /* Initialize parameters. */
11667 
11668     iwl = 0;
11669     nit = 0;
11670 
11671 /* Store the coordinates of N1 and N2. */
11672 
11673 L2:
11674     x1 = x[n1];
11675     y1 = y[n1];
11676     z1 = z__[n1];
11677     x2 = x[n2];
11678     y2 = y[n2];
11679     z2 = z__[n2];
11680 
11681 /* Set NR and NL to adjacent neighbors of N1 such that */
11682 /*   NR LEFT N2->N1 and NL LEFT N1->N2, */
11683 /*   (NR Forward N1->N2 or NL Forward N1->N2), and */
11684 /*   (NR Forward N2->N1 or NL Forward N2->N1). */
11685 
11686 /*   Initialization:  Set N1FRST and N1LST to the first and */
11687 /*     (signed) last neighbors of N1, respectively, and */
11688 /*     initialize NL to N1FRST. */
11689 
11690     lpl = lend[n1];
11691     n1lst = list[lpl];
11692     lp = lptr[lpl];
11693     n1frst = list[lp];
11694     nl = n1frst;
11695     if (n1lst < 0) {
11696         goto L4;
11697     }
11698 
11699 /*   N1 is an interior node.  Set NL to the first candidate */
11700 /*     for NR (NL LEFT N2->N1). */
11701 
11702 L3:
11703     if (left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11704         goto L4;
11705     }
11706     lp = lptr[lp];
11707     nl = list[lp];
11708     if (nl != n1frst) {
11709         goto L3;
11710     }
11711 
11712 /*   All neighbors of N1 are strictly left of N1->N2. */
11713 
11714     goto L5;
11715 
11716 /*   NL = LIST(LP) LEFT N2->N1.  Set NR to NL and NL to the */
11717 /*     following neighbor of N1. */
11718 
11719 L4:
11720     nr = nl;
11721     lp = lptr[lp];
11722     nl = (i__1 = list[lp], abs(i__1));
11723     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[nl], &y[nl], &z__[nl])) {
11724 
11725 /*   NL LEFT N1->N2 and NR LEFT N2->N1.  The Forward tests */
11726 /*     are employed to avoid an error associated with */
11727 /*     collinear nodes. */
11728 
11729         dp12 = x1 * x2 + y1 * y2 + z1 * z2;
11730         dp1l = x1 * x[nl] + y1 * y[nl] + z1 * z__[nl];
11731         dp2l = x2 * x[nl] + y2 * y[nl] + z2 * z__[nl];
11732         dp1r = x1 * x[nr] + y1 * y[nr] + z1 * z__[nr];
11733         dp2r = x2 * x[nr] + y2 * y[nr] + z2 * z__[nr];
11734         if ((dp2l - dp12 * dp1l >= 0. || dp2r - dp12 * dp1r >= 0.) && (dp1l -
11735                 dp12 * dp2l >= 0. || dp1r - dp12 * dp2r >= 0.)) {
11736             goto L6;
11737         }
11738 
11739 /*   NL-NR does not intersect N1-N2.  However, there is */
11740 /*     another candidate for the first arc if NL lies on */
11741 /*     the line N1-N2. */
11742 
11743         if (! left_(&x2, &y2, &z2, &x1, &y1, &z1, &x[nl], &y[nl], &z__[nl])) {
11744             goto L5;
11745         }
11746     }
11747 
11748 /*   Bottom of loop. */
11749 
11750     if (nl != n1frst) {
11751         goto L4;
11752     }
11753 
11754 /* Either the triangulation is invalid or N1-N2 lies on the */
11755 /*   convex hull boundary and an edge NR->NL (opposite N1 and */
11756 /*   intersecting N1-N2) was not found due to floating point */
11757 /*   error.  Try interchanging N1 and N2 -- NIT > 0 iff this */
11758 /*   has already been done. */
11759 
11760 L5:
11761     if (nit > 0) {
11762         goto L33;
11763     }
11764     nit = 1;
11765     n1 = n2;
11766     n2 = *in1;
11767     goto L2;
11768 
11769 /* Store the ordered sequence of intersecting edges NL->NR in */
11770 /*   IWK(1,IWL)->IWK(2,IWL). */
11771 
11772 L6:
11773     ++iwl;
11774     if (iwl > iwend) {
11775         goto L32;
11776     }
11777     iwk[(iwl << 1) + 1] = nl;
11778     iwk[(iwl << 1) + 2] = nr;
11779 
11780 /*   Set NEXT to the neighbor of NL which follows NR. */
11781 
11782     lpl = lend[nl];
11783     lp = lptr[lpl];
11784 
11785 /*   Find NR as a neighbor of NL.  The search begins with */
11786 /*     the first neighbor. */
11787 
11788 L7:
11789     if (list[lp] == nr) {
11790         goto L8;
11791     }
11792     lp = lptr[lp];
11793     if (lp != lpl) {
11794         goto L7;
11795     }
11796 
11797 /*   NR must be the last neighbor, and NL->NR cannot be a */
11798 /*     boundary edge. */
11799 
11800     if (list[lp] != nr) {
11801         goto L33;
11802     }
11803 
11804 /*   Set NEXT to the neighbor following NR, and test for */
11805 /*     termination of the store loop. */
11806 
11807 L8:
11808     lp = lptr[lp];
11809     next = (i__1 = list[lp], abs(i__1));
11810     if (next == n2) {
11811         goto L9;
11812     }
11813 
11814 /*   Set NL or NR to NEXT. */
11815 
11816     if (left_(&x1, &y1, &z1, &x2, &y2, &z2, &x[next], &y[next], &z__[next])) {
11817         nl = next;
11818     } else {
11819         nr = next;
11820     }
11821     goto L6;
11822 
11823 /* IWL is the number of arcs which intersect N1-N2. */
11824 /*   Store LWK. */
11825 
11826 L9:
11827     *lwk = iwl;
11828     iwend = iwl;
11829 
11830 /* Initialize for edge swapping loop -- all possible swaps */
11831 /*   are applied (even if the new arc again intersects */
11832 /*   N1-N2), arcs to the left of N1->N2 are stored in the */
11833 /*   left portion of IWK, and arcs to the right are stored in */
11834 /*   the right portion.  IWF and IWL index the first and last */
11835 /*   intersecting arcs. */
11836 
11837     iwf = 1;
11838 
11839 /* Top of loop -- set N0 to N1 and NL->NR to the first edge. */
11840 /*   IWC points to the arc currently being processed.  LFT */
11841 /*   .LE. 0 iff N0 LEFT N1->N2. */
11842 
11843 L10:
11844     lft = 0;
11845     n0 = n1;
11846     x0 = x1;
11847     y0 = y1;
11848     z0 = z1;
11849     nl = iwk[(iwf << 1) + 1];
11850     nr = iwk[(iwf << 1) + 2];
11851     iwc = iwf;
11852 
11853 /*   Set NEXT to the node opposite NL->NR unless IWC is the */
11854 /*     last arc. */
11855 
11856 L11:
11857     if (iwc == iwl) {
11858         goto L21;
11859     }
11860     iwcp1 = iwc + 1;
11861     next = iwk[(iwcp1 << 1) + 1];
11862     if (next != nl) {
11863         goto L16;
11864     }
11865     next = iwk[(iwcp1 << 1) + 2];
11866 
11867 /*   NEXT RIGHT N1->N2 and IWC .LT. IWL.  Test for a possible */
11868 /*     swap. */
11869 
11870     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11871             z__[next])) {
11872         goto L14;
11873     }
11874     if (lft >= 0) {
11875         goto L12;
11876     }
11877     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11878             z__[next])) {
11879         goto L14;
11880     }
11881 
11882 /*   Replace NL->NR with N0->NEXT. */
11883 
11884     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11885     iwk[(iwc << 1) + 1] = n0;
11886     iwk[(iwc << 1) + 2] = next;
11887     goto L15;
11888 
11889 /*   Swap NL-NR for N0-NEXT, shift columns IWC+1,...,IWL to */
11890 /*     the left, and store N0-NEXT in the right portion of */
11891 /*     IWK. */
11892 
11893 L12:
11894     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11895     i__1 = iwl;
11896     for (i__ = iwcp1; i__ <= i__1; ++i__) {
11897         iwk[(i__ - (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11898         iwk[(i__ - (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11899 /* L13: */
11900     }
11901     iwk[(iwl << 1) + 1] = n0;
11902     iwk[(iwl << 1) + 2] = next;
11903     --iwl;
11904     nr = next;
11905     goto L11;
11906 
11907 /*   A swap is not possible.  Set N0 to NR. */
11908 
11909 L14:
11910     n0 = nr;
11911     x0 = x[n0];
11912     y0 = y[n0];
11913     z0 = z__[n0];
11914     lft = 1;
11915 
11916 /*   Advance to the next arc. */
11917 
11918 L15:
11919     nr = next;
11920     ++iwc;
11921     goto L11;
11922 
11923 /*   NEXT LEFT N1->N2, NEXT .NE. N2, and IWC .LT. IWL. */
11924 /*     Test for a possible swap. */
11925 
11926 L16:
11927     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x[next], &y[next], &
11928             z__[next])) {
11929         goto L19;
11930     }
11931     if (lft <= 0) {
11932         goto L17;
11933     }
11934     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x[next], &y[next], &
11935             z__[next])) {
11936         goto L19;
11937     }
11938 
11939 /*   Replace NL->NR with NEXT->N0. */
11940 
11941     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11942     iwk[(iwc << 1) + 1] = next;
11943     iwk[(iwc << 1) + 2] = n0;
11944     goto L20;
11945 
11946 /*   Swap NL-NR for N0-NEXT, shift columns IWF,...,IWC-1 to */
11947 /*     the right, and store N0-NEXT in the left portion of */
11948 /*     IWK. */
11949 
11950 L17:
11951     swap_(&next, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11952     i__1 = iwf;
11953     for (i__ = iwc - 1; i__ >= i__1; --i__) {
11954         iwk[(i__ + (1<<1)) + 1] = iwk[(i__ << 1) + 1];
11955         iwk[(i__ + (1<<1)) + 2] = iwk[(i__ << 1) + 2];
11956 /* L18: */
11957     }
11958     iwk[(iwf << 1) + 1] = n0;
11959     iwk[(iwf << 1) + 2] = next;
11960     ++iwf;
11961     goto L20;
11962 
11963 /*   A swap is not possible.  Set N0 to NL. */
11964 
11965 L19:
11966     n0 = nl;
11967     x0 = x[n0];
11968     y0 = y[n0];
11969     z0 = z__[n0];
11970     lft = -1;
11971 
11972 /*   Advance to the next arc. */
11973 
11974 L20:
11975     nl = next;
11976     ++iwc;
11977     goto L11;
11978 
11979 /*   N2 is opposite NL->NR (IWC = IWL). */
11980 
11981 L21:
11982     if (n0 == n1) {
11983         goto L24;
11984     }
11985     if (lft < 0) {
11986         goto L22;
11987     }
11988 
11989 /*   N0 RIGHT N1->N2.  Test for a possible swap. */
11990 
11991     if (! left_(&x0, &y0, &z0, &x[nr], &y[nr], &z__[nr], &x2, &y2, &z2)) {
11992         goto L10;
11993     }
11994 
11995 /*   Swap NL-NR for N0-N2 and store N0-N2 in the right */
11996 /*     portion of IWK. */
11997 
11998     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
11999     iwk[(iwl << 1) + 1] = n0;
12000     iwk[(iwl << 1) + 2] = n2;
12001     --iwl;
12002     goto L10;
12003 
12004 /*   N0 LEFT N1->N2.  Test for a possible swap. */
12005 
12006 L22:
12007     if (! left_(&x[nl], &y[nl], &z__[nl], &x0, &y0, &z0, &x2, &y2, &z2)) {
12008         goto L10;
12009     }
12010 
12011 /*   Swap NL-NR for N0-N2, shift columns IWF,...,IWL-1 to the */
12012 /*     right, and store N0-N2 in the left portion of IWK. */
12013 
12014     swap_(&n2, &n0, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12015     i__ = iwl;
12016 L23:
12017     iwk[(i__ << 1) + 1] = iwk[(i__ - (1<<1)) + 1];
12018     iwk[(i__ << 1) + 2] = iwk[(i__ - (1<<1)) + 2];
12019     --i__;
12020     if (i__ > iwf) {
12021         goto L23;
12022     }
12023     iwk[(iwf << 1) + 1] = n0;
12024     iwk[(iwf << 1) + 2] = n2;
12025     ++iwf;
12026     goto L10;
12027 
12028 /* IWF = IWC = IWL.  Swap out the last arc for N1-N2 and */
12029 /*   store zeros in IWK. */
12030 
12031 L24:
12032     swap_(&n2, &n1, &nl, &nr, &list[1], &lptr[1], &lend[1], &lp21);
12033     iwk[(iwc << 1) + 1] = 0;
12034     iwk[(iwc << 1) + 2] = 0;
12035 
12036 /* Optimization procedure -- */
12037 
12038     *ier = 0;
12039     if (iwc > 1) {
12040 
12041 /*   Optimize the set of new arcs to the left of IN1->IN2. */
12042 
12043         nit = iwc - (1<<2);
12044         i__1 = iwc - 1;
12045         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12046                 nit, &iwk[3], &ierr);
12047         if (ierr != 0 && ierr != 1) {
12048             goto L34;
12049         }
12050         if (ierr == 1) {
12051             *ier = 5;
12052         }
12053     }
12054     if (iwc < iwend) {
12055 
12056 /*   Optimize the set of new arcs to the right of IN1->IN2. */
12057 
12058         nit = iwend - (iwc<<2);
12059         i__1 = iwend - iwc;
12060         optim_(&x[1], &y[1], &z__[1], &i__1, &list[1], &lptr[1], &lend[1], &
12061                 nit, &iwk[(iwc + (1<<1)) + 1], &ierr);
12062         if (ierr != 0 && ierr != 1) {
12063             goto L34;
12064         }
12065         if (ierr == 1) {
12066             goto L35;
12067         }
12068     }
12069     if (*ier == 5) {
12070         goto L35;
12071     }
12072 
12073 /* Successful termination (IER = 0). */
12074 
12075     return 0;
12076 
12077 /* IN1 and IN2 were adjacent on input. */
12078 
12079 L30:
12080     *ier = 0;
12081     return 0;
12082 
12083 /* Invalid input parameter. */
12084 
12085 L31:
12086     *ier = 1;
12087     return 0;
12088 
12089 /* Insufficient space reserved for IWK. */
12090 
12091 L32:
12092     *ier = 2;
12093     return 0;
12094 
12095 /* Invalid triangulation data structure or collinear nodes */
12096 /*   on convex hull boundary. */
12097 
12098 L33:
12099     *ier = 3;
12100 /*      WRITE (*,130) IN1, IN2 */
12101 /*  130 FORMAT (//5X,'*** Error in EDGE:  Invalid triangula', */
12102 /*     .        'tion or null triangles on boundary'/ */
12103 /*     .        9X,'IN1 =',I4,', IN2=',I4/) */
12104     return 0;
12105 
12106 /* Error flag (other than 1) returned by OPTIM. */
12107 
12108 L34:
12109     *ier = 4;
12110 /*      WRITE (*,140) NIT, IERR */
12111 /*  140 FORMAT (//5X,'*** Error in OPTIM (called from EDGE):', */
12112 /*     .        '  NIT = ',I4,', IER = ',I1,' ***'/) */
12113     return 0;
12114 
12115 /* Error flag 1 returned by OPTIM. */
12116 
12117 L35:
12118     *ier = 5;
12119     return 0;
12120 } /* edge_ */
12121 
12122 /* Subroutine */ int getnp_(double *x, double *y, double *z__,
12123         int *list, int *lptr, int *lend, int *l, int *
12124         npts, double *df, int *ier)
12125 {
12126     /* System generated locals */
12127     int i__1, i__2;
12128 
12129     /* Local variables */
12130     static int i__, n1;
12131     static double x1, y1, z1;
12132     static int nb, ni, lp, np, lm1;
12133     static double dnb, dnp;
12134     static int lpl;
12135 
12136 
12137 /* *********************************************************** */
12138 
12139 /*                                              From STRIPACK */
12140 /*                                            Robert J. Renka */
12141 /*                                  Dept. of Computer Science */
12142 /*                                       Univ. of North Texas */
12143 /*                                           renka@cs.unt.edu */
12144 /*                                                   07/28/98 */
12145 
12146 /*   Given a Delaunay triangulation of N nodes on the unit */
12147 /* sphere and an array NPTS containing the indexes of L-1 */
12148 /* nodes ordered by angular distance from NPTS(1), this sub- */
12149 /* routine sets NPTS(L) to the index of the next node in the */
12150 /* sequence -- the node, other than NPTS(1),...,NPTS(L-1), */
12151 /* that is closest to NPTS(1).  Thus, the ordered sequence */
12152 /* of K closest nodes to N1 (including N1) may be determined */
12153 /* by K-1 calls to GETNP with NPTS(1) = N1 and L = 2,3,...,K */
12154 /* for K .GE. 2. */
12155 
12156 /*   The algorithm uses the property of a Delaunay triangula- */
12157 /* tion that the K-th closest node to N1 is a neighbor of one */
12158 /* of the K-1 closest nodes to N1. */
12159 
12160 
12161 /* On input: */
12162 
12163 /*       X,Y,Z = Arrays of length N containing the Cartesian */
12164 /*               coordinates of the nodes. */
12165 
12166 /*       LIST,LPTR,LEND = Triangulation data structure.  Re- */
12167 /*                        fer to Subroutine TRMESH. */
12168 
12169 /*       L = Number of nodes in the sequence on output.  2 */
12170 /*           .LE. L .LE. N. */
12171 
12172 /* The above parameters are not altered by this routine. */
12173 
12174 /*       NPTS = Array of length .GE. L containing the indexes */
12175 /*              of the L-1 closest nodes to NPTS(1) in the */
12176 /*              first L-1 locations. */
12177 
12178 /* On output: */
12179 
12180 /*       NPTS = Array updated with the index of the L-th */
12181 /*              closest node to NPTS(1) in position L unless */
12182 /*              IER = 1. */
12183 
12184 /*       DF = Value of an increasing function (negative cos- */
12185 /*            ine) of the angular distance between NPTS(1) */
12186 /*            and NPTS(L) unless IER = 1. */
12187 
12188 /*       IER = Error indicator: */
12189 /*             IER = 0 if no errors were encountered. */
12190 /*             IER = 1 if L < 2. */
12191 
12192 /* Modules required by GETNP:  None */
12193 
12194 /* Intrinsic function called by GETNP:  ABS */
12195 
12196 /* *********************************************************** */
12197 
12198 
12199 /* Local parameters: */
12200 
12201 /* DNB,DNP =  Negative cosines of the angular distances from */
12202 /*              N1 to NB and to NP, respectively */
12203 /* I =        NPTS index and DO-loop index */
12204 /* LM1 =      L-1 */
12205 /* LP =       LIST pointer of a neighbor of NI */
12206 /* LPL =      Pointer to the last neighbor of NI */
12207 /* N1 =       NPTS(1) */
12208 /* NB =       Neighbor of NI and candidate for NP */
12209 /* NI =       NPTS(I) */
12210 /* NP =       Candidate for NPTS(L) */
12211 /* X1,Y1,Z1 = Coordinates of N1 */
12212 
12213     /* Parameter adjustments */
12214     --x;
12215     --y;
12216     --z__;
12217     --list;
12218     --lptr;
12219     --lend;
12220     --npts;
12221 
12222     /* Function Body */
12223     lm1 = *l - 1;
12224     if (lm1 < 1) {
12225         goto L6;
12226     }
12227     *ier = 0;
12228 
12229 /* Store N1 = NPTS(1) and mark the elements of NPTS. */
12230 
12231     n1 = npts[1];
12232     x1 = x[n1];
12233     y1 = y[n1];
12234     z1 = z__[n1];
12235     i__1 = lm1;
12236     for (i__ = 1; i__ <= i__1; ++i__) {
12237         ni = npts[i__];
12238         lend[ni] = -lend[ni];
12239 /* L1: */
12240     }
12241 
12242 /* Candidates for NP = NPTS(L) are the unmarked neighbors */
12243 /*   of nodes in NPTS.  DNP is initially greater than -cos(PI) */
12244 /*   (the maximum distance). */
12245 
12246     dnp = 2.;
12247 
12248 /* Loop on nodes NI in NPTS. */
12249 
12250     i__1 = lm1;
12251     for (i__ = 1; i__ <= i__1; ++i__) {
12252         ni = npts[i__];
12253         lpl = -lend[ni];
12254         lp = lpl;
12255 
12256 /* Loop on neighbors NB of NI. */
12257 
12258 L2:
12259         nb = (i__2 = list[lp], abs(i__2));
12260         if (lend[nb] < 0) {
12261             goto L3;
12262         }
12263 
12264 /* NB is an unmarked neighbor of NI.  Replace NP if NB is */
12265 /*   closer to N1. */
12266 
12267         dnb = -(x[nb] * x1 + y[nb] * y1 + z__[nb] * z1);
12268         if (dnb >= dnp) {
12269             goto L3;
12270         }
12271         np = nb;
12272         dnp = dnb;
12273 L3:
12274         lp = lptr[lp];
12275         if (lp != lpl) {
12276             goto L2;
12277         }
12278 /* L4: */
12279     }
12280     npts[*l] = np;
12281     *df = dnp;
12282 
12283 /* Unmark the elements of NPTS. */
12284 
12285     i__1 = lm1;
12286     for (i__ = 1; i__ <= i__1; ++i__) {
12287         ni = npts[i__];
12288         lend[ni] = -lend[ni];
12289 /* L5: */
12290     }
12291     return 0;
12292 
12293 /* L is outside its valid range. */
12294 
12295 L6:
12296     *ier = 1;
12297     return 0;
12298 } /* getnp_ */
12299 
12300 /* Subroutine */ int insert_(int *k, int *lp, int *list, int *
12301         lptr, int *lnew)
12302 {
12303     static int lsav;
12304 
12305 
12306 /* *********************************************************** */
12307 
12308 /*                                              From STRIPACK */
12309 /*                                            Robert J. Renka */
12310 /*                                  Dept. of Computer Science */
12311 /*                                       Univ. of North Texas */
12312 /*                                           renka@cs.unt.edu */
12313 /*                                                   07/17/96 */
12314 
12315 /*   This subroutine inserts K as a neighbor of N1 following */
12316 /* N2, where LP is the LIST pointer of N2 as a neighbor of */
12317 /* N1.  Note that, if N2 is the last neighbor of N1, K will */
12318 /* become the first neighbor (even if N1 is a boundary node). */
12319 
12320 /*   This routine is identical to the similarly named routine */
12321 /* in TRIPACK. */
12322 
12323 
12324 /* On input: */
12325 
12326 /*       K = Index of the node to be inserted. */
12327 
12328 /*       LP = LIST pointer of N2 as a neighbor of N1. */
12329 
12330 /* The above parameters are not altered by this routine. */
12331 
12332 /*       LIST,LPTR,LNEW = Data structure defining the trian- */
12333 /*                        gulation.  Refer to Subroutine */
12334 /*                        TRMESH. */
12335 
12336 /* On output: */
12337 
12338 /*       LIST,LPTR,LNEW = Data structure updated with the */
12339 /*                        addition of node K. */
12340 
12341 /* Modules required by INSERT:  None */
12342 
12343 /* *********************************************************** */
12344 
12345 
12346     /* Parameter adjustments */
12347     --lptr;
12348     --list;
12349 
12350     /* Function Body */
12351     lsav = lptr[*lp];
12352     lptr[*lp] = *lnew;
12353     list[*lnew] = *k;
12354     lptr[*lnew] = lsav;
12355     ++(*lnew);
12356     return 0;
12357 } /* insert_ */
12358 
12359 long int inside_(double *p, int *lv, double *xv, double *yv,
12360         double *zv, int *nv, int *listv, int *ier)
12361 {
12362     /* Initialized data */
12363 
12364     static double eps = .001;
12365 
12366     /* System generated locals */
12367     int i__1;
12368     long int ret_val = 0;
12369 
12370     /* Builtin functions */
12371     //double sqrt(double);
12372 
12373     /* Local variables */
12374     static double b[3], d__;
12375     static int k, n;
12376     static double q[3];
12377     static int i1, i2, k0;
12378     static double v1[3], v2[3], cn[3], bp, bq;
12379     static int ni;
12380     static double pn[3], qn[3], vn[3];
12381     static int imx;
12382     static long int lft1, lft2, even;
12383     static int ierr;
12384     static long int pinr, qinr;
12385     static double qnrm, vnrm;
12386     extern /* Subroutine */ int intrsc_(double *, double *,
12387             double *, double *, int *);
12388 
12389 
12390 /* *********************************************************** */
12391 
12392 /*                                              From STRIPACK */
12393 /*                                            Robert J. Renka */
12394 /*                                  Dept. of Computer Science */
12395 /*                                       Univ. of North Texas */
12396 /*                                           renka@cs.unt.edu */
12397 /*                                                   12/27/93 */
12398 
12399 /*   This function locates a point P relative to a polygonal */
12400 /* region R on the surface of the unit sphere, returning */
12401 /* INSIDE = TRUE if and only if P is contained in R.  R is */
12402 /* defined by a cyclically ordered sequence of vertices which */
12403 /* form a positively-oriented simple closed curve.  Adjacent */
12404 /* vertices need not be distinct but the curve must not be */
12405 /* self-intersecting.  Also, while polygon edges are by defi- */
12406 /* nition restricted to a single hemisphere, R is not so */
12407 /* restricted.  Its interior is the region to the left as the */
12408 /* vertices are traversed in order. */
12409 
12410 /*   The algorithm consists of selecting a point Q in R and */
12411 /* then finding all points at which the great circle defined */
12412 /* by P and Q intersects the boundary of R.  P lies inside R */
12413 /* if and only if there is an even number of intersection */
12414 /* points between Q and P.  Q is taken to be a point immedi- */
12415 /* ately to the left of a directed boundary edge -- the first */
12416 /* one that results in no consistency-check failures. */
12417 
12418 /*   If P is close to the polygon boundary, the problem is */
12419 /* ill-conditioned and the decision may be incorrect.  Also, */
12420 /* an incorrect decision may result from a poor choice of Q */
12421 /* (if, for example, a boundary edge lies on the great cir- */
12422 /* cle defined by P and Q).  A more reliable result could be */
12423 /* obtained by a sequence of calls to INSIDE with the ver- */
12424 /* tices cyclically permuted before each call (to alter the */
12425 /* choice of Q). */
12426 
12427 
12428 /* On input: */
12429 
12430 /*       P = Array of length 3 containing the Cartesian */
12431 /*           coordinates of the point (unit vector) to be */
12432 /*           located. */
12433 
12434 /*       LV = Length of arrays XV, YV, and ZV. */
12435 
12436 /*       XV,YV,ZV = Arrays of length LV containing the Carte- */
12437 /*                  sian coordinates of unit vectors (points */
12438 /*                  on the unit sphere).  These values are */
12439 /*                  not tested for validity. */
12440 
12441 /*       NV = Number of vertices in the polygon.  3 .LE. NV */
12442 /*            .LE. LV. */
12443 
12444 /*       LISTV = Array of length NV containing the indexes */
12445 /*               (for XV, YV, and ZV) of a cyclically-ordered */
12446 /*               (and CCW-ordered) sequence of vertices that */
12447 /*               define R.  The last vertex (indexed by */
12448 /*               LISTV(NV)) is followed by the first (indexed */
12449 /*               by LISTV(1)).  LISTV entries must be in the */
12450 /*               range 1 to LV. */
12451 
12452 /* Input parameters are not altered by this function. */
12453 
12454 /* On output: */
12455 
12456 /*       INSIDE = TRUE if and only if P lies inside R unless */
12457 /*                IER .NE. 0, in which case the value is not */
12458 /*                altered. */
12459 
12460 /*       IER = Error indicator: */
12461 /*             IER = 0 if no errors were encountered. */
12462 /*             IER = 1 if LV or NV is outside its valid */
12463 /*                     range. */
12464 /*             IER = 2 if a LISTV entry is outside its valid */
12465 /*                     range. */
12466 /*             IER = 3 if the polygon boundary was found to */
12467 /*                     be self-intersecting.  This error will */
12468 /*                     not necessarily be detected. */
12469 /*             IER = 4 if every choice of Q (one for each */
12470 /*                     boundary edge) led to failure of some */
12471 /*                     internal consistency check.  The most */
12472 /*                     likely cause of this error is invalid */
12473 /*                     input:  P = (0,0,0), a null or self- */
12474 /*                     intersecting polygon, etc. */
12475 
12476 /* Module required by INSIDE:  INTRSC */
12477 
12478 /* Intrinsic function called by INSIDE:  SQRT */
12479 
12480 /* *********************************************************** */
12481 
12482 
12483 /* Local parameters: */
12484 
12485 /* B =         Intersection point between the boundary and */
12486 /*               the great circle defined by P and Q */
12487 /* BP,BQ =     <B,P> and <B,Q>, respectively, maximized over */
12488 /*               intersection points B that lie between P and */
12489 /*               Q (on the shorter arc) -- used to find the */
12490 /*               closest intersection points to P and Q */
12491 /* CN =        Q X P = normal to the plane of P and Q */
12492 /* D =         Dot product <B,P> or <B,Q> */
12493 /* EPS =       Parameter used to define Q as the point whose */
12494 /*               orthogonal distance to (the midpoint of) */
12495 /*               boundary edge V1->V2 is approximately EPS/ */
12496 /*               (2*Cos(A/2)), where <V1,V2> = Cos(A). */
12497 /* EVEN =      TRUE iff an even number of intersection points */
12498 /*               lie between P and Q (on the shorter arc) */
12499 /* I1,I2 =     Indexes (LISTV elements) of a pair of adjacent */
12500 /*               boundary vertices (endpoints of a boundary */
12501 /*               edge) */
12502 /* IERR =      Error flag for calls to INTRSC (not tested) */
12503 /* IMX =       Local copy of LV and maximum value of I1 and */
12504 /*               I2 */
12505 /* K =         DO-loop index and LISTV index */
12506 /* K0 =        LISTV index of the first endpoint of the */
12507 /*               boundary edge used to compute Q */
12508 /* LFT1,LFT2 = long int variables associated with I1 and I2 in */
12509 /*               the boundary traversal:  TRUE iff the vertex */
12510 /*               is strictly to the left of Q->P (<V,CN> > 0) */
12511 /* N =         Local copy of NV */
12512 /* NI =        Number of intersections (between the boundary */
12513 /*               curve and the great circle P-Q) encountered */
12514 /* PINR =      TRUE iff P is to the left of the directed */
12515 /*               boundary edge associated with the closest */
12516 /*               intersection point to P that lies between P */
12517 /*               and Q (a left-to-right intersection as */
12518 /*               viewed from Q), or there is no intersection */
12519 /*               between P and Q (on the shorter arc) */
12520 /* PN,QN =     P X CN and CN X Q, respectively:  used to */
12521 /*               locate intersections B relative to arc Q->P */
12522 /* Q =         (V1 + V2 + EPS*VN/VNRM)/QNRM, where V1->V2 is */
12523 /*               the boundary edge indexed by LISTV(K0) -> */
12524 /*               LISTV(K0+1) */
12525 /* QINR =      TRUE iff Q is to the left of the directed */
12526 /*               boundary edge associated with the closest */
12527 /*               intersection point to Q that lies between P */
12528 /*               and Q (a right-to-left intersection as */
12529 /*               viewed from Q), or there is no intersection */
12530 /*               between P and Q (on the shorter arc) */
12531 /* QNRM =      Euclidean norm of V1+V2+EPS*VN/VNRM used to */
12532 /*               compute (normalize) Q */
12533 /* V1,V2 =     Vertices indexed by I1 and I2 in the boundary */
12534 /*               traversal */
12535 /* VN =        V1 X V2, where V1->V2 is the boundary edge */
12536 /*               indexed by LISTV(K0) -> LISTV(K0+1) */
12537 /* VNRM =      Euclidean norm of VN */
12538 
12539     /* Parameter adjustments */
12540     --p;
12541     --zv;
12542     --yv;
12543     --xv;
12544     --listv;
12545 
12546     /* Function Body */
12547 
12548 /* Store local parameters, test for error 1, and initialize */
12549 /*   K0. */
12550 
12551     imx = *lv;
12552     n = *nv;
12553     if (n < 3 || n > imx) {
12554         goto L11;
12555     }
12556     k0 = 0;
12557     i1 = listv[1];
12558     if (i1 < 1 || i1 > imx) {
12559         goto L12;
12560     }
12561 
12562 /* Increment K0 and set Q to a point immediately to the left */
12563 /*   of the midpoint of edge V1->V2 = LISTV(K0)->LISTV(K0+1): */
12564 /*   Q = (V1 + V2 + EPS*VN/VNRM)/QNRM, where VN = V1 X V2. */
12565 
12566 L1:
12567     ++k0;
12568     if (k0 > n) {
12569         goto L14;
12570     }
12571     i1 = listv[k0];
12572     if (k0 < n) {
12573         i2 = listv[k0 + 1];
12574     } else {
12575         i2 = listv[1];
12576     }
12577     if (i2 < 1 || i2 > imx) {
12578         goto L12;
12579     }
12580     vn[0] = yv[i1] * zv[i2] - zv[i1] * yv[i2];
12581     vn[1] = zv[i1] * xv[i2] - xv[i1] * zv[i2];
12582     vn[2] = xv[i1] * yv[i2] - yv[i1] * xv[i2];
12583     vnrm = sqrt(vn[0] * vn[0] + vn[1] * vn[1] + vn[2] * vn[2]);
12584     if (vnrm == 0.) {
12585         goto L1;
12586     }
12587     q[0] = xv[i1] + xv[i2] + eps * vn[0] / vnrm;
12588     q[1] = yv[i1] + yv[i2] + eps * vn[1] / vnrm;
12589     q[2] = zv[i1] + zv[i2] + eps * vn[2] / vnrm;
12590     qnrm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
12591     q[0] /= qnrm;
12592     q[1] /= qnrm;
12593     q[2] /= qnrm;
12594 
12595 /* Compute CN = Q X P, PN = P X CN, and QN = CN X Q. */
12596 
12597     cn[0] = q[1] * p[3] - q[2] * p[2];
12598     cn[1] = q[2] * p[1] - q[0] * p[3];
12599     cn[2] = q[0] * p[2] - q[1] * p[1];
12600     if (cn[0] == 0. && cn[1] == 0. && cn[2] == 0.) {
12601         goto L1;
12602     }
12603     pn[0] = p[2] * cn[2] - p[3] * cn[1];
12604     pn[1] = p[3] * cn[0] - p[1] * cn[2];
12605     pn[2] = p[1] * cn[1] - p[2] * cn[0];
12606     qn[0] = cn[1] * q[2] - cn[2] * q[1];
12607     qn[1] = cn[2] * q[0] - cn[0] * q[2];
12608     qn[2] = cn[0] * q[1] - cn[1] * q[0];
12609 
12610 /* Initialize parameters for the boundary traversal. */
12611 
12612     ni = 0;
12613     even = TRUE_;
12614     bp = -2.;
12615     bq = -2.;
12616     pinr = TRUE_;
12617     qinr = TRUE_;
12618     i2 = listv[n];
12619     if (i2 < 1 || i2 > imx) {
12620         goto L12;
12621     }
12622     lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12623 
12624 /* Loop on boundary arcs I1->I2. */
12625 
12626     i__1 = n;
12627     for (k = 1; k <= i__1; ++k) {
12628         i1 = i2;
12629         lft1 = lft2;
12630         i2 = listv[k];
12631         if (i2 < 1 || i2 > imx) {
12632             goto L12;
12633         }
12634         lft2 = cn[0] * xv[i2] + cn[1] * yv[i2] + cn[2] * zv[i2] > 0.;
12635         if (lft1 == lft2) {
12636             goto L2;
12637         }
12638 
12639 /*   I1 and I2 are on opposite sides of Q->P.  Compute the */
12640 /*     point of intersection B. */
12641 
12642         ++ni;
12643         v1[0] = xv[i1];
12644         v1[1] = yv[i1];
12645         v1[2] = zv[i1];
12646         v2[0] = xv[i2];
12647         v2[1] = yv[i2];
12648         v2[2] = zv[i2];
12649         intrsc_(v1, v2, cn, b, &ierr);
12650 
12651 /*   B is between Q and P (on the shorter arc) iff */
12652 /*     B Forward Q->P and B Forward P->Q       iff */
12653 /*     <B,QN> > 0 and <B,PN> > 0. */
12654 
12655         if (b[0] * qn[0] + b[1] * qn[1] + b[2] * qn[2] > 0. && b[0] * pn[0] +
12656                 b[1] * pn[1] + b[2] * pn[2] > 0.) {
12657 
12658 /*   Update EVEN, BQ, QINR, BP, and PINR. */
12659 
12660             even = ! even;
12661             d__ = b[0] * q[0] + b[1] * q[1] + b[2] * q[2];
12662             if (d__ > bq) {
12663                 bq = d__;
12664                 qinr = lft2;
12665             }
12666             d__ = b[0] * p[1] + b[1] * p[2] + b[2] * p[3];
12667             if (d__ > bp) {
12668                 bp = d__;
12669                 pinr = lft1;
12670             }
12671         }
12672 L2:
12673         ;
12674     }
12675 
12676 /* Test for consistency:  NI must be even and QINR must be */
12677 /*   TRUE. */
12678 
12679     if (ni != ni / 2 << 1 || ! qinr) {
12680         goto L1;
12681     }
12682 
12683 /* Test for error 3:  different values of PINR and EVEN. */
12684 
12685     if (pinr != even) {
12686         goto L13;
12687     }
12688 
12689 /* No error encountered. */
12690 
12691     *ier = 0;
12692     ret_val = even;
12693     return ret_val;
12694 
12695 /* LV or NV is outside its valid range. */
12696 
12697 L11:
12698     *ier = 1;
12699     return ret_val;
12700 
12701 /* A LISTV entry is outside its valid range. */
12702 
12703 L12:
12704     *ier = 2;
12705     return ret_val;
12706 
12707 /* The polygon boundary is self-intersecting. */
12708 
12709 L13:
12710     *ier = 3;
12711     return ret_val;
12712 
12713 /* Consistency tests failed for all values of Q. */
12714 
12715 L14:
12716     *ier = 4;
12717     return ret_val;
12718 } /* inside_ */
12719 
12720 /* Subroutine */ int intadd_(int *kk, int *i1, int *i2, int *
12721         i3, int *list, int *lptr, int *lend, int *lnew)
12722 {
12723     static int k, n1, n2, n3, lp;
12724     extern /* Subroutine */ int insert_(int *, int *, int *,
12725             int *, int *);
12726     extern int lstptr_(int *, int *, int *, int *);
12727 
12728 
12729 /* *********************************************************** */
12730 
12731 /*                                              From STRIPACK */
12732 /*                                            Robert J. Renka */
12733 /*                                  Dept. of Computer Science */
12734 /*                                       Univ. of North Texas */
12735 /*                                           renka@cs.unt.edu */
12736 /*                                                   07/17/96 */
12737 
12738 /*   This subroutine adds an interior node to a triangulation */
12739 /* of a set of points on the unit sphere.  The data structure */
12740 /* is updated with the insertion of node KK into the triangle */
12741 /* whose vertices are I1, I2, and I3.  No optimization of the */
12742 /* triangulation is performed. */
12743 
12744 /*   This routine is identical to the similarly named routine */
12745 /* in TRIPACK. */
12746 
12747 
12748 /* On input: */
12749 
12750 /*       KK = Index of the node to be inserted.  KK .GE. 1 */
12751 /*            and KK must not be equal to I1, I2, or I3. */
12752 
12753 /*       I1,I2,I3 = Indexes of the counterclockwise-ordered */
12754 /*                  sequence of vertices of a triangle which */
12755 /*                  contains node KK. */
12756 
12757 /* The above parameters are not altered by this routine. */
12758 
12759 /*       LIST,LPTR,LEND,LNEW = Data structure defining the */
12760 /*                             triangulation.  Refer to Sub- */
12761 /*                             routine TRMESH.  Triangle */
12762 /*                             (I1,I2,I3) must be included */
12763 /*                             in the triangulation. */
12764 
12765 /* On output: */
12766 
12767 /*       LIST,LPTR,LEND,LNEW = Data structure updated with */
12768 /*                             the addition of node KK.  KK */
12769 /*                             will be connected to nodes I1, */
12770 /*                             I2, and I3. */
12771 
12772 /* Modules required by INTADD:  INSERT, LSTPTR */
12773 
12774 /* *********************************************************** */
12775 
12776 
12777 /* Local parameters: */
12778 
12779 /* K =        Local copy of KK */
12780 /* LP =       LIST pointer */
12781 /* N1,N2,N3 = Local copies of I1, I2, and I3 */
12782 
12783     /* Parameter adjustments */
12784     --lend;
12785     --lptr;
12786     --list;
12787 
12788     /* Function Body */
12789     k = *kk;
12790 
12791 /* Initialization. */
12792 
12793     n1 = *i1;
12794     n2 = *i2;
12795     n3 = *i3;
12796 
12797 /* Add K as a neighbor of I1, I2, and I3. */
12798 
12799     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
12800     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12801     lp = lstptr_(&lend[n2], &n3, &list[1], &lptr[1]);
12802     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12803     lp = lstptr_(&lend[n3], &n1, &list[1], &lptr[1]);
12804     insert_(&k, &lp, &list[1], &lptr[1], lnew);
12805 
12806 /* Add I1, I2, and I3 as neighbors of K. */
12807 
12808     list[*lnew] = n1;
12809     list[*lnew + 1] = n2;
12810     list[*lnew + 2] = n3;
12811     lptr[*lnew] = *lnew + 1;
12812     lptr[*lnew + 1] = *lnew + 2;
12813     lptr[*lnew + 2] = *lnew;
12814     lend[k] = *lnew + 2;
12815     *lnew += 3;
12816     return 0;
12817 } /* intadd_ */
12818 
12819 /* Subroutine */ int intrsc_(double *p1, double *p2, double *cn,
12820         double *p, int *ier)
12821 {
12822     /* Builtin functions */
12823     //double sqrt(double);
12824 
12825     /* Local variables */
12826     static int i__;
12827     static double t, d1, d2, pp[3], ppn;
12828 
12829 
12830 /* *********************************************************** */
12831 
12832 /*                                              From STRIPACK */
12833 /*                                            Robert J. Renka */
12834 /*                                  Dept. of Computer Science */
12835 /*                                       Univ. of North Texas */
12836 /*                                           renka@cs.unt.edu */
12837 /*                                                   07/19/90 */
12838 
12839 /*   Given a great circle C and points P1 and P2 defining an */
12840 /* arc A on the surface of the unit sphere, where A is the */
12841 /* shorter of the two portions of the great circle C12 assoc- */
12842 /* iated with P1 and P2, this subroutine returns the point */
12843 /* of intersection P between C and C12 that is closer to A. */
12844 /* Thus, if P1 and P2 lie in opposite hemispheres defined by */
12845 /* C, P is the point of intersection of C with A. */
12846 
12847 
12848 /* On input: */
12849 
12850 /*       P1,P2 = Arrays of length 3 containing the Cartesian */
12851 /*               coordinates of unit vectors. */
12852 
12853 /*       CN = Array of length 3 containing the Cartesian */
12854 /*            coordinates of a nonzero vector which defines C */
12855 /*            as the intersection of the plane whose normal */
12856 /*            is CN with the unit sphere.  Thus, if C is to */
12857 /*            be the great circle defined by P and Q, CN */
12858 /*            should be P X Q. */
12859 
12860 /* The above parameters are not altered by this routine. */
12861 
12862 /*       P = Array of length 3. */
12863 
12864 /* On output: */
12865 
12866 /*       P = Point of intersection defined above unless IER */
12867 /*           .NE. 0, in which case P is not altered. */
12868 
12869 /*       IER = Error indicator. */
12870 /*             IER = 0 if no errors were encountered. */
12871 /*             IER = 1 if <CN,P1> = <CN,P2>.  This occurs */
12872 /*                     iff P1 = P2 or CN = 0 or there are */
12873 /*                     two intersection points at the same */
12874 /*                     distance from A. */
12875 /*             IER = 2 if P2 = -P1 and the definition of A is */
12876 /*                     therefore ambiguous. */
12877 
12878 /* Modules required by INTRSC:  None */
12879 
12880 /* Intrinsic function called by INTRSC:  SQRT */
12881 
12882 /* *********************************************************** */
12883 
12884 
12885 /* Local parameters: */
12886 
12887 /* D1 =  <CN,P1> */
12888 /* D2 =  <CN,P2> */
12889 /* I =   DO-loop index */
12890 /* PP =  P1 + T*(P2-P1) = Parametric representation of the */
12891 /*         line defined by P1 and P2 */
12892 /* PPN = Norm of PP */
12893 /* T =   D1/(D1-D2) = Parameter value chosen so that PP lies */
12894 /*         in the plane of C */
12895 
12896     /* Parameter adjustments */
12897     --p;
12898     --cn;
12899     --p2;
12900     --p1;
12901 
12902     /* Function Body */
12903     d1 = cn[1] * p1[1] + cn[2] * p1[2] + cn[3] * p1[3];
12904     d2 = cn[1] * p2[1] + cn[2] * p2[2] + cn[3] * p2[3];
12905 
12906     if (d1 == d2) {
12907         *ier = 1;
12908         return 0;
12909     }
12910 
12911 /* Solve for T such that <PP,CN> = 0 and compute PP and PPN. */
12912 
12913     t = d1 / (d1 - d2);
12914     ppn = 0.;
12915     for (i__ = 1; i__ <= 3; ++i__) {
12916         pp[i__ - 1] = p1[i__] + t * (p2[i__] - p1[i__]);
12917         ppn += pp[i__ - 1] * pp[i__ - 1];
12918 /* L1: */
12919     }
12920 
12921 /* PPN = 0 iff PP = 0 iff P2 = -P1 (and T = .5). */
12922 
12923     if (ppn == 0.) {
12924         *ier = 2;
12925         return 0;
12926     }
12927     ppn = sqrt(ppn);
12928 
12929 /* Compute P = PP/PPN. */
12930 
12931     for (i__ = 1; i__ <= 3; ++i__) {
12932         p[i__] = pp[i__ - 1] / ppn;
12933 /* L2: */
12934     }
12935     *ier = 0;
12936     return 0;
12937 } /* intrsc_ */
12938 
12939 int jrand_(int *n, int *ix, int *iy, int *iz)
12940 {
12941     /* System generated locals */
12942     int ret_val;
12943 
12944     /* Local variables */
12945     static float u, x;
12946 
12947 
12948 /* *********************************************************** */
12949 
12950 /*                                              From STRIPACK */
12951 /*                                            Robert J. Renka */
12952 /*                                  Dept. of Computer Science */
12953 /*                                       Univ. of North Texas */
12954 /*                                           renka@cs.unt.edu */
12955 /*                                                   07/28/98 */
12956 
12957 /*   This function returns a uniformly distributed pseudo- */
12958 /* random int in the range 1 to N. */
12959 
12960 
12961 /* On input: */
12962 
12963 /*       N = Maximum value to be returned. */
12964 
12965 /* N is not altered by this function. */
12966 
12967 /*       IX,IY,IZ = int seeds initialized to values in */
12968 /*                  the range 1 to 30,000 before the first */
12969 /*                  call to JRAND, and not altered between */
12970 /*                  subsequent calls (unless a sequence of */
12971 /*                  random numbers is to be repeated by */
12972 /*                  reinitializing the seeds). */
12973 
12974 /* On output: */
12975 
12976 /*       IX,IY,IZ = Updated int seeds. */
12977 
12978 /*       JRAND = Random int in the range 1 to N. */
12979 
12980 /* Reference:  B. A. Wichmann and I. D. Hill, "An Efficient */
12981 /*             and Portable Pseudo-random Number Generator", */
12982 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
12983 /*             pp. 188-190. */
12984 
12985 /* Modules required by JRAND:  None */
12986 
12987 /* Intrinsic functions called by JRAND:  INT, MOD, float */
12988 
12989 /* *********************************************************** */
12990 
12991 
12992 /* Local parameters: */
12993 
12994 /* U = Pseudo-random number uniformly distributed in the */
12995 /*     interval (0,1). */
12996 /* X = Pseudo-random number in the range 0 to 3 whose frac- */
12997 /*       tional part is U. */
12998 
12999     *ix = *ix * 171 % 30269;
13000     *iy = *iy * 172 % 30307;
13001     *iz = *iz * 170 % 30323;
13002     x = (float) (*ix) / 30269.f + (float) (*iy) / 30307.f + (float) (*iz) /
13003             30323.f;
13004     u = x - (int) x;
13005     ret_val = (int) ((float) (*n) * u + 1.f);
13006     return ret_val;
13007 } /* jrand_ */
13008 
13009 long int left_(double *x1, double *y1, double *z1, double *x2,
13010         double *y2, double *z2, double *x0, double *y0,
13011         double *z0)
13012 {
13013     /* System generated locals */
13014     long int ret_val;
13015 
13016 
13017 /* *********************************************************** */
13018 
13019 /*                                              From STRIPACK */
13020 /*                                            Robert J. Renka */
13021 /*                                  Dept. of Computer Science */
13022 /*                                       Univ. of North Texas */
13023 /*                                           renka@cs.unt.edu */
13024 /*                                                   07/15/96 */
13025 
13026 /*   This function determines whether node N0 is in the */
13027 /* (closed) left hemisphere defined by the plane containing */
13028 /* N1, N2, and the origin, where left is defined relative to */
13029 /* an observer at N1 facing N2. */
13030 
13031 
13032 /* On input: */
13033 
13034 /*       X1,Y1,Z1 = Coordinates of N1. */
13035 
13036 /*       X2,Y2,Z2 = Coordinates of N2. */
13037 
13038 /*       X0,Y0,Z0 = Coordinates of N0. */
13039 
13040 /* Input parameters are not altered by this function. */
13041 
13042 /* On output: */
13043 
13044 /*       LEFT = TRUE if and only if N0 is in the closed */
13045 /*              left hemisphere. */
13046 
13047 /* Modules required by LEFT:  None */
13048 
13049 /* *********************************************************** */
13050 
13051 /* LEFT = TRUE iff <N0,N1 X N2> = det(N0,N1,N2) .GE. 0. */
13052 
13053     ret_val = *x0 * (*y1 * *z2 - *y2 * *z1) - *y0 * (*x1 * *z2 - *x2 * *z1) +
13054             *z0 * (*x1 * *y2 - *x2 * *y1) >= -0.000001;
13055 
13056 
13057     return ret_val;
13058 } /* left_ */
13059 
13060 int lstptr_(int *lpl, int *nb, int *list, int *lptr)
13061 {
13062     /* System generated locals */
13063     int ret_val;
13064 
13065     /* Local variables */
13066     static int nd, lp;
13067 
13068 
13069 /* *********************************************************** */
13070 
13071 /*                                              From STRIPACK */
13072 /*                                            Robert J. Renka */
13073 /*                                  Dept. of Computer Science */
13074 /*                                       Univ. of North Texas */
13075 /*                                           renka@cs.unt.edu */
13076 /*                                                   07/15/96 */
13077 
13078 /*   This function returns the index (LIST pointer) of NB in */
13079 /* the adjacency list for N0, where LPL = LEND(N0). */
13080 
13081 /*   This function is identical to the similarly named */
13082 /* function in TRIPACK. */
13083 
13084 
13085 /* On input: */
13086 
13087 /*       LPL = LEND(N0) */
13088 
13089 /*       NB = Index of the node whose pointer is to be re- */
13090 /*            turned.  NB must be connected to N0. */
13091 
13092 /*       LIST,LPTR = Data structure defining the triangula- */
13093 /*                   tion.  Refer to Subroutine TRMESH. */
13094 
13095 /* Input parameters are not altered by this function. */
13096 
13097 /* On output: */
13098 
13099 /*       LSTPTR = Pointer such that LIST(LSTPTR) = NB or */
13100 /*                LIST(LSTPTR) = -NB, unless NB is not a */
13101 /*                neighbor of N0, in which case LSTPTR = LPL. */
13102 
13103 /* Modules required by LSTPTR:  None */
13104 
13105 /* *********************************************************** */
13106 
13107 
13108 /* Local parameters: */
13109 
13110 /* LP = LIST pointer */
13111 /* ND = Nodal index */
13112 
13113     /* Parameter adjustments */
13114     --lptr;
13115     --list;
13116 
13117     /* Function Body */
13118     lp = lptr[*lpl];
13119 L1:
13120     nd = list[lp];
13121     if (nd == *nb) {
13122         goto L2;
13123     }
13124     lp = lptr[lp];
13125     if (lp != *lpl) {
13126         goto L1;
13127     }
13128 
13129 L2:
13130     ret_val = lp;
13131     return ret_val;
13132 } /* lstptr_ */
13133 
13134 int nbcnt_(int *lpl, int *lptr)
13135 {
13136     /* System generated locals */
13137     int ret_val;
13138 
13139     /* Local variables */
13140     static int k, lp;
13141 
13142 
13143 /* *********************************************************** */
13144 
13145 /*                                              From STRIPACK */
13146 /*                                            Robert J. Renka */
13147 /*                                  Dept. of Computer Science */
13148 /*                                       Univ. of North Texas */
13149 /*                                           renka@cs.unt.edu */
13150 /*                                                   07/15/96 */
13151 
13152 /*   This function returns the number of neighbors of a node */
13153 /* N0 in a triangulation created by Subroutine TRMESH. */
13154 
13155 /*   This function is identical to the similarly named */
13156 /* function in TRIPACK. */
13157 
13158 
13159 /* On input: */
13160 
13161 /*       LPL = LIST pointer to the last neighbor of N0 -- */
13162 /*             LPL = LEND(N0). */
13163 
13164 /*       LPTR = Array of pointers associated with LIST. */
13165 
13166 /* Input parameters are not altered by this function. */
13167 
13168 /* On output: */
13169 
13170 /*       NBCNT = Number of neighbors of N0. */
13171 
13172 /* Modules required by NBCNT:  None */
13173 
13174 /* *********************************************************** */
13175 
13176 
13177 /* Local parameters: */
13178 
13179 /* K =  Counter for computing the number of neighbors */
13180 /* LP = LIST pointer */
13181 
13182     /* Parameter adjustments */
13183     --lptr;
13184 
13185     /* Function Body */
13186     lp = *lpl;
13187     k = 1;
13188 
13189 L1:
13190     lp = lptr[lp];
13191     if (lp == *lpl) {
13192         goto L2;
13193     }
13194     ++k;
13195     goto L1;
13196 
13197 L2:
13198     ret_val = k;
13199     return ret_val;
13200 } /* nbcnt_ */
13201 
13202 int nearnd_(double *p, int *ist, int *n, double *x,
13203         double *y, double *z__, int *list, int *lptr, int
13204         *lend, double *al)
13205 {
13206     /* System generated locals */
13207     int ret_val, i__1;
13208 
13209     /* Builtin functions */
13210     //double acos(double);
13211 
13212     /* Local variables */
13213     static int l;
13214     static double b1, b2, b3;
13215     static int i1, i2, i3, n1, n2, n3, lp, nn, nr;
13216     static double ds1;
13217     static int lp1, lp2;
13218     static double dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
13219     static int lpl;
13220     static double dsr;
13221     static int nst, listp[25], lptrp[25];
13222     extern /* Subroutine */ int trfind_(int *, double *, int *,
13223             double *, double *, double *, int *, int *,
13224             int *, double *, double *, double *, int *,
13225             int *, int *);
13226     extern int lstptr_(int *, int *, int *, int *);
13227 
13228 
13229 /* *********************************************************** */
13230 
13231 /*                                              From STRIPACK */
13232 /*                                            Robert J. Renka */
13233 /*                                  Dept. of Computer Science */
13234 /*                                       Univ. of North Texas */
13235 /*                                           renka@cs.unt.edu */
13236 /*                                                   07/28/98 */
13237 
13238 /*   Given a point P on the surface of the unit sphere and a */
13239 /* Delaunay triangulation created by Subroutine TRMESH, this */
13240 /* function returns the index of the nearest triangulation */
13241 /* node to P. */
13242 
13243 /*   The algorithm consists of implicitly adding P to the */
13244 /* triangulation, finding the nearest neighbor to P, and */
13245 /* implicitly deleting P from the triangulation.  Thus, it */
13246 /* is based on the fact that, if P is a node in a Delaunay */
13247 /* triangulation, the nearest node to P is a neighbor of P. */
13248 
13249 
13250 /* On input: */
13251 
13252 /*       P = Array of length 3 containing the Cartesian coor- */
13253 /*           dinates of the point P to be located relative to */
13254 /*           the triangulation.  It is assumed without a test */
13255 /*           that P(1)**2 + P(2)**2 + P(3)**2 = 1. */
13256 
13257 /*       IST = Index of a node at which TRFIND begins the */
13258 /*             search.  Search time depends on the proximity */
13259 /*             of this node to P. */
13260 
13261 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
13262 
13263 /*       X,Y,Z = Arrays of length N containing the Cartesian */
13264 /*               coordinates of the nodes. */
13265 
13266 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13267 /*                        gulation.  Refer to TRMESH. */
13268 
13269 /* Input parameters are not altered by this function. */
13270 
13271 /* On output: */
13272 
13273 /*       NEARND = Nodal index of the nearest node to P, or 0 */
13274 /*                if N < 3 or the triangulation data struc- */
13275 /*                ture is invalid. */
13276 
13277 /*       AL = Arc length (angular distance in radians) be- */
13278 /*            tween P and NEARND unless NEARND = 0. */
13279 
13280 /*       Note that the number of candidates for NEARND */
13281 /*       (neighbors of P) is limited to LMAX defined in */
13282 /*       the PARAMETER statement below. */
13283 
13284 /* Modules required by NEARND:  JRAND, LSTPTR, TRFIND, STORE */
13285 
13286 /* Intrinsic functions called by NEARND:  ABS, ACOS */
13287 
13288 /* *********************************************************** */
13289 
13290 
13291 /* Local parameters: */
13292 
13293 /* B1,B2,B3 =  Unnormalized barycentric coordinates returned */
13294 /*               by TRFIND */
13295 /* DS1 =       (Negative cosine of the) distance from P to N1 */
13296 /* DSR =       (Negative cosine of the) distance from P to NR */
13297 /* DX1,..DZ3 = Components of vectors used by the swap test */
13298 /* I1,I2,I3 =  Nodal indexes of a triangle containing P, or */
13299 /*               the rightmost (I1) and leftmost (I2) visible */
13300 /*               boundary nodes as viewed from P */
13301 /* L =         Length of LISTP/LPTRP and number of neighbors */
13302 /*               of P */
13303 /* LMAX =      Maximum value of L */
13304 /* LISTP =     Indexes of the neighbors of P */
13305 /* LPTRP =     Array of pointers in 1-1 correspondence with */
13306 /*               LISTP elements */
13307 /* LP =        LIST pointer to a neighbor of N1 and LISTP */
13308 /*               pointer */
13309 /* LP1,LP2 =   LISTP indexes (pointers) */
13310 /* LPL =       Pointer to the last neighbor of N1 */
13311 /* N1 =        Index of a node visible from P */
13312 /* N2 =        Index of an endpoint of an arc opposite P */
13313 /* N3 =        Index of the node opposite N1->N2 */
13314 /* NN =        Local copy of N */
13315 /* NR =        Index of a candidate for the nearest node to P */
13316 /* NST =       Index of the node at which TRFIND begins the */
13317 /*               search */
13318 
13319 
13320 /* Store local parameters and test for N invalid. */
13321 
13322     /* Parameter adjustments */
13323     --p;
13324     --lend;
13325     --z__;
13326     --y;
13327     --x;
13328     --list;
13329     --lptr;
13330 
13331     /* Function Body */
13332     nn = *n;
13333     if (nn < 3) {
13334         goto L6;
13335     }
13336     nst = *ist;
13337     if (nst < 1 || nst > nn) {
13338         nst = 1;
13339     }
13340 
13341 /* Find a triangle (I1,I2,I3) containing P, or the rightmost */
13342 /*   (I1) and leftmost (I2) visible boundary nodes as viewed */
13343 /*   from P. */
13344 
13345     trfind_(&nst, &p[1], n, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &lend[
13346             1], &b1, &b2, &b3, &i1, &i2, &i3);
13347 
13348 /* Test for collinear nodes. */
13349 
13350     if (i1 == 0) {
13351         goto L6;
13352     }
13353 
13354 /* Store the linked list of 'neighbors' of P in LISTP and */
13355 /*   LPTRP.  I1 is the first neighbor, and 0 is stored as */
13356 /*   the last neighbor if P is not contained in a triangle. */
13357 /*   L is the length of LISTP and LPTRP, and is limited to */
13358 /*   LMAX. */
13359 
13360     if (i3 != 0) {
13361         listp[0] = i1;
13362         lptrp[0] = 2;
13363         listp[1] = i2;
13364         lptrp[1] = 3;
13365         listp[2] = i3;
13366         lptrp[2] = 1;
13367         l = 3;
13368     } else {
13369         n1 = i1;
13370         l = 1;
13371         lp1 = 2;
13372         listp[l - 1] = n1;
13373         lptrp[l - 1] = lp1;
13374 
13375 /*   Loop on the ordered sequence of visible boundary nodes */
13376 /*     N1 from I1 to I2. */
13377 
13378 L1:
13379         lpl = lend[n1];
13380         n1 = -list[lpl];
13381         l = lp1;
13382         lp1 = l + 1;
13383         listp[l - 1] = n1;
13384         lptrp[l - 1] = lp1;
13385         if (n1 != i2 && lp1 < 25) {
13386             goto L1;
13387         }
13388         l = lp1;
13389         listp[l - 1] = 0;
13390         lptrp[l - 1] = 1;
13391     }
13392 
13393 /* Initialize variables for a loop on arcs N1-N2 opposite P */
13394 /*   in which new 'neighbors' are 'swapped' in.  N1 follows */
13395 /*   N2 as a neighbor of P, and LP1 and LP2 are the LISTP */
13396 /*   indexes of N1 and N2. */
13397 
13398     lp2 = 1;
13399     n2 = i1;
13400     lp1 = lptrp[0];
13401     n1 = listp[lp1 - 1];
13402 
13403 /* Begin loop:  find the node N3 opposite N1->N2. */
13404 
13405 L2:
13406     lp = lstptr_(&lend[n1], &n2, &list[1], &lptr[1]);
13407     if (list[lp] < 0) {
13408         goto L3;
13409     }
13410     lp = lptr[lp];
13411     n3 = (i__1 = list[lp], abs(i__1));
13412 
13413 /* Swap test:  Exit the loop if L = LMAX. */
13414 
13415     if (l == 25) {
13416         goto L4;
13417     }
13418     dx1 = x[n1] - p[1];
13419     dy1 = y[n1] - p[2];
13420     dz1 = z__[n1] - p[3];
13421 
13422     dx2 = x[n2] - p[1];
13423     dy2 = y[n2] - p[2];
13424     dz2 = z__[n2] - p[3];
13425 
13426     dx3 = x[n3] - p[1];
13427     dy3 = y[n3] - p[2];
13428     dz3 = z__[n3] - p[3];
13429     if (dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) + dz3 *
13430             (dx2 * dy1 - dx1 * dy2) <= 0.) {
13431         goto L3;
13432     }
13433 
13434 /* Swap:  Insert N3 following N2 in the adjacency list for P. */
13435 /*        The two new arcs opposite P must be tested. */
13436 
13437     ++l;
13438     lptrp[lp2 - 1] = l;
13439     listp[l - 1] = n3;
13440     lptrp[l - 1] = lp1;
13441     lp1 = l;
13442     n1 = n3;
13443     goto L2;
13444 
13445 /* No swap:  Advance to the next arc and test for termination */
13446 /*           on N1 = I1 (LP1 = 1) or N1 followed by 0. */
13447 
13448 L3:
13449     if (lp1 == 1) {
13450         goto L4;
13451     }
13452     lp2 = lp1;
13453     n2 = n1;
13454     lp1 = lptrp[lp1 - 1];
13455     n1 = listp[lp1 - 1];
13456     if (n1 == 0) {
13457         goto L4;
13458     }
13459     goto L2;
13460 
13461 /* Set NR and DSR to the index of the nearest node to P and */
13462 /*   an increasing function (negative cosine) of its distance */
13463 /*   from P, respectively. */
13464 
13465 L4:
13466     nr = i1;
13467     dsr = -(x[nr] * p[1] + y[nr] * p[2] + z__[nr] * p[3]);
13468     i__1 = l;
13469     for (lp = 2; lp <= i__1; ++lp) {
13470         n1 = listp[lp - 1];
13471         if (n1 == 0) {
13472             goto L5;
13473         }
13474         ds1 = -(x[n1] * p[1] + y[n1] * p[2] + z__[n1] * p[3]);
13475         if (ds1 < dsr) {
13476             nr = n1;
13477             dsr = ds1;
13478         }
13479 L5:
13480         ;
13481     }
13482     dsr = -dsr;
13483     if (dsr > 1.) {
13484         dsr = 1.;
13485     }
13486     *al = acos(dsr);
13487     ret_val = nr;
13488     return ret_val;
13489 
13490 /* Invalid input. */
13491 
13492 L6:
13493     ret_val = 0;
13494     return ret_val;
13495 } /* nearnd_ */
13496 
13497 /* Subroutine */ int optim_(double *x, double *y, double *z__,
13498         int *na, int *list, int *lptr, int *lend, int *
13499         nit, int *iwk, int *ier)
13500 {
13501     /* System generated locals */
13502     int i__1, i__2;
13503 
13504     /* Local variables */
13505     static int i__, n1, n2, lp, io1, io2, nna, lp21, lpl, lpp;
13506     static long int swp;
13507     static int iter;
13508     extern /* Subroutine */ int swap_(int *, int *, int *,
13509             int *, int *, int *, int *, int *);
13510     static int maxit;
13511     extern long int swptst_(int *, int *, int *, int *,
13512             double *, double *, double *);
13513 
13514 
13515 /* *********************************************************** */
13516 
13517 /*                                              From STRIPACK */
13518 /*                                            Robert J. Renka */
13519 /*                                  Dept. of Computer Science */
13520 /*                                       Univ. of North Texas */
13521 /*                                           renka@cs.unt.edu */
13522 /*                                                   07/30/98 */
13523 
13524 /*   Given a set of NA triangulation arcs, this subroutine */
13525 /* optimizes the portion of the triangulation consisting of */
13526 /* the quadrilaterals (pairs of adjacent triangles) which */
13527 /* have the arcs as diagonals by applying the circumcircle */
13528 /* test and appropriate swaps to the arcs. */
13529 
13530 /*   An iteration consists of applying the swap test and */
13531 /* swaps to all NA arcs in the order in which they are */
13532 /* stored.  The iteration is repeated until no swap occurs */
13533 /* or NIT iterations have been performed.  The bound on the */
13534 /* number of iterations may be necessary to prevent an */
13535 /* infinite loop caused by cycling (reversing the effect of a */
13536 /* previous swap) due to floating point inaccuracy when four */
13537 /* or more nodes are nearly cocircular. */
13538 
13539 
13540 /* On input: */
13541 
13542 /*       X,Y,Z = Arrays containing the nodal coordinates. */
13543 
13544 /*       NA = Number of arcs in the set.  NA .GE. 0. */
13545 
13546 /* The above parameters are not altered by this routine. */
13547 
13548 /*       LIST,LPTR,LEND = Data structure defining the trian- */
13549 /*                        gulation.  Refer to Subroutine */
13550 /*                        TRMESH. */
13551 
13552 /*       NIT = Maximum number of iterations to be performed. */
13553 /*             NIT = 4*NA should be sufficient.  NIT .GE. 1. */
13554 
13555 /*       IWK = int array dimensioned 2 by NA containing */
13556 /*             the nodal indexes of the arc endpoints (pairs */
13557 /*             of endpoints are stored in columns). */
13558 
13559 /* On output: */
13560 
13561 /*       LIST,LPTR,LEND = Updated triangulation data struc- */
13562 /*                        ture reflecting the swaps. */
13563 
13564 /*       NIT = Number of iterations performed. */
13565 
13566 /*       IWK = Endpoint indexes of the new set of arcs */
13567 /*             reflecting the swaps. */
13568 
13569 /*       IER = Error indicator: */
13570 /*             IER = 0 if no errors were encountered. */
13571 /*             IER = 1 if a swap occurred on the last of */
13572 /*                     MAXIT iterations, where MAXIT is the */
13573 /*                     value of NIT on input.  The new set */
13574 /*                     of arcs is not necessarily optimal */
13575 /*                     in this case. */
13576 /*             IER = 2 if NA < 0 or NIT < 1 on input. */
13577 /*             IER = 3 if IWK(2,I) is not a neighbor of */
13578 /*                     IWK(1,I) for some I in the range 1 */
13579 /*                     to NA.  A swap may have occurred in */
13580 /*                     this case. */
13581 /*             IER = 4 if a zero pointer was returned by */
13582 /*                     Subroutine SWAP. */
13583 
13584 /* Modules required by OPTIM:  LSTPTR, SWAP, SWPTST */
13585 
13586 /* Intrinsic function called by OPTIM:  ABS */
13587 
13588 /* *********************************************************** */
13589 
13590 
13591 /* Local parameters: */
13592 
13593 /* I =       Column index for IWK */
13594 /* IO1,IO2 = Nodal indexes of the endpoints of an arc in IWK */
13595 /* ITER =    Iteration count */
13596 /* LP =      LIST pointer */
13597 /* LP21 =    Parameter returned by SWAP (not used) */
13598 /* LPL =     Pointer to the last neighbor of IO1 */
13599 /* LPP =     Pointer to the node preceding IO2 as a neighbor */
13600 /*             of IO1 */
13601 /* MAXIT =   Input value of NIT */
13602 /* N1,N2 =   Nodes opposite IO1->IO2 and IO2->IO1, */
13603 /*             respectively */
13604 /* NNA =     Local copy of NA */
13605 /* SWP =     Flag set to TRUE iff a swap occurs in the */
13606 /*             optimization loop */
13607 
13608     /* Parameter adjustments */
13609     --x;
13610     --y;
13611     --z__;
13612     iwk -= 3;
13613     --list;
13614     --lptr;
13615     --lend;
13616 
13617     /* Function Body */
13618     nna = *na;
13619     maxit = *nit;
13620     if (nna < 0 || maxit < 1) {
13621         goto L7;
13622     }
13623 
13624 /* Initialize iteration count ITER and test for NA = 0. */
13625 
13626     iter = 0;
13627     if (nna == 0) {
13628         goto L5;
13629     }
13630 
13631 /* Top of loop -- */
13632 /*   SWP = TRUE iff a swap occurred in the current iteration. */
13633 
13634 L1:
13635     if (iter == maxit) {
13636         goto L6;
13637     }
13638     ++iter;
13639     swp = FALSE_;
13640 
13641 /*   Inner loop on arcs IO1-IO2 -- */
13642 
13643     i__1 = nna;
13644     for (i__ = 1; i__ <= i__1; ++i__) {
13645         io1 = iwk[(i__ << 1) + 1];
13646         io2 = iwk[(i__ << 1) + 2];
13647 
13648 /*   Set N1 and N2 to the nodes opposite IO1->IO2 and */
13649 /*     IO2->IO1, respectively.  Determine the following: */
13650 
13651 /*     LPL = pointer to the last neighbor of IO1, */
13652 /*     LP = pointer to IO2 as a neighbor of IO1, and */
13653 /*     LPP = pointer to the node N2 preceding IO2. */
13654 
13655         lpl = lend[io1];
13656         lpp = lpl;
13657         lp = lptr[lpp];
13658 L2:
13659         if (list[lp] == io2) {
13660             goto L3;
13661         }
13662         lpp = lp;
13663         lp = lptr[lpp];
13664         if (lp != lpl) {
13665             goto L2;
13666         }
13667 
13668 /*   IO2 should be the last neighbor of IO1.  Test for no */
13669 /*     arc and bypass the swap test if IO1 is a boundary */
13670 /*     node. */
13671 
13672         if ((i__2 = list[lp], abs(i__2)) != io2) {
13673             goto L8;
13674         }
13675         if (list[lp] < 0) {
13676             goto L4;
13677         }
13678 
13679 /*   Store N1 and N2, or bypass the swap test if IO1 is a */
13680 /*     boundary node and IO2 is its first neighbor. */
13681 
13682 L3:
13683         n2 = list[lpp];
13684         if (n2 < 0) {
13685             goto L4;
13686         }
13687         lp = lptr[lp];
13688         n1 = (i__2 = list[lp], abs(i__2));
13689 
13690 /*   Test IO1-IO2 for a swap, and update IWK if necessary. */
13691 
13692         if (! swptst_(&n1, &n2, &io1, &io2, &x[1], &y[1], &z__[1])) {
13693             goto L4;
13694         }
13695         swap_(&n1, &n2, &io1, &io2, &list[1], &lptr[1], &lend[1], &lp21);
13696         if (lp21 == 0) {
13697             goto L9;
13698         }
13699         swp = TRUE_;
13700         iwk[(i__ << 1) + 1] = n1;
13701         iwk[(i__ << 1) + 2] = n2;
13702 L4:
13703         ;
13704     }
13705     if (swp) {
13706         goto L1;
13707     }
13708 
13709 /* Successful termination. */
13710 
13711 L5:
13712     *nit = iter;
13713     *ier = 0;
13714     return 0;
13715 
13716 /* MAXIT iterations performed without convergence. */
13717 
13718 L6:
13719     *nit = maxit;
13720     *ier = 1;
13721     return 0;
13722 
13723 /* Invalid input parameter. */
13724 
13725 L7:
13726     *nit = 0;
13727     *ier = 2;
13728     return 0;
13729 
13730 /* IO2 is not a neighbor of IO1. */
13731 
13732 L8:
13733     *nit = iter;
13734     *ier = 3;
13735     return 0;
13736 
13737 /* Zero pointer returned by SWAP. */
13738 
13739 L9:
13740     *nit = iter;
13741     *ier = 4;
13742     return 0;
13743 } /* optim_ */
13744 
13745 /* Subroutine */ int projct_(double *px, double *py, double *pz,
13746         double *ox, double *oy, double *oz, double *ex,
13747         double *ey, double *ez, double *vx, double *vy,
13748         double *vz, long int *init, double *x, double *y,
13749         double *z__, int *ier)
13750 {
13751     /* Builtin functions */
13752     //double sqrt(double);
13753 
13754     /* Local variables */
13755     static double s, sc, xe, ye, ze, xh, yh, zh, xv, yv, zv, xw, yw, zw,
13756             oes, xoe, yoe, zoe, xep, yep, zep;
13757 
13758 
13759 /* *********************************************************** */
13760 
13761 /*                        From PLTPACK, SCRPLOT, and STRIPACK */
13762 /*                                            Robert J. Renka */
13763 /*                                  Dept. of Computer Science */
13764 /*                                       Univ. of North Texas */
13765 /*                                           renka@cs.unt.edu */
13766 /*                                                   07/18/90 */
13767 
13768 /*   Given a projection plane and associated coordinate sys- */
13769 /* tem defined by an origin O, eye position E, and up-vector */
13770 /* V, this subroutine applies a perspective depth transform- */
13771 /* ation T to a point P = (PX,PY,PZ), returning the point */
13772 /* T(P) = (X,Y,Z), where X and Y are the projection plane */
13773 /* coordinates of the point that lies in the projection */
13774 /* plane and on the line defined by P and E, and Z is the */
13775 /* depth associated with P. */
13776 
13777 /*   The projection plane is defined to be the plane that */
13778 /* contains O and has normal defined by O and E. */
13779 
13780 /*   The depth Z is defined in such a way that Z < 1, T maps */
13781 /* lines to lines (and planes to planes), and if two distinct */
13782 /* points have the same projection plane coordinates, then */
13783 /* the one closer to E has a smaller depth.  (Z increases */
13784 /* monotonically with orthogonal distance from P to the plane */
13785 /* that is parallel to the projection plane and contains E.) */
13786 /* This depth value facilitates depth sorting and depth buf- */
13787 /* fer methods. */
13788 
13789 
13790 /* On input: */
13791 
13792 /*       PX,PY,PZ = Cartesian coordinates of the point P to */
13793 /*                  be mapped onto the projection plane.  The */
13794 /*                  half line that contains P and has end- */
13795 /*                  point at E must intersect the plane. */
13796 
13797 /*       OX,OY,OZ = Coordinates of O (the origin of a coordi- */
13798 /*                  nate system in the projection plane).  A */
13799 /*                  reasonable value for O is a point near */
13800 /*                  the center of an object or scene to be */
13801 /*                  viewed. */
13802 
13803 /*       EX,EY,EZ = Coordinates of the eye-position E defin- */
13804 /*                  ing the normal to the plane and the line */
13805 /*                  of sight for the projection.  E must not */
13806 /*                  coincide with O or P, and the angle be- */
13807 /*                  tween the vectors O-E and P-E must be */
13808 /*                  less than 90 degrees.  Note that E and P */
13809 /*                  may lie on opposite sides of the projec- */
13810 /*                  tion plane. */
13811 
13812 /*       VX,VY,VZ = Coordinates of a point V which defines */
13813 /*                  the positive Y axis of an X-Y coordinate */
13814 /*                  system in the projection plane as the */
13815 /*                  half-line containing O and the projection */
13816 /*                  of O+V onto the plane.  The positive X */
13817 /*                  axis has direction defined by the cross */
13818 /*                  product V X (E-O). */
13819 
13820 /* The above parameters are not altered by this routine. */
13821 
13822 /*       INIT = long int switch which must be set to TRUE on */
13823 /*              the first call and when the values of O, E, */
13824 /*              or V have been altered since a previous call. */
13825 /*              If INIT = FALSE, it is assumed that only the */
13826 /*              coordinates of P have changed since a previ- */
13827 /*              ous call.  Previously stored quantities are */
13828 /*              used for increased efficiency in this case. */
13829 
13830 /* On output: */
13831 
13832 /*       INIT = Switch with value reset to FALSE if IER = 0. */
13833 
13834 /*       X,Y = Projection plane coordinates of the point */
13835 /*             that lies in the projection plane and on the */
13836 /*             line defined by E and P.  X and Y are not */
13837 /*             altered if IER .NE. 0. */
13838 
13839 /*       Z = Depth value defined above unless IER .NE. 0. */
13840 
13841 /*       IER = Error indicator. */
13842 /*             IER = 0 if no errors were encountered. */
13843 /*             IER = 1 if the inner product of O-E with P-E */
13844 /*                     is not positive, implying that E is */
13845 /*                     too close to the plane. */
13846 /*             IER = 2 if O, E, and O+V are collinear.  See */
13847 /*                     the description of VX,VY,VZ. */
13848 
13849 /* Modules required by PROJCT:  None */
13850 
13851 /* Intrinsic function called by PROJCT:  SQRT */
13852 
13853 /* *********************************************************** */
13854 
13855 
13856 /* Local parameters: */
13857 
13858 /* OES =         Norm squared of OE -- inner product (OE,OE) */
13859 /* S =           Scale factor for computing projections */
13860 /* SC =          Scale factor for normalizing VN and HN */
13861 /* XE,YE,ZE =    Local copies of EX, EY, EZ */
13862 /* XEP,YEP,ZEP = Components of the vector EP from E to P */
13863 /* XH,YH,ZH =    Components of a unit vector HN defining the */
13864 /*                 positive X-axis in the plane */
13865 /* XOE,YOE,ZOE = Components of the vector OE from O to E */
13866 /* XV,YV,ZV =    Components of a unit vector VN defining the */
13867 /*                 positive Y-axis in the plane */
13868 /* XW,YW,ZW =    Components of the vector W from O to the */
13869 /*                 projection of P onto the plane */
13870 
13871     if (*init) {
13872 
13873 /* Compute parameters defining the transformation: */
13874 /*   17 adds, 27 multiplies, 3 divides, 2 compares, and */
13875 /*   2 square roots. */
13876 
13877 /* Set the coordinates of E to local variables, compute */
13878 /*   OE = E-O and OES, and test for OE = 0. */
13879 
13880         xe = *ex;
13881         ye = *ey;
13882         ze = *ez;
13883         xoe = xe - *ox;
13884         yoe = ye - *oy;
13885         zoe = ze - *oz;
13886         oes = xoe * xoe + yoe * yoe + zoe * zoe;
13887         if (oes == 0.) {
13888             goto L1;
13889         }
13890 
13891 /* Compute S = (OE,V)/OES and VN = V - S*OE. */
13892 
13893         s = (xoe * *vx + yoe * *vy + zoe * *vz) / oes;
13894         xv = *vx - s * xoe;
13895         yv = *vy - s * yoe;
13896         zv = *vz - s * zoe;
13897 
13898 /* Normalize VN to a unit vector. */
13899 
13900         sc = xv * xv + yv * yv + zv * zv;
13901         if (sc == 0.) {
13902             goto L2;
13903         }
13904         sc = 1. / sqrt(sc);
13905         xv = sc * xv;
13906         yv = sc * yv;
13907         zv = sc * zv;
13908 
13909 /* Compute HN = VN X OE (normalized). */
13910 
13911         xh = yv * zoe - yoe * zv;
13912         yh = xoe * zv - xv * zoe;
13913         zh = xv * yoe - xoe * yv;
13914         sc = sqrt(xh * xh + yh * yh + zh * zh);
13915         if (sc == 0.) {
13916             goto L2;
13917         }
13918         sc = 1. / sc;
13919         xh = sc * xh;
13920         yh = sc * yh;
13921         zh = sc * zh;
13922     }
13923 
13924 /* Apply the transformation:  13 adds, 12 multiplies, */
13925 /*                            1 divide, and 1 compare. */
13926 
13927 /* Compute EP = P-E, S = OES/(OE,EP), and W = OE - S*EP. */
13928 
13929     xep = *px - xe;
13930     yep = *py - ye;
13931     zep = *pz - ze;
13932     s = xoe * xep + yoe * yep + zoe * zep;
13933     if (s >= 0.) {
13934         goto L1;
13935     }
13936     s = oes / s;
13937     xw = xoe - s * xep;
13938     yw = yoe - s * yep;
13939     zw = zoe - s * zep;
13940 
13941 /* Map W into X = (W,HN), Y = (W,VN), compute Z = 1+S, and */
13942 /*   reset INIT. */
13943 
13944     *x = xw * xh + yw * yh + zw * zh;
13945     *y = xw * xv + yw * yv + zw * zv;
13946     *z__ = s + 1.;
13947     *init = FALSE_;
13948     *ier = 0;
13949     return 0;
13950 
13951 /* (OE,EP) .GE. 0. */
13952 
13953 L1:
13954     *ier = 1;
13955     return 0;
13956 
13957 /* O, E, and O+V are collinear. */
13958 
13959 L2:
13960     *ier = 2;
13961     return 0;
13962 } /* projct_ */
13963 
13964 /* Subroutine */ int scoord_(double *px, double *py, double *pz,
13965         double *plat, double *plon, double *pnrm)
13966 {
13967     /* Builtin functions */
13968     //double sqrt(double), atan2(double, double), asin(double);
13969 
13970 
13971 /* *********************************************************** */
13972 
13973 /*                                              From STRIPACK */
13974 /*                                            Robert J. Renka */
13975 /*                                  Dept. of Computer Science */
13976 /*                                       Univ. of North Texas */
13977 /*                                           renka@cs.unt.edu */
13978 /*                                                   08/27/90 */
13979 
13980 /*   This subroutine converts a point P from Cartesian coor- */
13981 /* dinates to spherical coordinates. */
13982 
13983 
13984 /* On input: */
13985 
13986 /*       PX,PY,PZ = Cartesian coordinates of P. */
13987 
13988 /* Input parameters are not altered by this routine. */
13989 
13990 /* On output: */
13991 
13992 /*       PLAT = Latitude of P in the range -PI/2 to PI/2, or */
13993 /*              0 if PNRM = 0.  PLAT should be scaled by */
13994 /*              180/PI to obtain the value in degrees. */
13995 
13996 /*       PLON = Longitude of P in the range -PI to PI, or 0 */
13997 /*              if P lies on the Z-axis.  PLON should be */
13998 /*              scaled by 180/PI to obtain the value in */
13999 /*              degrees. */
14000 
14001 /*       PNRM = Magnitude (Euclidean norm) of P. */
14002 
14003 /* Modules required by SCOORD:  None */
14004 
14005 /* Intrinsic functions called by SCOORD:  ASIN, ATAN2, SQRT */
14006 
14007 /* *********************************************************** */
14008 
14009     *pnrm = sqrt(*px * *px + *py * *py + *pz * *pz);
14010     if (*px != 0. || *py != 0.) {
14011         *plon = atan2(*py, *px);
14012     } else {
14013         *plon = 0.;
14014     }
14015     if (*pnrm != 0.) {
14016         *plat = asin(*pz / *pnrm);
14017     } else {
14018         *plat = 0.;
14019     }
14020     return 0;
14021 } /* scoord_ */
14022 
14023 double store_(double *x)
14024 {
14025     /* System generated locals */
14026     double ret_val;
14027 
14028 
14029 /* *********************************************************** */
14030 
14031 /*                                              From STRIPACK */
14032 /*                                            Robert J. Renka */
14033 /*                                  Dept. of Computer Science */
14034 /*                                       Univ. of North Texas */
14035 /*                                           renka@cs.unt.edu */
14036 /*                                                   05/09/92 */
14037 
14038 /*   This function forces its argument X to be stored in a */
14039 /* memory location, thus providing a means of determining */
14040 /* floating point number characteristics (such as the machine */
14041 /* precision) when it is necessary to avoid computation in */
14042 /* high precision registers. */
14043 
14044 
14045 /* On input: */
14046 
14047 /*       X = Value to be stored. */
14048 
14049 /* X is not altered by this function. */
14050 
14051 /* On output: */
14052 
14053 /*       STORE = Value of X after it has been stored and */
14054 /*               possibly truncated or rounded to the single */
14055 /*               precision word length. */
14056 
14057 /* Modules required by STORE:  None */
14058 
14059 /* *********************************************************** */
14060 
14061     stcom_1.y = *x;
14062     ret_val = stcom_1.y;
14063     return ret_val;
14064 } /* store_ */
14065 
14066 /* Subroutine */ int swap_(int *in1, int *in2, int *io1, int *
14067         io2, int *list, int *lptr, int *lend, int *lp21)
14068 {
14069     /* System generated locals */
14070     int i__1;
14071 
14072     /* Local variables */
14073     static int lp, lph, lpsav;
14074     extern int lstptr_(int *, int *, int *, int *);
14075 
14076 
14077 /* *********************************************************** */
14078 
14079 /*                                              From STRIPACK */
14080 /*                                            Robert J. Renka */
14081 /*                                  Dept. of Computer Science */
14082 /*                                       Univ. of North Texas */
14083 /*                                           renka@cs.unt.edu */
14084 /*                                                   06/22/98 */
14085 
14086 /*   Given a triangulation of a set of points on the unit */
14087 /* sphere, this subroutine replaces a diagonal arc in a */
14088 /* strictly convex quadrilateral (defined by a pair of adja- */
14089 /* cent triangles) with the other diagonal.  Equivalently, a */
14090 /* pair of adjacent triangles is replaced by another pair */
14091 /* having the same union. */
14092 
14093 
14094 /* On input: */
14095 
14096 /*       IN1,IN2,IO1,IO2 = Nodal indexes of the vertices of */
14097 /*                         the quadrilateral.  IO1-IO2 is re- */
14098 /*                         placed by IN1-IN2.  (IO1,IO2,IN1) */
14099 /*                         and (IO2,IO1,IN2) must be trian- */
14100 /*                         gles on input. */
14101 
14102 /* The above parameters are not altered by this routine. */
14103 
14104 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14105 /*                        gulation.  Refer to Subroutine */
14106 /*                        TRMESH. */
14107 
14108 /* On output: */
14109 
14110 /*       LIST,LPTR,LEND = Data structure updated with the */
14111 /*                        swap -- triangles (IO1,IO2,IN1) and */
14112 /*                        (IO2,IO1,IN2) are replaced by */
14113 /*                        (IN1,IN2,IO2) and (IN2,IN1,IO1) */
14114 /*                        unless LP21 = 0. */
14115 
14116 /*       LP21 = Index of IN1 as a neighbor of IN2 after the */
14117 /*              swap is performed unless IN1 and IN2 are */
14118 /*              adjacent on input, in which case LP21 = 0. */
14119 
14120 /* Module required by SWAP:  LSTPTR */
14121 
14122 /* Intrinsic function called by SWAP:  ABS */
14123 
14124 /* *********************************************************** */
14125 
14126 
14127 /* Local parameters: */
14128 
14129 /* LP,LPH,LPSAV = LIST pointers */
14130 
14131 
14132 /* Test for IN1 and IN2 adjacent. */
14133 
14134     /* Parameter adjustments */
14135     --lend;
14136     --lptr;
14137     --list;
14138 
14139     /* Function Body */
14140     lp = lstptr_(&lend[*in1], in2, &list[1], &lptr[1]);
14141     if ((i__1 = list[lp], abs(i__1)) == *in2) {
14142         *lp21 = 0;
14143         return 0;
14144     }
14145 
14146 /* Delete IO2 as a neighbor of IO1. */
14147 
14148     lp = lstptr_(&lend[*io1], in2, &list[1], &lptr[1]);
14149     lph = lptr[lp];
14150     lptr[lp] = lptr[lph];
14151 
14152 /* If IO2 is the last neighbor of IO1, make IN2 the */
14153 /*   last neighbor. */
14154 
14155     if (lend[*io1] == lph) {
14156         lend[*io1] = lp;
14157     }
14158 
14159 /* Insert IN2 as a neighbor of IN1 following IO1 */
14160 /*   using the hole created above. */
14161 
14162     lp = lstptr_(&lend[*in1], io1, &list[1], &lptr[1]);
14163     lpsav = lptr[lp];
14164     lptr[lp] = lph;
14165     list[lph] = *in2;
14166     lptr[lph] = lpsav;
14167 
14168 /* Delete IO1 as a neighbor of IO2. */
14169 
14170     lp = lstptr_(&lend[*io2], in1, &list[1], &lptr[1]);
14171     lph = lptr[lp];
14172     lptr[lp] = lptr[lph];
14173 
14174 /* If IO1 is the last neighbor of IO2, make IN1 the */
14175 /*   last neighbor. */
14176 
14177     if (lend[*io2] == lph) {
14178         lend[*io2] = lp;
14179     }
14180 
14181 /* Insert IN1 as a neighbor of IN2 following IO2. */
14182 
14183     lp = lstptr_(&lend[*in2], io2, &list[1], &lptr[1]);
14184     lpsav = lptr[lp];
14185     lptr[lp] = lph;
14186     list[lph] = *in1;
14187     lptr[lph] = lpsav;
14188     *lp21 = lph;
14189     return 0;
14190 } /* swap_ */
14191 
14192 long int swptst_(int *n1, int *n2, int *n3, int *n4,
14193         double *x, double *y, double *z__)
14194 {
14195     /* System generated locals */
14196     long int ret_val;
14197 
14198     /* Local variables */
14199     static double x4, y4, z4, dx1, dx2, dx3, dy1, dy2, dy3, dz1, dz2, dz3;
14200 
14201 
14202 /* *********************************************************** */
14203 
14204 /*                                              From STRIPACK */
14205 /*                                            Robert J. Renka */
14206 /*                                  Dept. of Computer Science */
14207 /*                                       Univ. of North Texas */
14208 /*                                           renka@cs.unt.edu */
14209 /*                                                   03/29/91 */
14210 
14211 /*   This function decides whether or not to replace a */
14212 /* diagonal arc in a quadrilateral with the other diagonal. */
14213 /* The decision will be to swap (SWPTST = TRUE) if and only */
14214 /* if N4 lies above the plane (in the half-space not contain- */
14215 /* ing the origin) defined by (N1,N2,N3), or equivalently, if */
14216 /* the projection of N4 onto this plane is interior to the */
14217 /* circumcircle of (N1,N2,N3).  The decision will be for no */
14218 /* swap if the quadrilateral is not strictly convex. */
14219 
14220 
14221 /* On input: */
14222 
14223 /*       N1,N2,N3,N4 = Indexes of the four nodes defining the */
14224 /*                     quadrilateral with N1 adjacent to N2, */
14225 /*                     and (N1,N2,N3) in counterclockwise */
14226 /*                     order.  The arc connecting N1 to N2 */
14227 /*                     should be replaced by an arc connec- */
14228 /*                     ting N3 to N4 if SWPTST = TRUE.  Refer */
14229 /*                     to Subroutine SWAP. */
14230 
14231 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14232 /*               coordinates of the nodes.  (X(I),Y(I),Z(I)) */
14233 /*               define node I for I = N1, N2, N3, and N4. */
14234 
14235 /* Input parameters are not altered by this routine. */
14236 
14237 /* On output: */
14238 
14239 /*       SWPTST = TRUE if and only if the arc connecting N1 */
14240 /*                and N2 should be swapped for an arc con- */
14241 /*                necting N3 and N4. */
14242 
14243 /* Modules required by SWPTST:  None */
14244 
14245 /* *********************************************************** */
14246 
14247 
14248 /* Local parameters: */
14249 
14250 /* DX1,DY1,DZ1 = Coordinates of N4->N1 */
14251 /* DX2,DY2,DZ2 = Coordinates of N4->N2 */
14252 /* DX3,DY3,DZ3 = Coordinates of N4->N3 */
14253 /* X4,Y4,Z4 =    Coordinates of N4 */
14254 
14255     /* Parameter adjustments */
14256     --z__;
14257     --y;
14258     --x;
14259 
14260     /* Function Body */
14261     x4 = x[*n4];
14262     y4 = y[*n4];
14263     z4 = z__[*n4];
14264     dx1 = x[*n1] - x4;
14265     dx2 = x[*n2] - x4;
14266     dx3 = x[*n3] - x4;
14267     dy1 = y[*n1] - y4;
14268     dy2 = y[*n2] - y4;
14269     dy3 = y[*n3] - y4;
14270     dz1 = z__[*n1] - z4;
14271     dz2 = z__[*n2] - z4;
14272     dz3 = z__[*n3] - z4;
14273 
14274 /* N4 lies above the plane of (N1,N2,N3) iff N3 lies above */
14275 /*   the plane of (N2,N1,N4) iff Det(N3-N4,N2-N4,N1-N4) = */
14276 /*   (N3-N4,N2-N4 X N1-N4) > 0. */
14277 
14278     ret_val = dx3 * (dy2 * dz1 - dy1 * dz2) - dy3 * (dx2 * dz1 - dx1 * dz2) +
14279             dz3 * (dx2 * dy1 - dx1 * dy2) > 0.;
14280     return ret_val;
14281 } /* swptst_ */
14282 
14283 /* Subroutine */ int trans_(int *n, double *rlat, double *rlon,
14284         double *x, double *y, double *z__)
14285 {
14286     /* System generated locals */
14287     int i__1;
14288 
14289     /* Builtin functions */
14290     //double cos(double), sin(double);
14291 
14292     /* Local variables */
14293     static int i__, nn;
14294     static double phi, theta, cosphi;
14295 
14296 
14297 /* *********************************************************** */
14298 
14299 /*                                              From STRIPACK */
14300 /*                                            Robert J. Renka */
14301 /*                                  Dept. of Computer Science */
14302 /*                                       Univ. of North Texas */
14303 /*                                           renka@cs.unt.edu */
14304 /*                                                   04/08/90 */
14305 
14306 /*   This subroutine transforms spherical coordinates into */
14307 /* Cartesian coordinates on the unit sphere for input to */
14308 /* Subroutine TRMESH.  Storage for X and Y may coincide with */
14309 /* storage for RLAT and RLON if the latter need not be saved. */
14310 
14311 
14312 /* On input: */
14313 
14314 /*       N = Number of nodes (points on the unit sphere) */
14315 /*           whose coordinates are to be transformed. */
14316 
14317 /*       RLAT = Array of length N containing latitudinal */
14318 /*              coordinates of the nodes in radians. */
14319 
14320 /*       RLON = Array of length N containing longitudinal */
14321 /*              coordinates of the nodes in radians. */
14322 
14323 /* The above parameters are not altered by this routine. */
14324 
14325 /*       X,Y,Z = Arrays of length at least N. */
14326 
14327 /* On output: */
14328 
14329 /*       X,Y,Z = Cartesian coordinates in the range -1 to 1. */
14330 /*               X(I)**2 + Y(I)**2 + Z(I)**2 = 1 for I = 1 */
14331 /*               to N. */
14332 
14333 /* Modules required by TRANS:  None */
14334 
14335 /* Intrinsic functions called by TRANS:  COS, SIN */
14336 
14337 /* *********************************************************** */
14338 
14339 
14340 /* Local parameters: */
14341 
14342 /* COSPHI = cos(PHI) */
14343 /* I =      DO-loop index */
14344 /* NN =     Local copy of N */
14345 /* PHI =    Latitude */
14346 /* THETA =  Longitude */
14347 
14348     /* Parameter adjustments */
14349     --z__;
14350     --y;
14351     --x;
14352     --rlon;
14353     --rlat;
14354 
14355     /* Function Body */
14356     nn = *n;
14357     i__1 = nn;
14358     for (i__ = 1; i__ <= i__1; ++i__) {
14359         phi = rlat[i__];
14360         theta = rlon[i__];
14361         cosphi = cos(phi);
14362         x[i__] = cosphi * cos(theta);
14363         y[i__] = cosphi * sin(theta);
14364         z__[i__] = sin(phi);
14365 /* L1: */
14366     }
14367     return 0;
14368 } /* trans_ */
14369 
14370 /* Subroutine */ int trfind_(int *nst, double *p, int *n,
14371         double *x, double *y, double *z__, int *list, int
14372         *lptr, int *lend, double *b1, double *b2, double *b3,
14373         int *i1, int *i2, int *i3)
14374 {
14375     /* Initialized data */
14376 
14377     static int ix = 1;
14378     static int iy = 2;
14379     static int iz = 3;
14380 
14381     /* System generated locals */
14382     int i__1;
14383     double d__1, d__2;
14384 
14385     /* Local variables */
14386     static double q[3];
14387     static int n0, n1, n2, n3, n4, nf;
14388     static double s12;
14389     static int nl, lp;
14390     static double xp, yp, zp;
14391     static int n1s, n2s;
14392     static double eps, tol, ptn1, ptn2;
14393     static int next;
14394     extern int jrand_(int *, int *, int *, int *);
14395     extern double store_(double *);
14396     extern int lstptr_(int *, int *, int *, int *);
14397 
14398 
14399 /* *********************************************************** */
14400 
14401 /*                                              From STRIPACK */
14402 /*                                            Robert J. Renka */
14403 /*                                  Dept. of Computer Science */
14404 /*                                       Univ. of North Texas */
14405 /*                                           renka@cs.unt.edu */
14406 /*                                                   11/30/99 */
14407 
14408 /*   This subroutine locates a point P relative to a triangu- */
14409 /* lation created by Subroutine TRMESH.  If P is contained in */
14410 /* a triangle, the three vertex indexes and barycentric coor- */
14411 /* dinates are returned.  Otherwise, the indexes of the */
14412 /* visible boundary nodes are returned. */
14413 
14414 
14415 /* On input: */
14416 
14417 /*       NST = Index of a node at which TRFIND begins its */
14418 /*             search.  Search time depends on the proximity */
14419 /*             of this node to P. */
14420 
14421 /*       P = Array of length 3 containing the x, y, and z */
14422 /*           coordinates (in that order) of the point P to be */
14423 /*           located. */
14424 
14425 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14426 
14427 /*       X,Y,Z = Arrays of length N containing the Cartesian */
14428 /*               coordinates of the triangulation nodes (unit */
14429 /*               vectors).  (X(I),Y(I),Z(I)) defines node I */
14430 /*               for I = 1 to N. */
14431 
14432 /*       LIST,LPTR,LEND = Data structure defining the trian- */
14433 /*                        gulation.  Refer to Subroutine */
14434 /*                        TRMESH. */
14435 
14436 /* Input parameters are not altered by this routine. */
14437 
14438 /* On output: */
14439 
14440 /*       B1,B2,B3 = Unnormalized barycentric coordinates of */
14441 /*                  the central projection of P onto the un- */
14442 /*                  derlying planar triangle if P is in the */
14443 /*                  convex hull of the nodes.  These parame- */
14444 /*                  ters are not altered if I1 = 0. */
14445 
14446 /*       I1,I2,I3 = Counterclockwise-ordered vertex indexes */
14447 /*                  of a triangle containing P if P is con- */
14448 /*                  tained in a triangle.  If P is not in the */
14449 /*                  convex hull of the nodes, I1 and I2 are */
14450 /*                  the rightmost and leftmost (boundary) */
14451 /*                  nodes that are visible from P, and */
14452 /*                  I3 = 0.  (If all boundary nodes are vis- */
14453 /*                  ible from P, then I1 and I2 coincide.) */
14454 /*                  I1 = I2 = I3 = 0 if P and all of the */
14455 /*                  nodes are coplanar (lie on a common great */
14456 /*                  circle. */
14457 
14458 /* Modules required by TRFIND:  JRAND, LSTPTR, STORE */
14459 
14460 /* Intrinsic function called by TRFIND:  ABS */
14461 
14462 /* *********************************************************** */
14463 
14464 
14465     /* Parameter adjustments */
14466     --p;
14467     --lend;
14468     --z__;
14469     --y;
14470     --x;
14471     --list;
14472     --lptr;
14473 
14474     /* Function Body */
14475 
14476 /* Local parameters: */
14477 
14478 /* EPS =      Machine precision */
14479 /* IX,IY,IZ = int seeds for JRAND */
14480 /* LP =       LIST pointer */
14481 /* N0,N1,N2 = Nodes in counterclockwise order defining a */
14482 /*              cone (with vertex N0) containing P, or end- */
14483 /*              points of a boundary edge such that P Right */
14484 /*              N1->N2 */
14485 /* N1S,N2S =  Initially-determined values of N1 and N2 */
14486 /* N3,N4 =    Nodes opposite N1->N2 and N2->N1, respectively */
14487 /* NEXT =     Candidate for I1 or I2 when P is exterior */
14488 /* NF,NL =    First and last neighbors of N0, or first */
14489 /*              (rightmost) and last (leftmost) nodes */
14490 /*              visible from P when P is exterior to the */
14491 /*              triangulation */
14492 /* PTN1 =     Scalar product <P,N1> */
14493 /* PTN2 =     Scalar product <P,N2> */
14494 /* Q =        (N2 X N1) X N2  or  N1 X (N2 X N1) -- used in */
14495 /*              the boundary traversal when P is exterior */
14496 /* S12 =      Scalar product <N1,N2> */
14497 /* TOL =      Tolerance (multiple of EPS) defining an upper */
14498 /*              bound on the magnitude of a negative bary- */
14499 /*              centric coordinate (B1 or B2) for P in a */
14500 /*              triangle -- used to avoid an infinite number */
14501 /*              of restarts with 0 <= B3 < EPS and B1 < 0 or */
14502 /*              B2 < 0 but small in magnitude */
14503 /* XP,YP,ZP = Local variables containing P(1), P(2), and P(3) */
14504 /* X0,Y0,Z0 = Dummy arguments for DET */
14505 /* X1,Y1,Z1 = Dummy arguments for DET */
14506 /* X2,Y2,Z2 = Dummy arguments for DET */
14507 
14508 /* Statement function: */
14509 
14510 /* DET(X1,...,Z0) .GE. 0 if and only if (X0,Y0,Z0) is in the */
14511 /*                       (closed) left hemisphere defined by */
14512 /*                       the plane containing (0,0,0), */
14513 /*                       (X1,Y1,Z1), and (X2,Y2,Z2), where */
14514 /*                       left is defined relative to an ob- */
14515 /*                       server at (X1,Y1,Z1) facing */
14516 /*                       (X2,Y2,Z2). */
14517 
14518 
14519 /* Initialize variables. */
14520 
14521     xp = p[1];
14522     yp = p[2];
14523     zp = p[3];
14524     n0 = *nst;
14525     if (n0 < 1 || n0 > *n) {
14526         n0 = jrand_(n, &ix, &iy, &iz);
14527     }
14528 
14529 /* Compute the relative machine precision EPS and TOL. */
14530 
14531     eps = 1.;
14532 L1:
14533     eps /= 2.;
14534     d__1 = eps + 1.;
14535     if (store_(&d__1) > 1.) {
14536         goto L1;
14537     }
14538     eps *= 2.;
14539     tol = eps * 4.;
14540 
14541 /* Set NF and NL to the first and last neighbors of N0, and */
14542 /*   initialize N1 = NF. */
14543 
14544 L2:
14545     lp = lend[n0];
14546     nl = list[lp];
14547     lp = lptr[lp];
14548     nf = list[lp];
14549     n1 = nf;
14550 
14551 /* Find a pair of adjacent neighbors N1,N2 of N0 that define */
14552 /*   a wedge containing P:  P LEFT N0->N1 and P RIGHT N0->N2. */
14553 
14554     if (nl > 0) {
14555 
14556 /*   N0 is an interior node.  Find N1. */
14557 
14558 L3:
14559         if (xp * (y[n0] * z__[n1] - y[n1] * z__[n0]) - yp * (x[n0] * z__[n1]
14560                 - x[n1] * z__[n0]) + zp * (x[n0] * y[n1] - x[n1] * y[n0]) <
14561                 -1e-10) {
14562             lp = lptr[lp];
14563             n1 = list[lp];
14564             if (n1 == nl) {
14565                 goto L6;
14566             }
14567             goto L3;
14568         }
14569     } else {
14570 
14571 /*   N0 is a boundary node.  Test for P exterior. */
14572 
14573         nl = -nl;
14574         if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf]
14575                 - x[nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) <
14576                 -1e-10) {
14577 
14578 /*   P is to the right of the boundary edge N0->NF. */
14579 
14580             n1 = n0;
14581             n2 = nf;
14582             goto L9;
14583         }
14584         if (xp * (y[nl] * z__[n0] - y[n0] * z__[nl]) - yp * (x[nl] * z__[n0]
14585                 - x[n0] * z__[nl]) + zp * (x[nl] * y[n0] - x[n0] * y[nl]) <
14586                 -1e-10) {
14587 
14588 /*   P is to the right of the boundary edge NL->N0. */
14589 
14590             n1 = nl;
14591             n2 = n0;
14592             goto L9;
14593         }
14594     }
14595 
14596 /* P is to the left of arcs N0->N1 and NL->N0.  Set N2 to the */
14597 /*   next neighbor of N0 (following N1). */
14598 
14599 L4:
14600     lp = lptr[lp];
14601     n2 = (i__1 = list[lp], abs(i__1));
14602     if (xp * (y[n0] * z__[n2] - y[n2] * z__[n0]) - yp * (x[n0] * z__[n2] - x[
14603             n2] * z__[n0]) + zp * (x[n0] * y[n2] - x[n2] * y[n0]) < -1e-10) {
14604         goto L7;
14605     }
14606     n1 = n2;
14607     if (n1 != nl) {
14608         goto L4;
14609     }
14610     if (xp * (y[n0] * z__[nf] - y[nf] * z__[n0]) - yp * (x[n0] * z__[nf] - x[
14611             nf] * z__[n0]) + zp * (x[n0] * y[nf] - x[nf] * y[n0]) < -1e-10) {
14612         goto L6;
14613     }
14614 
14615 /* P is left of or on arcs N0->NB for all neighbors NB */
14616 /*   of N0.  Test for P = +/-N0. */
14617 
14618     d__2 = (d__1 = x[n0] * xp + y[n0] * yp + z__[n0] * zp, abs(d__1));
14619     if (store_(&d__2) < 1. - eps * 4.) {
14620 
14621 /*   All points are collinear iff P Left NB->N0 for all */
14622 /*     neighbors NB of N0.  Search the neighbors of N0. */
14623 /*     Note:  N1 = NL and LP points to NL. */
14624 
14625 L5:
14626         if (xp * (y[n1] * z__[n0] - y[n0] * z__[n1]) - yp * (x[n1] * z__[n0]
14627                 - x[n0] * z__[n1]) + zp * (x[n1] * y[n0] - x[n0] * y[n1]) >
14628                 -1e-10) {
14629             lp = lptr[lp];
14630             n1 = (i__1 = list[lp], abs(i__1));
14631             if (n1 == nl) {
14632                 goto L14;
14633             }
14634             goto L5;
14635         }
14636     }
14637 
14638 /* P is to the right of N1->N0, or P = +/-N0.  Set N0 to N1 */
14639 /*   and start over. */
14640 
14641     n0 = n1;
14642     goto L2;
14643 
14644 /* P is between arcs N0->N1 and N0->NF. */
14645 
14646 L6:
14647     n2 = nf;
14648 
14649 /* P is contained in a wedge defined by geodesics N0-N1 and */
14650 /*   N0-N2, where N1 is adjacent to N2.  Save N1 and N2 to */
14651 /*   test for cycling. */
14652 
14653 L7:
14654     n3 = n0;
14655     n1s = n1;
14656     n2s = n2;
14657 
14658 /* Top of edge-hopping loop: */
14659 
14660 L8:
14661 
14662     *b3 = xp * (y[n1] * z__[n2] - y[n2] * z__[n1]) - yp * (x[n1] * z__[n2] -
14663             x[n2] * z__[n1]) + zp * (x[n1] * y[n2] - x[n2] * y[n1]);
14664      if (*b3 < -1e-10) {
14665 
14666 /*   Set N4 to the first neighbor of N2 following N1 (the */
14667 /*     node opposite N2->N1) unless N1->N2 is a boundary arc. */
14668 
14669         lp = lstptr_(&lend[n2], &n1, &list[1], &lptr[1]);
14670         if (list[lp] < 0) {
14671             goto L9;
14672         }
14673         lp = lptr[lp];
14674         n4 = (i__1 = list[lp], abs(i__1));
14675 
14676 /*   Define a new arc N1->N2 which intersects the geodesic */
14677 /*     N0-P. */
14678         if (xp * (y[n0] * z__[n4] - y[n4] * z__[n0]) - yp * (x[n0] * z__[n4]
14679                 - x[n4] * z__[n0]) + zp * (x[n0] * y[n4] - x[n4] * y[n0]) <
14680                 -1e-10) {
14681             n3 = n2;
14682             n2 = n4;
14683             n1s = n1;
14684             if (n2 != n2s && n2 != n0) {
14685                 goto L8;
14686             }
14687         } else {
14688             n3 = n1;
14689             n1 = n4;
14690             n2s = n2;
14691             if (n1 != n1s && n1 != n0) {
14692                 goto L8;
14693             }
14694         }
14695 
14696 /*   The starting node N0 or edge N1-N2 was encountered */
14697 /*     again, implying a cycle (infinite loop).  Restart */
14698 /*     with N0 randomly selected. */
14699 
14700         n0 = jrand_(n, &ix, &iy, &iz);
14701         goto L2;
14702     }
14703 
14704 /* P is in (N1,N2,N3) unless N0, N1, N2, and P are collinear */
14705 /*   or P is close to -N0. */
14706 
14707     if (*b3 >= eps) {
14708 
14709 /*   B3 .NE. 0. */
14710 
14711         *b1 = xp * (y[n2] * z__[n3] - y[n3] * z__[n2]) - yp * (x[n2] * z__[n3]
14712                  - x[n3] * z__[n2]) + zp * (x[n2] * y[n3] - x[n3] * y[n2]);
14713         *b2 = xp * (y[n3] * z__[n1] - y[n1] * z__[n3]) - yp * (x[n3] * z__[n1]
14714                  - x[n1] * z__[n3]) + zp * (x[n3] * y[n1] - x[n1] * y[n3]);
14715         if (*b1 < -tol || *b2 < -tol) {
14716 
14717 /*   Restart with N0 randomly selected. */
14718 
14719             n0 = jrand_(n, &ix, &iy, &iz);
14720             goto L2;
14721         }
14722     } else {
14723 
14724 /*   B3 = 0 and thus P lies on N1->N2. Compute */
14725 /*     B1 = Det(P,N2 X N1,N2) and B2 = Det(P,N1,N2 X N1). */
14726 
14727         *b3 = 0.;
14728         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14729         ptn1 = xp * x[n1] + yp * y[n1] + zp * z__[n1];
14730         ptn2 = xp * x[n2] + yp * y[n2] + zp * z__[n2];
14731         *b1 = ptn1 - s12 * ptn2;
14732         *b2 = ptn2 - s12 * ptn1;
14733         if (*b1 < -tol || *b2 < -tol) {
14734 
14735 /*   Restart with N0 randomly selected. */
14736 
14737             n0 = jrand_(n, &ix, &iy, &iz);
14738             goto L2;
14739         }
14740     }
14741 
14742 /* P is in (N1,N2,N3). */
14743 
14744     *i1 = n1;
14745     *i2 = n2;
14746     *i3 = n3;
14747     if (*b1 < 0.f) {
14748         *b1 = 0.f;
14749     }
14750     if (*b2 < 0.f) {
14751         *b2 = 0.f;
14752     }
14753     return 0;
14754 
14755 /* P Right N1->N2, where N1->N2 is a boundary edge. */
14756 /*   Save N1 and N2, and set NL = 0 to indicate that */
14757 /*   NL has not yet been found. */
14758 
14759 L9:
14760     n1s = n1;
14761     n2s = n2;
14762     nl = 0;
14763 
14764 /*           Counterclockwise Boundary Traversal: */
14765 
14766 L10:
14767 
14768     lp = lend[n2];
14769     lp = lptr[lp];
14770     next = list[lp];
14771      if (xp * (y[n2] * z__[next] - y[next] * z__[n2]) - yp * (x[n2] * z__[next]
14772              - x[next] * z__[n2]) + zp * (x[n2] * y[next] - x[next] * y[n2])
14773             >= -1e-10) {
14774 
14775 /*   N2 is the rightmost visible node if P Forward N2->N1 */
14776 /*     or NEXT Forward N2->N1.  Set Q to (N2 X N1) X N2. */
14777 
14778         s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14779         q[0] = x[n1] - s12 * x[n2];
14780         q[1] = y[n1] - s12 * y[n2];
14781         q[2] = z__[n1] - s12 * z__[n2];
14782         if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14783             goto L11;
14784         }
14785         if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14786             goto L11;
14787         }
14788 
14789 /*   N1, N2, NEXT, and P are nearly collinear, and N2 is */
14790 /*     the leftmost visible node. */
14791 
14792         nl = n2;
14793     }
14794 
14795 /* Bottom of counterclockwise loop: */
14796 
14797     n1 = n2;
14798     n2 = next;
14799     if (n2 != n1s) {
14800         goto L10;
14801     }
14802 
14803 /* All boundary nodes are visible from P. */
14804 
14805     *i1 = n1s;
14806     *i2 = n1s;
14807     *i3 = 0;
14808     return 0;
14809 
14810 /* N2 is the rightmost visible node. */
14811 
14812 L11:
14813     nf = n2;
14814     if (nl == 0) {
14815 
14816 /* Restore initial values of N1 and N2, and begin the search */
14817 /*   for the leftmost visible node. */
14818 
14819         n2 = n2s;
14820         n1 = n1s;
14821 
14822 /*           Clockwise Boundary Traversal: */
14823 
14824 L12:
14825         lp = lend[n1];
14826         next = -list[lp];
14827         if (xp * (y[next] * z__[n1] - y[n1] * z__[next]) - yp * (x[next] *
14828                 z__[n1] - x[n1] * z__[next]) + zp * (x[next] * y[n1] - x[n1] *
14829                  y[next]) >= -1e-10) {
14830 
14831 /*   N1 is the leftmost visible node if P or NEXT is */
14832 /*     forward of N1->N2.  Compute Q = N1 X (N2 X N1). */
14833 
14834             s12 = x[n1] * x[n2] + y[n1] * y[n2] + z__[n1] * z__[n2];
14835             q[0] = x[n2] - s12 * x[n1];
14836             q[1] = y[n2] - s12 * y[n1];
14837             q[2] = z__[n2] - s12 * z__[n1];
14838             if (xp * q[0] + yp * q[1] + zp * q[2] >= 0.) {
14839                 goto L13;
14840             }
14841             if (x[next] * q[0] + y[next] * q[1] + z__[next] * q[2] >= 0.) {
14842                 goto L13;
14843             }
14844 
14845 /*   P, NEXT, N1, and N2 are nearly collinear and N1 is the */
14846 /*     rightmost visible node. */
14847 
14848             nf = n1;
14849         }
14850 
14851 /* Bottom of clockwise loop: */
14852 
14853         n2 = n1;
14854         n1 = next;
14855         if (n1 != n1s) {
14856             goto L12;
14857         }
14858 
14859 /* All boundary nodes are visible from P. */
14860 
14861         *i1 = n1;
14862         *i2 = n1;
14863         *i3 = 0;
14864         return 0;
14865 
14866 /* N1 is the leftmost visible node. */
14867 
14868 L13:
14869         nl = n1;
14870     }
14871 
14872 /* NF and NL have been found. */
14873 
14874     *i1 = nf;
14875     *i2 = nl;
14876     *i3 = 0;
14877     return 0;
14878 
14879 /* All points are collinear (coplanar). */
14880 
14881 L14:
14882     *i1 = 0;
14883     *i2 = 0;
14884     *i3 = 0;
14885     return 0;
14886 } /* trfind_ */
14887 
14888 /* Subroutine */ int trlist_(int *n, int *list, int *lptr,
14889         int *lend, int *nrow, int *nt, int *ltri, int *
14890         ier)
14891 {
14892     /* System generated locals */
14893     int ltri_dim1, ltri_offset, i__1, i__2;
14894 
14895     /* Local variables */
14896     static int i__, j, i1, i2, i3, n1, n2, n3, ka, kn, lp, kt, nm2, lp2,
14897             lpl, isv;
14898     static long int arcs;
14899     static int lpln1;
14900 
14901 
14902 /* *********************************************************** */
14903 
14904 /*                                              From STRIPACK */
14905 /*                                            Robert J. Renka */
14906 /*                                  Dept. of Computer Science */
14907 /*                                       Univ. of North Texas */
14908 /*                                           renka@cs.unt.edu */
14909 /*                                                   07/20/96 */
14910 
14911 /*   This subroutine converts a triangulation data structure */
14912 /* from the linked list created by Subroutine TRMESH to a */
14913 /* triangle list. */
14914 
14915 /* On input: */
14916 
14917 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
14918 
14919 /*       LIST,LPTR,LEND = Linked list data structure defin- */
14920 /*                        ing the triangulation.  Refer to */
14921 /*                        Subroutine TRMESH. */
14922 
14923 /*       NROW = Number of rows (entries per triangle) re- */
14924 /*              served for the triangle list LTRI.  The value */
14925 /*              must be 6 if only the vertex indexes and */
14926 /*              neighboring triangle indexes are to be */
14927 /*              stored, or 9 if arc indexes are also to be */
14928 /*              assigned and stored.  Refer to LTRI. */
14929 
14930 /* The above parameters are not altered by this routine. */
14931 
14932 /*       LTRI = int array of length at least NROW*NT, */
14933 /*              where NT is at most 2N-4.  (A sufficient */
14934 /*              length is 12N if NROW=6 or 18N if NROW=9.) */
14935 
14936 /* On output: */
14937 
14938 /*       NT = Number of triangles in the triangulation unless */
14939 /*            IER .NE. 0, in which case NT = 0.  NT = 2N-NB-2 */
14940 /*            if NB .GE. 3 or 2N-4 if NB = 0, where NB is the */
14941 /*            number of boundary nodes. */
14942 
14943 /*       LTRI = NROW by NT array whose J-th column contains */
14944 /*              the vertex nodal indexes (first three rows), */
14945 /*              neighboring triangle indexes (second three */
14946 /*              rows), and, if NROW = 9, arc indexes (last */
14947 /*              three rows) associated with triangle J for */
14948 /*              J = 1,...,NT.  The vertices are ordered */
14949 /*              counterclockwise with the first vertex taken */
14950 /*              to be the one with smallest index.  Thus, */
14951 /*              LTRI(2,J) and LTRI(3,J) are larger than */
14952 /*              LTRI(1,J) and index adjacent neighbors of */
14953 /*              node LTRI(1,J).  For I = 1,2,3, LTRI(I+3,J) */
14954 /*              and LTRI(I+6,J) index the triangle and arc, */
14955 /*              respectively, which are opposite (not shared */
14956 /*              by) node LTRI(I,J), with LTRI(I+3,J) = 0 if */
14957 /*              LTRI(I+6,J) indexes a boundary arc.  Vertex */
14958 /*              indexes range from 1 to N, triangle indexes */
14959 /*              from 0 to NT, and, if included, arc indexes */
14960 /*              from 1 to NA, where NA = 3N-NB-3 if NB .GE. 3 */
14961 /*              or 3N-6 if NB = 0.  The triangles are or- */
14962 /*              dered on first (smallest) vertex indexes. */
14963 
14964 /*       IER = Error indicator. */
14965 /*             IER = 0 if no errors were encountered. */
14966 /*             IER = 1 if N or NROW is outside its valid */
14967 /*                     range on input. */
14968 /*             IER = 2 if the triangulation data structure */
14969 /*                     (LIST,LPTR,LEND) is invalid.  Note, */
14970 /*                     however, that these arrays are not */
14971 /*                     completely tested for validity. */
14972 
14973 /* Modules required by TRLIST:  None */
14974 
14975 /* Intrinsic function called by TRLIST:  ABS */
14976 
14977 /* *********************************************************** */
14978 
14979 
14980 /* Local parameters: */
14981 
14982 /* ARCS =     long int variable with value TRUE iff are */
14983 /*              indexes are to be stored */
14984 /* I,J =      LTRI row indexes (1 to 3) associated with */
14985 /*              triangles KT and KN, respectively */
14986 /* I1,I2,I3 = Nodal indexes of triangle KN */
14987 /* ISV =      Variable used to permute indexes I1,I2,I3 */
14988 /* KA =       Arc index and number of currently stored arcs */
14989 /* KN =       Index of the triangle that shares arc I1-I2 */
14990 /*              with KT */
14991 /* KT =       Triangle index and number of currently stored */
14992 /*              triangles */
14993 /* LP =       LIST pointer */
14994 /* LP2 =      Pointer to N2 as a neighbor of N1 */
14995 /* LPL =      Pointer to the last neighbor of I1 */
14996 /* LPLN1 =    Pointer to the last neighbor of N1 */
14997 /* N1,N2,N3 = Nodal indexes of triangle KT */
14998 /* NM2 =      N-2 */
14999 
15000 
15001 /* Test for invalid input parameters. */
15002 
15003     /* Parameter adjustments */
15004     --lend;
15005     --list;
15006     --lptr;
15007     ltri_dim1 = *nrow;
15008     ltri_offset = 1 + ltri_dim1;
15009     ltri -= ltri_offset;
15010 
15011     /* Function Body */
15012     if (*n < 3 || (*nrow != 6 && *nrow != 9)) {
15013         goto L11;
15014     }
15015 
15016 /* Initialize parameters for loop on triangles KT = (N1,N2, */
15017 /*   N3), where N1 < N2 and N1 < N3. */
15018 
15019 /*   ARCS = TRUE iff arc indexes are to be stored. */
15020 /*   KA,KT = Numbers of currently stored arcs and triangles. */
15021 /*   NM2 = Upper bound on candidates for N1. */
15022 
15023     arcs = *nrow == 9;
15024     ka = 0;
15025     kt = 0;
15026     nm2 = *n - 2;
15027 
15028 /* Loop on nodes N1. */
15029 
15030     i__1 = nm2;
15031     for (n1 = 1; n1 <= i__1; ++n1) {
15032 
15033 /* Loop on pairs of adjacent neighbors (N2,N3).  LPLN1 points */
15034 /*   to the last neighbor of N1, and LP2 points to N2. */
15035 
15036         lpln1 = lend[n1];
15037         lp2 = lpln1;
15038 L1:
15039         lp2 = lptr[lp2];
15040         n2 = list[lp2];
15041         lp = lptr[lp2];
15042         n3 = (i__2 = list[lp], abs(i__2));
15043         if (n2 < n1 || n3 < n1) {
15044             goto L8;
15045         }
15046 
15047 /* Add a new triangle KT = (N1,N2,N3). */
15048 
15049         ++kt;
15050         ltri[kt * ltri_dim1 + 1] = n1;
15051         ltri[kt * ltri_dim1 + 2] = n2;
15052         ltri[kt * ltri_dim1 + 3] = n3;
15053 
15054 /* Loop on triangle sides (I2,I1) with neighboring triangles */
15055 /*   KN = (I1,I2,I3). */
15056 
15057         for (i__ = 1; i__ <= 3; ++i__) {
15058             if (i__ == 1) {
15059                 i1 = n3;
15060                 i2 = n2;
15061             } else if (i__ == 2) {
15062                 i1 = n1;
15063                 i2 = n3;
15064             } else {
15065                 i1 = n2;
15066                 i2 = n1;
15067             }
15068 
15069 /* Set I3 to the neighbor of I1 that follows I2 unless */
15070 /*   I2->I1 is a boundary arc. */
15071 
15072             lpl = lend[i1];
15073             lp = lptr[lpl];
15074 L2:
15075             if (list[lp] == i2) {
15076                 goto L3;
15077             }
15078             lp = lptr[lp];
15079             if (lp != lpl) {
15080                 goto L2;
15081             }
15082 
15083 /*   I2 is the last neighbor of I1 unless the data structure */
15084 /*     is invalid.  Bypass the search for a neighboring */
15085 /*     triangle if I2->I1 is a boundary arc. */
15086 
15087             if ((i__2 = list[lp], abs(i__2)) != i2) {
15088                 goto L12;
15089             }
15090             kn = 0;
15091             if (list[lp] < 0) {
15092                 goto L6;
15093             }
15094 
15095 /*   I2->I1 is not a boundary arc, and LP points to I2 as */
15096 /*     a neighbor of I1. */
15097 
15098 L3:
15099             lp = lptr[lp];
15100             i3 = (i__2 = list[lp], abs(i__2));
15101 
15102 /* Find J such that LTRI(J,KN) = I3 (not used if KN > KT), */
15103 /*   and permute the vertex indexes of KN so that I1 is */
15104 /*   smallest. */
15105 
15106             if (i1 < i2 && i1 < i3) {
15107                 j = 3;
15108             } else if (i2 < i3) {
15109                 j = 2;
15110                 isv = i1;
15111                 i1 = i2;
15112                 i2 = i3;
15113                 i3 = isv;
15114             } else {
15115                 j = 1;
15116                 isv = i1;
15117                 i1 = i3;
15118                 i3 = i2;
15119                 i2 = isv;
15120             }
15121 
15122 /* Test for KN > KT (triangle index not yet assigned). */
15123 
15124             if (i1 > n1) {
15125                 goto L7;
15126             }
15127 
15128 /* Find KN, if it exists, by searching the triangle list in */
15129 /*   reverse order. */
15130 
15131             for (kn = kt - 1; kn >= 1; --kn) {
15132                 if (ltri[kn * ltri_dim1 + 1] == i1 && ltri[kn * ltri_dim1 + 2]
15133                          == i2 && ltri[kn * ltri_dim1 + 3] == i3) {
15134                     goto L5;
15135                 }
15136 /* L4: */
15137             }
15138             goto L7;
15139 
15140 /* Store KT as a neighbor of KN. */
15141 
15142 L5:
15143             ltri[j + 3 + kn * ltri_dim1] = kt;
15144 
15145 /* Store KN as a neighbor of KT, and add a new arc KA. */
15146 
15147 L6:
15148             ltri[i__ + 3 + kt * ltri_dim1] = kn;
15149             if (arcs) {
15150                 ++ka;
15151                 ltri[i__ + 6 + kt * ltri_dim1] = ka;
15152                 if (kn != 0) {
15153                     ltri[j + 6 + kn * ltri_dim1] = ka;
15154                 }
15155             }
15156 L7:
15157             ;
15158         }
15159 
15160 /* Bottom of loop on triangles. */
15161 
15162 L8:
15163         if (lp2 != lpln1) {
15164             goto L1;
15165         }
15166 /* L9: */
15167     }
15168 
15169 /* No errors encountered. */
15170 
15171     *nt = kt;
15172     *ier = 0;
15173     return 0;
15174 
15175 /* Invalid input parameter. */
15176 
15177 L11:
15178     *nt = 0;
15179     *ier = 1;
15180     return 0;
15181 
15182 /* Invalid triangulation data structure:  I1 is a neighbor of */
15183 /*   I2, but I2 is not a neighbor of I1. */
15184 
15185 L12:
15186     *nt = 0;
15187     *ier = 2;
15188     return 0;
15189 } /* trlist_ */
15190 
15191 /* Subroutine */ int trlprt_(int *n, double *x, double *y,
15192         double *z__, int *iflag, int *nrow, int *nt, int *
15193         ltri, int *lout)
15194 {
15195     /* Initialized data */
15196 
15197     static int nmax = 9999;
15198     static int nlmax = 58;
15199 
15200     /* System generated locals */
15201     int ltri_dim1, ltri_offset, i__1;
15202 
15203     /* Local variables */
15204     static int i__, k, na, nb, nl, lun;
15205 
15206 
15207 /* *********************************************************** */
15208 
15209 /*                                              From STRIPACK */
15210 /*                                            Robert J. Renka */
15211 /*                                  Dept. of Computer Science */
15212 /*                                       Univ. of North Texas */
15213 /*                                           renka@cs.unt.edu */
15214 /*                                                   07/02/98 */
15215 
15216 /*   This subroutine prints the triangle list created by Sub- */
15217 /* routine TRLIST and, optionally, the nodal coordinates */
15218 /* (either latitude and longitude or Cartesian coordinates) */
15219 /* on long int unit LOUT.  The numbers of boundary nodes, */
15220 /* triangles, and arcs are also printed. */
15221 
15222 
15223 /* On input: */
15224 
15225 /*       N = Number of nodes in the triangulation. */
15226 /*           3 .LE. N .LE. 9999. */
15227 
15228 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15229 /*               coordinates of the nodes if IFLAG = 0, or */
15230 /*               (X and Y only) arrays of length N containing */
15231 /*               longitude and latitude, respectively, if */
15232 /*               IFLAG > 0, or unused dummy parameters if */
15233 /*               IFLAG < 0. */
15234 
15235 /*       IFLAG = Nodal coordinate option indicator: */
15236 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
15237 /*                         Cartesian coordinates) are to be */
15238 /*                         printed (to 6 decimal places). */
15239 /*               IFLAG > 0 if only X and Y (assumed to con- */
15240 /*                         tain longitude and latitude) are */
15241 /*                         to be printed (to 6 decimal */
15242 /*                         places). */
15243 /*               IFLAG < 0 if only the adjacency lists are to */
15244 /*                         be printed. */
15245 
15246 /*       NROW = Number of rows (entries per triangle) re- */
15247 /*              served for the triangle list LTRI.  The value */
15248 /*              must be 6 if only the vertex indexes and */
15249 /*              neighboring triangle indexes are stored, or 9 */
15250 /*              if arc indexes are also stored. */
15251 
15252 /*       NT = Number of triangles in the triangulation. */
15253 /*            1 .LE. NT .LE. 9999. */
15254 
15255 /*       LTRI = NROW by NT array whose J-th column contains */
15256 /*              the vertex nodal indexes (first three rows), */
15257 /*              neighboring triangle indexes (second three */
15258 /*              rows), and, if NROW = 9, arc indexes (last */
15259 /*              three rows) associated with triangle J for */
15260 /*              J = 1,...,NT. */
15261 
15262 /*       LOUT = long int unit number for output.  If LOUT is */
15263 /*              not in the range 0 to 99, output is written */
15264 /*              to unit 6. */
15265 
15266 /* Input parameters are not altered by this routine. */
15267 
15268 /* On output: */
15269 
15270 /*   The triangle list and nodal coordinates (as specified by */
15271 /* IFLAG) are written to unit LOUT. */
15272 
15273 /* Modules required by TRLPRT:  None */
15274 
15275 /* *********************************************************** */
15276 
15277     /* Parameter adjustments */
15278     --z__;
15279     --y;
15280     --x;
15281     ltri_dim1 = *nrow;
15282     ltri_offset = 1 + ltri_dim1;
15283     ltri -= ltri_offset;
15284 
15285     /* Function Body */
15286 
15287 /* Local parameters: */
15288 
15289 /* I =     DO-loop, nodal index, and row index for LTRI */
15290 /* K =     DO-loop and triangle index */
15291 /* LUN =   long int unit number for output */
15292 /* NA =    Number of triangulation arcs */
15293 /* NB =    Number of boundary nodes */
15294 /* NL =    Number of lines printed on the current page */
15295 /* NLMAX = Maximum number of print lines per page (except */
15296 /*           for the last page which may have two addi- */
15297 /*           tional lines) */
15298 /* NMAX =  Maximum value of N and NT (4-digit format) */
15299 
15300     lun = *lout;
15301     if (lun < 0 || lun > 99) {
15302         lun = 6;
15303     }
15304 
15305 /* Print a heading and test for invalid input. */
15306 
15307 /*      WRITE (LUN,100) N */
15308     nl = 3;
15309     if (*n < 3 || *n > nmax || (*nrow != 6 && *nrow != 9) || *nt < 1 || *nt >
15310             nmax) {
15311 
15312 /* Print an error message and exit. */
15313 
15314 /*        WRITE (LUN,110) N, NROW, NT */
15315         return 0;
15316     }
15317     if (*iflag == 0) {
15318 
15319 /* Print X, Y, and Z. */
15320 
15321 /*        WRITE (LUN,101) */
15322         nl = 6;
15323         i__1 = *n;
15324         for (i__ = 1; i__ <= i__1; ++i__) {
15325             if (nl >= nlmax) {
15326 /*            WRITE (LUN,108) */
15327                 nl = 0;
15328             }
15329 /*          WRITE (LUN,103) I, X(I), Y(I), Z(I) */
15330             ++nl;
15331 /* L1: */
15332         }
15333     } else if (*iflag > 0) {
15334 
15335 /* Print X (longitude) and Y (latitude). */
15336 
15337 /*        WRITE (LUN,102) */
15338         nl = 6;
15339         i__1 = *n;
15340         for (i__ = 1; i__ <= i__1; ++i__) {
15341             if (nl >= nlmax) {
15342 /*            WRITE (LUN,108) */
15343                 nl = 0;
15344             }
15345 /*          WRITE (LUN,104) I, X(I), Y(I) */
15346             ++nl;
15347 /* L2: */
15348         }
15349     }
15350 
15351 /* Print the triangulation LTRI. */
15352 
15353     if (nl > nlmax / 2) {
15354 /*        WRITE (LUN,108) */
15355         nl = 0;
15356     }
15357     if (*nrow == 6) {
15358 /*        WRITE (LUN,105) */
15359     } else {
15360 /*        WRITE (LUN,106) */
15361     }
15362     nl += 5;
15363     i__1 = *nt;
15364     for (k = 1; k <= i__1; ++k) {
15365         if (nl >= nlmax) {
15366 /*          WRITE (LUN,108) */
15367             nl = 0;
15368         }
15369 /*        WRITE (LUN,107) K, (LTRI(I,K), I = 1,NROW) */
15370         ++nl;
15371 /* L3: */
15372     }
15373 
15374 /* Print NB, NA, and NT (boundary nodes, arcs, and */
15375 /*   triangles). */
15376 
15377     nb = (*n << 1) - *nt - 2;
15378     if (nb < 3) {
15379         nb = 0;
15380         na = *n * 3 - 6;
15381     } else {
15382         na = *nt + *n - 1;
15383     }
15384 /*      WRITE (LUN,109) NB, NA, NT */
15385     return 0;
15386 
15387 /* Print formats: */
15388 
15389 /*  100 FORMAT (///18X,'STRIPACK (TRLIST) Output,  N = ',I4) */
15390 /*  101 FORMAT (//8X,'Node',10X,'X(Node)',10X,'Y(Node)',10X, */
15391 /*     .        'Z(Node)'//) */
15392 /*  102 FORMAT (//16X,'Node',8X,'Longitude',9X,'Latitude'//) */
15393 /*  103 FORMAT (8X,I4,3D17.6) */
15394 /*  104 FORMAT (16X,I4,2D17.6) */
15395 /*  105 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors'/ */
15396 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15397 /*     .        'KT2',4X,'KT3'/) */
15398 /*  106 FORMAT (//1X,'Triangle',8X,'Vertices',12X,'Neighbors', */
15399 /*     .        14X,'Arcs'/ */
15400 /*     .        4X,'KT',7X,'N1',5X,'N2',5X,'N3',4X,'KT1',4X, */
15401 /*     .        'KT2',4X,'KT3',4X,'KA1',4X,'KA2',4X,'KA3'/) */
15402 /*  107 FORMAT (2X,I4,2X,6(3X,I4),3(2X,I5)) */
15403 /*  108 FORMAT (///) */
15404 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
15405 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
15406 /*     .        ' Triangles') */
15407 /*  110 FORMAT (//1X,10X,'*** Invalid Parameter:  N =',I5, */
15408 /*     .        ', NROW =',I5,', NT =',I5,' ***') */
15409 } /* trlprt_ */
15410 
15411 /* Subroutine */ int trmesh_(int *n, double *x, double *y,
15412         double *z__, int *list, int *lptr, int *lend, int
15413         *lnew, int *near__, int *next, double *dist, int *ier)
15414 {
15415     /* System generated locals */
15416     int i__1, i__2;
15417 
15418     /* Local variables */
15419     static double d__;
15420     static int i__, j, k;
15421     static double d1, d2, d3;
15422     static int i0, lp, nn, lpl;
15423     extern long int left_(double *, double *, double *, double
15424             *, double *, double *, double *, double *,
15425             double *);
15426     static int nexti;
15427     extern /* Subroutine */ int addnod_(int *, int *, double *,
15428             double *, double *, int *, int *, int *,
15429             int *, int *);
15430 
15431 
15432 /* *********************************************************** */
15433 
15434 /*                                              From STRIPACK */
15435 /*                                            Robert J. Renka */
15436 /*                                  Dept. of Computer Science */
15437 /*                                       Univ. of North Texas */
15438 /*                                           renka@cs.unt.edu */
15439 /*                                                   03/04/03 */
15440 
15441 /*   This subroutine creates a Delaunay triangulation of a */
15442 /* set of N arbitrarily distributed points, referred to as */
15443 /* nodes, on the surface of the unit sphere.  The Delaunay */
15444 /* triangulation is defined as a set of (spherical) triangles */
15445 /* with the following five properties: */
15446 
15447 /*  1)  The triangle vertices are nodes. */
15448 /*  2)  No triangle contains a node other than its vertices. */
15449 /*  3)  The interiors of the triangles are pairwise disjoint. */
15450 /*  4)  The union of triangles is the convex hull of the set */
15451 /*        of nodes (the smallest convex set that contains */
15452 /*        the nodes).  If the nodes are not contained in a */
15453 /*        single hemisphere, their convex hull is the en- */
15454 /*        tire sphere and there are no boundary nodes. */
15455 /*        Otherwise, there are at least three boundary nodes. */
15456 /*  5)  The interior of the circumcircle of each triangle */
15457 /*        contains no node. */
15458 
15459 /* The first four properties define a triangulation, and the */
15460 /* last property results in a triangulation which is as close */
15461 /* as possible to equiangular in a certain sense and which is */
15462 /* uniquely defined unless four or more nodes lie in a common */
15463 /* plane.  This property makes the triangulation well-suited */
15464 /* for solving closest-point problems and for triangle-based */
15465 /* interpolation. */
15466 
15467 /*   The algorithm has expected time complexity O(N*log(N)) */
15468 /* for most nodal distributions. */
15469 
15470 /*   Spherical coordinates (latitude and longitude) may be */
15471 /* converted to Cartesian coordinates by Subroutine TRANS. */
15472 
15473 /*   The following is a list of the software package modules */
15474 /* which a user may wish to call directly: */
15475 
15476 /*  ADDNOD - Updates the triangulation by appending a new */
15477 /*             node. */
15478 
15479 /*  AREAS  - Returns the area of a spherical triangle. */
15480 
15481 /*  AREAV  - Returns the area of a Voronoi region associated */
15482 /*           with an interior node without requiring that the */
15483 /*           entire Voronoi diagram be computed and stored. */
15484 
15485 /*  BNODES - Returns an array containing the indexes of the */
15486 /*             boundary nodes (if any) in counterclockwise */
15487 /*             order.  Counts of boundary nodes, triangles, */
15488 /*             and arcs are also returned. */
15489 
15490 /*  CIRCLE - Computes the coordinates of a sequence of uni- */
15491 /*           formly spaced points on the unit circle centered */
15492 /*           at (0,0). */
15493 
15494 /*  CIRCUM - Returns the circumcenter of a spherical trian- */
15495 /*             gle. */
15496 
15497 /*  CRLIST - Returns the set of triangle circumcenters */
15498 /*             (Voronoi vertices) and circumradii associated */
15499 /*             with a triangulation. */
15500 
15501 /*  DELARC - Deletes a boundary arc from a triangulation. */
15502 
15503 /*  DELNOD - Updates the triangulation with a nodal deletion. */
15504 
15505 /*  EDGE   - Forces an arbitrary pair of nodes to be connec- */
15506 /*             ted by an arc in the triangulation. */
15507 
15508 /*  GETNP  - Determines the ordered sequence of L closest */
15509 /*             nodes to a given node, along with the associ- */
15510 /*             ated distances. */
15511 
15512 /*  INSIDE - Locates a point relative to a polygon on the */
15513 /*             surface of the sphere. */
15514 
15515 /*  INTRSC - Returns the point of intersection between a */
15516 /*             pair of great circle arcs. */
15517 
15518 /*  JRAND  - Generates a uniformly distributed pseudo-random */
15519 /*             int. */
15520 
15521 /*  LEFT   - Locates a point relative to a great circle. */
15522 
15523 /*  NEARND - Returns the index of the nearest node to an */
15524 /*             arbitrary point, along with its squared */
15525 /*             distance. */
15526 
15527 /*  PROJCT - Applies a perspective-depth projection to a */
15528 /*             point in 3-space. */
15529 
15530 /*  SCOORD - Converts a point from Cartesian coordinates to */
15531 /*             spherical coordinates. */
15532 
15533 /*  STORE  - Forces a value to be stored in main memory so */
15534 /*             that the precision of floating point numbers */
15535 /*             in memory locations rather than registers is */
15536 /*             computed. */
15537 
15538 /*  TRANS  - Transforms spherical coordinates into Cartesian */
15539 /*             coordinates on the unit sphere for input to */
15540 /*             Subroutine TRMESH. */
15541 
15542 /*  TRLIST - Converts the triangulation data structure to a */
15543 /*             triangle list more suitable for use in a fin- */
15544 /*             ite element code. */
15545 
15546 /*  TRLPRT - Prints the triangle list created by Subroutine */
15547 /*             TRLIST. */
15548 
15549 /*  TRMESH - Creates a Delaunay triangulation of a set of */
15550 /*             nodes. */
15551 
15552 /*  TRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15553 /*             file containing a triangulation plot. */
15554 
15555 /*  TRPRNT - Prints the triangulation data structure and, */
15556 /*             optionally, the nodal coordinates. */
15557 
15558 /*  VRPLOT - Creates a level-2 Encapsulated Postscript (EPS) */
15559 /*             file containing a Voronoi diagram plot. */
15560 
15561 
15562 /* On input: */
15563 
15564 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15565 
15566 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15567 /*               coordinates of distinct nodes.  (X(K),Y(K), */
15568 /*               Z(K)) is referred to as node K, and K is re- */
15569 /*               ferred to as a nodal index.  It is required */
15570 /*               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all */
15571 /*               K.  The first three nodes must not be col- */
15572 /*               linear (lie on a common great circle). */
15573 
15574 /* The above parameters are not altered by this routine. */
15575 
15576 /*       LIST,LPTR = Arrays of length at least 6N-12. */
15577 
15578 /*       LEND = Array of length at least N. */
15579 
15580 /*       NEAR,NEXT,DIST = Work space arrays of length at */
15581 /*                        least N.  The space is used to */
15582 /*                        efficiently determine the nearest */
15583 /*                        triangulation node to each un- */
15584 /*                        processed node for use by ADDNOD. */
15585 
15586 /* On output: */
15587 
15588 /*       LIST = Set of nodal indexes which, along with LPTR, */
15589 /*              LEND, and LNEW, define the triangulation as a */
15590 /*              set of N adjacency lists -- counterclockwise- */
15591 /*              ordered sequences of neighboring nodes such */
15592 /*              that the first and last neighbors of a bound- */
15593 /*              ary node are boundary nodes (the first neigh- */
15594 /*              bor of an interior node is arbitrary).  In */
15595 /*              order to distinguish between interior and */
15596 /*              boundary nodes, the last neighbor of each */
15597 /*              boundary node is represented by the negative */
15598 /*              of its index. */
15599 
15600 /*       LPTR = Set of pointers (LIST indexes) in one-to-one */
15601 /*              correspondence with the elements of LIST. */
15602 /*              LIST(LPTR(I)) indexes the node which follows */
15603 /*              LIST(I) in cyclical counterclockwise order */
15604 /*              (the first neighbor follows the last neigh- */
15605 /*              bor). */
15606 
15607 /*       LEND = Set of pointers to adjacency lists.  LEND(K) */
15608 /*              points to the last neighbor of node K for */
15609 /*              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and */
15610 /*              only if K is a boundary node. */
15611 
15612 /*       LNEW = Pointer to the first empty location in LIST */
15613 /*              and LPTR (list length plus one).  LIST, LPTR, */
15614 /*              LEND, and LNEW are not altered if IER < 0, */
15615 /*              and are incomplete if IER > 0. */
15616 
15617 /*       NEAR,NEXT,DIST = Garbage. */
15618 
15619 /*       IER = Error indicator: */
15620 /*             IER =  0 if no errors were encountered. */
15621 /*             IER = -1 if N < 3 on input. */
15622 /*             IER = -2 if the first three nodes are */
15623 /*                      collinear. */
15624 /*             IER =  L if nodes L and M coincide for some */
15625 /*                      M > L.  The data structure represents */
15626 /*                      a triangulation of nodes 1 to M-1 in */
15627 /*                      this case. */
15628 
15629 /* Modules required by TRMESH:  ADDNOD, BDYADD, COVSPH, */
15630 /*                                INSERT, INTADD, JRAND, */
15631 /*                                LEFT, LSTPTR, STORE, SWAP, */
15632 /*                                SWPTST, TRFIND */
15633 
15634 /* Intrinsic function called by TRMESH:  ABS */
15635 
15636 /* *********************************************************** */
15637 
15638 
15639 /* Local parameters: */
15640 
15641 /* D =        (Negative cosine of) distance from node K to */
15642 /*              node I */
15643 /* D1,D2,D3 = Distances from node K to nodes 1, 2, and 3, */
15644 /*              respectively */
15645 /* I,J =      Nodal indexes */
15646 /* I0 =       Index of the node preceding I in a sequence of */
15647 /*              unprocessed nodes:  I = NEXT(I0) */
15648 /* K =        Index of node to be added and DO-loop index: */
15649 /*              K > 3 */
15650 /* LP =       LIST index (pointer) of a neighbor of K */
15651 /* LPL =      Pointer to the last neighbor of K */
15652 /* NEXTI =    NEXT(I) */
15653 /* NN =       Local copy of N */
15654 
15655     /* Parameter adjustments */
15656     --dist;
15657     --next;
15658     --near__;
15659     --lend;
15660     --z__;
15661     --y;
15662     --x;
15663     --list;
15664     --lptr;
15665 
15666     /* Function Body */
15667     nn = *n;
15668     if (nn < 3) {
15669         *ier = -1;
15670         return 0;
15671     }
15672 
15673 /* Store the first triangle in the linked list. */
15674 
15675     if (! left_(&x[1], &y[1], &z__[1], &x[2], &y[2], &z__[2], &x[3], &y[3], &
15676             z__[3])) {
15677 
15678 /*   The first triangle is (3,2,1) = (2,1,3) = (1,3,2). */
15679 
15680         list[1] = 3;
15681         lptr[1] = 2;
15682         list[2] = -2;
15683         lptr[2] = 1;
15684         lend[1] = 2;
15685 
15686         list[3] = 1;
15687         lptr[3] = 4;
15688         list[4] = -3;
15689         lptr[4] = 3;
15690         lend[2] = 4;
15691 
15692         list[5] = 2;
15693         lptr[5] = 6;
15694         list[6] = -1;
15695         lptr[6] = 5;
15696         lend[3] = 6;
15697 
15698     } else if (! left_(&x[2], &y[2], &z__[2], &x[1], &y[1], &z__[1], &x[3], &
15699             y[3], &z__[3])) {
15700 
15701 /*   The first triangle is (1,2,3):  3 Strictly Left 1->2, */
15702 /*     i.e., node 3 lies in the left hemisphere defined by */
15703 /*     arc 1->2. */
15704 
15705         list[1] = 2;
15706         lptr[1] = 2;
15707         list[2] = -3;
15708         lptr[2] = 1;
15709         lend[1] = 2;
15710 
15711         list[3] = 3;
15712         lptr[3] = 4;
15713         list[4] = -1;
15714         lptr[4] = 3;
15715         lend[2] = 4;
15716 
15717         list[5] = 1;
15718         lptr[5] = 6;
15719         list[6] = -2;
15720         lptr[6] = 5;
15721         lend[3] = 6;
15722 
15723     } else {
15724 
15725 /*   The first three nodes are collinear. */
15726 
15727         *ier = -2;
15728         return 0;
15729     }
15730 
15731 /* Initialize LNEW and test for N = 3. */
15732 
15733     *lnew = 7;
15734     if (nn == 3) {
15735         *ier = 0;
15736         return 0;
15737     }
15738 
15739 /* A nearest-node data structure (NEAR, NEXT, and DIST) is */
15740 /*   used to obtain an expected-time (N*log(N)) incremental */
15741 /*   algorithm by enabling constant search time for locating */
15742 /*   each new node in the triangulation. */
15743 
15744 /* For each unprocessed node K, NEAR(K) is the index of the */
15745 /*   triangulation node closest to K (used as the starting */
15746 /*   point for the search in Subroutine TRFIND) and DIST(K) */
15747 /*   is an increasing function of the arc length (angular */
15748 /*   distance) between nodes K and NEAR(K):  -Cos(a) for arc */
15749 /*   length a. */
15750 
15751 /* Since it is necessary to efficiently find the subset of */
15752 /*   unprocessed nodes associated with each triangulation */
15753 /*   node J (those that have J as their NEAR entries), the */
15754 /*   subsets are stored in NEAR and NEXT as follows:  for */
15755 /*   each node J in the triangulation, I = NEAR(J) is the */
15756 /*   first unprocessed node in J's set (with I = 0 if the */
15757 /*   set is empty), L = NEXT(I) (if I > 0) is the second, */
15758 /*   NEXT(L) (if L > 0) is the third, etc.  The nodes in each */
15759 /*   set are initially ordered by increasing indexes (which */
15760 /*   maximizes efficiency) but that ordering is not main- */
15761 /*   tained as the data structure is updated. */
15762 
15763 /* Initialize the data structure for the single triangle. */
15764 
15765     near__[1] = 0;
15766     near__[2] = 0;
15767     near__[3] = 0;
15768     for (k = nn; k >= 4; --k) {
15769         d1 = -(x[k] * x[1] + y[k] * y[1] + z__[k] * z__[1]);
15770         d2 = -(x[k] * x[2] + y[k] * y[2] + z__[k] * z__[2]);
15771         d3 = -(x[k] * x[3] + y[k] * y[3] + z__[k] * z__[3]);
15772         if (d1 <= d2 && d1 <= d3) {
15773             near__[k] = 1;
15774             dist[k] = d1;
15775             next[k] = near__[1];
15776             near__[1] = k;
15777         } else if (d2 <= d1 && d2 <= d3) {
15778             near__[k] = 2;
15779             dist[k] = d2;
15780             next[k] = near__[2];
15781             near__[2] = k;
15782         } else {
15783             near__[k] = 3;
15784             dist[k] = d3;
15785             next[k] = near__[3];
15786             near__[3] = k;
15787         }
15788 /* L1: */
15789     }
15790 
15791 /* Add the remaining nodes */
15792 
15793     i__1 = nn;
15794     for (k = 4; k <= i__1; ++k) {
15795         addnod_(&near__[k], &k, &x[1], &y[1], &z__[1], &list[1], &lptr[1], &
15796                 lend[1], lnew, ier);
15797         if (*ier != 0) {
15798             return 0;
15799         }
15800 
15801 /* Remove K from the set of unprocessed nodes associated */
15802 /*   with NEAR(K). */
15803 
15804         i__ = near__[k];
15805         if (near__[i__] == k) {
15806             near__[i__] = next[k];
15807         } else {
15808             i__ = near__[i__];
15809 L2:
15810             i0 = i__;
15811             i__ = next[i0];
15812             if (i__ != k) {
15813                 goto L2;
15814             }
15815             next[i0] = next[k];
15816         }
15817         near__[k] = 0;
15818 
15819 /* Loop on neighbors J of node K. */
15820 
15821         lpl = lend[k];
15822         lp = lpl;
15823 L3:
15824         lp = lptr[lp];
15825         j = (i__2 = list[lp], abs(i__2));
15826 
15827 /* Loop on elements I in the sequence of unprocessed nodes */
15828 /*   associated with J:  K is a candidate for replacing J */
15829 /*   as the nearest triangulation node to I.  The next value */
15830 /*   of I in the sequence, NEXT(I), must be saved before I */
15831 /*   is moved because it is altered by adding I to K's set. */
15832 
15833         i__ = near__[j];
15834 L4:
15835         if (i__ == 0) {
15836             goto L5;
15837         }
15838         nexti = next[i__];
15839 
15840 /* Test for the distance from I to K less than the distance */
15841 /*   from I to J. */
15842 
15843         d__ = -(x[i__] * x[k] + y[i__] * y[k] + z__[i__] * z__[k]);
15844         if (d__ < dist[i__]) {
15845 
15846 /* Replace J by K as the nearest triangulation node to I: */
15847 /*   update NEAR(I) and DIST(I), and remove I from J's set */
15848 /*   of unprocessed nodes and add it to K's set. */
15849 
15850             near__[i__] = k;
15851             dist[i__] = d__;
15852             if (i__ == near__[j]) {
15853                 near__[j] = nexti;
15854             } else {
15855                 next[i0] = nexti;
15856             }
15857             next[i__] = near__[k];
15858             near__[k] = i__;
15859         } else {
15860             i0 = i__;
15861         }
15862 
15863 /* Bottom of loop on I. */
15864 
15865         i__ = nexti;
15866         goto L4;
15867 
15868 /* Bottom of loop on neighbors J. */
15869 
15870 L5:
15871         if (lp != lpl) {
15872             goto L3;
15873         }
15874 /* L6: */
15875     }
15876     return 0;
15877 } /* trmesh_ */
15878 
15879 /* Subroutine */ int trplot_(int *lun, double *pltsiz, double *
15880         elat, double *elon, double *a, int *n, double *x,
15881         double *y, double *z__, int *list, int *lptr, int
15882         *lend, char *, long int *numbr, int *ier, short )
15883 {
15884     /* Initialized data */
15885 
15886     static long int annot = TRUE_;
15887     static double fsizn = 10.;
15888     static double fsizt = 16.;
15889     static double tol = .5;
15890 
15891     /* System generated locals */
15892     int i__1, i__2;
15893     double d__1;
15894 
15895     /* Builtin functions */
15896     //double atan(double), sin(double);
15897     //int i_dnnt(double *);
15898     //double cos(double), sqrt(double);
15899 
15900     /* Local variables */
15901     static double t;
15902     static int n0, n1;
15903     static double p0[3], p1[3], cf, r11, r12, r21, ct, r22, r23, sf;
15904     static int ir, lp;
15905     static double ex, ey, ez, wr, tx, ty;
15906     static int lpl;
15907     static double wrs;
15908     static int ipx1, ipx2, ipy1, ipy2, nseg;
15909     extern /* Subroutine */ int drwarc_(int *, double *, double *,
15910              double *, int *);
15911 
15912 
15913 /* *********************************************************** */
15914 
15915 /*                                              From STRIPACK */
15916 /*                                            Robert J. Renka */
15917 /*                                  Dept. of Computer Science */
15918 /*                                       Univ. of North Texas */
15919 /*                                           renka@cs.unt.edu */
15920 /*                                                   03/04/03 */
15921 
15922 /*   This subroutine creates a level-2 Encapsulated Post- */
15923 /* script (EPS) file containing a graphical display of a */
15924 /* triangulation of a set of nodes on the surface of the unit */
15925 /* sphere.  The visible portion of the triangulation is */
15926 /* projected onto the plane that contains the origin and has */
15927 /* normal defined by a user-specified eye-position. */
15928 
15929 
15930 /* On input: */
15931 
15932 /*       LUN = long int unit number in the range 0 to 99. */
15933 /*             The unit should be opened with an appropriate */
15934 /*             file name before the call to this routine. */
15935 
15936 /*       PLTSIZ = Plot size in inches.  A circular window in */
15937 /*                the projection plane is mapped to a circu- */
15938 /*                lar viewport with diameter equal to .88* */
15939 /*                PLTSIZ (leaving room for labels outside the */
15940 /*                viewport).  The viewport is centered on the */
15941 /*                8.5 by 11 inch page, and its boundary is */
15942 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
15943 
15944 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
15945 /*                   the center of projection E (the center */
15946 /*                   of the plot).  The projection plane is */
15947 /*                   the plane that contains the origin and */
15948 /*                   has E as unit normal.  In a rotated */
15949 /*                   coordinate system for which E is the */
15950 /*                   north pole, the projection plane con- */
15951 /*                   tains the equator, and only northern */
15952 /*                   hemisphere nodes are visible (from the */
15953 /*                   point at infinity in the direction E). */
15954 /*                   These are projected orthogonally onto */
15955 /*                   the projection plane (by zeroing the z- */
15956 /*                   component in the rotated coordinate */
15957 /*                   system).  ELAT and ELON must be in the */
15958 /*                   range -90 to 90 and -180 to 180, respec- */
15959 /*                   tively. */
15960 
15961 /*       A = Angular distance in degrees from E to the boun- */
15962 /*           dary of a circular window against which the */
15963 /*           triangulation is clipped.  The projected window */
15964 /*           is a disk of radius r = Sin(A) centered at the */
15965 /*           origin, and only visible nodes whose projections */
15966 /*           are within distance r of the origin are included */
15967 /*           in the plot.  Thus, if A = 90, the plot includes */
15968 /*           the entire hemisphere centered at E.  0 .LT. A */
15969 /*           .LE. 90. */
15970 
15971 /*       N = Number of nodes in the triangulation.  N .GE. 3. */
15972 
15973 /*       X,Y,Z = Arrays of length N containing the Cartesian */
15974 /*               coordinates of the nodes (unit vectors). */
15975 
15976 /*       LIST,LPTR,LEND = Data structure defining the trian- */
15977 /*                        gulation.  Refer to Subroutine */
15978 /*                        TRMESH. */
15979 
15980 /*       TITLE = Type CHARACTER variable or constant contain- */
15981 /*               ing a string to be centered above the plot. */
15982 /*               The string must be enclosed in parentheses; */
15983 /*               i.e., the first and last characters must be */
15984 /*               '(' and ')', respectively, but these are not */
15985 /*               displayed.  TITLE may have at most 80 char- */
15986 /*               acters including the parentheses. */
15987 
15988 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
15989 /*               nodal indexes are plotted next to the nodes. */
15990 
15991 /* Input parameters are not altered by this routine. */
15992 
15993 /* On output: */
15994 
15995 /*       IER = Error indicator: */
15996 /*             IER = 0 if no errors were encountered. */
15997 /*             IER = 1 if LUN, PLTSIZ, or N is outside its */
15998 /*                     valid range. */
15999 /*             IER = 2 if ELAT, ELON, or A is outside its */
16000 /*                     valid range. */
16001 /*             IER = 3 if an error was encountered in writing */
16002 /*                     to unit LUN. */
16003 
16004 /*   The values in the data statement below may be altered */
16005 /* in order to modify various plotting options. */
16006 
16007 /* Module required by TRPLOT:  DRWARC */
16008 
16009 /* Intrinsic functions called by TRPLOT:  ABS, ATAN, COS, */
16010 /*                                          DBLE, NINT, SIN, */
16011 /*                                          SQRT */
16012 
16013 /* *********************************************************** */
16014 
16015 
16016     /* Parameter adjustments */
16017     --lend;
16018     --z__;
16019     --y;
16020     --x;
16021     --list;
16022     --lptr;
16023 
16024     /* Function Body */
16025 
16026 /* Local parameters: */
16027 
16028 /* ANNOT =     long int variable with value TRUE iff the plot */
16029 /*               is to be annotated with the values of ELAT, */
16030 /*               ELON, and A */
16031 /* CF =        Conversion factor for degrees to radians */
16032 /* CT =        Cos(ELAT) */
16033 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16034 /* FSIZN =     Font size in points for labeling nodes with */
16035 /*               their indexes if NUMBR = TRUE */
16036 /* FSIZT =     Font size in points for the title (and */
16037 /*               annotation if ANNOT = TRUE) */
16038 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16039 /*               left corner of the bounding box or viewport */
16040 /*               box */
16041 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16042 /*               right corner of the bounding box or viewport */
16043 /*               box */
16044 /* IR =        Half the width (height) of the bounding box or */
16045 /*               viewport box in points -- viewport radius */
16046 /* LP =        LIST index (pointer) */
16047 /* LPL =       Pointer to the last neighbor of N0 */
16048 /* N0 =        Index of a node whose incident arcs are to be */
16049 /*               drawn */
16050 /* N1 =        Neighbor of N0 */
16051 /* NSEG =      Number of line segments used by DRWARC in a */
16052 /*               polygonal approximation to a projected edge */
16053 /* P0 =        Coordinates of N0 in the rotated coordinate */
16054 /*               system or label location (first two */
16055 /*               components) */
16056 /* P1 =        Coordinates of N1 in the rotated coordinate */
16057 /*               system or intersection of edge N0-N1 with */
16058 /*               the equator (in the rotated coordinate */
16059 /*               system) */
16060 /* R11...R23 = Components of the first two rows of a rotation */
16061 /*               that maps E to the north pole (0,0,1) */
16062 /* SF =        Scale factor for mapping world coordinates */
16063 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16064 /*               to viewport coordinates in [IPX1,IPX2] X */
16065 /*               [IPY1,IPY2] */
16066 /* T =         Temporary variable */
16067 /* TOL =       Maximum distance in points between a projected */
16068 /*               triangulation edge and its approximation by */
16069 /*               a polygonal curve */
16070 /* TX,TY =     Translation vector for mapping world coordi- */
16071 /*               nates to viewport coordinates */
16072 /* WR =        Window radius r = Sin(A) */
16073 /* WRS =       WR**2 */
16074 
16075 
16076 /* Test for invalid parameters. */
16077 
16078     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3) {
16079         goto L11;
16080     }
16081     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16082         goto L12;
16083     }
16084 
16085 /* Compute a conversion factor CF for degrees to radians */
16086 /*   and compute the window radius WR. */
16087 
16088     cf = atan(1.) / 45.;
16089     wr = sin(cf * *a);
16090     wrs = wr * wr;
16091 
16092 /* Compute the lower left (IPX1,IPY1) and upper right */
16093 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16094 /*   The coordinates, specified in default user space units */
16095 /*   (points, at 72 points/inch with origin at the lower */
16096 /*   left corner of the page), are chosen to preserve the */
16097 /*   square aspect ratio, and to center the plot on the 8.5 */
16098 /*   by 11 inch page.  The center of the page is (306,396), */
16099 /*   and IR = PLTSIZ/2 in points. */
16100 
16101     d__1 = *pltsiz * 36.;
16102     ir = i_dnnt(&d__1);
16103     ipx1 = 306 - ir;
16104     ipx2 = ir + 306;
16105     ipy1 = 396 - ir;
16106     ipy2 = ir + 396;
16107 
16108 /* Output header comments. */
16109 
16110 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16111 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16112 /*     .        '%%BoundingBox:',4I4/ */
16113 /*     .        '%%Title:  Triangulation'/ */
16114 /*     .        '%%Creator:  STRIPACK'/ */
16115 /*     .        '%%EndComments') */
16116 
16117 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16118 /*   of a viewport box obtained by shrinking the bounding box */
16119 /*   by 12% in each dimension. */
16120 
16121     d__1 = (double) ir * .88;
16122     ir = i_dnnt(&d__1);
16123     ipx1 = 306 - ir;
16124     ipx2 = ir + 306;
16125     ipy1 = 396 - ir;
16126     ipy2 = ir + 396;
16127 
16128 /* Set the line thickness to 2 points, and draw the */
16129 /*   viewport boundary. */
16130 
16131     t = 2.;
16132 /*      WRITE (LUN,110,ERR=13) T */
16133 /*      WRITE (LUN,120,ERR=13) IR */
16134 /*      WRITE (LUN,130,ERR=13) */
16135 /*  110 FORMAT (F12.6,' setlinewidth') */
16136 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16137 /*  130 FORMAT ('stroke') */
16138 
16139 /* Set up an affine mapping from the window box [-WR,WR] X */
16140 /*   [-WR,WR] to the viewport box. */
16141 
16142     sf = (double) ir / wr;
16143     tx = ipx1 + sf * wr;
16144     ty = ipy1 + sf * wr;
16145 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16146 /*  140 FORMAT (2F12.6,' translate'/ */
16147 /*    .        2F12.6,' scale') */
16148 
16149 /* The line thickness must be changed to reflect the new */
16150 /*   scaling which is applied to all subsequent output. */
16151 /*   Set it to 1.0 point. */
16152 
16153     t = 1. / sf;
16154 /*      WRITE (LUN,110,ERR=13) T */
16155 
16156 /* Save the current graphics state, and set the clip path to */
16157 /*   the boundary of the window. */
16158 
16159 /*      WRITE (LUN,150,ERR=13) */
16160 /*      WRITE (LUN,160,ERR=13) WR */
16161 /*      WRITE (LUN,170,ERR=13) */
16162 /*  150 FORMAT ('gsave') */
16163 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
16164 /*  170 FORMAT ('clip newpath') */
16165 
16166 /* Compute the Cartesian coordinates of E and the components */
16167 /*   of a rotation R which maps E to the north pole (0,0,1). */
16168 /*   R is taken to be a rotation about the z-axis (into the */
16169 /*   yz-plane) followed by a rotation about the x-axis chosen */
16170 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
16171 /*   E is the north or south pole. */
16172 
16173 /*           ( R11  R12  0   ) */
16174 /*       R = ( R21  R22  R23 ) */
16175 /*           ( EX   EY   EZ  ) */
16176 
16177     t = cf * *elon;
16178     ct = cos(cf * *elat);
16179     ex = ct * cos(t);
16180     ey = ct * sin(t);
16181     ez = sin(cf * *elat);
16182     if (ct != 0.) {
16183         r11 = -ey / ct;
16184         r12 = ex / ct;
16185     } else {
16186         r11 = 0.;
16187         r12 = 1.;
16188     }
16189     r21 = -ez * r12;
16190     r22 = ez * r11;
16191     r23 = ct;
16192 
16193 /* Loop on visible nodes N0 that project to points */
16194 /*   (P0(1),P0(2)) in the window. */
16195 
16196     i__1 = *n;
16197     for (n0 = 1; n0 <= i__1; ++n0) {
16198         p0[2] = ex * x[n0] + ey * y[n0] + ez * z__[n0];
16199         if (p0[2] < 0.) {
16200             goto L3;
16201         }
16202         p0[0] = r11 * x[n0] + r12 * y[n0];
16203         p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16204         if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16205             goto L3;
16206         }
16207         lpl = lend[n0];
16208         lp = lpl;
16209 
16210 /* Loop on neighbors N1 of N0.  LPL points to the last */
16211 /*   neighbor of N0.  Copy the components of N1 into P. */
16212 
16213 L1:
16214         lp = lptr[lp];
16215         n1 = (i__2 = list[lp], abs(i__2));
16216         p1[0] = r11 * x[n1] + r12 * y[n1];
16217         p1[1] = r21 * x[n1] + r22 * y[n1] + r23 * z__[n1];
16218         p1[2] = ex * x[n1] + ey * y[n1] + ez * z__[n1];
16219         if (p1[2] < 0.) {
16220 
16221 /*   N1 is a 'southern hemisphere' point.  Move it to the */
16222 /*     intersection of edge N0-N1 with the equator so that */
16223 /*     the edge is clipped properly.  P1(3) is set to 0. */
16224 
16225             p1[0] = p0[2] * p1[0] - p1[2] * p0[0];
16226             p1[1] = p0[2] * p1[1] - p1[2] * p0[1];
16227             t = sqrt(p1[0] * p1[0] + p1[1] * p1[1]);
16228             p1[0] /= t;
16229             p1[1] /= t;
16230         }
16231 
16232 /*   If node N1 is in the window and N1 < N0, bypass edge */
16233 /*     N0->N1 (since edge N1->N0 has already been drawn). */
16234 
16235         if (p1[2] >= 0. && p1[0] * p1[0] + p1[1] * p1[1] <= wrs && n1 < n0) {
16236             goto L2;
16237         }
16238 
16239 /*   Add the edge to the path.  (TOL is converted to world */
16240 /*     coordinates.) */
16241 
16242         if (p1[2] < 0.) {
16243             p1[2] = 0.;
16244         }
16245         d__1 = tol / sf;
16246         drwarc_(lun, p0, p1, &d__1, &nseg);
16247 
16248 /* Bottom of loops. */
16249 
16250 L2:
16251         if (lp != lpl) {
16252             goto L1;
16253         }
16254 L3:
16255         ;
16256     }
16257 
16258 /* Paint the path and restore the saved graphics state (with */
16259 /*   no clip path). */
16260 
16261 /*      WRITE (LUN,130,ERR=13) */
16262 /*      WRITE (LUN,190,ERR=13) */
16263 /*  190 FORMAT ('grestore') */
16264     if (*numbr) {
16265 
16266 /* Nodes in the window are to be labeled with their indexes. */
16267 /*   Convert FSIZN from points to world coordinates, and */
16268 /*   output the commands to select a font and scale it. */
16269 
16270         t = fsizn / sf;
16271 /*        WRITE (LUN,200,ERR=13) T */
16272 /*  200   FORMAT ('/Helvetica findfont'/ */
16273 /*     .          F12.6,' scalefont setfont') */
16274 
16275 /* Loop on visible nodes N0 that project to points */
16276 /*   P0 = (P0(1),P0(2)) in the window. */
16277 
16278         i__1 = *n;
16279         for (n0 = 1; n0 <= i__1; ++n0) {
16280             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
16281                 goto L4;
16282             }
16283             p0[0] = r11 * x[n0] + r12 * y[n0];
16284             p0[1] = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
16285             if (p0[0] * p0[0] + p0[1] * p0[1] > wrs) {
16286                 goto L4;
16287             }
16288 
16289 /*   Move to P0 and draw the label N0.  The first character */
16290 /*     will will have its lower left corner about one */
16291 /*     character width to the right of the nodal position. */
16292 
16293 /*          WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16294 /*          WRITE (LUN,220,ERR=13) N0 */
16295 /*  210     FORMAT (2F12.6,' moveto') */
16296 /*  220     FORMAT ('(',I3,') show') */
16297 L4:
16298             ;
16299         }
16300     }
16301 
16302 /* Convert FSIZT from points to world coordinates, and output */
16303 /*   the commands to select a font and scale it. */
16304 
16305     t = fsizt / sf;
16306 /*      WRITE (LUN,200,ERR=13) T */
16307 
16308 /* Display TITLE centered above the plot: */
16309 
16310     p0[1] = wr + t * 3.;
16311 /*      WRITE (LUN,230,ERR=13) TITLE, P0(2) */
16312 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
16313 /*     .        ' moveto') */
16314 /*      WRITE (LUN,240,ERR=13) TITLE */
16315 /*  240 FORMAT (A80/'  show') */
16316     if (annot) {
16317 
16318 /* Display the window center and radius below the plot. */
16319 
16320         p0[0] = -wr;
16321         p0[1] = -wr - 50. / sf;
16322 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16323 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
16324         p0[1] -= t * 2.;
16325 /*        WRITE (LUN,210,ERR=13) P0(1), P0(2) */
16326 /*        WRITE (LUN,260,ERR=13) A */
16327 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
16328 /*     .          ',  ELON = ',F8.2,') show') */
16329 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
16330     }
16331 
16332 /* Paint the path and output the showpage command and */
16333 /*   end-of-file indicator. */
16334 
16335 /*      WRITE (LUN,270,ERR=13) */
16336 /*  270 FORMAT ('stroke'/ */
16337 /*     .        'showpage'/ */
16338 /*     .        '%%EOF') */
16339 
16340 /* HP's interpreters require a one-byte End-of-PostScript-Job */
16341 /*   indicator (to eliminate a timeout error message): */
16342 /*   ASCII 4. */
16343 
16344 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
16345 /*  280 FORMAT (A1) */
16346 
16347 /* No error encountered. */
16348 
16349     *ier = 0;
16350     return 0;
16351 
16352 /* Invalid input parameter LUN, PLTSIZ, or N. */
16353 
16354 L11:
16355     *ier = 1;
16356     return 0;
16357 
16358 /* Invalid input parameter ELAT, ELON, or A. */
16359 
16360 L12:
16361     *ier = 2;
16362     return 0;
16363 
16364 /* Error writing to unit LUN. */
16365 
16366 /* L13: */
16367     *ier = 3;
16368     return 0;
16369 } /* trplot_ */
16370 
16371 /* Subroutine */ int trprnt_(int *n, double *x, double *y,
16372         double *z__, int *iflag, int *list, int *lptr,
16373         int *lend, int *lout)
16374 {
16375     /* Initialized data */
16376 
16377     static int nmax = 9999;
16378     static int nlmax = 58;
16379 
16380     /* System generated locals */
16381     int i__1;
16382 
16383     /* Local variables */
16384     static int k, na, nb, nd, nl, lp, nn, nt, inc, lpl, lun, node, nabor[
16385             400];
16386 
16387 
16388 /* *********************************************************** */
16389 
16390 /*                                              From STRIPACK */
16391 /*                                            Robert J. Renka */
16392 /*                                  Dept. of Computer Science */
16393 /*                                       Univ. of North Texas */
16394 /*                                           renka@cs.unt.edu */
16395 /*                                                   07/25/98 */
16396 
16397 /*   This subroutine prints the triangulation adjacency lists */
16398 /* created by Subroutine TRMESH and, optionally, the nodal */
16399 /* coordinates (either latitude and longitude or Cartesian */
16400 /* coordinates) on long int unit LOUT.  The list of neighbors */
16401 /* of a boundary node is followed by index 0.  The numbers of */
16402 /* boundary nodes, triangles, and arcs are also printed. */
16403 
16404 
16405 /* On input: */
16406 
16407 /*       N = Number of nodes in the triangulation.  N .GE. 3 */
16408 /*           and N .LE. 9999. */
16409 
16410 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16411 /*               coordinates of the nodes if IFLAG = 0, or */
16412 /*               (X and Y only) arrays of length N containing */
16413 /*               longitude and latitude, respectively, if */
16414 /*               IFLAG > 0, or unused dummy parameters if */
16415 /*               IFLAG < 0. */
16416 
16417 /*       IFLAG = Nodal coordinate option indicator: */
16418 /*               IFLAG = 0 if X, Y, and Z (assumed to contain */
16419 /*                         Cartesian coordinates) are to be */
16420 /*                         printed (to 6 decimal places). */
16421 /*               IFLAG > 0 if only X and Y (assumed to con- */
16422 /*                         tain longitude and latitude) are */
16423 /*                         to be printed (to 6 decimal */
16424 /*                         places). */
16425 /*               IFLAG < 0 if only the adjacency lists are to */
16426 /*                         be printed. */
16427 
16428 /*       LIST,LPTR,LEND = Data structure defining the trian- */
16429 /*                        gulation.  Refer to Subroutine */
16430 /*                        TRMESH. */
16431 
16432 /*       LOUT = long int unit for output.  If LOUT is not in */
16433 /*              the range 0 to 99, output is written to */
16434 /*              long int unit 6. */
16435 
16436 /* Input parameters are not altered by this routine. */
16437 
16438 /* On output: */
16439 
16440 /*   The adjacency lists and nodal coordinates (as specified */
16441 /* by IFLAG) are written to unit LOUT. */
16442 
16443 /* Modules required by TRPRNT:  None */
16444 
16445 /* *********************************************************** */
16446 
16447     /* Parameter adjustments */
16448     --lend;
16449     --z__;
16450     --y;
16451     --x;
16452     --list;
16453     --lptr;
16454 
16455     /* Function Body */
16456 
16457 /* Local parameters: */
16458 
16459 /* I =     NABOR index (1 to K) */
16460 /* INC =   Increment for NL associated with an adjacency list */
16461 /* K =     Counter and number of neighbors of NODE */
16462 /* LP =    LIST pointer of a neighbor of NODE */
16463 /* LPL =   Pointer to the last neighbor of NODE */
16464 /* LUN =   long int unit for output (copy of LOUT) */
16465 /* NA =    Number of arcs in the triangulation */
16466 /* NABOR = Array containing the adjacency list associated */
16467 /*           with NODE, with zero appended if NODE is a */
16468 /*           boundary node */
16469 /* NB =    Number of boundary nodes encountered */
16470 /* ND =    Index of a neighbor of NODE (or negative index) */
16471 /* NL =    Number of lines that have been printed on the */
16472 /*           current page */
16473 /* NLMAX = Maximum number of print lines per page (except */
16474 /*           for the last page which may have two addi- */
16475 /*           tional lines) */
16476 /* NMAX =  Upper bound on N (allows 4-digit indexes) */
16477 /* NODE =  Index of a node and DO-loop index (1 to N) */
16478 /* NN =    Local copy of N */
16479 /* NT =    Number of triangles in the triangulation */
16480 
16481     nn = *n;
16482     lun = *lout;
16483     if (lun < 0 || lun > 99) {
16484         lun = 6;
16485     }
16486 
16487 /* Print a heading and test the range of N. */
16488 
16489 /*      WRITE (LUN,100) NN */
16490     if (nn < 3 || nn > nmax) {
16491 
16492 /* N is outside its valid range. */
16493 
16494 /*        WRITE (LUN,110) */
16495         return 0;
16496     }
16497 
16498 /* Initialize NL (the number of lines printed on the current */
16499 /*   page) and NB (the number of boundary nodes encountered). */
16500 
16501     nl = 6;
16502     nb = 0;
16503     if (*iflag < 0) {
16504 
16505 /* Print LIST only.  K is the number of neighbors of NODE */
16506 /*   that have been stored in NABOR. */
16507 
16508 /*        WRITE (LUN,101) */
16509         i__1 = nn;
16510         for (node = 1; node <= i__1; ++node) {
16511             lpl = lend[node];
16512             lp = lpl;
16513             k = 0;
16514 
16515 L1:
16516             ++k;
16517             lp = lptr[lp];
16518             nd = list[lp];
16519             nabor[k - 1] = nd;
16520             if (lp != lpl) {
16521                 goto L1;
16522             }
16523             if (nd <= 0) {
16524 
16525 /*   NODE is a boundary node.  Correct the sign of the last */
16526 /*     neighbor, add 0 to the end of the list, and increment */
16527 /*     NB. */
16528 
16529                 nabor[k - 1] = -nd;
16530                 ++k;
16531                 nabor[k - 1] = 0;
16532                 ++nb;
16533             }
16534 
16535 /*   Increment NL and print the list of neighbors. */
16536 
16537             inc = (k - 1) / 14 + 2;
16538             nl += inc;
16539             if (nl > nlmax) {
16540 /*            WRITE (LUN,108) */
16541                 nl = inc;
16542             }
16543 /*          WRITE (LUN,104) NODE, (NABOR(I), I = 1,K) */
16544 /*          IF (K .NE. 14) */
16545 /*           WRITE (LUN,107) */
16546 /* L2: */
16547         }
16548     } else if (*iflag > 0) {
16549 
16550 /* Print X (longitude), Y (latitude), and LIST. */
16551 
16552 /*        WRITE (LUN,102) */
16553         i__1 = nn;
16554         for (node = 1; node <= i__1; ++node) {
16555             lpl = lend[node];
16556             lp = lpl;
16557             k = 0;
16558 
16559 L3:
16560             ++k;
16561             lp = lptr[lp];
16562             nd = list[lp];
16563             nabor[k - 1] = nd;
16564             if (lp != lpl) {
16565                 goto L3;
16566             }
16567             if (nd <= 0) {
16568 
16569 /*   NODE is a boundary node. */
16570 
16571                 nabor[k - 1] = -nd;
16572                 ++k;
16573                 nabor[k - 1] = 0;
16574                 ++nb;
16575             }
16576 
16577 /*   Increment NL and print X, Y, and NABOR. */
16578 
16579             inc = (k - 1) / 8 + 2;
16580             nl += inc;
16581             if (nl > nlmax) {
16582 /*            WRITE (LUN,108) */
16583                 nl = inc;
16584             }
16585 /*          WRITE (LUN,105) NODE, X(NODE), Y(NODE), (NABOR(I), I = 1,K) */
16586 /*          IF (K .NE. 8) */
16587 /*           PRINT *,K */
16588 /*           WRITE (LUN,107) */
16589 /* L4: */
16590         }
16591     } else {
16592 
16593 /* Print X, Y, Z, and LIST. */
16594 
16595 /*        WRITE (LUN,103) */
16596         i__1 = nn;
16597         for (node = 1; node <= i__1; ++node) {
16598             lpl = lend[node];
16599             lp = lpl;
16600             k = 0;
16601 
16602 L5:
16603             ++k;
16604             lp = lptr[lp];
16605             nd = list[lp];
16606             nabor[k - 1] = nd;
16607             if (lp != lpl) {
16608                 goto L5;
16609             }
16610             if (nd <= 0) {
16611 
16612 /*   NODE is a boundary node. */
16613 
16614                 nabor[k - 1] = -nd;
16615                 ++k;
16616                 nabor[k - 1] = 0;
16617                 ++nb;
16618             }
16619 
16620 /*   Increment NL and print X, Y, Z, and NABOR. */
16621 
16622             inc = (k - 1) / 5 + 2;
16623             nl += inc;
16624             if (nl > nlmax) {
16625 /*            WRITE (LUN,108) */
16626                 nl = inc;
16627             }
16628 /*          WRITE (LUN,106) NODE, X(NODE), Y(NODE),Z(NODE), (NABOR(I), I = 1,K) */
16629 /*          IF (K .NE. 5) */
16630 /*           print *,K */
16631 /*           WRITE (LUN,107) */
16632 /* L6: */
16633         }
16634     }
16635 
16636 /* Print NB, NA, and NT (boundary nodes, arcs, and */
16637 /*   triangles). */
16638 
16639     if (nb != 0) {
16640         na = nn * 3 - nb - 3;
16641         nt = (nn << 1) - nb - 2;
16642     } else {
16643         na = nn * 3 - 6;
16644         nt = (nn << 1) - 4;
16645     }
16646 /*      WRITE (LUN,109) NB, NA, NT */
16647     return 0;
16648 
16649 /* Print formats: */
16650 
16651 /*  100 FORMAT (///15X,'STRIPACK Triangulation Data ', */
16652 /*     .        'Structure,  N = ',I5//) */
16653 /*  101 FORMAT (1X,'Node',31X,'Neighbors of Node'//) */
16654 /*  102 FORMAT (1X,'Node',5X,'Longitude',6X,'Latitude', */
16655 /*     .        18X,'Neighbors of Node'//) */
16656 /*  103 FORMAT (1X,'Node',5X,'X(Node)',8X,'Y(Node)',8X, */
16657 /*     .        'Z(Node)',11X,'Neighbors of Node'//) */
16658 /*  104 FORMAT (1X,I4,4X,14I5/(1X,8X,14I5)) */
16659 /*  105 FORMAT (1X,I4,2D15.6,4X,8I5/(1X,38X,8I5)) */
16660 /*  106 FORMAT (1X,I4,3D15.6,4X,5I5/(1X,53X,5I5)) */
16661 /*  107 FORMAT (1X) */
16662 /*  108 FORMAT (///) */
16663 /*  109 FORMAT (/1X,'NB = ',I4,' Boundary Nodes',5X, */
16664 /*     .        'NA = ',I5,' Arcs',5X,'NT = ',I5, */
16665 /*     .        ' Triangles') */
16666 /*  110 FORMAT (1X,10X,'*** N is outside its valid', */
16667 /*     .        ' range ***') */
16668 } /* trprnt_ */
16669 
16670 /* Subroutine */ int vrplot_(int *lun, double *pltsiz, double *
16671         elat, double *elon, double *a, int *n, double *x,
16672         double *y, double *z__, int *nt, int *listc, int *
16673         lptr, int *lend, double *xc, double *yc, double *zc,
16674         char *, long int *numbr, int *ier, short)
16675 {
16676     /* Initialized data */
16677 
16678     static long int annot = TRUE_;
16679     static double fsizn = 10.;
16680     static double fsizt = 16.;
16681     static double tol = .5;
16682 
16683     /* System generated locals */
16684     int i__1;
16685     double d__1;
16686 
16687     /* Builtin functions */
16688     //double atan(double), sin(double);
16689     //int i_dnnt(double *);
16690     //double cos(double), sqrt(double);
16691 
16692     /* Local variables */
16693     static double t;
16694     static int n0;
16695     static double p1[3], p2[3], x0, y0, cf, r11, r12, r21, ct, r22, r23,
16696             sf;
16697     static int ir, lp;
16698     static double ex, ey, ez, wr, tx, ty;
16699     static long int in1, in2;
16700     static int kv1, kv2, lpl;
16701     static double wrs;
16702     static int ipx1, ipx2, ipy1, ipy2, nseg;
16703     extern /* Subroutine */ int drwarc_(int *, double *, double *,
16704              double *, int *);
16705 
16706 
16707 /* *********************************************************** */
16708 
16709 /*                                              From STRIPACK */
16710 /*                                            Robert J. Renka */
16711 /*                                  Dept. of Computer Science */
16712 /*                                       Univ. of North Texas */
16713 /*                                           renka@cs.unt.edu */
16714 /*                                                   03/04/03 */
16715 
16716 /*   This subroutine creates a level-2 Encapsulated Post- */
16717 /* script (EPS) file containing a graphical depiction of a */
16718 /* Voronoi diagram of a set of nodes on the unit sphere. */
16719 /* The visible portion of the diagram is projected orthog- */
16720 /* onally onto the plane that contains the origin and has */
16721 /* normal defined by a user-specified eye-position. */
16722 
16723 /*   The parameters defining the Voronoi diagram may be com- */
16724 /* puted by Subroutine CRLIST. */
16725 
16726 
16727 /* On input: */
16728 
16729 /*       LUN = long int unit number in the range 0 to 99. */
16730 /*             The unit should be opened with an appropriate */
16731 /*             file name before the call to this routine. */
16732 
16733 /*       PLTSIZ = Plot size in inches.  A circular window in */
16734 /*                the projection plane is mapped to a circu- */
16735 /*                lar viewport with diameter equal to .88* */
16736 /*                PLTSIZ (leaving room for labels outside the */
16737 /*                viewport).  The viewport is centered on the */
16738 /*                8.5 by 11 inch page, and its boundary is */
16739 /*                drawn.  1.0 .LE. PLTSIZ .LE. 8.5. */
16740 
16741 /*       ELAT,ELON = Latitude and longitude (in degrees) of */
16742 /*                   the center of projection E (the center */
16743 /*                   of the plot).  The projection plane is */
16744 /*                   the plane that contains the origin and */
16745 /*                   has E as unit normal.  In a rotated */
16746 /*                   coordinate system for which E is the */
16747 /*                   north pole, the projection plane con- */
16748 /*                   tains the equator, and only northern */
16749 /*                   hemisphere points are visible (from the */
16750 /*                   point at infinity in the direction E). */
16751 /*                   These are projected orthogonally onto */
16752 /*                   the projection plane (by zeroing the z- */
16753 /*                   component in the rotated coordinate */
16754 /*                   system).  ELAT and ELON must be in the */
16755 /*                   range -90 to 90 and -180 to 180, respec- */
16756 /*                   tively. */
16757 
16758 /*       A = Angular distance in degrees from E to the boun- */
16759 /*           dary of a circular window against which the */
16760 /*           Voronoi diagram is clipped.  The projected win- */
16761 /*           dow is a disk of radius r = Sin(A) centered at */
16762 /*           the origin, and only visible vertices whose */
16763 /*           projections are within distance r of the origin */
16764 /*           are included in the plot.  Thus, if A = 90, the */
16765 /*           plot includes the entire hemisphere centered at */
16766 /*           E.  0 .LT. A .LE. 90. */
16767 
16768 /*       N = Number of nodes (Voronoi centers) and Voronoi */
16769 /*           regions.  N .GE. 3. */
16770 
16771 /*       X,Y,Z = Arrays of length N containing the Cartesian */
16772 /*               coordinates of the nodes (unit vectors). */
16773 
16774 /*       NT = Number of Voronoi region vertices (triangles, */
16775 /*            including those in the extended triangulation */
16776 /*            if the number of boundary nodes NB is nonzero): */
16777 /*            NT = 2*N-4. */
16778 
16779 /*       LISTC = Array of length 3*NT containing triangle */
16780 /*               indexes (indexes to XC, YC, and ZC) stored */
16781 /*               in 1-1 correspondence with LIST/LPTR entries */
16782 /*               (or entries that would be stored in LIST for */
16783 /*               the extended triangulation):  the index of */
16784 /*               triangle (N1,N2,N3) is stored in LISTC(K), */
16785 /*               LISTC(L), and LISTC(M), where LIST(K), */
16786 /*               LIST(L), and LIST(M) are the indexes of N2 */
16787 /*               as a neighbor of N1, N3 as a neighbor of N2, */
16788 /*               and N1 as a neighbor of N3.  The Voronoi */
16789 /*               region associated with a node is defined by */
16790 /*               the CCW-ordered sequence of circumcenters in */
16791 /*               one-to-one correspondence with its adjacency */
16792 /*               list (in the extended triangulation). */
16793 
16794 /*       LPTR = Array of length 3*NT = 6*N-12 containing a */
16795 /*              set of pointers (LISTC indexes) in one-to-one */
16796 /*              correspondence with the elements of LISTC. */
16797 /*              LISTC(LPTR(I)) indexes the triangle which */
16798 /*              follows LISTC(I) in cyclical counterclockwise */
16799 /*              order (the first neighbor follows the last */
16800 /*              neighbor). */
16801 
16802 /*       LEND = Array of length N containing a set of */
16803 /*              pointers to triangle lists.  LP = LEND(K) */
16804 /*              points to a triangle (indexed by LISTC(LP)) */
16805 /*              containing node K for K = 1 to N. */
16806 
16807 /*       XC,YC,ZC = Arrays of length NT containing the */
16808 /*                  Cartesian coordinates of the triangle */
16809 /*                  circumcenters (Voronoi vertices). */
16810 /*                  XC(I)**2 + YC(I)**2 + ZC(I)**2 = 1. */
16811 
16812 /*       TITLE = Type CHARACTER variable or constant contain- */
16813 /*               ing a string to be centered above the plot. */
16814 /*               The string must be enclosed in parentheses; */
16815 /*               i.e., the first and last characters must be */
16816 /*               '(' and ')', respectively, but these are not */
16817 /*               displayed.  TITLE may have at most 80 char- */
16818 /*               acters including the parentheses. */
16819 
16820 /*       NUMBR = Option indicator:  If NUMBR = TRUE, the */
16821 /*               nodal indexes are plotted at the Voronoi */
16822 /*               region centers. */
16823 
16824 /* Input parameters are not altered by this routine. */
16825 
16826 /* On output: */
16827 
16828 /*       IER = Error indicator: */
16829 /*             IER = 0 if no errors were encountered. */
16830 /*             IER = 1 if LUN, PLTSIZ, N, or NT is outside */
16831 /*                     its valid range. */
16832 /*             IER = 2 if ELAT, ELON, or A is outside its */
16833 /*                     valid range. */
16834 /*             IER = 3 if an error was encountered in writing */
16835 /*                     to unit LUN. */
16836 
16837 /* Module required by VRPLOT:  DRWARC */
16838 
16839 /* Intrinsic functions called by VRPLOT:  ABS, ATAN, COS, */
16840 /*                                          DBLE, NINT, SIN, */
16841 /*                                          SQRT */
16842 
16843 /* *********************************************************** */
16844 
16845 
16846     /* Parameter adjustments */
16847     --lend;
16848     --z__;
16849     --y;
16850     --x;
16851     --zc;
16852     --yc;
16853     --xc;
16854     --listc;
16855     --lptr;
16856 
16857     /* Function Body */
16858 
16859 /* Local parameters: */
16860 
16861 /* ANNOT =     long int variable with value TRUE iff the plot */
16862 /*               is to be annotated with the values of ELAT, */
16863 /*               ELON, and A */
16864 /* CF =        Conversion factor for degrees to radians */
16865 /* CT =        Cos(ELAT) */
16866 /* EX,EY,EZ =  Cartesian coordinates of the eye-position E */
16867 /* FSIZN =     Font size in points for labeling nodes with */
16868 /*               their indexes if NUMBR = TRUE */
16869 /* FSIZT =     Font size in points for the title (and */
16870 /*               annotation if ANNOT = TRUE) */
16871 /* IN1,IN2 =   long int variables with value TRUE iff the */
16872 /*               projections of vertices KV1 and KV2, respec- */
16873 /*               tively, are inside the window */
16874 /* IPX1,IPY1 = X and y coordinates (in points) of the lower */
16875 /*               left corner of the bounding box or viewport */
16876 /*               box */
16877 /* IPX2,IPY2 = X and y coordinates (in points) of the upper */
16878 /*               right corner of the bounding box or viewport */
16879 /*               box */
16880 /* IR =        Half the width (height) of the bounding box or */
16881 /*               viewport box in points -- viewport radius */
16882 /* KV1,KV2 =   Endpoint indexes of a Voronoi edge */
16883 /* LP =        LIST index (pointer) */
16884 /* LPL =       Pointer to the last neighbor of N0 */
16885 /* N0 =        Index of a node */
16886 /* NSEG =      Number of line segments used by DRWARC in a */
16887 /*               polygonal approximation to a projected edge */
16888 /* P1 =        Coordinates of vertex KV1 in the rotated */
16889 /*               coordinate system */
16890 /* P2 =        Coordinates of vertex KV2 in the rotated */
16891 /*               coordinate system or intersection of edge */
16892 /*               KV1-KV2 with the equator (in the rotated */
16893 /*               coordinate system) */
16894 /* R11...R23 = Components of the first two rows of a rotation */
16895 /*               that maps E to the north pole (0,0,1) */
16896 /* SF =        Scale factor for mapping world coordinates */
16897 /*               (window coordinates in [-WR,WR] X [-WR,WR]) */
16898 /*               to viewport coordinates in [IPX1,IPX2] X */
16899 /*               [IPY1,IPY2] */
16900 /* T =         Temporary variable */
16901 /* TOL =       Maximum distance in points between a projected */
16902 /*               Voronoi edge and its approximation by a */
16903 /*               polygonal curve */
16904 /* TX,TY =     Translation vector for mapping world coordi- */
16905 /*               nates to viewport coordinates */
16906 /* WR =        Window radius r = Sin(A) */
16907 /* WRS =       WR**2 */
16908 /* X0,Y0 =     Projection plane coordinates of node N0 or */
16909 /*               label location */
16910 
16911 
16912 /* Test for invalid parameters. */
16913 
16914     if (*lun < 0 || *lun > 99 || *pltsiz < 1. || *pltsiz > 8.5 || *n < 3 || *
16915             nt != 2 * *n - 4) {
16916         goto L11;
16917     }
16918     if (abs(*elat) > 90. || abs(*elon) > 180. || *a > 90.) {
16919         goto L12;
16920     }
16921 
16922 /* Compute a conversion factor CF for degrees to radians */
16923 /*   and compute the window radius WR. */
16924 
16925     cf = atan(1.) / 45.;
16926     wr = sin(cf * *a);
16927     wrs = wr * wr;
16928 
16929 /* Compute the lower left (IPX1,IPY1) and upper right */
16930 /*   (IPX2,IPY2) corner coordinates of the bounding box. */
16931 /*   The coordinates, specified in default user space units */
16932 /*   (points, at 72 points/inch with origin at the lower */
16933 /*   left corner of the page), are chosen to preserve the */
16934 /*   square aspect ratio, and to center the plot on the 8.5 */
16935 /*   by 11 inch page.  The center of the page is (306,396), */
16936 /*   and IR = PLTSIZ/2 in points. */
16937 
16938     d__1 = *pltsiz * 36.;
16939     ir = i_dnnt(&d__1);
16940     ipx1 = 306 - ir;
16941     ipx2 = ir + 306;
16942     ipy1 = 396 - ir;
16943     ipy2 = ir + 396;
16944 
16945 /* Output header comments. */
16946 
16947 /*      WRITE (LUN,100,ERR=13) IPX1, IPY1, IPX2, IPY2 */
16948 /*  100 FORMAT ('%!PS-Adobe-3.0 EPSF-3.0'/ */
16949 /*     .        '%%BoundingBox:',4I4/ */
16950 /*     .        '%%Title:  Voronoi diagram'/ */
16951 /*     .        '%%Creator:  STRIPACK'/ */
16952 /*     .        '%%EndComments') */
16953 /* Set (IPX1,IPY1) and (IPX2,IPY2) to the corner coordinates */
16954 /*   of a viewport box obtained by shrinking the bounding box */
16955 /*   by 12% in each dimension. */
16956 
16957     d__1 = (double) ir * .88;
16958     ir = i_dnnt(&d__1);
16959     ipx1 = 306 - ir;
16960     ipx2 = ir + 306;
16961     ipy1 = 396 - ir;
16962     ipy2 = ir + 396;
16963 
16964 /* Set the line thickness to 2 points, and draw the */
16965 /*   viewport boundary. */
16966 
16967     t = 2.;
16968 /*      WRITE (LUN,110,ERR=13) T */
16969 /*      WRITE (LUN,120,ERR=13) IR */
16970 /*      WRITE (LUN,130,ERR=13) */
16971 /*  110 FORMAT (F12.6,' setlinewidth') */
16972 /*  120 FORMAT ('306 396 ',I3,' 0 360 arc') */
16973 /*  130 FORMAT ('stroke') */
16974 
16975 /* Set up an affine mapping from the window box [-WR,WR] X */
16976 /*   [-WR,WR] to the viewport box. */
16977 
16978     sf = (double) ir / wr;
16979     tx = ipx1 + sf * wr;
16980     ty = ipy1 + sf * wr;
16981 /*      WRITE (LUN,140,ERR=13) TX, TY, SF, SF */
16982 /*  140 FORMAT (2F12.6,' translate'/ */
16983 /*     .        2F12.6,' scale') */
16984 
16985 /* The line thickness must be changed to reflect the new */
16986 /*   scaling which is applied to all subsequent output. */
16987 /*   Set it to 1.0 point. */
16988 
16989     t = 1. / sf;
16990 /*      WRITE (LUN,110,ERR=13) T */
16991 
16992 /* Save the current graphics state, and set the clip path to */
16993 /*   the boundary of the window. */
16994 
16995 /*      WRITE (LUN,150,ERR=13) */
16996 /*      WRITE (LUN,160,ERR=13) WR */
16997 /*      WRITE (LUN,170,ERR=13) */
16998 /*  150 FORMAT ('gsave') */
16999 /*  160 FORMAT ('0 0 ',F12.6,' 0 360 arc') */
17000 /*  170 FORMAT ('clip newpath') */
17001 
17002 /* Compute the Cartesian coordinates of E and the components */
17003 /*   of a rotation R which maps E to the north pole (0,0,1). */
17004 /*   R is taken to be a rotation about the z-axis (into the */
17005 /*   yz-plane) followed by a rotation about the x-axis chosen */
17006 /*   so that the view-up direction is (0,0,1), or (-1,0,0) if */
17007 /*   E is the north or south pole. */
17008 
17009 /*           ( R11  R12  0   ) */
17010 /*       R = ( R21  R22  R23 ) */
17011 /*           ( EX   EY   EZ  ) */
17012 
17013     t = cf * *elon;
17014     ct = cos(cf * *elat);
17015     ex = ct * cos(t);
17016     ey = ct * sin(t);
17017     ez = sin(cf * *elat);
17018     if (ct != 0.) {
17019         r11 = -ey / ct;
17020         r12 = ex / ct;
17021     } else {
17022         r11 = 0.;
17023         r12 = 1.;
17024     }
17025     r21 = -ez * r12;
17026     r22 = ez * r11;
17027     r23 = ct;
17028 
17029 /* Loop on nodes (Voronoi centers) N0. */
17030 /*   LPL indexes the last neighbor of N0. */
17031 
17032     i__1 = *n;
17033     for (n0 = 1; n0 <= i__1; ++n0) {
17034         lpl = lend[n0];
17035 
17036 /* Set KV2 to the first (and last) vertex index and compute */
17037 /*   its coordinates P2 in the rotated coordinate system. */
17038 
17039         kv2 = listc[lpl];
17040         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17041         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17042         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17043 
17044 /*   IN2 = TRUE iff KV2 is in the window. */
17045 
17046         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17047 
17048 /* Loop on neighbors N1 of N0.  For each triangulation edge */
17049 /*   N0-N1, KV1-KV2 is the corresponding Voronoi edge. */
17050 
17051         lp = lpl;
17052 L1:
17053         lp = lptr[lp];
17054         kv1 = kv2;
17055         p1[0] = p2[0];
17056         p1[1] = p2[1];
17057         p1[2] = p2[2];
17058         in1 = in2;
17059         kv2 = listc[lp];
17060 
17061 /*   Compute the new values of P2 and IN2. */
17062 
17063         p2[0] = r11 * xc[kv2] + r12 * yc[kv2];
17064         p2[1] = r21 * xc[kv2] + r22 * yc[kv2] + r23 * zc[kv2];
17065         p2[2] = ex * xc[kv2] + ey * yc[kv2] + ez * zc[kv2];
17066         in2 = p2[2] >= 0. && p2[0] * p2[0] + p2[1] * p2[1] <= wrs;
17067 
17068 /* Add edge KV1-KV2 to the path iff both endpoints are inside */
17069 /*   the window and KV2 > KV1, or KV1 is inside and KV2 is */
17070 /*   outside (so that the edge is drawn only once). */
17071 
17072         if (! in1 || (in2 && kv2 <= kv1)) {
17073             goto L2;
17074         }
17075         if (p2[2] < 0.) {
17076 
17077 /*   KV2 is a 'southern hemisphere' point.  Move it to the */
17078 /*     intersection of edge KV1-KV2 with the equator so that */
17079 /*     the edge is clipped properly.  P2(3) is set to 0. */
17080 
17081             p2[0] = p1[2] * p2[0] - p2[2] * p1[0];
17082             p2[1] = p1[2] * p2[1] - p2[2] * p1[1];
17083             t = sqrt(p2[0] * p2[0] + p2[1] * p2[1]);
17084             p2[0] /= t;
17085             p2[1] /= t;
17086         }
17087 
17088 /*   Add the edge to the path.  (TOL is converted to world */
17089 /*     coordinates.) */
17090 
17091         if (p2[2] < 0.) {
17092             p2[2] = 0.f;
17093         }
17094         d__1 = tol / sf;
17095         drwarc_(lun, p1, p2, &d__1, &nseg);
17096 
17097 /* Bottom of loops. */
17098 
17099 L2:
17100         if (lp != lpl) {
17101             goto L1;
17102         }
17103 /* L3: */
17104     }
17105 
17106 /* Paint the path and restore the saved graphics state (with */
17107 /*   no clip path). */
17108 
17109 /*      WRITE (LUN,130,ERR=13) */
17110 /*      WRITE (LUN,190,ERR=13) */
17111 /*  190 FORMAT ('grestore') */
17112     if (*numbr) {
17113 
17114 /* Nodes in the window are to be labeled with their indexes. */
17115 /*   Convert FSIZN from points to world coordinates, and */
17116 /*   output the commands to select a font and scale it. */
17117 
17118         t = fsizn / sf;
17119 /*        WRITE (LUN,200,ERR=13) T */
17120 /*  200   FORMAT ('/Helvetica findfont'/ */
17121 /*     .          F12.6,' scalefont setfont') */
17122 
17123 /* Loop on visible nodes N0 that project to points (X0,Y0) in */
17124 /*   the window. */
17125 
17126         i__1 = *n;
17127         for (n0 = 1; n0 <= i__1; ++n0) {
17128             if (ex * x[n0] + ey * y[n0] + ez * z__[n0] < 0.) {
17129                 goto L4;
17130             }
17131             x0 = r11 * x[n0] + r12 * y[n0];
17132             y0 = r21 * x[n0] + r22 * y[n0] + r23 * z__[n0];
17133             if (x0 * x0 + y0 * y0 > wrs) {
17134                 goto L4;
17135             }
17136 
17137 /*   Move to (X0,Y0), and draw the label N0 with the origin */
17138 /*     of the first character at (X0,Y0). */
17139 
17140 /*          WRITE (LUN,210,ERR=13) X0, Y0 */
17141 /*          WRITE (LUN,220,ERR=13) N0 */
17142 /*  210     FORMAT (2F12.6,' moveto') */
17143 /*  220     FORMAT ('(',I3,') show') */
17144 L4:
17145             ;
17146         }
17147     }
17148 
17149 /* Convert FSIZT from points to world coordinates, and output */
17150 /*   the commands to select a font and scale it. */
17151 
17152     t = fsizt / sf;
17153 /*      WRITE (LUN,200,ERR=13) T */
17154 
17155 /* Display TITLE centered above the plot: */
17156 
17157     y0 = wr + t * 3.;
17158 /*      WRITE (LUN,230,ERR=13) TITLE, Y0 */
17159 /*  230 FORMAT (A80/'  stringwidth pop 2 div neg ',F12.6, */
17160 /*     .        ' moveto') */
17161 /*      WRITE (LUN,240,ERR=13) TITLE */
17162 /*  240 FORMAT (A80/'  show') */
17163     if (annot) {
17164 
17165 /* Display the window center and radius below the plot. */
17166 
17167         x0 = -wr;
17168         y0 = -wr - 50. / sf;
17169 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17170 /*        WRITE (LUN,250,ERR=13) ELAT, ELON */
17171         y0 -= t * 2.;
17172 /*        WRITE (LUN,210,ERR=13) X0, Y0 */
17173 /*        WRITE (LUN,260,ERR=13) A */
17174 /*  250   FORMAT ('(Window center:  ELAT = ',F7.2, */
17175 /*     .          ',  ELON = ',F8.2,') show') */
17176 /*  260   FORMAT ('(Angular extent:  A = ',F5.2,') show') */
17177     }
17178 
17179 /* Paint the path and output the showpage command and */
17180 /*   end-of-file indicator. */
17181 
17182 /*      WRITE (LUN,270,ERR=13) */
17183 /*  270 FORMAT ('stroke'/ */
17184 /*     .        'showpage'/ */
17185 /*     .        '%%EOF') */
17186 
17187 /* HP's interpreters require a one-byte End-of-PostScript-Job */
17188 /*   indicator (to eliminate a timeout error message): */
17189 /*   ASCII 4. */
17190 
17191 /*      WRITE (LUN,280,ERR=13) CHAR(4) */
17192 /*  280 FORMAT (A1) */
17193 
17194 /* No error encountered. */
17195 
17196     *ier = 0;
17197     return 0;
17198 
17199 /* Invalid input parameter LUN, PLTSIZ, N, or NT. */
17200 
17201 L11:
17202     *ier = 1;
17203     return 0;
17204 
17205 /* Invalid input parameter ELAT, ELON, or A. */
17206 
17207 L12:
17208     *ier = 2;
17209     return 0;
17210 
17211 /* Error writing to unit LUN. */
17212 
17213 /* L13: */
17214     *ier = 3;
17215     return 0;
17216 } /* vrplot_ */
17217 
17218 /* Subroutine */ int random_(int *ix, int *iy, int *iz,
17219         double *rannum)
17220 {
17221     static double x;
17222 
17223 
17224 /*   This routine returns pseudo-random numbers uniformly */
17225 /* distributed in the interval (0,1).  int seeds IX, IY, */
17226 /* and IZ should be initialized to values in the range 1 to */
17227 /* 30,000 before the first call to RANDOM, and should not */
17228 /* be altered between subsequent calls (unless a sequence */
17229 /* of random numbers is to be repeated by reinitializing the */
17230 /* seeds). */
17231 
17232 /* Reference:  B. A. Wichmann and I. D. Hill, An Efficient */
17233 /*             and Portable Pseudo-random Number Generator, */
17234 /*             Applied Statistics, Vol. 31, No. 2, 1982, */
17235 /*             pp. 188-190. */
17236 
17237     *ix = *ix * 171 % 30269;
17238     *iy = *iy * 172 % 30307;
17239     *iz = *iz * 170 % 30323;
17240     x = (double) (*ix) / 30269. + (double) (*iy) / 30307. + (
17241             double) (*iz) / 30323.;
17242     *rannum = x - (int) x;
17243     return 0;
17244 } /* random_ */
17245 
17246 #undef TRUE_
17247 #undef FALSE_
17248 #undef abs
17249 
17250 /*################################################################################################
17251 ##########  strid.f -- translated by f2c (version 20030320). ###################################
17252 ######   You must link the resulting object file with the libraries: #############################
17253 ####################    -lf2c -lm   (in that order)   ############################################
17254 ################################################################################################*/
17255 
17256 
17257 
17258 EMData* Util::mult_scalar(EMData* img, float scalar)
17259 {
17260         ENTERFUNC;
17261         /* Exception Handle */
17262         if (!img) {
17263                 throw NullPointerException("NULL input image");
17264         }
17265         /* ============  output = scalar*input  ================== */
17266 
17267         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17268         int size = nx*ny*nz;
17269         EMData * img2 = img->copy_head();
17270         float *img_ptr  =img->get_data();
17271         float *img2_ptr = img2->get_data();
17272         for (int i=0;i<size;i++)img2_ptr[i] = img_ptr[i]*scalar;
17273         img2->update();
17274 
17275         if(img->is_complex()) {
17276                 img2->set_complex(true);
17277                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17278         }
17279         EXITFUNC;
17280         return img2;
17281 }
17282 
17283 EMData* Util::madn_scalar(EMData* img, EMData* img1, float scalar)
17284 {
17285         ENTERFUNC;
17286         /* Exception Handle */
17287         if (!img) {
17288                 throw NullPointerException("NULL input image");
17289         }
17290         /* ==============   output = img + scalar*img1   ================ */
17291 
17292         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17293         int size = nx*ny*nz;
17294         EMData * img2 = img->copy_head();
17295         float *img_ptr  =img->get_data();
17296         float *img2_ptr = img2->get_data();
17297         float *img1_ptr = img1->get_data();
17298         for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i]*scalar;
17299         img2->update();
17300         if(img->is_complex()) {
17301                 img2->set_complex(true);
17302                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17303         }
17304 
17305         EXITFUNC;
17306         return img2;
17307 }
17308 
17309 EMData* Util::addn_img(EMData* img, EMData* img1)
17310 {
17311         ENTERFUNC;
17312         /* Exception Handle */
17313         if (!img) {
17314                 throw NullPointerException("NULL input image");
17315         }
17316         /* ==============   output = img + img1   ================ */
17317 
17318         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17319         int size = nx*ny*nz;
17320         EMData * img2 = img->copy_head();
17321         float *img_ptr  =img->get_data();
17322         float *img2_ptr = img2->get_data();
17323         float *img1_ptr = img1->get_data();
17324         for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + img1_ptr[i];
17325         img2->update();
17326         if(img->is_complex()) {
17327                 img2->set_complex(true);
17328                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17329         }
17330 
17331         EXITFUNC;
17332         return img2;
17333 }
17334 
17335 EMData* Util::subn_img(EMData* img, EMData* img1)
17336 {
17337         ENTERFUNC;
17338         /* Exception Handle */
17339         if (!img) {
17340                 throw NullPointerException("NULL input image");
17341         }
17342         /* ==============   output = img - img1   ================ */
17343 
17344         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17345         int size = nx*ny*nz;
17346         EMData * img2 = img->copy_head();
17347         float *img_ptr  =img->get_data();
17348         float *img2_ptr = img2->get_data();
17349         float *img1_ptr = img1->get_data();
17350         for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] - img1_ptr[i];
17351         img2->update();
17352         if(img->is_complex()) {
17353                 img2->set_complex(true);
17354                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17355         }
17356 
17357         EXITFUNC;
17358         return img2;
17359 }
17360 
17361 EMData* Util::muln_img(EMData* img, EMData* img1)
17362 {
17363         ENTERFUNC;
17364         /* Exception Handle */
17365         if (!img) {
17366                 throw NullPointerException("NULL input image");
17367         }
17368         /* ==============   output = img * img1   ================ */
17369 
17370         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17371         int size = nx*ny*nz;
17372         EMData * img2 = img->copy_head();
17373         float *img_ptr  =img->get_data();
17374         float *img2_ptr = img2->get_data();
17375         float *img1_ptr = img1->get_data();
17376         if(img->is_complex()) {
17377                 for (int i=0; i<size; i+=2) {
17378                         img2_ptr[i]   = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17379                         img2_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17380                 }
17381                 img2->set_complex(true);
17382                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17383         } else {
17384                 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] * img1_ptr[i];
17385                 img2->update();
17386         }
17387 
17388         EXITFUNC;
17389         return img2;
17390 }
17391 
17392 EMData* Util::divn_img(EMData* img, EMData* img1)
17393 {
17394         ENTERFUNC;
17395         /* Exception Handle */
17396         if (!img) {
17397                 throw NullPointerException("NULL input image");
17398         }
17399         /* ==============   output = img / img1   ================ */
17400 
17401         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17402         int size = nx*ny*nz;
17403         EMData * img2 = img->copy_head();
17404         float *img_ptr  =img->get_data();
17405         float *img2_ptr = img2->get_data();
17406         float *img1_ptr = img1->get_data();
17407         if(img->is_complex()) {
17408                 float  sq2;
17409                 for (int i=0; i<size; i+=2) {
17410                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17411                         img2_ptr[i]   = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17412                         img2_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17413                 }
17414                 img2->set_complex(true);
17415                 if(img->is_fftodd()) img2->set_fftodd(true); else img2->set_fftodd(false);
17416         } else {
17417                 for (int i=0; i<size; i++) img2_ptr[i] = img_ptr[i] / img1_ptr[i];
17418                 img2->update();
17419         }
17420 
17421         EXITFUNC;
17422         return img2;
17423 }
17424 
17425 EMData* Util::divn_filter(EMData* img, EMData* img1)
17426 {
17427         ENTERFUNC;
17428         /* Exception Handle */
17429         if (!img) {
17430                 throw NullPointerException("NULL input image");
17431         }
17432         /* ========= img /= img1 ===================== */
17433 
17434         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17435         int size = nx*ny*nz;
17436         EMData * img2 = img->copy_head();
17437         float *img_ptr  =img->get_data();
17438         float *img1_ptr = img1->get_data();
17439         float *img2_ptr = img2->get_data();
17440         if(img->is_complex()) {
17441                 for (int i=0; i<size; i+=2) {
17442                         if(img1_ptr[i] > 1.e-10f) {
17443                         img2_ptr[i]   = img_ptr[i]  /img1_ptr[i];
17444                         img2_ptr[i+1] = img_ptr[i+1]/img1_ptr[i];
17445                         } else img2_ptr[i] = img2_ptr[i+1] = 0.0f;
17446                 }
17447         } else  throw ImageFormatException("Only Fourier image allowed");
17448 
17449         img->update();
17450 
17451         EXITFUNC;
17452         return img2;
17453 }
17454 
17455 void Util::mul_scalar(EMData* img, float scalar)
17456 {
17457         ENTERFUNC;
17458         /* Exception Handle */
17459         if (!img) {
17460                 throw NullPointerException("NULL input image");
17461         }
17462         /* ============  output = scalar*input  ================== */
17463 
17464         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17465         int size = nx*ny*nz;
17466         float *img_ptr  =img->get_data();
17467         for (int i=0;i<size;i++) img_ptr[i] *= scalar;
17468         img->update();
17469 
17470         EXITFUNC;
17471 }
17472 
17473 void Util::mad_scalar(EMData* img, EMData* img1, float scalar)
17474 {
17475         ENTERFUNC;
17476         /* Exception Handle */
17477         if (!img) {
17478                 throw NullPointerException("NULL input image");
17479         }
17480         /* ==============   img += scalar*img1   ================ */
17481 
17482         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17483         int size = nx*ny*nz;
17484         float *img_ptr  =img->get_data();
17485         float *img1_ptr = img1->get_data();
17486         for (int i=0;i<size;i++)img_ptr[i] += img1_ptr[i]*scalar;
17487         img1->update();
17488 
17489         EXITFUNC;
17490 }
17491 
17492 void Util::add_img(EMData* img, EMData* img1)
17493 {
17494         ENTERFUNC;
17495         /* Exception Handle */
17496         if (!img) {
17497                 throw NullPointerException("NULL input image");
17498         }
17499         /* ========= img += img1 ===================== */
17500 
17501         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17502         int size = nx*ny*nz;
17503         float *img_ptr  = img->get_data();
17504         float *img1_ptr = img1->get_data();
17505         for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i];
17506         img->update();
17507 
17508         EXITFUNC;
17509 }
17510 
17511 void Util::add_img_abs(EMData* img, EMData* img1)
17512 {
17513         ENTERFUNC;
17514         /* Exception Handle */
17515         if (!img) {
17516                 throw NullPointerException("NULL input image");
17517         }
17518         /* ========= img += img1 ===================== */
17519 
17520         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17521         int size = nx*ny*nz;
17522         float *img_ptr  = img->get_data();
17523         float *img1_ptr = img1->get_data();
17524         for (int i=0;i<size;i++) img_ptr[i] += abs(img1_ptr[i]);
17525         img->update();
17526 
17527         EXITFUNC;
17528 }
17529 
17530 void Util::add_img2(EMData* img, EMData* img1)
17531 {
17532         ENTERFUNC;
17533         /* Exception Handle */
17534         if (!img) {
17535                 throw NullPointerException("NULL input image");
17536         }
17537         /* ========= img += img1**2 ===================== */
17538 
17539         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17540         int size = nx*ny*nz;
17541         float *img_ptr  = img->get_data();
17542         float *img1_ptr = img1->get_data();
17543         if(img->is_complex()) {
17544                 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] ;
17545         } else {
17546                 for (int i=0;i<size;i++) img_ptr[i] += img1_ptr[i]*img1_ptr[i];
17547         }
17548         img->update();
17549 
17550         EXITFUNC;
17551 }
17552 
17553 void Util::sub_img(EMData* img, EMData* img1)
17554 {
17555         ENTERFUNC;
17556         /* Exception Handle */
17557         if (!img) {
17558                 throw NullPointerException("NULL input image");
17559         }
17560         /* ========= img -= img1 ===================== */
17561 
17562         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17563         int size = nx*ny*nz;
17564         float *img_ptr  = img->get_data();
17565         float *img1_ptr = img1->get_data();
17566         for (int i=0;i<size;i++) img_ptr[i] -= img1_ptr[i];
17567         img->update();
17568 
17569         EXITFUNC;
17570 }
17571 
17572 void Util::mul_img(EMData* img, EMData* img1)
17573 {
17574         ENTERFUNC;
17575         /* Exception Handle */
17576         if (!img) {
17577                 throw NullPointerException("NULL input image");
17578         }
17579         /* ========= img *= img1 ===================== */
17580 
17581         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17582         int size = nx*ny*nz;
17583         float *img_ptr  = img->get_data();
17584         float *img1_ptr = img1->get_data();
17585         if(img->is_complex()) {
17586                 for (int i=0; i<size; i+=2) {
17587                         float tmp     = img_ptr[i] * img1_ptr[i]   - img_ptr[i+1] * img1_ptr[i+1] ;
17588                         img_ptr[i+1] = img_ptr[i] * img1_ptr[i+1] + img_ptr[i+1] * img1_ptr[i] ;
17589                         img_ptr[i]   = tmp;
17590 
17591                 }
17592         } else {
17593                 for (int i=0;i<size;i++) img_ptr[i] *= img1_ptr[i];
17594         }
17595         img->update();
17596 
17597         EXITFUNC;
17598 }
17599 
17600 void Util::div_img(EMData* img, EMData* img1)
17601 {
17602         ENTERFUNC;
17603         /* Exception Handle */
17604         if (!img) {
17605                 throw NullPointerException("NULL input image");
17606         }
17607         /* ========= img /= img1 ===================== */
17608 
17609         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17610         int size = nx*ny*nz;
17611         float *img_ptr  = img->get_data();
17612         float *img1_ptr = img1->get_data();
17613         if(img->is_complex()) {
17614                 float  sq2;
17615                 for (int i=0; i<size; i+=2) {
17616                         sq2 = 1.0f/(img1_ptr[i] * img1_ptr[i]   + img1_ptr[i+1] * img1_ptr[i+1]);
17617                         float tmp    = sq2*(img_ptr[i] * img1_ptr[i]   + img_ptr[i+1] * img1_ptr[i+1]) ;
17618                         img_ptr[i+1] = sq2*(img_ptr[i+1] * img1_ptr[i] - img_ptr[i] * img1_ptr[i+1]) ;
17619                         img_ptr[i]   = tmp;
17620                 }
17621         } else {
17622                 for (int i=0; i<size; i++) img_ptr[i] /= img1_ptr[i];
17623         }
17624         img->update();
17625 
17626         EXITFUNC;
17627 }
17628 
17629 void Util::div_filter(EMData* img, EMData* img1)
17630 {
17631         ENTERFUNC;
17632         /* Exception Handle */
17633         if (!img) {
17634                 throw NullPointerException("NULL input image");
17635         }
17636         /* ========= img /= img1 ===================== */
17637 
17638         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
17639         int size = nx*ny*nz;
17640         float *img_ptr  = img->get_data();
17641         float *img1_ptr = img1->get_data();
17642         if(img->is_complex()) {
17643                 for (int i=0; i<size; i+=2) {
17644                         if(img1_ptr[i] > 1.e-10f) {
17645                         img_ptr[i]   /= img1_ptr[i];
17646                         img_ptr[i+1] /= img1_ptr[i];
17647                         } else img_ptr[i] = img_ptr[i+1] = 0.0f;
17648                 }
17649         } else throw ImageFormatException("Only Fourier image allowed");
17650 
17651         img->update();
17652 
17653         EXITFUNC;
17654 }
17655 
17656 #define img_ptr(i,j,k)  img_ptr[2*(i-1)+((j-1)+((k-1)*ny))*nxo]
17657 
17658 EMData* Util::pack_complex_to_real(EMData* img)
17659 {
17660         ENTERFUNC;
17661         /* Exception Handle */
17662         if (!img) {
17663                 throw NullPointerException("NULL input image");
17664         }
17665         /* ==============   img is modulus of a complex image in FFT format (so its imaginary parts are zero),
17666                               output is img packed into real image with Friedel part added,   ================ */
17667 
17668         int nxo=img->get_xsize(), ny=img->get_ysize(), nz=img->get_zsize();
17669         int nx = nxo - 2 + img->is_fftodd();
17670         int lsd2 = (nx + 2 - nx%2) / 2; // Extended x-dimension of the complex image
17671         int nyt, nzt;
17672         int nx2 = nx/2;
17673         int ny2 = ny/2; if(ny2 == 0) nyt =0; else nyt=ny;
17674         int nz2 = nz/2; if(nz2 == 0) nzt =0; else nzt=nz;
17675         int nx2p = nx2+nx%2;
17676         int ny2p = ny2+ny%2;
17677         int nz2p = nz2+nz%2;
17678         EMData& power = *(new EMData()); // output image
17679         power.set_size(nx, ny, nz);
17680         power.set_array_offsets(-nx2,-ny2,-nz2);
17681         //img->set_array_offsets(1,1,1);
17682         float *img_ptr  = img->get_data();
17683         for (int iz = 1; iz <= nz; iz++) {
17684                 int jz=iz-1;
17685                 if(jz>=nz2p) jz=jz-nzt;
17686                 for (int iy = 1; iy <= ny; iy++) {
17687                         int jy=iy-1;
17688                         if(jy>=ny2p) jy=jy-nyt;
17689                         for (int ix = 1; ix <= lsd2; ix++) {
17690                                 int jx=ix-1;
17691                                 if(jx>=nx2p) jx=jx-nx;
17692                                 power(jx,jy,jz) = img_ptr(ix,iy,iz); //real(img->cmplx(ix,iy,iz));
17693                         }
17694                 }
17695         }
17696 //  Create the Friedel related half
17697         int  nzb, nze, nyb, nye, nxb, nxe;
17698         nxb =-nx2+(nx+1)%2;
17699         nxe = nx2-(nx+1)%2;
17700         if(ny2 == 0) {nyb =0; nye = 0;} else {nyb =-ny2+(ny+1)%2; nye = ny2-(ny+1)%2;}
17701         if(nz2 == 0) {nzb =0; nze = 0;} else {nzb =-nz2+(nz+1)%2; nze = nz2-(nz+1)%2;}
17702         for (int iz = nzb; iz <= nze; iz++) {
17703                 for (int iy = nyb; iy <= nye; iy++) {
17704                         for (int ix = 1; ix <= nxe; ix++) { // Note this loop begins with 1 - FFT should create correct Friedel related 0 plane
17705                                 power(-ix,-iy,-iz) = power(ix,iy,iz);
17706                         }
17707                 }
17708         }
17709         if(ny2 != 0)  {
17710                 if(nz2 != 0)  {
17711                         if(nz%2 == 0) {  //if nz even, fix the first slice
17712                                 for (int iy = nyb; iy <= nye; iy++) {
17713                                         for (int ix = nxb; ix <= -1; ix++) {
17714                                                 power(ix,iy,-nz2) = power(-ix,-iy,-nz2);
17715                                         }
17716                                 }
17717                                 if(ny%2 == 0) {  //if ny even, fix the first line
17718                                         for (int ix = nxb; ix <= -1; ix++) {
17719                                                 power(ix,-ny2,-nz2) = power(-ix,-ny2,-nz2);
17720                                         }
17721                                 }
17722                         }
17723                 }
17724                 if(ny%2 == 0) {  //if ny even, fix the first column
17725                         for (int iz = nzb; iz <= nze; iz++) {
17726                                 for (int ix = nxb; ix <= -1; ix++) {
17727                                         power(ix,-ny2,-iz) = power(-ix,-ny2,iz);
17728                                 }
17729                         }
17730                 }
17731 
17732         }
17733         power.update();
17734         power.set_array_offsets(0,0,0);
17735         return &power;
17736 }
17737 #undef  img_ptr
17738 
17739 float Util::ang_n(float peakp, string mode, int maxrin)
17740 {
17741     if (mode == "f" || mode == "F")
17742         return fmodf(((peakp-1.0f) / maxrin+1.0f)*360.0f,360.0f);
17743     else
17744         return fmodf(((peakp-1.0f) / maxrin+1.0f)*180.0f,180.0f);
17745 }
17746 
17747 
17748 void Util::Normalize_ring( EMData* ring, const vector<int>& numr )
17749 {
17750     float* data = ring->get_data();
17751     float av=0.0;
17752     float sq=0.0;
17753     float nn=0.0;
17754     int nring = numr.size()/3;
17755     for( int i=0; i < nring; ++i )
17756     {
17757         int numr3i = numr[3*i+2];
17758         int numr2i = numr[3*i+1]-1;
17759         float w = numr[3*i]*2*M_PI/float(numr[3*i+2]);
17760         for( int j=0; j < numr3i; ++j )
17761         {
17762             int jc = numr2i+j;
17763             av += data[jc] * w;
17764             sq += data[jc] * data[jc] * w;
17765             nn += w;
17766         }
17767     }
17768 
17769     float avg = av/nn;
17770     float sgm = sqrt( (sq-av*av/nn)/nn );
17771     int n = ring->get_xsize() * ring->get_ysize() * ring->get_zsize();
17772     for( int i=0; i < n; ++i )
17773     {
17774         data[i] -= avg;
17775         data[i] /= sgm;
17776     }
17777 
17778     ring->update();
17779 }
17780 
17781 vector<float> Util::multiref_polar_ali_2d(EMData* image, const vector< EMData* >& crefim,
17782                 float xrng, float yrng, float step, string mode,
17783                 vector<int>numr, float cnx, float cny) {
17784 
17785     // Manually extract.
17786 /*    vector< EMAN::EMData* > crefim;
17787     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17788     crefim.reserve(crefim_len);
17789 
17790     for(std::size_t i=0;i<crefim_len;i++) {
17791         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17792         crefim.push_back(proxy());
17793     }
17794 */
17795 
17796         size_t crefim_len = crefim.size();
17797 
17798         int   ky = int(2*yrng/step+0.5)/2;
17799         int   kx = int(2*xrng/step+0.5)/2;
17800         int   iref, nref=0, mirror=0;
17801         float iy, ix, sx=0, sy=0;
17802         float peak = -1.0E23f;
17803         float ang=0.0f;
17804         for (int i = -ky; i <= ky; i++) {
17805                 iy = i * step ;
17806                 for (int j = -kx; j <= kx; j++) {
17807                         ix = j*step ;
17808                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17809 
17810                         Normalize_ring( cimage, numr );
17811 
17812                         Frngs(cimage, numr);
17813                         //  compare with all reference images
17814                         // for iref in xrange(len(crefim)):
17815                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17816                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17817                                 double qn = retvals["qn"];
17818                                 double qm = retvals["qm"];
17819                                 if(qn >= peak || qm >= peak) {
17820                                         sx = -ix;
17821                                         sy = -iy;
17822                                         nref = iref;
17823                                         if (qn >= qm) {
17824                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17825                                                 peak = static_cast<float>(qn);
17826                                                 mirror = 0;
17827                                         } else {
17828                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17829                                                 peak = static_cast<float>(qm);
17830                                                 mirror = 1;
17831                                         }
17832                                 }
17833                         }  delete cimage; cimage = 0;
17834                 }
17835         }
17836         float co, so, sxs, sys;
17837         co = static_cast<float>( cos(ang*pi/180.0) );
17838         so = static_cast<float>( -sin(ang*pi/180.0) );
17839         sxs = sx*co - sy*so;
17840         sys = sx*so + sy*co;
17841         vector<float> res;
17842         res.push_back(ang);
17843         res.push_back(sxs);
17844         res.push_back(sys);
17845         res.push_back(static_cast<float>(mirror));
17846         res.push_back(static_cast<float>(nref));
17847         res.push_back(peak);
17848         return res;
17849 }
17850 
17851 vector<float> Util::multiref_polar_ali_2d_peaklist(EMData* image, const vector< EMData* >& crefim,
17852                 float xrng, float yrng, float step, string mode,
17853                 vector<int>numr, float cnx, float cny) {
17854 
17855         size_t crefim_len = crefim.size();
17856 
17857         int   ky = int(2*yrng/step+0.5)/2;
17858         int   kx = int(2*xrng/step+0.5)/2;
17859         float iy, ix;
17860         vector<float> peak(crefim_len*5, -1.0e23f);
17861         for (int i = -ky; i <= ky; i++) {
17862                 iy = i * step ;
17863                 for (int j = -kx; j <= kx; j++) {
17864                         ix = j*step ;
17865                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17866                         Normalize_ring( cimage, numr );
17867                         Frngs(cimage, numr);
17868                         for (int iref = 0; iref < (int)crefim_len; iref++) {
17869                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
17870                                 double qn = retvals["qn"];
17871                                 double qm = retvals["qm"];
17872                                 if(qn >= peak[iref*5] || qm >= peak[iref*5]) {
17873                                         if (qn >= qm) {
17874                                                 peak[iref*5] = static_cast<float>(qn);
17875                                                 peak[iref*5+1] = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17876                                                 peak[iref*5+2] = -ix;
17877                                                 peak[iref*5+3] = -iy;
17878                                                 peak[iref*5+4] = 0;
17879                                         } else {
17880                                                 peak[iref*5] = static_cast<float>(qm);
17881                                                 peak[iref*5+1] = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17882                                                 peak[iref*5+2] = -ix;
17883                                                 peak[iref*5+3] = -iy;
17884                                                 peak[iref*5+4] = 1;
17885                                         }
17886                                 }
17887                         }  delete cimage; cimage = 0;
17888                 }
17889         }
17890         for (int iref = 0; iref < (int)crefim_len; iref++) {
17891                 float ang = peak[iref*5+1];
17892                 float sx = peak[iref*5+2];
17893                 float sy = peak[iref*5+3];
17894                 float co =  cos(ang*pi/180.0);
17895                 float so = -sin(ang*pi/180.0);
17896                 float sxs = sx*co - sy*so;
17897                 float sys = sx*so + sy*co;
17898                 peak[iref*5+2] = sxs;
17899                 peak[iref*5+3] = sys;
17900         }
17901         return peak;
17902 }
17903 
17904 
17905 vector<float> Util::multiref_polar_ali_2d_delta(EMData* image, const vector< EMData* >& crefim,
17906                 float xrng, float yrng, float step, string mode,
17907                 vector<int>numr, float cnx, float cny, float delta_start, float delta) {
17908 
17909     // Manually extract.
17910 /*    vector< EMAN::EMData* > crefim;
17911     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17912     crefim.reserve(crefim_len);
17913 
17914     for(std::size_t i=0;i<crefim_len;i++) {
17915         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17916         crefim.push_back(proxy());
17917     }
17918 */
17919 
17920         size_t crefim_len = crefim.size();
17921 
17922         int   ky = int(2*yrng/step+0.5)/2;
17923         int   kx = int(2*xrng/step+0.5)/2;
17924         int   iref, nref=0, mirror=0;
17925         float iy, ix, sx=0, sy=0;
17926         float peak = -1.0E23f;
17927         float ang=0.0f;
17928         for (int i = -ky; i <= ky; i++) {
17929                 iy = i * step ;
17930                 for (int j = -kx; j <= kx; j++) {
17931                         ix = j*step ;
17932                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
17933 
17934                         Normalize_ring( cimage, numr );
17935 
17936                         Frngs(cimage, numr);
17937                         //  compare with all reference images
17938                         // for iref in xrange(len(crefim)):
17939                         for ( iref = 0; iref < (int)crefim_len; iref++) {
17940                                 Dict retvals = Crosrng_ms_delta(crefim[iref], cimage, numr, delta_start, delta);
17941                                 double qn = retvals["qn"];
17942                                 double qm = retvals["qm"];
17943                                 if(qn >= peak || qm >= peak) {
17944                                         sx = -ix;
17945                                         sy = -iy;
17946                                         nref = iref;
17947                                         if (qn >= qm) {
17948                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
17949                                                 peak = static_cast<float>(qn);
17950                                                 mirror = 0;
17951                                         } else {
17952                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
17953                                                 peak = static_cast<float>(qm);
17954                                                 mirror = 1;
17955                                         }
17956                                 }
17957                         }  delete cimage; cimage = 0;
17958                 }
17959         }
17960         float co, so, sxs, sys;
17961         co = static_cast<float>( cos(ang*pi/180.0) );
17962         so = static_cast<float>( -sin(ang*pi/180.0) );
17963         sxs = sx*co - sy*so;
17964         sys = sx*so + sy*co;
17965         vector<float> res;
17966         res.push_back(ang);
17967         res.push_back(sxs);
17968         res.push_back(sys);
17969         res.push_back(static_cast<float>(mirror));
17970         res.push_back(static_cast<float>(nref));
17971         res.push_back(peak);
17972         return res;
17973 }
17974 
17975 vector<float> Util::multiref_polar_ali_2d_nom(EMData* image, const vector< EMData* >& crefim,
17976                 float xrng, float yrng, float step, string mode,
17977                 vector< int >numr, float cnx, float cny) {
17978 
17979     // Manually extract.
17980 /*    vector< EMAN::EMData* > crefim;
17981     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
17982     crefim.reserve(crefim_len);
17983 
17984     for(std::size_t i=0;i<crefim_len;i++) {
17985         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
17986         crefim.push_back(proxy());
17987     }
17988 */
17989         size_t crefim_len = crefim.size();
17990 
17991         int   ky = int(2*yrng/step+0.5)/2;
17992         int   kx = int(2*xrng/step+0.5)/2;
17993         int   iref, nref=0;
17994         float iy, ix, sx=0, sy=0;
17995         float peak = -1.0E23f;
17996         float ang=0.0f;
17997         for (int i = -ky; i <= ky; i++) {
17998                 iy = i * step ;
17999                 for (int j = -kx; j <= kx; j++) {
18000                         ix = j*step ;
18001                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18002                         Frngs(cimage, numr);
18003                         //  compare with all reference images
18004                         // for iref in xrange(len(crefim)):
18005                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18006                                 Dict retvals = Crosrng_ns(crefim[iref], cimage, numr);
18007                                 double qn = retvals["qn"];
18008                                 if(qn >= peak) {
18009                                         sx = -ix;
18010                                         sy = -iy;
18011                                         nref = iref;
18012                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18013                                         peak = static_cast<float>(qn);
18014                                 }
18015                         }  delete cimage; cimage = 0;
18016                 }
18017         }
18018         float co, so, sxs, sys;
18019         co = static_cast<float>( cos(ang*pi/180.0) );
18020         so = static_cast<float>( -sin(ang*pi/180.0) );
18021         sxs = sx*co - sy*so;
18022         sys = sx*so + sy*co;
18023         vector<float> res;
18024         res.push_back(ang);
18025         res.push_back(sxs);
18026         res.push_back(sys);
18027         res.push_back(static_cast<float>(nref));
18028         res.push_back(peak);
18029         return res;
18030 }
18031 
18032 vector<float> Util::multiref_polar_ali_2d_local(EMData* image, const vector< EMData* >& crefim,
18033                 float xrng, float yrng, float step, float ant, string mode,
18034                 vector<int>numr, float cnx, float cny) {
18035 
18036     // Manually extract.
18037 /*    vector< EMAN::EMData* > crefim;
18038     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18039     crefim.reserve(crefim_len);
18040 
18041     for(std::size_t i=0;i<crefim_len;i++) {
18042         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18043         crefim.push_back(proxy());
18044     }
18045 */
18046         size_t crefim_len = crefim.size();
18047         const float qv = static_cast<float>( pi/180.0 );
18048 
18049         Transform * t = image->get_attr("xform.projection");
18050         Dict d = t->get_params("spider");
18051         if(t) {delete t; t=0;}
18052         float phi = d["phi"];
18053         float theta = d["theta"];
18054         int   ky = int(2*yrng/step+0.5)/2;
18055         int   kx = int(2*xrng/step+0.5)/2;
18056         int   iref, nref=0, mirror=0;
18057         float iy, ix, sx=0, sy=0;
18058         float peak = -1.0E23f;
18059         float ang=0.0f;
18060         float imn1 = sin(theta*qv)*cos(phi*qv);
18061         float imn2 = sin(theta*qv)*sin(phi*qv);
18062         float imn3 = cos(theta*qv);
18063         vector<float> n1(crefim_len);
18064         vector<float> n2(crefim_len);
18065         vector<float> n3(crefim_len);
18066         for ( iref = 0; iref < (int)crefim_len; iref++) {
18067                         n1[iref] = crefim[iref]->get_attr("n1");
18068                         n2[iref] = crefim[iref]->get_attr("n2");
18069                         n3[iref] = crefim[iref]->get_attr("n3");
18070         }
18071         for (int i = -ky; i <= ky; i++) {
18072             iy = i * step ;
18073             for (int j = -kx; j <= kx; j++) {
18074                 ix = j*step;
18075                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18076 
18077                 Normalize_ring( cimage, numr );
18078 
18079                 Frngs(cimage, numr);
18080                 //  compare with all reference images
18081                 // for iref in xrange(len(crefim)):
18082                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18083                         if(abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18084                                 Dict retvals = Crosrng_ms(crefim[iref], cimage, numr);
18085                                 double qn = retvals["qn"];
18086                                 double qm = retvals["qm"];
18087                                 if(qn >= peak || qm >= peak) {
18088                                         sx = -ix;
18089                                         sy = -iy;
18090                                         nref = iref;
18091                                         if (qn >= qm) {
18092                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18093                                                 peak = static_cast<float>( qn );
18094                                                 mirror = 0;
18095                                         } else {
18096                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18097                                                 peak = static_cast<float>( qm );
18098                                                 mirror = 1;
18099                                         }
18100                                 }
18101                         }
18102                 }  delete cimage; cimage = 0;
18103             }
18104         }
18105         float co, so, sxs, sys;
18106         if(peak == -1.0E23) {
18107                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18108                 nref = -1;
18109         } else {
18110                 co =  cos(ang*qv);
18111                 so = -sin(ang*qv);
18112                 sxs = sx*co - sy*so;
18113                 sys = sx*so + sy*co;
18114         }
18115         vector<float> res;
18116         res.push_back(ang);
18117         res.push_back(sxs);
18118         res.push_back(sys);
18119         res.push_back(static_cast<float>(mirror));
18120         res.push_back(static_cast<float>(nref));
18121         res.push_back(peak);
18122         return res;
18123 }
18124 
18125 vector<float> Util::multiref_polar_ali_2d_local_psi(EMData* image, const vector< EMData* >& crefim,
18126                 float xrng, float yrng, float step, float ant, float psi_max, string mode,
18127                 vector<int>numr, float cnx, float cny) {
18128 
18129     // Manually extract.
18130 /*    vector< EMAN::EMData* > crefim;
18131     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18132     crefim.reserve(crefim_len);
18133 
18134     for(std::size_t i=0;i<crefim_len;i++) {
18135         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18136         crefim.push_back(proxy());
18137     }
18138 */
18139         size_t crefim_len = crefim.size();
18140         const float qv = static_cast<float>(pi/180.0);
18141 
18142         Transform* t = image->get_attr("xform.projection");
18143         Dict d = t->get_params("spider");
18144         if(t) {delete t; t=0;}
18145         float phi = d["phi"];
18146         float theta = d["theta"];
18147         float psi = d["psi"];
18148         int ky = int(2*yrng/step+0.5)/2;
18149         int kx = int(2*xrng/step+0.5)/2;
18150         int iref, nref = 0, mirror = 0;
18151         float iy, ix, sx = 0, sy = 0;
18152         float peak = -1.0E23f;
18153         float ang = 0.0f;
18154         float imn1 = sin(theta*qv)*cos(phi*qv);
18155         float imn2 = sin(theta*qv)*sin(phi*qv);
18156         float imn3 = cos(theta*qv);
18157         vector<float> n1(crefim_len);
18158         vector<float> n2(crefim_len);
18159         vector<float> n3(crefim_len);
18160         for (iref = 0; iref < (int)crefim_len; iref++) {
18161                         n1[iref] = crefim[iref]->get_attr("n1");
18162                         n2[iref] = crefim[iref]->get_attr("n2");
18163                         n3[iref] = crefim[iref]->get_attr("n3");
18164         }
18165         bool nomirror = (theta<90.0) || ((theta==90.0) && (psi<psi_max));
18166         if (!nomirror) {
18167                 phi = fmod(phi+540.0f, 360.0f);
18168                 theta = 180-theta;
18169                 psi = fmod(540.0f-psi, 360.0f);
18170         }
18171         for (int i = -ky; i <= ky; i++) {
18172             iy = i * step ;
18173             for (int j = -kx; j <= kx; j++) {
18174                 ix = j*step;
18175                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18176 
18177                 Normalize_ring(cimage, numr);
18178 
18179                 Frngs(cimage, numr);
18180                 //  compare with all reference images
18181                 // for iref in xrange(len(crefim)):
18182                 for (iref = 0; iref < (int)crefim_len; iref++) {
18183                         if (abs(n1[iref]*imn1 + n2[iref]*imn2 + n3[iref]*imn3)>=ant) {
18184                                 if (nomirror) {
18185                                         Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 0);
18186                                         double qn = retvals["qn"];
18187                                         if (qn >= peak) {
18188                                                 sx = -ix;
18189                                                 sy = -iy;
18190                                                 nref = iref;
18191                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18192                                                 peak = static_cast<float>(qn);
18193                                                 mirror = 0;
18194                                         }
18195                                 } else {
18196                                         Dict retvals = Crosrng_sm_psi(crefim[iref], cimage, numr, psi, 1);
18197                                         double qn = retvals["qn"];
18198                                         if (qn >= peak) {
18199                                                 sx = -ix;
18200                                                 sy = -iy;
18201                                                 nref = iref;
18202                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18203                                                 peak = static_cast<float>(qn);
18204                                                 mirror = 1;
18205                                         }
18206                                 }
18207                         }
18208                 }  delete cimage; cimage = 0;
18209             }
18210         }
18211         float co, so, sxs, sys;
18212         if(peak == -1.0E23) {
18213                 ang=0.0; sxs=0.0; sys=0.0; mirror=0;
18214                 nref = -1;
18215         } else {
18216                 co =  cos(ang*qv);
18217                 so = -sin(ang*qv);
18218                 sxs = sx*co - sy*so;
18219                 sys = sx*so + sy*co;
18220         }
18221         vector<float> res;
18222         res.push_back(ang);
18223         res.push_back(sxs);
18224         res.push_back(sys);
18225         res.push_back(static_cast<float>(mirror));
18226         res.push_back(static_cast<float>(nref));
18227         res.push_back(peak);
18228         return res;
18229 }
18230 
18231 
18232 vector<float> Util::multiref_polar_ali_helical(EMData* image, const vector< EMData* >& crefim,
18233                 float xrng, float yrng, float step, float psi_max, string mode,
18234                 vector<int>numr, float cnx, float cny, int ynumber) {
18235 
18236         size_t crefim_len = crefim.size();
18237 
18238         int   iref, nref=0, mirror=0;
18239         float iy, ix, sx=0, sy=0;
18240         float peak = -1.0E23f;
18241         float ang=0.0f;
18242         int   kx = int(2*xrng/step+0.5)/2;
18243         //if ynumber==-1, use the old code which process x and y direction equally.
18244         if(ynumber==-1) {
18245                 int   ky = int(2*yrng/step+0.5)/2;
18246                 for (int i = -ky; i <= ky; i++) {
18247                         iy = i * step ;
18248                         for (int j = -kx; j <= kx; j++)  {
18249                                 ix = j*step ;
18250                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18251 
18252                                 Normalize_ring( cimage, numr );
18253 
18254                                 Frngs(cimage, numr);
18255                                 //  compare with all reference images
18256                                 // for iref in xrange(len(crefim)):
18257                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18258                                         Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18259                                         double qn = retvals["qn"];
18260                                         double qm = retvals["qm"];
18261                                         if(qn >= peak || qm >= peak) {
18262                                                 sx = -ix;
18263                                                 sy = -iy;
18264                                                 nref = iref;
18265                                                 if (qn >= qm) {
18266                                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18267                                                         peak = static_cast<float>(qn);
18268                                                         mirror = 0;
18269                                                 } else {
18270                                                         ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18271                                                         peak = static_cast<float>(qm);
18272                                                         mirror = 1;
18273                                                 }
18274                                         }
18275                                 }  
18276                                 delete cimage; cimage = 0;
18277                         }
18278                    }
18279         }
18280         //if ynumber is given, it should be even. We need to check whether it is zero
18281         else if(ynumber==0) {
18282                 sy = 0.0f;
18283                 for (int j = -kx; j <= kx; j++) {
18284                         ix = j*step ;
18285                         EMData* cimage = Polar2Dm(image, cnx+ix, cny, numr, mode);
18286 
18287                         Normalize_ring( cimage, numr );
18288 
18289                         Frngs(cimage, numr);
18290                         //  compare with all reference images
18291                         // for iref in xrange(len(crefim)):
18292                         for ( iref = 0; iref < (int)crefim_len; iref++)  {
18293                                 Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18294                                 double qn = retvals["qn"];
18295                                 double qm = retvals["qm"];
18296                                 if(qn >= peak || qm >= peak) {
18297                                         sx = -ix;
18298                                         nref = iref;
18299                                         if (qn >= qm) {
18300                                                 ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18301                                                 peak = static_cast<float>(qn);
18302                                                 mirror = 0;
18303                                         } else {
18304                                                 ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18305                                                 peak = static_cast<float>(qm);
18306                                                 mirror = 1;
18307                                         }
18308                                 }
18309                         } 
18310                         delete cimage; cimage = 0;
18311                 }                       
18312         } else {
18313                 int   ky = int(ynumber/2);              
18314                 float stepy=2*yrng/ynumber;
18315                 //std::cout<<"yrng="<<yrng<<"ynumber="<<ynumber<<"stepy=="<<stepy<<"stepx=="<<step<<std::endl;
18316                 for (int i = -ky+1; i <= ky; i++) {
18317                         iy = i * stepy ;
18318                         for (int j = -kx; j <= kx; j++) {
18319                                 ix = j*step ;
18320                                 EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18321 
18322                                 Normalize_ring( cimage, numr );
18323 
18324                                 Frngs(cimage, numr);
18325                                 //  compare with all reference images
18326                                 // for iref in xrange(len(crefim)):
18327                                 for ( iref = 0; iref < (int)crefim_len; iref++) {
18328                                         Dict retvals = Crosrng_psi_0_180(crefim[iref], cimage, numr, psi_max);
18329                                         double qn = retvals["qn"];
18330                                         double qm = retvals["qm"];
18331                                         if(qn >= peak || qm >= peak) {
18332                                                 sx = -ix;
18333                                                 sy = -iy;
18334                                                 nref = iref;
18335                                                 if (qn >= qm) {
18336                                                         ang = ang_n(retvals["tot"], mode, numr[numr.size()-1]);
18337                                                         peak = static_cast<float>(qn);
18338                                                         mirror = 0;
18339                                                 } else {
18340                                                         ang = ang_n(retvals["tmt"], mode, numr[numr.size()-1]);
18341                                                         peak = static_cast<float>(qm);
18342                                                         mirror = 1;
18343                                                 }
18344                                         }
18345                                 }
18346                                 delete cimage; cimage = 0;
18347                         }
18348                 }
18349         }
18350         float co, so, sxs, sys;
18351         co = static_cast<float>( cos(ang*pi/180.0) );
18352         so = static_cast<float>( -sin(ang*pi/180.0) );
18353         sxs = sx*co - sy*so;
18354         sys = sx*so + sy*co;
18355         vector<float> res;
18356         res.push_back(ang);
18357         res.push_back(sxs);
18358         res.push_back(sys);
18359         res.push_back(static_cast<float>(mirror));
18360         res.push_back(static_cast<float>(nref));
18361         res.push_back(peak);
18362         return res;
18363 }
18364 
18365 void  Util::multiref_peaks_ali2d(EMData* image, EMData* crefim,
18366                         float xrng, float yrng, float step, string mode,
18367                         vector< int >numr, float cnx, float cny,
18368                         EMData *peaks, EMData *peakm) {
18369 
18370         int   maxrin = numr[numr.size()-1];
18371 
18372         int   ky = int(2*yrng/step+0.5)/2;
18373         int   kx = int(2*xrng/step+0.5)/2;
18374 
18375         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18376         float *p_ccf1ds = peaks->get_data();
18377 
18378         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18379         float *p_ccf1dm = peakm->get_data();
18380 
18381         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18382                 p_ccf1ds[i] = -1.e20f;
18383                 p_ccf1dm[i] = -1.e20f;
18384         }
18385 
18386         for (int i = -ky; i <= ky; i++) {
18387                 float iy = i * step;
18388                 for (int j = -kx; j <= kx; j++) {
18389                         float ix = j*step;
18390                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18391                         Frngs(cimage, numr);
18392                         Crosrng_msg_vec(crefim, cimage, numr,
18393                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18394                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18395                         delete cimage; cimage = 0;
18396                 }
18397         }
18398         return;
18399 }
18400 
18401 void  Util::multiref_peaks_compress_ali2d(EMData* image, EMData* crefim, float xrng, float yrng,
18402      float step, string mode, vector<int>numr, float cnx, float cny, EMData *peaks, EMData *peakm,
18403      EMData *peaks_compress, EMData *peakm_compress) {
18404 
18405         int   maxrin = numr[numr.size()-1];
18406 
18407         int   ky = int(2*yrng/step+0.5)/2;
18408         int   kx = int(2*xrng/step+0.5)/2;
18409 
18410         peaks->set_size(maxrin, 2*kx+3, 2*ky+3);
18411         float *p_ccf1ds = peaks->get_data();
18412 
18413         peakm->set_size(maxrin, 2*kx+3, 2*ky+3);
18414         float *p_ccf1dm = peakm->get_data();
18415 
18416         peaks_compress->set_size(maxrin, 1, 1);
18417         float *p_ccf1ds_compress = peaks_compress->get_data();
18418 
18419         peakm_compress->set_size(maxrin, 1, 1);
18420         float *p_ccf1dm_compress = peakm_compress->get_data();
18421 
18422         for ( int i = 0; i<maxrin*(2*kx+3)*(2*ky+3); i++) {
18423                 p_ccf1ds[i] = -1.e20f;
18424                 p_ccf1dm[i] = -1.e20f;
18425         }
18426 
18427         for (int i = -ky; i <= ky; i++) {
18428                 float iy = i * step;
18429                 for (int j = -kx; j <= kx; j++) {
18430                         float ix = j*step;
18431                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18432                         Frngs(cimage, numr);
18433                         Crosrng_msg_vec(crefim, cimage, numr,
18434                           p_ccf1ds+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin,
18435                           p_ccf1dm+(j+kx+1+((i+ky+1)*(2*kx+3)))*maxrin);
18436                         delete cimage; cimage = 0;
18437                 }
18438         }
18439         for (int x=0; x<maxrin; x++) {
18440                 float maxs = -1.0e22f;
18441                 float maxm = -1.0e22f;
18442                 for (int i=1; i<=2*ky+1; i++) {
18443                         for (int j=1; j<=2*kx+1; j++) {
18444                                 if (p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x] > maxs) maxs = p_ccf1ds[(i*(2*kx+3)+j)*maxrin+x];
18445                                 if (p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x] > maxm) maxm = p_ccf1dm[(i*(2*kx+3)+j)*maxrin+x];
18446                         }
18447                 }
18448                 p_ccf1ds_compress[x] = maxs;
18449                 p_ccf1dm_compress[x] = maxm;
18450         }
18451         return;
18452 }
18453 
18454 struct ccf_point
18455 {
18456     float value;
18457     int i;
18458     int j;
18459     int k;
18460     int mirror;
18461 };
18462 
18463 
18464 struct ccf_value
18465 {
18466     bool operator()( const ccf_point& a, const ccf_point& b )
18467     {
18468         return a.value > b.value;
18469     }
18470 };
18471 
18472 
18473 vector<float>  Util::ali2d_ccf_list(EMData* image, EMData* crefim,
18474                         float xrng, float yrng, float step, string mode,
18475                         vector< int >numr, float cnx, float cny, double T) {
18476 
18477         int   maxrin = numr[numr.size()-1];
18478 
18479         int   ky = int(2*yrng/step+0.5)/2;
18480         int   kx = int(2*xrng/step+0.5)/2;
18481 
18482         float *p_ccf1ds = (float *)malloc(maxrin*sizeof(float));
18483         float *p_ccf1dm = (float *)malloc(maxrin*sizeof(float));
18484         int vol = maxrin*(2*kx+1)*(2*ky+1);
18485         vector<ccf_point> ccf(2*vol);
18486         ccf_point temp;
18487 
18488         int index = 0;
18489         for (int i = -ky; i <= ky; i++) {
18490                 float iy = i * step;
18491                 for (int j = -kx; j <= kx; j++) {
18492                         float ix = j*step;
18493                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18494                         Frngs(cimage, numr);
18495                         Crosrng_msg_vec(crefim, cimage, numr, p_ccf1ds, p_ccf1dm);
18496                         for (int k=0; k<maxrin; k++) {
18497                                 temp.value = p_ccf1ds[k];
18498                                 temp.i = k;
18499                                 temp.j = j;
18500                                 temp.k = i;
18501                                 temp.mirror = 0;
18502                                 ccf[index] = temp;
18503                                 index++;
18504                                 temp.value = p_ccf1dm[k];
18505                                 temp.mirror = 1;
18506                                 ccf[index] = temp;
18507                                 index++;
18508                         }
18509                         delete cimage; cimage = 0;
18510                 }
18511         }
18512 
18513         delete p_ccf1ds;
18514         delete p_ccf1dm;
18515         std::sort(ccf.begin(), ccf.end(), ccf_value());
18516 
18517         double qt = (double)ccf[0].value;
18518         vector <double> p(2*vol), cp(2*vol);
18519 
18520         double sump = 0.0;
18521         for (int i=0; i<2*vol; i++) {
18522                 p[i] = pow(double(ccf[i].value)/qt, 1.0/T);
18523                 sump += p[i];
18524         }
18525         for (int i=0; i<2*vol; i++) {
18526                 p[i] /= sump;
18527         }
18528         for (int i=1; i<2*vol; i++) {
18529                 p[i] += p[i-1];
18530         }
18531         p[2*vol-1] = 2.0;
18532 
18533         float t = get_frand(0.0f, 1.0f);
18534         int select = 0;
18535         while (p[select] < t)   select += 1;
18536 
18537         vector<float> a(6);
18538         a[0] = ccf[select].value;
18539         a[1] = (float)ccf[select].i;
18540         a[2] = (float)ccf[select].j;
18541         a[3] = (float)ccf[select].k;
18542         a[4] = (float)ccf[select].mirror;
18543         a[5] = (float)select;
18544         return a;
18545 }
18546 
18547 
18548 /*
18549 void Util::multiref_peaks_ali(EMData* image, const vector< EMData* >& crefim,
18550                         float xrng, float yrng, float step, string mode,
18551                         vector< int >numr, float cnx, float cny,
18552                         EMData *peaks, EMData *peakm, int nphi, int ntheta) {
18553 
18554 // formerly known as apmq
18555     // Determine shift and rotation between image and many reference
18556     // images (crefim, weights have to be applied) quadratic
18557     // interpolation
18558 
18559 
18560     // Manually extract.
18561 *//*    vector< EMAN::EMData* > crefim;
18562     std::size_t crefim_len = PyObject_Length(crefim_list.ptr());
18563     crefim.reserve(crefim_len);
18564 
18565     for(std::size_t i=0;i<crefim_len;i++) {
18566         boost::python::extract<EMAN::EMData*> proxy(crefim_list[i]);
18567         crefim.push_back(proxy());
18568     }
18569 */
18570 /*
18571         int   maxrin = numr[numr.size()-1];
18572 
18573         size_t crefim_len = crefim.size();
18574 
18575         int   iref;
18576         int   ky = int(2*yrng/step+0.5)/2;
18577         int   kx = int(2*xrng/step+0.5)/2;
18578         int   tkx = 2*kx+3;
18579         int   tky = 2*ky+3;
18580 
18581         peaks->set_size(maxrin, nphi, ntheta, tkx, tky);
18582         float *p_ccf1ds = peaks->get_data();
18583 
18584 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k+(l*nz))*ny))*nx]
18585 //#define img_ptr(i,j,k,l)  img_ptr[i+(j+(k*ny))*nx]
18586         peakm->set_size(maxrin, nphi, ntheta, tkx, tky);
18587         float *p_ccf1dm = peakm->get_data();
18588 
18589         for ( int i = 0; i<maxrin*(int)crefim_len*(2*kx+3)*(2*ky+3); i++) {
18590                 p_ccf1ds[i] = -1.e20f;
18591                 p_ccf1dm[i] = -1.e20f;
18592         }
18593 
18594         float  iy, ix;
18595         for (int i = -ky; i <= ky; i++) {
18596                 iy = i * step ;
18597                 for (int j = -kx; j <= kx; j++) {
18598                         ix = j*step ;
18599                         EMData* cimage = Polar2Dm(image, cnx+ix, cny+iy, numr, mode);
18600                         Frngs(cimage, numr);
18601                         //  compare with all reference images
18602                         // for iref in xrange(len(crefim)):
18603                         for ( iref = 0; iref < (int)crefim_len; iref++) {
18604                                 Crosrng_msg_vec(crefim[iref], cimage, numr,
18605                                         p_ccf1ds+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin,
18606                                         p_ccf1dm+(iref + (j+kx+1+((i+ky+1)*tkx))*(int)crefim_len )*maxrin);
18607                         }
18608                         delete cimage; cimage = 0;
18609                 }
18610         }
18611         return;
18612 }
18613 */
18614 vector<float> Util::twoD_fine_ali(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18615 
18616         EMData *rot;
18617 
18618         const int nmax=3, mmax=3;
18619         char task[60], csave[60];
18620         long int lsave[4];
18621         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18622         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];
18623         long int SIXTY=60;
18624 
18625         //     We wish to have no output.
18626         iprint = -1;
18627 
18628         //c     We specify the tolerances in the stopping criteria.
18629         factr=1.0e1;
18630         pgtol=1.0e-5;
18631 
18632         //     We specify the dimension n of the sample problem and the number
18633         //        m of limited memory corrections stored.  (n and m should not
18634         //        exceed the limits nmax and mmax respectively.)
18635         n=3;
18636         m=3;
18637 
18638         //     We now provide nbd which defines the bounds on the variables:
18639         //                    l   specifies the lower bounds,
18640         //                    u   specifies the upper bounds.
18641         //                    x   specifies the initial guess
18642         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18643         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18644         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18645 
18646 
18647         //     We start the iteration by initializing task.
18648         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18649         strcpy(task,"START");
18650         for (int i=5;i<60;i++)  task[i]=' ';
18651 
18652         //     This is the call to the L-BFGS-B code.
18653         // (* call the L-BFGS-B routine with task='START' once before loop *)
18654         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18655         //int step = 1;
18656 
18657         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18658         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18659 
18660                 if (strncmp(task,"FG",2)==0) {
18661                 //   the minimization routine has returned to request the
18662                 //   function f and gradient g values at the current x
18663 
18664                 //        Compute function value f for the sample problem.
18665                 rot = new EMData();
18666                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2], 1.0f);
18667                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18668                 //f = -f;
18669                 delete rot;
18670 
18671                 //        Compute gradient g for the sample problem.
18672                 float dt = 1.0e-3f;
18673                 rot = new EMData();
18674                 rot = image->rot_scale_trans2D((float)x[0]+dt, (float)x[1], (float)x[2], 1.0f);
18675                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18676                 //f1 = -f1;
18677                 g[0] = (f1-f)/dt;
18678                 delete rot;
18679 
18680                 dt = 1.0e-2f;
18681                 rot = new EMData();
18682                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1]+dt, (float)x[2], 1.0f);
18683                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18684                 //f2 = -f2;
18685                 g[1] = (f2-f)/dt;
18686                 delete rot;
18687 
18688                 rot = new EMData();
18689                 rot = image->rot_scale_trans2D((float)x[0], (float)x[1], (float)x[2]+dt, 1.0f);
18690                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18691                 //f3 = -f3;
18692                 g[2] = (f3-f)/dt;
18693                 delete rot;
18694                 }
18695 
18696                 //c          go back to the minimization routine.
18697                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18698                 //step++;
18699         }
18700 
18701         //printf("Total step is %d\n", step);
18702         vector<float> res;
18703         res.push_back(static_cast<float>(x[0]));
18704         res.push_back(static_cast<float>(x[1]));
18705         res.push_back(static_cast<float>(x[2]));
18706         //res.push_back(step);
18707         return res;
18708 }
18709 
18710 vector<float> Util::twoD_fine_ali_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
18711 
18712         EMData *rot;
18713 
18714         const int nmax=3, mmax=3;
18715         char task[60], csave[60];
18716         long int lsave[4];
18717         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18718         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];
18719         long int SIXTY=60;
18720 
18721         //     We wish to have no output.
18722         iprint = -1;
18723 
18724         //c     We specify the tolerances in the stopping criteria.
18725         factr=1.0e1;
18726         pgtol=1.0e-5;
18727 
18728         //     We specify the dimension n of the sample problem and the number
18729         //        m of limited memory corrections stored.  (n and m should not
18730         //        exceed the limits nmax and mmax respectively.)
18731         n=3;
18732         m=3;
18733 
18734         //     We now provide nbd which defines the bounds on the variables:
18735         //                    l   specifies the lower bounds,
18736         //                    u   specifies the upper bounds.
18737         //                    x   specifies the initial guess
18738         x[0] = ang; nbd[0] = 2; l[0] = ang-2.0; u[0] = ang+2.0;
18739         x[1] = sxs; nbd[1] = 2; l[1] = sxs-1.5; u[1] = sxs+1.5;
18740         x[2] = sys; nbd[2] = 2; l[2] = sys-1.5; u[2] = sys+1.5;
18741 
18742 
18743         //     We start the iteration by initializing task.
18744         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18745         strcpy(task,"START");
18746         for (int i=5;i<60;i++)  task[i]=' ';
18747 
18748         //     This is the call to the L-BFGS-B code.
18749         // (* call the L-BFGS-B routine with task='START' once before loop *)
18750         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18751         //int step = 1;
18752 
18753         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18754         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18755 
18756                 if (strncmp(task,"FG",2)==0) {
18757                 //   the minimization routine has returned to request the
18758                 //   function f and gradient g values at the current x
18759 
18760                 //        Compute function value f for the sample problem.
18761                 rot = new EMData();
18762                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18763                 f = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18764                 //f = -f;
18765                 delete rot;
18766 
18767                 //        Compute gradient g for the sample problem.
18768                 float dt = 1.0e-3f;
18769                 rot = new EMData();
18770                 rot = image->rot_scale_conv7((float)((x[0]+dt)*pi/180), (float)x[1], (float)x[2], kb, 1.0f);
18771                 f1 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18772                 //f1 = -f1;
18773                 g[0] = (f1-f)/dt;
18774                 delete rot;
18775 
18776                 rot = new EMData();
18777                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1]+dt, (float)x[2], kb, 1.0);
18778                 f2 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18779                 //f2 = -f2;
18780                 g[1] = (f2-f)/dt;
18781                 delete rot;
18782 
18783                 rot = new EMData();
18784                 rot = image->rot_scale_conv7((float)(x[0]*pi/180), (float)x[1], (float)x[2]+dt, kb, 1.0f);
18785                 f3 = rot->cmp("sqeuclidean", refim, Dict("mask", mask));
18786                 //f3 = -f3;
18787                 g[2] = (f3-f)/dt;
18788                 delete rot;
18789                 }
18790 
18791                 //c          go back to the minimization routine.
18792                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18793                 //step++;
18794         }
18795 
18796         //printf("Total step is %d\n", step);
18797         vector<float> res;
18798         res.push_back(static_cast<float>(x[0]));
18799         res.push_back(static_cast<float>(x[1]));
18800         res.push_back(static_cast<float>(x[2]));
18801         //res.push_back(step);
18802         return res;
18803 }
18804 
18805 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) {
18806 
18807         EMData *proj, *proj2;
18808 
18809         const int nmax=5, mmax=5;
18810         char task[60], csave[60];
18811         long int lsave[4];
18812         long int n, m, iprint, nbd[nmax], iwa[3*nmax], isave[44];
18813         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];
18814         long int SIXTY=60;
18815 
18816         //     We wish to have no output.
18817         iprint = -1;
18818 
18819         //c     We specify the tolerances in the stopping criteria.
18820         factr=1.0e1;
18821         pgtol=1.0e-5;
18822 
18823         //     We specify the dimension n of the sample problem and the number
18824         //        m of limited memory corrections stored.  (n and m should not
18825         //        exceed the limits nmax and mmax respectively.)
18826         n=5;
18827         m=5;
18828 
18829         //     We now provide nbd which defines the bounds on the variables:
18830         //                    l   specifies the lower bounds,
18831         //                    u   specifies the upper bounds.
18832         //                    x   specifies the initial guess
18833         x[0] = phi;     nbd[0] = 2;     l[0] = phi-2.0;         u[0] = phi+2.0;
18834         x[1] = theta;   nbd[1] = 2;     l[1] = theta-2.0;       u[1] = theta+2.0;
18835         x[2] = psi;     nbd[2] = 2;     l[2] = psi-2.0;         u[2] = psi+2.0;
18836         x[3] = sxs;     nbd[3] = 2;     l[3] = sxs-2.0;         u[3] = sxs+2.0;
18837         x[4] = sys;     nbd[4] = 2;     l[4] = sys-2.0;         u[4] = sys+2.0;
18838 
18839 
18840         //     We start the iteration by initializing task.
18841         // (**MUST clear remaining chars in task with spaces (else crash)!**)
18842         strcpy(task,"START");
18843         for (int i=5;i<60;i++)  task[i]=' ';
18844 
18845         //     This is the call to the L-BFGS-B code.
18846         // (* call the L-BFGS-B routine with task='START' once before loop *)
18847         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18848         int step = 1;
18849 
18850         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
18851         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
18852 
18853                 if (strncmp(task,"FG",2)==0) {
18854                 //   the minimization routine has returned to request the
18855                 //   function f and gradient g values at the current x
18856 
18857                 //        Compute function value f for the sample problem.
18858                 proj = new EMData();
18859                 proj2 = new EMData();
18860                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18861                 proj->fft_shuffle();
18862                 proj->center_origin_fft();
18863                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18864                 proj->do_ift_inplace();
18865                 int M = proj->get_ysize()/2;
18866                 proj2 = proj->window_center(M);
18867                 f = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18868                 //f = -f;
18869                 delete proj;
18870                 delete proj2;
18871 
18872                 //        Compute gradient g for the sample problem.
18873                 float dt = 1.0e-3f;
18874                 proj = new EMData();
18875                 proj2 = new EMData();
18876                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0]+dt, "theta", (float)x[1], "psi", (float)x[2])), kb);
18877                 proj->fft_shuffle();
18878                 proj->center_origin_fft();
18879                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18880                 proj->do_ift_inplace();
18881                 proj2 = proj->window_center(M);
18882                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18883                 //ft = -ft;
18884                 delete proj;
18885                 delete proj2;
18886                 g[0] = (ft-f)/dt;
18887 
18888                 proj = new EMData();
18889                 proj2 = new EMData();
18890                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1]+dt, "psi", (float)x[2])), kb);
18891                 proj->fft_shuffle();
18892                 proj->center_origin_fft();
18893                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18894                 proj->do_ift_inplace();
18895                 proj2 = proj->window_center(M);
18896                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18897                 //ft = -ft;
18898                 delete proj;
18899                 delete proj2;
18900                 g[1] = (ft-f)/dt;
18901 
18902                 proj = new EMData();
18903                 proj2 = new EMData();
18904                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2]+dt)), kb);
18905                 proj->fft_shuffle();
18906                 proj->center_origin_fft();
18907                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4], "z_shift", 0.0f));
18908                 proj->do_ift_inplace();
18909                 proj2 = proj->window_center(M);
18910                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18911                 //ft = -ft;
18912                 delete proj;
18913                 delete proj2;
18914                 g[2] = (ft-f)/dt;
18915 
18916                 proj = new EMData();
18917                 proj2 = new EMData();
18918                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18919                 proj->fft_shuffle();
18920                 proj->center_origin_fft();
18921                 proj->process_inplace("filter.shift", Dict("x_shift", x[3]+dt, "y_shift", x[4], "z_shift", 0.0f));
18922                 proj->do_ift_inplace();
18923                 proj2 = proj->window_center(M);
18924                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18925                 //ft = -ft;
18926                 delete proj;
18927                 delete proj2;
18928                 g[3] = (ft-f)/dt;
18929 
18930                 proj = new EMData();
18931                 proj2 = new EMData();
18932                 proj = volft->extract_plane(Transform(Dict("type", "SPIDER", "phi", (float)x[0], "theta", (float)x[1], "psi", (float)x[2])), kb);
18933                 proj->fft_shuffle();
18934                 proj->center_origin_fft();
18935                 proj->process_inplace("filter.shift", Dict("x_shift", x[3], "y_shift", x[4]+dt, "z_shift", 0.0f));
18936                 proj->do_ift_inplace();
18937                 proj2 = proj->window_center(M);
18938                 ft = proj2->cmp("sqeuclidean", refim, Dict("mask", mask));
18939                 //ft = -ft;
18940                 delete proj;
18941                 delete proj2;
18942                 g[4] = (ft-f)/dt;
18943                 }
18944 
18945                 //c          go back to the minimization routine.
18946                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
18947                 step++;
18948         }
18949 
18950         //printf("Total step is %d\n", step);
18951         vector<float> res;
18952         res.push_back(static_cast<float>(x[0]));
18953         res.push_back(static_cast<float>(x[1]));
18954         res.push_back(static_cast<float>(x[2]));
18955         res.push_back(static_cast<float>(x[3]));
18956         res.push_back(static_cast<float>(x[4]));
18957         //res.push_back(step);
18958         return res;
18959 }
18960 
18961 
18962 vector<float> Util::twoD_fine_ali_SD(EMData* image, EMData *refim, EMData* mask, float ang, float sxs, float sys) {
18963 
18964         double  x[4];
18965         int n;
18966         int l = 3;
18967         int m = 200;
18968         double e = 1e-9;
18969         double step = 0.01;
18970         float (*my_func)(EMData* , EMData* , EMData* , float , float , float) = ccc_images;
18971 
18972         x[1] = ang;
18973         x[2] = sxs;
18974         x[3] = sys;
18975 
18976         Steepda(x, step, e, l, m, &n, my_func, image, refim, mask);   // Call steepest descent optimization subroutine
18977         //printf("Took %d steps\n", n);
18978 
18979         vector<float> res;
18980         res.push_back(static_cast<float>(x[1]));
18981         res.push_back(static_cast<float>(x[2]));
18982         res.push_back(static_cast<float>(x[3]));
18983         res.push_back(static_cast<float>(n));
18984         return res;
18985 }
18986 
18987 vector<float> Util::multi_align_error(vector<float> args, vector<float> all_ali_params) {
18988         
18989         const int nmax=args.size(), mmax=nmax;
18990         char task[60], csave[60];
18991         long int lsave[4];
18992         long int n, m, iprint, isave[44];
18993         long int* nbd = new long int[nmax];
18994         long int* iwa = new long int[3*nmax];
18995         double f, factr, pgtol;
18996         double* x = new double[nmax];
18997         double* l = new double[nmax];
18998         double* u = new double[nmax];
18999         double* g = new double[nmax];
19000         double dsave[29];
19001         double* wa = new double[2*mmax*nmax+4*nmax+12*mmax*mmax+12*mmax];
19002         long int SIXTY=60;
19003 
19004         int num_ali = nmax/3+1;
19005         int nima = all_ali_params.size()/(num_ali*4);
19006         
19007         //     We wish to have no output.
19008         iprint = -1;
19009 
19010         //c     We specify the tolerances in the stopping criteria.
19011         factr=1.0e1;
19012         pgtol=1.0e-9;
19013 
19014         //     We specify the dimension n of the sample problem and the number
19015         //        m of limited memory corrections stored.  (n and m should not
19016         //        exceed the limits nmax and mmax respectively.)
19017         n=nmax;
19018         m=mmax;
19019 
19020         //     We now provide nbd which defines the bounds on the variables:
19021         //                    l   specifies the lower bounds,
19022         //                    u   specifies the upper bounds.
19023         //                    x   specifies the initial guess
19024         for (int i=0; i<nmax; i++) {
19025                 x[i] = args[i]; 
19026                 nbd[i] = 0;
19027         }
19028         
19029         //     We start the iteration by initializing task.
19030         // (**MUST clear remaining chars in task with spaces (else crash)!**)
19031         strcpy(task,"START");
19032         for (int i=5;i<60;i++)  task[i]=' ';
19033 
19034         //     This is the call to the L-BFGS-B code.
19035         // (* call the L-BFGS-B routine with task='START' once before loop *)
19036         setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19037         int step = 1;
19038 
19039         // (* while routine returns "FG" or "NEW_X" in task, keep calling it *)
19040         while (strncmp(task,"FG",2)==0 || strncmp(task,"NEW_X",5)==0) {
19041 
19042                 if (strncmp(task,"FG",2)==0) {
19043                 //   the minimization routine has returned to request the
19044                 //   function f and gradient g values at the current x
19045 
19046                 //        Compute function value f for the sample problem.
19047                 f = multi_align_error_func(x, all_ali_params, nima, num_ali);
19048 
19049                 //        Compute gradient g for the sample problem.
19050                 multi_align_error_dfunc(x, all_ali_params, nima, num_ali, g);
19051                 }
19052 
19053                 //c          go back to the minimization routine.
19054                 setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave,SIXTY,SIXTY);
19055                 step++;
19056         }
19057 
19058         //printf("Total step is %d\n", step);
19059         vector<float> res;
19060         for (int i=0; i<nmax; i++) res.push_back(static_cast<float>(x[i]));
19061         res.push_back(f);
19062         
19063         delete[] nbd;
19064         delete[] iwa;
19065         delete[] x;
19066         delete[] l;
19067         delete[] u;
19068         delete[] g;
19069         delete[] wa;
19070         
19071         return res;
19072 
19073 }
19074 
19075 float Util::multi_align_error_func(double* x, vector<float> all_ali_params, int nima, int num_ali) {
19076 
19077         float x1 = 1.0;
19078         float y1 = 0.0;
19079         float x2 = 0.0;
19080         float y2 = 1.0;
19081 
19082         float all_var = 0;
19083         float* x1_new = new float[num_ali];
19084         float* y1_new = new float[num_ali];
19085         float* x2_new = new float[num_ali];
19086         float* y2_new = new float[num_ali];
19087 
19088         for (int i=0; i<nima; i++) {
19089                 float alpha2 = all_ali_params[(num_ali-1)*(nima*4)+i*4];
19090                 float sx2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+1];
19091                 float sy2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+2];
19092                 
19093                 rot_shift(x1, y1, alpha2, sx2, sy2, x1_new+num_ali-1, y1_new+num_ali-1);
19094                 rot_shift(x2, y2, alpha2, sx2, sy2, x2_new+num_ali-1, y2_new+num_ali-1);
19095                 for (int j=0; j<num_ali-1; j++) {
19096                         float alpha1 = all_ali_params[j*(nima*4)+i*4];
19097                         float sx1 = all_ali_params[j*(nima*4)+i*4+1];
19098                         float sy1 = all_ali_params[j*(nima*4)+i*4+2];
19099                         int mirror1 = static_cast<int>(all_ali_params[j*(nima*4)+i*4+3]);
19100 
19101                         float alphai = x[j*3];
19102                         float sxi = x[j*3+1];
19103                         float syi = x[j*3+2];
19104 
19105                         float alpha12, sx12, sy12;
19106                         int mirror12;
19107                         if (mirror1 == 0) {
19108                                 alpha12 = fmod(alpha1+alphai, 360.0f);
19109                                 rot_shift(sx1, sy1, alphai, sxi, syi, &sx12, &sy12);
19110                                 mirror12 = 0;
19111                         } else {
19112                                 alpha12 = fmod(alpha1-alphai, 360.0f);
19113                                 rot_shift(sx1, sy1, -alphai, -sxi, syi, &sx12, &sy12);
19114                                 mirror12 = 1;
19115                         }
19116 
19117                         rot_shift(x1, y1, alpha12, sx12, sy12, x1_new+j, y1_new+j);
19118                         rot_shift(x2, y2, alpha12, sx12, sy12, x2_new+j, y2_new+j);
19119                 }
19120                 
19121                 float p = var(x1_new, num_ali)+var(y1_new, num_ali)+var(x2_new, num_ali)+var(y2_new, num_ali);
19122                 all_var += p;
19123         }
19124         delete[] x1_new;
19125         delete[] y1_new;
19126         delete[] x2_new;
19127         delete[] y2_new;
19128         return all_var/static_cast<float>(nima);
19129 }
19130 
19131 void Util::multi_align_error_dfunc(double* x, vector<float> all_ali_params, int nima, int num_ali, double* g) {
19132 
19133         
19134         for (int i=0; i<num_ali*3-3; i++) g[i] = 0.0;
19135         
19136         float x1 = 1.0;
19137         float y1 = 0.0;
19138         float x2 = 0.0;
19139         float y2 = 1.0;
19140 
19141         float* x1_new = new float[num_ali];
19142         float* y1_new = new float[num_ali];
19143         float* x2_new = new float[num_ali];
19144         float* y2_new = new float[num_ali];
19145 
19146         float* alpha12_0 = new float[num_ali-1];
19147         float* dalpha12 = new float[num_ali-1];
19148         float* dsx12 = new float[num_ali-1];
19149         float* dsy12 = new float[num_ali-1];
19150         float* mirror1_0 = new float[num_ali-1];
19151 
19152         for (int i=0; i<nima; i++) {
19153                 
19154                 float alpha2 = all_ali_params[(num_ali-1)*(nima*4)+i*4];
19155                 float sx2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+1];
19156                 float sy2 = all_ali_params[(num_ali-1)*(nima*4)+i*4+2];
19157                 
19158                 rot_shift(x1, y1, alpha2, sx2, sy2, x1_new+num_ali-1, y1_new+num_ali-1);
19159                 rot_shift(x2, y2, alpha2, sx2, sy2, x2_new+num_ali-1, y2_new+num_ali-1);
19160                 
19161                 for (int j=0; j<num_ali-1; j++) {
19162                         float alpha1 = all_ali_params[j*(nima*4)+i*4];
19163                         float sx1 = all_ali_params[j*(nima*4)+i*4+1];
19164                         float sy1 = all_ali_params[j*(nima*4)+i*4+2];
19165                         int mirror1 = static_cast<int>(all_ali_params[j*(nima*4)+i*4+3]);
19166 
19167                         float alphai = x[j*3];
19168                         float sxi = x[j*3+1];
19169                         float syi = x[j*3+2];
19170 
19171                         float cosi = cos(alphai/180.0f*M_PI);
19172                         float sini = sin(alphai/180.0f*M_PI);
19173                         
19174                         float alpha12, sx12, sy12;
19175                         int mirror12;
19176                         if (mirror1 == 0) {
19177                                 alpha12 = fmod(alpha1+alphai, 360.0f);
19178                                 rot_shift(sx1, sy1, alphai, sxi, syi, &sx12, &sy12);
19179                                 mirror12 = 0;
19180                         } else {
19181                                 alpha12 = fmod(alpha1-alphai, 360.0f);
19182                                 rot_shift(sx1, sy1, -alphai, -sxi, syi, &sx12, &sy12);
19183                                 mirror12 = 1;
19184                         }
19185 
19186                         rot_shift(x1, y1, alpha12, sx12, sy12, x1_new+j, y1_new+j);
19187                         rot_shift(x2, y2, alpha12, sx12, sy12, x2_new+j, y2_new+j);
19188                 
19189                         alpha12_0[j] = alpha12;
19190                         mirror1_0[j] = mirror1;
19191                         if (mirror1 == 0) {
19192                                 dalpha12[j] = M_PI/180.0f;
19193                                 dsx12[j] = (-sini*sx1+cosi*sy1)/180.0f*M_PI;
19194                                 dsy12[j] = (-cosi*sx1-sini*sy1)/180.0f*M_PI;
19195                         } else {
19196                                 dalpha12[j] = -M_PI/180.0f;
19197                                 dsx12[j] = (sini*(-sx1)-cosi*sy1)/180.0f*M_PI;
19198                                 dsy12[j] = (-cosi*(-sx1)-sini*sy1)/180.0f*M_PI;
19199                         }
19200                 }
19201 
19202                 for (int j=0; j<num_ali-1; j++) {
19203                         float cosa = cos(alpha12_0[j]/180.0f*M_PI);
19204                         float sina = sin(alpha12_0[j]/180.0f*M_PI);
19205                         float diffx1 = x1_new[j]-mean(x1_new, num_ali);
19206                         float diffx2 = x2_new[j]-mean(x2_new, num_ali);
19207                         float diffy1 = y1_new[j]-mean(y1_new, num_ali);
19208                         float diffy2 = y2_new[j]-mean(y2_new, num_ali);
19209 
19210                         float p = diffx1*((-x1*sina+y1*cosa)*dalpha12[j]+dsx12[j])+diffx2*((-x2*sina+y2*cosa)*dalpha12[j]+dsx12[j])+diffy1*((-x1*cosa-y1*sina)*dalpha12[j]+dsy12[j])+diffy2*((-x2*cosa-y2*sina)*dalpha12[j]+dsy12[j]);
19211                         g[j*3] += p;
19212                 
19213                         p = diffx1+diffx2;
19214                         if (mirror1_0[j] == 0) g[j*3+1] += p;
19215                         else g[j*3+1] -= p;
19216 
19217                         p = diffy1+diffy2;
19218                         g[j*3+2] += p;
19219                 }
19220         }
19221 
19222         delete[] x1_new;
19223         delete[] y1_new;
19224         delete[] x2_new;
19225         delete[] y2_new;
19226         delete[] alpha12_0;
19227         delete[] dalpha12;
19228         delete[] dsx12;
19229         delete[] dsy12;
19230         delete[] mirror1_0;
19231         
19232 }
19233 
19234 float Util::ccc_images(EMData* image, EMData* refim, EMData* mask, float ang, float sx, float sy) {
19235 
19236         EMData *rot= new EMData();
19237         float ccc;
19238 
19239         rot = image->rot_scale_trans2D(ang, sx, sy, 1.0);
19240         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19241         delete rot;
19242         return ccc;
19243 }
19244 
19245 vector<float> Util::twoD_fine_ali_SD_G(EMData* image, EMData *refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sxs, float sys) {
19246 
19247         double  x[4];
19248         int n;
19249         int l = 3;
19250         int m = 200;
19251         double e = 1e-9;
19252         double step = 0.001;
19253         float (*my_func)(EMData* , EMData* , EMData* , Util::KaiserBessel&, float , float , float) = ccc_images_G;
19254 
19255         x[1] = ang;
19256         x[2] = sxs;
19257         x[3] = sys;
19258 
19259         Steepda_G(x, step, e, l, m, &n, my_func, image, refim, mask, kb);   // Call steepest descent optimization subroutine
19260         //printf("Took %d steps\n", n);
19261 
19262         vector<float> res;
19263         res.push_back(static_cast<float>(x[1]));
19264         res.push_back(static_cast<float>(x[2]));
19265         res.push_back(static_cast<float>(x[3]));
19266         res.push_back(static_cast<float>(n));
19267         return res;
19268 }
19269 
19270 
19271 float Util::ccc_images_G(EMData* image, EMData* refim, EMData* mask, Util::KaiserBessel& kb, float ang, float sx, float sy) {
19272 
19273         EMData *rot= new EMData();
19274         float ccc;
19275 
19276         rot = image->rot_scale_conv7(static_cast<float>(ang*pi/180.0), sx, sy, kb, 1.0f);
19277         ccc = -rot->cmp("sqeuclidean", refim, Dict("mask", mask));
19278         delete rot;
19279         return ccc;
19280 }
19281 
19282 #define img_ptr(i,j,k)  img_ptr[i+(j+(k*ny))*nx]
19283 #define img2_ptr(i,j,k) img2_ptr[i+(j+(k*ny))*nx]
19284 EMData* Util::move_points(EMData* img, float qprob, int ri, int ro)
19285 {
19286         ENTERFUNC;
19287         /* Exception Handle */
19288         if (!img) {
19289                 throw NullPointerException("NULL input image");
19290         }
19291 
19292         int newx, newy, newz;
19293         bool  keep_going;
19294         cout << " entered   " <<endl;
19295         int nx=img->get_xsize(),ny=img->get_ysize(),nz=img->get_zsize();
19296         //int size = nx*ny*nz;
19297         EMData * img2 = new EMData();
19298         img2->set_size(nx,ny,nz);
19299         img2->to_zero();
19300         float *img_ptr  =img->get_data();
19301         float *img2_ptr = img2->get_data();
19302         int r2 = ro*ro;
19303         int r3 = r2*ro;
19304         int ri2 = ri*ri;
19305         int ri3 = ri2*ri;
19306 
19307         int n2 = nx/2;
19308 
19309         for (int k=-n2; k<=n2; k++) {           //cout << " k   "<<k <<endl;
19310                 float z2 = static_cast<float>(k*k);
19311                 for (int j=-n2; j<=n2; j++) {
19312                         float y2 = z2 + j*j;
19313                         if(y2 <= r2) {
19314                                                                                         //cout << "  j  "<<j <<endl;
19315 
19316                                 for (int i=-n2; i<=n2; i++) {
19317                                         float x2 = y2 + i*i;
19318                                         if(x2 <= r3) {
19319                                                                                         //cout << "  i  "<<i <<endl;
19320                                                 int ib = i+n2; int jb = j+n2; int kb = k+n2;
19321                                                 if(x2 >= ri3) {
19322                                                         //  this is the outer shell, here points can only vanish
19323                                                         if(img_ptr(ib,jb,kb) == 1.0f) {
19324                                                                 //cout << "  1  "<<ib <<endl;
19325                                                                 if(Util::get_frand(0.0f, 1.0f) > qprob){
19326                                                                         img2_ptr(ib,jb,kb) = 0.0f;
19327                                                                         keep_going = true;
19328                                                                 //cout << "  try  "<<ib <<endl;
19329                                                                         while(keep_going) {
19330                                                                                 newx = Util::get_irand(-ro,ro);
19331                                                                                 newy = Util::get_irand(-ro,ro);
19332                                                                                 newz = Util::get_irand(-ro,ro);
19333                                                                                 if(newx*newx+newy*newy+newz*newz <= r3) {
19334                                                                                         newx += n2; newy += n2; newz += n2;
19335                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
19336                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
19337                                                                                                 keep_going = false; }
19338                                                                                 }
19339                                                                         }
19340                                                                 }   else  img2_ptr(ib,jb,kb) = 1.0f;
19341                                                         }
19342                                                 }  else  {
19343                                                         // this is inner shell, the point can only move (or vanish, if all neighbors exist)
19344                                                         if(img_ptr(ib,jb,kb) == 1.0) {
19345                                                                 if(Util::get_frand(0.0f,1.0f) > qprob) {
19346                                                                         //  find out the number of neighbors
19347                                                                         float  numn = -1.0f;  // we already know the central one is 1
19348                                                                         for (newz = -1; newz <= 1; newz++)
19349                                                                                 for (newy = -1; newy <= 1; newy++)
19350                                                                                         for (newx = -1; newx <= 1; newx++)
19351                                                                                                 numn += img_ptr(ib+newx,jb+newy,kb+newz);
19352                                                                         img2_ptr(ib,jb,kb) = 0.0;
19353                                                                         if(numn == 26.0f) {
19354                                                                                 //  all neighbors exist, it has to vanish
19355                                                                                 keep_going = true;
19356                                                                                 while(keep_going) {
19357                                                                                         newx = Util::get_irand(-ro,ro);
19358                                                                                         newy = Util::get_irand(-ro,ro);
19359                                                                                         newz = Util::get_irand(-ro,ro);
19360                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
19361                                                                                                 newx += n2; newy += n2; newz += n2;
19362                                                                                                 if( img_ptr(newx,newy,newz) == 0.0f) {
19363                                                                                                         if(newx*newx+newy*newy+newz*newz < r3) {
19364                                                                                                                 if(newx*newx+newy*newy+newz*newz < r3) {
19365                                                                                                                         newx += n2; newy += n2; newz += n2;
19366                                                                                                                         if( img_ptr(newx,newy,newz) == 0.0f) {
19367                                                                                                                                 img2_ptr(newx,newy,newz) = 1.0f;
19368                                                                                                                                 keep_going = false; }
19369                                                                                                                 }
19370                                                                                                         }
19371                                                                                                 }
19372                                                                                         }
19373                                                                                 }
19374                                                                         }  else if(numn == 25.0f) {
19375                                                                                 // there is only one empty neighbor, move there
19376                                                                                 for (newz = -1; newz <= 1; newz++) {
19377                                                                                         for (newy = -1; newy <= 1; newy++) {
19378                                                                                                 for (newx = -1; newx <= 1; newx++) {
19379                                                                                                         if( newx != 0 && newy != 0 && newz != 0) {
19380                                                                                                                 if(img_ptr(newx+ib,newy+jb,newz+kb) == 0.0f) {
19381                                                                                                                         img2_ptr(newx+ib,newy+jb,newz+kb) = 1.0f;
19382                                                                                                                         }
19383                                                                                                         }
19384                                                                                                 }
19385                                                                                         }
19386                                                                                 }
19387                                                                         }  else {
19388                                                                                 //  more than one neighbor is zero, select randomly one and move there
19389                                                                                 keep_going = true;
19390                                                                                 while(keep_going) {
19391                                                                                         newx = Util::get_irand(-1,1);
19392                                                                                         newy = Util::get_irand(-1,1);
19393                                                                                         newz = Util::get_irand(-1,1);
19394                                                                                         if(newx != 0 && newy != 0 && newz != 0)  {
19395                                                                                                 if(img_ptr(ib+newx,jb+newy,kb+newz) == 0.0f) {
19396                                                                                                         img2_ptr(ib+newx,jb+newy,kb+newz) = 1.0f;//?????
19397                                                                                                         keep_going = false;
19398                                                                                                 }
19399                                                                                         }
19400                                                                                 }
19401                                                                         }
19402                                                                 }  else img2_ptr(ib,jb,kb) = 1.0f;
19403                                                         }
19404                                                 }
19405                                         }
19406                                 }
19407                         }
19408                 }
19409         }
19410         //for (int i=0;i<size;i++) img2_ptr[i] = img_ptr[i] + Util::get_irand(1,7);
19411         img2->update();
19412 
19413         EXITFUNC;
19414         return img2;
19415 }
19416 #undef img_ptr
19417 #undef img2_ptr
19418 
19419 struct point3d_t
19420 {
19421         point3d_t( int ix, int iy, int iz ): x(ix), y(iy), z(iz) {}
19422 
19423         int x;
19424         int y;
19425         int z;
19426 };
19427 
19428 
19429 int find_group( int ix, int iy, int iz, int grpid, EMData* mg, EMData* visited )
19430 {
19431         int offs[][3] = { {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, {0, 0, 1} };
19432         int noff = 6;
19433 
19434         int nx = visited->get_xsize();
19435         int ny = visited->get_ysize();
19436         int nz = visited->get_zsize();
19437 
19438         vector< point3d_t > pts;
19439         pts.push_back( point3d_t(ix, iy, iz) );
19440         visited->set_value_at( ix, iy, iz, (float)grpid );
19441 
19442         int start = 0;
19443         int end = pts.size();
19444 
19445         while( end > start ) {
19446                 for(int i=start; i < end; ++i ) {
19447                         int ix = pts[i].x;
19448                         int iy = pts[i].y;
19449                         int iz = pts[i].z;
19450 
19451                         for( int j=0; j < noff; ++j ) {
19452                                 int jx = ix + offs[j][0];
19453                                 int jy = iy + offs[j][1];
19454                                 int jz = iz + offs[j][2];
19455 
19456                                 if( jx < 0 || jx >= nx ) continue;
19457                                 if( jy < 0 || jy >= ny ) continue;
19458                                 if( jz < 0 || jz >= nz ) continue;
19459 
19460 
19461                                 if( (*mg)(jx, jy, jz)>0 && (*visited)(jx, jy, jz)==0.0 ) {
19462                                     pts.push_back( point3d_t(jx, jy, jz) );
19463                                     visited->set_value_at( jx, jy, jz, (float)grpid );
19464                                 }
19465 
19466                         }
19467                 }
19468 
19469                 start = end;
19470                 end = pts.size();
19471         }
19472         return pts.size();
19473 }
19474 
19475 
19476 EMData* Util::get_biggest_cluster( EMData* mg )
19477 {
19478         int nx = mg->get_xsize();
19479         int ny = mg->get_ysize();
19480         int nz = mg->get_zsize();
19481 
19482         EMData* visited = new EMData();
19483         visited->set_size( nx, ny, nz );
19484         visited->to_zero();
19485         int grpid = 0;
19486         int maxgrp = 0;
19487         int maxsize = 0;
19488         for( int iz=0; iz < nz; ++iz ) {
19489                 for( int iy=0; iy < ny; ++iy ) {
19490                         for( int ix=0; ix < nx; ++ix ) {
19491                                 if( (*mg)(ix, iy, iz)==0.0 ) continue;
19492 
19493                                 if( (*visited)(ix, iy, iz) > 0.0 ) {
19494                                         // visited before, must be in other group.
19495                                         continue;
19496                                 }
19497 
19498                                 grpid++;
19499                                 int grpsize = find_group( ix, iy, iz, grpid, mg, visited );
19500                                 if( grpsize > maxsize ) {
19501                                         maxsize = grpsize;
19502                                         maxgrp = grpid;
19503                                 }
19504                         }
19505                 }
19506         }
19507 
19508         Assert( maxgrp > 0 );
19509 
19510         int npoint = 0;
19511         EMData* result = new EMData();
19512         result->set_size( nx, ny, nz );
19513         result->to_zero();
19514 
19515         for( int iz=0; iz < nz; ++iz ) {
19516                 for( int iy=0; iy < ny; ++iy ) {
19517                         for( int ix=0; ix < nx; ++ix ) {
19518                                 if( (*visited)(ix, iy, iz)==maxgrp ) {
19519                                         (*result)(ix,iy,iz) = 1.0;
19520                                         npoint++;
19521                                 }
19522                         }
19523                 }
19524         }
19525 
19526         Assert( npoint==maxsize );
19527         delete visited;
19528         return result;
19529 
19530 }
19531 
19532 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)
19533 {
19534         int   ix, iy, iz;
19535         int   i,  j, k;
19536         int   nr2, nl2;
19537         float  dzz, az, ak;
19538         float  scx, scy, scz;
19539         int offset = 2 - nx%2;
19540         int lsm = nx + offset;
19541         EMData* ctf_img1 = new EMData();
19542         ctf_img1->set_size(lsm, ny, nz);
19543         float freq = 1.0f/(2.0f*ps);
19544         scx = 2.0f/float(nx);
19545         if(ny>=1) scy = 2.0f/float(ny); else scy=0.0f;
19546         if(nz>=1) scz = 2.0f/float(nz); else scz=0.0f;
19547         nr2 = ny/2 ;
19548         nl2 = nz/2 ;
19549         for ( k=0; k<nz;k++) {
19550                 iz = k;  if(k>nl2) iz=k-nz;
19551                 for ( j=0; j<ny;j++) {
19552                         iy = j;  if(j>nr2) iy=j - ny;
19553                         for ( i=0; i<lsm/2; i++) {
19554                                 ix=i;
19555                                 ak=pow(ix*ix*scx*scx+iy*scy*iy*scy+iz*scz*iz*scz, 0.5f)*freq;
19556                                 if(ak!=0) az=0.0; else az=M_PI;
19557                                 dzz = dz + dza/2.0f*sin(2*(az-azz*M_PI/180.0f));
19558                                 (*ctf_img1) (i*2,j,k)   = Util::tf(dzz, ak, voltage, cs, wgh, b_factor, sign);
19559                                 (*ctf_img1) (i*2+1,j,k) = 0.0f;
19560                         }
19561                 }
19562         }
19563         ctf_img1->update();
19564         ctf_img1->set_complex(true);
19565         ctf_img1->set_ri(true);
19566         //ctf_img1->attr_dict["is_complex"] = 1;
19567         //ctf_img1->attr_dict["is_ri"] = 1;
19568         if(nx%2==0) ctf_img1->set_fftodd(false); else ctf_img1->set_fftodd(true);
19569         return ctf_img1;
19570 }
19571 /*
19572 #define  cent(i)     out[i+N]
19573 #define  assign(i)   out[i]
19574 vector<float> Util::cluster_pairwise(EMData* d, int K) {
19575 
19576         int nx = d->get_xsize();
19577         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19578         vector<float> out(N+K+2);
19579         if(N*(N-1)/2 != nx) {
19580                 //print  "  incorrect dimension"
19581                 return out;}
19582         //  assign random objects as centers
19583         for(int i=0; i<N; i++) assign(i) = float(i);
19584         // shuffle
19585         for(int i=0; i<N; i++) {
19586                 int j = Util::get_irand(0,N-1);
19587                 float temp = assign(i);
19588                 assign(i) = assign(j);
19589                 assign(j) = temp;
19590         }
19591         for(int k=0; k<K; k++) cent(k) = float(assign(k));
19592         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
19593         //
19594         for(int i=0; i<N; i++) assign(i) = 0.0f;
19595         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
19596         bool change = true;
19597         int it = -1;
19598         while(change && disp < dispold) {
19599                 change = false;
19600                 dispold = disp;
19601                 it++;
19602                 //cout<<"Iteration:  "<<it<<endl;
19603                 // dispersion is a sum of distance from objects to object center
19604                 disp = 0.0f;
19605                 for(int i=0; i<N; i++) {
19606                         qm = 1.0e23f;
19607                         for(int k=0; k<K; k++) {
19608                                 if(float(i) == cent(k)) {
19609                                         qm = 0.0f;
19610                                         na = (float)k;
19611                                 } else {
19612                                         float dt = (*d)(mono(i,int(cent(k))));
19613                                         if(dt < qm) {
19614                                                 qm = dt;
19615                                                 na = (float)k;
19616                                         }
19617                                 }
19618                         }
19619                         disp += qm;
19620                         if(na != assign(i)) {
19621                                 assign(i) = na;
19622                                 change = true;
19623                         }
19624                 }
19625         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
19626                 //print disp
19627                 //print  assign
19628                 // find centers
19629                 for(int k=0; k<K; k++) {
19630                         qm = 1.0e23f;
19631                         for(int i=0; i<N; i++) {
19632                                 if(assign(i) == float(k)) {
19633                                         float q = 0.0;
19634                                         for(int j=0; j<N; j++) {
19635                                                 if(assign(j) == float(k)) {
19636                                                                 //it cannot be the same object
19637                                                         if(i != j)  q += (*d)(mono(i,j));
19638                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
19639                                                 }
19640                                         }
19641                                         if(q < qm) {
19642                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
19643                                                 qm = q;
19644                                                 cent(k) = float(i);
19645                                         }
19646                                 }
19647                         }
19648                 }
19649         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
19650         }
19651         out[N+K] = disp;
19652         out[N+K+1] = float(it);
19653         return  out;
19654 }
19655 #undef  cent
19656 #undef  assign
19657 */
19658 #define  cent(i)     out[i+N]
19659 #define  assign(i)   out[i]
19660 vector<float> Util::cluster_pairwise(EMData* d, int K, float T, float F) {
19661         int nx = d->get_xsize();
19662         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19663         vector<float> out(N+K+2);
19664         if(N*(N-1)/2 != nx) {
19665                 //print  "  incorrect dimension"
19666                 return out;}
19667         //  assign random objects as centers
19668         for(int i=0; i<N; i++) assign(i) = float(i);
19669         // shuffle
19670         for(int i=0; i<N; i++) {
19671                 int j = Util::get_irand(0,N-1);
19672                 float temp = assign(i);
19673                 assign(i) = assign(j);
19674                 assign(j) = temp;
19675         }
19676         for(int k=0; k<K; k++) cent(k) = float(assign(k));
19677         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;
19678         //
19679         for(int i=0; i<N; i++) assign(i) = 0.0f;
19680         float qm, dispold = 1.1e22f, disp = 1.0e22f, na=0.0f;
19681         bool change = true;
19682         int it = -1;
19683         int ct = -1;
19684         while ((change && disp < dispold) || ct > 0) {
19685 
19686                 change = false;
19687                 dispold = disp;
19688                 it++;
19689 
19690                 // dispersion is a sum of distance from objects to object center
19691                 disp = 0.0f;
19692                 ct = 0;
19693                 for(int i=0; i<N; i++) {
19694                         qm = 1.0e23f;
19695                         for(int k=0; k<K; k++) {
19696                                 if(float(i) == cent(k)) {
19697                                         qm = 0.0f;
19698                                         na = (float)k;
19699                                 } else {
19700                                         float dt = (*d)(mono(i,int(cent(k))));
19701                                         if(dt < qm) {
19702                                                 qm = dt;
19703                                                 na = (float)k;
19704                                         }
19705                                 }
19706                         }
19707 
19708 
19709                         // Simulated annealing
19710                         if(exp(-1.0/float(T)) > Util::get_irand(1,1000)/1000.0) {
19711                             na = (float)(Util::get_irand(0, K));
19712                             qm = (*d)(mono(i,int(na)));
19713                             ct++;
19714                         }
19715 
19716                         disp += qm;
19717 
19718                         if(na != assign(i)) {
19719                                 assign(i) = na;
19720                                 change = true;
19721                         }
19722                 }
19723 
19724                 //cout<<"Iteration:  "<<it<< " disp " <<disp << " T " << T << " disturb " << ct << endl;
19725                 T = T*F;
19726 
19727         //for(int k=0; k<N; k++) cout<<assign(k)<<"    ";cout<<endl;
19728                 //print disp
19729                 //print  assign
19730                 // find centers
19731                 for(int k=0; k<K; k++) {
19732                         qm = 1.0e23f;
19733                         for(int i=0; i<N; i++) {
19734                                 if(assign(i) == float(k)) {
19735                                         float q = 0.0;
19736                                         for(int j=0; j<N; j++) {
19737                                                 if(assign(j) == float(k)) {
19738                                                                 //it cannot be the same object
19739                                                         if(i != j)  q += (*d)(mono(i,j));
19740                                                         //cout<<q<<"   "<<i<<"   "<<j<<"   "<<k<<endl;}
19741                                                 }
19742                                         }
19743                                         if(q < qm) {
19744                                                 //cout<<qm<<"   "<<q<<"   "<<i<<"   "<<k<<endl;
19745                                                 qm = q;
19746                                                 cent(k) = float(i);
19747                                         }
19748                                 }
19749                         }
19750                 }
19751         //for(int k=0; k<K; k++) cout<<cent(k)<<"    ";cout<<endl;cout<<disp<<endl;
19752         }
19753         out[N+K] = disp;
19754         out[N+K+1] = float(it);
19755         return  out;
19756 }
19757 #undef  cent
19758 #undef  assign
19759 /*
19760 #define  groupping(i,k)   group[i + k*m]
19761 vector<float> Util::cluster_equalsize(EMData* d, int m) {
19762         int nx = d->get_xsize();
19763         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19764         int K = N/m;
19765         //cout<<"  K  "<<K<<endl;
19766         vector<float> group(N+1);
19767         if(N*(N-1)/2 != nx) {
19768                 //print  "  incorrect dimension"
19769                 return group;}
19770         bool active[N];
19771         for(int i=0; i<N; i++) active[i] = true;
19772 
19773         float dm, qd;
19774         int   ppi, ppj;
19775         for(int k=0; k<K; k++) {
19776                 // find two most similiar objects among active
19777                 cout<<"  k  "<<k<<endl;
19778                 dm = 1.0e23;
19779                 for(int i=1; i<N; i++) {
19780                         if(active[i]) {
19781                                 for(int j=0; j<i; j++) {
19782                                         if(active[j]) {
19783                                                 qd = (*d)(mono(i,j));
19784                                                 if(qd < dm) {
19785                                                         dm = qd;
19786                                                         ppi = i;
19787                                                         ppj = j;
19788                                                 }
19789                                         }
19790                                 }
19791                         }
19792                 }
19793                 groupping(0,k) = float(ppi);
19794                 groupping(1,k) = float(ppj);
19795                 active[ppi] = false;
19796                 active[ppj] = false;
19797 
19798                 // find progressively objects most similar to those in the current list
19799                 for(int l=2; l<m; l++) {
19800                         //cout<<"  l  "<<l<<endl;
19801                         dm = 1.0e23;
19802                         for(int i=0; i<N; i++) {
19803                                 if(active[i]) {
19804                                         qd = 0.0;
19805                                         for(int j=0; j<l; j++) { //j in groupping[k]:
19806                         //cout<<"  groupping(j,k)  "<<groupping(j,k)<<"   "<<j<<endl;
19807                                                 int jj = int(groupping(j,k));
19808                         //cout<<"   "<<jj<<endl;
19809                                                 qd += (*d)(mono(i,jj));
19810                                         }
19811                                         if(qd < dm) {
19812                                                 dm = qd;
19813                                                 ppi = i;
19814                                         }
19815                                 }
19816                         }
19817                         groupping(l,k) = float(ppi);
19818                         active[ppi] = false;
19819                         //cout<<"  k  "<<k<<"  l  "<<l<<"  ppi  "<<ppi<<"  dm  "<<dm<<endl;
19820                 }
19821                 //for(int j=k*m; j<(k+1)*m; j++) cout<<group[j]<<endl;
19822         }
19823         // there might be remaining objects when N is not divisible by m, simply put them in one group
19824         if(N%m != 0) {
19825                 int j = K*m;
19826                 K++;
19827                 //cout<<"  N%m  "<<N%m<<"   "<<K<<endl;
19828                 for(int i=0; i<N; i++) {
19829                         if(active[i]) {
19830                                 group[j] = float(i);
19831                 //cout<<"  j,i  "<<j<<"   "<<i<<"   "<<K-1 <<"   "<< group[j]<<endl;
19832                                 j++;
19833                         }
19834                 }
19835         }
19836 
19837         int  cent[K];
19838          // find centers
19839         for(int k=0; k<K; k++) {
19840                 float qm = 1.0e23f;
19841                 for(int i=0; i<N; i++) {
19842                         if(group[i] == float(k)) {
19843                                 qd = 0.0;
19844                                 for(int j=0; j<N; j++) {
19845                                         if(group[j] == float(k)) {
19846                                                 //it cannot be the same object
19847                                                 if(i != j)  qd += (*d)(mono(i,j));
19848                                         }
19849                                 }
19850                                 if(qd < qm) {
19851                                         qm = qd;
19852                                         cent[k] = i;
19853                                 }
19854                         }
19855                 }
19856         }
19857         // dispersion is a sum of distances from objects to object center
19858         float disp = 0.0f;
19859         for(int i=0; i<N; i++) {
19860                 for(int k=0; k<K; k++) if(i == cent[k]) disp += (*d)(mono(i,cent[k]));
19861         }
19862         group[N] = disp;
19863         return  group;
19864 }
19865 #undef  groupping
19866 */
19867 
19868 vector<float> Util::cluster_equalsize(EMData* d) {
19869         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
19870         int nx = d->get_xsize();
19871         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19872         int K = N/2;
19873         vector<float> group(N);
19874         if(N*(N-1)/2 != nx) {
19875                 //print  "  incorrect dimension"
19876                 return group;}
19877         //bool active[N];       //this does not compile in VS2005. --Grant Tang
19878         bool * active = new bool[N];
19879         for(int i=0; i<N; i++) active[i] = true;
19880 
19881         float dm, qd;
19882         int   ppi = 0, ppj = 0;
19883         for(int k=0; k<K; k++) {
19884                 // find pairs of most similiar objects among active
19885                 //cout<<"  k  "<<k<<endl;
19886                 dm = 1.0e23f;
19887                 for(int i=1; i<N; i++) {
19888                         if(active[i]) {
19889                                 for(int j=0; j<i; j++) {
19890                                         if(active[j]) {
19891                                                 qd = (*d)(i*(i - 1)/2 + j);
19892                                                 if(qd < dm) {
19893                                                         dm = qd;
19894                                                         ppi = i;
19895                                                         ppj = j;
19896                                                 }
19897                                         }
19898                                 }
19899                         }
19900                 }
19901                 group[2*k] = float(ppi);
19902                 group[1+2*k] = float(ppj);
19903                 active[ppi] = false;
19904                 active[ppj] = false;
19905         }
19906 
19907         delete [] active;
19908         active = NULL;
19909         return  group;
19910 }
19911 /*
19912 #define son(i,j)=i*(i-1)/2+j
19913 vector<float> Util::cluster_equalsize(EMData* d) {
19914         //  WORKS ONLY FOR NUMBER OF OBJECTS N=l^2   !!
19915         int nx = d->get_xsize();
19916         int N = 1 + int((sqrt(1.0 + 8.0*nx)-1.0)/2.0);
19917         int K = N/2;
19918         vector<float> group(N);
19919         if(N*(N-1)/2 != nx) {
19920                 //print  "  incorrect dimension"
19921                 return group;}
19922         //bool active[N];
19923         int  active[N];
19924         for(int i=0; i<N; i++) active[i] = i;
19925 
19926         float dm, qd;
19927         int   ppi = 0, ppj = 0, ln = N;
19928         for(int k=0; k<K; k++) {
19929                 // find pairs of most similiar objects among active
19930                 //cout<<"  k:  "<<k<<endl;
19931                 dm = 1.0e23;
19932                 for(int i=1; i<ln; i++) {
19933                         for(int j=0; j<i; j++) {
19934                                 //qd = (*d)(mono(active[i],active[j]));
19935                                 qd = (*d)(active[i]*(active[i] - 1)/2 + active[j]);
19936                                 if(qd < dm) {
19937                                         dm = qd;
19938                                         ppi = i;
19939                                         ppj = j;
19940                                 }
19941                         }
19942                 }
19943                 group[2*k]   = float(active[ppi]);
19944                 group[1+2*k] = float(active[ppj]);
19945                 //  Shorten the list
19946                 if(ppi > ln-3 || ppj > ln - 3) {
19947                         if(ppi > ln-3 && ppj > ln - 3) {
19948                         } else if(ppi > ln-3) {
19949                                 if(ppi == ln -1) active[ppj] = active[ln-2];
19950                                 else             active[ppj] = active[ln-1];
19951                         } else { // ppj>ln-3
19952                                 if(ppj == ln -1) active[ppi] = active[ln-2];
19953                                 else             active[ppi] = active[ln-1];
19954                         }
19955                 } else {
19956                         active[ppi] = active[ln-1];
19957                         active[ppj] = active[ln-2];
19958                 }
19959                 ln = ln - 2;
19960         }
19961         return  group;
19962 }
19963 
19964 */
19965 #define data(i,j) group[i*ny+j]
19966 vector<float> Util::vareas(EMData* d) {
19967         const float step=0.001f;
19968         int ny = d->get_ysize();
19969         //  input emdata should have size 2xN, where N is number of points
19970         //  output vector should be 2xN, first element is the number of elements
19971         //  associated with this point, second is 0 is the element is touching the border, 1 if it is interior
19972         vector<float> group(2*ny);
19973         for(int i=0; i<2*ny; i++) group[i] = 0.0f;
19974         int K = int(1.0f/step) +1;
19975         int hit = 0;
19976         for(int kx=0; kx<=K; kx++) {
19977                 float tx = kx*step;
19978                 for(int ky=0; ky<=K; ky++) {
19979                         float ty = ky*step;
19980                         float dm = 1.0e23f;
19981                         for(int i=0; i<ny; i++) {
19982                                 float qd = pow(tx-(*d)(0,i),2) + pow(ty-(*d)(1,i),2);
19983                                 if( qd < dm) {
19984                                         dm = qd;
19985                                         hit = i;
19986                                 }
19987                         }
19988                         data(0,hit) += 1.0f;
19989                         if(kx == 0 || ky == 0 || kx == K || ky == K)  data(1,hit) = 1.0f;
19990                 }
19991         }
19992         return  group;
19993 }
19994 #undef data
19995 
19996 EMData* Util::get_slice(EMData *vol, int dim, int index) {
19997 
19998         int nx = vol->get_xsize();
19999         int ny = vol->get_ysize();
20000         int nz = vol->get_zsize();
20001         float *vol_data = vol->get_data();
20002         int new_nx, new_ny;
20003 
20004         if (nz == 1)
20005                 throw ImageDimensionException("Error: Input must be a 3-D object");
20006         if ((dim < 1) || (dim > 3))
20007                 throw ImageDimensionException("Error: dim must be 1 (x-dimension), 2 (y-dimension) or 3 (z-dimension)");
20008         if (((dim == 1) && (index < 0 || index > nx-1)) ||
20009           ((dim == 1) && (index < 0 || index > nx-1)) ||
20010           ((dim == 1) && (index < 0 || index > nx-1)))
20011                 throw ImageDimensionException("Error: index exceeds the size of the 3-D object");
20012 
20013         if (dim == 1) {
20014                 new_nx = ny;
20015                 new_ny = nz;
20016         } else if (dim == 2) {
20017                 new_nx = nx;
20018                 new_ny = nz;
20019         } else {
20020                 new_nx = nx;
20021                 new_ny = ny;
20022         }
20023 
20024         EMData *slice = new EMData();
20025         slice->set_size(new_nx, new_ny, 1);
20026         float *slice_data = slice->get_data();
20027 
20028         if (dim == 1) {
20029                 for (int x=0; x<new_nx; x++)
20030                         for (int y=0; y<new_ny; y++)
20031                                 slice_data[y*new_nx+x] = vol_data[(y*ny+x)*nx+index];
20032         } else if (dim == 2) {
20033                 for (int x=0; x<new_nx; x++)
20034                         for (int y=0; y<new_ny; y++)
20035                                 slice_data[y*new_nx+x] = vol_data[(y*ny+index)*nx+x];
20036         } else {
20037                 for (int x=0; x<new_nx; x++)
20038                         for (int y=0; y<new_ny; y++)
20039                                 slice_data[y*new_nx+x] = vol_data[(index*ny+y)*nx+x];
20040         }
20041 
20042         return slice;
20043 }
20044 
20045 void Util::image_mutation(EMData *img, float mutation_rate) {
20046         int nx = img->get_xsize();
20047         float min = img->get_attr("minimum");
20048         float max = img->get_attr("maximum");
20049         float* img_data = img->get_data();
20050         array_mutation(img_data, nx*nx, mutation_rate, min, max, 8, 0);
20051         return;
20052 }
20053 
20054 
20055 void Util::array_mutation(float *list, int len_list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20056 
20057         if (is_mirror != 0) {
20058                 for (int i=0; i<len_list; i++) {
20059                         int r = rand()%10000;
20060                         float f = r/10000.0f;
20061                         if (f < mutation_rate) list[i] = 1-list[i];
20062                 }
20063         } else {
20064                 map<int, vector<int> >  graycode;
20065                 map<vector<int>, int> rev_graycode;
20066                 vector <int> gray;
20067 
20068                 int K=1;
20069                 for (int i=0; i<L; i++) K*=2;
20070 
20071                 for (int k=0; k<K; k++) {
20072                         int shift = 0;
20073                         vector <int> gray;
20074                         for (int i=L-1; i>-1; i--) {
20075                                 int t = ((k>>i)%2-shift)%2;
20076                                 gray.push_back(t);
20077                                 shift += t-2;
20078                         }
20079                         graycode[k] = gray;
20080                         rev_graycode[gray] = k;
20081                 }
20082 
20083                 float gap = (K-1)/(max_val-min_val);
20084                 for (int i=0; i<len_list; i++) {
20085                         float val = list[i];
20086                         if (val < min_val) { val = min_val; }
20087                         else if  (val > max_val) { val = max_val; }
20088                         int k = int((val-min_val)*gap+0.5);
20089                         vector<int> gray = graycode[k];
20090                         bool changed = false;
20091                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20092                                 int r = rand()%10000;
20093                                 float f = r/10000.0f;
20094                                 if (f < mutation_rate) {
20095                                         *p = 1-*p;
20096                                         changed = true;
20097                                 }
20098                         }
20099                         if (changed) {
20100                                 k = rev_graycode[gray];
20101                                 list[i] = k/gap+min_val;
20102                         }
20103                 }
20104         }
20105 
20106 }
20107 
20108 vector<float> Util::list_mutation(vector<float> list, float mutation_rate, float min_val, float max_val, int L, int is_mirror) {
20109 
20110         if (is_mirror != 0) {
20111                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20112                         int r = rand()%10000;
20113                         float f = r/10000.0f;
20114                         if (f < mutation_rate) *q = 1-*q;
20115                 }
20116         } else {
20117                 map<int, vector<int> >  graycode;
20118                 map<vector<int>, int> rev_graycode;
20119                 vector <int> gray;
20120 
20121                 int K=1;
20122                 for (int i=0; i<L; i++) K*=2;
20123 
20124                 for (int k=0; k<K; k++) {
20125                         int shift = 0;
20126                         vector <int> gray;
20127                         for (int i=L-1; i>-1; i--) {
20128                                 int t = ((k>>i)%2-shift)%2;
20129                                 gray.push_back(t);
20130                                 shift += t-2;
20131                         }
20132                         graycode[k] = gray;
20133                         rev_graycode[gray] = k;
20134                 }
20135 
20136                 float gap = (K-1)/(max_val-min_val);
20137                 for (vector<float>::iterator q=list.begin(); q!=list.end(); q++) {
20138                         float val = *q;
20139                         if (val < min_val) { val = min_val; }
20140                         else if  (val > max_val) { val = max_val; }
20141                         int k = int((val-min_val)*gap+0.5);
20142                         vector<int> gray = graycode[k];
20143                         bool changed = false;
20144                         for (vector<int>::iterator p=gray.begin(); p!=gray.end(); p++) {
20145                                 int r = rand()%10000;
20146                                 float f = r/10000.0f;
20147                                 if (f < mutation_rate) {
20148                                         *p = 1-*p;
20149                                         changed = true;
20150                                 }
20151                         }
20152                         if (changed) {
20153                                 k = rev_graycode[gray];
20154                                 *q = k/gap+min_val;
20155                         }
20156                 }
20157         }
20158         return list;
20159 }
20160 
20161 
20162 bool Util::sanitycheck(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* output){
20163         //cout<<"sanitycheck called\n";
20164         int total_cost = *output;
20165         int num_matches = *(output+1);
20166 
20167         int cost=0;
20168         int* intx;
20169         int intx_size;
20170         int* intx_next(0);
20171         int intx_next_size = 0;
20172         int curclass;
20173         int curclass_size;
20174         //cout<<"cost by match: [";
20175         for(int i = 0; i < num_matches; i++){
20176                 curclass = *(output+2+ i*nParts);
20177                 // check feasibility
20178                 if (*(argParts + Indices[curclass]+1) == -5) {cout<<"infeasible match!\n"; return 0;}
20179                 *(argParts + Indices[curclass]+1) = -5;
20180                 // initialize intx to the number of elements in class curclass of partition 0 of the i-th match
20181                 curclass_size = *(dimClasses+curclass)-2;
20182                 intx = new int[curclass_size];
20183                 for (int ic = 0; ic < curclass_size; ic++) *(intx+ic) = *(argParts + Indices[curclass]+2+ic);
20184                 intx_size = curclass_size;
20185 
20186                 for (int j=1; j < nParts; j++){
20187                       curclass = *(output+2+ i*nParts+j);
20188                       if (*(argParts + Indices[j*K+curclass]+1)==-5){cout<<"infeasible match!\n"; return 0;}
20189                       *(argParts + Indices[j*K+curclass]+1)=-5;
20190                       // compute the intersection of intx and class curclass of partition j of the i-th match
20191                       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);
20192                       intx_next = new int[intx_next_size];
20193                       Util::k_means_cont_table_(intx,argParts + Indices[j*K+curclass]+2, intx_next, intx_size, *(dimClasses + j*K+curclass)-2,1);
20194                       delete[] intx;
20195                       intx=intx_next;
20196                       intx_size= intx_next_size;
20197                       if (j==nParts-1) delete[] intx_next;
20198                 }
20199 
20200                 if (intx_next_size <= T) {cout << "something wrong with solution!\n"; return 0;}
20201                 //cout <<intx_next_size<<",";
20202                 cost = cost + intx_next_size;
20203         }
20204         //cout<<"]\n";
20205         if (cost != total_cost) {cout << "something wrong with solution!\n"; return 0;}
20206 
20207         return 1;
20208 
20209 }
20210 
20211 
20212 // Given J, returns the J matches with the largest weight
20213 // matchlist has room for J matches
20214 // costlist has J elements to record cost of the J largest matches
20215 
20216 void Util::search2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* matchlist, int* costlist, int J){
20217         
20218         // some temp variables
20219         bool flag = 0;
20220         int nintx;
20221         int* dummy(0);
20222         //int* ret;
20223         int* curbranch = new int[nParts];
20224         
20225         //initialize costlist to all 0
20226         for(int jit= 0; jit< J; jit++) *(costlist+jit) = 0;
20227         
20228         
20229         for(int a=0; a<K; a++)
20230         {
20231         
20232                 // check that class a of partition 0 is active and has greater than T elements. If not the case, then skip to the next class
20233                 if (*(argParts + Indices[a] + 1) < 1) continue;
20234                 if (*(dimClasses + a)-2 <= T) continue;
20235 
20236                 // 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
20237 
20238                 for( int i=1; i < nParts; i++){
20239                         flag = 0; // if flag stays 0 then no class in this partition has more than T objects in common with a, which implies no feasible match (> T) with class a of part 0 is possible.
20240                         for(int j=0; j < K; j++){
20241                                 if (*(argParts + Indices[i*K+j] + 1) < 1) continue;
20242                                 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);
20243                                 if (nintx > T) flag=1;
20244                                 else *(argParts + Indices[i*K+j] + 1) =-4;
20245                         }
20246                         if (flag==0) {break;}
20247                 }
20248 
20249                 // explore determines J matchs with the largest weight greater than T where class in partition 0 is class a
20250                 *curbranch = a;
20251 
20252                 if (flag > 0) // Each partition has one or more active class
20253                         Util::explore2(argParts, Indices, dimClasses, nParts, K, T, argParts+Indices[a]+2, *(dimClasses+a)-2, argParts+Indices[a]+2,
20254                         *(dimClasses+a)-2,0, J, matchlist, costlist, curbranch);
20255                         
20256                 // take all the classes marked as -4 and remark it as 1 in preparation for next round
20257                 for( int i=1; i < nParts; i++){
20258                         for(int j=0; j < K; j++){
20259                                 if (*(argParts + Indices[i*K+j] + 1) == -4) *(argParts + Indices[i*K+j] + 1) =1;
20260 
20261                         }
20262                 }
20263         }
20264         
20265         delete[] curbranch;
20266 }
20267 
20268 // returns J largest matches
20269 void Util::explore2(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int* curintx, int size_curintx, int* next, int size_next, int depth, int J, int* matchlist, int*costlist, int* curbranch){
20270 
20271 // depth is the level which is going to be explored in the current iteration
20272         int* curintx2(0);
20273         int nintx = size_curintx;
20274         
20275         
20276         // 2. take the intx of next and cur. Prune if <= T
20277         if (depth >0){
20278                 nintx = Util::k_means_cont_table_(curintx,next, curintx2, size_curintx, size_next,0);
20279                 if (nintx <= T) return; //prune!
20280         }
20281 
20282         // 1. we're at a leaf with weight > T, so determine if there is any empty space. If so, put it in. If not, determine if current cost is larger than any of the cost in matchlist, if so, replace the  smallest one in matchlist
20283         if (depth == (nParts-1)) {
20284                 
20285                 int replace = 0;
20286                 int ind_smallest = -1;
20287                 int smallest_cost = -1;
20288                 
20289                 for (int jit = 0; jit < J; jit++){
20290                         if (*(costlist+jit) < nintx){
20291                                 replace = 1;
20292                                 if (ind_smallest == -1) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20293                                 if (*(costlist+jit) < smallest_cost) {ind_smallest = jit; smallest_cost = *(costlist+jit);}
20294                         }       
20295                 }
20296                 
20297                 if (replace > 0){
20298                         // replace the smallest cost in matchlist with the current stuff
20299                         *(costlist + ind_smallest) = nintx;
20300                         for (int xit = 0; xit < nParts; xit++)
20301                                 *(matchlist + ind_smallest*nParts + xit) = *(curbranch+xit);
20302                                 
20303                 }
20304                 
20305                 return; 
20306         }
20307         
20308 
20309         // 3. have not yet reached a leaf, and current weight is still greather than T, so keep on going.
20310 
20311         if (depth > 0){
20312                 curintx2 = new int[nintx]; // put the intersection set in here
20313                 Util::k_means_cont_table_(curintx,next,curintx2, size_curintx, size_next,1);
20314         }
20315 
20316         if (depth == 0){
20317                 // set curintx2 to curintx
20318                 curintx2 = new int[size_curintx];
20319                 for (int cp = 0; cp < size_curintx; cp++) *(curintx2+cp) = *(curintx+cp);
20320         }
20321 
20322 
20323         // recursion (non-leaf case)
20324         depth=depth+1;
20325         // we now consider each of the classes in partition depth and recurse upon each of them
20326         for (int i=0; i < K; i++){
20327 
20328                 if (*(argParts + Indices[depth*K+i] + 1) < 1) continue; // class is not active so move on
20329                 size_next = (*(dimClasses + depth*K+i ))-2;
20330                 if (size_next <= T) continue;
20331                 *(curbranch+depth) = i;
20332                 Util::explore2(argParts,Indices, dimClasses, nParts, K, T, curintx2, nintx, argParts + Indices[depth*K+i] + 2, size_next, depth,J, matchlist,
20333                         costlist, curbranch);
20334                 
20335         }
20336 
20337         delete[] curintx2;
20338 }
20339 
20340 void Util::initial_prune(vector <vector <int*> > & Parts, int* dimClasses, int nParts, int K, int T) {
20341         //cout<<"initial_prune\n";
20342         // simple initial pruning. For class indClass of partition indPart:
20343         // 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
20344         // If for some partition (not equal to indPart) for which there is no such class, then return 0 immediately.
20345 
20346         // 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
20347 
20348         // remember when calling k_means_cont_table, the first element of each class is an index, and actual number of elements in the class
20349         // (as stored in dimClasses) should be decremented by 1 accordingly, and second is dummy variable
20350 
20351         int* dummy(0);
20352         int* cref;
20353         int cref_size;
20354         int* ccomp;
20355         int ccomp_size;
20356         int nintx;
20357         for (int i=0; i < nParts; i++){
20358                 for (int j =0; j < K; j++){
20359 
20360                         // consider class Parts[i][j]
20361                         cref = Parts[i][j];//incr by 1 since first element is index and second is dummy
20362                         cref_size = (*(dimClasses+i*K+(*cref)))-2;
20363 
20364 
20365                         if (cref_size <= T){
20366 
20367                                 *cref = -1;
20368                                 continue;
20369                         }
20370                         bool done = 0;
20371                         for (int a = 0; a < nParts; a++){
20372                                 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
20373                                 bool hasActive=0;
20374                                 for (unsigned int b=0; b < Parts[a].size(); b++){
20375                                         // get the card of the intx between Parts[i][j] and Parts[a][b] using k_means_cont_table
20376                                         // remember first element of each class is the index of the class
20377                                         ccomp = Parts[a][b];
20378                                         ccomp_size= (*(dimClasses+a*K+(*ccomp)))-2;
20379                                         nintx = Util::k_means_cont_table_(cref+2,ccomp+2, dummy, cref_size, ccomp_size,0);
20380 
20381 
20382                                         if (nintx <= T)
20383                                                 *(ccomp+1) = 0; // class Parts[a][b] is 'inactive' for cref
20384                                         else{
20385                                                 *(ccomp+1)=1; // class Parts[a][b] is 'active' for cref
20386                                                 hasActive=1;
20387                                         }
20388                                 }
20389                                 // see if partition a has at least one active class.if not then we're done with cref
20390                                 if (hasActive < 1){
20391                                    done=1;
20392                                    break;
20393                                 }
20394 
20395                         }
20396 
20397                         if (done > 0){
20398                                 // remove class j from partition i
20399 
20400                                 *cref = -1; // mark for deletion later
20401                                 continue; // move on to class Parts[i][j+1]
20402                         }
20403 
20404                         // now we adopt more aggressive measures. we find the match with the largest weight which has class Parts[i][j] for partition i.
20405                         // 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.
20406 
20407                         // (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.
20408                         // Note that if there are ALOT of partitions, then sorting the partititions may actually take longer than the time saved by ordering hte
20409                         // partitions. However, we do not have a lot of partitions, (usually around 10), so sorting time will be trivial compared to exploring time.
20410 
20411                         // explore returns one if there is a feasible matching containing class cref which has weight gt T, and otherwise returns 0
20412                         //bool found = 1;
20413                         bool found = explore(Parts, dimClasses, nParts, K, T, i, cref+2, cref_size, cref, cref_size,0);
20414 
20415                         if (found<1){ // There is NO feasible matching with class j (cref)  with weight greater than T, so delete this class from Parts
20416                                 // Parts[i].erase(Parts[i].begin()+j);
20417                                 *cref = -1;
20418                         }
20419                 }
20420 
20421                 // Erase from Parts[i] all the classes that's being designated for erasure
20422 
20423                 for (int d = K-1; d > -1; d--){
20424                         if (*(Parts[i][d]) < 0) Parts[i].erase(Parts[i].begin()+d);
20425                 }
20426 
20427         }
20428         //cout <<"number of classes left in each partition after initial prune\n";      
20429         // Print out how many classes are left in each partition
20430         //for (int i =0; i < nParts; i++)
20431         //      cout << Parts[i].size()<<", ";
20432         //cout << "\n";
20433 }
20434 
20435 
20436 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) {
20437 
20438 
20439         if (size_next <= T) return 0;
20440 
20441         // take the intx of next and cur
20442         int* curintx2(0);
20443         int nintx = Util::k_means_cont_table_(curintx, next+2, curintx2, size_curintx, size_next,0);
20444         if (nintx <= T) return 0;
20445 
20446         int old_depth=depth;
20447         if (depth == partref) depth = depth + 1; // we skip classes in partref
20448         if (depth == (nParts)) { if (old_depth>0) return 1;}
20449 
20450         // have not yet reached a leaf, and current weight is still greather than T, so keep on going.
20451 
20452         curintx2 = new int[nintx]; // put the intersection set in here
20453         Util::k_means_cont_table_(curintx,next+2,curintx2, size_curintx, size_next,1);
20454 
20455         // if (old_depth > 0) {delete[] curintx;} // don't delete when depth == 0 because curintx in that case is a pointer into Parts
20456 
20457         // we now consider each of the classes in partition (depth+1) in turn
20458         bool gt_thresh;
20459         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
20460 
20461         for (int i=0; i < num_classes; i++){
20462                 if (*(Parts[depth][i]+1) < 1) continue; // class is not active so move on
20463                 size_next = (*(dimClasses + depth*K+(*(Parts[depth][i])) ))-2;
20464                 gt_thresh = explore(Parts,dimClasses, nParts, K, T, partref, curintx2,nintx, Parts[depth][i], size_next, depth+1);
20465                 if (gt_thresh) return 1;
20466         }
20467         delete[] curintx2;
20468         return 0;
20469 }
20470 
20471 
20472 
20473 
20474 vector<int> Util::bb_enumerateMPI_(int* argParts, int* dimClasses, int nParts, int K, int T, int n_guesses, int LARGEST_CLASS, int J,
20475 int max_branching, float stmult, int branchfunc, int LIM) {
20476 
20477         
20478         // 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
20479         // 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
20480         // Make a vector of nParts vectors of K int* each
20481          int* Indices = new int[nParts*K];
20482          int ind_c = 0;
20483          for (int i=0; i < nParts; i++){
20484                  for(int j = 0; j < K; j++){
20485                          Indices[i*K + j] = ind_c;
20486                          ind_c = ind_c + *(dimClasses+i*K + j);
20487 
20488                  }
20489          }
20490 
20491         // do initial pruning on argParts and return the pruned partitions
20492 
20493         // Make a vector of nParts vectors of K int* each
20494         vector <vector <int*> > Parts(nParts,vector<int*>(K));
20495         ind_c = 0;
20496         int argParts_size=0;
20497         for (int i=0; i < nParts; i++){
20498                 for(int j = 0; j < K; j++){
20499                         Parts[i][j]=argParts + ind_c;
20500                         ind_c = ind_c + *(dimClasses+i*K + j);
20501                         argParts_size = argParts_size + *(dimClasses+i*K + j);
20502 
20503                 }
20504         }
20505 
20506         // in the following we call initial_prune with Parts which is a vector. This is not the most
20507         // efficient since vector does not allow for direct addressing. But since initial_prune doesn't have very high complexity, and
20508         // 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.....
20509 
20510         // comment out for testing
20511         Util::initial_prune(Parts, dimClasses, nParts, K,T);
20512         for(int i = 0; i < nParts; i++){
20513                 for(int j=0; j < K; j++){
20514                         *(argParts + Indices[i*K + j]+1) = -1;
20515                 }
20516         }
20517 
20518         int num_classes;
20519         int old_index;
20520         for(int i=0; i<nParts; i++){
20521                 num_classes = Parts[i].size();// number of classes in partition i after pruning
20522                 for (int j=0; j < num_classes; j++){
20523                         old_index = *(Parts[i][j]);
20524                         //cout << "old_index: " << old_index<<"\n";
20525                         *(argParts + Indices[i*K + old_index]+1) = 1;
20526                 }
20527         }
20528 
20529 
20530         // if we're not doing mpi then keep going and call branchMPI and return the output
20531         //cout <<"begin partition matching\n";
20532         //int* dummy(0);
20533         int* output = Util::branchMPI(argParts, Indices,dimClasses, nParts, K, T,0,n_guesses,LARGEST_CLASS, J, max_branching, stmult,
20534         branchfunc, LIM);
20535         
20536         //cout<<"total cost: "<<*output<<"\n";
20537         //cout<<"number of matches: "<<*(output+1)<<"\n";
20538         // 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
20539         bool correct = Util::sanitycheck(argParts, Indices,dimClasses, nParts, K, T,output);
20540 
20541         // something is wrong with output of branchMPI!
20542         if (correct < 1){
20543                 cout << "something is wrong with output of branchMPI!\n";
20544                 vector<int> ret(1);
20545                 ret[0]=-1;
20546                 return ret;
20547         }
20548 
20549         // output is not nonsense, so now put it into a single dimension vector and return
20550         // 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
20551         // and the rest is the list of matches. output is one dimensional
20552 
20553         int output_size = 2+ *(output+1) * nParts;
20554         vector<int> ret(output_size);
20555         for (int i = 0; i < output_size; i++){
20556                 ret[i]=*(output+i);
20557         }
20558         return ret;
20559 
20560 }
20561 
20562 
20563 int branch_all=0;
20564 int* Util::branchMPI(int* argParts, int* Indices, int* dimClasses, int nParts, int K, int T, int curlevel,int n_guesses, int
20565 LARGEST_CLASS, int J, int max_branching, float stmult, int branchfunc, int LIM) {
20566 
20567 //*************************************
20568 //testing search2
20569 if (1 == 0){
20570 cout <<"begin test search2\n";
20571 int* matchlist = new int[J*nParts];
20572 int* costlist = new int[J];
20573 for (int jit = 0; jit < nParts; jit++) *(costlist+jit) = 0;
20574 Util::search2(argParts,Indices, dimClasses, nParts, K,  T, matchlist,costlist,J);
20575 
20576 for (int jit = 0; jit < J; jit++) {
20577   cout << *(costlist +jit)<<": ";
20578   for (int yit = 0; yit < nParts; yit++)
20579         cout << *(matchlist + jit*nParts + yit)<<",";
20580   cout <<"\n";  
20581 
20582 }
20583 cout <<"end test search2\n";
20584 int* ret = new int[1];
20585 *ret=1;
20586 delete [] matchlist;
20587 delete [] costlist;
20588 return ret;
20589 }
20590 //**************************************
20591 
20592         // Base Case: we're at a leaf, no more feasible matches possible
20593         if (curlevel > K -1){
20594                 int* res = new int[2];
20595                 *res = 0;
20596                 *(res+1)=0;
20597                 return res;
20598         }
20599 
20600         // branch dynamically depending on results of search 2!
20601         
20602         int* matchlist = new int[J*nParts];
20603         int* costlist = new int[J];
20604         Util::search2(argParts,Indices, dimClasses, nParts, K,  T, matchlist,costlist,J);
20605         
20606         
20607         // each class in the matches found by findTopLargest is encoded by the original index of the first element of the class in argPart
20608         // each match contains nParts classes, with the i-th class belonging to the i-th partition.
20609 
20610         // if there are no feasible matches with cost gt T, then return 0
20611         for (int jit = 0; jit < J ; jit++){
20612         
20613                 if (costlist[jit] > T) break;
20614                 if (jit == J-1){
20615                         int* res = new int[2];
20616                         *res = 0;
20617                         *(res+1)=0;
20618                         return res;
20619                 }
20620         }
20621         
20622 
20623         
20624         // note that costlist and matchlist are NOT sorted by weight, and branch factor takes care of that...
20625         if (curlevel==0) branch_all = 0;
20626         int* newcostlist;
20627         int* newmatchlist;
20628         
20629         int nBranches = -1;
20630         
20631         if (branchfunc == 2)
20632                 nBranches = branch_factor_2(costlist,matchlist,J, T, nParts, curlevel, max_branching, LIM); // branch based on distribution of top J (weighted) matches  with cost > T
20633 
20634         if (branchfunc == 3)
20635                 nBranches = branch_factor_3(costlist,matchlist,J, T, nParts, curlevel, max_branching, K, LIM); // branch based on distribution of top J (weighted) matches  with cost > T
20636 
20637         if (branchfunc == 4)
20638                 nBranches = branch_factor_4(costlist,matchlist,J, T, nParts, curlevel, max_branching, stmult); // branch based on distribution of top J (weighted) matches  with cost > T
20639 
20640         newcostlist= new int[nBranches];
20641         newmatchlist = new int[nBranches*nParts];
20642         for (int i=0; i<nBranches; i++){
20643                 *(newcostlist + i) = *(costlist+i);
20644                 for (int j=0; j< nParts; j++)
20645                         *(newmatchlist + i*nParts + j) = *(matchlist + i*nParts+j);
20646         }
20647 
20648         delete[] costlist;
20649         delete[] matchlist;
20650         
20651         int* maxreturn = new int[2];//initialize to placeholder
20652         *maxreturn=0;
20653         *(maxreturn+1)=0;
20654         // some temporary variables
20655         int old_index;
20656         int totalcost;
20657         int nmatches;
20658         //int offset;
20659 
20660         for(int i=0; i < nBranches ; i++){
20661 
20662                 // consider the i-th match returned by findTopLargest
20663                 //if (newcostlist[i] <= T) continue;
20664 
20665                 // 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.
20666                 // After branch returns, compute overall cost, unmark  the classes just marked as 1 again in preparation for next loop.
20667 
20668                 for(int j=0; j < nParts; j++){
20669                         // matchlist[i*nParts + j] is the original index of the class belonging to the j-th partition in the i-th match.
20670                         old_index=newmatchlist[i*nParts + j];
20671                         *(argParts + Indices[j*K+old_index] + 1) = -2;
20672                 }
20673 
20674                 
20675                 int* ret = Util::branchMPI(argParts, Indices, dimClasses, nParts, K, T,curlevel+1,n_guesses, LARGEST_CLASS,
20676                 J,max_branching, stmult,branchfunc, LIM);
20677                 
20678                 // first element of ret is the total cost of all the matches in ret, and second element is the number of matches in ret
20679                 totalcost = newcostlist[i] + *ret;
20680 
20681                 //if (curlevel == 0) {
20682                 //      cout <<"totalcost*****************************************************************: "<<totalcost<<", costlist["<<i<<"]="<<newcostlist[i]<<", *ret="<<*ret<<", level: "<<curlevel<<"\n";
20683                         
20684                 //}
20685                 if (totalcost > *maxreturn) // option 1
20686                 {
20687                         nmatches = 1 + *(ret+1);
20688                         delete[] maxreturn; // get rid of the old maxreturn
20689                         maxreturn = new int[2+nmatches*nParts];
20690                         *maxreturn = totalcost;
20691                         
20692                         *(maxreturn + 1)= nmatches;
20693                         int nret = 2+(nmatches-1)*nParts;
20694                         for(int iret=2; iret <nret;iret++) *(maxreturn+iret)=*(ret+iret);
20695                         for(int imax=0; imax<nParts;imax++) *(maxreturn+nret+imax)=newmatchlist[i*nParts + imax];
20696                 }
20697 
20698 
20699                 delete[] ret;
20700 
20701                 // unmark the marked classes in preparation for the next iteration
20702 
20703                 for(int j=0; j < nParts; j++){
20704                         old_index=newmatchlist[i*nParts + j];
20705                         *(argParts + Indices[j*K+old_index] + 1) = 1;
20706                 }
20707 
20708         }
20709 
20710         delete[] newmatchlist;
20711         delete[] newcostlist;
20712         
20713         return maxreturn;
20714 
20715 }
20716 
20717 int* costlist_global;
20718 
20719 // make global costlist
20720 bool jiafunc(int i, int j){
20721         return (costlist_global[j] < costlist_global[i]) ;
20722 
20723 }
20724 
20725 // Given J matches, branch always on the first one (i.e., the one with the largest weight, so the worst case we just end up doing greedy).
20726 // Branch on the second one only if it is INFEASIBLE with the first, so you know it will never occur in any branching beginning with the first.
20727 // Branch on subsequent ones only if its infeasible with ALL the ones which we have previously decided to branch on.
20728 // The other option is to use LIM - so we branch on a match if its infeasible with at least LIM matches which we have previously decoded to branch on.
20729 // For now, LIM is defaulted to -1, which means we branch on a match only if it is infeasible to ALL matches we have previously decided to branch on.
20730 int Util::branch_factor_2(int* costlist, int* matchlist, int J, int T, int nParts, int curlevel, int max_branching, int LIM){
20731         
20732         int ntot=0;
20733         for (int jit=0; jit < J; jit++){
20734                 if (*(costlist+jit) > T) ntot++;
20735         }
20736 
20737         int cur;
20738         // sort matchlist by cost
20739         int* indx = new int[J];
20740         for (int jit=0; jit < J; jit++) indx[jit]=jit;
20741         vector<int> myindx (indx, indx+J);
20742         vector<int>::iterator it;
20743         costlist_global=costlist;
20744         sort(myindx.begin(), myindx.begin()+J, jiafunc);
20745 
20746         // put matchlist in the order of mycost
20747         int* templist = new int[J];
20748         int* temp2list = new int[J*nParts];
20749         int next = 0;
20750         
20751         for (it=myindx.begin(); it!=myindx.end();++it){
20752                 cur = *(costlist + *it);
20753                 if (cur > T){
20754                         
20755                         templist[next] = cur;
20756                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
20757                         next = next + 1;
20758                 }
20759         }
20760         
20761         for (int jit=0; jit < ntot; jit++){
20762                 *(costlist+jit)=*(templist + jit);
20763                 //cout <<*(costlist+jit)<<", ";
20764                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
20765         }
20766         //cout <<"\n";
20767         
20768         delete [] indx;
20769         //compute the average 
20770         
20771         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
20772         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
20773         
20774         
20775         int B=1;
20776         int B_init=B;
20777         int infeasible=0;
20778         
20779         for (int i=B_init; i<ntot; i++){
20780                 if (i==ntot) continue;
20781                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
20782                 // branch on
20783                 infeasible = 0;
20784                 if (LIM < 0) LIM = B;
20785                 for (int j=0; j<B; j++){
20786                         
20787                         for (int vit=0; vit<nParts; vit++){
20788                                 if (temp2list[i*nParts+vit] == matchlist[j*nParts+vit]) {infeasible++; break;}
20789                         }
20790                         if (infeasible >= LIM) break;
20791                 }
20792                 
20793                 if (infeasible >= LIM){
20794                         *(costlist+B)=*(templist+i);
20795                         for (int vit=0; vit < nParts; vit++)
20796                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
20797                         B=B+1;  
20798                 }
20799         }
20800         
20801         delete [] templist;
20802         delete [] temp2list;
20803         //cout<<"**************************************** "<<B<<" ***************************\n";
20804         
20805         if (branch_all < max_branching){
20806                 if (B>1)
20807                         {branch_all = branch_all + B -1 ; }
20808         }
20809         else B=1;
20810         
20811         return B;
20812         
20813 
20814 }
20815 
20816 
20817 // similar to branch_factor_2 except we branch on a match if it is infeasible with all other matches in matchlist (not just the ones we branch on). LIM plays similar role here.
20818 int Util::branch_factor_3(int* costlist, int* matchlist, int J, int T, int nParts, int curlevel, int max_branching, int K, int LIM){
20819         
20820         int ntot=0;
20821         for (int jit=0; jit < J; jit++){
20822                 if (*(costlist+jit) > T) ntot++;
20823         }
20824 
20825         int cur;
20826         // sort matchlist by cost
20827         int* indx = new int[J];
20828         for (int jit=0; jit < J; jit++) indx[jit]=jit;
20829         vector<int> myindx (indx, indx+J);
20830         vector<int>::iterator it;
20831         costlist_global=costlist;
20832         sort(myindx.begin(), myindx.begin()+J, jiafunc);
20833 
20834         // put matchlist in the order of mycost
20835         int* templist = new int[J];
20836         int* temp2list = new int[J*nParts];
20837         int next = 0;
20838         
20839         for (it=myindx.begin(); it!=myindx.end();++it){
20840                 cur = *(costlist + *it);
20841                 if (cur > T){
20842                         
20843                         templist[next] = cur;
20844                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
20845                         next = next + 1;
20846                 }
20847         }
20848         
20849         for (int jit=0; jit < ntot; jit++){
20850                 *(costlist+jit)=*(templist + jit);
20851                 //cout <<*(costlist+jit)<<", ";
20852                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
20853         }
20854         //cout <<"\n";
20855         
20856         delete [] indx;
20857         //compute the average 
20858         
20859         // partition matches into groups where matches in each group is mutually feasible, and matches from different groups are not feasible
20860         // The number of branches to explore are the mutually exclusive matches. Take the largest of these
20861         
20862         
20863         int B=1;
20864         int B_init=B;
20865         int infeasible=0;
20866         // if we're near the bottom of the tree then explore more... this is because the larger weights are not likely to change much,
20867         // whereas the smaller ones can have many permutations
20868         if (LIM < 0) LIM = ntot-1;
20869         for (int i=B_init; i<ntot; i++){
20870                 if (i==ntot) continue;
20871                 // look at the i-th match. branch on it only if it is infeasible with ALL previous matches we have decided to
20872                 // branch on
20873                 infeasible = 0;
20874                 
20875                 for (int j=0; j<ntot; j++){
20876                         if (j == i) continue;
20877                         for (int vit=0; vit<nParts; vit++){
20878                                 if (temp2list[i*nParts+vit] == temp2list[j*nParts+vit]) {infeasible++; break;}
20879                         }
20880                         if (infeasible >= LIM) break;
20881                 }
20882                 
20883                 if (infeasible >= LIM){
20884                         *(costlist+B)=*(templist+i);
20885                         for (int vit=0; vit < nParts; vit++)
20886                                 *(matchlist+B*nParts + vit)=*(temp2list+i*nParts + vit);
20887                         B=B+1;  
20888                 }
20889         }
20890         
20891         delete [] templist;
20892         delete [] temp2list;
20893         //cout<<"**************************************** "<<B<<" ***************************\n";
20894         
20895         
20896         if (branch_all < max_branching){
20897                 if (B>1)
20898                         {branch_all = branch_all + B-1;}
20899         }
20900         else B=1;
20901         
20902         return B;
20903         
20904 
20905 }
20906 
20907 // We branch based on distribution of the cost of the J largest matches. Roughly speaking, if there is a match which has significantly larger weight than others, then we branch just on that
20908 // match. Otherwise, we branch on similar weighted matches.
20909 // As before we always branch on the match with the largest cost so worst case we'll get greedy.
20910 // We compute standard dev of the J costs, and then if the difference between the cost of a match and the largest cost is within stmult*standard dev, then we branch on it.
20911 int Util::branch_factor_4(int* costlist, int* matchlist, int J, int T, int nParts, int curlevel, int max_branching, float stmult){
20912         int sum=0;
20913         float average =0;
20914         int ntot=0;
20915         for (int jit=0; jit < J; jit++){
20916                 if (*(costlist+jit) > T) {ntot++; sum = sum +*(costlist+jit);}
20917         }
20918         average = ((float)sum)/((float)ntot);
20919         int cur;
20920         // sort matchlist by cost
20921         int* indx = new int[J];
20922         for (int jit=0; jit < J; jit++) indx[jit]=jit;
20923         vector<int> myindx (indx, indx+J);
20924         vector<int>::iterator it;
20925         costlist_global=costlist;
20926         sort(myindx.begin(), myindx.begin()+J, jiafunc);
20927 
20928         // put matchlist in the order of mycost
20929         int* templist = new int[J];
20930         int* temp2list = new int[J*nParts];
20931         int next = 0;
20932         
20933         for (it=myindx.begin(); it!=myindx.end();++it){
20934                 cur = *(costlist + *it);
20935                 if (cur > T){
20936                         
20937                         templist[next] = cur;
20938                         for (int vt = 0; vt < nParts; vt++) temp2list[next*nParts + vt] = matchlist[ (*it)*nParts + vt];
20939                         next = next + 1;
20940                 }
20941         }
20942         
20943         for (int jit=0; jit < ntot; jit++){
20944                 *(costlist+jit)=*(templist + jit);
20945                 //cout <<*(costlist+jit)<<", ";
20946                 for (int vit=0; vit < nParts; vit++) matchlist[jit*nParts + vit]= temp2list[jit*nParts + vit];
20947         }
20948         //cout <<"\n";
20949         
20950         delete [] indx;
20951         delete [] templist;
20952         delete [] temp2list;
20953         
20954         if (ntot == 1) return 1;
20955         
20956         // look at the average, standard dev etc. If standard dev very small, i.e., costs very similar, then branch on the similar
20957         // costs
20958         float sq_sum=0.0;
20959         //cout <<"costlist:";
20960         for (int i=0; i< ntot; i++){
20961                 sq_sum = sq_sum + (float) pow((float) *(costlist+i) - average, (float)2.0);
20962                 //cout <<*(costlist+i)<<", ";
20963         }       
20964         //cout <<"\n";
20965         
20966         float variance = sq_sum/ntot;
20967         float stdev = (float)pow((float)variance,(float)0.5);
20968         
20969         //cout <<"stdev: "<<int(stdev)<<"\n";
20970         
20971         int B=1;
20972         int largest = *costlist;
20973         //cout <<"largest: "<<largest<<"\n";
20974         for (int i=1; i<ntot; i++){
20975                 int cur = *(costlist+i);
20976                 if (largest-cur < (float)(stdev*stmult)) B++;
20977                 else break;
20978         
20979         }
20980         //cout <<"B: "<<B<<"\n";
20981         if (branch_all < max_branching){
20982                 if (B>1)
20983                         {branch_all = branch_all + B-1;}
20984         }
20985         else B=1;
20986         
20987         return B;
20988         
20989 
20990 }

Generated on Thu Dec 9 13:45:52 2010 for EMAN2 by  doxygen 1.3.9.1